一种动态环境下移动机器人路径规划方法

摘要

本发明公开了一种基于全局路径规划和局部滚动预测避碰规划相结合的双层规划方法，来解决动态环境下移动机器人路径规划问题。该方法主要包括两部分：全局路径规划和局部滚动预测避碰规划。本发明能够更好地实现机器人导航，提高机器人的智能性。运用双层规划方法能够避免规划初始的盲目性，减小问题的搜索空间；针对动态障碍物运行方向的不确定性，采用两种碰撞预测策略及两种相应的碰撞避免策略，能够很好地避开动态障碍物；特别地，为更好地适应环境的变化，在第二层规划中，加入基于行为方法中的Follow_wall（沿墙）行为，可使得在环境发生变化时，移动机器人仍能够安全无碰地到达目标点。

说明书

技术领域

本发明涉及机器人路径规划、人工智能等领域，具体涉及基于全局路径规划和局部滚动 预测避碰规划相结合的双层规划方法进行动态环境下移动机器人路径规划的方法。

背景技术

二十世纪八十年代初，移动机器人的研究开始兴起，目前其研究成果主要有排爆机器人、 机器鱼、无人飞行器等，这些应用要求机器人具有很高的智能性。机器人导航是实现机器人 智能化的关键技术，而路径规划作为机器人导航的重要组成部分受到了广泛关注。经过多年 的研究，众多学者已经提出许多优秀的路径规划算法，如早期的可视图法、人工势场法、栅 格法以及后期的蚁群算法、遗传算法、神经网络算法等。由此可知机器人路径规划问题已成 为机器人相关技术中的重要研究内容。

蚁群算法是解决机器人路径规划问题最常用方法之一，它是由意大利学者M.Dorigo于 1991年首次提出，该算法模拟了自然界中蚂蚁的觅食行为，其计算过程主要包括两个阶段： 信息素的累积阶段和蚂蚁间的协作阶段。前者包括各个可行解根据累积的信息不断调整自身 结构的过程，即蚂蚁不断选择从信息素浓度高的路径上经过，进而使得该路径上蚂蚁留下的 信息素浓度越来越大，而信息素浓度低的路径，蚂蚁选择的概率会越来越小，随着时间推移 会被慢慢淘汰；在蚂蚁间的协作阶段，可行解相互间不断进行信息交流，以希望发现更加优 秀的路径，产生更好的解。蚁群算法的优点有鲁棒性强、具有高度并行性，但其容易陷入局 部最优解。

目前机器人路径规划研究多数还停留在全局环境下，即环境信息全部已知，但在实际情 况下，移动机器人对环境信息的掌握通常是不完全的，并且环境中还存在动态障碍物。在动 态环境下，由于动态障碍物运动的不确定性，机器人需要不断利用传感器获取的信息，根据 动态障碍物当前时刻的运行状态预测其在下一时刻的运行轨迹，以此进行避碰规划。动态环 境下路径规划问题已被证明为NP问题，能够高效地解决该问题，将很大程度上提高机器人 的智能性。目前已有的动态环境下路径规划方法可归纳为三类，分别是基于滚动窗口的规划 方法、行为控制方法和概率统计方法。

(1)基于滚动窗口的规划方法

滚动窗口规划方法是基于预测控制理论的一种次优方法，其基本思想是将机器人的感知 区域视为滚动窗口，在此滚动窗口内实施局部路径规划，首先需要确定局部子目标，然后采 用碰撞预测和碰撞避免策略以确保生成的局部路径与滚动窗口内动态障碍物无碰。每完成一 次局部路径规划，驱动机器人行进一步，进入下一个滚动窗口，刷新滚动窗口内环境信息， 再实施相同的策略，直到移动机器人运行到全局目标点。该方法实时性强，为复杂多变的动 态环境下路径规划问题提供了一个很好的思路，但在规划初期存在盲目性。

(2)行为控制方法

行为控制方法作为一种常用的机器人避碰与协调方法，能够适应于复杂和动态的工作环 境，它将机器人路径规划过程分解为一些具体、简单的行为集合，如趋向目标行为、沿墙行 为、避障行为和搜索行为等。行为控制方法能够很大程度上降低系统的计算复杂度，提高机 器人的反应速度，但各种行为的设计与实现是行为控制方法的难点。

(3)基于概率统计的方法

概率统计方法认为移动机器人的运动符合一定的概率分布，该方法的核心是建立动态障 碍物的运动模型，处理其运动不确定性问题，以此估算出移动机器人与动态障碍物的碰撞概 率。该方法可以削弱动态障碍物运动不确定性对机器人路径规划的影响，但动态障碍物的运 动模型很难建立。

现有技术中存在的缺陷主要有三点：（1）规划初期存在盲目性。由于动态环境下只能依 靠实时获取的环境信息进行规划，没有全局导向，规划初期可能存在盲目性。（2）动态障碍 物运动不确定性与预测避碰问题。在一些复杂的场景中，动态障碍物的运行速度和方向可能 随时发生变化，此时很难预测机器人在下一时刻是否与动态障碍物发生碰撞，从而很难给出 相应的避碰策略。（3）不能很好地适应环境的变化。机器人运行环境具有随机性和不确定性， 一旦环境发生变化，会对算法效果产生很大影响。

发明内容

本发明提供一种有效的动态环境下移动机器人路径规划方法，使其能够更好的应用于机 器人导航领域，现在动态环境下机器人路径规划方法的难点包括：如何消除规划初期的盲目 性，针对动态障碍物运动的不确定性如何提出有效的预测避碰策略以及如何适应环境变化。 这些都会对动态环境下移动机器人路径规划造成相当大的阻碍。

为了解决上述技术问题，本发明采用如下技术方案：一种动态环境下移动机器人路径规 划方法，包括如下步骤：

步骤一.利用栅格法对移动机器人运行空间进行环境建模；

步骤二.利用改进蚁群算法，不考虑环境中动态障碍物，建立全局路径（以下称为初始 路径），之后移动机器人沿此初始路径边行走边进行局部预测避碰；

二-1.设置改进蚁群算法的参数，包括蚁群中蚂蚁数量m、算法最大迭代次数N_max、信 息素权值α、启发式信息权值β、信息素衰减系数和信息素惩罚系数；初始化当前代数N＝0， 栅格地图中每条相邻边的信息素强度τ_ij＝τ₀（τ₀为常数），信息素上下限，信息素增量Δτ_ij＝0， 每个栅格的访问数T_i＝0，启发式信息函数η_rs＝1d_rs，移动机器人的初始点S和目标点G；

二-2.蚁群中蚂蚁k（k=1,2，…，m）从初始点开始出发，并将初始点添加到蚂蚁k的 禁忌表tabuk中，然后按照公式

$p_{\mathrm{rs}}^k=\left(\begin{array}{cc}\frac{{\left[{\tau }_{\mathrm{rs}}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{\mathrm{rs}}\left(t\right)\right]}^{\beta }{D_s}^{-1}{T_s}^{-1}}{\underset{s\in {\mathrm{allowed}}_k}{\Sigma }{\left[{\tau }_{\mathrm{rs}}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{\mathrm{rs}}\left(t\right)\right]}^{\beta }{D}_s^{-1}{T}_s^{-1}}& ,s\in {\mathrm{allowed}}_k\\ 0& ,\mathrm{otherwise}\end{array}\right)$

选择下个节点s，并将节点s添加到禁忌表tabu_k中；其中设置一个随机选择参数 q₀∈(0,1)，q为(0,1)之间随机数，当q＜q₀时，随机选择当前节点r周围的任一可行节点，否 则采用转移概率公式选择下一个可行点，rand(allowed_k)表示从allowed_k中随机选择一个节 点，S表示根据转移概率公式所选择的下一个节点；转移概率公式中D_s表示下一个待选节点 s到目标点的距离，(s_x,s_y)与(G_x,G_y)分别表示节点s与目标点 G的坐标。T_s表示下一节点s累计被访问的次数；

二-3.具体地，当蚂蚁i（i∈k）在搜索路径时进入死锁状态，马上启用“回退-惩罚”策 略，在蚂蚁i到达目标点时，令k=k+1，返回步骤二-2，直到k=m，即这一代所有蚂蚁完 成搜索；所述“退回-惩罚”策略步骤如下：

二-3-a.当蚂蚁i陷入死锁状态时，允许其回退一步；

二-3-b.更新蚂蚁i的禁忌表，将死锁节点从禁忌表中删除；

二-3-c.对死锁边上的信息素进行惩罚，以防其它蚂蚁再次经过死锁边；

二-3-d.蚂蚁在当前路径上重新选择新的节点;

二-4.利用每只蚂蚁的禁忌表，计算它们搜索到的路径长度，并找出其中的最短路径， 对最短路径全局信息素进行调整，再核查调整后的信息素浓度，根据信息素限定原则

${\tau }_{\mathrm{rs}}=\left(\begin{array}{cc}{\tau }_{\mathrm{rs}},& {\tau }_{\min }<{\tau }_{\mathrm{rs}}<{\tau }_{\max }\\ {\tau }_{\min },& {\tau }_{\mathrm{rs}}<{\tau }_{\min }\\ {\tau }_{\max },& {\tau }_{\mathrm{rs}}>{\tau }_{\max }\end{array}\right)$

将其限制在[τ_min,τ_max]区间内;

二-5.令迭代次数N=N+1，当N＜＝N_max，清空每只蚂蚁的禁忌表，返回步骤二-2进行 下轮迭代，直到N＞N_max，输出全局路径；

步骤三.若移动机器人到达目标点，输出最终的全局无碰路径,否则，继续执行步骤四；

步骤四.将移动机器人的感知区域视为滚动窗口，刷新滚动窗口内信息，包括动态障碍 物当前位置和初始路径等信息，机器人每行走一个步长，就刷新一次滚动窗口内信息，直到 机器人到达目标点；

四-1.若在滚动窗口内环境发生变化，且有静态障碍物停留在初始路径上，启动 Follow_wall行为，该行为可使机器人沿着静态障碍物边界绕过障碍物，并回到初始路径上， 继续预测下一滚动窗口；

四-2.在滚动窗口内进行碰撞预测，若没有检测到碰撞，机器人沿着初始路径移动一个 步长，跳转到步骤四；若预测到碰撞则进入步骤四-3；

四-3.预测到碰撞则启动碰撞避免策略，规划一条局部路径，机器人沿着规划好的局部 路径行走一个步长，跳转到步骤三。

进一步地，所述碰撞预测包含直线碰撞预测和随机碰撞预测,直线碰撞预测步骤如下：

2.1获取机器人与动态障碍物在当前滚动窗口中的运行轨迹；

2.2若机器人与动态障碍物的运行轨迹不相交，此时机器人与动态障碍物在T内不会发 生碰撞；

2.3若机器人与动态障碍物在T内的运行轨迹相交，但它们的运行方向不同，则它们有 可能发生碰撞，再进一步根据机器人和动态障碍物分别到达相交点的时间t1和t2来判断。 若它们到达相交点的时间差小于时间限值ΔT，则可判断发生侧撞，否则，没有发生碰撞；

2.4若机器人与动态障碍物的运行轨迹相交，且它们的运行方向相反，此时机器人必定 与动态障碍物发生正撞；

2.5若机器人与动态障碍物的运行轨迹相交，且它们的运行方向相同，则它们有可能发 生碰撞，此时获取当前滚动窗口内，距离机器人或动态障碍物最远的轨迹交点，再利用(2-3) 中的方法判断机器人与动态障碍物发生碰撞与否；

所述T为机器人移动一个步长所需时间,T内动态障碍物所能到达的区域范围，称为该障 碍物的T-膨化区域。

进一步地，随机碰撞预测步骤如下：

3.1建立动态障碍物的T-膨化区域；

3.2若原始路径不会与膨化区域相交，则肯定不会发生碰撞；

3.3若原始路径与膨化路径相交，则有可能会发生碰撞。

进一步地，所述碰撞避免策略包含直线碰撞避免和随机碰撞避免，直线碰撞避免策略步 骤如下：

4.1若预测到机器人与动态障碍物发生侧面碰撞，机器人只需在到达碰撞点之前停顿t 时间即可，然后沿着初始路径行进一个步长；

4.2若预测到机器人与动态障碍物发生正面碰撞，则不能再按照原始路径行进，需要重 新规划一条新的局部路径；

4.3确定局部子目标，并将局部子目标设为滚动窗口边界与原始路径的交点所在栅格；

4.4将碰撞发生所在栅格视为静态障碍物，再利用改进蚁群算法在此滚动窗口内规划一 条无碰最优的局部路径。

进一步地，随机碰撞避免策略步骤如下：

5.1在由“随机碰撞预测”方法预测到可能发生碰撞时，将机器人一个步长所需时间T 划分为μ等份，即T=μΔt，Δt时间机器人行走步长为Δε；

5.2启动“随机碰撞预测”方法，预测机器人与动态障碍物在未来Δt时间内是否发生碰 撞；

5.3若不会发生碰撞，机器人沿初始路径行走Δε，进入下一Δt滚动窗口，转（5.2），直 至走完一个步长ε，进入下一T滚动窗口；

5.4若会发生碰撞，对滚动窗口内所有动态障碍物进行Δt膨化处理，生成局部子目标， 将滚动窗口内所有动态障碍物的Δt膨化区域作为静态障碍物，利用步骤一至步骤二获取局部 最优路径；

5.5机器人沿着规划的局部路径行走一个步长ε，进入下一T滚动窗口。

与现有技术相比，本发明具有以下有益效果：以改进蚁群算法、滚动窗口原理以及 Follow_wall行为作为主要处理工具，利用全局路径规划和局部滚动预测避碰规划相结合的 双层规划方法来实现动态环境下移动机器人路径规划。

A.利用双层规划的架构，第一层规划为机器人提供一个全局导向，能够改善规划初期 的盲目性；

B.利用改进蚁群算法作为第一层规划方法，可避免初始路径陷入局部最优解，一定程 度上有利于获取全局无碰路径；

C.在第二层规划中加入基于行为方法中的Follow_wall行为，使得规划过程中能够更 好地适应环境的变化；

D.针对动态障碍物做直线运动和随机运动，我们给出两种碰撞预测方法，两种方法都 能够有效地预测机器人与障碍物在下一时刻的碰撞情况；

E.对应两种碰撞预测方法，我们给出两种碰撞避免策略，两种策略都能够有效地避开 滚动窗口中的动态障碍物。

附图说明

图1为动态环境下移动机器人路径规划整体流程图；

图2为基于栅格法的环境建模示例图；

图3为蚁群算法中路径进入死锁状态图；

图4为改进蚁群算法流程图；

图5为Follow_wall行为示意图；

图6为“直线碰撞预测”方法流程图；

图7为“随机碰撞预测”方法流程图；

图8为“直线碰撞避免”策略流程图；

图9为“随机碰撞避免”策略流程图；

具体实施方式

下面将结合附图及具体实施方式对本发明作进一步的描述。

参见图1,一种动态环境下移动机器人路径规划方法，首先利用栅格法对机 器人运行空间进行环境建模，接着进入第一层规划全局路径规划，利用改进蚁 群算法为移动机器人规划一条全局路径，然后进入第二层规划局部滚动预测避 碰规划，机器人沿着规划好的全局路径，边走边进行局部预测避碰，随着滚动 窗口的推进，最终可以获取一条全局无碰路径。

其中，如图2所示，利用栅格法进行环境建模的主要思想是：将机器人运 行空间E的左下角设为坐标原点，横向向右为X轴，纵向向上为Y轴，将E划 分为n*m个相同大小的方形栅格，对划分好的栅格从左到右，从下到上进行编 号，栅格编号i与坐标(x_i,y_i)的对应关系如下公式所示。x_i＝imodN y_i＝(int)(i/N)。

建立环境模型后，接着是全局路径规划，不考虑环境中动态障碍物，利用 改进蚁群算法为机器人规划一条全局路径。对蚁群算法的改进有三点：

(1)转移概率调整

为增加解的多样性，在转移概率中增加一个随机扰动，具体做法是，设置 一个随机选择参数q₀∈(0,1)，q为(0,1)之间随机数，当q＜q₀时，随机选择当前节 点r周围的任一可行节点，否则采用转移概率公式选择下一个可行点，如下公式 (1)所示：

其中rand(allowed_k)表示从allowed_k中随机选择一个节点，S表示根据转移概率 公式所选择的下一个节点。此外对转移概率公式新增两个启发式因子，以增强 改进蚁群算法全局搜索能力，新的转移概率公式如(2)所示：

$p_{\mathrm{rs}}^k=\left(\begin{array}{cc}\frac{{\left[{\tau }_{\mathrm{rs}}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{\mathrm{rs}}\left(t\right)\right]}^{\beta }{D_s}^{-1}{T_s}^{-1}}{\underset{s\in {\mathrm{allowed}}_k}{\Sigma }{\left[{\tau }_{\mathrm{rs}}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{\mathrm{rs}}\left(t\right)\right]}^{\beta }{D}_s^{-1}{T}_s^{-1}}& ,s\in {\mathrm{allowed}}_k\\ 0& ,\mathrm{otherwise}\end{array}\right)---\left(2\right)$

其中D_s表示下一个待选节点s到目标点的距离，(s_x,s_y)与(G_x,G_y)分别表示节点s与目标点G的坐标。T_s表示下一节点s累计被访 问的次数。

(2)信息素阀值限定

为了进一步减小改进蚁群算法陷入局部最优解的几率，提高算法全局搜索 能力，借鉴最大最小蚂蚁系统（MMAS）的特征，在每一代循环结束后，对最 优路径信息素强度进行全局更新时，引入信息素上下限。信息素限定原则如(3) 式所示：

${\tau }_{\mathrm{rs}}=\left(\begin{array}{cc}{\tau }_{\mathrm{rs}},& {\tau }_{\min }<{\tau }_{\mathrm{rs}}<{\tau }_{\max }\\ {\tau }_{\min },& {\tau }_{\mathrm{rs}}<{\tau }_{\min }\\ {\tau }_{\max },& {\tau }_{\mathrm{rs}}>{\tau }_{\max }\end{array}\right)---\left(3\right)$

(3)死锁处理

在一些复杂环境下，如障碍物较多且呈现“U”型或“V”型，移动机器人 很难避开这些障碍物而陷入死锁状态。如图3所示，当移动机器人运行到R位 置时，不能再向R周围的任一位置移动，此时蚂蚁陷入死锁状态。

本发明提出一种“回退-惩罚”策略来解决死锁问题。定义处于死锁位置的 节点为死锁节点，如图3中的R节点，定义进入死锁点R的边为死锁边，如图 3中的边QR。“回退-惩罚”策略的具体思想如下所示：

（1）当某只蚂蚁陷入死锁状态时，允许其回退一步；

（2）更新该蚂蚁的禁忌表，将死锁节点从禁忌表中删除；

（3）对死锁边上的信息素进行惩罚，以防其它蚂蚁再次经过死锁边；

（4）蚂蚁在当前路径上重新选择新的节点。

改进蚁群算法流程图如图4所示，基于改进蚁群算法的全局路径规划包括 如下步骤：

（1）设置改进蚁群算法的参数，包括蚁群中蚂蚁数量m、算法最大迭代 次数N_max、信息素权值α、启发式信息权值β、信息素衰减系数和信 息素惩罚系数。初始化当前代数N＝0，栅格地图中每条相邻边的信 息素强度τ_ij＝τ₀（τ₀为常数），信息素上下限，信息素增量Δτ_ij＝0， 每个栅格的访问数T_i＝0，启发式信息函数η_rs＝1d_rs，移动机器人的 初始点S和目标点G。

（2）蚁群中蚂蚁k（k=1,2，…，m）从初始点开始出发，并将初始点添 加到蚂蚁k的禁忌表tabu_k中，然后按照公式(1)和(2)选择下个节点s， 并将节点s添加到禁忌表tabu_k中。

（3）若蚂蚁k在搜索路径时进入死锁状态，马上启用“回退-惩罚”策略， 在蚂蚁k到达目标点时，令k=k+1，返回（2），直到k=m，即这 一代所有蚂蚁完成搜索。

（4）利用每只蚂蚁的禁忌表，计算它们搜索到的路径长度，并找出其中 的最短路径，对最短路径全局信息素进行调整，再核查调整后的信 息素浓度，将其限制在[τ_min,τ_max]区间内。

（5）令迭代次数N=N+1，当N＜＝N_max，清空每只蚂蚁的禁忌表，返回

（2）进行下轮迭代，直到N＞N_max，改进蚁群算法结束，输出全局

最优路径。

由第一层规划获取全局路径后，接着进入局部滚动预测避碰规划，该部分 是动态环境下机器人路径规划核心内容。为更好地适应环境变化，当环境中静 态障碍物位置发生变动，并停留在初始路径上时，我们引入基于行为控制方法 中的Follow_wall行为，如图5所示。该行为可使得机器人沿着障碍物边界回 到初始路径上。

当机器人在其滚动窗口中感知到动态障碍物存在时，需要预测它们在下一 时刻是否发生碰撞，针对动态障碍物直线运动和随机运动，我们采用两种碰撞 预测方法，分别称为“直线碰撞预测”和“随机碰撞预测”。“直线碰撞预测” 方法的具体步骤如下，流程图如图6。

（1）获取机器人与动态障碍物在当前滚动窗口中的运行轨迹；

（2）若机器人与动态障碍物的运行轨迹不相交，此时机器人与动态障碍 物在T内不会发生碰撞；

（3）若机器人与动态障碍物在T内的运行轨迹相交，但它们的运行方向 不同，则它们有可能发生碰撞，再进一步根据机器人和动态障碍物 分别到达相交点的时间t1和t2来判断。若它们到达相交点的时间 差小于时间限值ΔT，则可判断发生侧撞，否则，没有发生碰撞；

（4）若机器人与动态障碍物的运行轨迹相交，且它们的运行方向相反。 此时机器人必定与动态障碍物发生正撞；

（5）若机器人与动态障碍物的运行轨迹相交，且它们的运行方向相同， 则它们有可能发生碰撞。此时，获取当前滚动窗口内，距离机器人 或动态障碍物最远的轨迹交点，再利用(2)中的方法判断机器人与动 态障碍物发生与否；

动态障碍物随机运动时，虽然无法预测其在下一时刻精确的路径信息，但 是可以预测其在下一时刻T（机器人移动一个步长所需时间）内所能到达的区域 范围，称为T-膨化区域，然后判断初始路径与该膨化区域的相交情况即可预测 碰撞的可能性。“随机碰撞预测”方法的具体步骤如下，其流程图如图7所示。

（1）建立动态障碍物的膨化模型；

（2）若原始路径不会与膨化区域相交，则肯定不会发生碰撞；

（3）若原始路径与膨化路径相交，则有可能会发生碰撞；

在根据碰撞预测方法预测到机器人与动态障碍物会发生碰撞时，需要采用 碰撞避免策略避开它们之间的碰撞。针对两种碰撞预测方法，我们给出两种相 应的碰撞避免策略，分别称之为“直线碰撞避免”策略和“随机碰撞避免”策 略。“直线碰撞避免”策略的流程图见图8，具体步骤如下所示：

（1）若预测到机器人与动态障碍物发生侧面碰撞，机器人只需在到达碰 撞点之前停顿t时间即可，然后沿着初始路径行进一个步长；

（2）若预测到机器人与动态障碍物发生正面碰撞，则不能再按照原始路 径行进，需要重新规划一条新的局部路径；

（3）确定局部子目标。为了保证机器人在之后的滚动窗口内能够沿着原 始路径行进，可将局部子目标设为滚动窗口边界与原始路径的交点 所在栅格。由于原始路径为全局无碰路径，故子目标不会落在静态 障碍物上；

（4）将碰撞发生所在栅格视为静态障碍物，再利用改进蚁群算法在此滚 动窗口内规划一条无碰最优的局部路径；

（5）机器人沿着这条局部路径行走一个步长，进入下一个滚动窗口。

“随机碰撞避免”策略的流程图见图9，具体步骤如下所示：

（1）在由“随机碰撞预测”方法预测到可能发生碰撞时，将机器人一个 步长所需时间T划分为μ等份，即T=μΔt，Δt时间机器人行走步长 为Δε；

（2）启动“随机碰撞预测”方法，预测机器人与动态障碍物在未来Δt时 间内是否发生碰撞；

（3）若不会发生碰撞，机器人沿初始路径行走Δε，进入下一Δt滚动窗口， 转（2），直至走完一个步长ε，进入下一T滚动窗口；

（4）若会发生碰撞，对滚动窗口内所有动态障碍物进行Δt膨化处理，生 成局部子目标，将滚动窗口内所有动态障碍物的Δt膨化区域作为静 态障碍物，利用改进蚁群算法获取局部最优路径；

（5）机器人沿着规划的局部路径行走一个步长ε，进入下一T滚动窗口。