appollo-参考线平滑设定

参考线平滑设定
from https://github.com/ApolloAuto/apollo/blob/master/docs/specs/reference_line_smoother_cn.md

二次规划（QP）+样条插值

1. 目标函数
   1.1 分段寻路路径
   将寻路路径划分为 n 段，每段用2个多项式表示：

$$x=f_i(t)=a_{i0}+a_{i1}\ast t+a_{i2}\ast t^2+a_{i3}\ast t^3+a_{i4}\ast t^4+a_{i5}\ast t^5$$
$$y=g_i(t)=b_{i0}+b_{i1}\ast t+b_{i2}\ast t^2+b_{i3}\ast t^3+b_{i4}\ast t^4+b_{i5}\ast t^5$$
1.2 定义样条段优化目标函数

$$cost=\sum _{i=1}^n(\underset{0}{\overset{t_i}{\int }}(f_i^{\prime \prime \prime }{)}^2(t)dt+\underset{0}{\overset{t_i}{\int }}(g_i^{\prime \prime \prime }{)}^2(t)dt)$$

1.3 将开销（cost）函数转换为QP公式
QP公式：

$$\begin{gathered} \frac{1}{2}\cdot x^T\cdot H\cdot x+f^T\cdot x \\ s.t.LB\le x\le UB \\ {A}_{eq}x=b_{eq} \\ Ax\le b \end{gathered}$$
2 约束条件
2.1 平滑节点约束
该约束的目的是使样条的节点更加平滑。假设两个段$se{g}_k$ 和$se{g}_{k+1}$互相连接，且$se{g}_k$的累计值 s 为$s_k$。计算约束的等式为：

$$f_k(s_k)=f_{k+1}(s_0)$$
同样地，该公式也适用于下述等式：

$$\begin{gathered} f_k^{\prime }(s_k)=f_{k+1}^{\prime }(s_0) \\ {f}_k^{\prime \prime }(s_k)=f_{k+1}^{\prime \prime }(s_0) \\ {f}_k^{\prime \prime \prime }(s_k)=f_{k+1}^{\prime \prime \prime }(s_0) \\ {g}_k(s_k)=g_{k+1}(s_0) \\ {g}_k^{\prime }(s_k)=g_{k+1}^{\prime }(s_0) \\ {g}_k^{\prime \prime }(s_k)=g_{k+1}^{\prime \prime }(s_0) \\ {g}_k^{\prime \prime \prime }(s_k)=g_{k+1}^{\prime \prime \prime }(s_0) \end{gathered}$$
2.2 点采样边界约束
在路径上均匀的取样 m 个点并检查这些点的预定义边界。

$$\begin{gathered} f_i(t_l)-x_l<boundary \\ {g}_i(t_l)-y_l<boundary \end{gathered}$$