离散点平滑

基于优化的离散点平滑

模型检测到的车道线，最终的输出的是车道线的点坐标​(x,y)，这些点能够大概描述车道线在画面中的位置，但是如果想要拟合出车道线的高阶方程，可能还是不够平滑，一些离散点会影响拟合的效果。

基于优化的离散点平滑.png

优化变量：车道线点坐标​(x_i,y_i)

优化目标有三个，分别是平滑度、长度和相对参考点的偏移量。因此目标函数可以设计为：

cost=cost_1+cost_2+cost_3

其中​cost_1是平滑度代价，​cost_2是长度代价，​cost_3是相对参考点偏移量代价。

cost_1=\sum_{i=1}^{n-1}(x_{i-1}+x_{i+1}-2\times x_i)^2+(y_{i-1}+y_{i+1}-2\times y_i)^2
cost_2=\sum_{i=0}^{n-1}(x_{i}-x_{i+1})^2+(y_{i}-y_{i+1})^2
cost_3=\sum_{i=0}^{n-1}(x_{i}-x_{i_{ref}})^2+(y_{i}-y_{i_{ref}})^2

​cost_1可以理解为下图中向量​\overrightarrow {P_1P_0}和​\overrightarrow {P_1P_2}相加的结果，也就是新的向量​\overrightarrow {P_1P_3}的模。这个值越小，意味着这三个点的连线越平直，那么这条线也就更加平滑。

​cost_2可以理解为点之间距离的平方和。

​cost_3可以理解为优化后离散点相对原始参考点偏移距离平方之和。

约束条件

位置约束

即偏移量不应太大，应该被限制在一定范围内。

x_{i_{ref}} - x_l \leq x_i \leq x_{i_{ref}}+x_r
y_{i_{ref}} - y_l \leq y_i \leq y_{i_{ref}}+y_r

曲率约束

此外还需要考虑到曲率的约束，车道线的弯曲程度往往不会太大。就算是较大曲率的弯道，车也转不过去...在自动驾驶路径规划中，一般会用车辆的转弯半径作为曲率的约束。

假设​P_0，​P_1，​P_2三点在一个圆上面，当​\theta_1很小时，向量​\overrightarrow {P_1P_0}和​\overrightarrow {P_1P_2}的模 ​\Delta s 近似于弧长，因此：

\theta_1=\frac{\Delta s}{R}
\theta_2=\frac{\pi-\theta_1}{2}

坐标系转换

车道线检测结果在模型输出中是像素中的点坐标，但是控制模块是不能直接使用的，需要把坐标从像素坐标系转换至世界坐标系。

曲线拟合

幂函数可以逼近任意函数。

f(x)=\sum^{N}_{i=0}{k_i}{x^2}

上式中，N代表多项式系数，一般 ​N=5。

当​N=5时，多项式为：

f(x)=k_0+k_1x+k_2x^2+k_3x^3+k_4x^4+k_5x^5

最小二乘法（Least Square Method，LSM）

最小二乘法通过最小化误差（也叫残差）的平方和来寻找最优函数匹配。对于 ​m 维数据集​P(x,y)，​P内的数据点​P_i(x_i,y_i)有：

f(x_i)=k_0+k_1x_i+k_2x_i^2+k_3x_i^3+k_4x_i^4+k_5x_i^5

其中 ​i 是样本维度，​P(x,y)内的点可以是相同的任意维度。

则​P(x,y)内的所有点误差平方和为：

R^2=\sum_{i=1}^{m}[f(x_i)-y_i]^2

求解目标：函数的系数 ​k_j 应使误差平方和 ​R^2 取得极小值。此处的​j指的是多项式各项系数的下标，​j=(0,1,2,...n)。

代数形式

对于最优系数来说，其误差平方和 ​R^2 与多项式系数 ​k_j 的偏导数满足：

\frac{\partial(R^2)}{\partial k_j}=0

即

\sum_i^m\frac{\partial[k_0+k_1x_i+k_2x_i^2+k_3x_i^3+...+k_ix_i^n-y_i]^2}{\partial{k_j}}=0

即

\sum_i^m[2(k_0+k_1x_i+k_2x_i^2+k_3x_i^3+...+k_ix_i^n-y_i)x_i^j]=0

比如当​n=2时，方程为​f(x)=y=k_0+k_1x

R^2(k_0,k_1)=\sum_{i=1}^{2}[(k_0+k_1x_i)-y_i]
\frac{\partial(R^2)}{\partial{k_0}}=2\sum_{i=1}^{2}[(k_0+k_1x_i)-y_i]=0
\frac{\partial(R^2)}{\partial{k_1}}=2\sum_{i=1}^{2}[(k_0+k_1x_i)-y_i]x_i=0
2 k_1 + k_0\sum_{i=1}^{2}x_i =\sum_{i=1}^{2}y_i
k_0\sum_{i=1}^{2}x+k_1 \Sigma_{i=1}^{2}x_i^2 = \sum_{i=1}^{2}x_iy_i

矩阵形式

上面内容写为矩阵格式就是：

X = \begin{bmatrix} m & \Sigma_{i=1}^{m}x_i & \Sigma_{i=1}^{m}x_i ^2 & \cdots &\Sigma_{i=1}^{m}x_i ^n \\ \\ \Sigma_{i=1}^{m}x_i & \Sigma_{i=1}^{m}x_i ^2 & \Sigma_{i=1}^{m}x_i ^3 & \cdots & \Sigma_{i=1}^{m}x_i ^{n+1}\\ \\ \vdots &\vdots &\vdots & \ddots & \vdots \\ \\ \Sigma_{i=1}^{m}x_i^{n} & \Sigma_{i=1}^{m}x_i^{n+1} & \Sigma_{i=1}^{m}x_i^{n+2} & \cdots & \Sigma_{i=1}^{m}x_i ^{2n} \end{bmatrix}
K= \begin{bmatrix} k_0\\ \\ k_1\\ \\ \vdots\\ \\ k_n \\ \end{bmatrix}
y= \begin{bmatrix} \Sigma_{i=1}^{m}y_i\\ \\ \Sigma_{i=1}^{m}x_iy_i\\ \\ \vdots\\ \\ \Sigma_{i=1}^{m}x^ny_i \end{bmatrix}

因此有​XK = y ​\rightarrow ​K=X^{-1}y，​K就是多项式系数向量，​X和​y是数据集中的点。

参考文章

[1] https://blog.csdn.net/i_chaoren/article/details/79822574
[2] https://zhuanlan.zhihu.com/p/268884807
[3] https://mathworld.wolfram.com/LeastSquaresFitting.html
[4] https://zhuanlan.zhihu.com/p/342740447