CN112987735A - 一种基于Delaunay三角形的移动机器人安全路径规划方法 - Google Patents

Abstract

本发明涉及一种基于Delaunay三角形的移动机器人安全路径规划方法，提取工作环境的全局代价地图可行域的轮廓，按照设定的窗口尺寸和步长在地图内滑动窗口，计算窗口覆盖区域的分形维度；根据分形维度得到每个窗口内初始点的位置分布；以所有窗口内初始点为顶点构建Delaunay三角形网；遍历三角形网，计算每个三角形的重心，以所有三角形的重心为顶点再次构建Delaunay三角形网；遍历新生成的三角形网，剔除位于障碍物内的顶点和通过障碍物的边，将剩余顶点、边及其连接关系构成路径网；在路径网内搜索从起点至终点的一条路径；逐渐收缩路径，直至收缩前后路径相同，获取最优路径。本发明规划的路径安全可靠，且极大地提高路径搜索和剪枝的效率。

Description

一种基于Delaunay三角形的移动机器人安全路径规划方法

技术领域

本发明涉及移动机器人路径规划技术领域，尤其是涉及一种基于Delaunay三角形的移动机器人安全路径规划方法。

背景技术

路径规划是移动机器人移动的基础，是保障移动机器人在不同目标点之间正确、安全移动的必要组成部分。如何基于全局代价快速地得到移动机器人当前位置到目标位置的最优路径是移动机器人领域的一个研究热点。现阶段路径规划方法主要可以分为：

基于搜索的路径规划方法：例如利用A*算法、Dijkstra算法等进行路径搜索，将代价地图的每一个像素点作为一个节点，从某一节点出发遍历邻域寻找下一节点进而找到从起点到终点的路径，能够保证任意点可达，搜索效率较高，该类方法一般能够搜索到最短或者接近最短的全局路径，然而这种方法在搜索路径的过程中遇到障碍物会选择贴近障碍物边界绕行，这种情况下得到的路径贴近障碍物边界，既不是最优路径也不接近最优，且控制机器人沿着障碍物边界移动对机器人的运动控制方法提出了较高的要求，这类方法得到的路径并不安全。

基于概率的路径规划方法：例如基于快速搜索随机树算法(RRT)、概率路线图算法(PRM)等进行路径规划，不可避免的使用随机操作，或是随机游走或是初始化随机点，基于随机通常不会使离散点靠近障碍物边界，得到的路径相较于搜索的路径规划方法更加安全，但是这类方法的搜索效率与任意点可达存在矛盾，如果任意点可达则搜索时间将会加长，保证搜索效率则难以保证任意点可达，而且此类方法搜索的路径不具备稳定性，得到的路径随搜索次数的变化而变化。

基于智能的路径规划方法：如基于蚁群算法，鱼群算法、遗传算法、神经网络等进行路径规划，主要仿照自然界生物的习性建立搜索算法；启发式方法往往无法得到移动机器人当前位置到目标位置的最优路径，此类方法相较于前两种方法更加智能，但其搜索效率普遍低于其他方法，耗时长且很多情况下无法得到起点到终点的最佳路径甚至无法得到可行路径，且存在难以收敛和震荡等问题，较难应用于实际。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于Delaunay三角形的移动机器人安全路径规划方法。

本发明的目的可以通过以下技术方案来实现：

一种基于Delaunay三角形的移动机器人安全路径规划方法，该方法包括如下步骤：

S1：建立并读取工作环境的全局代价地图，基于Canny算子提取全局代价地图可行域的轮廓，获取轮廓地图。

S2：设定窗口尺寸和步长，按照设定的窗口尺寸和步长在轮廓地图内滑动窗口，并计算每个窗口对应覆盖区域的分形维度。

S3：根据每个窗口的分形维度，通过一个映射函数分别得到每个窗口内初始点的位置分布。

S4：以所有窗口内初始点为顶点构建Delaunay三角形网。

S5：遍历步骤S4生成的Delaunay三角形网，计算其中每个三角形的重心，以所有三角形的重心为顶点再次构建Delaunay三角形网。

S6：遍历步骤S5再次生成的Delaunay三角形网，基于Bresenham算法剔除位于障碍物内的顶点和通过障碍物的边，将剩余的顶点、边及其连接关系构成路径网。

S7：基于改进的深度优先搜索的A*算法在路径网内搜索从起点至终点的一条路径。

S8：逐渐收缩步骤S7得到的路径，直至收缩前后路径相同，获取最优路径。

步骤S1中，全局代价地图的可行域的轮廓由曲线构成，该曲线的维度介于直线的维度和面的维度之间。

本发明方法的步骤S2的具体内容为：

设定窗口的尺寸和步长，在轮廓地图内进行窗口滑动，若当前窗口内不包含边界信息，即表示当前窗口覆盖的轮廓地图范围内没有可行域轮廓，则不计算分形维度；若当前窗口覆盖的轮廓地图范围内包含可行域轮廓，则基于box-counting算法计算分形维度。基于box-counting算法计算分形维度的具体内容为：

21)分别选取box边长为[2¹,2²,...,2ⁿ]；

22)分别以不同边长的box在当前窗口区域内滑动，统计含有地图轮廓的box的数量，遍历得到所有边长的box的统计结果构成一维数组count；

23)根据分形理论获取当前窗口区域内得到的含有地图轮廓的box的数量与box的边长呈负相关，即：

式中：ε为box的边长，S为待计算分形维度的区域，函数N(ε)为区域S内边长为ε的box的个数；

以y＝log(count)、x＝log(box)＝[log2,2log2,...,nlog2]进行线性拟合，将拟合直线的斜率作为当前窗口区域的分形维度。

本发明方法的步骤S3的具体内容为：

33)将步骤S2得到当前窗口的分形维度记为D_i，并令参数d_i＝D_i-1.5，使分形维度分布在区间[0,1]之间；

34)选择映射函数

其中S_size为窗口的边长，step为步长，以step为步长在窗口区域内轮廓上选点，选择的点即为当前窗口内的所需要的初始点。

本发明步骤S4中，基于Bowyer-Watson算法以所有的初始点为顶点构建Delaunay三角形网。

进一步地，步骤S6的具体内容为：

去除重新构建的Delaunay三角形网中位于障碍物内部的顶点，基于Bresenham算法去除经过障碍物的边，将剩余的边、顶点及其连接关系作为路径网，并以路径网中的顶点为节点，将路径网中所有与该节点存在直接相连的边的顶点作为该节点的相邻节点，所述路径网包括顶点和边的连接关系，以所述路径网中的顶点为改进A*算法所考虑的节点，以路径网中顶点直接的连接关系确定节点的相邻节点。

步骤S7中，基于改进的深度优先搜索的A*算法在路径网内搜索从起点至终点的一条路径具体包括下列步骤：

71)初始化pathlist和deadlist为空，所述pathlist用以存储搜索得到的路径上的节点，所述deadlist用以存储搜索过程中被抛弃的节点；

72)将起始节点放入pathlist并设置为当前节点；

73)若当前节点是目标节点，则寻找路径并停止搜索，否则基于路径网遍历所有相邻且不处于pathlist和deadlist中的节点；

74)若不存在符合条件的相邻节点，则将当前节点放入deadlist，并将上一个节点设为当前节点，从步骤73)继续运行；

75)遍历所有相邻节点，基于评估函数F(s)计算每个节点的移动代价，其中F(s)＝G(s)+H(g)，G(s)为相邻节点到当前节点的像素距离，H(g)为相邻节点到目标节点的像素距离；

76)将代价最小的节点设为当前节点并放入pathlist中，从步骤73)继续运行。

步骤S8中，通过循环剪枝算法逐渐收缩步骤S7得到的路径，直至收缩前后路径相同，获取最优路径。具体步骤包括：

81)将步骤S7得到的路径设为当前路径；

82)遍历当前路径，每次取出相邻的三个节点n_i-1，n_i，n_i+1；

83)分别计算各节点之间的距离L(n_i-1,n_i)、L(n_i,n_i+1)、L(n_i-1,n_i+1)，其中：

式中，a_ix、a_iy分别为节点a_i的横、纵坐标；b_ix、b_iy分别为节点b_i的横、纵坐标；

84)基于Bresenham算法判断边是否经过障碍物；

85)如果L(n_i-1,n_i+1)<L(n_i-1,n_i)+L(n_i,n_i+1)，则删除节点n_i，将节点n_i-1、n_i+1连接构成新的子路径；

86)判断剩余节点构成的路径是否与当前路径相同，若相同，则路径已经得到最大收缩；若不相同，则将剩余节点构成的路径设为当前路径，从步骤82)继续运行。

本发明提供的基于Delaunay三角形的移动机器人安全路径规划方法，相较于现有技术至少包括如下有益效果：

1)本发明首先依据障碍物的分布在全局代价地图上构建路径网，以三角形的重心为Delaunay三角形的顶点，使得最终得到的路径网的每一个节点都不会靠近障碍物边界，从而使得到的路径更加安全可靠；

2)相比于原始的代价地图，路径网的节点数量大大减少，能够极大的提高路径搜索和剪枝的效率；

3)相较于概率路线图算法，本发明方法根据障碍物的分布生成离散点而不是在地图内随机生成离散点，使得生成的路径网具有稳定性得到的路径，不会随搜索次数的变化而变化，且生成的路径网能够遍布在工作环境的每一处可行域，在保证环境内任意点可达的前提下具有比概率路线图法更少的顶点和边的数量。

附图说明

图1为实施例中基于Delaunay三角形的移动机器人安全路径规划方法的流程示意图；

图2为概率路线图算法在某一次实施例中的搜索效果图；

图3为概率路线图算法在相同条件下的另一次实施例中的搜索效果图；

图4为A*算法在实施例中的搜索效果图；

图5为实施例中基于Delaunay三角形的移动机器人安全路径规划算法生成的路径网示意图；

图6为实施例中基于Delaunay三角形的移动机器人安全路径规划算法得到的路径效果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

实施例

如图1所示，本发明涉及一种基于Delaunay三角形的移动机器人安全路径规划方法，该方法包括如下步骤：

步骤一、读取建立的工作环境的全局代价地图，基于Canny算子提取全局代价地图可行域的轮廓。

工作环境的代价地图为通过其他建图算法(如cartographer建图算法等)得到的工作环境的二维地图经过膨胀处理得到的代价地图(costmap)。二值分割时，膨胀处理所覆盖的区域均认为是障碍物区域，以可行域为考虑区域进行轮廓提取而不是对障碍物进行轮廓提取，从而保证后续计算得到的初始点均分布在可行域内。全局代价地图的可行域轮廓由曲线构成，曲线的维度介于直线的维度和面的维度之间，即维度位于区间[1，2]内。

步骤二、按照设定的窗口尺寸和步长在轮廓地图内滑动窗口，分别计算每个窗口覆盖区域的分形维度。具体地：

设定滑动窗口的尺寸和步长，在轮廓地图内滑动窗口分别计算每个窗口覆盖区域的分形维度，其中如果窗口覆盖区域不包含轮廓信息则该窗口的分形维度为0，如果窗口覆盖区域包含轮廓信息，则基于box-counting算法计算分形维度。

依据分形理论，分形维度的计算需要计算极限，而轮廓地图以像素为度量单位属于数字量，因此根据实验将参数n设为

其中p为当前滑动的窗口的尺寸，以n为box-counting算法中box的数量和边长参数：即box边长分别为[2¹,2²,...,2ⁿ]。

进一步地，基于box-counting算法计算分形维度的过程为：

211)选取box边长分别为[2¹,2²,...,2ⁿ]；

212)分别以不同边长的box在当前窗口区域内滑动，统计含有地图轮廓的box的数量，遍历得到所有边长的box的统计结果构成一维数组count；

213)由分形理论可以得到当前窗口区域内得到的含有地图轮廓的box的数量与box的边长呈负相关，即：

其中，S是待计算分形维度的区域，函数N(ε)是区域S内边长为ε的box的个数；因此以y＝log(count)、x＝log(box)＝[log2,2log2,...,nlog2]进行线性拟合，拟合直线的斜率即为当前窗口区域的分形维度。

根据当前窗口区域的分形维度得到初始点的数量和分布，全局代价地图的可行域轮廓由曲线构成，曲线的维度介于直线的维度和面的维度之间，即维度位于区间[1，2]内，计算初始点的具体方法可解释为：

221)将上述过程得到当前窗口的分形维度记为D_i，并令参数d_i＝D_i-1.5，使分形维度分布在区间[0,1]之内。

222)根据每个窗口的分形维度，通过映射函数

(其中S_size是窗口的边长，step为步长)得到步长，按照步长从当前窗口区域的轮廓上选择点即为当前窗口的初始点，所有窗口的初始点构成全局的初始点。

在本实施例中，作为优选方案，将滑动窗口的尺寸设置为代价地图宽的五分之一，步长即为滑动窗口的长度，在轮廓地图内滑动窗口分别计算窗口内曲线的分形维度。

步骤三、以全局的初始点为顶点，基于Bowyer-Watson算法构建Delaunay三角形网，三角形网由多个存在公共顶点和边的三角形构成，分别计算每个三角形的重心，以这些重心为顶点重新基于Bowyer-Watson算法构建Delaunay三角形网，并抛弃第一次构建的Delaunay三角形网。遍历第二次构建的Delaunay三角形网，去除其中位于障碍物内部的顶点，基于Bresenham算法去除经过障碍物的边，将剩余的边和顶点及其连接关系作为路径网，并以路径网中的顶点为节点而不是以代价地图中的每一个像素点为节点，而节点的相邻节点则由路径网决定，路径网中所有与该节点存在直接相连的边的顶点均为该节点的相邻节点。

步骤四、经过上述步骤，保存处理得到的结果供后续规划路径使用，保存的数据有：

1、路径网上的每个顶点及其与像素坐标；

2、路径网上顶点之间的连接关系：直接连接或不连接。

步骤五、以路径网中的顶点为节点，确定机器人的起始节点和目标节点，使用深度优先搜索的A*算法进行路径规划。

路径网中节点和边的数量远远少于全局代价地图像素数量，路径网中的相邻路径从像素层面分析相距较远，且每个节点的相邻节点数量随节点的不同而不同，传统A*算法会因为遍历每个相邻节点而导致更多的时间消耗，因此基于深度优先搜索的A*算法更适合路径网。该算法具体可解释为：

5.1、初始化pathlist和deadlist为空，其中pathlist存储本发明提出的路径规划算法搜索得到的路径上的节点，deadlist存储搜索过程中被抛弃的节点；

5.2、将起始节点放入pathlist并设置为当前节点；

5.3、如果当前节点是目标节点则找到路径并停止搜索，否则基于路径网遍历所有相邻且不处于pathlist和deadlist中的节点；

5.4、如果不存在符合条件的相邻节点，则将当前节点放入deadlist，并将上一个节点设为当前节点，从步骤5.3继续运行；

5.5、遍历所有相邻节点，基于评估函数F(s)计算每个节点的移动代价，其中F(s)＝G(s)+H(g)，G(s)是相邻节点到当前节点的像素距离，H(g)是相邻节点到目标节点的像素距离；

5.6、将代价最小的节点设为当前节点并放入pathlist中，从步骤5.3继续运行。

步骤六、将起点坐标、步骤五得到的以节点为代表的路径、终点坐标依顺序排列即可得到路径。得到的路径按照顺序依次为：起点-起始节点-起始节点到目标节点的路径-目标位置。

步骤七、由于节点的分布依赖于障碍物的分布，障碍物的分布多种多样，搜索得到的路径可能存在较多的弯折和多余路径段，因此通过循环剪枝删减路径中不必要的弯折和多余路径段，具体可解释为：

7.1、将上述算法得到的路径设为当前路径；

7.2、遍历当前路径，每次取出相邻的三个节点n_i-1，n_i，n_i+1；

7.3、分别计算各节点之间的距离L(n_i-1,n_i)、L(n_i,n_i+1)、L(n_i-1,n_i+1)，其中：

式中，a_ix、a_iy分别为节点a_i的横、纵坐标；b_ix、b_iy分别为节点b_i的横、纵坐标。

7.4、基于Bresenham算法判断边是否经过障碍物；

7.5、如果L(n_i-1,n_i+1)<L(n_i-1,n_i)+L(n_i,n_i+1)，则删除节点n_i，将节点n_i-1、n_i+1连接构成新的子路径；

7.6、判断剩余节点构成的路径是否与当前路径相同：如果相同则当前路径已经得到最大收缩，得到最优路径；如果不相同则将剩余节点构成的路径设为当前路径，从步骤7.2继续运行。

本发明提供的移动机器人路径规划方法与现有技术中的路径规划方法相比，最大的创新点有三点：第一点在于，A*算法的路径规划效果如图4所示，其搜索得到的路径中有大量子路径段贴近障碍物边界，这增加了机器人跟随路径过程中与障碍物相撞的风险；而相较于A*算法，本发明依据障碍物在全局代价地图中的分布进行路径规划，基于Delaunay三角形网构建路径网，使得最终得到的路径网的每一个节点都不会靠近障碍物边界从而使得到的路径更加安全可靠，从而降低了机器人与障碍物碰撞的风险。第二点在于路径网中的节点数量远少于全局代价地图的像素点数量，节点数量的减少能够接到的提高路径搜索和剪枝的效率；第三点在于，概率路线图算法的路径规划效果如图2、图3所示，这两幅图是同一个算法在相同的条件下连续两次的运行结果，这两幅图均为机器人建立的环境地图，其中黑色的线条(白色区域和灰色区域的交界处)代表墙壁等障碍物。白色区域代表机器人可以移动的区域，灰色区域代表机器人尚未探索的区域，两幅图是在机器人slam建图系统中默认的环境地图显示方式，各图中灰色区域颜色深浅略有不同，均代表未知区域；由结果可以看出概率路线图算法并不能够保证搜索得到的路径能够使用，两次结果的对比可以发现概率路线图算法得到的结果并不稳定，其搜索得到的路径随搜索次数的不同而不同。实际本发明提出的路径规划算法生成的路径网和得到的路径如图5和图6所示，这两幅图均为机器人建立的环境地图，其中黑色的线条(白色区域和灰色区域的交界处)代表墙壁等障碍物。白色区域代表机器人可以移动的区域，灰色区域代表机器人尚未探索的区域，两幅图是在机器人slam建图系统中默认的环境地图显示方式，各图中灰色区域颜色深浅略有不同，均代表未知区域；相较于概率路线图算法，本发明方法根据障碍物的分布生成离散点而不是在全局代价地图内随机生成离散点，这使得生成的路径网不具备随机性，得到的路径不随搜索次数的变化而变化，且生成的路径网能够遍布在工作环境可行域的每一处，在保证环境内任意点可达的前提下具有比概率路线图法更少的顶点和边的数量。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，包括下列步骤：

1)建立并读取工作环境的全局代价地图，基于Canny算子提取全局代价地图可行域的轮廓，获取轮廓地图；

2)设定窗口尺寸和步长，按照设定的窗口尺寸和步长在轮廓地图内滑动窗口，并计算每个窗口对应覆盖区域的分形维度；

3)根据每个窗口的分形维度，通过一个映射函数分别得到每个窗口内初始点的位置分布；

4)以所有窗口内初始点为顶点构建Delaunay三角形网；

5)遍历步骤4)生成的Delaunay三角形网，计算其中每个三角形的重心，以所有三角形的重心为顶点再次构建Delaunay三角形网；

6)遍历步骤5)再次生成的Delaunay三角形网，基于Bresenham算法剔除位于障碍物内的顶点和通过障碍物的边，将剩余的顶点、边及其连接关系构成路径网；

7)基于改进的深度优先搜索的A*算法在路径网内搜索从起点至终点的一条路径；

8)逐渐收缩步骤7)得到的路径，直至收缩前后路径相同，获取最优路径。

2.根据权利要求1所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤2)的具体内容为：

设定窗口的尺寸和步长，在轮廓地图内进行窗口滑动，若当前窗口内不包含边界信息，即表示当前窗口覆盖的轮廓地图范围内没有可行域轮廓，则不计算分形维度；若当前窗口覆盖的轮廓地图范围内包含可行域轮廓，则基于box-counting算法计算分形维度。

3.根据权利要求2所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，基于box-counting算法计算分形维度的具体内容为：

21)分别选取box边长为[2¹,2²,...,2ⁿ]；

22)分别以不同边长的box在当前窗口区域内滑动，统计含有地图轮廓的box的数量，遍历得到所有边长的box的统计结果构成一维数组count；

23)根据分形理论获取当前窗口区域内得到的含有地图轮廓的box的数量与box的边长呈负相关，即：

式中：ε为box的边长，S为待计算分形维度的区域，函数N(ε)为区域S内边长为ε的box的个数；

以y＝log(count)、x＝log(box)＝[log2,2log2,...,nlog2]进行线性拟合，将拟合直线的斜率作为当前窗口区域的分形维度。

4.根据权利要求1所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤1)中，全局代价地图的可行域的轮廓由曲线构成，该曲线的维度介于直线的维度和面的维度之间。

5.根据权利要求4所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤3)的具体内容为：

31)将步骤2)得到当前窗口的分形维度记为D_i，并令参数d_i＝D_i-1.5，使分形维度分布在区间[0,1]之间；

32)选择映射函数

其中S_size为窗口的边长，step为步长，以step为步长在窗口区域内轮廓上选点，选择的点即为当前窗口内的所需要的初始点。

6.根据权利要求1所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤4)中，基于Bowyer-Watson算法以所有的初始点为顶点构建Delaunay三角形网。

7.根据权利要求6所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤6)的具体内容为：

去除重新构建的Delaunay三角形网中位于障碍物内部的顶点，基于Bresenham算法去除经过障碍物的边，将剩余的边、顶点及其连接关系作为路径网，并以路径网中的顶点为节点，将路径网中所有与该节点存在直接相连的边的顶点作为该节点的相邻节点，所述路径网包括顶点和边的连接关系，以所述路径网中的顶点为改进A*算法所考虑的节点，以路径网中顶点直接的连接关系确定节点的相邻节点。

8.根据权利要求7所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤7)基于改进的深度优先搜索的A*算法在路径网内搜索从起点至终点的一条路径具体包括下列步骤：

71)初始化pathlist和deadlist为空，所述pathlist用以存储搜索得到的路径上的节点，所述deadlist用以存储搜索过程中被抛弃的节点；

72)将起始节点放入pathlist并设置为当前节点；

73)若当前节点是目标节点，则寻找路径并停止搜索，否则基于路径网遍历所有相邻且不处于pathlist和deadlist中的节点；

74)若不存在符合条件的相邻节点，则将当前节点放入deadlist，并将上一个节点设为当前节点，从步骤73)继续运行；

75)遍历所有相邻节点，基于评估函数F(s)计算每个节点的移动代价，其中F(s)＝G(s)+H(g)，G(s)为相邻节点到当前节点的像素距离，H(g)为相邻节点到目标节点的像素距离；

76)将代价最小的节点设为当前节点并放入pathlist中，从步骤73)继续运行。

9.根据权利要求1所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，步骤8)中，通过循环剪枝算法逐渐收缩步骤7)得到的路径，直至收缩前后路径相同，获取最优路径。

10.根据权利要求9所述的基于Delaunay三角形的移动机器人安全路径规划方法，其特征在于，通过循环剪枝算法逐渐收缩步骤7)得到的路径，直至收缩前后路径相同，获取最优路径的具体步骤包括：

81)将步骤7)得到的路径设为当前路径；

82)遍历当前路径，每次取出相邻的三个节点n_i-1，n_i，n_i+1；

83)分别计算各节点之间的距离L(n_i-1,n_i)、L(n_i,n_i+1)、L(n_i-1,n_i+1)，其中：

式中，a_ix、a_iy分别为节点a_i的横、纵坐标；b_ix、b_iy分别为节点b_i的横、纵坐标；

84)基于Bresenham算法判断边是否经过障碍物；

85)如果L(n_i-1,n_i+1)<L(n_i-1,n_i)+L(n_i,n_i+1)，则删除节点n_i，将节点n_i-1、n_i+1连接构成新的子路径；

86)判断剩余节点构成的路径是否与当前路径相同，若相同，则路径已经得到最大收缩；若不相同，则将剩余节点构成的路径设为当前路径，从步骤82)继续运行。

Images