相机IMU融合四部曲(三):MSF详细解读与使用
极品巧克力
前言
通过前两篇文章,《D-LG-EKF详细解读》和《误差状态四元数详细解读》,已经把相机和IMU融合的理论全部都推导一遍了。而且《误差状态四元数》还对实际操作中的可能遇到的一些情况,进行指导。
这些理论都已经比较完整了,那么,该如何在实际当中操作呢?该如何用到实际产品中呢?误差状态四元数,是有开源的程序的,但是它是集成在rtslam( https://www.openrobots.org/wiki/rtslam/ )里面的,不方便提取出来使用。
但还有另外一个开源的程序,ETH的MSF(https://github.com/ethz-asl/ethzasl_msf),它是独立的一个开源程序,可以比较方便地用在自己的工程里面,并且它的理论与误差状态四元数很接近,稍微有点不同,所以MSF开源程序就成了一个不错的选择。
所以,我把MSF的程序全部都通读一遍之后,结合程序来推导MSF的理论,总结成本文,包括MSF的实验,与各位分享。
1.基本模型
基本模型如下图所示。
MSF的可扩展性很好,程序里可以接入新的传感器,比如GPS,激光雷达,码盘等。
主要的理论还是误差状态四元数里面的理论。
相比于《卡尔曼四元数带外参融合方法》,它的方法更好。
首先,它的核心状态里面没有重力在世界坐标系下的表示。因为它用的就是《误差状态四元数》里面的5.3.1的方法,直接就是以水平坐标系为世界坐标系。只是初始位置根据当前加速度计的测量值和理论重力计算出来,初始位置带着一个协方差而已。因为反正最后都是要转换到水平坐标系下的,所以这种方法反而更加方便。
而且,IMU的世界坐标系和相机的世界坐标系,也不用绑在一起,同时建立。可以在IMU开启一小段时间以后,相机再获取它的世界坐标系。但相隔时间应该还是要尽量短点,因为位移单靠IMU的加速度计的积分,时间久了会很不准确,如果有轮子码盘的话,则又可以好一些。调整的目的,可能是因为世界坐标系和相机的世界坐标系,在时间戳上并不对应得很好,所以需要微调。
或者,为了避免这样的麻烦,直接把IMU的世界坐标系和相机的世界坐标系,绑在一起,同时建立。这样子,。
也应该用ETH的手眼标定方法,借用棋盘格先标定好,再固定住。
就直接用尺子量好,固定住。如果用双目相机的话,就不用考虑
了。
(如果加上轮速计的话,怎么更新呢?它的作用与IMU是相同的,都是相对值,而不是绝对值。所以,它应该与IMU实时融合,在主状态与相机融合后,IMU预测出一个位姿态,轮速计预测出一个位姿,然后两者融合出主状态。只有主状态才能与相机位姿融合。融合的话,可以用误差状态的思想来融合。)
所以,参考《误差状态四元数详细解读》,一个简单鲁棒的IMU与相机融合的模型应该是这样的。
核心状态为,。
核心状态上的扰动为,。
运动模型跟《误差状态四元数》里面的一样,
则相机位姿的观测方程为,
使用《误差状态四元数》里面的公式,
其中,
首先,要计算第一项关于的雅克比,
则用表示上式,则雅克比计算如下,
然后,第二项,要转成三个元素的形式,即角轴李代数的形式。
用matlab程序,解出上面的式子,以及关于的雅克比。
syms qicw qicx qicy qicz thetax thetay thetaz ...
qwiw qwix qwiy qwiz qzocw qzocx qzocy qzocz real
q1=[qicw,-qicx,-qicy,-qicz]';
q2=[1,-1/2*thetax,-1/2*thetay,-1/2*thetaz]';
q3=[qwiw,-qwix,-qwiy,-qwiz]';
q4=[qicw,qicx,qicy,qicz]';
q5=[qzocw,qzocx,qzocy,qzocz]';
temp = quaternion_mul(q1,q2);
temp = quaternion_mul(temp,q3);
temp = quaternion_mul(temp,q4);
temp = quaternion_mul(temp,q5);
temp
J=jacobian(2*temp(2:4,:),[thetax,thetay,thetaz])
simplify(J)
雅克比可以是,把temp转换成角轴,再关于求导。或者,角轴直接近似等于temp向量部分的2倍,再关于
求导,像上面的matlab程序这样。
这样子计算虽然准确,但是太麻烦了。论文里还用了近似的方法。为了方便地求雅克比,认为测量值近似为预测值直接转换出来,即,。
这时候,上式转换成角轴,就是,向量部分的2倍,即,。所以,雅克比为,
(这种用近似的方法,来算雅克比,虽然不如从原始公式上推导准确,但是可以极大地简化计算,也许可以给D-LG-EKF里面计算H矩阵时借鉴。)
所以,就得到了雅克比矩阵。
然后,。就可以计算了。其余的流程,就跟《误差状态四元数》里面一样了。
2.对延时的处理
前提条件是,各个传感器的时间戳得是同一个时间源的,或者,时间戳很稳定,可以通过一些方法把它们时间戳之间的对应关系找到。不同传感器的时间戳能准确对应上。
然后,因为测量值有时候会过一段时间才处理完,所以,把滤波中的状态都记录起来,然后,当有测量值过来的时候,更新对应时刻的状态。然后继续往后预测。如果有多个不同传感器的测量值,也是如此操作。
3.计算相对测量
如果要融合单目相机。考虑到单目尺度的情况,怕有时候尺度会突然发生变化。为了应对这种情况,就都计算相对测量,就是两个时刻之间的相对位姿态,这样子,这一段的尺度就可以滤得比较准确。然后,再把优化后的值,加入到原来的状态中,方法跟《误差状态四元数》中的传播差不多,就是把新滤出来的这段位姿的均值和扰动,加到原先状态的均值和扰动中去,整合出新的均值和扰动。
而如果要融合的是轮子码盘的话,则不必用这样的方法。因为虽然轮子码盘也有尺度问题,但是尺度是较稳定的。
如果是双目相机的话,也不必考虑这种尺度突然变化的情况。
4.准确地计算每帧的协方差
为了更好地与GPS融合,就需要当前状态需要有准确的协方差。
而以前计算出来的相机位姿的协方差是不准确的,没有考虑到累积误差造成的影响。造成了与IMU融合后的状态协方差也是不准确的。得准确地计算出要融合的每帧图像的协方差。这协方差,就是通过全局BA的方法,计算出来的。
但其实为了简化。如果真的要与GPS融合的话。
在即时建图的情况下,用上一段的方法。但是视觉的误差累积还是很可观的,所以如果是远距离的话,应该以GPS为外界绝对测量,视觉只是用来计算相对测量的。只能用短距离的视觉相对测量。在主状态之后,以此为起点,视觉的相对测量值与IMU的相对测量值融合,融合出相对测量状态,再把相对状态的均值和扰动合并进主状态以及主状态扰动,主状态再与GPS融合,融合出新的主状态。融合的话,可以用误差状态的思想来融合。
如果已经提前建图,回环检测都做了,地图点固定且准确了,则视觉协方差就是当前帧的协方差,不必通过全局BA算出。
5. MSF方法总结
MSF的方法考虑得很全面,这个理论框架,可以用来融合多种传感器。
我上面思考出来的方法,还考虑了与轮速计码盘,GPS融合的具体操作情况,以后需要时再用。
6.实验与改进
MSF的安装与跑例程,可以参考这篇文章,
然后,我自己用ORBSLAM2跑Euroc的V201数据集,生成轨迹数据,和IMU数据一起送入到MSF中运行。为什么要跑标准数据集呢?因为标准数据集提供了IMU噪声的真实参数,可以直接拿来使用,而且有真实的轨迹,groundtruth,可以用来评价融合结果的好坏。
可是运行结果却总是发散,融合后的轨迹锯齿非常严重。可是在理论上来讲,它应该能取得比较好的效果的呀?所以猜测应该是程序与理论没有对应上。
只好从程序里去查问题的原因。将程序里的所有的计算过程与算法公式一一对应起来之后,最终发现,是由程序里的2个地方导致的。
1.MSF程序有个隐含的假设,即图像的世界坐标系是水平的。而我送的是以第一帧为世界坐标系的,而V201的第一帧并不是水平的。
2. 经过仔细推导程序的计算过程,发现MSF程序中的qwv,本质上是qvw,这个导致参数的初值给错了。
将以上两个问题改过来之后,MSF就可以正常运行我自己提供的数据了。
其中,在y轴上的结果如下。
以上,黑色的线是真实值,绿色的线是ORBSLAM通过图像计算出来的位姿,红色的线是图像位姿和IMU融合后的结果,线上的每一个点都代表一个输出数据。可以看出,msf融合后的结果,不仅可以把位姿的输出频率提高到和IMU一样的频率,还可以让轨迹更加接近真实值。
但是,msf有一个缺点,那就是IMU的bias收敛得很慢,猜测是由于近似造成的。如下图所示。
上图是加速度计的bias的收敛情况,和陀螺仪的bias的收敛情况。也许可以通过修改这部分的公式,让bias收敛得更快。
7.参考文献
-
Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to MAV navigation[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2013:3923-3929.