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

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

【技术实现步骤摘要】
基于Dubins曲线的多车队形切换方法
本专利技术属于智能无人车领域，具体涉及一种基于Dubins曲线的多车队形切换方法。
技术介绍
自人工智能交通被研究开始，随着智能汽车技术的发展，多智能车编队协同驾驶已逐渐成为近年来的热点之一。当前研究主要集中于多车编队控制研究，而对于队形变换的策略考虑较少，因此为适应未来交通的发展，有必要对队形变换的策略进行深一步的研究。CN110262493A提出一种基于前车预瞄点及位置与速度等信息的队形变换方法，在收到变换的指令后根据队形形状进行计算几何位置，再根据几何位置预设参数，计算自车的目标速度和转角，进行变换，并且向其它车辆发送自车行驶状态，从而使下一车辆根据队形等信息进行队形变换，从而实现了队形变换。然而，有一些情况并未考虑：一方面，其并未考虑在变换过程中第一辆车即队形参考车辆的位置是变化的情况，可能导致跟随车辆按照此前目标点计算得到的预瞄点进行轨迹跟踪，队形变换时，随着参考车辆运动，目标点的变换，跟随车辆可能需要后退或进行其他操作，从而导致跟随车辆反复运动，在前进和后退或大转弯来回摆动的现象发生；另一方面其在变换后缺少队形保持的方法说明，整个系统有所欠缺，整个系统只有一套轨迹跟踪的执行方法，对队形的整体要求未加约束，其变换及控制效果有待改善，方法有待优化。
技术实现思路
本专利技术的目的在于提供一种基于Dubins曲线的多车队形切换方法。实现本专利技术目的的技术解决方案为：一种基于Dubins曲线的多车队形切换方法，包括如下步骤：步骤(1)：在接到变换指令后，车队系统根据几何约束计算目标点，系统应用车辆运动学模型和Dubins曲线在一定速度范围内进行规划，得到时间序列Ts，并得到最短的行驶时间Tc以及其轨迹队形的路径长度Sc和速度Vc；步骤(2)：计算领航车辆行驶Tc内的平均速度VL，以及行驶的路程长度SL，最终位置；随后依据队形要求计算出跟随车辆的新目标点Ppre；步骤(3)：根据步骤(2)得到的新目标点Ppre进行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹；步骤(4)：根据步骤(3)得到的最优轨迹，进行轨迹跟踪，并且在运动过程中进行终止条件判断；步骤(5)：达到判定的终止条件后，车队系统进入队形保持阶段。进一步的，所述步骤(3)具体步骤如下：步骤(3-1)：已知行驶时间Tc反算Dubins曲线，获得车辆从当前位置到新目标点Ppre的Dubins曲线长度Spre和此时Dubins曲线的速度Vpre，最大转弯半径R；步骤(3-2)：假如Vpre≤Vmax则车辆执行Vpre，否则系统剔除当前路径曲线并且从步骤(1)开始循环，选取次小的时间记为Tc，直至满足条件结束循环，其中Vmax为车辆速度最大值。进一步的，所述步骤(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为预测时刻，Hp是预测时刻总数，Hc为控制时刻总数，函数在第一项中考虑车辆自身的轨迹误差和编队误差；第二项限制车速和转向加速度，以使车辆平稳行驶，应用第三项以确保优化问题具有可行的解决方案；车辆的输入受到极限值的限制，并且在T时间段内输入的变化也需要被限制以确保车辆的稳定性；碰撞约束如下：umin≤uk,t≤umax△umin≤△uk,t≤△umaxu为输入列向量，包含车辆的速度和角速度，uk,t与Δuk,t分别表示u(k|t)和Δu(k|t)；避免碰撞约束条件如下：||PA-PB+κT(VA-VB)||>rA+rB其中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曲线得到一定速度范围内轨迹和时间序列，选取最小时间Tc。步骤2：领航车辆是匀速行驶，因此其速本文档来自技高网...

【技术保护点】
1.一种基于Dubins曲线的多车队形切换方法，其特征在于，包括如下步骤：/n步骤(1)：在接到变换指令后，车队系统根据几何约束计算目标点，系统应用车辆运动学模型和Dubins曲线在一定速度范围内进行规划，得到时间序列T

【技术特征摘要】
1.一种基于Dubins曲线的多车队形切换方法，其特征在于，包括如下步骤：
步骤(1)：在接到变换指令后，车队系统根据几何约束计算目标点，系统应用车辆运动学模型和Dubins曲线在一定速度范围内进行规划，得到时间序列Ts，并得到最短的行驶时间Tc以及其轨迹队形的路径长度Sc和速度Vc；
步骤(2)：计算领航车辆行驶Tc内的平均速度VL，以及行驶的路程长度SL，最终位置；随后依据队形要求计算出跟随车辆的新目标点Ppre；
步骤(3)：根据步骤(2)得到的新目标点Ppre进行重规划并迭代选取最接近耗费时间的轨迹从而得到最优轨迹；
步骤(4)：根据步骤(3)得到的最优轨迹，进行轨迹跟踪，并且在运动过程中进行终止条件判断；
步骤(5)：达到判定的终止条件后，车队系统进入队形保持阶段。


2.根据权利要求1所述的方法，其特征在于，所述步骤(3)具体步骤如下：
步骤(3-1)：已知行驶时间Tc反算Dubins曲线，获得车辆从当前位置到新目标点Ppre的Dubins曲线长度Spre和此时Dubins曲线的速度Vpre，最大转弯半径R；
步骤(3-2)：假如Vpre≤Vmax则车辆执行Vpre，否则系统剔除当前路径曲线并且从步骤(1)开始循环，选取次小的时间记为Tc，直至满足条件结束循环，其中Vmax为车辆速度最大值。


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...

【专利技术属性】
技术研发人员：万晨曦，皮大伟，王洪亮，谢伯元，王尔烈，王显会，王霞，孙晓旺，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏;32