相机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.

相机IMU融合四部曲(三):MSF详细解读与使用的更多相关文章

  1. 相机IMU融合四部曲(一):D-LG-EKF详细解读

    相机IMU融合四部曲(一):D-LG-EKF详细解读 极品巧克力 前言 前两篇文章<Google Cardbord的九轴融合算法>,<Madgwick算法详细解读>,讨论的都是 ...

  2. 相机IMU融合四部曲(二):误差状态四元数详细解读

    相机IMU融合四部曲(二):误差状态四元数详细解读 极品巧克力 前言 上一篇文章,<D-LG-EKF详细解读>中,讲了理论上的SE3上相机和IMU融合的思想.但是,还没有涉及到实际的操作, ...

  3. 详细解读Volley(三)—— ImageLoader & NetworkImageView

    ImageLoader是一个加载网络图片的封装类,其内部还是由ImageRequest来实现的.但因为源码中没有提供磁盘缓存的设置,所以咱们还需要去源码中进行修改,让我们可以更加自如的设定是否进行磁盘 ...

  4. VINS-mono详细解读

    VINS-mono详细解读 极品巧克力 前言 Vins-mono是香港科技大学开源的一个VIO算法,https://github.com/HKUST-Aerial-Robotics/VINS-Mono ...

  5. Madgwick算法详细解读

    Madgwick算法详细解读 极品巧克力 前言 接上一篇文章<Google Cardboard的九轴融合算法>. Madgwick算法是另外一种九轴融合的方法,广泛应用在旋翼飞行器上,效果 ...

  6. 相机-imu外参校准总结

    1. 研究背景及相关工作 1)研究背景 单目视觉惯性slam是一种旨在跟踪移动平台的增量运动并使用来自单个车载摄像头和imu传感器的测量结果同时构建周围环境地图的技术.视觉相机和惯性测量单元(imu) ...

  7. SVO详细解读

    SVO详细解读 极品巧克力 前言 接上一篇文章<深度滤波器详细解读>. SVO(Semi-Direct Monocular Visual Odometry)是苏黎世大学Scaramuzza ...

  8. MemCache超详细解读

    MemCache是什么 MemCache是一个自由.源码开放.高性能.分布式的分布式内存对象缓存系统,用于动态Web应用以减轻数据库的负载.它通过在内存中缓存数据和对象来减少读取数据库的次数,从而提高 ...

  9. MemCache超详细解读 图

    http://www.cnblogs.com/xrq730/p/4948707.html   MemCache是什么 MemCache是一个自由.源码开放.高性能.分布式的分布式内存对象缓存系统,用于 ...

随机推荐

  1. Java API 操作Zookeeper

    一.依赖 <dependency> <groupId>org.apache.zookeeper</groupId> <artifactId>zookee ...

  2. Unit05: JavaScript对象概述 、 常用内置对象一 、 常用内置对象二 、 常用内置对象三

    Unit05: JavaScript对象概述 . 常用内置对象一 . 常用内置对象二 . 常用内置对象三 常用内置对象使用演示: <!DOCTYPE html> <html> ...

  3. 减少TIME_WAIT时间的优化配置

    减少TIME_WAIT时间的优化配置 建立TCP需要三次握手才能建立,而断开连接则需要四次握手.整个过程如下图所示: net.ipv4.tcp_max_syn_backlog=8192 增加TCP S ...

  4. 二、Jetty的配置说明

    运行Jetty Web应用 在Jetty应用服务器中部署war项目很简单,只需把项目war包放入Jetty的webapps子目录即可.你都无需重启Jetty,Jetty会自动随时监听webapps目录 ...

  5. webpack快速入门(一):安装

    webpack入门系列,从安装到使用再到放弃....全套的哟,呵呵 首先说明,我是一个后端开发,好几年没碰过前端了,想当年写前端的时候jq还是主流,那时的前端还单纯的像个处女一样,哪有什么构建工具.依 ...

  6. H3C IRF2的三种配置情况

    H3C-IRF2-虚拟交换技术-强列鼻视看我文档不下载的呵呵.看了下面的东西你就会配IRF了. H3C-5120-HI  H3C-5120S   H3C5800 先到H3C官网上面看那些交换机支持IR ...

  7. TCP三次握手和四次挥手、HTTP协议

    TCP三次握手和四次挥手 首先我们知道HTTP协议通常承载于TCP协议之上,HTTPS承载于TLS或SSL协议层之上 通过上面这张图我们能够知道.     在Http工作之前,Web浏览器通过网络和W ...

  8. 我编辑的JAVA日历程序

    class calendar { public static void main(String[]args) { int yearIn ; yearIn = Integer.parseInt(args ...

  9. Mac Terminal终端光标的快捷键操作

    2016年08月18日 18:26:06 阅读数:4217 Mac Terminal终端和linux上终端光标的快捷键操作是一样的,都是来自Emacs这个神级的编辑器,由于我以前vim用的多,没怎么用 ...

  10. Cobalt strike 第二节生成报告

    0x00前言: 上一节我们说了怎么连接到服务器 0x01生成报告: 首先打开Cobalt Strike 点击Cobalt Strike -> Preferences Preferences Pe ...