基于多状态卡尔曼滤波的双目视觉导航设计

赵中堂，吴庆涛

(郑州航空工业管理学院智能工程学院， 郑州 450015)

状态与参数的联合估计在军事和民用方面均有十分广泛的运用。卡尔曼类滤波器，例如扩展卡尔曼、无迹卡尔曼、两阶段卡尔曼和鲁棒卡尔曼等滤波器，经常用于处理状态与参数联合估计的问题。

最常见的紧耦合视觉惯性导航是在滤波状态下增加三维特征位置，同时估计姿态和三维点，但该方法由于新的观测特征增加导致计算复杂度增加。为解决该问题，多状态约束卡尔曼滤波器(Multi-State Constrained Kalman Filter，MSCKF)通过滑动窗口代替估计状态向量中标志的位置，从而保持恒定的计算需求。与传统的扩展卡尔曼滤波(Extended KalmanFilter，EKF)视觉惯性导航方法相比，MSCKF方法可以在较大的导航范围内兼顾视觉惯性导航的效率和精度。在MSCKF的基础上，文献[1-2]对其进行了改进，提高了MSCKF的精度和在线校准性能。然而，由于状态更新滑动窗口的最小长度限制，MSCKF中的初始化问题仍未得到解决。文献[3]基于无迹卡尔曼滤波器实现相机与IMU(Inertial Measurement Unit)之间的自校准，传感器间耦合度低，只适用于单目视觉。通常情况下，为了实现高质态初始化，传感器模块最好保持静止一段时间。为提高MSCKF的初始化性能，本文提出了一种基于三焦点张量约束的初始化MSCKF方法。该方法将连续三幅图像之间的约束条件应用于初始状态估计，同时完成姿态计算，以满足MSCKF的最小滑动窗口约束。此外，为了提高系统的初始化精度，采用Sigma滤波器对非线性系统进行逼近，可以有效改善算法鲁棒性和初始化精度，进而使得双目视觉惯性导航具有更优异的性能。改进的MSCKF算法结合三焦点张量和Sigma滤波器进行初始化并对后续状态估计进行无缝切换，进而通过双目视觉惯性导航输出状态估计，其实现框图如图1。

图1 视觉惯性导航实现框图

1 状态估计和MSCKF

1.1 IMU传播模型

从IMU测量中进行状态传播，本文的IMU状态参数取为16维矢量，即

(1)

利用状态的连续时间动力学，可得：

(2)

其中，Ga是全局帧的加速因子；nbω和nba分别表示偏差bω和ba的高斯游走过程；Ω(ω)表示ω的四元数乘法矩阵，即

(3)

在惯性传感器中给定陀螺仪参数，其输出包含给定的偏差bω和ba以及高斯白点噪声nω和na。通过从陀螺仪中测得的参数ωm和am，可以由下式计算得到实际的角速度ω和实际的加速度a，即

(4)

根据式(2)，可得连续时间模型为

(5)

(6)

GδθI是误差四元数δq的角度偏差，15维IMU误差通过下式计算获得。

(7)

1.2 状态传播

与视觉辅助惯性EKF融合方法不同[5]，MSCKF算法不向状态向量添加新特征，当特征值失效或者附加位姿达到最大阈值时，执行更新程序。换句话说，MSCKF的状态向量保持了相机姿态的滑动窗口，如图2所示，n个摄像头姿态和m个特征彼此可见。因此，滑动窗口中的所有相机位姿受特征fi(i=1,2,…,m)约束。

图2 MSCKF约束框架示意图

在双目视觉惯性导航中，MSCKF的主要目的是估计IMU的全局方位和位置，通过与IMU的刚性连接可以得到相机的状态。在k时刻，全状态向量包括当前IMU状态估计，全状态向量表示为

(8)

(9)

(10)

在滤波器预测中，因为IMU的状态传播测量值是通过对其进行离散化获得，所以可以认为陀螺仪和加速度计的信号为一定时间间隔的采样值[6]。结合式(2)和式(5)，IMU的运动学误差可表示为

(11)

因此，IMU状态误差的线性化连续时间模型表示为

(12)

(13)

(14)

其中，I3表示维度为3×3的单位矩阵。

1.3 状态增强

(15)

假设MSCKF的状态已经包含相机的n个位姿，则第n+1个状态的协方差矩阵pk|k为

(16)

其中，I6n+15是(6n+15)×(6n+15)为单位矩阵，根据式(15)可得雅各布式Jk为：

(17)

1.4 协方差传播

(18)

考虑到计算的复杂性，采用欧拉积分法对采样时间Δt进行积分。因此，离散误差状态变换矩阵Φ(tk+Δt,tk)为

Φ(tk+Δt,tk)=I15+FΔt

(19)

(20)

1.5 状态修正

(21)

(22)

根据状态中的各个位姿的观测特征，计算出的测量误差为

(23)

2 三焦点张量约束

2.1 对极几何约束

图3 两视图对极几何约束基本模型示意图

2.2 三焦点张量

在对极几何约束下，三焦点张量涵盖三种不同视图之间的几何关系，如图4所示。它可以将两个视图中的对应点转换为第三个视图中的对应点[9]。分析两个视图中的极线约束，三焦点张量的本质是三维空间中点-线-点对应的几何约束，是一种对位姿的强约束。

图4 三焦点张量之间的几何关系示意图

因此，为了提高MSCKF的初始鲁棒性，基于三焦点张量的状态矢量表示为

(24)

(25)

2.3 初始状态更新

相机三视图投影矩阵记为：p1=[I|0Z,p2=[A′|a4],p3=[B′|b4]，其中，A′和B′分别为3×3矩阵，向量a4和b4分别表示各自相机的投影矩阵[10]。因此，根据投影矩阵和三维空间中的直线，三焦点张量标表示为

(26)

(27)

由于三焦点张量主要集中在三个连续帧图像上，假设三帧图像的对应特征点为{m1,m2,m3}，根据极限约束和三焦点张量约束可以推导出测量模型。因此，三帧图像之间的两个测量模型为

(28)

其中，(h1,h2)通过极限几何计算获得，然后根据IMU获取观测位姿(GRC,GpC)。

综上所述，总测量值zin表示为

(29)

3 Sigma滤波器

(30)

(31)

其中，λ=∂2(n+k)-n；∂为大于0的小常量[13]，本文取值0.2；k取值为3-n；高斯分布ρ=2。

4 实验结果与分析

算法运行平台为配备Intel Core i7处理器，内存为8G的笔记本电脑，实验数据为MH_05_difficult数据文件。

图5、图6分别表示了无障碍物情况和有障碍物情况下两种算法的轨迹跟踪误差，纵坐标为轨迹横向吻合误差，横坐标表示轨迹延伸方向。从图5可以看出本文算法采用三焦点张量有效实现初始化和后续导航之间的无缝过渡，跟踪效果良好；从图6可以看出本文算法在MSCKF基础上融合Sigma滤波器，有效提高轨迹估计的精度和跟踪鲁棒性。

图5 无障碍物轨迹跟踪误差

图6 有障碍物轨迹跟踪误差

由于系统引入了惯性元件，且通过Sigma滤波器的方法有效抑制了测量误差，双目视觉的初始位姿更加稳定，可以有效估计目标轨迹与障碍物的相对位置。预设100个停车点位，图7和图8分别位置和角度的跟踪估计误差，位置误差维持在±1 mm，角度误差维持在±1°，纠偏响应很快。

图7 位置跟踪估计误差

图8 角度跟踪估计误差

5 结论

1) 针对MSCKF的初始化阶段的缺陷，采用三焦点张量和Sigma滤波器优化初始化状态，无需计算实际场景中的三维点，直接通过连续帧图之间的约束条件实现初始化估计；

2) 利用Sigma滤波器对非线性系统进行逼近，既实现和后续导航之间的无缝过渡，又提高了系统状态估计的精度和鲁棒性；

3) 在工业双目视觉惯性导航领域具有应用前景。