离散点平滑

基于优化的离散点平滑

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

基于优化的离散点平滑

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

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

\textstyle cost=cost_1+cost_2+cost_3

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

\textstyle 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
\textstyle cost_2=\sum_{i=0}^{n-1}(x_{i}-x_{i+1})^2+(y_{i}-y_{i+1})^2
\textstyle cost_3=\sum_{i=0}^{n-1}(x_{i}-x_{i_{ref}})^2+(y_{i}-y_{i_{ref}})^2

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

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

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

约束条件

位置约束

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

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

曲率约束

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

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

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

坐标系转换

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

曲线拟合

多项式可以逼近任意函数。

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

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

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

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

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

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

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

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

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

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

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

代数形式

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

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

即

\textstyle \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

即

\textstyle \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

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

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

矩阵形式

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

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

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

参考文章

[1] 多项式曲线拟合 C++ 实现

[2] 知乎专栏：曲线拟合与最小二乘

[3] Least Squares Fitting - MathWorld

[4] 知乎专栏：最小二乘法原理