一种单目视觉信息辅助惯性导航算法

王晨琳，刘海颖，蒋 鑫

(南京航空航天大学 航天学院，南京 210016)

0 引言

自主导航有着突出的环境适应能力，在各个领域有着广阔的应用需求[1-2]。单一的惯性导航系统由于误差积累的特性不利于长时间的自主导航，解决这一问题的根本办法是采用高精度的惯性传感器，但是昂贵的成本有限制了其实用性。视觉导航方法由于其自主性、廉价性成为导航策略领域的研究热点，用视觉辅助惯性导航是一种很好的解决方案。

近年来，视觉辅助导航算法被应用于不同的场景。文献[3]将图像辅助导航用于飞行器着陆的情况做了可行性分析。文献[4]针对航天器交会对接的情境，利用双目视觉的技术，运用对偶四元数建立了2个航天器之间描述相对位姿参数的模型。文献[5]提出了一种基于空间关系几何约束的无人机景象匹配导航方法。在视觉信息和惯性传感器信息融合的算法中，现阶段普遍采用扩展卡尔曼滤波(extended Kalman filter，EKF)。文献[6]基于EKF算法，提出了一种视觉辅助惯性测量单元(inertial measurement unit，IMU)的无人机导航方法。文献[7]在飞行器在大海拔变化的场景下，研究了基于单目EKF的同步定位和制图(simultaneous location and mapping，SLAM)性能。文献[8]设计了一种多模态数据融合的EKF算法，用于结合视觉、激光、惯性导航和里程计的信息。根据视觉信息和惯性传感器信息的耦合程度可以分为松耦合和紧耦合。文献[9]针对融合视觉里程计和惯性导航系统的方案，设计了一种松耦合的间接反馈型卡尔曼滤波器。文献[10]提出了一种紧耦合的EKF算法用于视觉惯性导航组合的相对位姿测量，并与松耦合的EKF算法进行了比较。

为了充分利用所有导航子系统的信息资源，提高组合导航的滤波效率，选取惯性传感器的高采样率作为滤波器的采样周期，低采样率的相机传感器数据作为量测信息，在此基础上提出了一种多信息多速率融合的无损卡尔曼滤波(unscented Kalman filter,UKF)算法。

1 多速率数据融合的设计方案

一般情况下，卡尔曼滤波器的状态更新和量测更新在同步进行，这就需要在多传感器的情况下进行数据同步，舍去高采样率传感器的大部分数据。这个过程中人为地不可避免地降低了高采样率传感器的性能，滤波周期变长，见图1所示。

多传感器数据采样率不一致的情况如图2所示，滤波器首先判断相机传感器是否有采集图像，是否输入了图像数据并通过算法验证图像的质量。如果没有图像数据或者图像质量差，滤波器只做状态一步预测，将状态估计值作为输出。如果图像数据满足条件，则滤波器在空间上进行数据融合，当视觉数据的采样周期不固定以及视觉数据的采样点出现异常丢失时，滤波器也可以继续运行，这样提高了整个系统的冗余性。这种方案解决了组合导航系统中某一子系统测量失败导致的滤波性能下降的问题。

多速率数据融合算法流程如图3所示：首先，滤波器根据惯性传感器输入的加速度和角速度积分，获得系统状态的估计值；第二步是判定阶段，滤波器判断是否有图像数据输入，并且检验图像质量。由尺度不变特征变换(scale-invariant feature transform，SIFT)算法获取的特征点通过随机抽样一致算法(random sample consensus，RANSAC)[11]来进行匹配，通过匹配点的数量衡量图像质量，一方面有效地剔除了误匹配点，另一方面能够控制特征点的数量；第三步，根据系统状态的估计值获得载体运动信息，构建三焦点张量，将匹配特征点通过三焦点张量进行转移得到特征点位置的估计值；第四步，根据图像中的特征点位置的实际测量值和估计值校正系统状态量。

2 多速率数据融合的UKF算法

2.1 滤波器的状态预测阶段

(1)

式中：R为3×3的正交单位阵；T为三维平移向量；xp1，yp1，zp1为p点的位置坐标；xp2，yp2，zp2为p点经坐标系变换后的位置坐标。

根据三焦点张量的点转移确定状态向量，包含当前时刻t1载体运动状态和前2个时刻t2、t3载体的位姿为

(2)

结合动力学方程得到连续系统的状态方程为

(3)

式中：am、ωm分别表示加速度计和陀螺仪的测量值；ba和bω是加速度计和陀螺仪的零偏；na和nω是加速度计的陀螺仪高斯噪声；g代表当地的重力加速度；符号⊗代表四元数乘法。

(4)

间接法中连续系统的状态方程为

(5)

(6)

(7)

最后计算一步预测均方误差方程为

Pk|k-1=FdPk-1FdT+Qd

(8)

2.2 滤波器的测量更新阶段

单目相机提供的是以时间为标准的一连串图像序列信息，提取同一特征点在不同的图像中像素坐标作为观测量。在2个视图之间，相同的特征点通过基础矩阵联系起来，是一种极线约束。在三视图的情况下，相同的特征点通过三焦点张量联系起来，这是几何约束从二视图到三视图的扩展。三焦点张量可以由惯性导航系统的位姿解算得到，将得到的三焦点张量来预测第三幅图像中的特征点的像平面坐标,特征点转移的过程见图4所示。

首先确定3幅图像所在时刻t1、t2、t3，通过IMU数据预测得到3个时刻载体的绝对位姿。通过坐标系转化关系由IMU的绝对位姿转化为相机坐标系的绝对位姿。可以得到包含相机外参数的旋转矩阵R和平移向量T，从而得到本质矩阵

E=[T]XR

(9)

结合事先标定好的相机内参数矩阵K可以得到相机的基础矩阵F为

E=KTFK

(10)

令两两视图之间的基础矩阵记为F12和F23，二视图之间有如下对极几何的约束关系为

(11)

第一步，计算垂直于x1的对极线Le2=F12x；

第二步，计算过点x2并垂直于Le2的直线L2

(12)

第三步，计算转移点x3=[Γ1T,Γ2T,Γ3T]L2x1。

根据多视图几何约束得到的量测方程为

z(k)=[x1F12x2,x2F23x3,
[Γ1T,Γ2T,Γ3T]L2x1]T

(13)

最后计算滤波增益，更新系统状态

(14)

3 实验结果

本次实验采用自驾车釆集的实测数据验证算法的有效性。自驾车配备有全球定位系统(global positioning system，GPS)及IMU组合导航系统。IMU的型号为OXTS RT 3003,采样率是100 Hz。其中，陀螺仪的量程为±300(°)/s，角速度偏差为2(°)/h；加速度计的量程为±300 m/s2，加速度偏差为0.02 m/s2。灰色摄像机的数据经过校正后的图像分辨率为1 226个像素乘370个像素，图像采样率大致为10 Hz。

实验数据采用MATLAB进行仿真，选取2组有代表性的数据，第一组场景是在市区马路上，速度较慢。第二组场景选择在郊区的公路上，速度较快。市区低速场景的结果如图5和表1所示。

在市区低速场景下，单一的惯性导航系统误差积累速度很快，35 s以后位置误差达到了25 m。在加入视觉辅助信息后，将相机数据和惯性传感器数据同步采样率，位置误差积累速度明显降低。在35 s以后位置误差仅有5.3 m，明显优于单一的惯性导航系统，大大提高了定位精度。在单速率的情况下，由于现实场景中车辆颠簸，光照等客观因素的影响，相机采样率具有局限性和不确定性，无法达到惯性导航器件的高稳定和高采样率的水平。所以，在2种数据的同步中必须舍去大部分的惯性传感器的数据。并且如果相机出现故障，滤波器就无法工作，不利于系统的鲁棒性。多速率数据融合的算法融合了所有传感器的数据，将惯性传感器采集的高速率数据全部用于定位解算，提高了数据的利用率。多速率数据融合算法在全程的位置误差最大值为3.1 m，位置均方根误差相比于单速率算法更低，位置估计的稳定性也更高。在速度估计方面，由于滤波器估计速度来源于惯性传感器的信息，多速率算法的采样率远远高于单速率，得到的速度精度也更高，稳定性也更好。

数据处理方法位置误差最大值/m位置均方根误差/m速度误差最大值/(m·s-1)速度均方根误差/(m·s-1)单一IMU数据24.879 611.180 2单速率数据融合5.329 73.934 20.776 30.342 6多速率数据融合3.126 71.481 90.376 20.180 2

郊区高速场景的实验结果如图6和表2所示。

数据处理方法位置误差最大值/m位置均方根误差/m速度误差最大值/(m·s-1)速度均方根误差/(m·s-1)单一IMU数据32.225 016.832 0单速率数据融合29.588 113.849 32.002 00.945 8多速率数据融合14.569 87.493 61.207 60.487 2

在郊区高速场景下，单一的惯性导航系统表现良好。因为在高速情况下，测量数据是高动态的，惯性传感器的动态性能优越。单速率数据融合算法在位置和速度的解算结果上比单一的惯性导航系统效果略好，虽然一开始误差比较大，但是后期行程中误差呈下降趋势，有效地抑制了误差的扩散。多速率数据融合算法在位置和速度估计上明显优于前面2种算法，充分利用了高速率的惯性传感器的数据和视觉校正信息。

4 结束语

本文利用相机提供的视觉信息，提出了一种基于UKF的多速率数据融合算法，用于辅助惯性导航。该算法不需要同步所有传感器的采样率，一方面充分利用了所有传感器数据，提高了信息利用率，另一方面在视觉辅助信息不足时滤波器能够正常工作，提高了系统冗余性。通过采集的实测数据验证，这种视觉信息辅助惯性导航方法有效地抑制了惯性器件带来的定位误差，提高了导航的定位精度。