MSCKF-Based Visual-Wheel Odometry 轮速视觉融合里程计

Sliding Windows Filter（SWF）在VIO、SLAM这个领域应用非常广，比如MSCKF、OKVIS、VINS-Mono等等，几乎可以说是VIO的标配。

SWF可以分成基于滤波器的和基于优化的两种。最典型的基于滤波器的方法就是MSCKF算法了。它是基于EKF的算法，在marginalize state的时候处理比较简单，只需要把对应的covariance的对应行列直接丢弃就可以了。而基于优化的方法在边缘化时需要对Hessian矩阵做舒尔补，操作会复杂一些。

为了尝试一下SWF,我们先从简单的基于滤波的方法入手.本文实现了一个基于MSCKF的Visual+Wheel融合的Odometry.

下面是分别是仿真和用KAIST数据测试的结果

更多可参考

https://zhuanlan.zhihu.com/p/270670373

注意：这里为了凸显加了视觉校正效果，把wheel的noise设的比较大，轨迹不太平滑。如果想要平滑一些，可以适当调低wheel的noise。

代码请见

https://github.com/ydsf16/TinyGrapeKit

传感器配置

Visual部分，用一个单目相机。Wheel部分可以是左右轮速度或位移

坐标系统

轮速坐标系/Odometry坐标系{0}: 车辆后轴中心、贴地. x轴向前, y轴向左,z轴向上.

全局坐标系{G}:与初始时刻的轮速坐标系重合的坐标系。

相机坐标系{C}: x轴向右,y轴向下,z轴向前

内外参数

内参数：1）相机内参；

2）左右轮轴距b，左右轮速系数、 将编码器count转成距离米，或者速度转成m/s。

外参数：轮速坐标系到相机坐标系的旋转与平移.

我们假设这些内外参数都假设预先标定好的。实际上，也可以把这些内外参数放到状态向量里面估计，这也是很多论文里面常见的做法，比如OpenVINS。但是，具体这些内外参数能不能估计出来，是不是可观的，可以参考一下黄国权老师的论文

状态向量

滑窗里面的状态分成两类，一类是Odometry的位姿。

另一个类是一串相机位姿：

总的状态是当前Odometry位姿+N帧的相机位姿：

跟MSCKF一样，我们把协方差分块表示：

这里我们使用最简单的滑窗维护方式，当新的一帧进到滑窗后，就直接把老的一帧给边缘化掉。因为是EKF，就是直接把最后一帧相机pose从x中去掉，然后把对应的协方差的行和列删除掉.

Wheel Propagation

EKF算法分成两步：Propagation+Update。在这里，我们用wheel的信息进行状态的propagation，用视觉信息做update。下面先推导Propagation部分。这里的推导，参考了Mingyang Li的paper[2]。

按照IMU处理的方法，首先把ODE方程列出来

下面就可以对ODE方程进行积分了。简单起见，直接用欧拉积分了。如果考虑精度，可以使用中值或者龙格库塔：

用这个式子，就可以进行均值的Propagation。对于协方差的Propgation，我们先求雅克比。这里定义旋转的Error为：

状态增广

当新来一帧图像，可以通过odometry位姿，计算出相机位姿。

然后把它放到状态向量里面。相应的要把协方差矩阵进行扩展：

Update

7.1视觉Update

MSCKF的很大的优势就是没有把特征点放到状态向量里面，降低了计算量。

当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：

• 当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
• 另一种是边缘化的时候，也就是把最后一帧滑出窗口的时候，把在这一帧里面新建的特征点都丢弃，也就是都拿来做更新。

这里说的特征点/feature，同时表示3D点，也表示在图像上的2D位置。

A. 一个特征点的处理

因为与是正交的.所以

做了上面的操作之后，就可以得到一个不含特征点的线性方程：

B. 多个特征点的处理

上面一个特征点可以得到一个（18）式。多个特征点就可以得到很多的（18）式，把他们堆叠起来，有得到一个很大的线性方程：

这个方程的行数一般很大，直接用来做EKF更新效率很低。MSCKF又用QR分解，进行了一次压缩。具体的对H*做QR分解

带入到(19)式中，可以得到，

左右两边，同时乘以有

最后，我们得到一个压缩后的线性方程

这方程的行数最大和状态的维度相同。最终用来做EKF更新的也就是（20）式。

C. 边缘化

边缘化，或者说如何删除滑窗里的状态。前面也已经提到了，我们使用了最简单的策略。就直接把最老的一帧去掉。去掉的这帧里的所有特征点都被拿来做更新。

最后，总结一下，当一帧图像来了之后的处理步骤

• 增广状态
• 做特征点跟踪。
• 收集跟踪丢失的的所有特征点{}
• 收集将要被边缘化的帧中的所有特征点：{}
• 利用特征点{}和{}构造线性方程(20)，并执行EKF更新。
• 边缘化操作：将x中边缘化掉的pose去掉，将协方差矩阵中对应的行和列删除。

平面约束Update

一般车辆都是运动在平面上的，在更新的时候，我们引入一个平面约束。这部分参考了Stergios I. Roumeliotis 的PaperVINS On wheels[3]

实现

全部工程代码请见：

https://github.com/ydsf16/TinyGrapeKit

实验

仿真测试,很明显,VWO-MSCKF比纯Wheel里程计精度更好.

数据集测试

我们这里使用了KAIST数据集，链接是：

https://irap.kaist.ac.kr/dataset/

同样的,相比纯Wheel odom,精度会有所提高.

注意:这里的测试,Wheel内参数精度都是比较低的,所以raw odom的精度不是很高.如果wheel内参数精度很高的话,VWO-MSCKF精度不一定更高.

本文仅做学术分享，如有侵权，请联系删文。