基于环境建模与自适应窗口的移动机器人路径规划方法

摘要

基于环境建模与自适应窗口的移动机器人路径规划方法，涉及一种移动机器人实时路径规划方法。一个进行多约束局部环境建模和分析的步骤；一个可通过性分析的步骤；一个安全性分析的步骤；一个运动平稳性分析的步骤：一个目标引导性分析的步骤；一个采用自适应窗口的路径规划的步骤。由于具有更好的环境适应性和避障能力，获得较好的安全性和可达性，并具有较高的计算实时性，因此解决了不确定复杂环境下移动机器人实时生成避障路径的问题，提供了一种综合最优的路径选择方法，较好地满足移动机器人的避障要求，实现机器人的实时路径规划和控制，为移动机器人的导航应用提供一个有效的无碰撞路径规划方法。

说明书

技术领域

本发明涉及一种移动机器人路径规划方法，尤其是涉及一种基于局部环境建模和自适应窗口的移动机器人实时路径规划方法。

背景技术

路径规划是指找到一条从起始点到目标点之间的安全无碰撞路径，是机器人导航研究中的重要课题。按对环境知识的了解，可分为已知环境和未知环境下的路径规划。无论机器人路径规划属于哪种类别，采用何种规划方法，基本上都要遵循以下步骤：1)建立环境模型，即将机器人所处的现实世界进行抽象后建立相关的模型；2)搜索无碰撞路径，即在某个模型的空间中寻找合乎条件的路径的搜索算法。在全局规划中，机器人周围的环境信息是完全已知的，因而可以离线地规划出一条最优路径。最优的标准可以是最短、最安全或所需能量最小等。这方面已有许多解决方法，如单元分解法、投影法、路标法、栅格法、可视图法等。全局方法通常可以寻找最优解，但是需要预先知道环境的准确信息，并且计算量很大。与全局方法相比，实时的局部路径规划侧重于考虑机器人当前探测到的环境信息，让机器人具有更好的避碰能力。这方面最具代表性的研究是人工势场法、模糊逻辑法、滚动窗口法等。

移动机器人的路径规划算法是伴随着移动机器人的发展为满足机器人的需求而发展的。当今，无人地面、水下、空中机器人发展迅速，足球机器人比赛如火如荼，并且机器人正朝着微小型化和多机器人协作方向发展。随着星球探测和无人战争的需要，对机器人的研究越来越注重于在复杂、非结构化的未知复杂环境中自主导航。为了满足移动机器人发展的需要，路径规划正在并且将会向高维自由度机器人、多机器人协调、动态未知环境中的规划发展。

公告号为CN1230478的发明专利提供一种机器人行走路径规划装置和方法。该装置包括规划器、低通滤波器、伺服控制器和伺服马达。所述方法包括如下步骤：收到对应于预定目标位置的信号输入以后，计算初始速度分布曲线；通过使用一个单级低通滤波器，对初始速度分布曲线进行内插处理而获得经滤波处理的速度分布曲线；以及利用经滤波处理的速度分布曲线来控制伺服马达的加速和减速。

发明内容

本发明的目的在于提供一种基于环境建模与自适应窗口的移动机器人路径规划方法。

本发明包括以下步骤：

1)一个进行多约束局部环境建模和分析的步骤；

2)一个可通过性分析的步骤；

3)一个安全性分析的步骤；

4)一个运动平稳性分析的步骤：

5)一个目标引导性分析的步骤；

6)一个采用自适应窗口的路径规划的步骤。

在步骤1)中，所述多约束局部环境建模和分析的步骤是：机器人用激光传感器感知障碍物的信息，每一个障碍物探测点相对于机器人的位置可由激光传感器直接得出，角度扫描间隔为逆时针对激光扫描点进行标记为其中l_i以扫描角和对应的测量距离d_i来表示，即极坐标形式

根据以上传感器扫描探测到的局部环境信息，设计一个动态自适应规划窗口，其半径r_win可根据环境信息动态调整，把t时刻测距范围内探测到的局部环境信息在半径为r_win的半圆窗口中，表示为Win(r_win(t))＝{l₁，l₂，…，l_i，…，l₁₈₁}，其中扫描点l_i被重新定义为：

$l_i=\left\{(\theta ,\rho )\right|\theta ={\theta }_i,\rho =\left(\begin{array}{cc}{d}_i,& d_i<r_{\mathrm{win}}\\ r_{\mathrm{win}},& d_i\ge r_{\mathrm{win}}\end{array}\right)---\left(2\right)$

在步骤2)中，所述可通过性分析的步骤是：采用边界线扩展法对障碍物进行膨胀后，将机器人视为一个不记大小的质点，定义l_i方向上的可通行代价函数为：

$f_{l_i}^{\mathrm{trav}}\left({\rho }_i\right)=\left(\begin{array}{cc}0& ({\rho }_i=r_{\mathrm{win}})\\ +\infty & ({\rho }_i<r_{\mathrm{win}})\end{array}\right)---\left(3\right)$

若$f_{l_{i-1}}^{\mathrm{trav}}=+\infty ,$连续均为0，且$f_{l_{j+1}}^{\mathrm{trav}}=+\infty ,$则称序列l_i～l_j为局部窗口Win(r_win(t))中的一个可通过方向域，l_j、l_i分别为此方向域的左右边界。

在步骤3)中，所述安全性分析的步骤是：对可通过l_i序列来说，其与障碍物边界线的夹角越大，机器人沿此方向行进的安全性就越高，定义l_i的安全性度量函数为：

$f_{l_i}^{\mathrm{safe}}\left({\theta }_i\right)=\left(\begin{array}{cc}\min ({\theta }_i^{\mathrm{FDL}}-{\theta }_i,{\theta }_i-{\theta }_i^{\mathrm{FDR}})& ({\rho }_i=r_{\mathrm{win}})\\ +\infty & ({\rho }_i<r_{\mathrm{win}})\end{array}\right)---\left(4\right)$

其中θ_i^FDL、θ_i^FDR分别为FD(l_i)的左右边界l_i^FDL、l_i^FDR所对应的方向角。

在步骤4)中，所述运动平稳性分析的步骤是：机器人可以在360°范围任意转向，引入路径平滑性因素，定义l_i的平滑性代价函数为：

$f_{l_i}^{\mathrm{smooth}}\left({\theta }_i\right)=|{\theta }_R-{\theta }_i|---\left(5\right)$

在步骤5)中，所述目标引导性分析的步骤是：目标点P_G＝(x_G，y_G)对于局部环境中任一方向序列l_i的路径引导函数为

$f_{l_i}^{\mathrm{goal}}\left({\theta }_i\right)=|{\theta }_{\mathrm{goal}}-{\theta }_i|---\left(6\right)$

其中θ_goal＝arctan(y_G-y_R/x_G-x_R)为目标点P_G(x_G，y_G)相对于机器人的方向(θ_goal∈[-π，π])。

在步骤6)中，所述采用自适应窗口的路径规划的步骤是：在环境建模和分析基础上，设计一种基于自适应窗口的实时路径规划方法，包括机器人的趋向目标行为和机器人的沿墙行走行为。

所述机器人的趋向目标行为包括确定即时子目标和规划窗口自适应调整。

所述确定即时子目标的方法为：

规划窗口为Win(r_win(t))，若$P_G\notin \mathrm{Win}\left(r_{\mathrm{win}}\left(t\right)\right),$首先利用启发式函数f(l_i)＝g(l_i)+h(l_i)来选取使f(l_i)最小的扫描点作为子目标(记作l_gh)，即

$\left(\begin{array}{c}{l}_{\mathrm{gh}}=\arg \underset{l_i\in \mathrm{Win}\left(r_{\mathrm{win}}\left(t\right)\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=g\left(l_i\right)+h\left(l_i\right)\end{array}\right)---\left(7\right)$

式中：g(l_i)——机器人从当前位置向l_i方向行进的代价，h(l_i)——从l_i方向行进到目标点方向的代价，根据环境建模的分析，定义

$g\left(l_i\right)=f_{l_i}^{\mathrm{trav}},$$h\left(l_i\right)=f_{l_i}^{\mathrm{goal}}---\left(8\right)$

确定l_gh后，在其所在的可通过方向域FD(l_gh)中求安全性子目标l_safe和平稳性子目标l_smooth：

$\left(\begin{array}{c}{l}_{\mathrm{safe}}=\arg \underset{l_i\in \mathrm{FD}\left(l_{\mathrm{gh}}\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=f_{l_i}^{\mathrm{safe}}\left({\theta }_i\right)\end{array}\right)---\left(9\right)$

$\left(\begin{array}{c}{l}_{\mathrm{smooth}}=\arg \underset{l_i\in \mathrm{FD}\left(l_{\mathrm{gh}}\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=f_{l_i}^{\mathrm{smooth}}\left({\theta }_i\right)\end{array}\right)---\left(10\right)$

至此，根据不同的性能指标要求得到位于可通过方向域FD(l_gh)中的3种子目标点：l_gh＝(θ_gh，r_win)、l_safe＝(θ_safe，r_win)、l_smooth＝(θ_smooth，r_win)。综合考虑安全性和运动平稳性约束，对子目标方向进一步优化选择，最终即时子目标P_sub为：

P_sub＝{(θ_sub，ρ_sub)|θ_sub＝k_ghθ_gh+k_safeθ_safe+k_smoothθ_smooth，ρ_sub＝r_win(t)}            (11)

正系数k_gh、k_safe、k_smooth：k_gh+k_safe+k_smooth＝1。

所述规划窗口自适应调整的方法为：

动态窗口Win(r_win(t))中总的可通过方向域和记为定义如下：

在趋向目标行为中，Win(r_win(t))的调整原则为：当较大时，可以加大Win(r_win(t))，进一步扩展机器人的探测范围，寻找更优的子目标；当较小时，应该减小Win(r_win(t))，缩小机器人的探测范围，以便在狭窄空间中找到可行路径，即

r_win(t+1)＝r_win(t)+Δr            (13)

其中Δr为的函数，定义为：

当时，

Δr＝Δr_min                  (14a)

当时，

当时，

Δr＝Δr_max                    (14c)

其中为设置的常数，为最大值，Δr_min、Δr_max为定义的最大、最小值。

所述机器人的沿墙行走行为包括局部极小的判断、确定即时子目标和规划窗口的动态调整。

所述局部极小的判断方法为：

当机器人陷入局部极小时，机器人运动轨迹会出现振荡，或沿着某一闭合路径运动，从而不可能到达目标点。为了避免出现这种情况，需要提前预测机器人是否会进入局部极小状态，通过距离比较来判断：

D(P_sub，P_G)≥min(D(P_R，P_G)，D_leave)               (15)

其中D(P_sub，P_G)为子目标点与全局目标点之间的距离，D(P_R，P_G)为机器人与全局目标点之间的距离，D_leave为趋向目标的保证距离，在离开沿墙走行为时定义，初始化为D_leave＝D(P_S，P_G)。若条件(15)满足，而且目标点位于机器人的左侧，则follow_left_wall(沿左墙)行为被激活；若条件(15)满足，但目标点位于机器人的右侧，则follow_right_wall(沿右墙)行为被激活。一旦进入沿墙走行为，记录机器人与目标点之间的最短距离D_min。使用沿墙走解死锁算法而使移动机器人沿着障碍物的边界运动，直到满足以下退出条件

D(P_N，P_G)＜D_min               (16)

其中D(P_N，P_G)为点P_N和P_G之间的距离，P_N为沿墙走行为中下一时刻的预计到达位置。离开沿墙走行为时，更新D_leave＝D(P_R，P_G)。

所述确定即时子目标的方法为：

当follow_left_wall激活时，采用Win(r_win(t))中左起第一个障碍物的右边界线扩展线对应的扫描点为即时子目标；当follow_right_wall激活时，采用Win(r_win(t))中右起第一个障碍物的左边界线扩展线对应的扫描点为即时子目标。机器人对确定的子目标点进行跟踪，即可沿着障碍物行走；当机器人沿障碍绕行的距离超过设定值L_max1时仍未退出则自转180°再向另一绕行模式运动，在新的绕行模式下若绕行的距离超过设定值L_max2仍未退出(取L_max2＞2L_max1)，则停止路径(认为目标不可达)。

在沿墙走行为中，若发现目标点P_G位于Win(r_win(t))的可通过域中，则直接以P_G作为当前的即时子目标。

所述规划窗口的动态调整的方法为：

在沿墙走行为中，若为follow_left_wall，若$f_{l_{N_S}}^{\mathrm{trav}}=0,$则设置$r_{\mathrm{win}}(t+1)=d_{N_S},$否则窗口大小不变；若为follow_right_wall，若$f_{l_1}^{\mathrm{trav}}=0,$则设置r_win(t+1)＝d₁，否则窗口大小不变。

与已有技术相比，本发明由于具有更好的环境适应性和避障能力，获得较好的安全性和可达性，并具有较高的计算实时性。因此本发明解决了不确定复杂环境下移动机器人实时生成避障路径的问题，提供了一种综合最优的的路径选择方法，较好地满足移动机器人的避障要求，实现机器人的实时路径规划和控制，为移动机器人的导航应用提供一个有效的无碰撞路径规划方法。

附图说明

图1为子目标优选示意图。

图2为只考虑可通行性的行走路径。

图3为考虑了安全性的行走路径。

图4为进一步考虑了运动平稳性的行走路径。

图5为“回”形复杂环境下的行走路径。

图6为“U”形复杂环境下的行走路径。

图7为窗口半径r_win＝1.8m时的路径。

图8为窗口半径r_win＝7m时的路径。

图9为窗口大小自适应调整时的路径。

图10为窗口半径r_win(t)的动态调整过程。在图10中，横坐标为时间t/s，纵坐标为窗口半径r_win(t)m。

在图2～9中，起始点为S，目标点为G。

具体实施方式

下面结合具体附图和实施例对本发明作进一步说明。

实施例1

1)多约束局部环境建模和分析

机器人用激光传感器来感知障碍物的信息，每一个障碍物探测点相对于机器人的位置可由激光传感器直接得出，角度扫描间隔为逆时针对激光扫描点进行标记为其中l_i以扫描角和对应的测量距离d_i来表示，即极坐标形式

根据以上传感器扫描探测到的局部环境信息，设计一个动态自适应规划窗口，其半径r_win可根据环境信息动态调整，把t时刻测距范围内探测到的局部环境信息在半径为r_win的半圆窗口中表示为Win(r_win(t))＝{l₁，l₂，…，l_i，…，l₁₈₁}，其中扫描点l_i被重新定义为：

$l_i=\left\{(\theta ,\rho )\right|\theta ={\theta }_i,\rho =\left(\begin{array}{cc}{d}_i,& d_i<r_{\mathrm{win}}\\ r_{\mathrm{win}},& d_i\ge r_{\mathrm{win}}\end{array}\right)---\left(2\right)$

2)可通过性分析

采用边界线扩展法对障碍物进行膨胀后，将机器人视为一个不记大小的质点，定义l_i方向上的可通行代价函数为：

$f_{l_i}^{\mathrm{trav}}\left({\rho }_i\right)=\left(\begin{array}{cc}0& ({\rho }_i=r_{\mathrm{win}})\\ +\infty & ({\rho }_i<r_{\mathrm{win}})\end{array}\right)---\left(3\right)$

若$f_{l_{i-1}}^{\mathrm{trav}}=+\infty ,$连续均为0，且$f_{l_{j+1}}^{\mathrm{trav}}=+\infty ,$则称序列l_i～l_j为局部窗口Win(r_win(t))中的一个可通过方向域，l_j、l_i分别为此方向域的左右边界。

3)安全性分析

对可通过l_i序列来说，其与障碍物边界线的夹角越大，机器人沿此方向行进的安全性就越高，定义l_i的安全性度量函数为：

$f_{l_i}^{\mathrm{safe}}\left({\theta }_i\right)=\left(\begin{array}{cc}\min ({\theta }_i^{\mathrm{FDL}}-{\theta }_i,{\theta }_i-{\theta }_i^{\mathrm{FDR}})& ({\rho }_i=r_{\mathrm{win}})\\ +\infty & ({\rho }_i<r_{\mathrm{win}})\end{array}\right)---\left(4\right)$

其中θ_i^FDL、θ_i^FDR分别为FD(l_i)的左右边界l_i^FDL、l_i^FDR所对应的方向角。

4)运动平稳性分析

机器人可以在360°范围任意转向，但实际应用中总期望机器人在行进过程中能够减少转向操作，尽量沿着比较平滑的轨迹行驶，这样有利于车体定位操作，同时降低能量消耗。

为了能将路径的平稳性反映到路径规划模型中，引入路径平滑性因素，定义l_i的平滑性代价函数为

$f_{l_i}^{\mathrm{smooth}}\left({\theta }_i\right)=|{\theta }_R-{\theta }_i|---\left(5\right)$

5)目标引导性分析

目标点P_G＝(x_G，y_G)对于局部环境中任一方向序列l_i的路径引导函数为

$f_{l_i}^{\mathrm{goal}}\left({\theta }_i\right)=|{\theta }_{\mathrm{goal}}-{\theta }_i|---\left(6\right)$

其中θ_goal＝arctan(y_G-y_R/x_G-x_R)为目标点P_G(x_G，y_G)相对于机器人的方向(θ_goal∈[-π，π])。

6)采用自适应窗口的路径规划

在环境建模和分析基础上，设计一种基于自适应窗口的实时路径规划方法，包括趋向目标和沿墙行走两种行为如下：

(1)机器人的趋向目标行为

①即时子目标的确定

规划窗口为Win(r_win(t))，若$P_G\notin \mathrm{Win}\left(r_{\mathrm{win}}\left(t\right)\right),$首先利用启发式函数f(l_i)＝g(l_i)+h(l_i)来选取使f(l_i)最小的扫描点作为子目标(记作l_gh)，即

$\left(\begin{array}{c}{l}_{\mathrm{gh}}=\arg \underset{l_i\in \mathrm{Win}\left(r_{\mathrm{win}}\left(t\right)\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=g\left(l_i\right)+h\left(l_i\right)\end{array}\right)---\left(7\right)$

式中：g(l_i)——机器人从当前位置向l_i方向行进的代价，h(l_i)——从l_i方向行进到目标点方向的代价。根据环境建模的分析，定义

$g\left(l_i\right)=f_{l_i}^{\mathrm{trav}},h\left(l_i\right)=f_{l_i}^{\mathrm{goal}}---\left(8\right)$

确定l_gh后，在其所在的可通过方向域FD(l_gh)中求安全性子目标l_safe和平稳性子目标l_smooth：

$\left(\begin{array}{c}{l}_{\mathrm{safe}}=\arg \underset{l_i\in \mathrm{FD}\left(l_{\mathrm{gh}}\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=f_{l_i}^{\mathrm{safe}}\left({\theta }_i\right)\end{array}\right)---\left(9\right)$

$\left(\begin{array}{c}{l}_{\mathrm{smooth}}=\arg \underset{l_i\in \mathrm{FD}\left(l_{\mathrm{gh}}\right)}{\min }\left(f\left(l_i\right)\right)\\ f\left(l_i\right)=f_{l_i}^{\mathrm{smooth}}\left({\theta }_i\right)\end{array}\right)---\left(10\right)$

至此，根据不同的性能指标要求得到位于可通过方向域FD(l_gh)中的3种子目标点：l_gh＝(θ_gh，r_win)、l_safe＝(θ_safe，r_win)、l_smooth＝(θ_smooth，r_win)。综合考虑安全性和运动平稳性约束，对子目标方向进一步优化选择，最终即时子目标P_sub为：

P_sub＝{(θ_sub，ρ_sub)|θ_sub＝k_ghθ_gh+k_safeθ_safe+k_smoothθ_smooth，ρ_sub＝r_win(t)}         (11)

正系数k_gh、k_safe、k_smooth：k_gh+k_safe+k_smooth＝1。

②规划窗口自适应调整

动态窗口Win(r_win(t))中总的可通过方向域和记为定义如下：

在趋向目标行为中，Win(r_win(t))的调整原则为：当较大时，可以加大Win(r_win(t))，进一步扩展机器人的探测范围，寻找更优的子目标；当较小时，应该减小Win(r_win(t))，缩小机器人的探测范围，以便在狭窄空间中找到可行路径，即

r_win(t+1)＝r_win(t)+Δr              (13)

其中Δr为的函数，定义为：

当时，

Δr＝Δr_min              (14a)

当时，

当时，

Δr＝Δr_max              (14c)

其中为设置的常数，为最大值，Δr_min、Δr_max为定义的最大、最小值。

(2)机器人的沿墙行走行为

①局部极小的判断方法

当机器人陷入局部极小时，机器人运动轨迹会出现振荡，或沿着某一闭合路径运动，从而不可能到达目标点。为了避免出现这种情况，需要提前预测机器人是否会进入局部极小状态，通过距离比较来判断：

D(P_sub，P_G)≥min(D(P_R，P_G)，D_leave)                (15)

其中D(P_sub，P_G)为子目标点与全局目标点之间的距离，D(P_R，P_G)为机器人与全局目标点之间的距离，D_leave为趋向目标的保证距离，在离开沿墙走行为时定义，初始化为D_leave＝D(P_S，P_G)。若条件(15)满足，而且目标点位于机器人的左侧，则follow_left_wall(沿左墙)行为被激活；若条件(15)满足，但目标点位于机器人的右侧，则follow_right_wall(沿右墙)行为被激活。一旦进入沿墙走行为，记录机器人与目标点之间的最短距离D_min。使用沿墙走解死锁算法而使移动机器人沿着障碍物的边界运动，直到满足以下退出条件

D(P_N，P_G)＜D_min             (16)

其中D(P_N，P_G)为点P_N和P_G之间的距离，P_N为沿墙走行为中下一时刻的预计到达位置。离开沿墙走行为时，更新D_leave＝D(P_R，P_G)。

②即时子目标的确定

当follow_left_wall激活时，采用Win(r_win(t))中左起第一个障碍物的右边界线扩展线对应的扫描点为即时子目标；当follow_right_wall激活时，采用Win(r_win(t))中右起第一个障碍物的左边界线扩展线对应的扫描点为即时子目标。机器人对确定的子目标点进行跟踪，即可沿着障碍物行走；当机器人沿障碍绕行的距离超过设定值L_max1时仍未退出则自转180°再向另一绕行模式运动，在新的绕行模式下若绕行的距离超过设定值L_max2仍未退出(取L_max2＞2L_max1)，则停止路径(认为目标不可达)。

在沿墙走行为中，若发现目标点P_G位于Win(r_win(t))的可通过域中，则直接以P_G作为当前的即时子目标。

③规划窗口的动态调整_在沿墙走行为中，若为follow_left_wall，若$f_{l_{N_S}}^{\mathrm{trav}}=0,$则设置$r_{\mathrm{win}}(t+1)=d_{N_S},$否则窗口大小不变；若为follow_right_wall，若$f_{l_1}^{\mathrm{trav}}=0,$则设置r_win(t+1)＝d₁，否则窗口大小不变。

实施例2

本发明基于环境建模和自适应窗口的移动机器人路径规划方法，其路径规划中的行为决择和执行过程如下所述：

1)对系统参数r_win、k_gh、k_safe、k_smooth、k_FD、Δr_min、Δr_max、D_leave、L_max1、L_max2进行初始化，机器人刚启动时默认为MtG行为。

2)读取传感器数据，采用公式(1)的形式表示，再用公式(2)对l_i进行重新定义。

3)对障碍物A的边界进行膨化处理，采用公式(3)～(6)计算Win(r_win(t))中l_i的可通过性、安全性、运动平稳性和目标吸引性函数。

4)判断机器人当前处在哪种行为，若为趋向目标行为，转Step 5，否则转Step 6。

5)趋向目标运动(Motion toward Goal，MtG)(即5.1～5.3)，直到满足以下条件之一：

当到达目标点时，停止运动。

当发现局部极小(即满足公式(15))时，转步骤6)。

5.1)如果目标点位于可通过域中，则直接以目标点作为当前的子目标，否则利用公式(7)～(10)计算子目标点l_gh、l_safe和l_smooth，再用公式(11)对子目标进行优选，得到当前Win(r_win(t))内的即时子目标P_sub，如图1。

5.2)如果无法确定即时子目标，机器人根据目标方向自转90°进行探测(即以(θ_R±π/2，0)为子目标点)；

5.3)根据环境信息，采用公式(12)计算机器人的可通过方向域，然后用公式(13)和(14)对窗口Win(r_win(t))的半径进行调整，转步骤2)。

6)沿墙行走(Wall Following，WF)(即6.1～6.3)，直到满足以下条件之一：

当到达目标点时，停止运动。

当满足退出条件(即满足公式(16))时，转步骤5)。

当沿墙行走的距离大于设定值仍未退出时，则认为目标不可达，停止运动。

6.1)根据目标点位置，选择follow_left_wall或follow_right_wall；

6.2)如果目标点位于可通过域中，则直接以目标点作为当前的子目标，否则根据沿墙走行为，即：follow_left_wall激活时，采用Win(r_win(t))中左起第一个障碍物的右边界线扩展线对应的扫描点为即时子目标，即图1中的l_flw；当follow_right_wall激活时，采用Win(r_win(t))中右起第一个障碍物的左边界线扩展线对应的扫描点为即时子目标，即图1中的l_frw。

6.3)根据环境信息调整规划窗口的半径r_win(t+1)，转步骤2)。

实施例3

为验证多约束环境模型中各种因素对路径规划的影响，图2～4给出简单环境下的路径规划结果。其中，图2为k_gg＝1，k_safe＝k_smooth＝0时的路径，只考虑了可通行性，所以尽管其路径最短，但离障碍物的距离却最近，这种情况很不利于机器人的安全行驶；图3为k_gg＝0.70，k_safe＝0.30，k_smooth＝0时的路径，考虑了安全性，与图2相比安全性明显得到提高(路径远离障碍物)；图4为k_gg＝0.55，k_dafe＝0.20，k_smooth＝0.25时的路径，进一步考虑了运动平稳性，有利于减少机器人的转向操作。

实施例4

当障碍物形状过于复杂或特殊时，机器人单靠实时探测的局部环境知识容易陷人死锁或振荡状态。本发明借助有效的WF行为可以脱离局部极小而顺利到达目标点。图5，机器人先采用follow_left_wall，行走L_max1距离后未退出，再转向follow_right_wall行为，最后达到目标点；图6，机器人进入狭小的U形障碍物找不到子目标，自转90°后进入follow_left_wall行为，最后达到目标点。

实施例5

基于局部窗口的滚动规划算法中，如果窗口太小，机器人则不能及时提前进行避障行走；窗口太大，机器人则可能找不到可选子目标。如图7所示，为窗口半径较小时的路径，机器人不能及时进入MtG的绕行模式，所得路径弯曲较大；图8为窗口半径较大时规划得到的行走路径，机器人不能及时找到可行路径，两次陷入WF行为。

实施例6

采用基于自适应窗口的路径规划方法时，机器人能够根据环境信息对局部规划窗口的大小进行动态调整，所得路径如图9所示，其窗口大小的调整过程如图10所示。可以看出，图9中路径明显优于图7和图8中的路径，而且机器人路径规划的计算量与窗口大小无关，不会随着窗口的增大而增大每一次滚动规划中的局部规划时间。