相机IMU融合四部曲（三）:MSF详细解读与使用

相机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的安装与跑例程，能够参考这篇文章，

http://www.liuxiao.org/2016/07/ros-%E5%A4%9A%E4%BC%A0%E6%84%9F%E5%99%A8%E5%8D%A1%E5%B0%94%E6%9B%BC%E8%9E%8D%E5%90%88%E6%A1%86%E6%9E%B6-ethzasl-msf-framework-%E7%BC%96%E8%AF%91%E4%B8%8E%E4%BD%BF%E7%94%A8/。

而后，我本身用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.参考文献

1. 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.