基于A*算法优化的月面巡视器路径规划研究

(1 中国科学院遥感与数字地球研究所，北京 100101)(2 中国科学院大学，北京 100049)(3 北京空间飞行器总体设计部，北京 100094)

在我国的探月工程中，月面巡视器作为一个核心而又关键的系统，是目前对月球进行近距离探测的最直接有效的工具。月球表面地形复杂难以直观观测，巡视器的工作环境复杂且危险。因此在错综复杂的月球表面为巡视器规划出一条安全、高效的路线是保证巡视器能稳定工作的前提[1-2]。

月面巡视器所面临的任务环境非常复杂，尤其是月球的非结构化月面环境使得月面巡视器的导航与控制研究极具挑战性[3]。为了保证巡视器能够适应其复杂任务环境并顺利完成各种探测任务，需要巡视器具有高度的自主导航与控制能力。

国外，文献[4]提出一种适用于装备激光测距仪巡视器的路径算法，该算法以Dijkstra算法为基础，将地形倾斜度，地形粗糙度和路径长度三个指标作为该算法的成本函数，在通过激光测距仪生成的数字高程图(Digital Elevation Model，DEM)上规划路径。文献[5]提出了一种基于人工支持的自动路径规划算法，该算法可根据地图本身的特点设定路径成本函数用以优化数据，并与简单的基于时间的成本函数相比，参与者能够更好地优化更复杂的成本函数。国内，文献[6]探索出一种将A*算法和B样条函数相结合的路径规划技术，适用于月面环境静态且完全已知时做全局路径规划。文献[7]提出一种基于膨胀栅格直方图的方法，该方法考虑到传感器测量的不确定性和巡视器的尺寸，在栅格地图上对探测到的每个障碍物栅格点膨胀，该方法仅考虑路径避障但没有考虑到路径的平滑性和连续性，需要巡视器多次进行原地转向。文献[8]给出一种改进的蚁群算法，解决传统蚁群算法收敛速度慢、易陷入局部最优解的缺陷，提高月球车导航过程中路径规划的能力，但需要在运动中不断转向，降低了行驶安全性。文献[9]在栅格法的基础上提出了一种基于Theta*算法的Block-Guided Theta*算法，但算法存在局限性，只对于研究巡视器在广阔而平坦的月海区域的路径规划问题中有一定的参考价值。在月面路径规划研究方面已经提出了许多策略，并取得了不少成功的研究成果，但巡视器路径规划算法仍处在不断的发展和完善之中。

本文提出了一种基于巡视器运动约束与月表地形特点的路径规划算法，引入辅助线思想，将全局地图信息与局部地图信息相结合，生成一种规划成功率更高、行走效率更高的路径规划算法。

1 月面巡视器概况

六轮月面巡视器如图1所示,长1.5 m，宽1 m，左右两侧每侧3个车轮,车体前部桅杆上配置全景相机和导航相机用于获取巡视器前方大范围的图像，生成全局地图；车体前端避障相机用于获取巡视器前方一定范围内的精细地图，为巡视器的局部路径规划提供详细输入条件。巡视器具备曲率转向能力和原地转向能力，一般情况下巡视器采用曲率转向，最小转弯半径为1.5 m，特殊情况下采用原地转向。该巡视器具有至少20°的爬坡越障能力[10]。

路径规划的主要输入是依据立体相机拍摄生成的DEM，并根据DEM生成坡度图。依据巡视器本身的爬坡性能将坡度图转化为可行区域与不可行区域的二值地图(坡度阈值记为θT)。为了可以将巡视器视为质点进行路径规划，需要对地图中的不可行区域进行膨胀操作，膨胀半径rE一般大于巡视器长度的1/2。

2 基于局部信息的路径规划算法

2.1 基本原理

基于局部信息提出了滚动窗口避障算法，该算法利用局部地图信息，通过评价函数在规定的可选圆弧中选取最优圆弧作为执行路径，工作思路如下。

基于事先获得的全局地图(分辨率较低，主要用于指明前进方向)，设定起始点S和目标点G。然后巡视器开启避障相机，进行感知路况，有可能探测到新的障碍，巡视器结合新的探测环境和原来的全局路径方向进行局部路径规划和行走。每走一步，只考虑当前窗口即避障相机视场范围内的环境。避障相机的视场范围为巡视器前方半径为Rv的半圆区域，以此来设置滚动窗口。依据巡视器的转弯能力在滚动窗口内设置若干条待选圆弧，然后根据评价函数挑选一条最优的圆弧作为规划路径。

依据六轮巡视器的尺寸与越障能力，本文设置二值地图的阈值θT为巡视器最大越障坡度20°，膨胀半径rE为1 m。图2为根据坡度图生成的二值地图，该地图尺寸为17 m×17 m，分辨率为0.01 m。图中黑色区域为存在障碍物的不可行区域或相机视场外未知区域，白色区域为无障碍可行区域。

图2 二值地图Fig.2 Binary map

依据巡视器避障相机的性能，滚动窗口半径Rv=3 m[11]，如图3所示。依据巡视器的转弯能力，在滚动窗口内设置11条待选圆弧，圆弧半径长度固定为3 m，曲率半径不同，左右两边分别取5档，曲率半径分别为10 m，5 m，3 m，2 m和1.5 m，中间是直线(可以认为是曲率半径为∞的圆弧)。

图3 滚动窗口11条可选圆弧Fig.3 Optional arcs in rolling window

选择圆弧进行行走前需对圆弧进行二值判断，分成可行圆弧和不可行圆弧，然后对可行圆弧的可行程度值进行计算。若滚动窗口内的圆弧没有交到障碍，定义这样的圆弧为可行圆弧，否则为不可行圆弧。对于可行圆弧，为保证留有足够的空间进行转向，取其前2/3弧段为待评价的行走圆弧，即对于3 m的弧段，选取前2 m进行行走和评价。

将圆弧终点到规划目标的距离LEG作为搜索的启发函数。行走圆弧的可行程度值评价函数为

fcir=LEG

(1)

在待选圆弧中选取fcir为最小值的一个作为行走圆弧。

滚动窗口避障算法规划流程如图4所示，具体步骤如下。

步骤1：初始时，巡视器朝向目标。

步骤2：对其前方的滚动窗口进行感知，扩展出11条圆弧。

步骤3：根据判断条件选择出可行的行走圆弧，若无可行圆弧，则判断巡视器是否朝向目标：若不是则进行原地转向使巡视器朝向目标，进行步骤2；若是则规划结束，标记规划失败。

步骤4：对行走圆弧的可行程度值fcir进行评价。

步骤5：挑选fcir为最小值的行走圆弧作为行走路径。

步骤6：路径执行后，到达新的地点后，更新滚动窗口，执行步骤2至步骤6，直至巡视器到目标的距离小于一个行走步长。

步骤7：最后一步，巡视器原地转向，尝试沿直线到达目标。

图4 局部路径规划流程图Fig.4 Local path planning flow chart

2.2 仿真结果

以图 2中的二值障碍图为例，根据巡视器拍摄成像位置，设定起始点S像素坐标为(1460,1240)，目标点G像素坐标为(420,780)，其规划路径如图5所示。图5中黄色曲线为扩展圆弧，绿色曲线为可行的行走圆弧，蓝色曲线为最终的执行路径。本次路径规划共执行5次圆弧行走，在距离小于一个步长时原地转向，通过直线到达目标点，路径规划成功。

该算法对于起始点与目标点的选择具有一定的约束性。若选择不当，如起始点S坐标不变，将目标点G坐标改为(540，710)，如图6所示，巡视器执行首段圆弧后过于靠近障碍物，转向空间过小，待选圆弧皆不满足安全性要求，尝试原点转向后仍无可行圆弧，路径规划失败。

滚动窗口避障算法为启发式路劲规划算法，具有能快速趋向目标的优点。其输出路径是连续平滑圆弧，符合实际应用的巡视器行走与控制要求。缺点在于容易陷入局部死区导致规划失败。

3 引入全局辅助线的路径规划算法

3.1 基本原理

为弥补基于局部信息的滚动窗口避障算法的缺点，本文引入辅助线思想进行优化，同时使用局部信息与全局信息，首先生成一段从起点指向终点的折线段，在保证路径连续平滑的前提下，使巡视器尽可能的沿着辅助线运动，最终到达终点。

本文拟使用A*算法生成辅助线。A*算法是人工智能中的一种启发式搜索算法，它通过定义的代价函数来评估代价大小而确定最优路径，具有扩展节点少、鲁棒性好的优点，在全局路径上应用广泛[12-14]。引入全局辅助线的路径规划算法具体流程如下。

1.3 评定标准 3组患儿主要照顾者于治疗前、治疗1个月及3个月时填写SAS及SDS评定量表。SAS以标准分≥50分为阳性，SDS以标准分≥53分为阳性。比较3组治疗前后SAS、SDS评分及SAS、SDS阳性率。

进行A*算法规划路线前需先为二值地图划分栅格，栅格半径RG的设置会影响栅格赋值误差、辅助线规划速度与巡视器预留转向空间尺寸。为减少误差与计算时间以及保留一定的转向空间，本文将栅格半径RG设置为膨胀半径rE的1/2。

接着对栅格赋值，本文认为栅格内的黑色像素数量大于栅格总像素的1/2，或者栅格正中心像素为黑色像素则该栅格为障碍栅格。图7为赋值后的栅格地图，其中白色栅格为可通行栅格，黑色栅格为障碍栅格。

图7 栅格地图Fig.7 Grid map

选取起点所在栅格作为A*算法的起始栅格，终点所在栅格作为A*算法的终点栅格。这里会引发一个问题，即起点(或终点)所在栅格有可能被划定为障碍栅格，因而无法进行A*算法规划。为解决这个问题，选取起点(或终点)周围的8个栅格作为待选栅格，从中寻找非障碍栅格，并将此栅格作为新的起点(终点)栅格。若待选栅格中无非障碍栅格，则该起点(终点)实际位置可能为被障碍包围的不可到达区域，需从新选择起点(终点)坐标。

为了构造辅助线，需要寻找A*规划路径上的关键栅格，通过将关键栅格依次连接来生成辅助折线。

关键栅格定义如下：①相邻关键栅格之间的线段不穿越障碍栅格；②非相邻关键栅格之间的线段经过障碍栅格；③任意三个关键栅格不在同一直线上；④起始栅格与终点栅格均为关键栅格。

图8中绿色栅格为A*算法规划的路径，蓝色栅格为该路径中的除起始点与目标点外的关键栅格。

图8 提取关键栅格Fig.8 Extracting the key grids

依次连接关键栅格中心点，生成辅助线。图9中蓝色折线为生成的辅助线。

辅助线的使用方法如下。

首先将起始点巡视器的运动方向设置为和巡视器距离最近的辅助线段的方向，辅助线段方向为从靠近起始点的端点指向靠近目标点的端点的向量方向。更新滚动窗口并判定可行圆弧。扩展路径的评价函数为

fcir=w1LEG+w2LEA+w3θEA

(2)

式中：w1，w2，w3为权重常数；LEG为圆弧末端点距终点的距离；LEA为圆弧末端点距辅助线的距离；θEA为圆弧末端点运动方向与距离最近的辅助线段的角度差值。

其中w1LEG为启发函数，作用为使巡视器可以快速趋近于目标点，w1越大，趋近的速度越快。w2LEA+w3θEA为相似度函数，用来衡量可选圆弧与辅助线的相似程度。其中w2越大，选择的执行圆弧末端点越接近辅助线。w3越大，选择的执行圆弧末端点切线方向即巡视器行驶方向与辅助线方向更接近。w1决定了规划路径的长度，w2和w3共同决定了路径与辅助线的相似度，即规划的成功率，越相似成功率越高。当w1取1，w2和w3同时取0时评价函数变为优化前算法的评价函数。

选取fcir值最小的圆弧作为最终的执行圆弧。

若无可行圆弧，则将巡视器原地转向，使其朝向与最近的辅助线段方向相同。直至与终点的距离小于一个步长，再进行原地转向，使巡视器朝向终点，尝试直线行进到达终点。

图9 生成辅助线Fig.9 Generates auxiliary lines

3.2 仿真结果

使用局部信息算例2相同的输入，权重系数分别为w1=1，w2=1，w3=1；引入全局辅助线的规划路径如图10所示。其中黄色圆弧为不可行圆弧，绿色圆弧为可行圆弧，蓝色为选择的执行圆弧。

对于相同起始点与目标点的路径规划，引入全局辅助线前规划失败，引入后成功规划出一条平滑连续的路径。

图10 辅助线算例1路径规划结果Fig.10 Auxiliary line example 1 path planning result

同时引入全局辅助线后可以有效的缩短行进路程。如图 11所示，起始点坐标S(1460,1240)，目标点坐标G(420,780)。优化后算法首先进行五次圆弧行走后距离目标点距离小于一个步长，与局部信息算例2对比可以清楚的发现，优化后算法的最后一段直线距离小于优化前算法最后一段直线的距离。

图11 辅助线算例2路径规划结果Fig.11 Auxiliary line example 2 path planning result

4 权重系数优化及对比试验

本文对两幅地图分别使用优化前算法(局部信息)和3组不同权重系数的优化后算法(引入全局辅助线)进行4组仿真实验。其中3组优化后算法的权重参数分别设置如表1所示。

表1 3组优化后算法权重参数

两幅地图的起始点与目标点设置图12所示，其中红点为起点，蓝点为终点，每幅地图共有3个不同起点，其中图12(a)有60个不同终点，图12(b)有65个不同终点，共组成375对起始点与目标点。对比实验数据如表2所示。

图12 起始点与目标点数据组示意图Fig.12 Schematic diagram of starting point and target point data set

类别平均运动距离/m平均原地转向次数/次规划成功率/%图12(a)图12(b)图12(a)图12(b)图12(a)图12(b)优化前起点A12.2011.191.211.1076.8193.84起点B11.8611.261.031.0292.7598.46起点C10.4811.201.131.0379.7198.46平均11.5111.221.121.0583.0996.92优化后第一组起点A10.5111.511.001.00100.00100.00起点B9.9111.521.251.0088.2498.46起点C9.5611.301.001.00100.0098.46平均9.9911.441.081.0096.0798.97优化后第二组起点A10.5111.481.001.00100.00100.00起点B9.9511.371.001.0079.41100.00起点C9.5411.251.001.00100.0096.92平均10.0011.371.001.0093.1498.97优化后第三组起点A10.6811.521.001.00100.00100.00起点B10.0411.591.001.0098.53100.00起点C9.5711.461.001.00100.0098.46平均10.1011.521.001.0099.5199.49

由表2中数据可以看出，基于A*辅助线的路径规划方法优点：

(1)有效缩短巡视器的运动距离；

(2)有效减少巡视器的原地转向次数(受限起始点与目标点之间的障碍分布，极个别情况下会增加，如图12(a)优化后第一组起点B工况)；

(3)提高规划成功率，且当w2与w3的值越大时，成功率越高。

实际应用时，应选择权重常数使得w1LEG和w2LEA+w3θEA在一个数量级上，这既能保证路径快速趋向目标，又能保证较好的沿着辅助线运动。仅对于两幅地图的仿真试验，w1=0.1,w2=1.0,w3=1.0试验组相较其他两组试验组能较好的满足以上要求；试验也证明此权重常数的试验组相较于其他两组试验组成功率更高，且转动次数更少，图12(a)中算法的运动距离略长于其他两组优化后算法，但小于优化前算法，路径缩短量达到12.25%，且规划成功率显著提升，较优化前算法提升了16%，规划成功率超过99.00%。对于图12(b)，算法的运动距离虽有增加，但提高了成功率。在实际应用中，应针对不同的地图数据设定不同的权重系数，以满足w1LEG和w2LEA+w3θEA在一个数量级上的要求。

本文同时使用月面巡视器运用优化后算法进行试验，图13显示了本文算法规划的路径与巡视器真实行走的车辙印，如图所示，巡视器成功避开障碍物且行走路径平滑。试验表明，本文提出的算法在实际应用中具有可行性。

图13 规划路径与车辙印Fig.13 Planning path and track

5 结束语

本文根据巡视器的感知能力和移动能力，采用滚动窗口和基于辅助线的行走圆弧评价方法规划巡视器的局部路径。该算法将全局路径信息和局部路径规划结合在一起，避免路径陷入局部死区，规划成功率超过99.00%，显著提高了成功率；面对复杂路况可获取更短路径，路径缩短量达到12%；优化后算法平均转向次数较优化前算法减少5%，提高了巡视器的行走效率。仿真和试验证明了该方法的可行性，并在月面巡视器上进行了应用。后续将全局辅助线优化为曲线以进一步提高规划路径的全局平滑性。