移动智能体在三维凹形障碍物下的路径规划

任文杰 徐啟蕾

(青岛科技大学自动化与电子工程学院 山东 青岛 266061)

0 引 言

路径规划是移动智能体研究的主要内容之一，保证移动智能体能安全无碰撞地从起点行至终点。为了更好更快地完成移动智能体的任务，还需要使移动智能体的路径距离尽可能短，使规划时间尽可能短，保证任务完成的实时性。

目前对于三维环境下的路径规划成为当前移动智能体研究的热点。文献[1]对蚁群算法进行改进，对路径节点的选取方法、信息素的更新方法、启发函数的设计进行了改进，从而避免算法陷入局部最优解。文献[2]提出了一种基于LSTM-PPO的三维路径规划算法，将收集到的状态空间和动作状态引入长短时记忆网络，通过额外的奖惩函数和好奇心驱动避开大型障碍物，最后利用PPO算法的截断项机制使得规划策略更新的幅度更加优化，可以适用于存在多样障碍物的未知环境。文献[3]使用遗传算法对无人机的三维航迹进行规划，生成了较短也光滑的路径。文献[4]在改进势场函数的基础上，通过最优化方法对引力及斥力函数中的增益系数进行调整，从而改变合力的信息，使用人工势场法规划出了可行路径，可以更好地避免局部极小值。文献[5]对A*算法进行改进，综合飞行高度和航迹长度等权重因子，设计了变权值的路径估价函数，最后提出了航线优化算法，规划出了较好的无人机航迹路线。

当环境中存在凹形障碍物时，常用的路径规划算法易陷入局部最优[6]。对凹形障碍物的处理，是目前移动智能体路径规划研究的重难点。目前在环境中存在凹形障碍物的规划算法多为二维环境下的。文献[7]提出了一种突出点法对二维环境中的凹多边形障碍物进行处理，使其可以用于对凸多边形进行处理的规划算法。文献[8]提出了一种凸点法，解决复杂环境下两点间局部最优无碰路径的规划。文献[9]提出了一种双蚁群完全交叉算法，在蚁群算法的基础上增加新型的启发因子，解决了存在凹形障碍物的路径规划问题。文献[10]提出了一种组合导航算法，使移动智能体在运行态和避障态之间切换从而解决了局部振荡的问题。

但以上算法都不能普遍适用于三维环境中存在凹形障碍物的路径规划问题。本文提出一种非最优路径凹陷点处理的方法，对三维环境中存在凹形障碍物时的地图进行处理，避免存在凹形障碍物时的局部最优，保证了移动智能体路径规划的实时性和可达性。

1 凹形障碍物存在时A*算法的局部最优

A*算法为经典的启发式算法，在基于栅格法[11]建立的栅格地图中，从起点开始搜索其邻域点，在三维环境下即搜索其26个邻域点，如图1所示，每次计算当前节点的启发值F。

F=G+H

(1)

式中：G为从起点遍历到当前节点的实际距离；H为从当前节点到终点的估计距离。

图1 邻域点搜索示意图

每次计算完启发值，比较之后将启发值最小的点设置为下一个搜索点，直到搜索至终点，算法即结束。

如图2所示，障碍物为凹形障碍物，阴影部分为内部凹面，无法从一侧经由障碍物内部穿过到达另一侧。

图2 凹形障碍物A星算法搜索示意图

其中起点S、终点T分别位于障碍物两侧，S、T都是栅格点，且直线ST和障碍物存在交点H，即沿着直线ST无法从起点S到达终点T。此时使用A*算法来规划从起点S到终点T的路径。

由于A*算法优先搜索的都是F值最小的点，而F等于从起点经过当前节点再到终点的距离。由两点之间线段最短可知，F的最小值为从起点S到终点T的直线距离。

(2)

对于直线SH上的格点，则有：

F=Fmin

(3)

即SH上的路径点的F最小，故算法会优先沿着SH的方向搜索，搜索SH上的格点或者与SH距离最近的格点。由于A*算法是在三维栅格地图下搜索栅格点的，当SH上的点非栅格点时，则距离直线SH最近的栅格点优先搜索。最后会搜索到距障碍物点H最近的栅格点O，此时沿着ST方向无法继续搜索下去，则会搜索至凹形障碍物外一点P。

设从S点搜索到O点遍历的最短距离为LSO，从O点搜索到P点遍历的最短距离为LOP，而直接从S点遍历至P点的最短距离为LSP，从P点遍历到T的最短路径为LPT。则有：

LSO+LOP+LPT>LSP+LPT

(4)

即对该凹形障碍物内部从S搜索至O点，然后再搜索至P点得到的路径非最优路径，只能得到局部最优解。且对凹形障碍物内部的搜索需要较大的计算量，使算法的运行时间较长，无法保证规划的实时性。

2 非最优路径凹陷点处理

为了防止路径规划算法在凹形障碍物存在时的局部最优，对非最优路径凹陷点进行处理来避免对易陷入局部最优时的凹形障碍物内部的搜索。

2.1 障碍物膨化

对于空间存在的随机形状的障碍物，在栅格法建立的地图中其描述较为复杂。当地图中存在不规则形状的障碍物时将其进行填充，使其充满所在的方格。图3(a)-图3(d)分别是栅格地图中的几个障碍物，其中阴影部分为障碍物的表面以及障碍物所在的栅格点。

(a) (b)

(c) (d)图3 凹形障碍物膨化处理前后图

其次，当空间中存在多个障碍物时，存在着障碍物之间比较狭窄，对于移动智能体难以通行的情况，将障碍物进行膨化处理之后则两个比较接近的障碍物就被处理为一个障碍物。

如图3(c)所示，两个球形障碍物隔得比较近，中间部分对于移动智能体来说无法通过，故将两个球形障碍物以及其中间部分处理为一个障碍物，处理完成之后如图3(d)所示。这样处理减少了障碍物的个数，减少算法需要的存储空间。

设膨化处理之前的障碍物个数为m，处理之后的障碍物个数则为n。即处理完成之后的障碍物为{Oi|i=1,2,…,n}，且m≥n(m,n∈Z+)。

2.2 非最优路径凹陷点

对于膨化处理的障碍物Oi，设其三维坐标边界分别为Xi(max)、Xi(min)、Yi(max)、Yi(min)、Zi(max)、Zi(min)。则将边界范围内的格点定义为障碍物所占据的格点PIB。

PIBi={Pj|Xi(max)≥Xj≥Xi(min),

Yi(max)≥Yj≥Yi(min),Zi(max)≥Zj≥Zi(min)}

(5)

式中：j=1,2,…,k,k∈Z+；Xj、Yj、Zj分别表示点Pj的三维坐标。

即障碍物Oi所占据的格点为：

PIBi={Pj|j=1,2,…,k}(k∈Z+)

(6)

对于PIBi中的非障碍物格点Pj，同时满足以下条件即定义为非最优路径凹陷点。

(1) 存在障碍物点Pj1、Pj2，使式(7)同时成立。

Rj1=Rj=Rj2,Uj1=Uj=Uj2,Vj1>Vj>Vj2

(7)

(2) 存在障碍物点Pj3、Pj4，使式(8)同时成立。

Rj3=Rj=Rj4,Vj3=Vj=Vj4,Uj3>Vj>Uj4

(8)

(3) 存在障碍物点Pj5，使式(9)同时成立。

Uj5=Uj,Vj5=Vj,Rj5≠Rj

(9)

式中：j、j1、j2、j3、j4和j5是从1到k的整数，且互不相等。

Rm、Um和Vm(m=j,j1,j2,j3,j4,j5)分别代表点Pm的三维坐标。用Ri(max)、Ri(min)、Ui(max)、Ui(min)、Vi(max)和Vi(min)分别表示障碍物Oi的三维坐标的边界。

当Rm表示X轴坐标时，则Um表示Y轴坐标，Vm表示Z轴坐标(或者Vm表示Y轴坐标，Um表示Z轴坐标)；当Rm表示Y轴坐标时，则Um表示X轴坐标，Vm表示Z轴坐标(或者Vm表示X轴坐标，Um表示Z轴坐标)；当Rm表示Z轴坐标时，则Um表示X轴坐标，Vm表示Y轴坐标(或者Vm表示X轴坐标，Um表示Y轴坐标)。

Rm、Um和Vm的三维坐标表示与具体的障碍物形状有关系。由于膨化处理后的障碍物的最小单位是棱长为格点宽度的正方体，所以以轮廓为类六面体的形状来对障碍物的形状进行讨论。

图4(a)、图4(b)、图4(c)分别是几种凹形障碍物，阴影部分为其内部的凹陷表面，内部为非障碍物格点。当障碍物凹陷面位于六面体的正面或者后面时，如图4(a)所示，此时Rm为Y轴坐标，Um为X轴坐标，Vm为Z轴坐标(或者Um为Z轴坐标，Um为X轴坐标)。当障碍物凹陷面位于六面体的左面或者右面时，如图4(b)所示，此时Rm为X轴坐标，Um为Y轴坐标，Vm为Z轴坐标(或者Um为Z轴坐标，Vm为Y轴坐标)。当障碍物凹陷面位于六面体的上面或者下面(底面)时，如图4(c)所示，此时Rm为Z轴坐标，Um为X轴坐标，Vm为Y轴坐标(或者Um为Y轴坐标，Vm为X轴坐标)。

(a) (b) (c)图4 非最优路径凹陷点凹形障碍物

2.3 非最优路径凹陷点判定处理

图5(a)-图5(g)为一些凹形障碍物示意图。其中阴影部分为其凹陷表面，内部为非障碍物格点。

(a) (b) (c)

(d) (e) (f)

(g) (h)图5 凹形障碍物凹陷点判定

其中图5(a)、图5(b)、图5(c)所示的凹形障碍物，可以由障碍物外一侧的点经由障碍物内的凹陷格点到达另一侧，即该格点可以成为最优路径点。在对图5(a)所示的障碍物进行分析，内部非障碍物格点显然是不满足非最优路径凹陷点的判定条件，即其内部的凹陷点不是非最优路径凹陷点，图5(b)和图5(c)所示的障碍物也同理。

又如图5(d)、图5(e)、图5(f)所示的凹形障碍物，可以由障碍物的一侧从障碍物的内部穿过到达另一侧。显然，这些障碍物内部的栅格点也可以成为最优路径点。再对图5(d)所示的障碍物分析，其内部格点也不满足非最优路径凹陷点的判定条件，则内部的凹陷点也不是非最优路径凹陷点，图5(e)和图5(f)所示的障碍物也同理。

而如图5(g)所示的凹形障碍物，无法从障碍物的一侧经由内部格点直接穿越到达另一侧。为了对图5(g)所示的凹形障碍物下的可达路径点进行分析，建立三维坐标系XYZ-O，如图5(h)所示。其中对于非障碍物格点P，存在障碍物点A、B、C、D和E使下式成立：

1)YA=YP=YB,ZA=ZP=ZB,XB>XP>XA

2)XC=XP=XD,YC=YP=YD,ZC>ZP>ZD

3)XE=XP,ZE=ZP,YE>YP

即P点为非最优路径凹陷点，应避免对P点的遍历。对障碍物内部的格点判定之后，如果为非最优路径凹陷点，应避免对其的遍历；如果不是非最优路径凹陷点，则这些栅格点也应纳入最优路径的考虑。

完整的算法如下：

步骤1初始化栅格地图和膨化处理的凹形障碍物{Oi|i=1,2,…,n}。

步骤2从i=1开始对于当前障碍物Oi得到其占据的栅格点{Pj|j=1,2,…,k}。

步骤3从j=1开始判断当前节点Pj是否为障碍物点，若是则转移至步骤5；否则转移至下一步。

步骤4判断当前格点Pj是否为非最优路径凹陷点，若是则将该格点设置为障碍物点，然后转移至下一步；否则转移至下一步。

步骤5令j=j+1，并判断j是否大于k，若是则转移至下一步，否则转移至步骤3。

步骤6令i=i+1，并判断i是否大于n，若是则转移至步骤2，否则算法结束。

完整的算法包括非最优路径凹陷点的判定和处理两部分，如图6所示。

(a) 非最优路径凹陷点处理

(b) 非最优路径凹陷点判定图6 算法流程

本文方法只是一种环境建模的方法，对环境中存在的凹形障碍物建立地图时进行处理，处理完成之后可以使用其他路径规划算法来规划路径，如Dijkstra算法[12]、深度优先算法[13]和广度优先算法[14]等。

3 仿真设计与结果

使用栅格法建立三维栅格地图，设置格点宽度为1个单位长度。给定一膨化处理之后的凹形障碍物以及起点和终点，分别使用A*算法和蚁群算法规划出一条从起点到终点的最优路径。使用MATLAB软件，在i5-Y470(2.50 GHz)、4 GB内存的笔记本电脑上运行10次。

为了避免边界碰撞，设置地图范围为X、Y和Z轴0～21，障碍物与边界无交点，移动智能体的球心范围为X、Y和Z轴1～20，即路径点的范围。起点坐标为(5,2,5)，终点坐标为(15,18,15)，仿真结果如图7所示。

(a)

(b)

(c)

(d)

(e)图7 仿真结果

图7(a)为直接使用A*算法对凹形障碍物规划的结果，图7(b)为使用非最优路径凹陷点处理的A*算法对凹形障碍物规划的结果，图7(c)为A*算法对表面无凹陷的障碍物进行规划的结果，图7(d)为使用蚁群算法对凹形障碍物规划的结果，图7(e)为使用凹陷点处理的蚁群算法对凹形障碍物规划的结果。阴影部分为凹形障碍物的内部凹陷表面、起点S和终点T。

由规划得到的路径可知，使用非最优路径凹陷点处理的A*算法避免了对凹形障碍物内部的搜索；而直接使用A*算法规划得到一条局部最优的路径，路径长度明显较长。

由表1所示的仿真数据可以看出，在凹形障碍物存在的环境直接使用A*算法搜索得到的路径不是最优路径，规划的路径较长，且对凹陷点的遍历导致了运行时间过长。而使用非最优路径凹陷点处理的A*算法规划的路径明显变短，避免了对凹陷点的遍历，算法的计算量变小，保证了实时性。最后在和图7(a)、图7(b)外部轮廓一样的无凹陷障碍物存在时使用A*算法进行路径规划，如图7(c)所示，得到了和非最优路径凹陷点处理的A*算法规划时一样的路径，路径长度也相等，算法的运行时间也比较接近，说明非最优路径凹陷点处理有效地避免了对凹陷点的遍历，能有效地避免算法陷入局部最优。

表1 规划路径长度与算法运行时间表

选取蚁群为50只，迭代次数为200，使用蚁群算法得到了如图7(d)所示的仿真结果，但是由于多次遍历导致的算法的计算量增加，且路径长度比凹陷点处理的A*算法规划的路径要长。通过凹陷点处理以后的蚁群算法仿真结果如图7(e)所示，由于避免了对凹陷区域的遍历，使算法的计算量相应地减少，且相同的蚁群数量和迭代次数得到了更短的路径。

4 结 语

本文提出一种非最优路径凹陷点处理的方法。通过障碍物膨化处理，减少了所需要的存储空间，然后给出了非最优路径凹陷点的判定方法，以及对非最优路径凹陷点的处理方法，将使规划算法陷入局部最优的可达路径点转换为障碍物点，避免了规划算法对凹形障碍物的凹陷区域的遍历，从而避免算法得到局部最优解，适用于大范围凹形障碍物环境下的移动智能体的路径规划。