CN114428499A - 一种融合Astar与DWA算法的移动小车路径规划方法 - Google Patents

Abstract

本发明公开了一种融合Astar与DWA算法的移动小车路径规划方法，属于移动小车路径规划领域。本发明将移动小车从起点到终点所需经过的平面区域采用栅格法建立栅格地图，在所述栅格地图中预先设置起点与终点位置；运行改进的Astar算法，从所述栅格地图的起点进行全局路径规划，获得一条从起点到终点的全局最优路径；运行改进的DWA算法，以步骤2所述的全局最优路径为参考路径，从步骤1所述的起点到终点位置进行局部动态避障路径规划，为步骤1中所述的移动小车规划出一条最优的动态避障路径；改进的Astar算法相较于原Astar算法减少了路径的长度以及多余的路径转折点，此外本发明能够减小移动小车在运动过程中线速度与角速度的剧烈变化，保证了其运动的平稳性。

Description

一种融合Astar与DWA算法的移动小车路径规划方法

技术领域

本发明属于移动小车路径规划领域，具体涉及到一种融合Astar与DWA算法的移动小车路径规划方法。

背景技术

Astar算法因其优良的性能在全局路径规划问题中大量运用。然而，在栅格地图环境中，原Astar算法从当前节点向下一个节点访问的选择范围被限制在当前节点周围八个不同方向上的相邻节点，每个相邻运动方向的夹角均为

移动小车移动方向角度也被限制为

的整数倍，受此节点移动方向条件约束，原Astar算法规划出的最终路径距离不是最短，路径上因转折点较多不够平滑，同时无法躲避动态障碍物。

动态窗口法(Dynamic Window Approach，简称DWA算法)是一种常用的移动小车局部路径规划算法，该方法在进行局部路径规划时，考虑了移动小车的动力学性能，在轨迹评价函数的作用下，能够使移动小车安全到达目标点，但是当移动小车的位置与目标点位置比较接近时，两者之间的位置夹角θ会突然变大，此时移动小车会在方位角评价因子的驱使下，通过使自己的角速度增大，来减小θ，这样就会使移动小车行驶不稳定，出现震荡的现象。此外，原动态窗口法在进行局部路径规划时，由于没有全局最优路径作为参考，常易陷入局部最优，即“U”或“C”型障碍物内无法出来。

发明内容

本发明的目的在于为当前的移动小车在进行动态避障路径规划时所要解决的全局最优路径规划问题与动态避障问题提供一种解决方法，使移动小车能够自主的规划出一条从起点到终点的较短且较平滑的最优路径，同时在运动过程中能够安全的避开阻挡自身前进的动态障碍物，此外，移动小车在进行运动时还要符合移动小车自身的运动学，使其能够较为平稳的运动。

本发明的具体内容如下：

一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，包括下列步骤：

步骤1：将移动小车从起点到终点所需经过的平面区域采用栅格法建立栅格地图，在所述栅格地图中预先设置起点与终点位置；

步骤2：运行改进的Astar算法，从所述的栅格地图的起点进行全局路径规划，获得一条从起点到终点的全局最优路径；

步骤3：运行改进的DWA算法，以步骤2所述的全局最优路径为参考路径，从步骤1所述的起点到终点位置进行局部动态避障路径规划，为步骤1所述的移动小车规划出一条最优的动态避障路径。

步骤2中所述改进的Astar算法是在原Astar算法的扩展节点范围和移动方向上增加Astar算法扩展节点的层数；将可扩展节点数8个扩展为48个，将移动方向8个扩展为32个，其数学表达式为：

其中，R为改进Astar算法以当前节点为中心向外搜索的节点层数，这里令R＝2，N_R表示随节点扩展层数R变化而变化的节点下一秒可移动方向，S_R表示随节点扩展层数R变化而变化得可搜索节点个数。

步骤2中所述运行改进的Astar算法进行全局路径规划，得到一条从起点到终点的全局最优路径，其包括以下步骤：

步骤4：设置开放列表(Open List)和关闭列表(Closed List)，将起始节点S放入Open List中，此时起始节点S是Open List中的唯一节点，Closed List此时为空表；

步骤5：所述改进的Astar算法遍历搜索Open List中存储的其它待扩展节点，若此时Open List没有待搜索节点，则算法搜索结束退出，不存在可行路径；如果Open List不为空，则因为所述改进的Astar算法扩大搜索层数R＝2，且可扩展节点个数比所述原Astar算法有所增多，为此对扩大搜索邻域后所有待扩展节点进行约束条件判断筛选，约束条件如下：

(1)待扩展节点在环境模型地图之中；

(2)待扩展节点没有被障碍物隔断和占有，即待扩展节点可通行且有路径可以到达。

舍弃不符合约束条件的点，符合上述约束条件的节点即为可扩展节点，利用估价函数f(n)计算所有待扩展节点的估价函数f(n)值并进行比较，选择f(n)最小的待扩展节点n作为算法路径选择的下一节点，将此节点从Open List中删除，并将其纳入Closed List之中。

步骤6：判断算法扩展的节点是否和目标节点G重合，若重合，算法搜索路径成功；如果节点n不与目标节点G重合，则算法继续下一步骤；

步骤7：搜索节点n邻域内所有符合待搜索要求的子节点，设子节点为m，比较所有待扩展节点的估价函数f(m)值，选择其中f(m)最小的待扩展节点m并进行下一步骤的判断；

步骤8：检查上一步所得节点m和Open List、Closed List的存储关系：

(1)若节点m既不在Open List也不在Closed List之中，则将其添加到Open List当中，并给子节点m添加一个指向其父节点n的指针，存在可行路径时根据存储的指针信息返回输出最优路径；

(2)若节点m已经在Open List之中，则比较当前节点m的估价函数f(m)值和拥有不同父节点的已经存储在Open列表时的估价函数值f₀(m)大小，比较f(m)和f₀(m)值的大小，若f(m)较小，则表示找到更好的路径节点，同时更新节点m的父指针，并将节点m加入ClosedList列表之中，如果f₀(m)较小，保持节点存储不变；

(3)节点m已经存在于Closed List之中，则表示该节点已经在当前最优路径上，返回第四步继续对比判断扩展其他子节点；

步骤9：循环步骤5到步骤8，当算法下一节点是目标节点G或者Open List为空表时算法结束退出，输出当前最优路径或者算法无解。

步骤3中所述改进的DWA算法的表达式为：

G(v,ω)＝σ(α·Gdist(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+δ·Pdist(v,ω))

其中，v和ω分别为移动小车的线速度与角速度，G(v,ω)表示所述改进的DWA算法的轨迹评价函数，Gdist(v,ω)是目标距离评价因子，表示为移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与终点之间的距离，dist(v,ω)是障碍物距离评价因子，表示的是移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与障碍物之间的距离，vel(v,ω)是速度评价因子，表示的是当前的采样线速度v与角速度ω大小，Pdist(v,ω)是全局路径评价因子，表示的是移动小车在进行局部路径规划时的当前位置距全局最优路径的直线距离，σ为评价因子归一化处理函数，表示对四项评价因子进行归一化处理，α、β、γ、δ分别是所述改进的DWA算法的轨迹评价函数G(v,ω)中四项评价因子的权重参数。

步骤3中所述运行改进的DWA算法，以步骤2所述的全局最优路径为参考路径，从步骤1所述的起点到终点位置进行局部动态避障路径规划，其包括以下步骤：

步骤10：启用所述改进的DWA算法进行局部路径规划，首先要在模拟周期内计算出移动小车的运行轨迹，则移动小车在世界坐标系下第k+1时刻的位姿表达式为：

其中，k与k+1分别表示移动小车在世界坐标系xoy下第k与k+1时刻，x(k)与x(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的x轴坐标信息，y(k)与y(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的y轴坐标信息，θ(k)表示移动小车此时的姿态角，v和ω分别为移动小车此时的线速度与角速度，相邻时刻的间隔为t；

步骤11：对移动小车前进的速度空间进行速度矢量空间采样，在最大最小速度约束、动力学性能约束、安全约束三个条件约束下得到移动小车在接下来最适合的几组线速度v与角速度ω，即速度空间V_r，V_r的表达式如下：

V_r＝V_s∩V_d∩V_a

其中，V_s表示移动小车运动时的最大最小速度约束，V_d表示移动小车运动时的安全约束，V_a表示移动小车运动时的动力学性能约束；

步骤12：步骤11中所述的速度空间V_r中每一组线速度v与角速度ω都对应着一条可行轨迹，将多条轨迹代入所述改进的DWA算法轨迹评价函数G(v,ω)中，然后利用所得值判断选出一条最优轨迹，再以其对应的线速度v与角速度ω驱使移动小车运动，即得出当前运行周期内围绕所述全局最优路径规划的局部最优动态避障路径

本发明的有益效果在于：

1.本发明提出的一种融合Astar与DWA算法的移动小车路径规划方法，通过融合全局路径规划算法改进的Astar算法与局部路径规划算法改进的DWA算法能够实现彼此之间的优势互补，使移动小车能够以全局最优路径为基础的条件下进行局部动态避障路径规划，有效的解决了使用单一的全局路径规划方法或是局部路径规划方法所带来的无法躲避动态障碍物、全局路径并非最优等缺点，大大提高了移动小车的运行效率；

2.本发明中使用的改进的Astar算法，是在原Astar算法的基础上扩展搜索邻域，使待扩展的节点个数共有48个，不同的节点移动方向共有32个，使得改进的Astar算法去除了原Astar算法对当前节点向下一节点访问个数的限制，以及角度方向只可变换

整数倍的约束，大大减少了原Astar算法在栅格地图上进行全局路径规划时的拐点，使得整体路径规划较为平滑，同时也缩短了路径规划的时间与路程；

3.本发明中使用的改进的DWA算法，是在原DWA算法轨迹评价函数中添加了全局路径评价因子Pdist(v,ω)，以及用目标距离评价因子Gdist(v,ω)替换了原DWA算法轨迹评价函数中的方位角评价因子heading(v,ω)，这样做相较与原DWA算法在进行路径规划时的好处有两点：一是改进的DWA算法在进行路径规划时，会考虑全局最优路径来进行局部路径规划，这样可大大提高运行效率；二是相较于原DWA算法中的方位角评价因子heading(v,ω)，目标距离评价因子Gdist(v,ω)能够更好的控制移动小车行驶中的线速度v与角速度ω，使移动小车在实际运动中更加符合移动小车运动学，使得移动小车能够平稳的到达终点。

附图说明

图1是融合改进的Astar与DWA算法的路径规划流程图；

图2是改进的DWA算法在进行局部路径规划时的采样速度轨迹末端距目标距离评价因子的几何示意图；

图3是改进的DWA算法在进行局部路径规划时的当前位置距全局最优路径的距离评价因子的几何示意图；

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面通过附图中示出的具体实施例来描述本发明。但是应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

以下结合附图进一步说明本发明的具体实施方式。

一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，具体包括以下步骤：

步骤1：对当前移动小车所处环境采用栅格法建立栅格地图，并在栅格地图中根据当前移动小车所处位置与所达目标位置进行起点与终点的设置；

步骤2：在所述的栅格地图起点处，使用改进的Astar算法进行全局路径规划，得到一条从起点到终点的全局最优路径；

步骤3：在所述的栅格地图起点处，使用改进的DWA算法，以所述全局最优路径为基础进行局部动态避障路径规划直到终点，完成移动小车从所述起点到终点的最优动态避障路径规划过程。

所述步骤2中改进的Astar算法是在原Astar算法的扩展节点范围和方向的基础上增加Astar算法扩展节点的层数；将原有的可扩展节点数8个，移动方向8个扩展为48个待扩展节点个数，以及32个节点移动方向，其数学表达式为：

其中，R为改进Astar算法以当前节点为中心向外搜索的节点层数，这里令R＝2，N_R表示随节点扩展层数R变化而变化的节点下一秒可移动方向，S_R表示随节点扩展层数R变化而变化得可搜索节点个数。

图1流程图中包含本发明步骤2中所述使用改进的Astar算法在栅格地图上进行全局路径规划的过程，即得到一条从起点到终点的全局最优路径，其特征在于，包括以下步骤：

步骤4：设置开放列表(Open List)和关闭列表(Closed List)，将起始节点S放入Open List中，此时起始节点S是Open List中的唯一节点，Closed List此时为空表；

步骤5：所述改进的Astar算法遍历搜索Open List中存储的其它待扩展节点，若此时Open List没有待搜索节点，则算法搜索结束退出，不存在可行路径；如果Open List不为空，则因为所述改进的Astar算法扩大搜索层数R＝2，且节点可扩展节点个数比所述原Astar算法有所增多，为此对扩大搜索邻域后所有待扩展节点进行约束条件判断筛选，约束条件如下：

(1)待扩展节点在环境模型地图之中；

(2)待扩展节点没有被障碍物隔断和占有，即待扩展节点可通行且有路径可以到达。

舍弃不符合约束条件的点，符合上述约束条件的节点即为可扩展节点，利用估价函数f(n)计算所有待扩展节点的估价函数f(n)值并进行比较，选择f(n)最小的待扩展节点n作为算法路径选择的下一节点，将此节点从Open List中删除，并将其纳入Closed List之中。

步骤6：判断算法扩展的节点是否和目标节点G重合。若重合，算法搜索路径成功；如果节点n不与目标节点G重合，则算法继续下一步骤；

步骤7：搜索节点n邻域内所有符合待搜索要求的子节点。设子节点为m，比较所有待扩展节点的估价函数f(m)值，选择其中f(m)最小的待扩展节点m并进行下一步骤的判断；

步骤8：检查上一步所得节点m和Open List、Closed List的存储关系：

(1)若节点m既不在Open List也不在Closed List之中，则将其添加到Open List当中，并给子节点m添加一个指向其父节点n的指针，存在可行路径时根据存储的指针信息返回输出最优路径；

(2)若节点m已经在Open List之中，则比较当前节点m的估价函数f(m)值和拥有不同父节点的已经存储在Open列表时的估价函数值f₀(m)大小，比较f(m)和f₀(m)值的大小，若f(m)较小，则表示找到更好的路径节点，同时更新节点m的父指针，并将节点m加入ClosedList列表之中，如果f₀(m)较小，保持节点存储不变；

(3)节点m已经存在于Closed List之中，则表示该节点已经在当前最优路径上，返回第四步继续对比判断扩展其他子节点；

步骤9：循环步骤5到步骤8，当算法下一节点是目标节点G或者Open List为空表时算法结束退出，输出当前最优路径或者算法无解。

步骤3中所述改进的DWA算法的表达式为：

G(v,ω)＝σ(α·Gdist(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+δ·Pdist(v,ω))

其中，v和ω分别为移动小车的线速度与角速度，G(v,ω)表示所述改进的DWA算法的轨迹评价函数，Gdist(v,ω)是目标距离评价因子，表示为移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与终点之间的距离，其几何示意图如图2所示，dist(v,ω)是障碍物距离评价因子，表示的是移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与障碍物之间的距离，vel(v,ω)是速度评价因子，表示的是当前的采样线速度v与角速度ω大小，Pdist(v,ω)是全局路径评价因子，表示的是移动小车在进行局部路径规划时的当前位置距全局最优路径的直线距离，其几何示意图如图3所示，σ为评价因子归一化处理函数，表示对四项评价因子进行归一化处理，其作用是避免因某一项评价因子的值过大而造成该因子的优势太大，从而影响整体平衡，α、β、γ、δ分别是所述改进的DWA算法的轨迹评价函数G(v,ω)中四项评价因子的权重参数，其作用是控制归一化处理后的四项评价因子在整个评价函数中的影响力比重，权重参数越大，那么此评价因子在评价函数中的影响力越大。

图1流程图中包含本发明步骤3中所述使用改进的DWA算法在栅格地图上进行局部动态避障路径规划的过程，即以全局最优路径为基础进行局部动态避障路径规划直到终点，其特征在于，包括以下步骤：

步骤10：经过所述步骤4与步骤9利用所述改进的Astar算法对移动小车进行了从起点到终点的全局最优路径规划之后，启用所述改进的DWA算法进行局部路径规划，首先要在模拟周期内计算出移动小车的运行轨迹，则移动小车在世界坐标系下第k+1时刻的位姿表达式为：

其中，k与k+1分别表示移动小车在世界坐标系xoy下第k与k+1时刻，x(k)与x(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的x轴坐标信息，y(k)与y(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的y轴坐标信息，θ(k)表示移动小车此时的姿态角，v和ω分别为移动小车此时的线速度与角速度，相邻时刻的间隔为t；

步骤11：对移动小车前进的速度空间进行速度矢量空间采样，在最大最小速度约束、动力学性能约束、安全约束三个条件约束下得到移动小车在接下来最适合的几组线速度v与角速度ω，即速度空间V_r，V_r的表达式如下：

V_r＝V_s∩V_d∩V_a

其中，V_s表示移动小车运动时的最大最小速度约束，V_d表示移动小车运动时的安全约束，V_a表示移动小车运动时的动力学性能约束；

步骤12：步骤11中所述的速度空间V_r中每一组线速度v与角速度ω都对应着一条可行轨迹，将多条轨迹代入所述改进的DWA算法轨迹评价函数G(v,ω)中，然后利用所得值判断选出一条最优轨迹，再以其对应的线速度v与角速度ω驱使移动小车运动，即得出当前运行周期内围绕所述全局最优路径规划的局部最优动态避障路径。

以上显示和描述了本发明的基本原理、主要特征以及本发明的优点，本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (5)

1.一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，包括下列步骤：

步骤1：将移动小车从起点到终点所需经过的平面区域采用栅格法建立栅格地图，在所述栅格地图中预先设置起点与终点位置；

步骤2：运行改进的Astar算法，从所述的栅格地图的起点进行全局路径规划，获得一条从起点到终点的全局最优路径；

步骤3：运行改进的DWA算法，以步骤2所述的全局最优路径为参考路径，从步骤1所述的起点到终点位置进行局部动态避障路径规划，为步骤1所述的移动小车规划出一条最优的动态避障路径。

2.根据权利要求1所述的一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，步骤2中所述改进的Astar算法是在原Astar算法的扩展节点范围和移动方向上增加Astar算法扩展节点的层数；将可扩展节点数8个扩展为48个，将移动方向8个扩展为32个，其数学表达式为：

其中，R为改进Astar算法以当前节点为中心向外搜索的节点层数，这里令R＝2，N_R表示随节点扩展层数R变化而变化的节点下一秒可移动方向，S_R表示随节点扩展层数R变化而变化得可搜索节点个数。

3.根据权利要求1所述的一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，步骤2中所述运行改进的Astar算法进行全局路径规划，得到一条从起点到终点的全局最优路径，其包括以下步骤：

步骤4：设置开放列表(Open List)和关闭列表(Closed List)，将起始节点S放入OpenList中，此时起始节点S是Open List中的唯一节点，Closed List此时为空表；

步骤5：所述改进的Astar算法遍历搜索Open List中存储的其它待扩展节点，若此时Open List没有待搜索节点，则算法搜索结束退出，不存在可行路径；如果Open List不为空，则因为所述改进的Astar算法扩大搜索层数R＝2，且可扩展节点个数比所述原Astar算法有所增多，为此对扩大搜索邻域后所有待扩展节点进行约束条件判断筛选，约束条件如下：

(1)待扩展节点在环境模型地图之中；

(2)待扩展节点没有被障碍物隔断和占有，即待扩展节点可通行且有路径可以到达。

舍弃不符合约束条件的点，符合上述约束条件的节点即为可扩展节点，利用估价函数f(n)计算所有待扩展节点的估价函数f(n)值并进行比较，选择f(n)最小的待扩展节点n作为算法路径选择的下一节点，将此节点从Open List中删除，并将其纳入Closed List之中。

步骤6：判断算法扩展的节点是否和目标节点G重合，若重合，算法搜索路径成功；如果节点n不与目标节点G重合，则算法继续下一步骤；

步骤7：搜索节点n邻域内所有符合待搜索要求的子节点，设子节点为m，比较所有待扩展节点的估价函数f(m)值，选择其中f(m)最小的待扩展节点m并进行下一步骤的判断；

步骤8：检查上一步所得节点m和Open List、Closed List的存储关系：

(1)若节点m既不在Open List也不在Closed List之中，则将其添加到Open List当中，并给子节点m添加一个指向其父节点n的指针，存在可行路径时根据存储的指针信息返回输出最优路径；

(2)若节点m已经在Open List之中，则比较当前节点m的估价函数f(m)值和拥有不同父节点的已经存储在Open列表时的估价函数值f₀(m)大小，比较f(m)和f₀(m)值的大小，若f(m)较小，则表示找到更好的路径节点，同时更新节点m的父指针，并将节点m加入Closed List列表之中，如果f₀(m)较小，保持节点存储不变；

(3)节点m已经存在于Closed List之中，则表示该节点已经在当前最优路径上，返回第四步继续对比判断扩展其他子节点；

步骤9：循环步骤5到步骤8，当算法下一节点是目标节点G或者Open List为空表时算法结束退出，输出当前最优路径或者算法无解。

4.根据权利要求1所述的一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，步骤3中所述改进的DWA算法的表达式为：

G(v,ω)＝σ(α·Gdist(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+δ·Pdist(v,ω))

其中，v和ω分别为移动小车的线速度与角速度，G(v,ω)表示所述改进的DWA算法的轨迹评价函数，Gdist(v,ω)是目标距离评价因子，表示为移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与终点之间的距离，dist(v,ω)是障碍物距离评价因子，表示的是移动小车以当前选定的采样线速度v与角速度ω所对应的轨迹末端与障碍物之间的距离，vel(v,ω)是速度评价因子，表示的是当前的采样线速度v与角速度ω大小，Pdist(v,ω)是全局路径评价因子，表示的是移动小车在进行局部路径规划时的当前位置距全局最优路径的直线距离，σ为评价因子归一化处理函数，表示对四项评价因子进行归一化处理，α、β、γ、δ分别是所述改进的DWA算法的轨迹评价函数G(v,ω)中四项评价因子的权重参数。

5.根据权利要求1所述的一种融合Astar与DWA算法的移动小车路径规划方法，其特征在于，步骤3中所述运行改进的DWA算法，以步骤2所述的全局最优路径为参考路径，从步骤1所述的起点到终点位置进行局部动态避障路径规划，其包括以下步骤：

步骤10：启用所述改进的DWA算法进行局部路径规划，首先要在模拟周期内计算出移动小车的运行轨迹，则移动小车在世界坐标系下第k+1时刻的位姿表达式为：

其中，k与k+1分别表示移动小车在世界坐标系xoy下第k与k+1时刻，x(k)与x(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的x轴坐标信息，y(k)与y(k+1)分别表示移动小车在世界坐标系xoy下第k时刻与第k+1时刻的y轴坐标信息，θ(k)表示移动小车此时的姿态角，v和ω分别为移动小车此时的线速度与角速度，相邻时刻的间隔为t；

步骤11：对移动小车前进的速度空间进行速度矢量空间采样，在最大最小速度约束、动力学性能约束、安全约束三个条件约束下得到移动小车在接下来最适合的几组线速度v与角速度ω，即速度空间V_r，V_r的表达式如下：

V_r＝V_s∩V_d∩V_a

其中，V_s表示移动小车运动时的最大最小速度约束，V_d表示移动小车运动时的安全约束，V_a表示移动小车运动时的动力学性能约束；

步骤12：步骤11中所述的速度空间V_r中每一组线速度v与角速度ω都对应着一条可行轨迹，将多条轨迹代入所述改进的DWA算法轨迹评价函数G(v,ω)中，然后利用所得值判断选出一条最优轨迹，再以其对应的线速度v与角速度ω驱使移动小车运动，即得出当前运行周期内围绕所述全局最优路径规划的局部最优动态避障路径。

Images