一种基于人工势场法的无人驾驶车辆路径规划方法与流程

本发明涉及智能汽车技术领域，具体涉及一种基于人工势场法的无人驾驶车辆路径规划方法。

背景技术：

路径规划是无人驾驶车辆研究领域的一项核心技术，它是指无人驾驶车辆根据多种传感器探测出的行驶环境信息，规划出一条从起始点到目标点的无碰撞路线，实现路径的最优化。路径规划主要包含两个步骤：一是建立包含障碍区域与自由区域的环境地图，二是在环境地图中选择合适的路径搜索算法，快速实时地搜索可行路径。路径规划结果对车辆行驶起着导航作用。它引导车辆从当前位置行驶到达目标位置。目前在无人驾驶中运用的路径规划算法有很多，如遗传算法、粒子群算法、a*算法、rrt算法和人工势场算法等。其中，人工势场法具有计算量小、规划路径平滑和便于实时控制等优点而被广泛运用于机器人导航和避碰中。

人工势场法于1986年由khatib提出。其基本思想是，假设在汽车行驶道路中存在一种虚拟的势场力，目标点对主车产生引力，障碍物对主车产生斥力，主车在引力和斥力的合力控制作用下由高势场向低势场移动，最终到达目标位置。

但是，传统人工势场法仍然存在不足之处，当车辆在行进过程中有可能出现所受的斥力和引力大小相等方向相反，此时车辆在人工势场中所受到的合力为零，车辆陷入局部最优解，从而无法到达目标点。当目标点附近存在障碍物时，随着无人驾驶车辆不断地向目标点逼近，就可能出现障碍物对车辆的斥力远远大于目标点对车辆的引力，从而使得车辆在目标点附近徘徊，出现目标不可达的问题。

因此，传统人工势场法在复杂场景中存在易陷入局部最小点和目标不可达的问题，为解决这些问题，国内外学者对其进行了相应的改进研究：其一是通过在人工势场法的斥力场函数中加入汽车与目标点间距离的方法，使汽车只有在到达目标点时，斥力和引力才同时为零，改进后的人工势场法可以在静态环境中为车辆规划出安全的避障路径；其二是使用选取在障碍物一侧的中间目标点代替真正的目标点引导机器人，使机器人摆脱局部极小点；其三是通过分别引入rrt算法来弥补传统人工势场法容易形成局部最优解的不足；其四是分别通过引入斥力系数和调节因子以及道路边界斥力场模型，设立虚拟局部目标点，建立改进的路径规划模型从而有效地实现智能车辆的避撞局部路径；其五是通过优化吸引力场和排斥力场，并提出一种势场填充策略，使得移动机器人可以找到一条更好、无碰撞的目标路径。但是，上述改进仍然存在无法正常到达的问题。

技术实现要素：

本发明的发明目的是，针对上述问题，提供一种基于人工势场法的无人驾驶车辆路径规划方法，用以解决无人驾驶车辆存在易陷入局部最小点和目标不可达的问题。

为达到上述目的，本发明所采用的技术方案是：一种基于人工势场法的无人驾驶车辆路径规划方法，包括以下步骤：

1)、构建无人驾驶车行驶的二维空间模型，于所述二维空间模型内对无人驾驶车起始点、障碍物以及目标点的坐标进行定位，确定障碍物的个数n，确定无人驾驶车行驶的步长l；

2)、建立由所述障碍物对无人驾驶车产生的斥力场和所述目标点对车辆产生的引力场叠加而成的虚拟势场，所述虚拟势场对所述无人驾驶车辆产生的作用力引导着无人驾驶车辆朝目标行进；

3)使无人驾驶车行驶一个单位步长l，判断无人驾驶车是否陷入局部最小值点，如果是则调用步骤4)，否则进行步骤5)；

4)改变斥力在x轴上的分量后回到步骤2)重新开始；

5)判断无人驾驶车是否行进到目标点附近的影响距离内造成目标不可达，如果是则调用步骤6)，否则进行步骤7)；

6)在斥力势场函数中引入安全距离ρs和无人驾驶车到目标点之间的距离ρt后回到步骤(2)重新开始；

7)判断无人驾驶车车是否到达目标点，如果是则停止路径规划画出路径，否则回到步骤(2)重新开始。

作为一种改进的方式，步骤4)中，所述斥力在x轴上添加的变量系数为k，所述斥力在x轴上的分量为：

，，frep(x)为原所受斥力的合力大小,，ρ(x，xo)为被控对象到障碍物之间的距离，ρo为障碍物的最大影响距离；

所述斥力在y轴上添加的变量系数为δ，所述斥力场在y轴上的分量为：

作为一种改进的方式，步骤6)中，

所述斥力势场函数为：

其中，x＝(x,y)为无人驾驶车辆的当前位置坐标，xo＝(xo，yo)为障碍物坐标，ρ(x，xo)为被控对象到障碍物之间的距离，ρo为障碍物的最大影响距离，ρs为安全距离，ρt为无人驾驶车到目标点之间的距离，γ为安全距离所对应的斥力增益系数，β为未考虑添加安全距离时斥力增益系数。

其中，frep1和frep3的方向从障碍物指向无人驾驶车辆，frep2和frep4的方向从无人驾驶车辆指向目标点,

当ρ(x，xo)≤ρs时，

当ρ(x，xo)≤ρo且ρ(x，xo)＞ρs时，

作为一种改进的方式，步骤2)中，所述引力场函数为；

其中，α为引力增益系数，所述引力函数为：

作为一种改进的方式，所述二维空间模型为包含障碍区域与自由区域的环境地图。

作为一种改进的方式，目标点坐标为xt＝(xt，yt)，被控对象到目标点之间的距离ρt为

由于采用上述技术方案，本发明具有以下有益效果：

1、本申请主要针对传统人工势场法存在的不足进行改进，当无人驾驶车辆在行驶过程中陷入局部最小值点时，通过在斥力函数的分量上添加变量系数改变斥力的方向，从而改变合力方向，使得无人驾驶车跳出局部最小值点，继续向目标点行进。

2、当无人驾驶车行驶到目标点附近障碍物的影响距离范围内造成目标不可达问题时，通过加入无人驾驶车和目标点之间距离来提供新的斥力函数，并且添加安全距离来保证无人驾驶车行驶过程中的安全性。

3、本申请将目标点与车辆之间的距离因子引入到斥力函数中，并且在障碍物的影响范围内添加了安全距离，使得在车辆行驶到障碍物的安全距离内时所受到的斥力大于未添加安全距离之前的斥力，从而保障了车辆行驶过程中的安全性。

由于步骤4)中，所述斥力在x轴上添加的变量系数为k，所述斥力在x轴上的分量为：

所述斥力在y轴上添加的变量系数为δ，所述斥力场在y轴上的分量为：

，当合力为零时容易出现局部极小值点，车辆将会来回运动，因此对斥力在x轴和y轴上的分量分别进行改进，使得合力大于零以避免这些情况的出现，这样车辆才可以顺利到达目标位置。

由于步骤6)中，所述斥力势场函数为：

其中，x＝(x,y)为无人驾驶车辆的当前位置坐标，xo＝(xo，yo)为障碍物坐标，ρ(x，xo)为被控对象到障碍物之间的距离，ρo为障碍物的最大影响距离，ρs为安全距离，ρt为无人驾驶车到目标点之间的距离，γ为安全距离所对应的斥力增益系数，β为未考虑添加安全距离时斥力增益系数，当车辆行驶到目标点附近的障碍物影响范围内时，可能造成斥力大于引力，使得车辆在目标点附近徘徊，无法停止运动。因此，将目标点与车辆之间的距离因子引入到斥力函数中，并且在障碍物的影响范围内添加了安全距离，使得在车辆行驶到障碍物的安全距离内时所受到的斥力大于未添加安全距离之前的斥力，从而保障了车辆行驶过程中的安全性。

附图说明

图1是改进前斥力势场强度立体图；

图2是改进后斥力势场强度立体图；

图3是改进前斥力势场强度侧面图；

图4是改进后斥力势场强度侧面图；

图5是改进前总势场强度平面图；

图6是改进后总势场强度平面图；

图7是改进前总势场强度侧面图；

图8是改进后总势场强度侧面图；

图9是车辆陷入局部最小值点状态图；

图10是车辆处于目标不可达状态图；

图11是车辆跳出局部最小值点路径图；

图12是车辆目标可达的路径图；

图13是传统方法与改进方法的目标可达路径对比图。

具体实施方式

本发明公开了一种基于人工势场法的无人驾驶车辆路径规划方法，包括以下步骤：

1)、构建无人驾驶车行驶的二维空间模型，二维空间模型为包含障碍区域与自由区域的环境地图，确定障碍物的个数n，于二维空间模型内对无人驾驶车起始点、障碍物以及目标点的坐标进行定位，目标点坐标为xt＝(xt，yt)，被控对象(即无人驾驶车)到目标点之间的距离ρt为：确定无人驾驶车行驶的步长l。

2)、建立由障碍物对无人驾驶车产生的斥力场和目标点对车辆产生的引力场叠加而成的虚拟势场，其中，引力场函数为；α为引力增益系数，引力函数为：虚拟势场对无人驾驶车辆产生的作用力引导着无人驾驶车辆朝目标行进。

3)无人驾驶车行驶一个单位步长l，然后判断是否陷入局部最小值点，如果是则调用步骤4)，否则进行步骤5)。

4)改变斥力在x轴上的分量后回到步骤2)重新开始，具体改变公式为：

斥力在x轴上添加的变量系数为k，斥力在x轴上的分量为：

，，frep(x)为原所受斥力的合力大小,，ρ(x，xo)为被控对象到障碍物之间的距离，ρo为障碍物的最大影响距离；

斥力在y轴上添加的变量系数为δ，斥力场在y轴上的分量为：

，当合力为零时容易出现局部极小值点，车辆将会来回运动，因此使斥力在x轴和y轴上的分量进行改进，使得合力大于零以避免这些情况的出现，这样车辆才可以顺利到达目标位置。

5)判断无人驾驶车是否行进到目标点附近的影响距离内造成目标不可达，如果是则调用步骤6)，否则进行步骤7)；

6)在斥力势场函数中引入安全距离ρs和无人驾驶车到目标点之间的距离ρt后得到斥力势场函数为：

其中，x＝(x,y)为无人驾驶车辆的当前位置坐标，xo＝(xo，yo)为障碍物坐标，ρ(x，xo)为被控对象到障碍物之间的距离，ρo为障碍物的最大影响距离，ρs为安全距离，ρt为无人驾驶车到目标点之间的距离，γ为安全距离所对应的斥力增益系数，β为未考虑添加安全距离时斥力增益系数，回到步骤(2)重新开始(当车辆行驶到目标点附近的障碍物影响范围内时，可能造成斥力大于引力，使得车辆在目标点附近徘徊，无法停止运动。因此，将目标点与车辆之间的距离因子引入到斥力函数中，并且在障碍物的影响范围内添加了安全距离，使得在车辆行驶到障碍物的安全距离内时所受到的斥力大于未添加安全距离之前的斥力，从而保障了车辆行驶过程中的安全性)。

7)判断无人驾驶车车是否到达目标点，如果是则停止路径规划画出路径，否则回到步骤(2)重新开始。

其中，上述步骤6)中：斥力函数为：

其中,frep1和frep3的方向从障碍物指向无人驾驶车辆，frep2和frep4的方向从无人驾驶车辆指向目标点,

当ρ(x，xo)≤ρs时，

当ρ(x，xo)≤ρo且ρ(x，xo)＞ρs时，

图1和图2分别为加入安全距离前后的斥力势能强度立体图，图3和图4为加入安全距离前后的斥力势能强度侧面图，由图可知无人驾驶车的行驶空间为侧面图的横轴，障碍物的位置坐标分别为(1.5，1)、(5，6)、(9，5,5)，目标点位置坐标为(10,10)，纵轴代表斥力势能值。能够看出改进后的斥力势能值在障碍物附近有所上升，从而提高了车辆行驶的安全性。

图5和图6分别为加入安全距离前后总势能的平面图，此时横轴为y轴，同样能够看出改进后的斥力势能值在障碍物附近有所上升。

图7和图8分别为增加安全距离前后总势能的侧面图，能够看出改进前后目标点的势能为零，障碍物附近的势能出现突变，当无人驾驶车到达目标点附近时，势能值减小，从而也避免出现无人驾驶车辆到达目标点势能不为零的情况。

为了验证本申请的基于人工势场法的无人驾驶车辆路径规划方法的效果，因此设计实验对其进行仿真分析，改进人工势场算法实验步骤如下：

(1)构建无人驾驶车的运行空间，确定无人驾驶车的起始点和目标点的位置、引力和斥力的增益系数α和β、障碍物的个数n、障碍物影响距离ρo、安全距离ρs以及无人驾驶车行驶的步长l。

(2)建立虚拟势场并分别计算引力和斥力的大小。

(3)计算合力大小。

(4)无人驾驶车向下一位置运动后判断是否陷入局部最小值点，如果是则调用步骤(5)，否则进行步骤(6)。

(5)改变斥力在x轴上的分量后回到步骤(2)重新开始。

(6)无人驾驶车是否行进到目标点附近的影响距离内，造成目标不可达，如果是则调用步骤(7)，否则进行步骤(8)。

(7)在斥力函数中加入安全距离和无人驾驶车到目标点之间的距离后回到步骤(2)重新开始。

(8)无人驾驶车是否到达目标点，如果是则停止路径规划画出路径，否则回到步骤(2)重新开始。

根据以上的实验步骤在matlab仿真平台上分别对改进前后的人工势场法进行仿真实验。选取引力增益系数为15，斥力增益系数为4，障碍物影响距离为2.5，安全距离为1，无人驾驶车的行驶步长为0.2，最大迭代次数为600，无人驾驶车的起始点位置坐标为(0,0)，目标点位置坐标为(10,10)，γ根据多次重复实验选取最优值。

首先，只运用传统人工势场法进行仿真。当无人驾驶车、障碍物以及目标点在同一条直线上时，无人驾驶车在靠近目标点的同时所受的引力在逐渐减小，而无人驾驶车受到的斥力逐渐增大，无人驾驶车在某一点处会由于受力平衡，无法躲避障碍物，陷入局部最小值点，无法到达目标点，如图9所示。当目标点存在障碍物且无人驾驶车行驶到障碍物的影响距离内时，无人驾驶车的斥力大于引力，从而使无人驾驶车不能到达目标点，如图10所示。

运用改进后的人工势场法进行仿真得到的结果如图11和12所示。图11是在图9中无人驾驶车陷入局部最小值点的情况下对斥力在x轴和y轴上分量的改进，使得无人驾驶车跳出局部最小值点继续向目标点行进。图12是图10在无人驾驶车出现目标不可达的情况下对无人驾驶车的斥力添加调节因子和安全距离的改进，使得无人驾驶车在到达目标点时受力平衡。

从仿真结果可以看出改进后的算法能够很好地解决小车陷入局部最小值点和目标不可达的情况并引导着小车行进到目标点，并通过图13中传统加距离因子改进方法与本文改进距离因子及增加安全距离的方法所形成的目标可达路径进行对比，从而验证了改进人工势场法的有效性。

本申请介绍了传统人工势场法的基本原理，分析了传统人工势场法在路径规划中陷入局部极小点和目标不可达情况的原因，通过引入安全距离和调节因子来解决目标不可达问题的同时提高无人驾驶的安全性能，通过在斥力分量上增加可变系数的方式使无人驾驶车辆跳出局部最小值点。最后在matlab仿真环境下验证了改进算法的有效性。