CN114428499A - 一种融合Astar与DWA算法的移动小车路径规划方法 - Google Patents
一种融合Astar与DWA算法的移动小车路径规划方法 Download PDFInfo
- Publication number
- CN114428499A CN114428499A CN202111541463.5A CN202111541463A CN114428499A CN 114428499 A CN114428499 A CN 114428499A CN 202111541463 A CN202111541463 A CN 202111541463A CN 114428499 A CN114428499 A CN 114428499A
- Authority
- CN
- China
- Prior art keywords
- node
- algorithm
- path
- mobile trolley
- astar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 121
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000011156 evaluation Methods 0.000 claims description 57
- 238000005070 sampling Methods 0.000 claims description 13
- 238000010606 normalization Methods 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 9
- 230000004888 barrier function Effects 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 6
- 230000008859 change Effects 0.000 abstract description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开了一种融合Astar与DWA算法的移动小车路径规划方法,属于移动小车路径规划领域。本发明将移动小车从起点到终点所需经过的平面区域采用栅格法建立栅格地图,在所述栅格地图中预先设置起点与终点位置;运行改进的Astar算法,从所述栅格地图的起点进行全局路径规划,获得一条从起点到终点的全局最优路径;运行改进的DWA算法,以步骤2所述的全局最优路径为参考路径,从步骤1所述的起点到终点位置进行局部动态避障路径规划,为步骤1中所述的移动小车规划出一条最优的动态避障路径;改进的Astar算法相较于原Astar算法减少了路径的长度以及多余的路径转折点,此外本发明能够减小移动小车在运动过程中线速度与角速度的剧烈变化,保证了其运动的平稳性。
Description
技术领域
本发明属于移动小车路径规划领域,具体涉及到一种融合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,NR表示随节点扩展层数R变化而变化的节点下一秒可移动方向,SR表示随节点扩展层数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列表时的估价函数值f0(m)大小,比较f(m)和f0(m)值的大小,若f(m)较小,则表示找到更好的路径节点,同时更新节点m的父指针,并将节点m加入ClosedList列表之中,如果f0(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与角速度ω,即速度空间Vr,Vr的表达式如下:
Vr=Vs∩Vd∩Va
其中,Vs表示移动小车运动时的最大最小速度约束,Vd表示移动小车运动时的安全约束,Va表示移动小车运动时的动力学性能约束;
步骤12:步骤11中所述的速度空间Vr中每一组线速度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,NR表示随节点扩展层数R变化而变化的节点下一秒可移动方向,SR表示随节点扩展层数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列表时的估价函数值f0(m)大小,比较f(m)和f0(m)值的大小,若f(m)较小,则表示找到更好的路径节点,同时更新节点m的父指针,并将节点m加入ClosedList列表之中,如果f0(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与角速度ω,即速度空间Vr,Vr的表达式如下:
Vr=Vs∩Vd∩Va
其中,Vs表示移动小车运动时的最大最小速度约束,Vd表示移动小车运动时的安全约束,Va表示移动小车运动时的动力学性能约束;
步骤12:步骤11中所述的速度空间Vr中每一组线速度v与角速度ω都对应着一条可行轨迹,将多条轨迹代入所述改进的DWA算法轨迹评价函数G(v,ω)中,然后利用所得值判断选出一条最优轨迹,再以其对应的线速度v与角速度ω驱使移动小车运动,即得出当前运行周期内围绕所述全局最优路径规划的局部最优动态避障路径。
以上显示和描述了本发明的基本原理、主要特征以及本发明的优点,本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (5)
1.一种融合Astar与DWA算法的移动小车路径规划方法,其特征在于,包括下列步骤:
步骤1:将移动小车从起点到终点所需经过的平面区域采用栅格法建立栅格地图,在所述栅格地图中预先设置起点与终点位置;
步骤2:运行改进的Astar算法,从所述的栅格地图的起点进行全局路径规划,获得一条从起点到终点的全局最优路径;
步骤3:运行改进的DWA算法,以步骤2所述的全局最优路径为参考路径,从步骤1所述的起点到终点位置进行局部动态避障路径规划,为步骤1所述的移动小车规划出一条最优的动态避障路径。
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列表时的估价函数值f0(m)大小,比较f(m)和f0(m)值的大小,若f(m)较小,则表示找到更好的路径节点,同时更新节点m的父指针,并将节点m加入Closed List列表之中,如果f0(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与角速度ω,即速度空间Vr,Vr的表达式如下:
Vr=Vs∩Vd∩Va
其中,Vs表示移动小车运动时的最大最小速度约束,Vd表示移动小车运动时的安全约束,Va表示移动小车运动时的动力学性能约束;
步骤12:步骤11中所述的速度空间Vr中每一组线速度v与角速度ω都对应着一条可行轨迹,将多条轨迹代入所述改进的DWA算法轨迹评价函数G(v,ω)中,然后利用所得值判断选出一条最优轨迹,再以其对应的线速度v与角速度ω驱使移动小车运动,即得出当前运行周期内围绕所述全局最优路径规划的局部最优动态避障路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111541463.5A CN114428499A (zh) | 2021-12-16 | 2021-12-16 | 一种融合Astar与DWA算法的移动小车路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111541463.5A CN114428499A (zh) | 2021-12-16 | 2021-12-16 | 一种融合Astar与DWA算法的移动小车路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114428499A true CN114428499A (zh) | 2022-05-03 |
Family
ID=81311010
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111541463.5A Pending CN114428499A (zh) | 2021-12-16 | 2021-12-16 | 一种融合Astar与DWA算法的移动小车路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114428499A (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115016510A (zh) * | 2022-08-08 | 2022-09-06 | 武汉工程大学 | 一种机器人导航避障方法、装置以及存储介质 |
CN115079705A (zh) * | 2022-08-08 | 2022-09-20 | 海相(江苏)科技有限公司 | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 |
CN117572875A (zh) * | 2024-01-15 | 2024-02-20 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
CN118310508A (zh) * | 2024-06-13 | 2024-07-09 | 中建材智能自动化研究院有限公司 | 一种货物搬运机器路径规划系统及路径规划方法 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101409323B1 (ko) * | 2013-03-19 | 2014-07-02 | 국방과학연구소 | 무인자율차량 및 이의 동적환경 주행경로생성방법 |
CN108287469A (zh) * | 2017-11-28 | 2018-07-17 | 上海师范大学 | 基于人工势场引力因子的机械臂最优避障控制方法及装置 |
CN108508893A (zh) * | 2018-03-23 | 2018-09-07 | 西安电子科技大学 | 一种基于改进a算法的机器人能效最优路径规划方法 |
CN111174798A (zh) * | 2020-01-17 | 2020-05-19 | 长安大学 | 一种足式机器人路径规划方法 |
CN111504325A (zh) * | 2020-04-29 | 2020-08-07 | 南京大学 | 一种基于扩大搜索邻域的加权a*算法的全局路径规划方法 |
CN112230634A (zh) * | 2019-06-26 | 2021-01-15 | 北京海益同展信息科技有限公司 | 一种机器人避障方法和装置 |
CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
CN112486178A (zh) * | 2020-12-03 | 2021-03-12 | 哈尔滨理工大学 | 一种基于有向d*算法的动态路径规划方法 |
CN112650242A (zh) * | 2020-12-22 | 2021-04-13 | 天津理工大学 | 一种基于混合算法的移动机器人路径规划方法 |
CN113156964A (zh) * | 2021-04-30 | 2021-07-23 | 河海大学 | 一种基于改进动态窗口法的无人地面车辆路径规划方法 |
-
2021
- 2021-12-16 CN CN202111541463.5A patent/CN114428499A/zh active Pending
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101409323B1 (ko) * | 2013-03-19 | 2014-07-02 | 국방과학연구소 | 무인자율차량 및 이의 동적환경 주행경로생성방법 |
CN108287469A (zh) * | 2017-11-28 | 2018-07-17 | 上海师范大学 | 基于人工势场引力因子的机械臂最优避障控制方法及装置 |
CN108508893A (zh) * | 2018-03-23 | 2018-09-07 | 西安电子科技大学 | 一种基于改进a算法的机器人能效最优路径规划方法 |
CN112230634A (zh) * | 2019-06-26 | 2021-01-15 | 北京海益同展信息科技有限公司 | 一种机器人避障方法和装置 |
CN111174798A (zh) * | 2020-01-17 | 2020-05-19 | 长安大学 | 一种足式机器人路径规划方法 |
CN111504325A (zh) * | 2020-04-29 | 2020-08-07 | 南京大学 | 一种基于扩大搜索邻域的加权a*算法的全局路径规划方法 |
CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
CN112486178A (zh) * | 2020-12-03 | 2021-03-12 | 哈尔滨理工大学 | 一种基于有向d*算法的动态路径规划方法 |
CN112650242A (zh) * | 2020-12-22 | 2021-04-13 | 天津理工大学 | 一种基于混合算法的移动机器人路径规划方法 |
CN113156964A (zh) * | 2021-04-30 | 2021-07-23 | 河海大学 | 一种基于改进动态窗口法的无人地面车辆路径规划方法 |
Non-Patent Citations (1)
Title |
---|
张敬寒 等: "基于扩大搜索邻域A*算法的平滑路径规划", 《长春理工大学学报(自然科学版)》, vol. 41, no. 06, 31 December 2018 (2018-12-31), pages 124 - 127 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115016510A (zh) * | 2022-08-08 | 2022-09-06 | 武汉工程大学 | 一种机器人导航避障方法、装置以及存储介质 |
CN115079705A (zh) * | 2022-08-08 | 2022-09-20 | 海相(江苏)科技有限公司 | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 |
CN117572875A (zh) * | 2024-01-15 | 2024-02-20 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
CN117572875B (zh) * | 2024-01-15 | 2024-04-12 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
CN118310508A (zh) * | 2024-06-13 | 2024-07-09 | 中建材智能自动化研究院有限公司 | 一种货物搬运机器路径规划系统及路径规划方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114428499A (zh) | 一种融合Astar与DWA算法的移动小车路径规划方法 | |
CN112378408B (zh) | 一种实现轮式移动机器人实时避障的路径规划方法 | |
CN111780777A (zh) | 一种基于改进a*算法和深度强化学习的无人车路径规划方法 | |
Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
CN115079705A (zh) | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 | |
CN112068588A (zh) | 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法 | |
CN109491389A (zh) | 一种具有速度约束的机器人轨迹跟踪方法 | |
CN112506199B (zh) | 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 | |
CN112327829A (zh) | 分布式多机器人协同运动控制方法、系统、介质及应用 | |
CN114281084B (zh) | 一种基于改进a*算法的智能车全局路径规划方法 | |
CN112539750B (zh) | 一种智能运输车路径规划方法 | |
CN112925342B (zh) | 基于改进相互速度障碍法的无人机动态避障方法 | |
CN112269381A (zh) | 基于改进人工鱼群算法的移动机器人路径规划方法 | |
CN115562290A (zh) | 一种基于a星惩罚控制优化算法的机器人路径规划方法 | |
CN115328208A (zh) | 一种实现全局动态路径规划的无人机路径规划方法 | |
CN113467476A (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
CN115826586A (zh) | 一种融合全局算法和局部算法的路径规划方法及系统 | |
Ma et al. | An improved jump point search algorithm for home service robot path planning | |
Yu et al. | RDT-RRT: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles | |
CN116429144A (zh) | 基于改进Astar和DWA融合算法的自主车辆路径规划方法 | |
Yan et al. | A study of improved global path planning algorithm for parking robot based on Ros | |
CN115167466A (zh) | 一种移动机器人标准控制集局部路径规划方法及系统 | |
CN114442634A (zh) | 一种车辆的路径规划方法、装置、设备及介质 | |
Jiang et al. | Path planning method for mobile robot based on a hybrid algorithm |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |