一种基于DWA与人工势场融合的局部路径规划方法与流程

一种基于dwa与人工势场融合的局部路径规划方法
技术领域
1.本发明涉及人工智能技术领域，更为具体的，涉及一种基于dwa与人工势场融合的局部路径规划方法。


背景技术：

2.自动驾驶技术已经成为了现代车辆智能化一个重要的研究方向，且路径规划又是自动驾驶领域的一个关键技术。路径规划分为全局路径规划和局部路径规划，全局规划路径是根据已知的障碍物建立整体的路径，局部规划路径是根据无人车携带的传感器检测实时的障碍物信息，根据检测的障碍物信息建立局部环境模型，再根据新建立的环境进行路径规划。局部路径规划主要是用于动态环境下无人车的导航和避障，使无人车在可能存在危险以及未知静态障碍物，动态障碍物的环境内能够进行及时的处理。
3.dwa直接来源于移动机器人的运动动力学，适用于有限速度和加速度的约束。在dwa所允许的速度中，通过最大化目标函数，选择最优的速度。其目标函数包括向目标位置前进的度量、前进的速度以及预测轨迹与障碍物距离的度量，所有目标函数形成一个碰撞策略。在传统dwa方法中，速度评价项是在每个速度组的预测轨迹上越大越优，即只考虑速度组的线速度大小，目的是使无人车在避开障碍物的同时也能以较快的速度行驶，但在实际的运行中，速度并不是在每个预测轨迹上进度都越大越好，应该是在正确轨迹上的进度越大越好。
4.人工势场算法也是一种应用较为广泛的路径规划算法，因为其比较高效，准确，以及实时性较好。但人工势场算法会出现零势场位置，从而出现局部极小值的缺点，这样就无法准确的规划路径。并且实际环境是比较复杂，以及充满了不确定性，用人工势场算法会出现振荡的问题，虽然有人提出过人工势场改进的方法，但在实际运行中问题依然没有得到很好的解决。


技术实现要素：

5.本发明的目的在于克服现有技术的不足，提供一种基于dwa与人工势场融合的局部路径规划方法，不仅使速度选择更优，而且从总体上强化了障碍物和目标点的作用，实现更加准确的规划路径。
6.本发明的目的是通过以下方案实现的：
7.一种基于dwa与人工势场融合的局部路径规划方法，包括步骤：
8.s1、采集无人车起始位置，目标点，规划无人车全局路径，设置无人车局部动态地图大小；
9.s2、采集当前时刻无人车位置，传感器探测的障碍物位置参数；
10.s3、根据局部地图和已经规划好的全局路径计算无人车当前时刻的局部目标点；
11.s4、根据无人车当前时刻的位置，局部目标点和障碍物位置参数计算局部地图内的引力势场和斥力势场，并将引力势场和斥力势场乘以各自系数形成局部地图各个位置的
合势场；
12.s5、采集当前时刻的速度，根据无人车的加速度，计算当前时刻无人车所能达到速度的速度窗口；
13.s6、将当前的速度窗口均分离散为多组角速度合线速度的速度组；
14.s7、各速度组结合合势场梯度最速下降方向融合各速度组计算dwa速度评价项，并进行归一化；
15.s8、以每组速度持续前进设定时间后计算预测轨迹；
16.s9、计算dwa目标距离评价项和dwa障碍物距离评价项；
17.s10、将三个评价项乘以各自的重要系数，得到总的评分，选择评分最高的速度组作为当前时刻的最优行驶速度；
18.s11、如果到达最终目标点，则无人车停止运动，否则重复步骤s2-s10。
19.进一步地，在步骤s4中，包括子步骤：
20.s41，取局部动态地图与全局路径的交点作为当前局部目标点；
21.s42，计算局部地图内每个栅格相对于局部目标点的引力势场，计算公式如下：
[0022][0023]
式中，k
att
为无人车在行驶时目标所引起的引力势场函数的增益因子，x为各栅格位置，x
goal
为局部目标点位置；
[0024]
s43，采集传感器在局部地图内探测的障碍物位置参数；
[0025]
s44，根据障碍物位置参数计算局部地图内每个栅格的斥力势场，计算公式如下：
[0026][0027]
式中，k
rep
为无人车在行驶过程中障碍物引起的斥力势场函数的增益因子，x
obs
障碍物位置；s是斥力势场影响范围的半径；与引力作用相反，障碍物与无人车的距离越大则斥力作用越小，两当者间的距离大于某个距离时时，障碍物就不会再对无人车发生作用；
[0028]
s45，计算局部地图内各个栅格的合势场：u＝αu
att
(x)+βu
rep
(x)；
[0029]
式中，α为引力势场系数，β为斥力势场系数。
[0030]
进一步地，在步骤s9中，所述计算dwa目标距离评价项包括子步骤：计算每组速度预测轨迹末端与局部目标点的距离的倒数，并进行归一化；所述计算dwa障碍物距离评价项包括子步骤：首先计算各速度组预测轨迹是否和局部地图障碍物位置相交，若存在相交点则此速度组障碍物评价为0；如果障碍物与预测轨迹的最小距离小于无人车外接圆半径则此速度组障碍物评价为0；当障碍物与各速度组预测轨迹的最小距离大于一个安全范围时，则将这一项设置为一个常数；这一个安全范围选择无人车以最大的速度行驶，然后以最大加速度进行减速，无人车在减速到速度为0，即无人车停下来；这一段距离就是无人车的一个安全距离；另一方面，如果无人车在行驶过程中始终没有障碍物的话那么这一项也始终为一个常数；然后对此项进行归一化。
[0031]
进一步地，在步骤s5中，包括子步骤：设置速度窗口更新频率为设定数值。
[0032]
进一步地，在步骤s6中，所述多组角速度合线速度的速度组为20组。
[0033]
进一步地，在步骤s8中，所述设定时间为3秒。
[0034]
本发明的有益效果是：
[0035]
本发明实施例，一方面使速度评价项是在正确的预测轨迹上越快越好，另一方面弥补了人工势场算法出线局部极小值的缺点，以及在实际环境比较复杂时的振荡问题；不仅使速度选择更优，而且从总体上强化了障碍物和目标点的作用。
附图说明
[0036]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0037]
图1为梯度方向与速度关系示意图；
[0038]
图2为局部路径规划流程图；
[0039]
图3为速度窗口示意图。
具体实施方式
[0040]
本说明书中所有实施例公开的所有特征，或隐含公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合和/或扩展、替换。
[0041]
一种基于dwa与人工势场融合的局部路径规划方法，包括步骤：
[0042]
s1、采集无人车起始位置，目标点，规划无人车全局路径，设置无人车局部动态地图大小；
[0043]
s2、采集当前时刻无人车位置，传感器探测的障碍物位置参数；
[0044]
s3、根据局部地图和已经规划好的全局路径计算无人车当前时刻的局部目标点；
[0045]
s4、根据无人车当前时刻的位置，局部目标点和障碍物位置参数计算局部地图内的引力势场和斥力势场，并将引力势场和斥力势场乘以各自系数形成局部地图各个位置的合势场；
[0046]
s5、采集当前时刻的速度，根据无人车的加速度，计算当前时刻无人车所能达到速度的速度窗口；
[0047]
s6、将当前的速度窗口均分离散为多组角速度合线速度的速度组；
[0048]
s7、各速度组结合合势场梯度最速下降方向融合各速度组计算dwa速度评价项，并进行归一化；
[0049]
s8、以每组速度持续前进设定时间后计算预测轨迹；
[0050]
s9、计算dwa目标距离评价项和dwa障碍物距离评价项；
[0051]
s10、将三个评价项乘以各自的重要系数，得到总的评分，选择评分最高的速度组作为当前时刻的最优行驶速度；
[0052]
s11、如果到达最终目标点，则无人车停止运动，否则重复步骤s2-s10。
[0053]
在本发明可选的实施方式中，需要说明的是，在步骤s4中，包括子步骤：
[0054]
s41，取局部动态地图与全局路径的交点作为当前局部目标点；
[0055]
s42，计算局部地图内每个栅格相对于局部目标点的引力势场，计算公式如下：
[0056][0057]
式中，k
att
为无人车在行驶时目标所引起的引力势场函数的增益因子，x为各栅格位置，x
goal
为局部目标点位置；
[0058]
s43，采集传感器在局部地图内探测的障碍物位置参数；
[0059]
s44，根据障碍物位置参数计算局部地图内每个栅格的斥力势场，计算公式如下：
[0060][0061]
式中，k
rep
为无人车在行驶过程中障碍物引起的斥力势场函数的增益因子，x
obs
障碍物位置；s是斥力势场影响范围的半径；与引力作用相反，障碍物与无人车的距离越大则斥力作用越小，两当者间的距离大于某个距离时时，障碍物就不会再对无人车发生作用；
[0062]
s45，计算局部地图内各个栅格的合势场：u＝αu
att
(x)+βu
rep
(x)；
[0063]
式中，α为引力势场系数，β为斥力势场系数。
[0064]
在本发明可选的实施方式中，需要说明的是，在步骤s9中，所述计算dwa目标距离评价项包括子步骤：计算每组速度预测轨迹末端与局部目标点的距离的倒数，并进行归一化；所述计算dwa障碍物距离评价项包括子步骤：首先计算各速度组预测轨迹是否和局部地图障碍物位置相交，若存在相交点则此速度组障碍物评价为0；如果障碍物与预测轨迹的最小距离小于无人车外接圆半径则此速度组障碍物评价为0；当障碍物与各速度组预测轨迹的最小距离大于一个安全范围时，则将这一项设置为一个常数；这一个安全范围选择无人车以最大的速度行驶，然后以最大加速度进行减速，无人车在减速到速度为0，即无人车停下来；这一段距离就是无人车的一个安全距离；另一方面，如果无人车在行驶过程中始终没有障碍物的话那么这一项也始终为一个常数；然后对此项进行归一化。
[0065]
在本发明可选的实施方式中，需要说明的是，在步骤s5中，包括子步骤：设置速度窗口更新频率为设定数值。
[0066]
在本发明可选的实施方式中，需要说明的是，在步骤s6中，所述多组角速度合线速度的速度组为20组。
[0067]
在本发明可选的实施方式中，需要说明的是，在步骤s8中，所述设定时间为3秒。
[0068]
如图2，图3所示，本发明其他实施例还提出了一种基于dwa与人工势场融合的局部路径规划方法，一方面使速度评价项是在正确的预测轨迹上越快越好，一方面弥补了人工势场算法局部极小值的缺点，以及在实际环境比较复杂时的振荡问题。这是由于人工势场是dwa中速度评价项的一个作用项，当出现局部极小值时，人工势场不起作用，dwa为传统算法模式。当环境比较复杂势场出现振荡，只是会使速度评价项有影响，不会对dwa其他评价项产生影响，dwa相当于弱化了人工势场的振荡影响。人工势场考虑了障碍物和目标点，所以dwa速度评价项融合障碍物和目标点的影响，一方面使速度选择更优，另一方面从总体上强化了障碍物和目标点的作用。
[0069]
在人工势场构建中，包括采集无人车起点、局部目标点、局部障碍物位置参数，构
建局部地图引力势场和斥力势场的合势场，dwa速度评价项融合合势场梯度最快下降方向生成无人车当前最优前进轨迹。
[0070]
合势场构建分为两部分，引力势场构建和斥力势场构建，具体步骤如下：
[0071]
s41、取局部动态地图与全局路径的交点作为当前局部目标点；
[0072]
s42、计算局部地图内每个栅格相对于局部目标点的引力势场，计算公式如下：
[0073][0074]
式中，k
att
为无人车在行驶时目标所引起的引力势场函数的增益因子，x为各栅格位置，x
goal
为局部目标点位置；
[0075]
s43、采集传感器在局部地图内探测的障碍物位置参数；
[0076]
s44、根据障碍物位置参数计算局部地图内每个栅格的斥力势场，计算公式如下：
[0077][0078]
式中，k
rep
为无人车在行驶过程中障碍物引起的斥力势场函数的增益因子，x
obs
障碍物位置。s是斥力势场影响范围的半径。与引力作用相反，障碍物与无人车的距离越大则斥力作用越小，两当者间的距离大于某个距离时时，障碍物就不会再对无人车发生作用；
[0079]
s45、计算局部地图内各个栅格的合势场：u＝αu
att
(x)+βu
rep
(x)。
[0080]
传统dwa方法的速度评价项结合合势场梯度最速下降方向形成新的速度评价项。最速梯度下降方向与速度夹角如图1所示。
[0081]
即如图1所示，只针对dwa速度评价项来讲无人车当前最优速度是当前速度大小和速度方向与势场梯度最速下降方向共同决定。
[0082][0083]
其中e
(v,w)
为改进dwa速度评价项，为合势场梯度最速下降方向。
[0084]
在局部路径规划中，基于dwa与人工势场算法融合的局部路径规划方法的流程框图如图2所示：无人车向目标点前进局部路径规划具体步骤如下：
[0085]
s1、采集无人车起始位置，目标点，规划无人车全局路径，设置无人车局部动态地图大小；
[0086]
s2、采集当前时刻无人车位置，传感器探测的障碍物位置参数；
[0087]
s3、根据局部地图和已经规划好的全局路径计算无人车当前时刻的局部目标点；
[0088]
s4、根据无人车当前时刻的位置，局部目标点和障碍物位置参数计算局部地图内的引力势场和斥力势场，并将引力势场和斥力势场乘以各自系数形成局部地图各个位置的合势场；
[0089]
s5、采集当前时刻的速度，根据无人车的加速度，计算当前时刻无人车所能达到速度的速度窗口，速度窗口更新频率为0.1秒，如图3所示；
[0090]
s6、将当前的速度窗口均分离散为20组角速度合线速度的速度组；
[0091]
s7、各速度组结合合势场梯度最速下降方向融合各速度组计算dwa速度评价项，并
进行归一化；
[0092]
s8、以每组速度持续前进3秒计算预测轨迹；
[0093]
s9、dwa目标距离评价项，计算每组速度预测轨迹末端与局部目标点的距离的倒数，并进行归一化；
[0094]
计算dwa障碍物距离评价项，首先计算各速度组预测轨迹是否和局部地图障碍物位置相交，若存在相交点则此速度组障碍物评价为0。如果障碍物与预测轨迹的最小距离小于无人车外接圆半径则此速度组障碍物评价为0。当障碍物与各速度组预测轨迹的最小距离大于一个安全范围时，则将这一项设置为一个常数。这一个安全范围选择无人车以最大的速度行驶，然后以最大加速度进行减速，无人车在减速到速度为0，即无人车停下来。这一段距离就是无人车的一个安全距离。另一方面，如果无人车在行驶过程中始终没有障碍物的话那么这一项也始终为一个常数。然后对此项进行归一化；
[0095]
s10、将3个评价项乘以各自的重要系数，得到总的评分，选择评分最高的速度组作为当前时刻的最优行驶速度；
[0096]
s11、如果到达最终目标点，则无人车停止运动，否则重复步骤s2-s10。
[0097]
除以上实例以外，本领域技术人员根据上述公开内容获得启示或利用相关领域的知识或技术进行改动获得其他实施例，各个实施例的特征可以互换或替换，本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。
[0098]
本发明功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，在一台计算机设备(可以是个人计算机，服务器，或者网络设备等)以及相应的软件中执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、或者光盘等各种可以存储程序代码的介质，进行测试或者实际的数据在程序实现中存在于只读存储器(random access memory，ram)、随机存取存储器(random access memory，ram)等。