CN113848914A - 动态环境下碰撞系数人工势场法局部路径规划方法 - Google Patents

Abstract

本发明提出一种动态环境下碰撞系数人工势场法局部路径规划方法，针对动态障碍物，设计了根据碰撞角判断障碍物类型的避障策略，建立速度势场函数模型，根据车辆自身速度、制动减速度与障碍物的相对速度建立动态速度势场影响范围，计算动态障碍物与速度势场影响范围相交面积得到碰撞系数，进而建立不同的斥力函数模型，使智能车行驶更加安全，并且在动态规划中加入车辆动力学约束，使规划出来的轨迹更加符合车辆特性。并在MATLAB中对无人驾驶在有道路边界和动态障碍物的环境下行驶进行仿真，仿真结果验证了速度势场函数人工势场法的有效性。

Description

动态环境下碰撞系数人工势场法局部路径规划方法

技术领域

本发明属于智能驾驶路径规划和自主导航,无人驾驶汽车和移动机器人避障的局部路径规划技术领域，尤其涉及一种动态环境下碰撞系数人工势场法局部路径规划方法。

背景技术

智能汽车又被称为自动驾驶汽车，利用车载传感器感知环境并根据相应的指令自主控制车辆。为了降低交通事故的发生率，近几年随着计算机技术的发展,汽车行业逐渐朝着与电子技术、网络通讯相结合智能化方向发展,在智能技术背景下，我国交通事故死亡人数近几年呈现下降趋势，智能化作为其中的关键，使汽车的传统价值核心开始发生转变，自动驾驶汽车将会是未来的必经之路。智能汽车是汽车产业与新一轮技术革命相互融合的产物，方便社会的同时也是也为人们提供多功能的服务，它有利于改善交通道路问题，并推动汽车制造、网络通讯、服务等不同行业的发展。

路径规划是无人驾驶车辆信息感知和智能控制的桥梁，是实现自主驾驶的基础。其任务是根据一定的路径规划算法在有障碍物的环境内按照一定的评价标准，搜索一条最优路径。路径规划算法是在无人驾驶和人工智能领域兴起的一类新型优化算法，该算法迅速成为相关领域的研究热点。路径规划是保证智能车辆在不碰撞障碍物的情况下，获取一条从起点到终点的最优路线。当前，全局路径规划和局部路径规划是路径规划研究中的重点。其中，全局路径规划是在静态环境中搜索一条满足静态约束的最佳路径；局部路径规划是在未知环境下保证智能车辆可以实现实时避障，并保持局部最优。

发明内容

本发明的目的是提供一种动态环境下碰撞系数人工势场法局部路径规划方法，针对动态障碍物，设计了根据碰撞角判断障碍物类型的避障策略，建立速度势场函数模型，根据车辆自身速度、制动减速度与障碍物的相对速度建立动态速度势场影响范围，计算动态障碍物与速度势场影响范围相交面积得到碰撞系数，进而建立不同的斥力函数模型，使智能车行驶更加安全，并且在动态规划中加入车辆动力学约束，使规划出来的轨迹更加符合车辆特性。并在MATLAB中对无人驾驶在有道路边界和动态障碍物的环境下行驶进行仿真，仿真结果验证了速度势场函数人工势场法的有效性。

本发明所进行的工作包括：第一，针对动态障碍物，设计了根据碰撞角判断障碍物类型的避障策略，建立速度势场函数模型，根据车辆自身速度、制动减速度与障碍物的相对速度建立动态速度势场影响范围。第二，设计计算动态障碍物与速度势场影响范围相交面积得到碰撞系数，进而建立不同的斥力函数模型，使智能车行驶更加安全。

本发明具体采用以下技术方案：

一种动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于，包括以下步骤：

步骤S1：读取无人车当前位姿与目标点位置，并获取动静态障碍物位置信息；

步骤S2：加入动态障碍物并建立空间碰撞角速度势场：考虑智能车速度方向和障碍物速度方向，计算两者的相对速度与相对距离方向的角度值生成空间碰撞角，根据碰撞角的类型建立相应的速度势场函数；

步骤S3：计算空间碰撞角速度势场影响范围；

步骤S4：计算动态环境下碰撞系数；根据进入速度势场影响范围的面积大小决定斥力大小；

步骤S5：在动态规划中加入车辆动力学约束；

步骤S6：判断是否到达目标点，如果未到达则返回步骤S2。

进一步地，在步骤S2中，以V_C表示智能车速度方向，V_O表示障碍物当前的速度方向，V_CO代表智能车相对于障碍物的速度，β表示智能车与障碍物两者相对位置与相对速度方向夹角，定义为空间碰撞角；

建立的速度势场的斥力函数为：

其中，k_repv为速度影响系数，e_co表示相对速度在相对距离方向上的单位向量，速度方向作用力为势场函数的负梯度，表达式为：

速度势场产生斥力的方向为障碍物指向智能车，智能车所处环境的斥力势能表达式为：

势场函数由速度势场产生的势能U_repv(V)与距离势场产生的势能U_rep(X)两者叠加产生，所产生的斥力表达式为势场的负梯度，同样由速度势场产生的斥力F_repv(V)与距离势场产生斥力F_rep(X)两者叠加组合，智能车所受斥力表达式为：

式中

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围。

进一步地，在步骤S3中，以速度势场影响范围阈值为依据，当动态障碍物进入速度势场影响范围则进行制动或改变速度；所述速度势场影响范围阈值的表达式为：

式中，d₀为智能车和其他障碍物均静止时两者的安全距离，V_co为智能车与动态障碍物的相对速度，相对速度越大，两者相撞可能时间越短，速度势场影响范围越大以便提前预警并做出响应。t′为智能车在接收到制动信号并操作制动器的时间，一般为0.3～1.0秒，t″为制动蹄与制动鼓之间存在间隙需要反应的时间，一般为0.2～0.9秒，在这段时间内，智能车制动器还未开始工作，预留这距离防止两者相撞。V_c为智能车当前的速度，a_cmax是智能车的最大制动减速度，与智能车自身最大制动力和质量相关。

进一步地，设ρ_s为智能车根据与动态障碍物相对速度大小与自身运动学特性计算的速度势场的影响范围，当障碍物进入智能车速度势场范围内，斥力快速增大，S_o代表障碍物和速度势场影响范围的重叠部分面积，S_o越大表示智能车与动态障碍物距离越近，其产生的斥力越大，其速度势场产生的斥力函数表达式为：

其中，m为碰撞系数，表示危险的程度，根据S_o占速度势场影响范围面积的比例决定取值为[1,5]的实数，m随着S_o所占面积比例增大而增大，设障碍物的面积为S_ob，m取值为式：

进一步地，在步骤S5中考虑最大转角，设L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角；智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式为：

最大转角即最大内转角表达式为：

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式：

α-α₂≤α′≤α+α₂

动态环境中人工势场法寻优角度值应被限制在智能车最大转角范围内。

进一步地，在步骤S6中，将产生的轨迹进行可视化。

相较于现有技术，本发明及其优选方案针对动态障碍物，设计了根据碰撞角判断障碍物类型的避障策略，建立速度势场函数模型，根据车辆自身速度、制动减速度与障碍物的相对速度建立动态速度势场影响范围，计算动态障碍物与速度势场影响范围相交面积得到碰撞系数，进而建立不同的斥力函数模型，使智能车行驶更加安全，并且在动态规划中加入车辆动力学约束，使规划出来的轨迹更加符合车辆特性。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

图1为本发明实施例动态障碍物与智能车相对速度分析示意图。

图2为本发明实施例速度势场影响范围示意图。

图3为本发明实施例碰撞系数计算示意图。

图4为本发明实施例智能车简易转弯模型示意图。

图5为本发明实施例算法在静态环境下的路径示意图。

图6为本发明实施例算法在动态环境下的路径示意图。

图7为本发明实施例动态环境前20个路径点示意图。

图8为本发明实施例相向运动路径示意图。

图9为本发明实施例相对运动路径示意图。

图10为本发明实施例程序流程示意图。

具体实施方式

为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1-图10所示，本实施例提供的动态环境下碰撞系数人工势场法局部路径规划方法包括以下实施过程：

步骤一：读取无人车当前位姿与目标点位置，并获取动静态障碍物位置信息；

将无人车车载传感器如相机、激光雷达、GPS、惯性测量单元等得到的数据信息转化为坐标信息，输入车机系统，初始化人工势场法各个参数。

步骤二：加入动态障碍物并建立空间碰撞角速度势场；

动态环境中只考虑障碍物与智能车之间的距离是不合理的，也要考虑速度对智能车的影响。本实施例考虑智能车速度方向和障碍物速度方向，计算两者的相对速度与相对距离方向的角度值生成空间碰撞角，根据碰撞角的类型建立相应的速度势场函数。如图1所示为运动环境中智能车与动态障碍物车的相对速度分析。图中，V_C表示智能车速度方向，V_O是障碍物当前的速度方向，V_CO代表智能车相对于障碍物的速度，β表示智能车与障碍物两者相对位置(方向为障碍物指向智能车)与相对速度方向夹角，定义为空间碰撞角。将障碍物与智能车在斥力影响范围才产生斥力的距离势场与空间碰撞角β产生的速度势场两者结合考虑分为以下四种情况(以逆时针为正方向)：

(1)

动态障碍物朝着远离智能车方向运动，且不在斥力影响范围内，因此动态障碍物对智能车速度势场与距离势场都对智能车没有影响，不产生斥力。

(2)

动态障碍物仍是远离智能车方向运动，速度势场对智能车无影响，但障碍物在斥力影响范围内，因此动态障碍物可以简化为静态障碍物，只存在距离势场，速度势场无影响。

(3)

表明动态障碍物朝着智能车方向运动，但还没有进入障碍物斥力影响范围，在空间上与智能车存在碰撞可能，此时速度势场对智能车产生作用力，距离势场不起作用。

(4)

动态障碍物朝着智能车方向运动且在斥力影响范围内，表明速度势场和距离势场两者同时产生叠加，碰撞角的余弦值越大，表明动态障碍物与智能车距离越近，对智能车的威胁程度越高，速度势场越高，产生的斥力也就越大，基于上述碰撞角分析，建立速度势场的斥力函数为：

上述斥力势场函数中k_repv为速度影响系数，可以被设置为任意正实数，e_co表示相对速度在相对距离方向上的单位向量，速度方向作用力为势场函数的负梯度，表达式为：

速度势场产生斥力的方向为障碍物指向智能车，结合距离势场，智能车所处环境的斥力势能表达式为：

势场函数由速度势场产生的势能U_repv(V)与距离势场产生的势能U_rep(X)两者叠加产生，所产生的斥力表达式为势场的负梯度，同样由速度势场产生的斥力F_repv(V)与距离势场产生斥力F_rep(X)两者叠加组合，智能车所受斥力表达式为：

步骤三：计算空间碰撞角速度势场影响范围；

针对动态障碍物，在距离势场上其斥力影响范围是固定的，智能车只有在进入这个范围才会产生斥力。针对不同相对速度情况下，应采用不同的影响范围，在动态障碍物与智能车相对速度比较大时，且空间碰撞角β∈(90,90)、或βπ＝±π时，表明动态障碍物在快速靠近智能车，此时速度势场影响范围应该越大，有利于智能车采取制动减速或者是转方向盘改变相对速度方向，预留足够的操作时间，减少时间碰撞危险度。当智能车在速度势场的影响范围外且碰撞角β∈(90,90)、或βπ＝±π时，则表明动态障碍物对智能车的时间碰撞危险度比较小，对智能车有影响但智能车还存在足够的时间进行制动或者改变自身速度方向，此时可以再观察动态障碍物是否会改变速度方向大小和方向，若动态障碍物进入速度势场影响范围再进行制动或改变自身速度，因此速度势场的斥力函数影响范围应该存在一个阈值ρs，如图2所示，动态障碍物在阈值范围内外对智能车的碰撞危险程度不一，斥力应不同，并且阈值是根据障碍物和智能车的相对速度变化的，相对速度越大，两者相撞可能时间越短，速度势场影响范围越大以便提前预警并做出响应。

根据两者的相对速度结合智能车自身的制动特性设置动态的速度势场影响范围，保证智能车有足够的时间对障碍物做出相应的响应避开障碍物。根据智能车运动学特性，速度势场影响范围阈值确定表达式为：

步骤四：计算动态环境下碰撞系数；

智能车在路径规划过程中，在车载传感器扫描的范围内且不在速度势场影响范围内的动态障碍物，对智能车存在斥力但因与智能车距离远，智能车还有足够的时间进行减速制动或者转向，对智能车产生斥力应该较小。而进入速度势场影响范围内的动态障碍物对智能车威胁程度较大，因此面对动态障碍物，应根据进入速度势场影响范围的面积大小决定斥力大小，如图3所示，图中，ρ_s为智能车根据与动态障碍物相对速度大小与自身运动学特性计算的速度势场的影响范围，当障碍物进入智能车速度势场范围内，斥力应快速增大，V_co代表的智能车相对动态障碍物的速度，β表示为碰撞角，S_o代表的是障碍物和速度势场影响范围的重叠部分面积，S_o越大表示智能车与动态障碍物距离越近，其产生的斥力越大，其速度势场产生的斥力函数表达式为：

其中，m为碰撞系数，表示危险的程度，根据S_o占速度势场影响范围面积的比例决定取值为[1,5]的实数，m随着S_o所占面积比例增大而增大，设障碍物的面积为S_ob，m取值为式：

步骤五：在动态规划中加入车辆动力学约束；

现有的路径规划算法，通常把智能车看成一个可运动的质点，在实际行驶中，智能车在转弯、制动等运动情况下，为了不发生侧滑等不安全动作，规划路径应该满足智能车的运动特性。根据车辆转向的特点，转向过程中的最大转角有限制的。如图4所示。

图中L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角。智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式为：

最大转角即最大内转角表达式为

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式：

α-α₂≤α′≤α+α₂

其中以逆时针为正方向，智能车在转弯过程有最大转角限制，故在动态环境中人工势场法寻优角度值应被限制在智能车最大转角范围内，从而规划出来的路径符合智能车运动学模型，为下一步轨迹追踪奠定基础。

步骤六：判断是否到达目标点，并将产生的轨迹进行可视化。图5到图9为采用Matlab进行仿真模拟的示意图，以证明本实施例以上提供方案的有效性。

本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的动态环境下碰撞系数人工势场法局部路径规划方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。

Claims (6)

1.一种动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于，包括以下步骤：

步骤S1：读取无人车当前位姿与目标点位置，并获取动静态障碍物位置信息；

步骤S2：加入动态障碍物并建立空间碰撞角速度势场：考虑智能车速度方向和障碍物速度方向，计算两者的相对速度与相对距离方向的角度值生成空间碰撞角，根据碰撞角的类型建立相应的速度势场函数；

步骤S3：计算空间碰撞角速度势场影响范围；

步骤S4：计算动态环境下碰撞系数；根据进入速度势场影响范围的面积大小决定斥力大小；

步骤S5：在动态规划中加入车辆动力学约束；

步骤S6：判断是否到达目标点，如果未到达则返回步骤S2。

2.根据权利要求1所述的动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于：在步骤S2中，以V_C表示智能车速度方向，V_O表示障碍物当前的速度方向，V_CO代表智能车相对于障碍物的速度，β表示智能车与障碍物两者相对位置与相对速度方向夹角，定义为空间碰撞角；

建立的速度势场的斥力函数为：

其中，k_repv为速度影响系数，e_co表示相对速度在相对距离方向上的单位向量，速度方向作用力为势场函数的负梯度，表达式为：

速度势场产生斥力的方向为障碍物指向智能车，智能车所处环境的斥力势能表达式为：

势场函数由速度势场产生的势能U_repv(V)与距离势场产生的势能U_rep(X)两者叠加产生，所产生的斥力表达式为势场的负梯度，同样由速度势场产生的斥力F_repv(V)与距离势场产生斥力F_rep(X)两者叠加组合，智能车所受斥力表达式为：

式中

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围。

3.根据权利要求2所述的动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于：

在步骤S3中，以速度势场影响范围阈值为依据，当动态障碍物进入速度势场影响范围则进行制动或改变速度；所述速度势场影响范围阈值的表达式为：

式中，d₀为智能车和其他障碍物均静止时两者的安全距离，V_co为智能车与动态障碍物的相对速度；t′为智能车在接收到制动信号并操作制动器的时间，t″为制动蹄与制动鼓之间存在间隙需要反应的时间；V_c为智能车当前的速度，a_cmax是智能车的最大制动减速度，与智能车自身最大制动力和质量相关。

4.根据权利要求3所述的动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于：

设ρ_s为智能车根据与动态障碍物相对速度大小与自身运动学特性计算的速度势场的影响范围，当障碍物进入智能车速度势场范围内，斥力快速增大，S_o代表障碍物和速度势场影响范围的重叠部分面积，S_o越大表示智能车与动态障碍物距离越近，其产生的斥力越大，其速度势场产生的斥力函数表达式为：

其中，m为碰撞系数，表示危险的程度，根据S_o占速度势场影响范围面积的比例决定取值为[1,5]的实数，m随着S_o所占面积比例增大而增大，设障碍物的面积为S_ob，m取值为式：

5.根据权利要求4所述的动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于：

在步骤S5中考虑最大转角，设L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角；智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式为：

最大转角即最大内转角表达式为：

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式：

α-α₂≤α′≤α+α₂

动态环境中人工势场法寻优角度值应被限制在智能车最大转角范围内。

6.根据权利要求1所述的动态环境下碰撞系数人工势场法局部路径规划方法，其特征在于：在步骤S6中，将产生的轨迹进行可视化。

Images