参考线平滑-FemPosDeviation-OSQP

FemPosDeviation参考线平滑方法是离散点平滑方法，Fem是Finite element estimate的意思。

1. 优化目标

1.1 平滑性

参考线平滑的首要目标当然是平滑性，使用向量的模$|\overrightarrow{P_2{P}_2^{\prime }}|$来表示，显然$|\overrightarrow{P_2{P}_2^{\prime }}|$越小，三个点$P_1,P_2,P_3$越接近一条直线，越平滑。
$$J_{smooth}=|\overrightarrow{P_2{P}_2^{\prime }}|=|\overrightarrow{P_2{P}_1}+\overrightarrow{P_2{P}_3}|=|(x_1-x_2,y_1-y_2)+(x_3-x_2,y_3-y_2)|=\sqrt{(x_1+x_3-2x_2{)}^2+(y_1+y_3-2y_2{)}^2}\text{\hspace{1em}(1-1)}$$

Jsmooth=[x1,y1,x2,y2,x3,y3][10−2010010−201−2040−200−2040−210−2010010−201][x1,y1,x2,y2,x3,y3]T(1-2) J_{smooth} = [x_1, y_1, x_2, y_2, x_3, y_3] \left[\begin{matrix} 1 & 0 & -2 & 0 & 1 & 0 \\ 0 & 1 & 0 & -2 & 0 & 1 \\ -2 & 0 & 4 & 0 & -2 & 0 \\ 0 & -2 & 0 & 4 & 0 & -2 \\ 1 & 0 & -2 & 0 & 1 & 0 \\ 0 & 1 & 0 & -2 & 0 & 1 \end{matrix}\right] [x_1, y_1, x_2, y_2, x_3, y_3]^T \tag{1-2} Jsmooth​=[x1​,y1​,x2​,y2​,x3​,y3​] ​10−2010​010−201​−2040−20​0−2040−2​10−2010​010−201​ ​[x1​,y