基于贝塞尔曲线的变道轨迹规划

基于贝塞尔曲线的变道轨迹规划

车辆的换道与超车是驾驶员常见的驾驶操作之一，无人驾驶车辆在行驶过程中也会频繁的面临此工况，车辆行驶过程中必须根据行驶环境中车车之间的相对速度与距离，以及车辆周边其他环境的变化信息，相应做出调整进而完成驾驶要求。在这个过程中，车辆必须对安全换道和超车的通过性做出准确评估，从而使车辆安全的运行。因此，无人车的轨迹规划是保证车辆安全行驶的重要组成部分。

在无人车辆人局部路径规划过程中，需要满足车辆运行过程中的各种约束，一方面要并保证车辆不发生事故，遵守物理条件的要求，另一方面也要满足高速运行的实时性要求。
路径规划最初是由计算机图形学发展而来的，经过多年发展逐渐寻找到目前较好的适用于车辆的路径规划方法：
1、搜索法基本思想是将空间分解为多个栅格，寻找一条最优连通路线作为目标路径例如 A*，D*算法等；
2、人造势场法将目标环境模拟为能量场，障碍区为斥力，建议通过的区域为引力极，但该方法对物体边缘复杂难以识别，同时容易进入局部最小不能得到最优解；
3、神经网络与遗传算法进行的路径规划方法也逐渐出现，但这些方法计算较为复杂，通常用于全局规划。

因此对于适应车辆安全换道的轨迹并不是任意曲线都可以实现的，对于车辆系统提出换道轨迹设计至少应满足以下准则：

1、轨迹连续；
2、轨迹曲率可导；
3、轨迹容易被车辆跟随，且容易生成；

通常都采用曲线拟合的方法来对离散化的路点进行处理和规划。

超车规划

路径规划主要是在行驶过程中保证车辆的安全性，在超车或换道过程中，常见的事故主要为追尾、角撞、刮蹭、驶出车道，事故原因都是在碰撞点前横向和纵向距离不足导致两车发生接触，因此需要通过几何约束来进行设置关键点，保证碰撞危险点前在两个方向上都合理。

轨迹曲线分析

根据车和障碍物，或者目标位置选定一系列的特征点，然后通过对特征点的初步样条拟合形成初始的目标路径。根据车辆动力学来说，车辆的行驶半径和横向加速度的关系如下：


其中公式里面的p和R分别是对应的路径的曲率和曲率半径，m为车辆的质量，ay是车辆行驶过程当中的横向加速度，v 为车速，假设车辆行驶过程中不生成侧滑，所以它的横向加速度和车辆轨迹曲率有一个线性关系。且要求车辆的横向加速度连续，则轨迹曲率也要连续。
则也就是说车辆轨迹的拟合函数光滑可导，曲率可导最低也是三阶贝塞尔，因此必须要选择高阶贝塞尔曲线来进行轨迹拟合。

贝塞尔样条曲线

贝塞尔曲线是通过一组多边折线的各顶点来定义的。在每个顶点中，曲线经过第一点和最后一点，其余各点则定义曲线的导数、阶次和形状。同时通过控制第一条和最后一条折线的方向可以定义曲线起点和终点的切线方向。

上图为四阶贝塞尔曲线的数学表达式，其中B(t)表示贝塞尔拟合曲线，t属于[0,1]，P0/P1/p2/P3/P4代表构成贝塞尔曲线的5个顶点坐标，
那么如何根据贝塞尔曲线进行路劲规划呢，主要是以下两个步骤：
1、根据前方车辆的位置选定关键点位置，并进行初步的路径规划；
2、根据具体的优化指标再进行关键点的重新规划，进一步进行路径的调整；

路径规划关键点位的选取

我们假设前方车辆叫C0，以较高的车速v0在右侧前进，后车C1则以恒定车速v1准备超车，则此时就需要进行轨迹规划，生成一条合适的超车轨迹。

首先根据分析：在C1进行左转超车的时候，一定要去注意和远离C0的左后方危险点，也就是点1。
点1位于该线段转向方向上的端点。

在超车准备过程中一旦横截面和转向方向确定，关键点 1就被确定下来，通过以点 1 为圆心，车辆的宽度和两车横向安全距离为半径做圆，该圆范围内即为转向过程中的危险区域，为保证超车安全，自车的中心线离该区域越远越好，这样可以避免车辆在横向的碰撞危险。
点 2 通过纵向最小安全距离确定，即通过该距离用来避免在超车过程中可能出现的前后碰撞事故。

碰撞检测公式：


C1车和 Co 两车之间的最小无碰撞距离是同时由两车的纵向相对加速度，初始的相对纵向速度和碰撞点的时间 tp共同决定的。



其中：
P2是车辆行驶的中心线上；
P3是车辆转向过程中距离前车最近的那个点，也是转向过程中最危险，最容易发送碰撞的那个点，通过P2向危险圆形区域做切线，再与车辆转向方向相切的切点就是P3；
P4是根据车辆的横向行驶宽度约束得到的，中国国内单车道宽度为3.5m，一般的车宽度为1.7m~2.0m。在保证轮胎还在线性区内，尽可能大的转角，获取对应的车辆在车道内的最大横向运动空间H，采用圆弧进行起始点规划，其中以H为圆弧拱高，对应弦长为目标的距离长度。

P6则是P2P3的延长线，与车辆变道完成后的中心平行线相交点，这里可以保证车辆远离P3后，车尾部不会与前车发生剐蹭。
他是路径与危险区域的共切线。
过点 5与行驶方向平行做直线，同时过点 2 与点 3 做直线，这样起始和最终的路径方向则控制在这两条直线上。
P1则是变道发起时刻的车辆自身位置；
P5是期望变道点的车辆位置；

四次贝塞尔曲线的形成至少需要五个点，为了控制车辆在接近前车的过程中的方向，需要增加辅助点，在路径规划过程中，路径根据环境变化实时刷新，因此也需要更为密集的关键点选取。在这里我们采取 0.1L 为一个间隔，去生成这5个点来获取规划路径。

结论

通过连接上述关键点我们可以得到一条连续的折线，以该折线作为生成贝塞尔样条的结构线，将关键点带入贝塞尔曲线函数内，则可以生成初步的路径。结构线是作为路径规划的参考，几何上满足了超车过程中的目标要求，实际行驶过程中越接近该线则越能满足目标条件，但通常会降低行驶速度，或者损失车辆的稳定性，因此通过对路径的优化可以达到各性能之间的最优平衡。