[发明专利]基于Dubins曲线的多车队形切换方法

权利要求书

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的性能相同。