参考线平滑 - Fem Pos Deviation

代价函数

共由三部分组成：
平滑代价
长度代价
偏移代价

平滑代价

为三个点组成的两个向量之差模长，越短越平滑。
$$\begin{array}{rl}cos{t}_1=& \sum _{i=1}^{N-1}((x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2)\\ =& {\left[\begin{array}{c}{x}_{i-1}\\ y_{i-1}\\ x_i\\ y_i\\ x_{i+1}\\ y_{i+1}\end{array}\right]}^{\top }\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]\left[\begin{array}{c}{x}_{i-1}\\ y_{i-1}\\ x_i\\ y_i\\ x_{i+1}\\ y_{i+1}\end{array}\right]\end{array}$$

长度代价

$$\begin{array}{rl}cos{t}_2=& \sum _{i=0}^{N-1}((x_i-x_{i+1}{)}^2+(y_i-y_{i+1}{)}^2)\\ =& {\left[\begin{array}{c}{x}_i\\ y_i\\ x_{i+1}\\ y_{i+1}\end{array}\right]}^{\top }\left[\begin{array}{cccc}1& 0& -1& 0\\ 0& 1& 0& -1\\ -1& 0& 1& 0\\ 0& -1& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ x_{i+1}\\ y_{i+1}\end{array}\right]\end{array}$$

偏移代价

$$\begin{array}{rl}cos{t}_2=\sum _{i=0}^N((x_i-x_{i\mathrm{ref}}{)}^2+(y_i-y_{i\mathrm{ref}}{)}^2)=& {\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]}^{\top }\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]+2\left[\begin{array}{c}-x_{i\mathrm{ref}}\\ -y_{i\mathrm{ref}}\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]+const\end{array}$$

总代价

$$cost=\frac{1}{2}\sum _{i=1}^3{w}_{i}cos{t}_i$$
$w_i$是权重。

约束

位置约束（偏移约束）

$$\begin{array}{rl}{x}_i& \in [x_{i\mathrm{ref}}-x_l,x_{i\mathrm{ref}}+x_u]\\ y_i& \in [y_{i\mathrm{ref}}-y_l,y_{i\mathrm{ref}}+y_u]\end{array}$$

曲率约束

如图，假设$P_i,i=0,1,2$三点位于同一个圆上，当曲率越小弧长采样越小时，${\theta }_1$较小，且此时弧长和弦长近似，有
$${\theta }_1=\frac{\mathrm{d}s}{R}$$
根据等腰三角形$O{P}_0{P}_1$的几何关系有
$${\theta }_2=\frac{\pi -{\theta }_1}{2}$$
由于相邻边长相等，故平行四边形$P_0{P}_1{P}_2{P}_3$是菱形，因此有
$$\begin{array}{r}|\overrightarrow{P_1{P}_3}|=2|\overrightarrow{P_{1}C}|=2\mathrm{d}s\cos {\theta }_2=2\mathrm{d}s\sin \frac{{\theta }_1}{2}=\frac{\mathrm{d}{s}^2}{R}=\mathrm{d}{s}^2\kappa \end{array}$$
因此曲率约束
$$(x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2\le (\mathrm{d}{s}^2{\kappa }_{\max }{)}^2,i=1,\cdots ,N-1$$
约束虽然有了，但不是仿射形式，需要线性化。令
$$\begin{array}{rl}{f}_i& =(x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2\\ X_i& ={\left[\begin{array}{cccccc}{x}_{i-1}& y_{i-1}& x_i& y_i& x_{i+1}& y_{i+1}\end{array}\right]}^{\top },i=1,\cdots ,N\end{array}$$
一阶泰勒
$$f_i=f(X_{i\mathrm{ref}})+f^{\prime }(X_{i\mathrm{ref}})(X_i-X_{i\mathrm{ref}})+\cdots ,i=1,\cdots ,N-1$$
其中
$$f^{\prime }(X_i)={\left[\begin{array}{c}2(x_{i-1}+x_{i+1}-2x_i)\\ 2(y_{i-1}+y_{i+1}-2y_i)\\ -4(x_{i-1}+x_{i+1}-2x_i)\\ -4(y_{i-1}+y_{i+1}-2y_i)\\ 2(x_{i-1}+x_{i+1}-2x_i)\\ 2(y_{i-1}+y_{i+1}-2y_i)\end{array}\right]}^{\top }$$
带入曲率约束方程，有
$$f^{\prime }(X_{i\mathrm{ref}})X_i\le (\mathrm{d}{s}^2{\kappa }_{\max }{)}^2-f(X_{i\mathrm{ref}})+f^{\prime }(X_{i\mathrm{ref}})X_{i\mathrm{ref}},i=1,\cdots ,N-1$$