状态与参数的联合估计在军事和民用方面均有十分广泛的运用。卡尔曼类滤波器，例如扩展卡尔曼、无迹卡尔曼、两阶段卡尔曼和鲁棒卡尔曼等滤波器，经常用于处理状态与参数联合估计的问题。
最常见的紧耦合视觉惯性导航是在滤波状态下增加三维特征位置，同时估计姿态和三维点，但该方法由于新的观测特征增加导致计算复杂度增加。为解决该问题，多状态约束卡尔曼滤波器(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)
其中，
表示从全局帧{G}到IMU帧{I}的旋转四元数；GvI和GpI分别表示IMU在全局帧中的速度和位置；bω和ba分别表示陀螺仪和加速度计的偏差[4]。
利用状态的连续时间动力学，可得：
(2)
其中，Ga是全局帧的加速因子；nbω和nba分别表示偏差bω和ba的高斯游走过程；Ω(ω)表示ω的四元数乘法矩阵，即
(3)
在惯性传感器中给定陀螺仪参数，其输出包含给定的偏差bω和ba以及高斯白点噪声nω和na。通过从陀螺仪中测得的参数ωm和am，可以由下式计算得到实际的角速度ω和实际的加速度a，即
(4)
根据式(2)，可得连续时间模型为
(5)
其中，
表示四元组
的旋转矩阵；g表示全局帧{G}的重力矢量，四元数误差表示为
(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)
其中，xIk表示IMU状态向量在k时刻的值；
表示相机的方向和位姿的估计值。因此，MSCKF的状态误差向量可以表示为
(9)
状态协方差
为(15+6n)×(15+6n)维矩阵，其可进一步表示为
(10)
其中，
表示IMU当前状态的协方差矩阵，维度为
表示相机状态估计的协方差矩阵，维度为
表示IMU状态和相机状态的互相方差。
在滤波器预测中，因为IMU的状态传播测量值是通过对其进行离散化获得，所以可以认为陀螺仪和加速度计的信号为一定时间间隔的采样值[6]。结合式(2)和式(5)，IMU的运动学误差可表示为
(11)
因此，IMU状态误差的线性化连续时间模型表示为
(12)
其中，
为噪音向量；F表示连续时间模型状态误差过度矩阵；G为噪声系数矩阵。F和G的表达式分别为
(13)
(14)
其中，I3表示维度为3×3的单位矩阵。
1.3 状态增强
当双目相机传来新数据时，MSCKF状态应添加当前相机位姿。IMU与相机之间的校准变换记为
求解的相机的位姿
为
(15)
其中，
由状态向量获得；
是四元素
的旋转矩阵。
假设MSCKF的状态已经包含相机的n个位姿，则第n+1个状态的协方差矩阵pk|k为
(16)
其中，I6n+15是(6n+15)×(6n+15)为单位矩阵，根据式(15)可得雅各布式Jk为：
(17)
1.4 协方差传播
IMU状态和相机姿态之间的协方差矩阵
在IMU采样周期Δt内通过乘以误差状态变换矩阵Φ(tk+Δt,tk)来传播[7]。因此，相机的状态协方差和IMU相机交叉关系为
(18)
考虑到计算的复杂性，采用欧拉积分法对采样时间Δt进行积分。因此，离散误差状态变换矩阵Φ(tk+Δt,tk)为
Φ(tk+Δt,tk)=I15+FΔt
(19)
进而可得IMU的预测协方差矩阵
为
(20)
1.5 状态修正
以跟踪特征fj作为更新状态，通过观测相机位姿
利用反深度最小二乘法估计其3D位姿Gpfi，各个观测特征可以表示为
(21)
其中，
表示图像噪声向量；特征Cipfi对应的位姿可以表示为
(22)
根据状态中的各个位姿的观测特征，计算出的测量误差为
(23)
2 三焦点张量约束
2.1 对极几何约束
两帧连续图像之间的变换可以通过对极几何来描述，其示意图如图3[8]。其中，ck-1是图像中的像点，X是场景中的三维点，通过小孔相机模型可知假如从像点m1向相机的中心ck-1反投影一条射线ck-1m1，则该射线必定经过对应像点的三维空间点X，但显然仅仅通过一个像点无法确定X的具体位置，因为在射线
上的任意空间点都可以通过“小孔”映射为像点m1。所以，设m2是三维点X的另一个像点，其对应相机的中心为ck，那么从像点也反投影一条射线ckm2，并且该射线也必定经过X，也就是说从一对匹配的像点反投影两条射线，必定相交于空间三维点X。
图3 两视图对极几何约束基本模型示意图
2.2 三焦点张量
在对极几何约束下，三焦点张量涵盖三种不同视图之间的几何关系，如图4所示。它可以将两个视图中的对应点转换为第三个视图中的对应点[9]。分析两个视图中的极线约束，三焦点张量的本质是三维空间中点-线-点对应的几何约束，是一种对位姿的强约束。
图4 三焦点张量之间的几何关系示意图
因此，为了提高MSCKF的初始鲁棒性，基于三焦点张量的状态矢量表示为
(24)
其中，
和
为根据三焦点张量约束推出的最后两个相机的位姿。式(24)中的初始状态矢量与式(8)一致，因此初始状态误差可表示为
(25)
2.3 初始状态更新
相机三视图投影矩阵记为：p1=[I|0Z, p2=[A′|a4], p3=[B′|b4]，其中，A′和B′分别为3×3矩阵，向量a4和b4分别表示各自相机的投影矩阵[10]。因此，根据投影矩阵和三维空间中的直线，三焦点张量标表示为
(26)
设三矩阵集合
构成三焦点张量，其中，
为双目相机内参数，通过三焦点张量可以使得
沿直线转移到
同理也可以转移到第三帧的
取极限
其中，
是前两个视角图像之间的基本矩阵。点
位于l2上，l2垂直于le，根据三焦点张量原理，第三帧中的对应点
为
(27)
由于三焦点张量主要集中在三个连续帧图像上，假设三帧图像的对应特征点为{m1,m2,m3}，根据极限约束和三焦点张量约束可以推导出测量模型。因此，三帧图像之间的两个测量模型为
(28)
其中，(h1,h2)通过极限几何计算获得，然后根据IMU获取观测位姿(GRC,GpC)。
综上所述，总测量值zin表示为
(29)
3 Sigma滤波器
双目视觉惯性导航作为一种非线性系统，如果初始化阶段不够准确则会导致后续误差不可控。因此，通过Sigma滤波器保证初始化阶段的位姿估计精度[11-12]。根据状态协方差矩阵Pk|k-1获得Sigma点
即
(30)
其中，初始状态矢量X0的维度为27，则Sigma滤波器的权重
和
表示为
(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) 在工业双目视觉惯性导航领域具有应用前景。
[1]LI M， MOURIKIS A I.Improving the accuracy of EKF-based visual-inertial odometry[C]//IEEE International Conference on Robotics & Automation.IEEE，2012:828-835.
[2]LI M， MOURIKIS A I.Online temporal calibration for camera-IMU systems:Theory and algorithms[J].The International Journal of Robotics Research，2014，33(7):947-964.
[3]KELLY J，SUKHATME G S.Visual-inertial sensor fusion:Localization，mapping and sensor-to-sensor self-calibration[J].Int J Robot Res,2011;30(1):56-79.
[4]梁龙凯，张丽英，何文超,等.概率神经网络多模型卡尔曼滤波定位导航算法[J].电子技术应用，2018，044(006):60-62,67.
[5]张华倩.卡尔曼滤波在车载组合导航中的应用研究[D].沈阳：沈阳工业大学，2017.
[6]于婷,徐爱功,付心如,等.一种自适应卡尔曼滤波组合导航定位方法[J].导航定位学报,2017,5(03):101-104.
[7]付心如,孙伟,徐爱功,等.组合导航抗差自适应卡尔曼滤波[J].测绘科学,2018,43(01):6-10，53.
[8]王敬东,薛重飞,魏雪迎,等.三焦点张量重投影视频稳像算法[J].中国图象图形学报,2017,22(07):935-945.
[9]魏雪迎，王敬东，王崟，等.基于特征点轨迹增长的视频稳像算法[J].红外技术，2019，41(02):87-92.
[10]张振杰,李建胜,赵漫丹,等.基于三视图几何约束的摄像机相对位姿估计[J].浙江大学学报(工学版),2018,52(01):151-159.
[11]SHARMA R，PANIGRAHI R K.Stokes based sigma filter for despeckling of compact PolSAR data[J].IET Radar，Sonar & Navigation，2018，12(4):475-483.
[12]曹倩霞，罗大庸，王正武.基于改进Sigma-Delta滤波的复杂场景背景估计[J].计算机工程，2014，40(9):225-228.
[13]SARKKA S.On Unscented Kalman Filtering for State Estimation of Continuous-Time Nonlinear Systems[J].IEEE Transactions on Automatic Control，2007，52(9):1631-1641.