CN110928290B - 五次曲线路径规划方法及车道中心线维护方法 - Google Patents

Abstract

本发明涉及自动驾驶技术领域，具体来说是一种五次曲线路径规划方法及车道中心线维护方法，在车辆的车身坐标系中采用四元组（x,y,r,k）描述车辆的状态模型，设置轨迹起点S的位置限制、曲率限制、以及姿态限制条件；设置轨迹终点T的位置限制、曲率限制、以及姿态限制条件；求解满足步骤a和步骤b中所有限制条件的n次曲线模型，n为大于等于5的正整数。由此方法出来的路径不仅满足起始点的位姿条件，同时对起始点的轨迹的曲率给出限制条件，保证了车辆的转向控制的平滑性。该方法的跟随性能非常好，避免了方向盘控制的阶跃跳变，实车测试验证结果也表明，根据该方法进行自动驾驶控制，人的乘车体验的舒适性与人类司机的控制效果没有明显区别。

Description

五次曲线路径规划方法及车道中心线维护方法

技术领域

本发明涉及自动驾驶技术领域，具体来说是一种五次曲线路径规划方法及车道中心线维护方法。

背景技术

自动驾驶中一个重要的任务是使计算机可以代替人来自动控制车辆的方向盘，保证车辆可以以合理的、安全的位置和姿态在道路上自动行驶。在L3的自动驾驶应用场景里最典型的功能就是车道保持，该功能对车辆的要求是使车辆在车道中间，沿着车道线的方向进行行驶，在多数的车道保持的应用场景中，要求车辆需要配置一个图像传感器或者其他定位传感器来检测车辆当前的位置与姿态，比如有些图像传感器能够检测车辆所在车道的车道线的形状模型，mobileye就是这种类型的可以用于相对定位的传感器，车道线模型描述了车辆与左右车道线的位置关系以及方向关系，由此提供了车辆位姿调整的传感器依据。

转向控制系统中的软件模块一般有三个，分别是路径规划模块、路径跟随模块、pid控制模块。路径规划的目的是规划一条车辆可以行驶的路径，这个路径给出了车辆调整姿态的过程，在自动驾驶领域一般使用三元组(x，y，r)来描述车辆的位置(x，y)和姿态r。路径规划出来的轨迹使车辆从一个不合理的位姿S＝(x0，y0，r0)逐渐调整到一个预设的合理的位姿T＝(x1，y1，r1)，保证车辆的运行轨迹符合车道保持或其他应用场景中对车辆姿态的要求。路径规划问题的数学描述可以表示为下面的公式：y＝path(x)。

路径规划算法难点在设计一种轨迹模型，使得该模型可以满足车辆从当前姿态到目标姿态变化的条件，多数的自动驾驶系统中使用三次曲线模型作为轨迹的模型，比日本明古屋大学的自动驾驶开源项目autoware仿真器中用到的路径规划曲线hermit曲线就是这种三次曲线模型，三次曲线的轨迹模型要求轨迹曲线满足起点和终点轨迹的位置和轨迹的一阶导数满足位置限制以及朝向限制，共4个限制方程，该方法的缺点是用路径跟随算法对三次曲线路径规划算法求解出来的路径进行跟随时，角速度曲线在轨迹的起点有一个阶跃，这种跳变导致在实车测试中，车辆的方向盘摆动非常大，车辆无法按照预设的轨迹进行跟随，即三次曲线的跟随性能非常差。

发明内容

本发明的目的在于解决现有技术的不足，提供一种五次曲线路径规划方法，提升路径跟随的精度；并提供一种采用五次曲线路径规划方法的车道中心线维护方法，能够在车道线突然不可见的情况下保持车身稳定。

为了实现上述目的，设计一种五次曲线路径规划方法，设车辆轨迹模型为path(x)，所述的方法首先进行路径规划步骤，而后再进行速度规划步骤，所述的路径规划步骤具体如下：在车辆的车身坐标系中采用四元组(x,y,r,k)描述车辆的状态模型，其中x为车辆的横坐标，y为车辆的纵坐标，r为姿态，即车辆轨迹与x轴的夹角，k为车辆轨迹的曲率，所述的方法包括如下步骤：步骤a.设置轨迹起点S的位置限制、姿态限制、以及曲率限制条件；步骤b.设置轨迹终点T的位置限制、姿态限制、以及曲率限制条件；步骤c.求解满足步骤a和步骤b中所有限制条件的最简多项式模型。

本发明还具有如下优选的技术方案：

所述的步骤a具体包括：步骤a1.设起点S的位置条件为x₀＝0，y₀＝0，并得到起点位置限制条件为path(0)＝0；步骤a2.设起点S的姿态条件为r₀＝0，tan(r₀)＝path’(x₀)，并设定起点姿态限制条件为path’(0)＝0；步骤a3.设置起点曲率限制条件及姿态限制条件为

其中x₀为起点S的横坐标，y₀为起点S的纵坐标，r₀为车辆在起点S的姿态，k₀为车辆轨迹在起点的曲率。

所述的步骤b具体包括：步骤b1.设终点T的终点位置限制条件Path(x₁)＝mid_lane(x₁)；步骤b2.设终点T的姿态条件为r₁＝atan(mid_lane’(x₁))且path‘(x₁)＝tan(r₁),由此得到姿态限制条件：path′(x₁)＝mid_lane′(x₁)；步骤b3.设置终点曲率限制条件为path″(x₁)＝mid_lane″(x₁)；其中，x₁为终点T的横坐标，r₁为车辆在终点T的姿态，k₁为车辆轨迹在终点的曲率，mid_lane(x)为车道线的中线模型方程。

所述的终点T的横坐标x₁＝Mdis，终点T的纵坐标y₁＝mid_lane(Mdis)，Mdis为车道线模型最大可信覆盖距离。

通过图像采集设备获得左右车道线的模型即y＝left_lane(x)和y＝right_lane(x)，中线模型定义为y＝mid_lane(x)＝0.5*(left_lane(x)+right_lane(x))。

通过所述的步骤c具体如下：设所述的车辆轨迹模型为path(x)为五次曲线模型：Y＝X*P，并根据公式P＝inv(X)*Y，获得曲线的参数向量P，其中：

所述的速度规划步骤具体如下：设位移模型s(t)＝D₄t⁴+D₃t³+D₂t²+D₁t+D₀，D₀、D₁、D₂、D₃、D₄、D₅为多项式系数，t表示时刻，将S(0)＝D₀＝0、D₁＝v₀、2D₂＝a₀t_g、

和/>

代入t_g时刻的终态位移条件S(t_g)＝S_g可得/>

式中v_g为车t_g时刻的线速度，a_g为车t_g时刻的线加速度，v₀为车当前时刻的线速度，a₀为车当前时刻的线加速度，由此求解得出t_g，并进而得到D₃和D₄的值，以确定位移模型s(t)。得到位移模型后，对轨迹path(x)上的每一点P_k进行积分求出轨迹上的每一点距离起点S的位移S_k，根据S_k＝s(t)，使用牛顿拉普拉斯算法计算出从S移动到P_k处所用的时间T_k，然后将T_k带入速度模型和加速度模型，计算出P_k处的线速度以及线加速度，从而完成速度规划。/>

本发明还设计一种采用如权利要求1-6任一所述的五次曲线路径规划方法的车道中心线维护方法，所述的方法以固定周期运行，运行的规划周期为H赫兹，具体包括如下步骤：

步骤1.追踪车道线特征点，对车道线质量进行判断，并丢弃差质量的车道线数据。

步骤2.根据上一规划周期计算出的车道线、本车历史轨迹、本车速度、角速度，融合预测出当前时刻的车道中心线。

步骤3.根据预测的车道中心线，对于轨迹规划的目标点，根据所述的五次曲线路径规划方法规划出局部轨迹。

所述的采用五次曲线路径规划方法的车道中心线维护方法还包括如下优选的技术方案：

所述的步骤1具体如下：设第t个规划周期中局部坐标系中的特征点集合为

其中t表示当前时刻，l表示局部坐标系，m_i表示在局部坐标系中的第i个特征点，/>

其中s为任意相邻两个特征点在x方向上的距离。

将特征点集合M_l ^t中的点转换到全局坐标系，记为

其中g表示全部坐标系，/>

表示在全局坐标系中的第i个特征点。

在第t+1个规划周期，根据接受到的左车道线或右车道线和车道宽度，把左车道线或右车道线平移到车道中心线的位置，计算k个局部坐标系特征点，记为

其中l表示局部坐标系，并根据第t+1个规划周期的本车全局定位信息把/>

中的每一个点转换到局部坐标系，得到的点集记为/>

针对

和/>

中的每一个点对第i个点求欧式距离，记为d_i，把所有k对点的距离求和得到/>

如果D超过一定的阈值h，则判定该车道线质量为差，并且丢弃该车道线数据；否则判定该车道线质量为好，保留该车道线数据。

所述的步骤2具体如下：若左车道线和右车道线判定质量均为差，则丢弃左右车道线，根据点集

采用最小二乘法拟合车道中心线曲线y＝f_t+1(x)。

所述的步骤3具体如下：根据车道中心线曲线y＝f_t+1(x)，给定轨迹规划目标点x方向坐标x_d，计算目标点y方向坐标y_d＝f_t+1(x_d)，并根据目标点(x_d，y_d)，采用所述的五次曲线路径规划方法规划出局部轨迹。

本发明的五次曲线规划方法同现有技术相比，其优点在于：使用四元组来描述车辆的状态模型(x，y，r，k)，设置了轨迹的位置限制、曲率限制、以及姿态限制条件，尤其是设置了轨迹的曲率限制条件，即设置了轨迹在起点和终点的曲率限制条件，给出了车辆在起点S处的曲率k₀和终点T处的曲率k₁的估算方法，并优选地使用五次曲线作为轨迹y＝path(x)的模型，保证了轨迹的曲率连续平滑变化，由此降低轨迹跟随的难度，同时，本发明根据位置和姿态进行路径规划，然后在已经规划的路径上采用位移模型进行速度规划，极大地降低了问题的维度和难度，运算简单且实时性高。并且本发明还涉及采用五次曲线规划方法的车道中心线维护方法，能够在车道线突然不可见的情况下保持车身稳定。

附图说明

图1是一实施方式中本发明车身坐标系的示意图。

图2是一实施方式中本发明的控制流程示意图。

图3是一实施方式中本发明仿真对比实验的结果示意图。

图中：1.图像采集设备2.运动测量设备(motion measurement)。

具体实施方式

下面结合附图对本发明作进一步说明，这种装置的结构和原理对本专业的人来说是非常清楚的。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例1

首先进行路径规划步骤，参见图1，本实施方式中所描述车辆起点S和终点T的状态模型、以及轨迹模型path(x)、车道线线模型全部使用车身坐标系表示，车身坐标系的坐标原点在车辆的两个后轮连线的中点，后轮连线为车身坐标系的y轴，从后轮连线中点向前轮连线中点看去，向左为车身坐标系的y轴的正方向，与y轴垂直向前的方向为车身坐标系的x轴正方向，即由后轮连线中点指向前轮连线中点的方向；运动测量设备测量的车辆的瞬时运动特征，主要包括车辆前进方向的速度vel，以及车辆所在二维平面的旋转速度yawrate；图像采集设备用于对车辆所在车道的左右两条车道线的检测，以及模型表示，左右车道线的模型定义为y＝lane(x)，本实施方式中使用最简单的姿态控制策略，即要求车辆始终沿着道路的中线行驶，图像采集设备可以获得左右车道线的模型即y＝left_lane(x)，y＝right_lane(x)，中线模型定义为y＝mid_lane(x)＝0.5*(left_lane(x)+right_lane(x))，同时要求图像算法要给出车道线模型最大可信覆盖距离，定义为Mdis。

图2是本实施方式所采用的转向控制系统的控制流程示意图，本实施方式中用到的道路跟随算法(path follower算法)是PurePursuit算法，PID控制算法使用P控制。

本实施方式中所述的方法在车辆的车身坐标系中采用四元组(x,y,r,k)描述车辆的状态模型，其中x为车辆的横坐标；y为车辆的纵坐标；r为姿态，定义的是车辆的朝向，即车辆轨迹与x轴的夹角；k为车辆轨迹的曲率，所述的方法包括如下步骤：

步骤a.设置轨迹起点S的位置限制、曲率限制、以及姿态限制条件，其包括如下步骤。

a1.设起点S的位置条件为x0＝0，y0＝0，并得到起点位置限制条件为path(0)＝0。

a2.设起点S的姿态条件为r0＝0，tan(r0)＝path’(x0)，并得到起点姿态限制条件为path’(0)＝0。

a3.通过运动测量设备读取车辆当前的瞬时状态特征，即角速度w₀和速度vel₀，设定起点S的曲率

且/>

由此得到/>

设置起点曲率限制条件为/>

步骤b.设置轨迹终点T的位置限制、曲率限制、以及姿态限制条件，由于从起点S到终点T的调整是一个过程，需要一定的调整时间，因此为了保证方向盘调整的时间尽量充裕，预期姿态设置在车道线模型的最大可信覆盖距离所在的中线位置，并且人在开车时，控制实现的运行轨迹的曲率大概等于道路的曲率，所以将终点T的曲率k₁设定为道路上的车道线的曲率，设定终点的姿态等于中线的方向。其具体包括如下步骤。

b1.设终点T的位置条件为x₁＝Mdis，y₁＝mid_lane(Mdis)，并得到终点位置限制条件Path(Mdis)＝mid_lane(Mdis)。

b2.设终点T的姿态条件为r₁＝atan(mid_lane’(Mdis))且path‘(Mdis)＝tan(r₁),并得到终点姿态限制条件：path′(x₁)＝mid_lane′(x₁)。

b3.设终点曲率等于车道线曲率

等于车道线曲率

从而得到终点曲率限制条件：path″(x₁)＝mid_lane″(x₁)。

由于路径跟随算法是通过在规划出来的轨迹上找到一个预瞄点，由此估算车辆的预期的角速度，再由PID算法根据当前角速度与预期角速度的差值来给出方向盘的控制量，车辆角速度w与曲率以K及车辆的速度vel的关系是w＝K*vel，车辆的速度是连续变化的，所以跟随曲线的连续性由轨迹的曲率k决定，因此本实施方式在步骤a和步骤b中设置了轨迹在起点和终点的曲率连续性限制，就能够保证跟随曲线的连续性。

步骤c.求解满足步骤a和步骤b中所有限制条件的n次曲线模型，由于三次曲线的轨迹模型无法满足曲率连续的限制条件，满足上诉6个条件方程的最简曲线模型就是5次曲线，因此本实施方式中n为大于等于5的正整数。本实施方式以5次曲线模型y＝path(x)＝c5*x^5+c4*x^4+c3*x^3+c2*x^2+c1*x+c0为例进行说明。五次曲线的参数向量定义为P，将上述6个限制方程整理为形如Y＝X*P的形式，其中：

最终根据公式P＝inv(X)*Y，获得曲线的参数向量P。

本实施方式中，为了验证该算法的跟随性能，做了两组仿真对比实验，仿真模拟的场景是车辆换道的过程，数据来源是人类司机控制下，录制的车辆的实际运动轨迹，将该轨迹的起点和终点作为本次实验的起点S和终点T，观察PurePursuit算法跟随出来的角速度曲线，对比算法是三次曲线规划算法。实验结果如图3所示。图中的曲线a为人类司机控制车辆时的角速度曲线，曲线c为三次曲线的角速跟随曲线，曲线b为本实施方式的五次曲线的角速度跟随曲线。在时间起点，可以看到三次曲线的角速度从接近0的一个很小的值跳变到了0.08，而五次曲线跟随出来的效果和人类司机非常接近，在时间起点的角速度值是平滑连续变化的。

五次曲线规划方法加入了曲率的限制保证了角速度控制的平滑性，使用相同的仿真实验进行验证，使用PurePursuit算法对五次曲线规划出来的轨迹进行跟随，跟随出来的角速度曲线的形状类似一个正弦曲线，在起点的角速度是平滑变化的，加上pid控制后进行试车测试，车辆实际执行出来的运行轨迹很好的跟随了规划的路径，实际跟随性能非常好。

由此方法出来的路径不仅满足起始点的位姿条件，同时对起始点的轨迹的曲率给出限制条件，保证了车辆的转向控制的平滑性。实验表明，与其他常用算法相比，该算法的跟随性能非常好，避免了方向盘控制的阶跃跳变，实车测试验证结果也表明，根据该算法进行自动驾驶控制，人的乘车体验的舒适性与人类司机的控制效果没有明显区别。

因此在路径规划步骤完成后，在由路径规划给出的路径上进行速度规划步骤，速度规划的目的是给出路径上每一点的速度与加速度，由于路径规划步骤已经规划出预期轨迹上每一点的位置信息，即由车当前的位置到轨迹上每一点的位移已经确定，那么速度规划的任务也就仅仅需要设计一个位移与时间的模型，假设位移模型为S＝S(t)，由于位移模型一阶导为速度模型S'＝v(t)，位移模型的二阶导为加速度模型v'＝a(t)，在自动驾驶的速度调整场景中，速度调整的稳态一般是速度达到预设值且加速度为零，所以加速度的变化趋势一般是从一个非零值或零值变化至零值，所以本实施方式中采用抛物线(二次曲线)来设计加速度模型，由此可知速度模型为三次曲线，位移模型为四次曲线，因此设位移模型S(t)＝D₄t⁴+D₃t³+D₂t²+D₁t+D₀，D₀、D₁、D₂、D₃、D₄、D₅为多项式系数，t表示时刻，式中已知车子当前时刻的位移为s(0)＝0，车子当前的线速度为v(0)＝v₀，车子当前的线加速度为a(0)＝a₀，以及车子所期望达到的期望时刻t_g的期望速度为v_g，期望加速度为a_g，而由当前时刻到达期望速度过程中车子的位移S_g是未知的，S_g一般由本车前方的障碍物(跟车对象)的行车速度与本车的速度共同决定，因此设定安全跟车距离fd(safety-follow-distance)，并设S_g＝(v₀+v_0bs)T-fd，式中T＝2s，v_0bs为跟车对象的当前行车速度。

已知上述条件之后，进行位移模型S_t的多项式系数的推导，当前时刻的起点位置条件为：

S(0)＝D₀＝0 ⑤

v(0)＝D₁＝v₀ ⑥

a(0)＝2D₂＝a₀t_g ⑦

t_g时刻时，

a(t_g)＝12D₄t_g ²+6D₃t_g+2D₂＝a_g ①

v(t_g)＝4D₄t_g ³+3D₃t_g ²+2D₂t_g+D₁ ②

由式①和式②可得，

/>

将上述式③-式⑦代入终态位移条件S(t_g)＝S_g可得

而后根据一元二次方程求根公式求解出t_g，将t_g代入式③和式④以确定D₃和D₄的值，并由此得到位移模型，得到位移模型后，对轨迹path(x)上的每一点Pk进行积分求出其距离起点S的位移Sk，根据S_k＝s(t)，使用牛顿拉普拉斯算法(牛顿拉普拉斯算法，又称牛顿迭代法)计算出从S移动到P_k处所用的时间T_k，然后将T_k带入速度模型和加速度模型，计算出P_k处的线速度以及线加速度，从而完成速度规划。

实施例2

自动驾驶时的轨迹规划需要清晰车道线才能实现，车道线突然不可见情况下车身将出现不稳定的情况，本实施例结合所述的五次曲线规划方法，实现对车道中心线的维护，在车道线突然不可见情况下保持车身的稳定。

步骤1：追踪车道线特征点，计算出收到的车道线质量为差质量，丢弃计算出为差质量的车道线数据，具体包括如下步骤1.1-1.8。

步骤1.1、算法以固定周期运行，运行周期为H赫兹，表示每1/H秒运行一次。

步骤1.2、在第t个规划周期，车道中心线以三次曲线表示(y＝f_t(x)＝c₃x³+c₂x²+c₁x+c₀)。

步骤1.3、在第t个规划周期，根据车道中心线计算局部坐标系中的特征点集合

其中t表示当前时刻t，l表示局部坐标系，m_i表示在局部坐标系中的第i个特征点，/>

其中s为任意相邻两个特征点在x方向上的距离。

步骤1.4、根据第t个规划周期的本车全局定位信息Pose_t把M中每一个点转换到全局坐标系，记为

其中t表示当前时刻t，Pose_t＝(x_t，y_t，r_t)为三元组，x_t和y_t分别为本车在全局坐标系中的x方向和y方向坐标，r_t为本车行进方向在全局坐标系中x-y平面上与x方向的夹角，g表示全局坐标系，m_i＝(x_i，y_i)表示在局部坐标系中的第i个特征点，/>

表示在全局坐标系中的第i个特征点。转换公式为：

步骤1.5、在第t+1个规划周期，根据接受到的左车道线和车道宽度，把左车道线平移到车道中心线的位置，以步骤三方法计算相同的k个局部坐标系特征点，记为

根据第t+1个规划周期的本车全局定位信息把/>

中的每一个点转换到局部坐标系，得到的点集记为/>

步骤1.6、针对

和/>

中的每一个对第i个点求欧式距离，记为d_i，把所有k对点的距离求和得到/>

如果D超过一定的阈值h，则判定左车道线质量为差，并且丢弃该车道线数据。否则判定左车道线质量为好，保留该车道线数据。/>

步骤1.7、在第t+1个规划周期，根据接受到的右车道线和车道宽度，把右车道线平移到车道中心线的位置，以步骤1.3方法计算相同的k个局部坐标系特征点，记为

根据第t+1个规划周期的本车全局定位信息把/>

中的每一个点转换到局部坐标系，得到的点集记为/>

步骤1.8、针对

和/>

中的每一个对第i个点求欧式距离，记为d_i，把所有k对点的距离求和得到/>

如果D超过一定的阈值h，则判定右车道线质量为差，并且丢弃该车道线数据。否则判定右车道线质量为好，保留该车道线数据。

步骤二：根据上一周期计算出的车道线、本车历史轨迹、本车速度、角速度，融合预测出当前时刻的车道中心线，具体包括如下步骤2.1-2.2。

步骤2.1、当车道线不可见时，采用步骤1.1-1.8分别判定左右车道线质量均为差，并丢弃左右车道线。

步骤2.2、根据步骤1.5中计算的点集

采用最小二乘法重新拟合车道中心线曲线参数(y＝f_t+1(x)＝c₃x³+c₂x²+c₁x+c₀)。

步骤三：根据预测的车道中心线，计算轨迹规划的目标点，规划出局部轨迹(包括轨迹坐标和速度、角速度)，具体为：根据目标点(x_d，y_d)，采用五次曲线拟合规划出局部轨迹，包括轨迹中每个点的坐标、速度和角速度。

步骤四：根据得到的局部轨迹计算刹车、油门、方向盘，控制车辆保持平稳行驶。

Claims (4)

1.一种五次曲线路径规划方法，设车辆轨迹模型为path(x)，其特征在于所述的方法首先进行路径规划步骤，而后再进行速度规划步骤，所述的路径规划步骤具体如下：在车辆的车身坐标系中采用四元组(x，y，r，k)描述车辆的状态模型，其中x为车辆的横坐标，y为车辆的纵坐标，r为姿态，即车辆轨迹与x轴的夹角，k为车辆轨迹的曲率，所述的方法包括如下步骤：

步骤a.设置轨迹起点S的位置限制、曲率限制、以及姿态限制条件；

步骤b.设置轨迹终点T的位置限制、曲率限制、以及姿态限制条件；

步骤c.求解满足步骤a和步骤b中所有限制条件的最简多项式模型；

所述的步骤a具体包括：

步骤a1.设起点S的位置条件为x₀＝0，y₀＝0，并得到起点位置限制条件为path(0)＝0；

步骤a2.设起点S的姿态条件为r₀＝0，tan(r₀)＝path’(x₀)，并设定起点姿态限制条件为path’(0)＝0；

步骤a3.设置起点曲率限制条件及姿态限制条件为

其中x₀为起点S的横坐标，y₀位起点S的纵坐标，r₀为车辆在起点S的姿态，k₀为车辆轨迹在起点的曲率，w₀为车辆当前的瞬时角速度，vel₀为车辆当前的速度；

所述的步骤b具体包括：

步骤b1.设终点T的终点位置限制条件Path(x₁)＝mid_lane(x₁)；

步骤b2.设终点T的姿态条件为r₁＝atan(mid_lane’(x₁))且path‘(x₁)＝tan(r₁)，由此得到姿态限制条件：path′(x₁)＝mid_lane′(x₁)；

步骤b3.设置终点曲率限制条件为path″(x₁)＝mid_lane″(x₁)；

其中，x₁为终点T的横坐标，r₁为车辆在终点T的姿态，k₁为车辆轨迹在终点的曲率，mid_lane(x)为车道线的中线模型方程；

所述的终点T的横坐标x₁＝Mdis，终点T的纵坐标y₁＝mid_lane(Mdis)，Mdis为车道线模型最大可信覆盖距离；

通过图像采集设备获得左右车道线的模型即y＝left_lane(x)和y＝right_lane(x)，中线模型定义为y＝mid_lane(x)＝0.5*(left_lane(x)+right_lane(x))；

通过所述的步骤c具体如下：设所述的车辆轨迹模型为path(x)为五次曲线模型：Y＝X*P，并根据公式P＝inv(X)*Y，获得曲线的参数向量P，其中：

所述的速度规划步骤具体如下：设位移模型s(t)＝D₄t⁴+D₃t³+D₂t²+D₁t+D₀，D₀、D₁、D_２、D₃、D₄、D₅为多项式系数，t表示时刻，将S(0)＝D₀＝0、D₁＝v₀、2D₂＝a₀t_g、

和/>

代入t_g时刻的终态位移条件S(t_g)＝S_g可得/>

式中v_g为车t_g时刻的线速度，a_g为车t_g时刻的线加速度，v₀为车当前时刻的线速度，a₀为车当前时刻的线加速度，由此求解得出t_g，并进而得到D₃和D₄的值，以确定位移模型s(t)，得到位移模型后，对轨迹path(x)上的每一点P_k进行积分求出轨迹上的每一点距离起点S的位移S_k根据S_k＝s(t)，使用牛顿拉普拉斯算法计算出从S移动到P_k处所用的时间T_k，然后将T_k带入速度模型和加速度模型，计算出P_k处的线速度以及加速度，从而完成速度规划。

2.一种采用如权利要求1所述的五次曲线路径规划方法的车道中心线维护方法，其特征在于所述的方法以固定周期运行，运行的规划周期为H赫兹，具体如下：

步骤1.追踪车道线特征点，对车道线质量进行判断，并丢弃差质量的车道线数据；

步骤2.根据上一规划周期计算出的车道线、本车历史轨迹、本车速度、角速度，融合预测出当前时刻的车道中心线；

步骤3.根据预测的车道中心线，对于轨迹规划的目标点，根据所述的五次曲线路径规划方法规划出局部轨迹。

3.如权利要求2所述的采用五次曲线路径规划方法的车道中心线维护方法，其特征在于所述的步骤1具体如下：

设第t个规划周期中局部坐标系中的特征点集合为

其中t

表示当前时刻，l表示局部坐标系，m_i表示在局部坐标系中的第i个特征点，

其中s为任意相邻两个特征点在x方向上的距离；

将特征点集合M_l ^t中的点转换到全局坐标系，记为

其中g表示全部坐标系，/>

表示在全局坐标系中的第i个特征点；

在第t+1个规划周期，根据接受到的左车道线或右车道线和车道宽度，把左车道线或右车道线平移到车道中心线的位置，计算k个局部坐标系特征点，记为

其中l表示局部坐标系，并根据第t+1个规划周期的本车全局定位信息把/>

中的每一个点转换到局部坐标系，得到的点集记为/>

针对

和/>

中的每一个点对第i个点求欧式距离，记为d_i，把所有k对点的距离求和得到/>

如果D超过阈值h，则判定该车道线质量为差，并且丢弃该车道线数据；否则判定该车道线质量为好，保留该车道线数据。

4.如权利要求2所述的采用五次曲线路径规划方法的车道中心线维护方法，其特征在于所述的步骤2具体如下：若左车道线和右车道线判定质量均为差，则丢弃左右车道线，根据点集

采用最小二乘法拟合车道中心线曲线y＝f_t+1(x)；

所述的步骤3具体如下：根据车道中心线曲线y＝f_t+1(x)，给定轨迹规划目标点x方向坐标x_d，计算目标点y方向坐标y_d＝f_t+1(x_d)，并根据目标点(x_d，y_d)，采用所述的五次曲线路径规划方法规划出局部轨迹。

Images