火星车自主导航与路径规划技术研究

魏祥泉，黄建明，顾冬晴，陈凤

（上海宇航系统工程研究所，上海 201109）

火星车自主导航与路径规划技术研究

魏祥泉，黄建明，顾冬晴，陈凤

（上海宇航系统工程研究所，上海 201109）

针对火星车工作任务环境，借鉴国外成功研究经验，给出了火星车自主导航敏感器配置，研究可在实际工程中应用的火星车自主导航与路径规划技术，包括DEM图构建与障碍物检测、全局路径规划、基于立体视觉相机的视觉里程计、多传感器融合位姿确定以及局部路径规划技术，并搭建了实验系统，在实验室内进行了仿真验证。相关研究结果可为我国后续开展火星车自主导航系统研究提供一定的基础。

火星车自主导航；路径规划；视觉里程计；多传感器信息融合

引用格式：魏祥泉，黄建明，顾冬晴，等. 火星车自主导航与路径规划技术研究［J］. 深空探测学报，2016，3（3）：275-281.

Reference format: Wei X Q，Huang J M，Gu D Q，et al. Researches on the techniques of autonomous navigation and path planning for mars rover ［J］. Journal of Deep Space Exploration，2016，3（3）：275-281.

0　引　言

火星由于其独特的科学价值和技术跨越性，成为人类开展深空探测的首选目标之一。火星车所面临的任务环境非常复杂，使得火星车的导航研究极具挑战性和特殊性，主要表现在以下几个方面。

1）非确定性导航环境。星面环境是一个非确定、非结构化的未知复杂环境，难以通过各种手段获得星面环境的全部信息，通过航拍图预先建立的星面地图受到分辨率和准确度等方面的限制。

2）大时延及带宽限制。由于通信延迟、持续通信时间的限制，难以实现连续控制，有限带宽难以支持连续的反馈信息（包括视频图像）传输。

3）环境感知的复杂性。由于地外星体环境的特殊性和复杂性，使星面环境感知手段受到限制，感知难度也大幅度增加，这将影响到导航系统所依赖的各种感知信息的精度、稳定性和可靠性。

4）火星车的安全性和可持续工作能力是行星车导航系统优先考虑的因素。

此外，行星车的正常运行还面临资源（如能源、存储器等）、操作时间有限、意外障碍等因素的影响。

综上所述，火星车是一个对外界环境高度开放的智能系统，为了保证火星车能够适应复杂的星面环境并顺利完成各种探测任务，需要火星车具有高度的自主导航能力［1］。

国外开展火星车研制的有美国、欧空局（ESA）和俄罗斯，其中只有美国的火星车成功发射并成功运行。美国共研制了三代火星车，1996年发射的“索杰纳”是首个火星车［2］，2004年发射的第二代火星车“勇气号”和“机遇号”找到了大量的地质学和矿物学证据证明火星上确实曾经存在大量的水［3］，第三代火星车“好奇号”于2012年8月登陆火星，是第一辆采用了“核动力驱动”的火星车［4-5］。ESA将在2018年4月发射的任务中使用ExoMars火星车［6］。表1对国外火星车自主导航系统进行了比较。

为使火星车能够安全平稳地到达科学探测的目标点，自主导航与路径规划主要完成如下功能。

1）环境感知与识别：利用相机和其他敏感器建立全局三维地形图并识别可视范围内障碍与危险。

2） 位姿确定：自主确定当前位置和姿态。

3）路径规划与避障：根据当前位置和姿态、环境感知结果及地面遥操作系统指定的远距离目标点位置，进行局部路径点分解和局部路径规划，保证火星车安全到达目标点。

表1　国外火星车自主导航系统比较Table 1　Comparison of autonomous navigation system for foreign mars rover

国内，宁晓琳等（2006）研究了一种基于纯天文观测的火星车自主导航方法，但天文导航不能输出连续的位置信息，需要与其他导航方法组合使用［7］。邱凯昌等（2010）研究了一种基于地面图像和卫星图像集成的火星车定位方法，定位精度和定位范围受限于地面图像的分辨率和覆盖范围［8］。娄路（2012）提出一种单目视觉与MEMS-IMU融合的导航定向方法，需要与其他定位方法结合导航和长距离定向［9］。国外对火星车导航与路径规划的研究较多［10-13］，可为我们开展相关工作提供一定的参考。

本文的目的是研究可在实际工程中应用的火星车自主导航与路径规划技术，为我国未来的火星车自主探测任务奠定技术基础。

1　火星车自主导航敏感器配置

火星车导航系统拟由一对全景相机、一对导航相机、两对避障相机、惯性测量组件、数字太阳敏感器和光电编码器组成，其布局和视场如图1所示。

图1　火星车自主导航与控制分系统组成示意图Fig.1　The schematic diagram of the autonomous navigation and control system of mars rover

导航相机视场45°，安装在火星车的桅杆上，在火星表面上方约1.6 m的高度，采用20 cm的基线和平行光轴；桅杆可以水平旋转360°，安装台可以上下旋转90°。一对导航相机组成被动立体视觉系统来探测潜在的地形危险，为火星车路径规划提供周围100 m范围内的局部或全景地形图像，可以与避障相机一起进行车载自主导航，也可以协助地面操作系统行进路线规划。视觉里程计（Visual Odometry，VO）用于在比较短的行驶距离范围内（＜10 m），根据连续的导航相机立体图像进行里程计算。

全景相机视场14°，安装在桅杆上，可对火星车周边地貌进行全景成像，既可用于形态学、大气和地质研究，也可用于三维现场描绘，为火星车导航提供辅助。采用30 cm的基线和平行光轴，一个是中分辨率相机，一个是高分辨率相机。需要进行远距离路径规划时，使用分辨率高于导航相机的全景相机。

两对避障相机分别安装在火星车的前部和后部，在火星表面上方大约50 cm高度，并且指向水平大约50°，均采用10 cm基线和平行光轴。避障相机主要用于危险检测、行驶过程中精确定位、获取火星表面清晰的近摄图像，以及辅助科学实验中仪器设备部署等，是进行近距离路径规划的主要依据。

惯性测量组件主要用于静态倾角测量和动态姿态预估。数字太阳敏感器主要用于确定火星车的偏航方位。光电编码器是一种通过光电转换将输出轴上的机械集合位移量转换成脉冲或数字量的传感器。

2　火星车自主导航与路径规划技术

火星车自主导航与路径规划技术流程如图2所示。

图2　火星车自主导航与路径规划技术流程Fig.2　Technological process for autonomous navigation and path planning

2.1DEM图构建与障碍物检测

2.1.1图像预处理

重点进行畸变校正、滤波和均衡化处理，以便于进行立体视觉匹配和视觉里程处理。图像滤波采用LoG算子对图像进行滤波，主要目的有两个：一是使图像中的弱纹理区域在一定程度上得到增强，另一个作用是消除两个成像图片之间的整体亮度差异。

图像分割的主要目的是将纹理增强后图像中纹理仍然较弱的区域提取出来，利用区域的整体性质对弱纹理区域进行障碍判别，采用k-均值聚类算法。

2.1.2立体视觉匹配

火星表面由各种各样的矿物质组成，表面粗糙度比环境光波长大得多，表面反射主要是漫散射，局域范围内的纹理色彩并不是很丰富，采用基于特征提取的方法来进行立体匹配获得的正确率不会很高。因此，所采用的算法不应致力于特征点、线或面的提取，而是应直接利用图像对进行特征匹配。

根据上述分析，使用基于灰度值相关的目标匹配方法。设图像对中的左幅图像为参考图像，对于其中的每一点选择一定尺寸（5×5，7×7或9×9）的窗口作为模板，沿这一点在右幅图像中对应的极线上采用同样大小的模板进行搜索，参考模板和候选模板内的各个像素灰度值之差最小时，两点被认为是对应点。这种方法也叫像素灰度差的绝对值（Sum of Absolute Differences，SAD）区域匹配法。公式为

其中，dmax和dmin是系统可能的最小和最大的视差值；m是窗口尺寸；Iright和Ileft是左右图像的灰度值。

2.1.3DEM图重建

DEM图的准确构建和障碍物的精确检测是火星车进行自主导航与控制的前提。进行DEM图重建主要包括DEM数据预处理、栅格地形高度估计和DEM图后处理、帧间融合等步骤。

1）DEM数据预处理

火星车DEM数据源于车载双目立体导航相机获取的立体图像，通过立体图像匹配和三维重建得到致密的点云数据。由于立体匹配过程中的误匹配点往往会导致重建后的点云数据出现错误点和悬浮点，因此需要对原始点云数据进行预处理，将不可靠点和错误点去除。此外还需将立体匹配和重建得到的三维点云数据转换到惯导坐标系下，需要进行坐标变换。

2）栅格地形高度估计

采用DEM图来表示地形时，一个栅格内通常包含了多个三维点数据。为每个栅格创建高度直方图以保存这些数据点的高度分布，然后根据直方图决定每个格子的高度。

若落入某一栅格的三维点个数n＜N（N为点数阈值），则认为该点云对于该栅格数据不造成影响，即没有得到该栅格的有效数据。若落入点数大于N的栅格，则对该栅格的平均高度进行估算。首先滤除栅格中所有高度Y＜H（H为高度阈值）的点，以减小大量地面点对障碍物高度估算的影响。对剩余点计算代数平均，得到平均高度为h，则最高点高度的估计为hmax=2×h。

由于误匹配可能造成点云数据中存在许多悬浮点，因此需要去除悬浮点对于栅格高度的影响。采用密度判断法解决悬浮点问题。栅格的估计密度为ρ=NTotal/h，其中，NTotal为所有落入该栅格的点数。若ρ＜ρ（其中ρ为密度阈值），则认为该栅格的估计高度h无效。若落入栅格点总数大于点数阈值，且栅格估算密度大于密度阈值的栅格，将该栅格的高度赋值为hmax，其余栅格高度则赋值为0。

3）DEM图后处理

后处理目标是修正格子的错误高度和为某些无数据区作插值。判断虚格子高度错误的依据是可视性原则：假想相机拍摄重建场景，所有高度的格子应该都能被拍摄到，没被拍到的格子其高度或者有错或者被遮挡。具体算法如下。

①检测障碍物，判据为格子高度比其前面或者左右格子的高度高30 cm。若检测到障碍物则继续，否则算法结束。

②如果障碍物格子后面的格子有高度且不符合可视性原则，而且障碍物格子的高度直方图是尖峰，则此障碍物格子是虚假的，把它设为无数据区。重复步骤①。

由于空间分辨率和误配的原因，有些格子会没有数据点落入，即存在无数据区，而且检测虚假高度时，把虚假高度的格子直接设为无数据区，因此需要作插值。把无数据格子八邻域中高度相近的格子高度的平均值作为其高度。

③帧间融合

视觉处理一部分场景会在连续好几帧出现，即存在帧间的数据冗余，因此可以运用帧间融合来消除障碍物虚报。具体做法为：当在当前帧发现障碍物时，到前n帧查看是否有这个障碍物。如果没有，则认为是虚假障碍，有则认为是真正的障碍。

2.2全局路径规划

常用的全局路径规划方法有模板匹配法、地图构建法、人工势场法以及其他一些人工智能方法。火星车全局路径规划算法要求计算速度快，占用内存小，可靠性高，重复计算简单，选择A*算法作为火星车全局路径规划方法。A*算法是一种典型的启发式搜索算法，是求解静态网络中最短路径的最有 效方法［14］。A*的估价函数是：f（n）=g（n）+h（n）。其中，f（n）是节点n从初始点到目标点的估价函数，g（n）是在状态空间中从初始节点到n节点的实际代价，h（n）是从h（n）到目标节点最佳路径的估计代价。A*算法的搜索步骤如下。

1）标记起始节点，并对其周围的未标记过的子节点进行扩展。

2）对每一个子节点，计算其估价函数值，按照估价函数值的大小进行排序，找出估价值最小的节点进行标记。如果被标记的子节点就是目标点，则停止搜索。

3）对新标记的子节点重复步骤2）。

2.3视觉里程计

视觉里程计是利用安装在移动自主探测车上的立体视觉相机获取图像序列，通过跟踪前后帧的相同特征点，精确估计车体6个自由度的更新（位置和姿态）。利用视觉里程计可以获得更为精确的火星车定位结果，并且结合方向传感器定期纠正视觉里程计方向的定位误差可以使得误差积累非常缓慢地随漫游距离线性增加。视觉里程计的工作分为三步：特征点提取、特征点匹配和跟踪、鲁棒运动估计。

通过性能与计算量等方面的比较与仿真分析，选择SIFT算法进行特征提取与特征匹配，SIFT特征关键点虽然不如角点、边缘检测等具有明显的视觉意义，但其描述了图像局部的结构性信息，体现了图像在尺度、局部结构方面的特征，并具有多量性和独特性。特征点立体匹配和特征点跟踪匹配完成后，经过坐标转换就可以得到一系列匹配点对在车体运动前后的摄相机坐标系下的三维坐标。为了兼顾算法的实时性和精确度，采用RANSAC结合线性最小二乘法求出车体运动参数的预估值，再从预估值出发运用非线性最小二乘法求出车体运动参数的精确估计值的方法。

结合RANSAC的线性最小二乘估计虽然可以有效去除外点，但所得的结果可能是不精确的。为了得到精确的估计结果，可采用Levenberg-Marquardt算法进行非线性最小二乘估计［15］。当某些前后帧之间图像变化较大，匹配点数量很少时就不再适用。在这种情况下，可以利用正交化方法来大致求取运动估计参数。在应用中要注意把握好各帧图像的采集间隔，保证前后两帧图像有较大的重叠率以提供足够多的特征跟踪匹配点对。

2.4多传感器融合位姿确定

火星车自主位姿确定采用了多种测量传感器，这些传感器的信息要通过有效的信息融合手段，才能获得高精度的火星车位姿参数估计。多传感器位姿确定的 信息融合将采用集中式卡尔曼滤波（Kalman Filter，KF）算法。图3为多传感器信息融合位姿确定原理框图。

图3　多传感器信息融合位姿确定原理框图Fig.3　Block diagram of multisensor information fusion for pose determination

多传感器位姿确定卡尔曼滤波将以捷联导航计算的位置误差、速度误差、姿态误差以及IMU的陀螺和加速度计测量误差，作为待估计的系统状态向量，采用卡尔曼滤波的状态更新和量测更新算法，可以实时得到系统状态的最优估计，然后采用反馈修正方式对捷联导航计算的位置、速度、姿态参数以及IMU的陀螺和加速度计测量值进行误差修正。因此，捷联导航计算输出的经误差修正的位姿参数，即为多传感器位姿确定信息融合的全局最优估计。

2.5局部路径规划

对于静态建立的全局路径，其为离散栅格点路径，并且由于全局地图的精确度较差，全局规划时得到的全局路径并不能完全避开所有的障碍物，这就需要局部路径规划过程来完成局部避障和全局路径点跟踪。常用局部规划方法为：VFH法、Ranger算法、Morphin算法、GESRALT算法和ND法等［16］。

由于已经建立了全局路径，因此对GESTALT方法进行简化来实现局部路径生成。在路径生成过程中，忽略GESTALT建立优异值地图过程，并且忽略其确定值，仅考虑建立二值栅格地图，如上述栅格地图建立中所描述，将整个地图标记为可行区域与不可行区域。之后建立一系列的轨迹组，从中选择接近最远全局路径点，并且不经过不可行区域的轨迹为下一步执行轨迹。具体方法如下。

1）建立一组符合火星车运动学的轨迹；

2）分析每条轨迹所经过的栅格，得到每条轨迹经过的全局路径栅格和不可行栅格，将其经过的离目标点最近的栅格作为其最终选择路径点；

3）选择经过最远全局路径点并且在可行区域的轨迹为下一步执行轨迹；

4）当没有局部轨迹经过路径点，并且没有穿过不可行区域时，选择最容易实现的轨迹，也就是弧度最小的轨迹，先绕过局部障碍物，之后再按照上述方法，寻找经过最远路径点并且不经过不可行区域的轨迹，直到到达目标点为止。

如图4所示，细弧线为规划轨迹组，其中粗轨迹为选择的不经过不可行区域，并且经过第三个路径点的轨迹。

图4　局部路径选择示意图Fig.4　Sketch map for local path selection

由于全局路径的精确度较差，因此规划好的全局路径点周围有可能出现障碍物，导致此路径点成为不可行区域，这种障碍物可称为局部障碍物，对于此种情况，使用上述介绍方法是并不能完成避障，并且可能会产生错误，因此需要采用如图5 所示的避障方案。当没有局部轨迹经过路径点，并且没有穿过不可行区域时，选择最容易实现的轨迹，也就是弧度最小的轨迹，先绕过局部障碍物，之后再按照上述方法，寻找经过最远路径点并且不经过不可行区域的轨迹，直到到达目标点为止。

图5　出现局部障碍物时的局部路径规划示意图Fig.5　Sketch map for local path selection in the case of local obstacle

3　火星车自主导航与路径规划仿真

为了验证火星车自主导航与路径规划方法的可行性，构建了实验系统进行仿真验证。

火星车自主导航与路径规划实验系统如图6所示，利用导航相机拍摄的实验地形图如图7所示，经处理后构建的DEM图如图8所示。使用A*算法建立的全局最优路径如图9所示，其中细线条即为最优全局路径。局部路径规划结果图10所示，其中细线轨迹为全局规划路径，粗线轨迹为局部跟踪之后的路径。

图6　火星车自主导航与路径规划实验系统组成Fig.6　Composition of experiment system of autonomous navigation and path planning for Mars Rover

图7　实验地形图Fig.7　Topographic map for experiment

图8　构建的DEM图Fig.8　Constructed DEM diagram

4　结束语

本文首先在火星车工作任务环境特性和国内外相关研究情况分析的基础上，给出了火星车自主导航敏感器配置；然后，重点研究可实际工程应用的火星车自主导航与路径规划技术，给出了自主导航与路径规划技术流程，研究了所涉及的DEM图构建与障碍物检测、全局路径规划、基于立体视觉相机的视觉里程计、多传感器融合位姿确定以及局部路径规划技术；最后利用实验搭建的半物理仿真系统在实验室内进行了仿真验证。相关研究结果可为我国后续开展火星车自主导航系统研究提供一定的基础。

图9　A*路径搜索结果Fig.9　Path search results for A* method

图10　局部路径规划结果Fig.10　Local path planning results

［1］Li R，Di K，Howard A B，et al. Rock modeling and matching for autonomous long-range Mars rover localization［J］. Journal of Field Robotics，2007，24（3）：187-203

［2］Matthies L，Gat E，Harrison R，et al. Mars microrover navigation：performance evaluation and enhance- ment［J］. Autonomous Robots，1995，2（4）：291-31

［3］Cheng Y，Maimone M W，Matthies L H. Visual odometry on the Mars exploration rovers［J］. IEEE Robotics and Automation Special Issue（MER），2006，13（2）：54-62

［4］张扬眉. 性能优异的美国“火星科学实验室”［J］. 国际太空. 2011（11）：17-23. Zhang Y M. American Mars Science Laboratory with excellent properties［J］. Space International，2011（11）：17-23.

［5］天力. 美好奇号漫游车登陆火星［J］. 中国航天，2012（9）：16-18 Tian L. Curiosity Mars Rover landed on Mars［J］. Erospace China，2012（9）：16-18.

［6］Griffiths A D，Coates A，Jaumann R. et. al. Context for the ESA ExoMars rover：the Panoramic Camera（PanCam） instrument［J］. International Journal of Astrobiology，2006，5（3）：269-275.

［7］宁晓琳，房建成. 一种基于纯天文观测的火星车自主导航方法［J］.空间科学学报. 2006，26（2）：142-147. Ning X L，Fang J C. A new autonomous navigation method for martian rover based on celestial observation［J］. Chin. J. Space Sci.，2006，26（2）：142-147.

［8］邸凯昌，岳宗玉，刘召芹. 基于地面图像和卫星图像集成的火星车定位新方法［J］. 航天器工程，2010，19（4）：11-16. Di K C，Yue Z Y，Liu Z Q. A new approach to Mars rover localization based on integration of ground and orbital images［J］. Spacecraft engineering，2010，19（4）：11-16.

［9］娄路. 基于视觉和MEMS-IMU融合的火星车导航定向技术［J］. 航天控制，2012，30（4）：32-36，48. Lou L. An approach of attitude estimation by using vision-aided IMU for Mars rover［J］. Aerospace Control，2012，30（4）：32-36，48.

［10］Estlin T，Gaines D. Automated decision-making for Mars rover onboard science［C］//AIAA 2007 Conference and Exhibit. Rohnert Park，California ：AIAA，2007.

［11］Joudrier L，Elfving A. Challenges of the ExoMars rover control［C］//AIAA Unmanned... Unlimited Conference. Seattle，Washington：AIAA，2009.

［12］Helmick D M，Angelova A. Terrain adaptive navigation for Mars rovers［C］// 2007 IEEE Aerospace Conference. ［S.l.］：IEEE，2007.

［13］Estlin T，Castano R. Automated targeting for the MER Rovers［C］// AIAA Unmanned... Unlimited Conference. Seattle，Washington：AIAA，2009.

［14］Stentz A. The focussed D* algorithm for real-time replanning［C］// Proceedings of IJCAI-95. ［S.l.］：IJCAI，1995.

［15］DUR E. Optical flow-based obstacle detection and avoidance behaviors for mobile robots used in unmaned planetary exploration［C］//2009 4th International Conference on Recent Advances in Space Technologies.［S.l.］：IEEE，2009.

［16］Goldberg S，Maimone M. Stereo vision and rover navigation software for planetary exploration［C］//IEEE Aerospace Conference. Big Sky，Montana：IEEE，2002.

通信地址：上海市闵行区元江路3888号805所20室（201109）

电话：（021）24186224

E-mail：weixiangquan@163.com

Researches on the Techniques of Autonomous Navigation and Path Planning for Mars Rover

WEI Xiangquan，HUANG Jianming，GU Dongqing，CHEN Feng
（Aerospace System Engineering Shanghai，Shanghai 201109， China）

Considering the mission environment，through drawing lessons from international research experience，the sensor configuration of autonomous navigation of mars rover is given，and the autonomous navigation and path planning techniques which can be used in aerospace engineering are investigated in this paper. According to the requirement of autonomous navigation and riding for rover on mars，the involved techniques are deeply researched，such as DEM（Digital Elevation Model）construction and obstacle detection，global path planning，video odometer based on stereo vision，pose determination based on multi-sensor fusion as well as local path planning. A set of experimental system was established，and the simulation experiment was carried in laboratory. The relative research results can offer some basis for the researches of autonomous navigation system for succeeding mars rover.

autonomous navigation；path planning；video odometer；multi-sensor fusion

V448.224

A

2095-7777（2016）03-0275-7

10.15982/j.issn.2095-7777.2016.03.012

魏祥泉（1974- ），男，博士，研究员， 主要研究方向：空间目标特性，视觉导航，模式识别与智能系统。

［责任编辑：宋宏］

2015-11-01；

2016-6-30

国家自然科学基金（61573247）