一种基于概率路线图的月球巡航车路径规划算法*
2020-04-28周相坡周依尔徐立军李小路
周相坡， 周依尔， 徐立军， 李小路
0 引 言
月球巡航车的路径规划是指月球巡航车通过自身传感器感知周围环境，自行规划出一条从起点到目标点的安全无碰撞路径，是巡航车完成自主探测任务的关键技术[1-2].月球表面的复杂地形增大了巡航车的探测难度，为保障巡航车探测过程中的安全，需要为其规划安全可靠的路径[3].传统的路径规划算法难以满足月球巡航车在安全方面的需求，本文旨在研究安全程度高的路径规划算法，保障月球巡航车能够安全完成探测任务.
路径规划算法主要分为基于图搜索的路径规划算法和基于采样的路径规划算法[4].基于图搜索的算法主要包括Dijstra，A*和D*等算法，其中Dijstra算法是一种经典的广度优先搜索算法，它从起点一层层向外扩展搜索，能够保证路径搜索结果是最短的；A*算法在Dijstra的基础上，增加了启发式函数，能够有目标性地向着目标点进行搜索，提高了算法的效率[5]；D*算法对A*算法进行了扩展，可适用于动态环境，D*算法可以智能缓存中间数据，在环境变化后，可以根据已规划的路径快速规划新的路径，“勇气号”火星巡航车采用了基于D*算法改进的Field-D*算法[6].基于采样的路径规划算法主要包括RRT和PRM算法，其中RRT算法以起点作为根节点，通过随机采样增加叶子节点的方式，生成一棵随机扩展树，当随机树中的叶子节点进入了目标区域，便可以在随机树上找到一条从起始点到目标点的路径[7]；PRM算法则对地图进行了采样处理，使用概率路线图的形式表示地图中的可通行区域，在生成概率路线图后，可基于Dijstra或A*算法在概率路线图上搜索最短路径[8].目前对于月球巡航车的路径规划算法，主要考虑月球巡航车行驶过程中的代价.文献[9]基于Dijstra算法进行路径规划，考虑了路径的距离代价与影响太阳能板发电效率的光照代价，并分析它们的不同权重对路径的影响.文献[10]综合考虑了地形可达、光照情况、通信可达等多种因素，基于A*算法设计了考虑地形行走代价、移动里程代价、操作控制代价等因素的平滑曲线路径搜索算法.
对于月球巡航车的路径规划算法，为了减少行驶代价，规划的路径往往会沿着障碍物边缘，降低了路径的安全性.月球巡航车沿着安全性低的路径行驶时，存在碰撞障碍物的潜在风险，难以满足月球巡航车在安全方面的需求.为提高路径的安全性，保障月球巡航车的安全，本文提出了基于PRM的改进算法，对PRM的采样方式进行改进，生成远离障碍物的安全路径.为了评价算法的安全性，提出了安全警戒系数和最小安全警戒系数两个指标，分别评估路径整体和局部的安全水平.最后在月球仿真环境下进行试验，对比PRM算法和A*，PRM算法，验证了改进PRM算法的有效性.
1 方 法
1.1 PRM算法
PRM是一种基于采样的路径规划算法，可以对地图进行简化，提高路径规划的效率.
在PRM算法中，移动机器人被看成一个质点，且不考虑机器人的运动学约束.假设机器人的运动环境为一个含有若干障碍物的二维平面空间，记为C，机器人可以自动移动的区域记为Cfree.PRM的基本思想是通过概率路线图表示机器人可运行的自由空间Cfree.概率路线图是一个无向图，使用G(V,E)表示，其中V表示点集，表示在地图中的位置；E表示边集，表示位置间可直线通行的路径.PRM算法包括学习阶段和查询阶段.在学习阶段，PRM算法对地图进行采样，生成概率路线图；在查询阶段，使用Dijstra或A*算法对概率路线图进行搜索，寻找起点到终点的最短路径.
(1)学习阶段.学习阶段的算法流程如算法1所示.
算法1:Learning Phase输入:map, k, l, 输出:G(V,E)1: V = [], E = []2: while length(V) < k3: p=random(2)×size(map)4: if feasible(p)5: V=[V, p]6: end if7: end while8: forh in V9: forq in V10: if feasible(
图1 概率路线图Fig.1 Probabilistic roadmap
(2)查询阶段.首先将给定的起点s和终点g连接到概率路线图中与其距离最近的节点，然后使用Dijstra或A*算法，从概率路线图中搜索出起点到终点的最短路径.
1.2 改进的PRM算法
由PRM算法原理可知，PRM算法的采样结果决定了概率路线图，当概率路线图生成后，概率路线图上的最短路径是确定的，所以采样结果决定了规划的路径.改进PRM算法的基本思想是对原始PRM算法的采样方式进行改进，通过增加远离障碍物区域的采样概率，减少距离障碍物近的区域的采样概率，控制采样点尽量远离障碍物，从而控制规划的路径尽量远离障碍物，提升路径的安全性.
为控制采样点尽量远离障碍物，借助距离变换地图进行采样.在图像处理领域中，距离变换可用于计算图像中的每一个非零点距离最近零点的距离，把二值图像转换成灰度图像，每个点的灰度值等于它到最近非零点的距离[11-12].因此可借助距离变换将原始地图中的每个点的值变换为该点到距其最近障碍物的距离，这样的地图称为距离变换地图.生成的距离变换地图如图2(b)所示，颜色越暗表示距离障碍物越近，颜色越亮表示距离障碍物越远.
图2 地图和距离变换地图Fig.2 Map and distance transform map
改进PRM算法的采样方式如算法2所示：
算法2:Sampling输入:map, dmap, k, t输出:V1: V = []2: while length(V) < k3: ps = []4: while length(ps) < t5: p=random(2)×size(map)6: if feasible(p)7: ps=[ps, p]8: end if9: end while10: pmax = max(dmap(ps))11: V= [V, pmax]12: end while13: returnV
采样算法输入为栅格地图map，距离变换地图dmap，采样点数量k和采样系数t.为了生成一个远离障碍物的采样点pmax，首先在栅格地图map中进行多次随机采样，剔除落在障碍物上的扫描点后，得到t个采样点ps；然后在距离变换地图dmap中获得t个采样点ps中距离障碍物的距离，并选取距离障碍物最远的采样点作为最终的采样点pmax，t值越大，采样点pmax远离障碍物的概率越大.将上述方法进行k次，得到k个远离障碍物的采样点pmax，将k个pmax组成点集V，然后将其输出.
通过Sampling算法，可采样得到远离障碍物的采样点集V.改进PRM算法的后续步骤与原始PRM相同，使用边集V构建概率路径图，并基于A*算法在概率路线图中搜索最短路径.改进的PRM算法，可以控制采样点远离障碍物，使规划的路径远离障碍物，提高路径的安全性.
2 试 验
2.1 月球环境仿真
在月球表面地形数据中，月表撞击坑和障碍物是决定建模逼真度的关键[13].假设只存在月表撞击坑和障碍物，对月球仿真环境进行建模.
撞击坑是指布满月球表面的环形凹坑构造，包括撞击坑环形山、辐射纹以及与撞击坑有关的隆起构造，撞击坑主要由坑底、坑唇组成.经过简化处理的撞击坑模型如图3(a)所示.
图3 撞击坑与石块模型Fig.3 Impact crater and rock model
采用双抛物线拟合并绕z轴旋转来构造月球坑模型.坑底部分采用公式(1)计算，坑唇部分模型采用式(2)计算.
(1)
(2)
式(1)、(2)中，(x,y,z)是撞击坑上的坐标，Dp为坑直径，dp为坑唇宽度，Hp为坑深，hp为坑唇高度.
障碍物主要是针对月面散落的大岩石块.一个标准的月球岩石的形状被认为是它的最小尺度和最大尺度的比介于1和1/5之间；标准高度等于直径的一半.经过简化处理的石块模型如图3(b)所示.
石块模型选用理想的抛物线模型，采用下式计算：
(3)
式(3)中，hs为石块的高度，ds为石块宽度.
建立100 m×100 m的月球表面环境，基于建立的撞击坑和石块的数学模型基础与月球地形地貌数据分析，在月球表面上，生成数目随机，大小随机的撞击坑和石块，结果如图4所示.其中撞击坑数目为3，石块的数目为6，撞击坑的最大宽度在5 m～25 m范围内，石块的最大宽度在2 m～15 m范围内.
图4 月球环境仿真结果Fig.4 Lunar environment simulation results
2.2 栅格地图生成
栅格地图是用二进制编码的栅格来表示环境地图，把不可通行的障碍栅格记为1(黑色)，可通行的自由栅格记为0(白色)，以此为基础进行路径搜索.
为生成栅格地图，首先对月球仿真环境进行采样，生成DEM数据，分辨率为0.2 m；然后从坡度，起伏度两个方面提取月球地形信息，通过对坡度和起伏度的限制，生成栅格地图[14-15].
为计算坡度和起伏度，设置如图5所示的3×3滑动窗口，其中Hi为滑动窗口中第i个栅格的高程值.使用滑动窗口遍历月球DEM的所有数据，并将窗口内的地形信息作为中心栅格的地形特征.
图5 滑动窗口Fig.5 Sliding window
地形的坡度α使用下式计算：
(4)
式(4)中，sx为中心栅格横向高程变化率；sy为中心栅格纵向高程变化率.sx和sy的计算公式如下：
(5)
(6)
式(5)、(6)中，R为DEM数据的分辨率.
地形的起伏度Hg使用下式计算：
Hg=Hmax-Hmin
(7)
式(7)中，Hmax为滑动窗口中高程最大值，Hmin为滑动窗口中高程最小值.
使用滑动窗口在月球DEM数据中依次计算，可以获得DEM数据中每个点的地形信息.将坡度阈值设置为20o，起伏度阈值设置为R/2，对于超出阈值的栅格设置为障碍.据此将月球DEM数据转换成生成的栅格地图如图6(a)所示.
图6 栅格地图Fig.6 Grid map
由图6(a)可知，通过地形信息能够有效的检测出障碍物，但撞击坑和石块模型的中部会被判断为可通行区域.在图6(a)的基础上，使用连通域对撞击坑和石块模型的中部进行检测，并在栅格地图上将其修改为障碍物，结果如图6(b)所示.
2.3 安全指标提出
为评估路径安全性，需要安全指标对其进行评估.目前，路径的评估指标主要包括路径长度和规划路径消耗时间，评估其安全性的指标较少.因此提出两个安全指标：安全警戒系数和最小安全警戒系数.
为评价路径的安全性，需要对路径进行均匀采样，并在距离变化地图上获得每个采样点距离障碍物的最小距离，得到集合D={d1,d2,…,dn}.
安全警戒系数为路径中所有采样点距离障碍物的平均距离，它反应了路径的整体安全水平.安全警戒系数Sf的计算公式表示如下：
(8)
最小安全警戒系数为路径中所有采样点距离障碍物的最近距离，它反应了路径的局部安全水平.最小安全警戒系数Sfmin的计算公式表示如下：
Sfmin=min(d1,d2,…,dn)
(9)
对于安全警戒系数和最小安全警戒系数，其计算结果数值越大，表明路径安全性越高.
2.4 试验分析
在月球仿真环境下，将改进的PRM算法与A*和PRM算法进行对比.A*算法的启发式函数设置为欧式距离，该启发式函数可以保证A*算法搜索的路径是最短的.A*算法在月球仿真环境下进行路径规划的结果如图7所示，靛蓝色线为A*算法规划的路径，当起点到终点存在可通行路径，A*算法一定能够搜索出一条可行路径，所以A*算法具有完备性.
图7 A*路径规划结果Fig.7 A* path planning results
不同于完备的A*算法，PRM算法是概率完备的，当采样点数较低时，PRM算法可能无法找到路径，当采样点数逐渐增加时，PRM算法能够找到可通行路径的概率逐渐增加到1.将PRM算法的采样点个数设置为340，步长阈值设置为13m，该参数能够保障PRM算法以接近1的概率找到起点到终点的路径.在上述参数下，PRM算法在月球仿真环境下进行路径规划的结果如图8所示，其中红色点为点集，蓝色线为边集，靛蓝色线为PRM算法规划的路径.由图7和图8可知，为保证路径较短，A*算法和PRM算法都会选择沿着障碍物的边缘前进，造成路径的安全性低.月球巡航车沿着低安全性的路径行驶时，存在碰撞障碍物的潜在风险，难以满足月球巡航车在安全方面的需求.
图8 PRM路径规划结果Fig.8 PRM path planning results
为提高路径的安全性，基于PRM算法进行改进.改进后的PRM算法，不仅需要确定采样点个数和步长阈值两个参数，还需要确定采样系数t.
当采样系数t过小时，如图9(a)所示，采样方式退化为随机采样，采样点会均匀分布在栅格地图上，规划的路径会沿着障碍物边缘，无法提高路径的安全性；当采样系数t过大时，如图9(b)所示，采样点会集中在距离障碍物较远的区域，栅格地图中的起点和终点间无法找到可行路径.
图9 采样系数对采样结果的影响Fig.9 The influence of sampling coefficient on sampling results
为确定合适的采样系数t，在采样点数和步长阈值相同，采样系数t不同的条件下，各进行100次实验，对路径规划的成功率，安全警戒系数和最小警戒安全系数进行统计，可得图10所示曲线.由图10可知，当采样系数t增大时，路径规划的成功率下降，安全警戒系数和最小警戒安全系数上升.当采样系数t大于3时，路径规划的成功率迅速下降至50%以下，安全警戒系数稳步增长，最小安全警戒系数稳定在1.10 m左右.为了平衡路径规划成功率，安全警戒系数和最小安全警戒系数，采样系数t选择为3.在保证改进PRM算法有较高的安全警戒系数和最小安全警戒系数的前提下，具有较高的路径规划的成功率.对于不同的地形，可以提前计算出成功率和安全指标的变化曲线，确定合适的采样系数，并将其存放于采样系数查找表.当月球巡航车行驶在不同地形下，可以通过采样系数查找表选取最合适的采样系数.
图10 采样系数的影响Fig.10 Effect of sampling factor
将采样点个数设置为340，步长阈值设置为13 m，采样系数t设置为3，改进的PRM算法路径规划结果如图11所示.由图11可知，改进的PRM算法，采样点会远离障碍物，所以最终规划的路径也会远离障碍物，并且对于邻近的两个障碍物，采样点有较大概率落在两个障碍物的中心位置，所以路径会从两个障碍物中心穿过，避免紧贴障碍物前进的危险情况，提高了路径的安全性.
图11 改进PRM算法的路径规划结果Fig.11 Improved PRM algorithm path planning results
在月球仿真环境下，对比A*算法，PRM算法和改进PRM算法的路径长度，消耗时间和安全警戒系数和最小安全警戒系数，数据整理为表1.计算机环境为CPU主频2.2 GHz@Intel(R) i5，内存8 GB，操作系统为Windows 10，实现软件为MATLAB 2016.在路径长度方面，A*算法具有规划路径最短的特点；PRM算法由于随机采样，规划的路径也是随机的，不具备路径最短的特点，其规划的路径较长；改进的PRM算法为了寻找较安全的路径而忽视了算法的长度，其规划的路径最长.在消耗时间方面，A*算法具有启发式搜索的特点，能够有目标性地向着终点搜索路径，其消耗时间最短；PRM算法在采样方式是随机、没有方向性的，在构建概率路线图时消耗了大量时间；改进的PRM算法，确定每一个采样点时，都要随机采样t次，消耗时间高于PRM算法.在安全指标方面，A*算法和PRM算法为使路径尽量短，生成的路径常沿着障碍物边缘，所以安全警戒系数和最小安全警戒系数较低；改进的PRM算法采用改进的采样方式，控制采样点尽量远离障碍物，所以生成的路径也会尽量远离障碍物，其安全警戒系数和最小安全警戒数值最高，说明其最安全.实际月球环境相对于仿真月球环境更加复杂，路径规划算法的安全性都会降低，A*算法、PRM算法和改善的PRM算法在路径安全性上的差距会更加明显.
表1 路径规划指标Tab.1 Path planning indicators
3 结 论
为提高月球巡航车在复杂环境下行驶的安全性，本文提出一种基于PRM的改进路径规划算法.该算法借助距离变换地图对PRM算法的采样方式进行改善，令采样点尽量远离障碍物，提高了路径的安全性.为评估路径的安全性，本文提出了安全指标：安全警戒系数和最小安全警戒系数，并在月球表面仿真环境下对A*，PRM和改进的PRM算法生成的路径进行安全评估，结果表明改进的PRM算法能够有效地提升路径的安全性.
