自动驾驶相关算法之车道线检测及循线行驶方案

视觉循轨迹的控制方法中，常用的控制量主要有两个量，横向距离偏差和航向角偏差。

假设当前车辆的状态$S(t) = \begin{pmatrix} x_t , y_t, \theta_t, v_t \end{pmatrix}$，其中$x_t, y_t$是车辆的位置，$\theta_t$是车辆的航向角，$v_t$是车辆的速度。设$c(cx, cy, \theta_c)$是参考轨迹上距离当前车辆位置最近的一点，则有：


? 图1. 车辆位置与参考轨迹观测量及控制量示意图。

1. 横向距离偏差(eod/ $e_{fa}$/$e_d$)： $ eod = e_{fa} = e_d = \sqrt{(cx- x_t)^2 + (cy-y_t)^2} $
2. 车辆航向角偏差(eoa/$\theta_e$): $ eoa=\theta_e = \theta_c - \theta_t $

控制方法 Stanley method

$ \delta(t) = \theta_e(t) + tan^{-1}(\frac{ke_{fa}(t)} {v(t)}) $

其中，$k$是调节系数。$\delta(t)$由两个部分决定，一个是当前的航向偏差，一个是横向距离。距离越大，调节速度越快。

低速情况下，车辆运动假设按照自行车模型，不考虑侧偏滑动，车辆状态的增量方程$\dot{S(t)}$有： $ \begin{bmatrix} \dot{x_t} \ \dot{y_t} \ \dot{\theta_t} \ \dot{\delta_t} \end{bmatrix} = \begin{bmatrix} cos(\theta_t) \ sin(\theta_t) \ \frac{tan(\delta_t)} {L} \ 0 \end{bmatrix} v_t \quad + \quad \begin{bmatrix} \0 \0 \0 \1 \end{bmatrix} \dot \delta $ 其中$S(t+1) = S(t) + \dot{S(t)} \Delta t $。

基于状态方程，并且做近似假设(方向变动非常小)，可以推算出$eod, eoa$的变化量： $ \begin{bmatrix} \dot{e_d(t)} \ \dot{\theta_e(t)} \ \dot{\delta(t)}

\end{bmatrix} = \begin{bmatrix} 0 \quad v_t \quad 0 \

0 \quad 0 \quad -\frac{v_t}{L} \ 0 \quad 0 \quad 0 \end{bmatrix} \begin{bmatrix} e_d(t) \ \theta_e(t) \ \delta_t(t) \end{bmatrix} \quad + \quad \begin{bmatrix} 0 \ 0 \ 1 \end{bmatrix} \dot{\delta(t)} $ 所以可以根据控制量计算出下一时刻的偏差量。

快速推算，如果按照控制周期0.2s算，速度在2m/s，距离只跟上一时刻的航向角偏差有关，所以，单个周期内，变化量不超过，$0.4\theta_e(t)$，考虑到误差控制在5度以内，实际距离变化不超过4cm。航向角的增量只跟当前输入的$\delta(t)$有关，可以估算得到，实际方差变化，不超过$\delta(t)$。

所以，我们可以确定基于Stanley方法，误差变化主要取决于速度和控制角度，控制角度越小，则实际变化量越小，则控制效果非常平滑，并且不存在明显抖动。

视觉循车道中的应用

主要问题：

? 如何寻找参考轨迹，并计算横向距离误差和航向角偏差？

参考轨迹计算

由于车辆一开始是保持在车道内，可以基于车道线检测，计算车道中心线作为实际的参考轨迹。

坐标定义：相机坐标系 x是图片的宽，y是图片的高度，通常照片底部y取值为0。 车体坐标系： x指向右，y指向前

相机坐标系到车体坐标系的变换： 相机坐标中此处是以俯视图做计算，所以只需要考虑像素与真实距离之间的变换关系即可，即横向一个像素代表的距离和纵向一个像素代表的距离

1. 对检测得到的每一条车道线，分别做多项式拟合(通常是二阶或者是三阶多项式)，主要是平滑车道线，可以用于计算车道线曲率等；
2. 根据计算得到的两条曲线，采样合成车道中心线
3. 假设左车道线上一点($y_i$, $x_{i}^l$)， 右车道线上一点($y_i$, $x_i^r$)，中心线上一点为$(y_i, \frac{ (x_i^l + x_i^r)} {2})$，由采样的n个点，拟合得到车道中心线。
4. 将车道中心线变换到机体坐标系上考虑(以图片的底部中点做为原点，然后乘以距离与像素的比值)。

注：计算得到的车道中心线的计算过程一定是在俯视图下计算得到

横向距离误差和航向角偏差计算

在俯视图下，摄像头所在位置定义为原点，简易计算，$y=0$时，中心线上的点$p(x_0, 0)$作为最近点，则有: $ e_d = | x_0| \ \theta_e = atan(dp) - \frac{\pi} {2} $ 其中$dp$是$p$的导数。

关键：计算高精度的航向偏差值，距离偏差值，换言之，需要低方差数据。

实验结果

此处主要计算横向距离(eod)和航向角偏差(eoa)的计算误差分布。对于这两个值主要关心其方差分布，不考虑其均值，只要方差分布比较小，其精度才会比较高。

实验测量条件

仿真环境下，对同一直线路段测试，该部分路段，不下发任何角度和速度控制指令(保证视觉检测部分没有明显变化)，测试次数12次。

对比12次实验数据，航向角的均值和误差值，均值单位是度，可以看出由于未做其他控制，度数应该比较稳定，但是检测得到的结果上来看，误差和均值变化都比较大。

对每一组数据，分别对比，由于未控制方向，所以计算得到的方向叫应该比较一致，并且方差应当比较小，这里可以看出得到的方差比较大，也就是检测得到的航向偏差值不稳定；

这里是对比12次实验过程的横向误差值，误差值平均值比较小，但是方差变化浮动比较大，也就是检测值在距离上也表现出不稳定；

具体几组数据的分析

Todo 这里弧度转成度表示

这是具体某一次开始的检测，对比法线第一次检测和第二次检测，图片之间的事件差为1.6s左右，角度值变化了3度左右，但是这个过程中，并没有控制，也即是检测的偏差值比较大。

同样在最后几帧，检测得到的弧度制变化非常大，基本转了2.8度。

结论

控制方法上不存在问题，主要原因在检测结果的不稳定上。

针对检测结果的不稳定，有两种解决方式，

1. 改进检测算法，目前检测算法上还有大幅度可以改进的点，从检测结果的稳定性上来考虑。
2. 基于检测结果以及误差的增量模型做卡尔曼滤波融合，平滑检测结果。但是需要解决两个问题，一个是初值的设定(设置为0是否可靠？不同场景衔接过程中如何保证初值的可靠性)，一个是方差的衡量，目前可以通过多次实验设定一个值。除此之外，应用卡尔曼滤波，还有一个重要的问题，基于模型推算是存在误差的，误差会累计下来，所以必须要提供一种方式能够纠正推算误差，这种纠正误差来源于检测结果，也就是说检测结果的误差必须是高精的(低方差)