CN112596516A - 基于Dubins曲线的多车队形切换方法 - Google Patents

Abstract

本发明涉及无人平台智能技术领域，公开一种基于Dubins曲线的多车队形切换方法。本发明提供了一种无人地面平台队形变换的方法；当车队接收到队形变换指令时，跟随车辆会根据自身状态及目标位置进行相应的轨迹规划；在得到初始的规划之后，根据得到的耗费时间，计算经过耗费时间后的新队形目标点；以此目标点为新的目标点进行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹；得到最优轨迹后再进行轨迹跟踪，当达到一定条件时进入队形保持阶段，最终完成变换过程。本发明以Dubsins曲线为基础进行迭代优化，每次最优计算后如果不满足条件会循环重新计算，最终得到最优轨迹，相比于此前的方案，其结果更加合理。

Description

基于Dubins曲线的多车队形切换方法

技术领域

本发明属于智能无人车领域，具体涉及一种基于Dubins曲线的多车队形切换方法。

背景技术

自人工智能交通被研究开始，随着智能汽车技术的发展，多智能车编队协同驾驶已逐渐成为近年来的热点之一。当前研究主要集中于多车编队控制研究，而对于队形变换的策略考虑较少，因此为适应未来交通的发展，有必要对队形变换的策略进行深一步的研究。

CN110262493A提出一种基于前车预瞄点及位置与速度等信息的队形变换方法，在收到变换的指令后根据队形形状进行计算几何位置，再根据几何位置预设参数，计算自车的目标速度和转角，进行变换，并且向其它车辆发送自车行驶状态，从而使下一车辆根据队形等信息进行队形变换，从而实现了队形变换。然而，有一些情况并未考虑：一方面，其并未考虑在变换过程中第一辆车即队形参考车辆的位置是变化的情况，可能导致跟随车辆按照此前目标点计算得到的预瞄点进行轨迹跟踪，队形变换时，随着参考车辆运动，目标点的变换，跟随车辆可能需要后退或进行其他操作，从而导致跟随车辆反复运动，在前进和后退或大转弯来回摆动的现象发生；另一方面其在变换后缺少队形保持的方法说明，整个系统有所欠缺，整个系统只有一套轨迹跟踪的执行方法，对队形的整体要求未加约束，其变换及控制效果有待改善，方法有待优化。

发明内容

本发明的目的在于提供一种基于Dubins曲线的多车队形切换方法。

实现本发明目的的技术解决方案为：一种基于Dubins曲线的多车队形切换方法，包括如下步骤：

步骤(1)：在接到变换指令后，车队系统根据几何约束计算目标点，系统应用车辆运动学模型和Dubins曲线在一定速度范围内进行规划，得到时间序列T_s，并得到最短的行驶时间T_c以及其轨迹队形的路径长度S_c和速度V_c；

步骤(2)：计算领航车辆行驶T_c内的平均速度V_L，以及行驶的路程长度S_L，最终位置；随后依据队形要求计算出跟随车辆的新目标点P_pre；

步骤(3)：根据步骤(2)得到的新目标点P_pre进行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹；

步骤(4)：根据步骤(3)得到的最优轨迹，进行轨迹跟踪，并且在运动过程中进行终止条件判断；

步骤(5)：达到判定的终止条件后，车队系统进入队形保持阶段。

进一步的，所述步骤(3)具体步骤如下：

步骤(3-1)：已知行驶时间T_c反算Dubins曲线，获得车辆从当前位置到新目标点P_pre的Dubins曲线长度S_pre和此时Dubins曲线的速度V_pre，最大转弯半径R；

步骤(3-2)：假如V_pre≤V_max则车辆执行V_pre，否则系统剔除当前路径曲线并且从步骤(1)开始循环，选取次小的时间记为T_c，直至满足条件结束循环，其中V_max为车辆速度最大值。

进一步的，所述步骤(3)“行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹”采用如下算法整合公式：

满足以下条件：

T(Dubins(pstart,pend,R),V)＝Dubins/V

其中path表示跟随车辆的最终轨迹，Dubins表示轨迹，T(Dubins)表示该轨迹所需时间，R为车辆最小转弯半径，P表示跟所车辆位置，下标start和end分别表示起始和最终，aL为车辆最大横向加速度，一般取0.4g，K为道路曲率，V为车辆速度向量，下标min，max分别表示最小值和最大值，ΔV为速度在时间间隔T的变化量。

进一步的，所述步骤(4)中“进行轨迹跟踪”的轨迹跟踪控制方法为MPC(模型预测控制)；

变换的终止条件如下：

||P-Po||≤0.5L，cos(φ-α)≥0.8；

P为车辆当前时刻位置，Po为目标点位置，后者cos(φ-α)为领航车和跟随车辆的航向角之差，L为队形的距离要求，φ为跟随车辆航向角，α为领航车航向角。

进一步的，所述步骤(5)中“队形保持阶段”的队形保持具体步骤为：

步骤(5-1)：将车辆运动学模型及领航跟随者模型线性离散化；

步骤(5-2)：构建MPC框架，融合自身轨迹及队形要求设计代价函数，结合ORCA理论设计碰撞约束。

进一步的，所述步骤(5-2)中的代价函数J(k)如下：

Q，R和ρ分别是加权因子，η为队形及轨迹列向量，η_ref为参考向量，ε为大于零的正实数，T为当前时刻，k+i为预测时刻，H_p是预测时刻总数，H_c为控制时刻总数，函数在第一项中考虑车辆自身的轨迹误差和编队误差；第二项限制车速和转向加速度，以使车辆平稳行驶，应用第三项以确保优化问题具有可行的解决方案；车辆的输入受到极限值的限制，并且在T时间段内输入的变化也需要被限制以确保车辆的稳定性；

碰撞约束如下：

u_min≤u_k,t≤u_max△u_min≤△u_k,t≤△u_max

u为输入列向量，包含车辆的速度和角速度，u_k,t与Δu_k,t分别表示u(k|t)和Δu(k|t)；

避免碰撞约束条件如下：

||P_A-P_B+κT(V_A-V_B)||>r_A+r_B

其中P为车辆位置，r为车辆的圆形模型的轮廓半径，下标A和B表示A车和B车，κ是合作决策系数，它取决于两辆车的决策措施，随着B采取减速等主动防撞措施，κ的值会增加，在大多数情况下κ为1.5，A和B的性能相同。

本发明与现有技术相比，其显著优点在于：

(1)与传统的队形变换策略相比，本发明以Dubsins曲线为基础进行迭代优化，每次最优计算后如果不满足条件会循环重新计算，最终得到最优轨迹，相比于此前的方案，其结果更加合理。

(2)与传统的编队保持策略相比，本发明对非线性运动学模型及队形模型进行拉格朗日线性化及相应的离散化处理，并结合了ORCA碰撞约束，取得了较好的效果。

附图说明

图1为整体队形切换策略框架图。

图2为算法流程图。

图3为Dubins曲线示意图。

图4为领航跟随者模型；其中下标L和F分别为领航车，跟随车。

图5为队形保持算法的框架图。

图6为保持策略的队形切换仿真图。

图7为采用本发明的队形切换仿真图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

实施例1

如图1所示，为整体的系统框架，系统接收到切换指令时，系统采用切换算法计算出相应轨迹，发送给跟随车辆，跟随车辆在行进的过程中会判断是否达到期望位置，若达到，则进入队形保持程序。例如，队形距离要求为20m，成等边三角形行驶，速度为3m/s。

步骤1：如图3所示，即为Dubins曲线示意图，图中描述的Dubins曲线为RSL，即右转直行左转，车辆由A点到B点，先右转，再直线行驶，最后要到达时左转，两个圆的半径为车辆的转弯半径。队形由三角行变为一字形，距离为8m，夹角为180°，可以得到跟随者的目标点。此时，A点位跟随者当前位置，B点即为车辆目标点，从而应用Dubins曲线得到一定速度范围内轨迹和时间序列，选取最小时间T_c。

步骤2：领航车辆是匀速行驶，因此其速度V_L依然是3m/s，经过时间T_c行驶的路程长度S_L，并得到最终位置。随后依据队形要求计算出跟随车辆的新目标点P_pre。

步骤3：根据时间Tc反算Dubins曲线，获得车辆从当前位置到P_pre的Dubins曲线长度S_pre和此时Dubins曲线的速度V_pre，最大转弯半径R。假如V_pre≤V_max则车辆执行V_pre，否则系统剔除当前路径曲线并且从步骤1开始循环，选取次小的时间记为T_c，直至满足条件结束循环。计算得出的最小时间为6s，速度为2.9m/s。

步骤四：跟随车辆得到其轨迹后，按照一定的策略进行跟踪，并且在运动过程中进行条件判断，当满足变换终止条件后进入队形保持阶段。其条件如下：

||P-Po||≤0.5L，cos(φ-α)≥0.8；

即领航者和跟随者距离小于10m，且其航向角小于37°时，车辆将进入保持阶段。

步骤5：达到判定条件后，车辆按照预定队形行驶，其队形保持算法设计如下：

如图4所示为领航跟随者模型示意图，图5为系统框架图，首先分别为车辆运动学和编队运动模型线性离散化，其模型具体表达式：

运动学模型中，X和Y分别是车辆在地面坐标系上的横坐标和纵坐标。δ_f是车辆转向角φ是车辆前进角.L是车辆轴距。R是车辆转弯半径。ω是车辆的角速度。

编队模型中L^d _LF是期望的队形距离。α^d _LF是车辆的相对方位，ф^d _LF是领航车相对跟随车的夹角，满足以下公式ф^d _LF＝θ_L–θ_F。d是车辆质心与车辆前轴之间的距离。“1”和“2”分别表示领航车辆和跟随车辆。

线性化如下：

上述方程中，V_v＝V₂和W_v＝W₂，

为车辆的状态变化向量，x_v为车辆状态向量，

为队形状态变化量，下标12表示属于队形的状态变化向量和状态向量，下标r表示各自的参考量。u表示输入转角和速度的列向量。

根据上述方程后进行离散化。

T是时间间隔，I是单位矩阵，A_v,A_LF,B_v，B_LF

其MPC总框架为：

Y(t)＝Ψ_tξ(t|t)+Θ_t△U(t)

其代价函数设计为：

Q，R和ρ分别是加权因子。t为当前时刻，k+i为预测时刻。该函数在第一项中考虑车辆自身的轨迹误差和编队误差。第二项限制车速和转向加速度，以使车辆平稳行驶。应用第三项以确保优化问题具有可行的解决方案，ε为大于零的正数。车辆的输入受到极限值的限制，并且在T时间段内输入的变化也需要被限制以确保车辆的稳定性。约束如下：

u_min≤u_k,t≤u_max △u_min≤△u_k,t≤△u_max

u_k,t与Δu_k,t分别表示u(k|t)和Δu(k|t)。

另外，碰撞约束被设计成确保车辆内部在行驶过程中不会发生碰撞，采用VO碰撞的概念，得到避免碰撞约束条件。

||P_A-P_B+T(V_A-V_B)||>r_A+r_B

其中P_A和P_B是车辆A和B的向量。T是时间常数。r_A和r_B是车辆A和B的半径，将A和B视为圆形。V_A和V_B也是向量，V_A是要优化的变量。然而，该方法忽略了车辆B的运动和决策过程，并且还导致了车辆路径的振动。实际上，B也会采取措施避免与B发生冲突，因此条件可以写为：

||P_A-P_B+κT(V_A-V_B)||>r_A+r_B

其中κ是合作决策系数，它取决于两辆车的决策措施，随着B采取减速等主动防撞措施，κ的值会增加，在大多数情况下κ为1.5，A和B的性能相同。

综合以上方程将队形维持问题转化为离散优化问题：

s.t:

Y(t)＝Ψ_tξ(t|t)+Θ_tΔU(t)ξ(k+1|t)＝A_k,tξ(k|t)+B_k,tΔu(k|t)k＝t，t+1，...，t+H_p-1△u_k,t＝0；k＝t+Hc,…,t+Hpu_min≤u_k,t≤u_max k＝t,...,t+Hp

△u_min≤△u_k,t≤△u_max k＝,t...,t+Hc-1||P_k,t-P_B,t,t+κT＝(V_k,t-V_B，t,t)||＞r_A+r_Bk＝t，...，t+H_c-1

仿真结果如图6和图7，由图中可以看出，采用本发明的队形切换方法，车队的跟踪效果相比传统的方法，效果更好，响应时间更短。

Claims (6)

1.一种基于Dubins曲线的多车队形切换方法，其特征在于，包括如下步骤：

步骤(1)：在接到变换指令后，车队系统根据几何约束计算目标点，系统应用车辆运动学模型和Dubins曲线在一定速度范围内进行规划，得到时间序列T_s，并得到最短的行驶时间T_c以及其轨迹队形的路径长度S_c和速度V_c；

步骤(2)：计算领航车辆行驶T_c内的平均速度V_L，以及行驶的路程长度S_L，最终位置；随后依据队形要求计算出跟随车辆的新目标点P_pre；

步骤(3)：根据步骤(2)得到的新目标点P_pre进行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹；

步骤(4)：根据步骤(3)得到的最优轨迹，进行轨迹跟踪，并且在运动过程中进行终止条件判断；

步骤(5)：达到判定的终止条件后，车队系统进入队形保持阶段。

2.根据权利要求1所述的方法，其特征在于，所述步骤(3)具体步骤如下：

步骤(3-1)：已知行驶时间T_c反算Dubins曲线，获得车辆从当前位置到新目标点P_pre的Dubins曲线长度S_pre和此时Dubins曲线的速度V_pre，最大转弯半径R；

步骤(3-2)：假如V_pre≤V_max则车辆执行V_pre，否则系统剔除当前路径曲线并且从步骤(1)开始循环，选取次小的时间记为T_c，直至满足条件结束循环，其中V_max为车辆速度最大值。

3.根据权利要求2所述的方法，其特征在于，所述步骤(3)“行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹”采用如下算法整合公式：

满足以下条件：

T(Dubins(pstart,pend,R),V)＝Dubins/V

其中path表示跟随车辆的最终轨迹，Dubins表示轨迹，T(Dubins)表示该轨迹所需时间，R为车辆最小转弯半径，P表示跟所车辆位置，下标start和end分别表示起始和最终，aL为车辆最大横向加速度，一般取0.4g，K为道路曲率，V为车辆速度向量，下标min，max分别表示最小值和最大值，ΔV为速度在时间间隔T的变化量。

4.根据权利要求3所述的方法，其特征在于，所述步骤(4)中“进行轨迹跟踪”的轨迹跟踪控制方法为MPC；

变换的终止条件如下：

||P-Po||≤0.5L，cos(φ-α)≥0.8；

P为车辆当前时刻位置，Po为目标点位置，后者cos(φ-α)为领航车和跟随车辆的航向角之差，L为队形的距离要求，φ为跟随车辆航向角，α为领航车航向角。

5.根据权利要求4所述的方法，其特征在于，所述步骤(5)中“队形保持阶段”的队形保持具体步骤为：

步骤(5-1)：将车辆运动学模型及领航跟随者模型线性离散化；

步骤(5-2)：构建MPC框架，融合自身轨迹及队形要求设计代价函数，结合ORCA理论设计碰撞约束。

6.根据权利要求5所述的方法，其特征在于，所述步骤(5-2)中的代价函数J(k)如下：

Q，R和ρ分别是加权因子，η为队形及轨迹列向量，η_ref为参考向量，ε为大于零的正实数，T为当前时刻，k+i为预测时刻，H_p是预测时刻总数，H_c为控制时刻总数，函数在第一项中考虑车辆自身的轨迹误差和编队误差；第二项限制车速和转向加速度，以使车辆平稳行驶，应用第三项以确保优化问题具有可行的解决方案；车辆的输入受到极限值的限制，并且在T时间段内输入的变化也需要被限制以确保车辆的稳定性；

碰撞约束如下：

u_min≤u_k,t≤u_max △u_min≤△u_k,t≤△u_max

u为输入列向量，包含车辆的速度和角速度，u_k,t与Δu_k,t分别表示u(k|t)和Δu(k|t)；

避免碰撞约束条件如下：

||P_A-P_B+κT(V_A-V_B)||>r_A+r_B

其中P为车辆位置，r为车辆的圆形模型的轮廓半径，下标A和B表示A车和B车，κ是合作决策系数，它取决于两辆车的决策措施，随着B采取减速等主动防撞措施，κ的值会增加，在大多数情况下κ为1.5，A和B的性能相同。

Images