基于动态三角网格和启发式搜索算法路径规划研究

1、 基于动态三角网格和启发式搜索算法路径规划研究 王文霞摘 要： 在静态三角网络的基础上设计实现了动态三角网络地图算法，通过改变三角网格地图障碍物变化过程中的网格节点代价值，设计相关算法模块并对新生成的网格地图实时获取更新信息，实现动态变化效果。结合搜索算法Anytime Replanning A*（ARA*）和D* Lite，实现在动态环境中的路径规划方法Anytime Dynamic A*（AD*），通过对网格地图中已扩展信息的保存和在有限时间内不断缩小膨胀因子，找到网格地图中代价值最小的节点，动态规划得到最优路径。在动态环境中，通过模拟动态场景图，并应用动态规划方法建立相应的数据搜索结构和

2、方法，对仿真效果进行分析和对比。Key： 动态三角网格； AD*算法； 路径规划； 模拟仿真： TN911.1?34； TM417 ： A ： 1004?373X（2017）11?0103?04Research on path planning based on dynamic triangular meshand heuristic search algorithmWANG Wenxia（Department of Computer Science and Technology， Yuncheng University， Yuncheng 044000， China）Abstract： Th

3、e dynamic triangular network map algorithm was designed and implemented on the basis of static triangular mesh to change the grid node cost value in the obstacle changing process of the triangular mesh map. The related algorithm was designed to acquire the updata information of the generated grid ma

4、p in real time to realize the dynamic change effect. The search algorithm Anytime Replanning A*（ARA*） and D* Lite are combined to realize the path planning method A*（AD*） in dynamic environmet. The extended information in grid map is saved， and the expansion factor is reduced constantly in the finit

5、e time to find the node with minimum cost value in the grid map， and obtain the optimul path of dynamic planning. The dynamic scene graph is simulated in dynamic environment. The dynamic planning method is used to establish the corresponding data search structure and method to analyze and contrast t

6、he simulation results.Keywords： dynamic triangular mesh； AD* algorithm； path planning； analog simulation随着社会经济的发展，找到合适、优化而且适用于各种额外条件如动态环境、大规模人群、任意宽度路径等一系列需求成为路径规划算法的研究方向1。本文研究在动态地图网格中通过动态搜索算法，实时更新地图信息的同时采用更加高效的搜索算法记录当前群体信息并反馈出动态环境中的环境变化情况，如人群规模、路径宽度等相关信息，以便在较短时间内避免碰撞和到达目的地。1 动态环境地图网格构建及生成1.1 动态三角网格设

7、计实现在构建动态网格（Dynamic Local Clearance Triangulation，DLCT）2的过程中需要保持原有的LCT三角网格3中对路径宽度的要求，在动态化的过程中添加和删除障碍物时通过多边形障碍物ID进行操作，在添加算法中把所有障碍物的限制条件遍历直至找到需要添加障碍物的ID；在之后的删除算法中会把与找到的障碍物ID相关的限制条件同时删除。由于在地图中障碍物之间可以相互覆盖，因此每个ID可能关联相互覆盖的限制条件。1.1.1 障碍物插入模块该模块在原有LCT（S）的存储单元中插入新的多边形障碍物，在插入过程中，给障碍物设置新的ID。S设置为现在LCT（S）已存在的限制规划

8、，O是将插入的新多边形障碍物。插入过程如下：（1） 将多边形障碍物O中的k条线段插入CDT中，对于需要修改的顶点和限制边界分别存储在两个List中。List V中存储所有已修改边的邻接顶点（其中包括因CDT交换的边）；List C中存儲在插入障碍物过程中受限制的边界。（2） 对于List C中的限制边界，通过搜索算法判断S是否被阻塞。对于每个被阻塞的遍历找到后，相关联顶点加入List V。如图1所示，通过搜索加入顶点两边可能因S引起阻隔。对每次遍历来说，相关联的阻隔顶点v被加入到List V中。图1 寻找被阻断过程（3） 在遍历过程所有被List V中顶点影响的顶点将重新测试其属性，判断在当前

9、情况是否需要被限制， Local_Ref函数定义和遍历了在变化过程中与V相邻的顶点。对于每一个所有与相邻的三角形都要被访问。设为当前与临近被访问的三角形，此时需要调用函数检测是否有遍历过的节点需要重新定义；函数查看的后继节点是否被影响，当找到被影响的路径时，顶点和若不在List V中则系统自动进行添加，然后重新测试所有在V中的顶点。1.1.2 障碍物删除模块该模块将把现有地图中的多边形障碍物移除，移除过程中通过在插入中设置的ID找到该障碍物。由于障碍物之间可以发生覆盖，故对于每一个障碍物来说可能同时具有多个ID。删除过程如下：（1） 将已存储的障碍物O从CDT中删除，并把所有已修改与边相关的顶

10、點信息存储在List V1中；（2） 将与List V1被限制顶点的周围临近顶点找到后存入List V2；（3） 把List V1中相关的顶点信息从CDT中删除；（4） 对于所有在执行过程中需要限制的顶点，通过调用Local_Ref函数再次遍历与其临近的顶点信息，并对发生变化的进行改变。1.1.3 动态三角网格构建过程基于动态三角网格地图的动态环境构建步骤如下：（1） 获取三角网格发生变化后的地图信息。（2） 获取地图中障碍物变化信息。（3） 地图网格中障碍物节点信息发生改变时，若有新节点加入，则添加后更新相邻节点；若有节点删除，则删除后更新相邻节点信息。（4） 反复执行步骤（3），直至所有的

11、更新信息都处理完毕。（5） 更新地图中三角网格节点信息。1.2 动态地图构建实现动态三角网格地图构建过程如图2所示，其中第一幅图展示了在原始地图中通过添加障碍物实现了地图中存在若干个障碍物的情况，然后通过改变地图网格中障碍物的个数、重新划分三角网格以及移动障碍物和删除算法等方法，实现了仿真过程中对真实地图情况的模拟和地图网格的构建过程。图2 动态地图实现效果2 启发式搜索动态路径规划方法2.1 启发式动态路径规划方法（AD*）Anytime Dynamic A*（AD*）算法是在原有的动态环境下将D* Lite和ARA*结合，解决了在动态环境下高效率的路径规划问题4。在AD*算法中利用ARA*

12、中膨胀因子不断递减的过程进行一系列路径搜索。当三角网格地图环境发生改变并影响到边的代价值时，在D*算法的OPEN表中对应节点的Key值发生相应改变。AD*算法具体执行过程如下：（1） 程序初始化。将OPEN，CLOSED和INCONS表清空，膨胀因子初始值为（2） 计算最短路径。计算最短路径的过程中节点的扩展顺序为从目标点goal到起始点start进行扩展，寻找出从源点到目标点的最短路径。（3） 个体从start开始沿路径向goal前进。（4） 在扩展过程中，将已扩展过的节点存储在INCONS表中。（5） 判断前进过程中网格代价值是否发生变化，如果发生变化，则以当前节点为新的起始点，并减小膨胀

13、因子的值。（6） 在下一次扩展过程中，用作为启发函数，在扩展OPEN表中剩余的点之前，把上一轮INCONS表中的点插入OPEN表中，在原来OPEN表的基础上修改所有与变化节点相关的rhs，key的值，清空CLOSE表。（7） 以新的start点为起始点，goal为目标点，运行步骤（3）直至到达目标点。2.2 地图网格密度计算基于三角网格密度的改进方法5，模拟人群仿真的过程在不同时间内每个三角网格参数值因agents数量的不同而发生变化，因此把密度信息添加到Key值的计算中，可以估算每一个OPEN表中节点的优先权从而得到最优路径。density（n）表示第个节点所在三角网格的密度值；是通过节点和

14、距离信息来估算到目标点的路径长度6。此时OPEN中扩展节点的优先权的计算公式如下：（1）式中：膨胀因子要满足条件将agents标记在不同三角形网格中，通过计算当前三角网格的密度信息决定不同节点在OPEN表中的优先权，此时Key的值计算公式（2）如下所示：（2）通过不断减小在搜索路径过程中膨胀因子的值找到最优路径。3 动态路径规划算法设计与实现3.1 搜索节点数据结构在动态环境中，以DLCT三角网格7作为地图的划分方法，将地图网格中的动态变化信息和移动个体动态搜索路径方法相关联，构造如下数据结构，在Map_Node中存储动态网格中障碍物发生变化后节点的构建网格信息；Agent_Node中记录移动

15、个体在寻路过程所扩展的三角网格边。规划路径时根据当前Open_Node节点中存放的信息计算最优路径。3.2 动态搜索算法实现在动态搜索算法中，当动态环境发生变化时更新三角网格地图信息，通过在OPEN表中扩展边，更新变化的顶点信息与其邻接节点，判断当前移动个体路径是否受到影响，未受影响的个体按原路径移动；由于网格变化需要重新寻路时，计算节点代价值如下所示：（3）通过计算新添加节点（添加障碍物）或位置变更节点（移动障碍物）的代价值得到当前三角网格地图的节点序列，在OPEN中节点进行扩展时根据代价值的排序重新规划路径，通过执行RecomputeShortestPath（）得到从当前位置为起始点到目标

16、点的路径规划方法8。例如，个体Agent移动到节点所连接的边edge时，移动障碍物模块至三角网格edge边移动方向的前方，此时动态规划域与个体移动域有交集，在此区域里由于顶点变化三角网格地图重新划分，若划分后节点不存在，通过不断执行Pred（s）找到前驱节点至此节点在更新后的网格地图中存在，同理，后继节点通过执行Succ（s）不断向后查找直至找到的后继节点与新生成的网格地图中匹配的节点（未找到则重新规划此区域的路径），在前驱节点和后继节点之间的区域D中根据式（3）计算得到网格地图变化后Agent个体从当前位置到目标点移动的规划路径。 4 基于动态环境的动态规划仿真与实现4.1 地图规模变化策略

17、为了验证动态搜索算法在不同规模三角网格地图上的适用性和模拟仿真场景中复杂逼真程度，同时证明DLCT网格划分方法对不同地图的划分结果的差异。本实验应用动态搜索算法AD*实现在不同规模地图上个体规划路径过程。如图3所示，图中地图网格的复杂程度由简到繁分别采用55，1010，2020，4040的网格拓扑结构，通过不断增大迷宫场景图的复杂程度，在地图中选择起始点和目标点后通过启发式动态路径搜索算法，AD*在不断变化的三角网格中进行路径规划，最终找到当前状态下的最优路径。通过采用不同规模地图对单一移动个体规划路径，不同的搜索算法寻路过程与地图的复杂程度正相关，随着地图复杂程度的增加，搜索路径的过程在地图

18、网格中与扩展的三角边总数呈正相关的趋势增长。因此，在复杂多变的环境中模拟真实场景时，采用高效的地图网格剖分方法能够使路径规划效率得到提高。4.2 群體移动策略4.2.1 碰撞检测方法采用的规避方法是随机处理方式和球体处理方法相结合，在Agents碰撞发生后进行处理，设置两个移动个体之间距离的最小值，若小于最小值时则认为碰撞发生。在碰撞发生后，若个体所在三角网格密度信息较大，则重新规划路径进行移动直至目标点；若个体所在三角网格在密度范围内则个体延时等待一段时间后，通过避让的方法绕过障碍物继续前行到达目标点。4.2.2 个体规避策略方法实现在动态三角网格地图环境中，当障碍物发生移动时，移动个体会根

19、据当前规划路径是否受到影响决定移动策略，根据对方向进行判断，若在移动过程中碰撞发生，记录当前位置信息CurPosition，判断当前三角网格密度值density，在密度满足阈值条件时应用规避方法重新规划路径继续移动；其他情况下，需改变移动方向，密度满足要求时进行重新规划。模拟仿真过程截图如图4图7所示，在图4和图5中，障碍物的移动没有对个体移动的路径产生影响，仍按照原规划路径前行；在图6中，当动态地图网格中障碍物变化导致网格布局发生改变，从而对个体原路径产生影响时，个体重新规划路径；在图7中，当地图网格发生动态改变时，若同时存在的若干移动个体发生碰撞，此时应用个体移动的规避策略对路径进行重新规

20、划。4.3 群体动态路径规划方法4.3.1 群体动态路径方法群体路径规划方法是在单独个体基础上对每个移动物体进行路径规划的过程。为了模拟大规模人群仿真技术提出群行为模拟方法，移动个体对不同的Agent的影响有很大差异，主要问题有如下几个方面：（1） 如在此移动个体之前有另一个体相向而行，则其中一个物体应该改变速度方向避免碰撞发生。（2） 若两个移动个体同向时，其中较慢的个体应在人群密度较小的情况下改变速度方向超过前方遮挡个体；而在人群密度较大时，跟在前方物体之后行走。综合以上问题描述，本文通过碰撞类型分类对优先级进行划分。按照优先级原则对仿真环境中的Agents个体进行优先级排序（模拟真实场景

21、中的出场顺序或根据重要程度排序等），根据优先级递减划分成不同的组别，对于中的每个Agent个体，保存其start，goal，position，DesSpeed，velDir变量信息。在规划群体路径过程中，根据Agents的优先级大小对速度方向进行修改，若在中Agents和中的移动个体移动方向相同则需要在判断优先级的条件下重新规划其余组别的路径方向。在采用以上方法避免碰撞发生的条件下，若碰撞发生，记录当前碰撞发生时移动个体的位置信息position，计算当前三角网格的密度density和相邻节点构造网格密度值，根据Agents的优先级以及已得到的密度信息进行重新排序，若此时的移动个体满足优先级条

22、件，则重新计算地图网格的代价值并重新规划由当前位置position到目标点的路径；若此时碰撞个体优先级较低，则处于等待状态至下一次重新规划。4.3.2 群体规划路径实现为了验证本文的群体动态路径规划方法，对在地图网格中可能发生碰撞的Agents个体采用碰撞规避策略对每个Agents个体进行路径规划，在仿真的过程中不断增加Agents规模，分别对Agents=50，Agents=100，Agents=150，Agents=200在不同时刻的寻路情况仿真模拟，实现结果截图如图8所示，图中不同颜色的小方格分别代表不同的Agents移动个体，每个移动个体从初始点出发到目标点规划出各自的路径，当有Agents个体之间发生相互碰撞时（此时原规划路径移动方向被阻挡），则Agent个体根据本文中采用的碰撞规避原则，采用AD*算法绕过对方后重新规划路径。图8 群体规划路径实现由仿真效果可知，随着网格地图中Agents个体数的不断增加，发生碰撞的几率明显增大，因此在动态环境中，在采用高效的动态路径搜索算法的同时，需要针对群体中发生碰撞的规避策略来避免碰撞发生。4.4 仿真实验结果分析通过对动态环境中的动态网格效果、动态路径规划算法进行验证，将二者结合实现本文关于动态环境中的路径规划方法的研究。在动态网格地图中分别移动和增添障碍物后，实现了地图网格发生变化情况的模拟。在动态