参考线平滑-FemPosDeviation-SQP

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 }}{|}^2=|\overrightarrow{P_2{P}_1}+\overrightarrow{P_2{P}_3}{|}^2=(x_1-x_2,y_1-y_2{)}^2+(x_3-x_2,y_3-y_2{)}^2\text{\hspace{1em}(1-1)}$$

$$J_{smooth}=[x_1,y_1,x_2,y_2,x_3,y_3]\left[\begin{array}{cccccc}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{array}\right][x_1,y_1,x_2,y_2,x_3,y_3{]}^T\text{\hspace{1em}(1-2)}$$

1.2 几何性

平滑后的参考线，希望能够保留原始道路的几何信息，不会把弯道的处的参考线平滑成一条直线。使用平滑后点与原始点的距离来表示。
$$J_{deviation}=|\overrightarrow{P_{r,1}{P}_1}{|}^2+|\overrightarrow{P_{r,2}{P}_2}{|}^2+|\overrightarrow{P_{r,3}{P}_3}{|}^2=(x_1-x_{1,r}{)}^2+(y_1-y_{1,r}{)}^2+(x_2-x_{2,r})$$