一种基于Mobileye提供车道线方程的后向车道线推算方法与流程

本发明属于车道线拟合方法技术领域，特别涉及一种基于mobileye摄像头传感器提供车道线方程的后向车道线推算方法。

背景技术：

车道线作为基本的交通标志线，是汽车行驶的约束与指示。在智能车研究中，车道线是智能车行驶轨迹规划的重要来源，车道线无论是对于车道保持、碰撞预警等辅助驾驶功能，还是360°环境感知、构建安全行驶区域都是一项关键技术。现有的车道线拟合技术多数仅考虑前向车道线的构建，但是只有前向车道线不足以描述完整车道的几何形状，结合后向车道线的构建是智能车全方位环境感知的基础，可辅助智能车得知后向来车所在车道、以及运动趋势(车道保持/换道)，为智能车的运动决策提供更多的信息。

《stochasticroadshapeestimation》中用到的是一种特殊的车道线模型，它将车道线的曲率和曲率变化率作为模型，但是该方法对不同环境的适应性不够理想；《anewapproachforlanedepartureidentification》由于所选模型的局限性，算法都只能检测直道，而不能适应弯道的检测；上述提到的方法中都有各自的局限性，且都只完成了前向车道线的拟合，缺少对后向车道线的描述。

技术实现要素：

为了克服上述问题，本发明提供一种基于mobileye提供车道线方程的后向车道线推算方法，采取ctrv(恒速度恒转角速度)模型，将mobileye(摄像头传感器)提供的历史前向车道线和上一时刻计算的后向车道线，经过坐标变换转移到当前车辆坐标系下，利用最小二乘法实现后向车道线方程推算。

为了实现上述目的，本发明提供如下技术方案：

一种基于mobileye提供车道线方程的后向车道线推测方法，该方法包括以下步骤：

步骤一、建立ctrv车辆运动学模型：

步骤二、基于ctrv车辆运动学模型的车道线坐标变换：

根据mobileye提供的前向车道线方程形式定义车辆坐标系下的后向车道线方程，基于步骤一建立的ctrv车辆运动学模型得到上一时刻车辆坐标系和当前时刻车辆坐标系的位置和方向关系，利用此关系将上一时刻车辆坐标系中的前向车道线和定义的后向车道线离散化，然后通过坐标变换计算当前时刻车辆坐标系中的后向车道线轨迹点，结合当前时刻mobileye提供的车辆坐标系下的前向车道线离散点，作为后向车道线方程拟合的输入信息；

步骤三、利用最小二乘法拟合后向车道线方程：

基于步骤二得到当前时刻的后向车道线轨迹点和前向车道线离散点，利用最小二乘法将当前时刻的后向车道线轨迹点和前向车道线离散点拟合为定义的后向车道线方程，实现后向车道线的有效求解。

步骤四、根据结构化道路中车道线平行的规律，对当前时刻质量差的后向车道线加以平行约束。

所述步骤一建立的ctrv车辆运动学模型如下：

其中，k为上一时刻，k+1为当前时刻，x(k)和x(k+1)分别表示k时刻和k+1时刻车辆坐标系的状态变量，t为k时刻与k+1时刻的时间差；

其中：当前时刻车辆坐标系的状态变量为x(k+1)，分别为k+1时刻车辆坐标系下目标点的横纵向坐标，vk+1表示当时刻k+1的车辆行驶速度；θk+1表示k+1时刻的车辆航向角；ωk+1表示k+1时刻的车辆横摆角速度；

上一时刻车辆坐标系的状态变量为x(k)，分别为k时刻车辆坐标系下目标点的横向、纵向坐标，vk表示k时刻车车辆速度；θk表示k时刻的车辆航向角；ωk表示k时刻的车辆横摆角速度。

所述步骤二，计算基于ctrv车辆运动学模型的上一时刻和当前时刻两个时刻的车辆坐标系间位置和角度关系：

两个时刻车辆坐标系间的角度差为：△θ＝ωkt

两个时刻车辆坐标系间的位置差为：

其中△px和△py分别为k时刻与k+1时刻的车辆坐标系间的横、纵向水平位移差；

mobileye提供的车辆坐标系下的k时刻前向车道线方程为：

mobileye提供的车辆坐标系下的k+1时刻前向车道线方程为：

根据mobileye提供的前向车道线方程形式定义的k时刻车辆坐标系下的后向车道线方程为：

其中，为k时刻前向车道线方程的常数项、一次项、二次项和三次项，为k时刻后向车道线方程的常数项、一次项、二次项和三次项，为k+1时刻前向车道线方程的常数项、一次项、二次项和三次项，x表示车辆坐标系纵向即x轴的自变量；

将k时刻的前向车道线方程和定义的后向车道线方程分别离散化后得到离散化的车道线的坐标点其中k时刻前向和后向车道线的离散点坐标均记为根据下标i的范围判断是前向还是后向的车道线离散点；

将按照下式进行坐标平移变换，得到上一时刻即k时刻平移后的前、后向车道线的坐标点坐标平移变换公式如下：

然后再对进行坐标旋转变换，即将k时刻车辆坐标系下的前、后向车道线离散点变换到k+1时刻车辆坐标系下，为该点坐标均出现在k+1时刻车辆坐标系的后方，为k+1时刻车辆坐标系下的后向车道线离散点坐标，坐标旋转变换公式如下：

坐标旋转变换：

将k+1时刻的后向车道线的坐标点与当前时刻即k+1时刻mobileye提供的前向车道线方程离散点两个点坐标整理至一个点坐标矩阵中，得到其中，n＝viewrange+2vkt+1，viewrange是mobileye摄像头传感器提供车道线方程的一个参数，表示探测车道线的长度，或x轴方向的距离。

所述步骤三中根据mobileye提供的前向车道线方程形式定义的k+1时刻车辆坐标系下的后向车道线方程为:其中b0,b1,b2,b3是后向车道线方程的待定参数，基于步骤二求得的k+1时刻车辆坐标系下的车道线离散点，即步骤二得到的点坐标矩阵利用最小二乘法按下式求解k+1时刻后向车道线方程的参数b0,b1,b2,b3：

b＝a^-1·y

其中：

所述步骤四中当前时刻某车道左侧后向车道线方程的参数为：bl0,bl1,bl2,bl3，该车道右侧后向车道线方程的参数为：br0,br1,br2,br3，若该车道左侧车道线方程的质量更好，右侧车道线方程的质量较差时，则根据线族平行原理使用bl0,bl1,bl2,bl3参数对br0,br1,br2,br3加以平行约束，即：

|bl1-br1|≤d1

|bl2-br2|≤d2

|bl3-br3|≤d3

其中d1.d2,d3分别为对一次项、二次项、三次项系数容许误差的阈值，具体为d1＝0.1,d2＝4×10^-4,d3＝10^-7，当右车道线方程的参数不满足约束条件时，则加以强制平行约束，令：

br1＝bl1

br2＝bl2

br3＝bl3

若该车道右侧车道线方程的质量更好，左侧车道线方程的质量较差时，则根据线族平行原理使用br0,br1,br2,br3参数对bl0,bl1,bl2,bl3加以平行约束，即：

|br1-bl1|≤d1

|br2-bl2|≤d2

|br3-bl3|≤d3

其中d1.d2,d3分别为对一次项、二次项、三次项系数容许误差的阈值，具体为d1＝0.1,d2＝4×10^-4,d3＝10^-7，当左车道线方程的参数不满足约束条件时，则加以强制平行约束，令：

bl1＝br1

bl2＝br2

bl3＝br3。

本发明的有益效果：

1.本发明建立了ctrv车辆运动学模型，解决了车辆横摆运动对描述车道线方程产生的运动学影响。

2.本发明采用了上一时刻和当前时刻的多段车道线方程信息，增加了算法的鲁棒性。

3.本发明基于结构化车道线相互平行的理论，使用质量较好的后向车道线方程对较差的车道线方程参数加以约束，使得到的后向车道线符合实际情况。

附图说明

图1为本发明方法流程图。

图2为本发明建立的ctrv模型示意图。

图3为前后两个时刻车辆坐标系下车道线与坐标系位置关系示意图。

图4为上一时刻车道线坐标变换后的离散点。

图5为本发明实施例推算出的四条后向车道线示意图。

具体实施方式

如图1所示，一种基于mobileye提供车道线方程的后向车道线推测方法，该方法包括以下步骤：

步骤一、建立ctrv车辆运动学模型：

由于车道线方程的描述与车辆坐标系紧密相关，因此考虑到车辆自身平移和旋转运动对车辆坐标系的影响，选用基于ctrv模型描述车辆运动，根据车辆运动学定律建立ctrv模型。

步骤二、基于ctrv车辆运动学模型的车道线坐标变换：

根据mobileye提供的前向车道线方程形式定义车辆坐标系下的后向车道线方程，基于步骤一建立的ctrv车辆运动学模型得到上一时刻车辆坐标系和当前时刻车辆坐标系的位置和方向关系，利用此关系将上一时刻车辆坐标系中的前向车道线和定义的后向车道线离散化，然后通过坐标变换计算当前时刻车辆坐标系中的后向车道线轨迹点，结合当前时刻mobileye提供的车辆坐标系下的前向车道线离散点，作为后向车道线方程拟合的输入信息；

步骤三、利用最小二乘法拟合后向车道线方程：

基于步骤二得到当前时刻的后向车道线轨迹点和前向车道线离散点，考虑到结构化道路的车道线连续不突变的特性，利用最小二乘法将当前时刻的后向车道线轨迹点和前向车道线离散点拟合为三次多项式描述的后向车道线方程，实现后向车道线的有效求解。

步骤四、根据结构化道路中车道线平行的规律，对当前时刻质量差的后向车道线加以平行约束。

如图2所示，所述步骤一建立的ctrv车辆运动学模型如下：

ctrv模型假设对象以恒定的速度和横摆角速度行驶，瞬时运动可视为匀速圆周运动，因此可以使用该模型描述车辆的平移和旋转运动对车辆坐标系变化的影响。如图二所示，为ctrv模型原理图，车辆运动学可建模过程为：

选取ctrv车辆运动学模型的状态变量

其中px、py分别表示车辆坐标系下目标点的横向、纵向距离即横纵向坐标，单位：m(米)，用于描述不同时刻车辆坐标系之间的位置关系；v表示车辆速度，单位：m/s(米/秒)；θ表示车辆航向角，单位：rad(弧度)，用来描述车辆坐标系的方向；ω表示车辆横摆角速度，单位：rad/s(弧度/秒)；

由于假设对象以恒定速度和横摆角速度运动，所以v和ω的变化率为零，各个量的变化率如下所示：

对状态变量的变化率进行积分，则表示一段时间内各状态变量的变化情况，假设离散的时间点k和连续的时间值tk相关，离散的时间点k+1和连续的时间值tk+1相关，tk+1与tk之间的时间差为t；具体地说：相当于有一条时间轴表示连续流动的时间，在时间轴上等距离画刻度标上刻度值，就相当于把连续的时间用离散的形式描述，从零向右查k个点就是时间点k，它的刻度就是tk，时间点k+1的刻度值就是tk+1，tk和tk+1表示从零到k和k+1时刻经过的时间；

其中，k为上一时刻，k+1为当前时刻，x(k)和x(k+1)分别表示k时刻和k+1时刻车辆坐标系的状态变量，t为k时刻与k+1时刻的时间差。

分为ωk＝0和ωk≠0两种情况，积分求解得到建立的ctrv车辆运动学模型：

其中ωk为k时刻的车辆横摆角速度，vk为k时刻的车辆行驶速度，θk为k时刻的车辆航向角。

ctrv模型的系统状态方程可表示为：x(k+1)＝f[x(k)]

其中：f表示x(k)与x(k+1)的函数对应关系。

如图3所示，所述步骤二，基于上述ctrv车辆运动学模型可分析得到前后两个时刻的车辆坐标系位置和角度关系，然后可计算出前后两时刻车辆坐标系之间的坐标变换关系，将上一时刻坐标系下的车道线离散点通过变换到当前车辆坐标系下，具体包括：

基于ctrv车辆运动学模型的前后两个时刻的车辆坐标系间位置和角度关系求解过程如下：

当前车辆坐标系的状态变量为：

上一时刻车辆坐标系的状态变量为：

前后两个时刻车辆坐标系间的角度差为：△θ＝θk+1-θk(1.5)

代入式(1.3)和(1.4)，得到：

△θ＝ωkt(1.6)

前后两个时刻车辆坐标系间的位置差为：

将上式(1.3)和(1.4)代入式(1.7)，由于我们只计算在车辆坐标系下的位置差，所以令θk＝0得到：

其中△px和△py分别为k与k+1两时刻的车辆坐标系间的横纵向水平位移差，单位：m(米)；

设在k时刻车辆坐标系下的车道线坐标点为(xi,yi)，单位为：米(m)；这里的(xi,yi)不是特指前向或者后向车道线的离散点，只是说明点坐标从k时刻车辆坐标系转换到k+1时刻车辆坐标系的变换过程，转换到k+1时刻车辆坐标系下的对应坐标点为(xi,yi)，单位为：米(m)，根据上面所求位置差和角度差，可得到坐标点的坐标转换计算公式为：

水平变换公式：

旋转变换公式：

首先对(xi,yi)做水平位移变换，得到中间量(xtrans,ytrans)，然后再对中间量做旋转变换得到当前时刻(k+1时刻)车辆坐标系下的对应坐标点(xi,yi)。

mobileye(摄像头传感器)提供的前向车道线方程为：y＝c0+c1x+c2x²+c3x³

其中，c0、c1、c2、c3为前向车道线方程的常数项、一次项、二次项和三次项，c0表示的几何意义为车道线方程与y轴的交点(y轴的截距)，c1表示x＝0处的车道线方程斜率，c2可近似表示x＝0处的车道线方程曲率，c3可近似表示x＝0处的车道线方程曲率变化率。

根据分段车道线模型建立的后向车道线方程为：yback＝b0+b1x+b2x²+b3x³

其中，b0、b1、b2、b3为前向车道线方程的常数项、一次项、二次项和三次项，b0表示的几何意义为车道线方程与y轴的交点(y轴的截距)，b1表示x＝0处的车道线方程斜率，b2可近似表示x＝0处的车道线方程曲率，b3可近似表示x＝0处的车道线方程曲率变化率。x表示车辆坐标系纵向(x轴)的自变量。

随着车辆的运动(即从k时刻车辆坐标系变为k+1时刻坐标系)，k时刻车辆坐标系下的前向车道线会有一部分落在k+1时刻车辆坐标系的x轴负向(即车辆后方)，但是落在k+1时刻车辆坐标系的x轴负向的范围比较小(大约是vkt米)，而且只用一段车道线的信息会导致算法的鲁棒性较差；我们注意到k时刻车辆坐标系下计算得到的后向车道线本就在k时刻的车辆后方，车辆向前运动，也即k时刻的后向车道线也在k+1时刻(当前时刻)的车辆后方，为我们计算后向车道线提供了更多的信息，另外由于结构化车道的车道线是连续的，也就是说当前时刻车辆前后的车道线是一条连续的曲线，所以也可以在上述两个后向车道线信息的基础上，额外加上当前时刻得到的前向车道线信息来共同用于当前时刻的后向车道线方程的计算，综上所述用于当前时刻车辆坐标系下的后向车道线方程的推算，一共使用了k时刻车辆坐标系下的前向和后向车道线方程(需要通过坐标变换计算到k+1时刻车辆坐标系下)，以及k+1时刻的前向车道线方程。

mobileye提供的k+1时刻车辆坐标系下的前向车道线方程为：

k时刻车辆坐标系下的前向和后向车道线方程分别为：

和

其中，表示mobileye提供的前向车道线方程在k时刻车辆坐标系下的系数，为k+1时刻前向车道线方程的常数项、一次项、二次项和三次项，表示k时刻车辆坐标系下计算的后向车道线方程系数。将其离散化，即令x＝xstart:1:xend，表示从xstart到xend间隔1m取一个点，xstart表示将车道线方程离散化的起点，xend表示将车道线方程离散化的终点，代入对应的方程yk和得到离散化的车道线上的坐标点这个坐标点是车道线的统一表达方式，既表示前向车道线，也表示后向车道线的离散点，后文会根据xstart＝0,xend＝vkt还是xstart＝-vkt,xend＝0来区分前后向车道线；为了避免计算的点存在大量重复(减少计算量)，对k时刻前向车道线方程的离散范围设置为：xstart＝0,xend＝vkt，对k时刻后向车道线方程的离散范围设置为：xstart＝-vkt,xend＝0，上述离散化后得到坐标点同理，令xstart＝1和xend＝viewrange代入对应方程yk+1，viewrange是mobileye摄像头传感器提供车道线方程的一个参数，表示探测车道线的长度(或x轴方向的距离)，得到k+1时刻车辆坐标系下前向车道线方程离散点

经过坐标变换后得到k+1时刻下的对应坐标点为这个坐标点是车道线的统一表达方式，表示k时刻坐标系下的前向或后向车道线离散点经过坐标变换后得到的k+1时刻车辆坐标系下的对应点；结合k+1时刻得到的前向车道线方程离散点将上述两个点坐标整理为一个点坐标矩阵中，得到其中，n＝viewrange+2vkt+1，共同用于k+1时刻车辆坐标系下后向车道线方程的计算。

如图4和图5所示，所述步骤三，基于上述方法求得的k+1时刻车辆坐标系下的车道线离散点坐标矩阵，即步骤二得到的点坐标矩阵利用最小二乘法进行后向车道线参数求解过程，具体包括：

最小二乘法(又称最小平方法)是一种数学优化技术，它通过最小化误差的平方和寻找数据的最佳函数匹配，利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。

最小二乘法是解决曲线拟合问题最常用的方法，其基本思路是：

我们根据分段车道线模型定义后向车道线方程为：

yback＝b0+b1x+b2x²+b3x³(1.11)

其中b0,b1,b2,b3是待定参数，已知一系列点坐标矩阵拟合准则是使n＝viewrange+2vkt+1与的距离δi的平方和最小。

为了寻找待定参数的最优估计值，对于给定的n组观测数据求解目标函数为：

式(1.12)可写为

由多项式求极值的必要条件，得

其中对于b来说m是b的下角标，表示b0,b1,b2,b3，对于x来说，是x的幂指数，表示x～x*2；bj和bm都表示b0,b1,b2,b3，具体表示哪一个参数由下角标m和j确定，为的m次幂，同理；

上式(1.14)用矩阵表示为

令

其中b为状态变量，即待定参数，a为状态方程的系数矩阵，由于a为一个对称正定矩阵，故存在唯一解，且解为：

b＝a^-1·y(1.16)

已知k+1时刻车辆坐标系内前向和后向车道线上的一系列坐标点即可利用最小二乘法求得后向车道线方程的系数b0,b1,b2,b3。

mobileye摄像头传感器最多可提供四条前向车道线方程(自车道左右两条车道线，左邻车道的左侧车道线，右邻车道的右侧车道线，所以是三条车道，共四条车道线，但是每条车道线的质量残次不齐，导致根据质量差的车道线方程求取的后向车道线效果很差；考虑到结构化道路中车道线是相互平行的，所以可以使用质量最好的后向车道线作为参考车道线，对质量差的其他后向车道线加以平行约束，对质量差的后向车道线加以修正，具体如下：

根据曲线平行的理论可知，只改变常数项的曲线族(曲线平移)满足车道线相互平行的条件，例如平面直角坐标系中，斜率为a，截距(常数项)为b的平行线族：

z＝ax+b,b∈(-∞,∞)(1.17)

只有常数项b不相同，且斜率a相同的两条直线相互平行；其中z为平面坐标系的函数名，x为x轴自变量，z＝ax+b举例表示平面直角坐标系中的直线族(若a斜率不变，则直线平行)。

所述步骤四中定义当前时刻本车道左侧后向车道线方程的参数为：bl0,bl1,bl2,bl3，本车道右侧后向车道线方程的参数为：br0,br1,br2,br3，假设本车道左侧车道线方程的质量更好，右侧车道线方程的质量较差，则可根据线族平移原理使用bl0,bl1,bl2,bl3参数对br0,br1,br2,br3加以平行约束：

d1,d2,d3分别为对一次项、二次项、三次项系数容许误差的阈值，根据《道路平面设计》高速路的曲率半径范围一般为200-10000米，根据相应计算可知，车道线方程的斜率范围为0-0.66(车道线方程长度为100时)，曲率范围为10^-5～0.005，曲率变化率范围为0～1.6×10^-6；据此设计阈值为d1＝0.1,d2＝4×10^-4,d3＝10^-7，当右车道线方程的参数不满足约束条件时，则加以强制平行约束：

若该车道右侧车道线方程的质量更好，左侧车道线方程的质量较差时，则根据线族平移原理使用br0,br1,br2,br3参数对bl0,bl1,bl2,bl3加以平行约束，即：

d1.d2,d3分别为对一次项、二次项、三次项系数容许误差的阈值，其中：根据《道路平面设计》高速路的曲率半径范围一般为200-10000米，根据相应计算可知，车道线方程的斜率范围为0-0.66(车道线方程长度为100时)，曲率范围为10^-5～0.005，曲率变化率范围为0～1.6×10^-6；据此设计阈值为d1＝0.1,d2＝4×10^-4,d3＝10^-7，当左车道线方程的参数不满足约束条件时，则加以强制平行约束，令：

bl1＝br1

bl2＝br2

bl3＝br3。

将待测的当前时刻四条后向车道线分别按照步骤一至步骤四进行操作，实现当前时刻四条后向车道线的有效求解。

在本发明中，由于在自车坐标下车道线方程的描述与车辆坐标系紧密相关，考虑到车辆自身平移和旋转运动对车辆坐标系的影响，选用恒速恒转角速度运动模型(constantturnrateandvelocity，简称ctrv模型)来描述自车运动。将车道线的离散点经过坐标转换计算，得到在当前车辆坐标系下的后向车道线离散点，基于最小二乘法拟合方法，同时考虑到结构化道路的车道线连续的特性，利用车道线的平行约束，构建后向车道线方程。

本发明中选用前向和后向车道线分段模型描述车道线，选用回旋线(三次函数方程)，既可以描述直线道路(二次项和三次项系数为零时)，也可对道路设计中的缓和曲线以及弯道进行描述，该方法有很强的适应性，且使用k时刻的前向和后向车道线、以及k+1时刻的前向车道线用于k+1时刻的后向车道线拟合，大大增加了后向车道线的鲁棒性。