具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本实施例提供了一种基于斥力场模型的特种车辆避让方法，包括，

以当前车辆的行驶方向构建行驶坐标系，获取障碍物的边界坐标，计算避障安全距离，构建障碍边界的斥力函数；

以当前车辆为基点，以特种车辆的行驶方向构建特种坐标系，获取特种车辆的边界坐标，构建特种车辆边界斥力函数；

在需要避让特种车辆时，计算障碍物边界斥力函数和特种车辆边界斥力函数在行驶坐标系的两个坐标轴上的合力；

基于车辆行驶方向的合力控制当前车辆的速度，基于垂直行驶方向的横向合力控制当前车辆的转向角度。

本实施例分别以当前车辆位置为基点构建两个坐标系以分析当前车辆周边的障碍物以及特种车辆的斥力场函数，基于斥力的合力做出避让特种车辆的决策，在及时避让特种车辆的同时确保能够安全的躲避障碍物，确保安全、高效的交通环境。

具体的，本实施例提供的基于斥力场模型的特种车辆避让方法包括以下步骤：

S1：以当前车辆的行驶方向构建行驶坐标系，获取障碍物的边界坐标，计算避障安全距离，构建障碍边界的斥力函数；

结合图2，所述构建行驶坐标系的方法为，

以当前车辆的几何中心为O点，以行驶方向为Y轴，以平行于车轴方向为X轴，构建XOY平面坐标系作为行驶坐标系。

所述计算避障安全距离的方法包括

计算障碍边界P_i(x_i，y_i)到当前车辆的距离：

其中，B为当前车辆的宽度，L为当前车辆的长度，满足下式，

引入当前车辆与障碍物的相对速度v，

v＝v₀-v_i

其中，v₀为当前车辆的速度，v_i为障碍物的速度。

进行避障判断时所需考虑的情况如下：

当障碍物位于当前车辆Y方向的前方且v大于零时，v越大，所需的避障安全距离就越大，

当障碍物位于当前车辆Y方向的前方且v小于零时，障碍物与当前车辆不会发生碰撞，不需要考虑避障安全距离；

当障碍物位于当前车辆Y方向的后方且v大于零时，v障碍物与当前车辆不会发生碰撞，不需要考虑避障安全距离；

当障碍物位于当前车辆Y方向的后方且v小于零时，v越大，所需的避障安全距离就越大。

基于以上分析，本实施例提供的避障安全距离计算公式为，

其中，k₁>0为安全系数，为经验值。

所述障碍边界的斥力函数构建方法为，

当ρ₁＝D₁时开始避障，则安全避障的斥力函数的最大范围为D₁，此时斥力场函数为，

其中，k_p为斥力系数；

当时，斥力场仅在X轴方向上有作用，其他情况下仅在Y轴方向有作用，因此将斥力场函数改进为，

记为

S2：以当前车辆为基点，以特种车辆的行驶方向构建特种坐标系，获取特种车辆的边界坐标，构建特种车辆边界斥力函数；

参考图3，所述特种坐标系的构建方法为：

以当前车辆的几何中心为坐标原点O，以特种车辆的行进方向为Y′轴，以垂直Y′轴的方向为X′轴。

构建特种车辆边界q_i点斥力函数的方法为，

计算q_i点到坐标原点O的距离，

在行驶坐标系XOY中，当前车辆的四个顶点坐标分别为，

特种坐标系X′OY′由行驶坐标系XOY绕原点旋转角度θ_i得到，其中|θ_i|<π，θ_i>0表示沿顺时针方向旋转，θ_i<0表示沿逆时针方向旋转；

行驶坐标系XOY到特种坐标系X′OY′的坐标转换公式为，

将当前车辆顶点的坐标A、B、C、D转换到坐标系X′OY′中，即A(x_A′,y_A′)、B(x_B′,y_B′)、C(x_C′,y_C′)、D(x_D′,y_D′)；

当前车辆边界点到Y′轴的最大距离为，

d₀＝max{|y_A′|,|y_B′|,|y_C′|,|y_D′|}

特征车辆到Y′轴的最小距离为，

d_min＝min{|y′_i|}

则当前车辆需要对特种车辆做出避让的临界条件为，

d_min<d₀

为保证安全裕量，将临界条件修正为，

d_min<γd₀

其中，γ>1为修正系数；

仿照S1中D₁的确定方法，确定安全避障距离D₂，

其中，k₂>0为安全系数，为经验值。

则特种车辆边界斥力模型为，

其中，k_q为斥力系数；

仅考虑特种车辆边界点q_i在X′轴方向上的斥力，则斥力函数为，

S3：在需要避让特种车辆时，计算障碍物边界斥力函数和特种车辆边界斥力函数在行驶坐标系的两个坐标轴上的合力；

首先将特种车辆边界的斥力函数F_q(i)分别分解在X轴和Y轴上，

F_q(i)＝F_qx(i)+F_qy(i)

其中，

F_qx(i)＝F_q(i)cosθ_i

F_qy(i)＝F_q(i)sinθ_i

则障碍物边界斥力函数和特种车辆边界斥力函数在坐标轴上的合力分别为，

S4：基于车辆行驶方向的合力控制当前车辆的速度，基于垂直行驶方向的横向合力控制当前车辆的转向角度；

具体的，控制转向角度α的方法为，

α＝k₃|F_x|

其中，k₃>0为常数，

当F_x在X轴的正半轴时，转向方向为X轴正方形，当F_x在X轴的负半轴时，转向方向为X轴负方向；

所述控制当前车辆速度的方法为，

当合力F_y与避让前的速度v₀方向相同时，避让后的车速v′₀为，

v′₀＝v₀+k₄|F_y|t

其中，k₄>0为常数，t为时间；

当合力F_y与避让前的速度v₀方向相反时，避让后的车速v′₀为，

v′₀＝v₀+k₅|F_y|t

其中，k₅<0为常数。

图4和图5显示了车辆避让特种车辆的过程，为了与行车轨迹相适应，图4和图5的读图顺序为从下向上。

进一步的，参考图6，本实施例还提供了一种基于斥力场模型的特种车辆避让系统，包括，

障碍物斥力函数计算模块，以当前车辆的行驶方向构建行驶坐标系，获取障碍物的边界坐标，计算避障安全距离，构建障碍边界的斥力函数；

特种车辆斥力函数计算模块，以当前车辆为基点，以特种车辆的行驶方向构建特种坐标系，获取特种车辆的边界坐标，构建特种车辆边界斥力函数；

合力计算模块，在需要避让特种车辆时，计算障碍物边界斥力函数和特种车辆边界斥力函数在行驶坐标系的两个坐标轴上的合力；

避让决策模块，基于车辆行驶方向的合力控制当前车辆的速度，基于垂直行驶方向的横向合力控制当前车辆的转向角度。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。