惯性/双目视觉里程计深组合导航方法

逯建军,任晓军，孙 伟，郭元江，李 群

(1.海军装备部，北京 100000；2.海军驻航天某院军代表室，北京 100000； 3.北京自动化控制设备研究所，北京 100074)

惯性/双目视觉里程计深组合导航方法

逯建军1,任晓军2，孙 伟3，郭元江3，李 群3

(1.海军装备部，北京 100000；2.海军驻航天某院军代表室，北京 100000； 3.北京自动化控制设备研究所，北京 100074)

针对现有视觉里程计测量噪声大、匹配精度低、实时性差的问题，研究一种基于Kalman滤波器的惯性/双目视觉里程计组合导航方法。在视觉里程计中引入惯性导航信息，辅助完成实时图截取、搜索区预测、输出速度校正等功能，提高视觉里程计的测量精度与计算速度。利用Kalman滤波器，实现视觉里程计对惯导累计误差的修正，提升组合导航系统的导航精度。车载试验结果表明，惯性/双目视觉里程计深组合导航的实时定位精度优于0.5%D(CEP)，具备工程应用条件。

双目视觉里程计；惯性导航；组合导航

0 引言

随着飞行器长时间、远程航行需求的不断增强，对导航系统的精度和自主性要求不断提高。纯惯性导航系统的导航误差随时间积累而不断增大，不能完全满足实际应用的需要[1-5]。惯性/双目视觉里程计深组合导航可在不提高惯性元件精度的情况下，提升导航系统的导航精度，节约成本。

视觉里程计技术的基本内涵[6]为：单目相机通过前后帧图像的差异，在已知距离的情况下可以测量出载体的速度。而双目视觉里程计[7-9]技术利用双目相机，通过左右两个相机图像差异测出距离，通过前后帧图像的差异测量出载体的速度。

传统的视觉里程计技术存在缺陷，受相机参数、测量距离的范围等影响，视觉里程计输出的速度信息与真实值将会存在一定的差异；并且，传统视觉里程技术在实时性和精确性方面难以同时满足组合导航的需求。

本文针对视觉里程计的技术特点，在求取图像差异(图像匹配)的过程中使用惯导系统的信息进行深组合，可以同时提升双目视觉里程计的实时性和精确性。应用组合导航技术时，利用Kalman滤波器对惯性、双目视觉里程计的误差同时进行估计，实现深组合导航。

1 双目视觉里程计算法

1.1 图像匹配算法

本文使用的图像匹配算法为归一化灰度值匹配法，其本质是衡量两幅图像相似程度的度量准则；其过程是在两幅图像中寻找相关性最高的点作为匹配结果。

归一化灰度值匹配算法的计算公式[10]为

(1)

图像匹配算法可以获得同一标志点在不同图像中的相对偏移量，由相对漂移量可推算出在现实场景中的偏移量。

1.2 双目视觉测距算法

双目测距，基于空间上存在偏移的双目图像，完成相机到匹配物体(左、右图像中均存在)的距离测量。测距原理示意图，如图1所示。

图1 双目测距原理Fig.1 The theory of ranging method by using two cameras

图1中，Q是待测距点(某匹配点)，其到相机的垂直距离为R，在左右相机上形成的像点分别是Q1和Q2。b是双目相机的基线长度，f是相机焦距，x1-x2是Q点在左右两幅图像上像点的位置差，又称为视差。

利用相似三角形原理，可得相机到Q点的距离为

(2)

由式(2)可知，利用上述方法的测距分辨率受到相机焦距和双目相机基线长度的影响，相机焦距与基线长度越短，测距分辨率越高，但会导致测距范围的降低，需要根据实际使用条件来确定相机焦距和基线长度。

1.3 视觉里程计算法

视觉里程计的基本原理如图 2所示：载体t1时刻位于点O1，t2时刻运动到O2。在全局世界坐标系中，载体位置和姿态的变化可以由六个参数(Δx,Δy,Δz,Δα,Δβ,Δγ)表示，前三个参数代表位置的变化，后三个参数代表姿态的变化。如果载体在移动过程中每一帧都能确定其与前一帧时的位置和姿态 ，那么就自然实现了载体的定位。对全局坐标系中一点A，载体在t1时刻观察到的A的坐标为(x1y1z1)T，t2时刻观察到的坐标为(x2y2z2)T，假如载体姿态发生了变化，也就是代表姿态的航向角、滚转角和俯仰角发生了变化。此三个角度的变化(Δα,Δβ,Δγ)T产生了维数为3×3旋转矩阵R，位置变化(Δx,Δy,Δz)T产生了平移向量T，则有：

(3)

图2 视觉里程计基本原理Fig.2 The basic theory of visual odometry

如果能在相邻两帧中实现像素点的跟踪，即对前一帧图像中的像素点，在下一帧图像中能找到它的对应点，并利用立体视觉分别计算出此像素点在前后两帧中的三维坐标，则根据式(3)，一对对应的特征点就能产生三个方程。这样，只要存在4个对应的特征点，就能计算出旋转矩阵R和平移矩阵T。如果特征点的数目大于4个，则需要通过多个方程求得(R,T)的最优值。

2 惯性辅助视觉里程计方法

惯性导航技术基于牛顿力学原理，使用陀螺和加速度计测量载体运动的角速度和加速度，获取载体的姿态、速度和位置信息，其优点是短时精度高，能够准确地测量载体姿态变化的情况等。

本文考虑利用惯性导航技术的优点，以提升视觉里程计性能为目的，提出几种方法。

2.1 视觉里程计实时图截取方法

为了保证视觉里程计测量的速度范围达到最大，希望截取的实时图像在当前帧图像中的位置坐标点与在上一帧图像中的位置坐标点的连线的中点位于基准图的中心位置坐标。本方法主要介绍应用于视觉里程计的实时图中心坐标计算，为便于分析，将分成两种情况进行讨论：

(1)无航向角变化情况

截取的实时图像中心在当前帧图像中的坐标计算公式如下所示。

(4)

式中，ureal_m、vreal_m表示截取的实时图像中心在当前帧图像中的坐标；unow_m、vnow_m表示当前时刻图像中心坐标；duimu、dvimu表示利用惯性信息计算得到的特征点偏移。

(2)有航向角变化情况

有航向角变化情况如图 3所示。

图3 实时图截取示意图Fig.3 The schematic diagram of real-time images capture

如图 3所示，实线边框为上一时刻图像，命名为Pbefore，代表基准图；点划线边框为无航向角变化情况的现时刻图像，命名为Pnow_o；虚线边框为有航向角变化情况的现时刻图像，命名为Pnow。Obefore为基准图中心位置，也代表上一时刻载体中心位置；Onow为现时刻图中心位置，也代表现时刻载体中心位置；Δφ表示两帧图像采样间隔时间内航向角变化，Obefore到Onow连线的中点为期望实时图截取位置，用五角星表示。

若按照式(4)计算实时图中心在当前时刻图像中的坐标，由于航向角发生变化，将会导致实际实时图截取位置偏离期望实时图截取位置，故需要根据航向角变化情况，调整实时图中心坐标，使得实时图截取位置与期望实时图截取位置一致。为方便计算速度，定义两个新变量：ureal_m_cut、vreal_m_cut为实时图中心在当前时刻图像中截取用的坐标，这两个变量只用于实时图截取，不参与速度计算，其计算公式如下。

(5)

式中，HΔ表示姿态变化矩阵，与变化的航向角相关。

2.2 视觉里程计搜索区预测方法

传统视觉里程计在图像匹配的过程中，往往将整张基准图作为匹配搜索的区域，造成系统资源的浪费以及实时性的降低，本文利用惯性信息对上一帧图像中的匹配区位置进行预测，提高匹配搜索的速度及精度，增强视觉里程计的实时性和精确性。

本方法主要介绍利用惯性速度信息来确定搜索区中心坐标，其中(us_m,vs_m)表示搜索区的中心坐标。主要分三种情况进行讨论：

(1)无姿态角变化情况

搜索区中心坐标计算公式如下：

(6)

(2)仅有滚转角变化情况

设前后帧载体滚转角变化为Δγ，则滚转角变化情况示意图如图4所示。

图4 滚转角变化情况示意图Fig.4 The schematic diagram of the condition of the roll angle changing

图4中，实线边框的矩阵为基准图像，命名为Pbefore，即上一帧图像；有两个虚线边框的矩阵，它们分别是无滚转角变化拍摄图像和有滚转角变化拍摄图像，分别命名为Pnow_o、Pnow；Onow_o、Onow分别是对应图像的中心点；绿色填充的五角星为无滚转角变化实时图位置，无填充的五角星为有滚转角变化实时图位置。

由图 4可知，u方向搜索区中心坐标计算公式不变。有滚转角变化实时图为无滚转角变化实时图沿OV轴移动了FuΔγ，故v方向搜索区中心坐标计算公式为

(7)

(3)仅有俯仰角变化情况

设前后帧载体俯仰角变化为Δθ，如图 5所示。

图5 俯仰角变化情况示意图Fig.5 The schematic diagram of the condition of the pitch angle changing

图5中，相关变量的定义与图 4一致，Pnow_o沿OU轴的反方向移动了FuΔθ，变成Pnow。

由图 5可知，v方向搜索区中心坐标计算公式不变。有滚转角变化实时图为无滚转角变化实时图沿OU轴反方向移动了FuΔθ，故u方向匹配区中心坐标计算公式为

(8)

v方向匹配区中心坐标计算公式不变。

(4)综合分析

当Δφ、Δγ、Δθ均较小时可认为其是解耦的，则搜索区中心坐标计算公式如式(9)。

(9)

2.3 视觉里程计输出速度校正方法

传统的视觉里程计在计算速度时，若载体姿态角发生变化，将影响视觉里程计测速精度，且影响程度与姿态角变化幅度、相机到特征点的距离成正比。本项目利用惯性的姿态角信息，对视觉里程计输出的速度信息进行高精度补偿，能够极大提升视觉里程计的测速精度。

视觉里程计输出速度的校正公式如下

(10)

3 深组合导航方法

利用惯性信息辅助视觉里程计得到测速信息后，即可进行组合导航计算。

3.1 状态量定义

本文设计的惯性/双目视觉里程计组合导航方法其误差来源主要包括3个方面：

1)惯导的导航误差；

2)惯导的惯性器件误差；

3)视觉里程计的误差。

在常规卡尔曼滤波[11]中加入视觉里程计测速误差状态量，共选取17个系统状态：

δVn,δVu,δVe,δL,δh,δλ,φn,φu,φe,x,y,z,εx,εy,εz,δVx_odo,δVz_odo。

其中：δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差；

δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差；

φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角；

εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

δVx_odo,δVz_odo分别表示视觉里程计沿x、z轴的测速误差。

3.2 系统状态方程与量测方程定义

系统状态方程为

(11)

式中：X(t)为上述17个状态；W(t)为系统白噪声；系统矩阵F(t)根据误差方程求取。

现简要描述F(t)方程

(12)

其中，Fvisual为视觉里程计的误差，采用一阶马尔科夫过程描述。

(13)

式中：TVx,TVz和ξVx,ξVz为各自对应的相关时间和白噪声。

滤波器量测方程形式如下

Z=HX+V

(14)

量测值Z为惯导系统和景象匹配分别给出的位置的差值，实际上是两者误差的差值

(15)

式中，V为量测噪声，考虑为白噪声。

(16)

3.3 组合导航步骤

惯性/双目视觉深组合导航的基本步骤如下：

1)相关变量初始化，Kalman滤波器初始化；

2)开始导航解算，并计算系统状态转移矩阵

(17)

3)到达卡尔曼滤波周期时，进行时间更新：

(18)

4)获取双目相机数据，利用标定好的相机基线距离以及相机焦距，通过双目测距算法，可以计算出相机与特征点的距离。

5)通过相机获取特征点数据流后，利用惯性信息辅助视觉里程计计算，获得载体速度信息。

6)将视觉里程计速度信息与惯性导航系统的速度信息之差作为Kalman滤波器的输入，并计算卡方，当卡方超过限定门限，不进行量测更新，即Kk+1=0；否则进行量测更新

(19)

(20)

8)若进行量测更新，对估计出的系统误差进行闭环修正，转入步骤(2)；若未进行量测更新，直接转入步骤(2)。

4 试验验证与分析

4.1 试验条件

试验设备为：

·惯性导航设备采用光纤捷联惯导系统，陀螺漂移：0.03～0.05(°)/h，加表零偏：100μg。

·可见光相机采用加拿大灰点公司的大黄蜂二代双目相机，分辨率：640×480，焦距：7.4μm。

·相机安装方式如图6(a)所示，惯性导航设备安装方式如图6(b)所示，且惯导的zb轴与xb、yb轴为右手定则关系。

行驶轨迹如图 7所示，行驶了将近6000s，由于车辆行驶的起点与终点为同一地点，在行驶轨迹中体现为“去程”和“回程”的两条曲线。

图6 (a)相机安装方式Fig.6 (a)The installation of camera

图6 (b)惯性导航设备安装方式Fig.6 (b)The installation of IMU

图7 行驶轨迹Fig.7 The vehicle path

4.2 试验结果

为便于观察视觉里程计与GPS输出速度的差异，取一段时间内的数据进行对比，视觉里程计原始输出结果如图8所示。

图 8 (a)为视觉里程计原始输出，可知单纯使用视觉里程计进行测速，噪声较大，使用本文提出的新方法进行输出速度优化后的视觉里程计输出为图 8(b)，可见测量噪声有一定程度的降低。

图8 (a)视觉里程计输出速度对比Fig.8 (a)The comparison of the output velocity of visual odometry

图8 (b)优化后的视觉里程计输出速度对比Fig.8 (b)The comparison of the output velocity of optimized visual odometry

全程的深组合导航结果如图 9所示。

图9 惯性/双目视觉里程计深组合导航误差Fig.9 The errors of INS/stereo visual odometry deeply integrated navigation.

由图 9可知，对里程大于5km以后的数据进行统计，惯性/双目视觉里程计深组合导航的定位精度优于0.5%D(CEP)，可以媲美传统的惯性/里程计组合导航精度。

5 总结

本文基于机器视觉导航原理，合理分析了视觉里程计的技术特点，利用双目相机测量到特征点的距离，通过视觉里程计获得速度信息，并通过惯性信息可以辅助提升视觉里程计的测速范围、测速精度以及实时性。将视觉里程计和惯导系统的速度信息作为观测量，通过滤波器估计视觉里程计和惯导系统的误差并进行修正，实现较高精度的组合导航精度。

通过车载试验表明，惯性/双目视觉里程计深组合导航的实时定位精度较高，具备工程应用条件。

[1] Feng Yang, Cheng Cheng, Quan Pan,et al.Practical integrated navigation fault detection algorithm based on sequential hypothesis testing[J].Journal of Systems Engineering and Electronics, 2011,22(1):146-149.

[2] 杨波，王跃钢，孟朝，郭志斌.车载捷联惯导/重力匹配/高度计组合导航方法[J].现代防御技术，2014，42(3)：53-57.

[3] 韩龄，熊智，于永军，刘建业.基于SAR辅助的惯导/星光组合导航系统研究[J].传感器与微系统，2011，30(7)：16-19.

[4] 于永军，徐锦法，张梁，熊智.惯导/双目视觉位姿估计算法研究[J].仪器仪表学报，2014，35(10)：2170-2176.

[5] 苏哲，许录平，张华，谢强.基于XPNAV和SINS的容错组合导航系统[J].华中科技大学学报(自然科学版)，2011，39(6)：19-23.

[6] 李宇波，朱效洲，卢惠民，张辉.视觉里程计技术综述[J].计算机应用研究，2012，29(8)：2801-2805.

[7] Pretto A，Menegatti E，Bennewitz M，et al.A visual odometry framework robust to motion blur[C]//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ: IEEE Press，2009: 1685-1692.

[8] 吴乃亮，闫飞，卜春光.基于视觉里程计的移动机器人三维场景重构[J].华中科技大学学报(自然科学版)，2015，43(1)：337-340.

[9] 吴伟仁，王大轶，邢琰，等.月球车巡视探测的双目视觉里程计算法与实验研究[J].中国科学：信息科学，2011，41(12)：1415-1422.

[10] 张青涛，杨学友，刘涛，等.基于快速模板匹配的智能视觉传感器设计[J].传感技术学报，2013，26(8)：1040-1044.

[11] 陈楸，曹卫涛.基于场景识别的无人机自主着陆组合导航研究[J].计算机仿真，2011，28(2):84-87,150.

INS/Stereo Visual Odometry Deeply Integrated Navigation Method

LU Jian-jun1，REN Xiao-jun2, SUN Wei3, GUO Yuan-jiang3, LI Qun3

(1.Equipment Department of the Navy, Beijing 100000,China;2.Naval Representative Office of the Research Academy, CASIC, Beijing 100000,China;3.Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

To resolve the problem of large measurement error, low precision of matching, poor real timing, the INS/stereo visual odometry integrated navigation method based on Kalman filter was designed.The INS is used to help the stereo visual odometry faster and more accuracy by the real-time images capture, search area prediction and measuring velocity adjusting.By the use of kalman filter, visual odometry can correct the accumulated error of INS to improve the precision of integrated navigation system.The vehicle-carried result shows that this method improves the precision of INS/stereo visual odometry deeply integrated navigation to 0.5%D(CEP) and possess the condition of engineering use.

Stereo visual odometry;INS;Integrated navigation

10.19306/j.cnki.2095-8110.2016.03.007

2015-12-17；

2016-01-26。

总装预研课题(51309030203)

逯建军(1978-)，男，主要从事制导与控制方面的研究。

U666.1

A

2095-8110(2016)03-0037-07