CN106225788B - 基于路径拓展蚁群算法的机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于路径拓展蚁群算法的机器人路径规划方法，将运用蚁群算法到机器人路径规划领域，提出路径拓展蚁群算法优化策略，优化机器人路径寻优效率，引入了信息素分布时变性、信息素更新策略、路径位置拐点优化和局部最优路径拓展，并加入位置拐点参数和总体评价作为路径的评价标准。通过对这三种算法进行仿真分析和实际试验，验证了基于路径拓展蚁群算法优化策略的机器人路径规划搜索能力更强，算法效率更高，所寻路径更短。有效抑制了算法陷入局部最优并实现了机器人最优路径搜索，使机器人可以快速地避开障碍物安全到达目标点。

Description

基于路径拓展蚁群算法的机器人路径规划方法

技术领域

本发明涉及一种路径规划技术，特别涉及一种基于路径拓展蚁群算法的机器人路径规划方法。

背景技术

路径规划技术是移动机器人研究领域的一个重要组成部分，主要目的是在有障碍物的环境中，根据一定的准则(如路径最短，位置拐点最少，用时最短等)，寻求一条从起始位置节点到目标位置节点之间的最优或次优安全无碰路径。路径规划又分为环境完全已知的全局路径规划和完全未知或部分未知的局部路径规划。

国内外对路径规划问题提出了许多可行算法，主要有可视图法、拓扑法、人工势场法等。近几年，具有启发式算法特点的计算智能技术因具有普遍适用性和较低复杂性而被用在求解移动机器人路径优化问题上，有神经网络、粒子群算法、遗传算法和蚁群算法等。这些算法各有优缺点，例如人工势场法简单且容易实现，但易陷入局部极小值；遗传算法具有很好的全局求解能力，但运算效率不高。其中，蚁群算法(ant colony optimization，ACO)的思想是源于蚂蚁觅食行为的演变，运用生物信息素(Pheromone)影响后续蚂蚁选择，在多代演变下完成路径优化。在不断的深化研究中，学者们对ACO的研究从开始单一的旅行商问题(TSP)领域拓展到了多个应用领域，已成为现在应用较为广泛的智能优化算法。

发明内容

本发明是针对路径规划提出的可行算法存在的问题，提出了一种基于路径拓展蚁群算法的机器人路径规划方法，将运用蚁群算法到机器人路径规划领域，提出路径拓展蚁群算法优化策略(Expansion Path of ant colony optimization，EP-ACO)，优化机器人路径寻优效率。

本发明的技术方案为：一种基于路径拓展蚁群算法的机器人路径规划方法，采用栅格法对机器人工作环境进行建模，获得随机地图，其中白色栅格为自由栅格，为机器人可行区域，黑色栅格为障碍栅格，为机器人不能通行的区域，单位栅格与机器人大小相当，并从左至右、从上至下对模型中的栅格进行编码，一个栅格代表一个位置节点，将路径拓展蚁群算法优化应用到移动机器人路径规划中，具体步骤如下：

1)设置最大循环次数为N_max和改进蚁群算法循环次数N_ACO，每段路径上信息素的初始值为0，设置起始点和目标点，将m只蚂蚁放于起始点；

2)每只蚂蚁根据如下状态移动规则公式选择下一个位置点，当蚂蚁到达目标点时，记录该蚂蚁路径长度及其所包含路段信息，并初始化禁忌表，

其中，s为当前有转移概率的位置节点，为蚂蚁k在位置节点i选择位置点j的转移概率；τ_ij(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度，α是次方，根据描述积累信息的重要性来设定；η_jE(t)表示从位置节点j向目标位置节点E移动的启发函数，β是次方，根据描述启发函数的重要性来设定；α和β均为正实数；η_jE(t)其值设定为E_p/L_jE，L_jE为位置节点j到目标位置节点E的距离，E_P为一适当正常数；allowed_k为t时刻允许蚂蚁k(k＝1，2，...，m)通过的位置节点集合；

3)当代k只蚂蚁全部路径规划完成后，比较出局部最优路径，运用路径位置拐点优化方法对局部最优路径进行优化，得出局部更优路径；

4)按改进蚁群算法信息素浓度更新对其该局部最优路径上的信息浓度进行全局更新，

更新策略：经过n个时刻，蚂蚁k完成了一次循环，既蚂蚁k找寻到当前的最优路径，对该路径的信息素浓度做出调整，该路径上路段(i，j)上的信息素量变化公式τ_ij(t+n)为：

τ_ij(t+n)＝ρ·τ_ij(t)+Δτ_ij(t，t+n)

其中，L_k为蚂蚁k在本次循环中所走的最优路径长度；Q(t)为蚂蚁k在最优路径上释放的信息素量；τ_ij(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度；表示蚂蚁k在时刻(t，t+n)留在路径(i，j)上的信息素量；Δτ_ij(t，t+n)表示本次循环中路径(i，j)的信息素的增量；ρ为信息素挥发率系数，设置系数ρ＜1来避免路径上信息素量的无限累加；

5)重复步骤2)、3)、4)直到循环次数N＞N_ACO，结束改进蚁群算法迭代；

6)判断是否存在最优路径；

7)运用局部最优路径拓展，对已寻局部最优路径拓展优化，寻找最优路径；

8)若循环次数N＞N_max则程序结束，否则转到步骤7；当达到最大循环次数为N_max时算法结束，数据库中保存从起始点到目标点的全局最优路径，并绘制最优路径坐标图为所需移动机器人路径规划。

所述步骤7)中局部最优路径拓展具体包括：

设已寻局部最优路径由w个位置节点组成，除起始点和目标点，在每个位置节点上放置三只蚂蚁进行拓展优化，当蚂蚁放置在第n个位置节点拓展寻路时，前n个位置节点的路径不变，在n位置节点选择下一个位置节点时，除原路径n-1和n+1位置点外，依照信息素浓度引导，运用步骤2)公式在局部最优路径周围其余位置节点中选择，找到拓展路径的下一个位置节点，后续位置节点选择根据信息素找寻目标点，在完成新路径规划，如果成功找到目标点时，记录该蚂蚁的路径长度以及包含的路段信息，在路径长度和位置拐点数量方面与局部最优路径对比，若更优，则更新局部最优路径，并根据当前局部最优路径更新信息素浓度，当w个位置节点中的所有蚂蚁都拓展完成时，本次局部最优路径拓展结束。

本发明的有益效果在于：本发明基于路径拓展蚁群算法的机器人路径规划方法，引入了信息素分布时变性、信息素更新策略、路径位置拐点优化和局部最优路径拓展，并加入位置拐点参数和总体评价作为路径的评价标准。通过对这三种算法进行仿真分析和实际试验，验证了基于路径拓展蚁群算法优化策略的机器人路径规划搜索能力更强，算法效率更高，所寻路径更短。有效抑制了算法陷入局部最优并实现了机器人最优路径搜索，使机器人可以快速地避开障碍物安全到达目标点。

附图说明

图1为本发明栅格地图模型示意图；

图2-1为本发明路径位置拐点优化情况一示意图；

图2-2为本发明路径位置拐点优化情况二示意图；

图2-3为本发明路径位置拐点优化情况三示意图；

图3为本发明路径拓展位置节点选择示意图；

图4为本发明局部最优路径拓展流程图；

图5为本发明路径拓展蚁群算法流程图；

图6-1为本发明基本蚁群算法路径规划结果图；

图6-2为本发明改进蚁群算法路径规划结果图；

图6-3为本发明路径拓展蚁群算法路径规划结果图。

具体实施方式

当机器人在复杂工作环境中行走，会受到诸多形状各异的障碍物阻挡。本发明采用栅格法对机器人工作环境进行建模，获得随机地图，如图1所示栅格地图模型图，其中白色栅格为自由栅格，为机器人可行区域，黑色栅格为障碍栅格，为机器人不能通行的区域。为了便于描述机器人行走轨迹，图1中单位栅格与机器人大小相当。并从左至右、从上至下对模型中的栅格进行编码，一个栅格代表一个位置节点。

在路径寻优过程中，蚁群算法是模拟蚁群觅食的行为，在指定环境中寻找最优路径的搜索算法。通过研究发现，蚂蚁行走时会通过在路径上释放出一种特殊的分泌物——信息素来寻找路径，当蚂蚁碰到一个未知路径选择问题时，会随机选择一条路径前进，同时留下一定量的信息素。当许多蚂蚁都走过同一路径后，该路径上的信息素浓度不断增强，后续蚂蚁选择该条路径的概率就增大；当蚂蚁行走的路径上存在障碍物时，蚂蚁能适应环境的变化，很快地重新找到新的路径。

基本蚁群算法属于启发式智能搜索，将信息正反馈原理和启发式算法有机结合，有利于发现较优解。然而，基本蚁群算法也存在以下待解决的问题：信息正反馈容易将路径搜索方向局限在一个很小的范围内，导致陷入局部最优路径，使算法出现停滞；由于积累信息趋向稳定，导致转移概率基本保持不变，算法也极易陷入停滞。本发明提出路径拓展蚁群算法(EP-ACO)，先通过信息素分布时变性、信息素更新策略、路径位置拐点优化等改进基本蚁群算法，得到优化的改进蚁群算法，再运用局部最优路径拓展，对已寻路径进一步优化。

一、改进蚁群算法

1、信息素分布时变性

蚁群算法对积累信息和启发信息的利用是随着路径搜索演化进程而变化的。运用EP-ACO搜索最优路径的过程可分为两个阶段，在这二个阶段中信息素的分布也有多样性的变化。

在路径规划第一阶段中，初始时刻可以利用的积累信息不多，各个路径位置节点上的信息素量相等，这时蚂蚁个体主要依靠启发信息，来探索完全陌生的空间，寻找优化路径。蚂蚁既要考虑下一段路的长度，也要考虑其信息素的分布强度，在相邻可行位置节点中选择下一个位置节点。

在路径优化的不断演进中，积累信息开始起主导作用，引导路径规划的方向，以此淡化局部特征的影响，加速解的收敛。

到了路径规划第二阶段，为了避免算法过早陷入局部最优路径，出现停滞现象，本发明对局部最优路径进行拓展，在已有信息素基础上“突变规划”，跳出局部最优路径，拓展规划空间，利于有效趋近全局最优路径。

2、信息素更新策略改进

(1)信息素更新方式

蚁群算法信息素更新分为局部更新和全局更新。局部更新策略为：蚂蚁个体每次从一个位置节点到下一个位置节点都会在该路径上释放信息素；全局更新策略又分为两种：在每一代规划路径周期结束时(所有蚂蚁个体路径规划完成时)，一种是选择更新全部有效路径的信息素；另一种是只有全局最优的蚂蚁才被允许释放信息素(只有那些属于全局最优路径边上的信息素才会得到增强)。

本发明采用第二种全局更新，因为其突出考虑每代全局最优路径的结果，隐含了信息反馈，能够使算法更快的收敛。虽然这种全局更新容易导致算法早熟、停滞现象，但是EP-ACO的局部路径拓展可以有效避免弊端带来的后果。本发明采用这种全局更新策略经过n个时刻，蚂蚁完成了一次循环(既蚂蚁k找寻到当前的最优路径)，对该路径的信息素浓度做出相应调整，该路径上路段(i，j)上的信息素量变化公式τ_ij(t+n)为：

τ_ij(t+n)＝ρ·τ_ij(t)+Δτ_ij(t，t+n) (1)

其中，L_k为蚂蚁k在本次循环中所走的最优路径长度；Q(t)为蚂蚁k在最优路径上释放的信息素量；τ_ij(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度；表示蚂蚁k在时刻(t，t+n)留在路径(i，j)上的信息素量，其值视蚂蚁表现的优劣程度而定。路径越短，信息素释放就越多；Δτ_ij(t，t+n)表示本次循环中路径(i，j)的信息素的增量；ρ为信息素挥发率系数，通常设置系数ρ＜1来避免路径上信息素量的无限累加。m是本次循环中路径(i，j)经过的蚁群数量。

因为信息素分布时随着累积信息和启发信息的变化也在不断地变化，所以采用时变函数Q(t)来改进基本蚁群算法中为常数项的信息素释放量Q。在路径规划演化进程中，路径规划越趋近于最优路径，局部最优路径也就越重要，所以随着路径规划进程不断进行，局部最优路径上释放的信息素量Q(t)逐渐变大，即：

Q(t)＝Q₀+k_Qt (2)

其中，k_Q值依据实验积累经验而设，不会使得Q(t)的变化量过大，在合理有效的范围内；Q₀为算法开始时初始的信息素释放量。全局信息素更新完成后，继续迭代直到满足停止条件(停止条件为达到最大迭代次数或陷入局部最优路径)。

(2)信息素浓度的限制

在改进蚁群算中的信息素更新方式，虽然能使路径规划范围集中在最优路径附近，从而加速算法的效率，但是，由于其过于强调最优路径的启发和引导作用，容易使算法导致早熟现象，通过引入最大最小蚁群系统，可以解决蚁群算法的早熟问题，最大最小蚁群系统采用区间限制信息素值域范围，具体方法为：

其中，τ_max为信息素浓度最大值；τ_min为信息素浓度最小值。

将信息素值限定在[τ_min，τ_max]之间，可以使最优路径上的信息素浓度之间的差异不会过大。通过对信息素浓度大小的限制，能一定程度上减少蚂蚁对局部最优路径的选择概率，从而解决了信息素浓度差异过大，导致的过早陷入局部最优路径问题。最大最小值具体设定是根据实际试验总结得来的。

3、路径规划优化

(1)位置节点选择优化

在基本蚁群算法中蚂蚁通过轮盘赌方法选择下一个位置节点，在t时刻，蚂蚁k在位置节点i选择位置点j的转移概率为

其中，s为当前有转移概率的位置节点，为蚂蚁k从位置节点i到位置节点j的选择概率；allowed_k为t时刻允许蚂蚁k(k＝1，2，...，m)通过的位置节点集合；τ_ij(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度，α是次方，根据描述积累信息的重要性来设定，是路径搜索的一种趋向信息，是蚂蚁从位置节点i向位置节点j移动的导向力度；η_ij(t)表示从位置节点i向位置节点j移动的启发函数，β是次方，根据描述启发函数的重要性来设定，是评价蚂蚁个体在位置节点i向位置节点j之间路段(i，j)上的搜索代价；α和β均为正实数。

但是基本蚁群算法选择路径期望值为当前位置节点到下一个位置节点的距离成反比的函数，这种情况下η_ij只有两种期望值，不利于下一个位置节点的更优选择。而采用位置节点j到目标位置节点的距离成正比的函数来设定期望值，随着位置节点j与目标位置节点的距离越小，选择该路径的期望值越大，因此使蚂蚁更加倾向于选择更接近目标位置节点的位置节点，使路径规划长度更优。下一个位置节点优化期望值后概率公式为

其中，η_jE为从位置节点j到目标位置节点E移动的启发函数，其值设定为E_P/L_jE，L_jE为位置节点j到目标位置节点E的距离，E_P为一适当正常数。

(2)路径位置拐点优化

在路径规划过程中，所得路径经常会出现一些不必要的位置节点，路径出现一些不需要的锐角或直角可以进行恰当的处理，减少机器人行走难度和长度。本发明有三种路径位置拐点处理方案如图2所示，假设连续的路径位置节点为n-1、n、n+1，如果|(n+1)-(n-1)|＝N或1时(N为栅格图的行数)，路径中存在锐角，这时将位置节点n删除，从而如图2-1有虚线路径代替原路径；如果|(n+1)-(n-1)|＝N+1或N-1时，路径中存在直角，这时将位置节点n删除，从而如图2-2有虚线路径代替原路径；如果|(n+1)-(n-1)|＝2N或2且n+1至n-1的中点为可行位置节点时，路径中存在直角，这时用n+1至n-1的中间位置节点n′代替位置节点n，从而如图2-3有虚线路径代替原路径。每次对局部最优路径进行路径位置拐点优化，直到无位置拐点可以优化时，即可得到最新局部最优路径。

二、局部最优路径拓展

为了提高蚁群算法的运算效率，减少搜索的盲目性，有效跳出局部最优，扩展搜索范围，保持解的多样性，在改进蚁群算法运行一定代数下，路径在局部区域达到最优，但不一定是全局最优路径，这时对已寻局部最优路径拓展优化，找出相对更优路径。

设已寻局部最优路径由w个位置节点(除起始点和目标点)组成，在每个位置节点上放置三只蚂蚁进行拓展优化。如图3所示，当蚂蚁放置在第n个位置节点拓展寻路时，前n个位置节点的路径不变，在n(5)位置节点选择下一个位置节点时，除原路径n-1(1)和n+1(6)位置点外，依照信息素浓度引导，运用优化期望值后概率公式(5)在其余位置节点2、3、4、7、8、9中选择，找到拓展路径的下一个位置节点，后续位置节点选择根据信息素找寻目标点。

在完成新路径规划，如果成功找到目标点时，记录该蚂蚁的路径长度以及包含的路段信息，在路径长度和位置拐点数量方面与局部最优路径对比，若更优，则更新局部最优路径，并根据当前局部最优路径更新信息素浓度。当w个位置节点中的所有蚂蚁都拓展完成时，本次局部最优路径拓展结束，流程如图4所示。

四、基于路径拓展蚁群算法的移动机器人路径规划步骤：

将EP-ACO应用到移动机器人路径规划中，应用的流程如图5所示，具体步骤如下。

步骤1：设置最大循环次数为N_max和改进蚁群算法循环次数N_ACO，每段路径上信息素的初始值为0，设置起始点和目标点，将m只蚂蚁放于起始点；

步骤2：每只蚂蚁根据状态移动规则公式(5)选择下一个位置点。当蚂蚁到达目标点时，记录该蚂蚁路径长度及其所包含路段信息，并初始化禁忌表，禁忌表是放置蚂蚁行走过的路径的，防止同一只蚂蚁再走同一路径；

步骤3：当代k只(一共有m个蚂蚁，不是所有的蚂蚁能完成路径规划，有k只蚂蚁完成了路径规划，k＜＝m)蚂蚁全部路径规划完成后，比较出局部最优路径，运用路径位置拐点优化方法对局部最优路径进行优化，得出局部更优路径；

步骤4：按改进蚁群算法信息素浓度更新公式(1)对其该局部最优路径上的信息浓度进行全局更新；

步骤5：重复步骤2、3、4直到循环次数N＞N_ACO，结束改进蚁群算法迭代；

步骤6：判断是否存在最优路径；

步骤7：运用局部最优路径拓展，对已寻局部最优路径拓展优化，寻找最优路径；

步骤8：若循环次数N＞N_max则程序结束，否则转到步骤7；当达到最大循环次数为N_max时算法结束，数据库中保存从起始点到目标点的全局最优路径，并绘制最优路径坐标图为所需移动机器人路径规划。

五、应用例

为了验证本发明算法的有效性，设置仿真环境如图6，设定障碍物分布在已知的全局静态40×40的栅格矩阵中，机器人起点为图6中位置节点1，终点为图6中位置节点1600，最优路径拓展蚁群算法主要参数如表1所示。全局路径规划结果如图6所示，图6-1为基本蚁群算法路径规划结果，图6-2为改进蚁群算法路径规划结果，图6-3为EP-ACO路径规划结果。可以看出，三种算法都能够成功避开障碍，并寻找到一条路径；但是，基本集群算法所寻路径有较多位置拐点，改进蚁群算法和EP-ACO路径规划的位置拐点较少，并且EP-ACO更进一步优化了路径长度和位置拐点数量。

表1

表2为三种蚁群算法分别运行30次的结果统计对比，其中位置拐角参数评价有效地改善了路径的平滑度；总体评价函数为2倍的位置拐角参数与路径长度之和。从表2中可以看出基本蚁群算法由于易陷入局部最优路径，出现早熟现象，所以在最优路径规划过程中花费时间较长，并且找寻的路径与全局最优路径有较大出入、位置拐点过多。

而改进蚁群算法在最优路径长度方面与EP-ACO几乎相同，从平均最优路径来看，其运行结果也相差不大，说明这两种优化算法在路径长度上的寻优效果相当。但是，从位置拐点来看，EP-ACO相比改进蚁群算法进一步优化位置拐点，提高机器人行走路径的平滑度，并缩短了寻找最优路径的时间，获得了更优路径，总体评价更优。因此，本发明方法不但能够抑制搜索陷入局部最优，还可有效地减少位置拐点个数，快速的寻出全局最优路径。

表2

Claims (2)

1.一种基于路径拓展蚁群算法的机器人路径规划方法，其特征在于，采用栅格法对机器人工作环境进行建模，获得随机地图，其中白色栅格为自由栅格，为机器人可行区域，黑色栅格为障碍栅格，为机器人不能通行的区域，单位栅格与机器人大小相当，并从左至右、从上至下对模型中的栅格进行编码，一个栅格代表一个位置节点，将路径拓展蚁群算法优化应用到移动机器人路径规划中，具体步骤如下：

1)设置最大循环次数为N_max和改进蚁群算法循环次数N_ACO，每段路径上信息素的初始值为0，设置起始点和目标点，将m只蚂蚁放于起始点；

2)每只蚂蚁根据如下状态移动规则公式选择下一个位置节点，当蚂蚁到达目标点时，记录该蚂蚁路径长度及其所包含路段信息，并初始化禁忌表，

其中，s为当前有转移概率的位置节点，为蚂蚁k在位置节点i选择位置节点j的转移概率；τ_ij(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度，α是次方，根据描述积累信息的重要性来设定；η_jE(t)表示从位置节点j向目标位置节点E移动的启发函数，β是次方，根据描述启发函数的重要性来设定；α和β均为正实数；η_jE(t)的值设定为E_P/L_jE,L_jE为位置节点j到目标位置节点E的距离，E_P为一适当正常数；allowed_k为t时刻允许蚂蚁k通过的位置节点集合，k＝1,2,…,m；

3)当代k只蚂蚁全部路径规划完成后，比较出局部最优路径：运用路径位置拐点优化方法对局部最优路径进行优化，得出最新局部最优路径；

4)按改进蚁群算法信息素浓度更新策略对最新局部最优路径上的信息素浓度进行全局更新，

更新策略：经过n个时刻，蚂蚁k完成了一次循环，即蚂蚁k找寻到当前的最优路径，对该路径的信息素浓度做出调整，该路径上路段(i，j)上的信息素量变化公式τ_ij(t+n)为：

τ_ij(t+n)＝ρ·τ_ij(t)+Δτ_ij(t,t+n)

其中，L_k为蚂蚁k在本次循环中所走的最优路径长度；Q(t)为蚂蚁k在最优路径上释放的信息素量；τ_{i j}(t)表示t时刻在位置节点i与位置节点j之间的路段(i，j)上的信息素浓度；表示蚂蚁k在时刻(t,t+n)留在路径(i，j)上的信息素量；Δτ_ij(t,t+n)表示本次循环中路径(i，j)的信息素的增量；ρ为信息素挥发率系数，设置系数ρ＜1来避免路径上信息素量的无限累加；

5)重复步骤2)、3)、4)直到循环次数N>N_ACO，结束改进蚁群算法迭代；

6)判断是否存在全局最优路径；

7)运用局部最优路径拓展，对已寻局部最优路径拓展优化，寻找全局最优路径；

8)若循环次数N>N_max则移动机器人路径规划结束，否则转到步骤7)；当达到最大循环次数N_max时路径拓展蚁群算法结束，数据库中保存从起始点到目标点的全局最优路径，并绘制最优路径坐标图为所需移动机器人路径规划。

2.根据权利要求1所述基于路径拓展蚁群算法的机器人路径规划方法，其特征在于，所述步骤7)中局部最优路径拓展具体包括：

设已寻局部最优路径由w个位置节点组成，除起始点和目标点，在每个位置节点上放置三只蚂蚁进行拓展优化，当蚂蚁放置在第n个位置节点拓展寻路时，前n个位置节点的路径不变，在第n个位置节点选择下一个位置节点时，除原路径n-1和n+1位置节点外，依照信息素浓度引导，运用步骤2)公式在局部最优路径周围其余位置节点中选择，找到拓展路径的下一个位置节点，后续位置节点选择根据信息素找寻目标点；

在完成拓展路径，成功找到目标点时，记录该蚂蚁的路径长度以及包含的路段信息，在路径长度和位置拐点数量方面与局部最优路径对比，若更优，则更新局部最优路径，并根据当前局部最优路径更新信息素浓度，当w个位置节点中的所有蚂蚁都拓展完成时，本次局部最优路径拓展结束。