基于视觉和MEMS-IMU融合的火星车导航定向技术*

娄 路

重庆交通大学信息科学与工程学院，重庆400074

星际探测车是一种具有高度自主能力、适合于在复杂的非结构化行星地表环境中(比如火星或月球)工作的探测移动机器人。为确保探测车在这样的复杂环境中执行探测任务，要求它具备高度的自主导航定向性能。近几年随着深空探测技术的发展，星际探测车的导航定向方法越来越多，根据测试手段、应用环境和误差累计特性的差别，一般可以将这些方法归纳为相对定向技术和绝对定向技术两类。相对定向技术主要包括以里程计、惯性导航系统为主体进行开发的导航技术［1］，该技术具有设备简单、实时性和完全自主不依赖外界环境的优点，能满足可靠的短距离导航精度。但是由于里程计、惯性导航系统具有一定的测量误差，随着测量误差的积累，导航精度随时间而降低，所以不适合于长时间、长距离导航定向。绝对定向方法主要有视觉导航、无线电导航以及利用太阳敏感器、星敏感器等进行定向［2］，其定向误差不随时间而增大，适合长距离的定向，但是自主性和实时性较差，容易受到周围环境的影响。

文献［3］研究了利用太阳、地球的天文导航方法与航位推算法相结合获得位置的月球车组合导航方法。针对月球车导航的特殊要求，文献［4］提出了天文导航与航位推算有机组合的月球车自主导航系统。文献［5］提出一种利用对太阳高度的连续观测，通过最小二乘法求解天文位置圆非线性方程组，获得漫游车固定点位置的天文导航方法。美国国家航空航天局的“勇气号”和“机遇号”火星车综合利用了里程计、惯性导航系统、视觉里程计和太阳敏感器这些相对定向和绝对定向技术进行导航定位［6］。

“勇气号”和“机遇号”火星车所用的惯性导航设备Litton LN-200，是一个小型化、高可靠性的光纤导航单元(IMU)，其在一个紧凑的盒体内包含3个光纤陀螺和3个微机电系统(MEMS)加速度计。IMU 姿态航向软件以8Hz 的频率计算和更新其姿态和位置，其中姿态更新由三轴加速度计和三轴陀螺仪测量获得，位置则通过IMU 结合里程计数器采用航迹推算法获得［7］。“勇气号”也采用视觉测程法(Visual Odometry)测算方向和位置，其基于连续拍摄的导航相机立体像对在二维影像平面和三维地面空间追踪特征点，并估计相对的位置和姿态实现车上实时定位，改正由航迹推算方法在车轮打滑时带来的较大定位误差［8］。由于火星车上计算机速度的限制，视觉测程法获取和处理一个立体像对，然后更新位置和姿态的整个过程需要近3min 的时间，速度太慢，因而无法用于“勇气号”和“机遇号”的全行程，只用于部分短距离关键路径上的局部定位，比如预计车轮打滑时、或接近指定的科学目标时［9］。除此之外，视觉测程法也有失败的时候，这主要发生在地形没有足够的特征点、特征点数目太少、火星车自身阴影影响等情况下。视觉测程法的缺点是计算速度慢、成功与否依赖于地形特征，因此只能应用于局部定位［10］。

近年来微电子机械惯性测试单元MEMS-IMU日益成熟，与传统高端惯性导航系统相比，MEMSIMU 可靠性差、精度低，但却具备体积小、成本低、功耗低的优势。比如“勇气号”火星车使用的光纤陀螺仪LN-200，重量接近1kg，功耗高达12W［11－12］，明显对探测车的行走控制和电源管理都不利。而欧洲空间局太空科学研究和技术中心正在研制的基于MEMS-IMU 芯片的行星探测车导航系统，总重量不超过200g，功耗小于1W，各项仿真测试参数的精度接近或超过设计期望指标［12］。

因此，针对MEMS-IMU 的发展趋势和视觉测程法采用标定立体摄像机、算法复杂且依赖于地形特征点等问题，本文提出一种基于MEMS-IMU 和单目摄像头的传感器融合方法，该方法采用加速度计的重力场分量去补偿修正陀螺仪的漂移偏差，获得较高的俯仰角和横滚角精度。采用单目摄像机，无需标定，设计了一个简单高效基于表象的视觉定向算法获取火星车行驶的航向角，该视觉定向方法能适用于各种地表环境。

1 基于传感器数据补偿融合的姿态估计

本文采用一个软件开源的MEMS-IMU 开发板作为姿态导航系统，该开发板是由一个8 位微处理器(ATmage328，8MHz 时钟)、三轴陀螺仪、三轴加速度计和三轴磁场计芯片组成。姿态计算可由微处理器单独完成，也可将传感器数据通过串口上传火星车主计算机，由其完成姿态计算。由于火星不存在固定的磁场，所以在本研究中三轴磁场计的定向输出只作为地面模拟实验环境下一个航向参考值。

将此开发板固定安装到轮式火星车上，构成一个捷联惯导系统。移动机器人运动过程中的姿态测算，通常用到2个坐标系统:一个是用来分析机器人运动的地面坐标系E;另一个是机器人自身的车体坐标系B，如图1 所示，其中θ 表示俯仰角，φ 表示横滚角，ψ 表示航向角。

图1 地面和车体坐标系

本文采用四元数法求解姿态。对于从地面坐标系E 到车体坐标系B 旋转一个角度θ，可以用一个四元数表示为:

于是采用四元数表示的旋转矩阵R 可以表示为:

进一步，车体姿态的欧拉角表达方式θ，φ，ψ可以用下式求解:

MEMS-IMU 的三轴陀螺仪可以测量输出车体在车体坐标系x，y，z 方向上的角速度ωx，ωy，ωz，如果用向量ω 表示为:

则四元数描述的车体相对地面坐标系的旋转可以用下式计算:

在初始姿态已知的情况下，利用时步积分法可以估测计算得到时刻t 车体的旋转状态

加速度计也可以单独测算探测车的旋转姿态，用与陀螺仪类似的方式，我们可以推导出与式(7)加速度计表示的时刻t 车体的旋转状态:

式中，▽f 为加速度计所计算获得的重力场分量。

陀螺仪存在一个固有的技术缺陷，就是随机漂移，特别是低成本MEMS-IMU 器件，陀螺仪长时精度很低，导致陀螺仪的输出值不可信。为解决陀螺仪漂移问题，本文采用了一种补偿滤波器算法(该算法在文献［13］中被提出并得到数学证明)，主要思路是:利用探测车姿态发生改变时，加速度计会测量获得俯仰角和横滚角坐标方向上产生的重力加速度分量，根据此分量计算得到的车体旋转状态，然后去更正陀螺仪产生的计算误差。

根据式(7)和式(8)我们设计了一个补偿融合公式:

式中，γt是权重系数，它是根据对陀螺仪误差与测算值之间的估值比自动调整。

该算法的原理可用图2 概述。

图2 补偿滤波器原理示意

本文采取的这种补偿融合算法计算量小，单片机可以完成整个计算过程，实时求解输出姿态角，工作频率达到50Hz。模拟实验的结果显示俯仰角(θ)、横滚角(φ)精度较高，而航向角在转弯时误差偏大。文献［9］的研究中采用卡尔曼滤波做类似融合算法，也同样提及航向角误差偏大的问题，但是没有分析原因。本文分析认为其原因也与本算法一样，是由于加速度计无法在航向角(水平方向)上对此方向轴上的陀螺仪漂移误差输出进行补偿修正所导致的。

2 基于单目摄像头的航向角测算

为了解决上述航向角误差偏大的问题，本文进一步提出利用单目视觉测算航向的思路，采取类似文献［10］中视觉里程计的算法，但与其采用立体摄像机获取视觉图片对、用角点特征算法进行图片特征提取、配对、跟踪和运动跟踪的复杂算法不同，本文采用了一种基于表象(appearance)模板匹配的算法，计算量小、实时性好。

一般而言，运动图片的位移可以用光流法计算，但是在地表图片比较一致、没有明显纹理特征的环境下，光流法无法获得特征点［14］。文献［10］也提到机遇号在它的火星探测区域中面临许多平坦、缺少图像特征的环境，使得基于角点特征的视觉里程计方法失效，只能依赖传统的里程计和惯性导航器件。

另一种计算运动图片位移的方法是模板匹配法［15］，在相邻图片帧里查找相似的区域图案，可以有效地工作于更广泛的地面环境中，包括沙砾、草地等无明显图像特征的地面。

本文采取一种改进的模板匹配算法，首先利用安装在探测车上的前视摄像头(与地面有一个固定倾角)，对地面环境连续抓取图片，分析相邻两幅图片的位移变化。具体过程是:先在第一幅图片的固定区域中随机生成部分采样点，然后在下一幅图片的预设搜索区域中进行采样像素点的循环搜索匹配，逐次缩小搜索范围直至检测到最相似的区域，从而确定图片位移距离，再通过位移距离换算得到摄像头航向偏转角度，如图3 所示。在进行图片区域相似度匹配时，采用欧式距离D 为判别依据。

图3 基于表象的模板匹配算法示意

通过模板匹配计算出相邻两幅图片的位移ΔU之后，通过式(11)可以得到航向角的偏移量Δψ:

最后通过公式(12)可以获取航向角ψ:

在本算法中，当所安装的摄像机(定焦距)对地面的探测角度确定之后，式(11)和(12)中M 只与摄像机视场角(Field of View)相关，是一个常量，测量一次即可获得。因此本算法有以下几个优势:

1)无需对摄像机做标定工作，方法简单、可靠;

2)本算法不提取图片视觉特征，只测量相邻图片之间像素位移，计算量小，能够达到每秒15帧的实时处理能力，因而对火星车行走速度没有限制;

3)与基于立体视觉的利用图片特征点(如SIFT)进行地表匹配的算法相比，本算法对环境的要求更低。实验证明在地貌相似没有明显特征的环境中(如沙化、灰尘地表)，该算法一样有效可靠。

3 实验结果及分析

在地面模拟试验环境中将上述方法应用到轮式火星车上，并进行了姿态估计和航向定位测试。同时在车上安装了光学跟踪球，通过一个室内运动光学跟踪系统(VICON)对火星车姿态和位置进行测算，得到真实基准数据。在试验中，采用地球重力加速度值代替火星重力加速度值。模拟实验结果表明，本文所提出的传感融合算法具有较好的姿态和航向估计精度。

从图4 典型实验曲线可以看出，基于单目视觉的航向角估测值(IMG － Yaw)非常接近真实值(VICON－Yaw)，并且平均精度高于MEMS －IMU所提供的基于地磁感应计输出的定向角输出(IMU－YAW)。

此外，多组测试结果显示:在平坦地面条件下能得到更好的输出结果，航向角平均误差(RMS)在1°左右，仰俯角和横滚角平均误差(RMS)在0.6°左右;在沙砾、石头和斜坡地表条件下，姿态和航向的测试精度有所下降，但是平均误差仍在3°左右。导致精度下降的原因可能是在沙砾、斜坡路况下轮式机器车产生的侧滑和摄像机的抖动，但仍需进一步的实验来验证和提高。

图4 姿态航向测试典型数据

表1 姿态航向测量误差(RMS)

4 结语

对于火星探测车在复杂地表环境下自主导航定位的需求，本文基于低成本、低功耗MEMS-IMU 和单目摄像机，提出一种传感器融合的简单有效的姿态航向估测方法，克服了陀螺仪漂移问题和地表环境单一性导致视频特征点失效的问题。模拟测试结果验证了该方法的有效性和可靠性。进一步的工作将考虑解决探测车侧滑、抖动对本算法精度的影响，以及结合其他绝对定位方法提高长距离的航向定位精度。

［1］岳富占，崔平远，崔祜涛.基于地球敏感器和加速度计的月球车自主定向算法研究［J］.宇航学报，2005，269(5):553-557. (YUE Fu-zhan，CUI Ping-yuan，CUI Hu-tao.Earth Sensor and Accelerometer Based Autonomous Heading Detection Algorithm Research of Lunar Rover［J］.Journal of Astronautics，2005，269(5):553-557.)

［2］岳富占，崔平远，崔祜涛.月球巡视探测器定位技术研究［J］.机器人，2006，28(2):235-239. (YUE Fuzhan，CUI Ping-yuan，CUI Hu-tao. Research on Localization Technologies for Lunar Rovers［J］.Robot，2006，28(2):235-239.)

［3］宁晓琳，房建成.一种基于天体观测的月球车位置姿态确定方法［J］. 北京航空航天大学学报，2006，32(7):756-759. (Ning Xiao-lin，Fang Jian-cheng. Position and Pose Estimation by Celestial Observation for Lunar Rovers［J］.Journal of Beijing University of Aeronautics and Astronautics，2006，32(7):756-759.)

［4］裴福俊，居鹤华，崔平远.基于联邦卡尔曼滤波的月球车自主导航方法［J］. 宇航学报，2009，30(4):1429-1434.(PEI Fu-jun，JU He-hua，CUI Ping-yuan.Autonomous Navigation Method Based on Federated Kalman Filter for Lunar Rover［J］.Journal of Astronautics，2009，30(4):1429-1434.)

［5］Y.Kuroda，T.Kurosaw.Accurate Localization in Combination with Planet Observation and Dead Reckoning for Lunar Rover［C］.Proc.of 2004 IEEE International Conference on Robotics and Automation (ICRA 2004 )，New Orleans，pp.2092-2097.

［6］邸凯昌.勇气号和机遇号火星车定位方法评述［J］.航天器工程，2009，18(5):1-5. (DI Kai-chang. Spacecraft Engineering［J］.A Review of Spirit and Opportunity Rover Localization Methods.2009，18(5):1-5.)

［7］K.Ali，C.Vanelli ，J.Biesiadecki.Attitude and Position Estimation on the Mars Exploration Rovers［C］.Proc of the 2005 IEEE Conference on Systems Man and Cybernetics，October 2005.

［8］R.Li，S.W.Squyres，R.E. Arvidson. Initial Results of Rover localization and topographic mapping for the 2003 Mars exploration rover mission［J］. Photogram2 Metric Engineering and Remote Sensing，Special issue on Mapping Mars，2005，71 (10):1129-1142.

［9］Maimone M ，Johnson A ，Cheng Y，et al.Autonomous Navigation Results from the Mars Exploration Rover(MER)Mission ［C］. 9th International Symposium on Experimental Robotics (ISER)，June 2004.

［10］M.Maimone，Y.Cheng，L.Matthies.Two Years of Visual Odometry on the Mars Exploration Rovers［J］.Journal of Field Robotics，2007，24(3):169-186.

［11］Instrument Information［EB/OL］. http://starbrite. jpl.nasa. gov/pds/viewInstrumentProfile. jsp? INSTRUMENT_ID=imu＆INSTRUMENT_HOST_ID=MER2.

［12］J. Hidalgo，P. Poulakis，J. Kohler，A. Barrientos，and J. Del-Cerro. ESTEC Testbed Capabilities for the Performance Characterization of Planetary Rover Localization Sensors-First Results on IMU Investigations［C］.11th Symposium on Advanced Space Technologies in Robotics and Automation，2011.

［13］R. Mahony，T. Hamel，J. M. Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group［J］.IEEE Transactions on Automatic Control，53(5):1203-1218.

［14］N. Nourani-Vatani，J. Roberts，M. Srinivasan. IMU Aided 3d Visual Odometry for Car-like Vehicles［C］. In Proceedings of the Australian Conference on Robotics and Automation，2008，Vol:1，pp.1-8.

［15］Berthold K P Horn. Robot Vision. MIT Electrical Engineering and Computer Science Series［D］. The MIT Press，1986.