一种月球车惯性/视觉组合导航方法研究

谷程鹏 邵济明 李广兴 施 梨

上海宇航系统工程研究所，上海201100

月球作为地球的天然卫星，有着丰富的资源[1]，是初期深空探测的理想目标。在探月任务中，月球车至关重要。月球车要在完全陌生的、非结构化的及复杂的月面环境中实现路径规划和给定的探测任务，需要精确获取自身的位置和姿态信息。现有月球车的自主导航主要采用惯性和视觉导航方式。惯性导航自主性强，姿态精度高，但其误差随时间累积，难以独立完成长时间的导航任务。视觉导航[2]自主性强，定位精度高，但其误差随距离累积。此外，天文导航[3]误差较高，且易受可观测性影响,在月面上定位性能较低。惯性和视觉导航均为相对导航方式，提高长时间、长距离下的定位精度，是月球车导航的关键技术之一。

目前在惯性/视觉组合导航方面，已有了较多的研究。惯性/视觉组合导航一般以惯性导航为主，以位置、速度和姿态为状态量建立PVA方程[4],或者以误差项为状态建立误差方程[5]；基于特征点图像坐标[6]、视觉导航的位姿输出[7]、视觉导航前后站点相对运动参数[8]、惯导和视觉导航测量之差[9-10]建立量测方程。文献[9]利用视觉位置输出估算载体的运动速度，并将其与惯性导航速度之差作为量测信息，从而完成组合导航方案设计，但是它需要构造视觉里程计的速度误差模型并将状态进行增广。文献[10]基于上一时刻组合导航的位置和姿态估计值构造观测量，从而避免了对状态的增广，简化了测量模型。但这是在假设组合导航估计值和真实值接近的条件下得到的。实际上，位置估计偏差可能就会引起滤波的发散。文献[11]用单目摄像头测算航向，对惯性导航解算结果进行修正，但这一算法只融合航向信息，且组合结构比较简单。文献[12]提出双目视觉加惯性元件的航天器相对导航方法，但该视觉测量是基于航天器上设置的已知特征点的，而在月球探测初期，月面上难以布置特征站点，无法依靠视觉来绝对定位。本文以误差项为状态量，利用前后2个时刻的惯性导航输出计算前后时刻相对运动参数，并将其与视觉导航输出之差作为滤波量测，既能融合所有的导航信息，又能避免对状态的增广。同时视觉导航的输出为相对量，故不需要绝对的特征点信息。针对测量噪声未知的问题，采用自适应卡尔曼滤波[13]，对噪声均值和方差进行在线估计。仿真结果表明，该惯性/视觉组合导航精度相比单一的导航方式有明显的提高，方法有效。

1 组合导航方案设计

1.1 组合导航系统状态方程

以惯性导航系统的导航输出参数误差和惯性元件误差作为状态向量，包括3个平台误差角、3个速度误差、3个位置误差、3个陀螺常值漂移和3个陀螺随机漂移和3个加速度计漂移共18维。推导出惯导系统误差方程[14]作为组合导航状态方程。

(1)

1.2 组合导航系统量测方程

1.2.1 惯导观测信息

根据前后时刻的惯导输出，计算k到k+1时刻的旋转和平移参数

(2)

(3)

式中：

1.2.2 视觉观测信息

(4)

由于月球车的位姿信息是递推获得，前一时刻的误差会不断累积，造成定位误差逐渐变大，难以用模型描述。然而，前后时刻的相对运动参数的解算误差只与算法误差、特征匹配误差及成像误差有关，其误差分布特性可以认为是不变的。基于该前后时刻的相对运动参数，设计惯性/视觉组合导航方案。因此，视觉观测信息可表示为

(5)

(6)

1.2.3 组合导航量测模型

利用惯导观测信息式(2)～(3)和视觉观测信息式(5)～(6)之差构造量测方程

(7)

(8)

显然，该差值与状态成线性关系，并叠加未知噪声。取旋转矩阵上三角三参数与平移矢量为观测量，得到最终的量测方程

(9)

式中：Hr由式(7)计算得到；Fd和Gd为离散状态矩阵和过程噪声矩阵。

1.3 自适应卡尔曼滤波方程

由于系统状态方程和量测方程均为线性，但量测噪声模型未知，滤波同时需要对量测噪声的均值和方差进行在线估计，故采用自适应卡尔曼滤波。噪声估计方程为

(10)

(11)

(12)

1.4 协方差传递

为了更好地说明各导航方式的性能，推导出相应的协方差传递方程为

(13)

(14)

(15)

(16)

式中：HV和Hk,k+1为式(14)求得的对应雅克比矩阵，且由于噪声未知，这里用组合导航估计的噪声矩阵来求视觉导航的过程噪声矩阵。

某一时刻的协方差代表了导航误差下限，以此来证明组合导航的优势。

2 仿真

2.1 仿真条件

仿真中月球车的轨迹选用美国勘探者3号探测器的着陆地点(月球2°56′N,336°40′E)作为起点，月球车以约0.2m/s的速度匀速运行。仿真进行100次，取均方误差作为各导航方式的误差结果。

基本参数：月球半径1738km，重力加速度g=1.633m/s2，自转周期27.32166天。惯性元件参数：陀螺常值漂移0.2(°)/h，随机漂移0.1(°)/h，相关时间Tg=3600s；加速度计随机漂移10-4gm，相关时间Ta=1800s；采样频率均为100Hz。立体视觉参数：焦距f=6mm，分辨率768×576，像素大小dx=dy=1/120mm，成像误差Δε=0.6pixel，匹配误差Δδ=0.6pixel，采样频率为0.25Hz。双目相机平行安装，基线距离0.2m，相机俯视角为20°，左相机坐标系相对本体坐标系的转换参数为

2.2 仿真结果

图1～2为视觉导航在不同运动下的前后时刻相对运动参数解算误差，横坐标为250次随机运动，解算过程中的图像特征点位置和数量同样随机。这里列出了经度误差和横滚角误差。可以看出，前后时刻相对运动参数的解算误差在一定区域之内，故用式(5)～(6)来描述视觉观测信息是合理的。

图1 视觉导航单步解算经度误差

图2 视觉导航单步解算横滚角误差

图3～8分别列出了惯性导航、视觉导航和组合导航的位置及姿态误差(不失一般性，图中列出前1000s时间结果)。从经纬度上看，惯性导航误差高于视觉导航，且均呈现高次增长现象。本文提出的组合导航方法能够明显地提高定位精度，同时惯性导航和视觉导航均为相对导航方式，组合导航的误差同样不断增长，但其增长趋势较单一，导航方式缓慢，减小了误差的发散程度。在高度通道上，由于惯性导航的误差已较小，组合导航的精度提升效果并不明显。从姿态上看，视觉导航误差明显高于惯性导航，而组合导航则能显著提升横滚角和俯仰角的精度，但航向角的精度并没有得到改善。此外，由于惯性导航的姿态精度较高，漂移较小，组合导航的姿态误差也较为平稳，能够控制在一定的范围之内。

图3 经度误差

图4 纬度误差

图5 高度误差

图6 横滚角误差

图7 俯仰角误差

图8 航向角误差

表1统计了各导航方法在200m(1000s)处的位置、姿态误差及协方差。由表1看出，视觉导航的位置精度相对惯性导航高，惯性导航的姿态精度相对视觉导航高。组合导航的位置和姿态精度均显著优于单一导航方式。在200m处，由于惯性和视觉导航的误差发散严重，组合导航精度提升效果较为明显。本文提出的组合导航方法的位置误差为1.092m，相比惯性导航减小了99.7%，相比视觉导航减小了95.1%。组合导航的横滚角和俯仰角精度提升2个数量级。从协方差传递情况上看，组合导航相比单一导航方式确有很大提高，证明了该组合导航方式的有效性。

表1 3种导航方式在200m处位置姿态误差和协方差

3 结论

提出一种基于前后时刻惯导观测量的月球车惯性/视觉组合导航方法，将前后时刻相对运动的惯导观测量转换为只与当前时刻惯性导航误差相关，从而避免了对状态的增广。利用视觉导航相对运动参数误差特性不变的特点，将相对运动的惯导和视觉观测量之差作为组合导航量测值，采用自适应卡尔曼滤波解决量测噪声未知的问题。仿真证明，该方法可以有效提高位置和姿态的精度，实现惯性导航和视觉导航的互补。

[1] 欧阳自远.我国月球探测的总体科学目标与发展战略[J].地球科学进展,2004,19(3):351-357.(Ouyang Ziyuan. Scientific Objectives of Chinese Lunar Exploration Project and Development Dtrategy[J]. Advance in Earth Science, 2004,19(3):351-357.)

[2] 刘传凯,王保丰,王镓,等.嫦娥三号巡视器的惯导与视觉组合定姿定位[J].飞行器测控学报,2014,33(3): 250-257.(Liu Chuankai, Wang Baofeng, Wang Jia, et al. Integrated INS and Vision-based Orientation Determination and Positioning of CE-3 Lunar Rover[J]. Journal of Spacecraft TT&C Technology, 2014,33(3): 250-257.)

[3] 施丽娟,熊智,王融,等.基于H∞滤波的空天飞行器惯性/天文组合导航方法[J].航天控制，2016,34(5):46-51.(Shi Lijuan, Xiong Zhi, Wang Rong, et al. An INS/CNS Integrated Navigation Method of Aerospace Vehicle Based on theH∞ Filter. Aerospace Control, 2016,34(5):46-51.)

[4] Wang T M, Wang C L, Liang J H, et al. Vision-aided Inertial Navigation for Small Unmanned Aerial Vehicles in GPS-denied Environments[J].International Journal of Advanced Robotic Systems,2013,10(5):1-12.

[5] Kim Y, Hwang D H. Vision/INS Integrated Navigation System for Poor Vision Navigation Environments[J]. Sensors, 2016, 16(10):1672.

[6] Kim Y, Hwang D H. Design of Vision/INS Integrated Navigation System in Poor Vision Navigation Environments[J]. International Conference on Control, Automation and Systems, Gwangju: ICCAS,2013:531-535.

[7] Corato F D, Innocenti M, Pollini L. An Entropy-like Approach to Vision-aided Inertial Navigation[J]. Ifac World Congress, 2011,44(1):13789-13794.

[8] Tardif J P, George M, Laverne M, et al. A New Approach to Vision-aided Inertial Navigation[C]. IEEE/RSJ International Conference on Intelligent Robot & Systems, Taipei: IEEE,2010,25(1):4161-4168.

[9] 逯建军,任晓军,孙伟,等.惯性/双目视觉里程计深组合导航方法[J].导航定位与授时,2016,3(3): 37-43.(Lu Jianjun, Ren Xiaojun, Sun Wei, et al. INS/Stereo Vision Odometry Deeply Integrated Navigation Method[J]. Navigation Positioning & Timing,2016,3(3): 37-43.)

[10] 徐勇志,宁晓琳.一种巡视器惯性/视觉组合导航新方法[J].空间科学学报,2015,35(6):721-729.(Xu Yongzhi, Ning Xiaolin. A New INS/VNS Integrated Navigation Method for Planetary Exploration Rover[J]. Chin. J. Space Sci, 2015,35(6):721-729.)

[11] 娄路.基于视觉和MEMS-IMU融合的火星车导航定向技术[J].航天控制,2012,30(4):32-36.(Lou Lu. An Approach of Attitude Estimation by Using Vision-aided IMU for Mars Rover[J]. Aerospace Control, 2012,30(4):32-36.)

[12] 赵逸伦,乔兵,靳永强,等.一种采用双目视觉加惯性测量的航天器组合相对导航方法[J].航天控制,2016,34(4): 47-52.(Zhao Yilun, Qiao Bing, Jin Yongqiang, et al. An Integrated Relative Navigation Algorithm for Spacecraft Based on Binocular Vision and Inertial Measurement[J]. Aerospace Control, 2016,34(4):47-52.)

[13] 周启帆,张海,王嫣然.一种基于冗余测量的自适应卡尔曼滤波算法[J].航空学报,2015,36(5):1596-1605.(Zhou Qifan, Zhang Hai, Wang Yanran. A Redundant Measurement Adaptive Kalman Filter Algorithm[J]. Acta Aeronautica et Astronautica Sinica,2015,36(5):1596-1605.)

[14] 刘建业,曾庆化,赵伟,等.导航系统理论与应用[M].西安:西北工业大学出版社,2010.(Liu Jianye, Zeng Qinghua, Zhao Wei, et al. Navigation System Theory and Application[M]. Xi′an: Northwestern Polytechnical University Press, 2010.)