一种全局与局部融合的动态路径规划方法

机器人移动过程中自主导航和避障应用研究的一个关键问题是如何提高移动机器人自主学习的能力，使其能够在未知环境下通过自主学习从起点移动到达指定目标点而不发生碰撞。

文献[1]针对多台移动式起重机缆索并联机器人的定位、避障规划与控制问题，采用三
维网格图方法构建环境图模型，并将相对定
位与绝对定位方法相结合，提出了一种基于网格的人工势场协同定位方法，对CPRMCS 系统进行了全局路径规划。

文献[2]针对非完整约束轮式机器人的非线性跟踪与避障控制问题，引入了扩展状态观测器估计轮式移动
Dynamic Path Planning Combining Dijkstra and
Dynamic Window Approach
ZHANG Jie ，ZHANG Lin
(College of Mathematics and Computer Application,Shangluo University,Shangluo 726000,Shaanxi)Abstract:Aiming at the needs of local and global optimal path planning and real-time obstacle avoidance for mobile robots,a dynamic path planning method combining improved Dijkstra and dynamic window method is proposed,which ensures that mobile robots can complete local path planning under the condition of achieving global dynamic obstacle avoidance.Experimental results show that,compared with the traditional Dijkstra algorithm,the proposed algorithm has smoother path planning,can avoid real obstacles,and can output control parameters,which is beneficial to the automatic control of the pared with the dynamic window method,the proposed algorithm ensures the global optimality of path planning and reduces the navigation time by 12.3%.This research has some reference value for the application of mobile robot autonomous navigation.
Key words:mobile robot;Dijkstra;dynamic window approach;path planning;autonomous navigation 一种全局与局部融合的动态路径规划方法
张洁，张林
（商洛学院数学与计算机应用学院，陕西商洛726000）
摘要：针对机器人移动过程中实时避障时局部路径规划及全局路径规划最优的需求，提
出了一种融合Dijkstra 方法和动态窗口法的动态路径规划方法，该方法可保证机器人在实现全局动态避障的条件下高效完成局部路径规划。

实验结果表明：本方法与传统的Dijkstra 算
法相比，规划的路径更加平滑，可实现实时避障的同时输出控制参数，为机器人的自动控制提供支撑；与动态窗口法相比，所提算法保证了路径规划的全局最优性，导航时间缩短了
12.3%。

可用于机器人或各类无人航行系统的自主导航。

关键词：移动机器人；动态窗口法；路径规划；自主导航中图分类号：TP242.6
文献标识码：A
文章编号：1674-0033（2021）02-0018-05
doi:10.13440/j.slxy.1674-0033.2021.0２.0０4
第35卷第２期２０21年４月
商洛学院学报
ＪｏｕｒｎａｌｏｆＳｈａｎｇｌｕｏＵｎｉｖｅｒｓｉｔｙ
Vol ．35Ｎｏ．２Ａｐｒ.２０21
收稿日期：2020-12-28
基金项目：公共大数据技术研究创新团队（SK2018-03-01）作者简介：张洁，女，陕西商州人，硕士，高级工程师
机器人的未知扰动和速度信息，并为复杂环境下的目标跟踪和避障，设计了一种非线性控制器，使机器人在障碍物检测区域内有效避障。

文献[3]针对移动机器人路径规划优化的问题，给出一个基于布谷鸟搜索算法设计的一种新的元启发式方法，该方法在机器人与目标和障碍物之间建立了满足机器人避障条件的函数关系，可用于移动机器人在未知或部分已知环境下的路径规划。

此外，常见的针对机器人路径规划的方法还包括基于生物启发式[4]的遗传算法、蚁群算法及细菌势场方法，基于模型的动态窗口法和人工势场法，基于节点的Dijkstra算法、A*算法及D*算法，基于采样的快速搜索随机树方法和Voronoi图方法[5]。

全局路径规划解决了机器人要去哪里的问题。

定位完成后，机器人将自己的位置[6]设为起点，目标点设为终点。

从起点到终点的全局路径规划过程中有三个基本要求：一是准确到达目标点；二是航行过程中不能与障碍物发生碰撞[7]；三是选择最快到达目的地的路径。

传统的Dijkstra算法虽然是最短路径算法，但属于贪心算法，需要遍历每个连接节点。

节点越多，耗时越长。

本文将Dijkstra算法和动态窗口方法相结合，提出一种新的机器人混合路径规划方法。

1算法原理
Dijkstra算法使用了贪心算法模式求解最短路径。

该算法解决关于有向图中单个源点到其他节点的最短路径问题[8]，其主要思路是每次迭代时选择的下一个节点是标记点之外距离源点最近的节点。

在机器人导航领域，这个算法将一个节点固定为起始点，从而寻找到目标点的最佳路径。

解决机器人导航过程中如何到达目的地的问题用全局路径规划，最优路径可以由机器人根据全局地图计算出，但很多不确定因素在实际移动过程中存在。

如全局地图与局部实际情况不符，某个地方多出行人或障碍物等情况[9]。

局部路径规划非常重要，否则如果只按照全局地图执行导航任务，如同盲人过马路一样危险。

局部路径规划主要是对局部未知障碍物进行避开障碍，避障后返回已计算的全局路径的值。

本算法的核心思想是将基于局部路径规划的动态窗口法和最短路径算法的全局规划算法结合起来使用，算法首先根据全局规划方法规划一个最短行驶路径，在机器人移动过程中使用动态窗口法不断进行局部规划，并根据局部规划和全局规划后的最优解进行实际的路径规划。

1.1算法1（全局规划算法）
输入：起始点p t
输出：最短路径d j
假设算法路径网中每个节点都有标号（d t, P t），d t是从出发点s到点t的最短路径长度；P t 表示从s到t的最短路径中t点的前一个点。

求解从出发点s到点t的最短的路径算法的基本过程为：
第一步，初始化。

出发点设置为Ｐs=null d s=0；所有其他点dｉ=∞，p i未定义标记起始点s，记k=s，其他所有点设为未标记。

第二步，检验从所有已标记的点k到其他的直接连接的未标记的点j的距离，并设置：dｊ＝ｍｉｎ［ｄｊ，ｄｋ＋ｗ（ｋ，ｊ）］(1)其中，表示ｗ（ｋ，ｊ）从k到j的路径长度。

第三步，选取下一个节点。

从所有未标记的点中选取路径最小的点i，点i被选为最短路径中的一点，并设为已标记的点。

第四步，找到点i的前一节点。

从已经标记的点集合中找到直接连接到点i的节点，并标记。

标记点i。

若所有的节点已标记，则算法结束。

否则，记k=i，转到第二步继续。

1.2算法2（局部规划算法）
动态窗口法（DWA方法）是基于机器人运动学和动力学[10]的局部路径规划算法，核心思想是将路径规划问题转化成速度矢量空间上的约束优化问题。

本算法中输入参数包括：当前状态、模型参数、目标点、评价函数的参数、障碍物位置和障碍物半径。

输出参数为：控制量和轨迹集合。

由于机器人运动可以分解为直线速度淄和旋转速度棕，所以V r(淄,棕)集合便是机器人的速度矢量空间，在此空间中实现对机器人运动控制的命令搜索。

速度矢量空间V r由速度矢量集合V s、速度动态窗口V a和许可速度矢量V d 的交集构成。

V r=V s∩V a∩V d(2)
张洁，张林：一种全局与局部融合的动态路径规划方法19第2期
式(2)中的V d是许可速度，表示机器人的加速度能力不导致碰撞的速度，可表示为：
V d=｛淄,棕)|淄≤2dis tan t(淄,棕)·a淄
姨棕≤
2dis tan t(淄,棕)·a棕
姨｝(3)式(3)中，dis tan t(淄,棕)表示机器人障碍物的最小距离，a淄，a棕为机器人平移的加速度和旋转加速度。

为了从速度矢量空间中得到机器人在k+1时刻的最优值，需要提前设定DWA的标准目标函数：
G(淄,棕)=酌·淄el(淄,棕)+α·heading(淄,棕)+β·dis tan(淄,棕)(4) heading(淄,棕)函数可以对机器人k+1时刻朝向目标点的程度作出评估，当机器人与目标点的夹角为0°时，此函数取最大值：
heading(淄,棕)=1-θ
π
(5)
dis tan(淄,棕)函数可以对机器人k+1时刻自身位置和目标点的距离作出评估，函数值越大说明与目标点距离越大，发生碰撞的几率越小；函数值越小说明离障碍物越近，发生碰撞几率越大。

dis tan t(淄,棕)定义如式(6)所示：
dis tan t(淄,棕)=
1
L
,if0≤l≤L
1,if L≤
≤
l
(6)
淄el(淄,棕)函数是机器人速度的大小，其定义如式(7)所示：
淄el(淄,棕)＝淄
淄max
(7)
其中淄是机器人平均速度，淄max是在淄ｒ机器人的最大速度。

1.3算法3（混合规划算法）
本算法的输入参数分别为算法1和算法2的输出参数。

输出参数是优化过的运动方向和时速。

算法首先会每隔一个时间间隔检测局部规划算法中计算出的局部移动速度和方向是否和全局的一致，如果一致算法会休眠一个时间周期后重新获取新的输入参数。

如果不一致，就会计算全局路径中可达的抽样节点到当前位置的距离d n及抽样节点到目标节点的距离d j。

然后计算d n与d j相加后的计算值，选择计算值最小的一条路径继续前进，一个时间周期后重新再检测输入参数直到到达目的地为止。

具体流
程如图1所示。

图1算法3的计算流程
2仿真实验与结果分析
2.1实验环境
仿真平台使用matlab进行验证，机器人状态包括坐标（x,y）、航向角、速度、角速度。

机器人初始位置设为（0,0），目标点位置为（10,10），共设置10个障碍物，坐标点分别为（0,2），（2,4），（2,5），（4,2），（5,4），（5,6），（5,9），（8,8），（8,9），（7,9），用于冲突判定的障碍物半径R=0.5，模拟区域范围为[-111，-1 11]。

评价函数参数[11]分别由航向得分比重、距离得分比重、速度得分比重、向前模拟轨迹的时间衡量，模拟实验结果包括走过的轨迹及导航时间。

DWA输入参数有返回控制量及轨迹，机器人移动到下一个时刻的状态量根据当前速度和
是
结束
２０２1年4月
商洛学院学报20
角速度推导[12]。

由坐标上两点之间的距离确定机器人是否到达目标点，记录机器人走过的所有位置坐标，作为机器人移动的历史数据。

将本文提出的融合方法与DWA 方法进行实验比对。

2.2实验结果分析
实验结果表明，在保证其他约束条件一致的情况下，两种算法都能成功实现移动机器人由起始点至目标点的导航，且图2机器人起始状态、图3中间状态和图4终止状态图表明，两种算法的移动轨迹基本一致，但根据表1和表2的导航用时数据，两种算法的导航用时有所不同。

在保持障碍物数量及位置不变及障碍物半径相同的条件下，8次实验中融合算法的导航用时比只使用DWA 算法的导航用时平均减少了5.467s ，减少比例约为12.3%。

图2机器人初始状态
图3机器人中间状态
图4机器人终止状态表1机器人8次导航用时
表2机器人8次导航平均用时
3结论
针对机器人在移动过程中的路径规划的问题，本文给出了一种融合Dijkstra 方法和动态窗口法的混合路径规划方法。

基于动态窗口算法，构造了一种考虑全局最优路径的评价函数，在保证路径规划的全局最优性的同时，可避免动态窗口法陷入局部最优的问题。

相比较传统Dijkstra 算法，本文提出的融合路径规划的方法可实现机器人在实时避开障碍的同时更加节省时间。

但因为实验中障碍物分布不复杂，地图较为简单，以及较为理想的条件设置的障碍物半
实验次数/次
融合算法导航用时/s
DWA 导航用时/s
138.96044.5692
38.30442.963339.17243.108439.46143.094538.81545.9546
38.65243.760739.29946.0618
38.465
45.424
算法
融合算法DWA 算法平均用时/s
38.891
44.367
-2
2
46810
时间/s
109876543210距离/m
障碍物
1086420
2
46
8
10
时间/s
距离/m
障碍物
张洁，张林：一种全局与局部融合的动态路径规划方法21
第2期时间/s
距离/m
10
864
20
2
4
6810
障碍物
径都相同，所以本文给出的方法在实际应用中还可进一步的扩展优化。

参考文献：
[1]ZI B,LIN J,QIAN S.Localization,obstacle avoidance
planning and control of a cooperative cable parallel robot for multiple mobile cranes[J].Robotics and Computer-integated Manufactring,2015,34(8):105-123.
[2]YANG H J,FAN X Z,SHI P.Nonlinear control for
tracking and obstacle avoidance of a wheeled mobile robot with nonholonomic constraint[J].IEEE Transactions on Control Systems Technology,2016,24(2): 741-746.
[3]MOHANTY P K,PARHI D R.Optimal path planning
for a mobile robot using cuckoo search algorithm[J].
Journal of Experimental&Theoretical Artificial Intelligence,2016,28(1-2):35-52.
[4]XIAO L,ZHANG Y N.Dynamic design,numerical
solution and effective verification of acceleration-level obstacle-avoidance scheme for robot manipulators[J].
International Journal of Systems Science,2016,47(4): 932-945.
[5]KIM B,PINEAU J.Socially adaptive path planning in
human environments using inverse reinforcement
learning[J].International Journal of Social Robotics, 2016,8(1):51-66.
[6]HAN D,NIE H,CHEN J B,et al.Dynamic obstacle
avoidance for manipulators using distance calculation and discrete detection[J].Robotics and Computer-integrated Manufacturing,2018,49(2):98-104.
[7]WEI K,REN B Y.A Method on Dynamic path
planning for robotic manipulator autonomous obstacle avoidance based on an improved RRT algorithm[J].
Sensors,2018,18(2):71.
[8]FERRERA E,CAPITAN J,CASTANO A R,et al.
Decentralized safe conflict resolution for multiple robots in dense scenarios[J].Robotics and Autonomous Systems,2017,91(5):179-193.
[9]TANVEER M H,RECCHIUTO C T,SGORBISSA A.
Analysis of path following and obstacle avoidance for multiple wheeled robots in a shared workspace[J].
Robotica,2019,37(1):80-108.
[10]申浩宇,吴洪涛,陈柏,等.基于主从任务转化的冗余度
机器人避障算法[J].机器人,2014,36(4):425-429. [11]吴玉香,王超,冼颖宪.基于稀疏直接法和图优化的移
动机器人SLAM[J].仪器仪表学报,2018,39(4):257-263.
[12]宋宇,李庆玲,康轶非,等.平方根容积Rao-Blackwillised
粒子滤波SLAM算法[J].自动化学报,2014,40(2):357-367.
２０２1年4月
商洛学院学报
22
（责任编辑：李堆淑）。