一种改进动态窗口算法的差速轮式机器人路径规划方法

1.本发明属于机器人学技术领域，具体涉及一种改进动态窗口算法的差速轮式机器人路径规划方法。


背景技术：

2.最初，移动机器人的应用局限于制造业，现在普遍使用在采矿、救援、物流、军事、太空、运输等领域。随着自动化技术的发展，机器人的应用越来越广泛，机器人在如今工业生产和生活中扮演着重要的角色。如今人们可以利用机器人技术实现自动驾驶，可以在工厂中使用机械臂大规模地进行快速机械加工，农业机器人的诞生极大减轻了农民的负担，特殊机器人已经被发射进太空，探索了前人从未探索过的领域。
3.在自主导航机器人执行任务时，机器人配备了多种传感器如激光雷达、深度相机等探测环境和进行定位。在得到了周围的环境建模后，在机器人起点位置和任务终点处需要得到安全和高效的线路，所以能够自主避开障碍物并高效的得到导航路径是机器人导航研究中重要的部分。路径规划分为全局路径规划和局部路径规划，全局导航中机器人需要根据包含有起点、终点和静态障碍物等信息的地图规划出一条路径。在本地路径规划中，移动机器人周围环境多变，需要在运行过程中得到周围障碍物信息规划出一条能够便于机器人跟踪的路径。
4.dijkstra在1959年发表的论文中提出一种基于贪心策略的最短路径搜索算法。nils nilsson于1968年在dijkstra基础上提出a*算法，极大地减少了路径搜索的复杂度。fox d.在1997首次提出动态窗口法，它与以前的方法不同，动态窗口法在机器人的线速度和角速度内进行采样，在采样出的路径中进行评价得到最佳的路径。lu c.等在原始dwa算法的基础上利用q-learning实现了自适应权重评价函数以适应不同的障碍物环境。lee d.h等提出一种基于有限记忆过滤的移动机器人避障算法，通过估计整体分布来避免动态障碍物，得到了相比传统dwa算法更加出色的性能。在目前的路径规划算法中，传统的a*算法无法完成整个路径规划的过程，仅能作为机器人全局路径规划的引导；相对应的传统dwa算法也仅能作为机器人局部路径规划，并且在复杂环境中适应性较差，规划出的路径会出现振荡问题。


技术实现要素：

5.为解决上述问题，本发明提供一种改进动态窗口算法的差速机器人路径规划方法，使用平滑a*路径进行全局引导，加入非线性人工斥力场障碍物评价函数，减少机器人运行时的振荡问题，同时根据障碍物环境自适应变化子评价函数权重增加算法的适应。
6.为实现上述目的，本发明提供了如下的技术方案。
7.一种基于改进动态窗口算法的差速轮式机器人规划方法，包括以下步骤：
8.s1，建立机器人运动学模型和周围环境的栅格地图，获取机器人起点和终点坐标，标记障碍物的坐标。其中机器人运动学模型如下：
[0009][0010]
式中，xk＝[pos
x pos
y aug υel ω]为机器人的状态，pos
x
为机器人的横坐标，posy为机器人的纵坐标，aug为机器人的航向角，υel为机器人的线速度，ω为机器人的角速度；f和b为状态转移矩阵，uk为输入的角速度和线速度，下标k代表第k个采样时间。
[0011]
s2，根据s1中的栅格地图，通过a*算法计算出全局规划路径。
[0012]
s3，建立平滑度函数，利用梯度下降法使得平滑度最高，平滑度函数如下：
[0013]
f＝c1||x
i-yi||+c2||y
i-y
i+1
||
[0014]
式中，传统a*算法路径规划的结果为点序列为[x1，x2，...，xn]，平滑后的路径规划的结果为点序列[y1，y2，...，yn]，c1c2为参数，||x
i-yi||为平滑后点与原始点的偏离程度，||y
i-y
i+1
||为平滑后点之间的距离，由梯度下降法求得yi使得||x
i-yi||2(平滑后点与原始点的偏离程度)与||y
i-y
i+1
||(平滑后点之间的距离)取值最小，f值越小路径越平滑。
[0015]
s4，初始化机器人运动学模型和机器人性能参数，包括机器人最大线速度υ
max
、角速度ω
max
、线加速度a
max
和角加速度α
max
以及参数采样率。
[0016]
s5，检测当前机器人位置。
[0017]
s6，根据机器人的性能参数得到角速度和线速度的区间范围，即速度窗口。
[0018]
s7，根据s4中设置的参数采样率和s6中得到的速度窗口，对速度窗口进行采样，得到线速度和角速度的多种组合。
[0019]
s8，根据s1中的机器人运动学模型和s7中的线速度、角速度组合，航迹推演得多个机器人运动学模型估计位置和与之相对应的线速度、角速度组合。
[0020]
s9，根据人工斥力势场公式计算机器人运动学模型估计位置与障碍物的距离，计算得到障碍物距离评价得分，计算公式如下：
[0021][0022]ureq
为斥力势场的势场函数，其中，k1为正比例增益系数，q为机器人当前坐标，q
obs
为障碍物坐标，ρ(q，q
obs
)为一矢量，方向从障碍物指向机器人，大小为机器人到障碍物距离，ρ0为一常数，表示障碍物斥力场对机器人产生作用的最大范围。斥力指斥力势场作用在机器人上的力，大小为斥力势场的负梯度，计算公式如下：
[0023][0024]
s10，根据机器人当前航向角计算机器人运动学模型估计位置与目标点的航向角偏差值，计算得到航向角评价得分，计算公式如下：
[0025]
heading(υ，ω)＝k2·
(tan(ρ(
qobs
，q))-aug)
[0026]
式中，k2为比例系数，q
obs
是障碍物坐标，q是机器人坐标，ρ(q
obs
，q)是一矢量，方向由机器人指向障碍物，auq为机器人航向角。
[0027]
s11，根据s7采样速度中的线速度绝对值计算速度评分，计算得到速度评价得分，计算公式如下：
[0028]
υel(υ，ω)＝k3·
|υ|
[0029]
其中，υ是机器人的线速度，ω为机器人的角速度，k3为比例参数。
[0030]
s12，计算得到评价机器人运动学模型估计位置与a*路径位置关系得分，评价函数如下：
[0031][0032]
式中，υ为机器人线速度，ω为机器人角速度，x
i+m
为机器人横坐标，y
i+m
为机器人纵坐标，d代表机器人与上述点的距离。
[0033]
s13，将s9、s10、s11和s12中的评价得分归一化后，分别乘以权重并相加，得到模型预估位置的总评价得分，计算公式如下：
[0034]
j(υ，ω)＝ω1·
heading(υ，ω)+w2·
uel(u，ω)+w3·ureq
(υ，ω)+w4·
astardist(υ，ω)
[0035]
式中，w1、w2、w3、w4分别是总评价得分公式中各子项的权重，w2、w4是固定参数，w1、w3为根据环境自适应参数，υ、ω分别是机器人采用的速度和线速度。
[0036]
s14，在步骤13中，将航向角和障碍物距离评价得分公式的权重之和设为一个定值，即设w1+w3＝w
′
，计算航向角评价子函数的权重，计算公式如下：
[0037][0038]
式中，k4为系数，d
obs
和θ可以看作与障碍物形成的夹角和与障碍物的距离，w1与两者的乘积正相关，式中是w1下限值且上限值小于w
′
。
[0039]
s15，在多种线速度和角速度的组合中选取评价分最高的组合。
[0040]
s16，根据评价分最高的线速度和角速度组合驱动机器人移动。
[0041]
s17，若机器人到达目标点则停止移动，否则重复s5-s16。
[0042]
本发明的有益效果：首先，本发明采用a*路径进行辅助引导，增强了传统动态窗口算法全局路径搜索能力；其次，本发明引入人工斥力势场非线性函数评价机器人与障碍物关系，取代了传统算法仅评价周围最近障碍物对机器人的影响，而是同时评价周围多个障碍物对机器人影响，减少了机器人运行时的振荡，提高了机器人运行时的稳定性；最后，本发明根据周围障碍物与机器人距离和角度判断障碍物对机器人的阻碍程度，引入自适应评价权重取代原始算法中固定不变的权重，增强了算法在不同环境中的适应能力，避免了机
器人无意义的避障行为，有效缩短了机器人运行路径的长度。
附图说明
[0043]
图1是本发明实例提供的算法流程图；
[0044]
图2是本发明实例提供的算法航迹推演原理图；
[0045]
图3是本发明实例提供的人工障碍物斥力势场示意图；
[0046]
图4是本发明实例提供的简单环境下算法优化前的原始算法路径规划结果图；
[0047]
图5是本发明实例提供的简单环境下本发明改进算法路径规划结果图；
[0048]
图6是本发明实例提供的复杂环境下算法优化前的原始算法路径规划结果图；
[0049]
图7是本发明实例提供的复杂环境下本发明改进算法路径规划结果图。
具体实施方式
[0050]
为了使本发明的目的、技术方案以及改进地点更加清除明白，以下结合示意图以及实际案例对本发明进行更加详细的说明。
[0051]
s1，建立运动学模型和周围环境的栅格地图，标记障碍物的坐标、起点和终点，其中机器人运动学模型如下：
[0052][0053]
式中，x＝[pos
x pos
y aug υel ω]为机器人的状态，pos
x
为机器人的横坐标，posy为机器人的纵坐标，aug为机器人的航向角，υel为机器人的线速度，ω为机器人的角速度；f和b为状态转移矩阵，u为输入的角速度和线速度，下标k代表第k个采样时间。
[0054]
s2，根据s2中的栅格地图，通过a*算法计算出全局规划路径。
[0055]
s3，建立平滑度函数，利用梯度下降法使得平滑度最高，平滑度函数如下：
[0056]
f＝c1||x
i-yi||+c2||y
i-y
i+1
||
[0057]
式中，传统a*算法路径规划的结果为点序列为[x1，x2，...，xn]，平滑后的路径规划的结果为点序列[y1，y2，...，yn]，c1c2为参数，||x
i-yi||为平滑后点与原始点的偏离程度，||y
i-y
i+1
||为平滑后点之间的距离，由梯度下降法求得yi使得||x
i-yi||2(平滑后点与原始点的偏离程度)与||y
i-y
i+1
||(平滑后点之间的距离)取值最小，f值越小路径越平滑。
[0058]
s4，初始化机器人运动学模型和机器人性能参数，包括机器人最大线速度为1m/s、角速度20
°
/s、线加速度为0.2m/s2和角加速度50
°
/s2；设计速度采样率为0.1s。
[0059]
s5，检测当前机器人位置。
stardist(υ，ω)
[0078]
式中，w1、w2、w3、w4分别是总评价得分公式中各子项的权重，w2、w4是固定参数，w1、ω3为根据环境自适应参数，υ、ω分别是机器人采用的速度和线速度。
[0079]
s14，在步骤13中，将航向角和障碍物距离评价得分公式的权重之和设为一个定值，即设ω1+w3＝w
′
，计算航向角评价子函数的权重，计算公式如下：
[0080][0081]
式中，k4为系数，d
obs
和θ可以看作与障碍物形成的夹角和与障碍物的距离，机器人与障碍物的夹角越小表示障碍物对阻碍机器人到达目标点的影响越大，障碍物距离机器人越近表示障碍物对机器人移动的阻碍越大。所以，w1与两者的乘积正相关，式中是w1下限值且上限值小于w
′
。
[0082]
s15，在多种速度组合中选取评价分最高的组合。
[0083]
s16，根据评价分最高的速度组合驱动机器人移动。
[0084]
s17，若机器人到达目标点则停止移动，否则重复s5-s16。
[0085]
在本实施例中：
[0086]
参照图1，本技术中的改进算法是在传统动态窗口法的基础上做出了几点改进，主要包括引入全局路径，非线性障碍物评价函数，评价函数中子评价函数的自适应权重。全局路径的产生依托于a*算法生成的初始路径，a*算法是一种全局路径规划算法，可以解决传统动态窗口法在全局环境中寻路较差的现象产生。引入非线性函数的原因是，传统动态窗口法仅评价最近的障碍物对机器人的影响，而在实际应用中，障碍物可以看成是由大量离散点组成的，需要同时评价周围多个障碍物共同对机器人产生的影响。
[0087]
如图2，在基础动态窗口法中机器人根据采样多组采样速度进行航迹推演，根据评价函数取评分最高的速度组合。障碍物距离仅与机器人最近的障碍物相关，这样的评价函数不能考虑多个障碍物对机器人位置的评价，有些情况下会产生较差的路径。当机器人进入两边为对称障碍物时，机器人的初始位置偏右，离左边障碍物距离较远，所有机器人偏左的路线将会得到更高的得分。当机器人运行至左边时，情况会发生转变，因为机器人仅对最近障碍物的距离进行评价，所以机器人偏右的路线此时会比偏左的路线获得更高的评价，导致路线十分震荡。本发明采用人工斥力场中非线性函数代替单一障碍物的评价方式可以有效解决这个问题。参照图3为两个平行直线障碍物上生成的斥力势场可视化图，可看障碍物形成了较高的斥力势场，且离障碍物越近，斥力势场越强大；而在平行障碍物中间可行走区域势场较低，说明引入斥力势场让障碍物和可行走区域有更好的区分度。
[0088]
在原始算法中，路径评价函数中的权重系数一直保持恒定，机器人对于障碍物多变的情况下适应性较差。在实际情况中，机器人面对的路况是十分复杂的，如果机器人身处复杂障碍物环境中时，机器人的首要任务必然是避开障碍物，所以障碍物距离评价函数的权重应该适当提高以远离障碍物保障机器人的安全性。但是当机器人与目标点之间的障碍物较少或者没有障碍物时，障碍物距离评价函数就失去了意义，此时应该减少障碍物距离评价子函数。提高航向角评价子函数和目标距离评价子函数以强化机器人径直驶向障碍物，这样可以缩短路径规划的距离。所以航向角评价函数的权重和避障评价函数是相互矛盾的，可以将航向角和避障评价函数的权重之和设为一个定值，设w1+w3＝w
′
，其中，w1为航向角评价函数权重，w3为避障评价函数权重，w
′
是定值参数。航向角评价子函数的权重可以
根据公式决定。在式中，d
obs
和θ可以看作与障碍物形成的夹角和与障碍物的距离，是下限值且小于ω
′
。
[0089]
本实施例的实验验证：
[0090]
参照图4、5和图6、7，本技术的实验部分分为小型简单场景和大型复杂场景，用以验证改进算法的有效性。参照图4和图5为简单场景下的算法效果对比，地图大小为9m
×
9m，起点坐标(1,1)，终点坐标(7,7)，初始航向角为0。传统算法和改进算法均能在简单障碍物环境中规划出可行路径。与传统算法相比，改进算法路径更短，规划路径的长度比传统动态窗口算法减少13.2％，运行时间减少21.3％。且振荡部分较少，更加平滑。参照图6和图7，本技术的改进算法在复杂环境中的路径同样优于传统算法，复杂场景中地图大小为9m
×
9m，起点坐标(0.5,25)，终点坐标(25,0.5)，初始航向角为π/4，相较于传统算法路径混乱的表现，改进后的算法在路径长度上减少23％，运行时间上减少11％，改进算法更加稳定，可以很好适应移动机器人在复杂环境下得到较平滑路径的要求。
[0091]
传统算法和改进算法在简单障碍物环境和复杂障碍物环境的路径规划结果如表1和表2所示。
[0092]
表1简单环境下的两种算法结果对比
[0093][0094]
表2复杂环境下的两种算法结果对比
[0095][0096]
以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。