论文实现：Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control

1. 多项式螺旋
曲率：
$$\begin{array}{r}\kappa (s)=a_0+a_{1}s+a_2{s}^2+a_3{s}^3+a_4{s}^4+a_5{s}^5\end{array}$$
机器人朝向：
$$\begin{array}{r}\theta (s)=a_{0}s+\frac{a_1{s}^2}{2}+\frac{a_2{s}^3}{3}+\frac{a_3{s}^4}{4}+\frac{a_4{s}^5}{5}+\frac{a_5{s}^6}{6}\end{array}$$
轨迹：
$$\begin{array}{r}x(s)={\int }_0^s\cos (\theta (s))ds\end{array}$$
$$\begin{array}{r}y(s)={\int }_0^s\sin (\theta (s))ds\end{array}$$

2. 边界条件
初始条件:$s=0，x=0,y=0,\theta =0$
结束条件：$s=s_f,x=x_f,y=y_f,\theta ={\theta }_f$
$$\begin{array}{r}{\mathbf{x}}_{\mathbf{b}}={\left[{\mathbf{x}}_{\mathbf{f}}{\mathbf{y}}_{\mathbf{f}}{\theta }_{\mathbf{f}}\right]}^{\mathbf{T}}\end{array}$$
参数：
$$\begin{array}{r}\mathbf{q}={\left[{\mathbf{a}}_0{\mathbf{a}}_1{\mathbf{a}}_2{\mathbf{a}}_3{\mathbf{a}}_4{\mathbf{a}}_5{\mathbf{s}}_{\mathbf{f}}\right]}^{\mathbf{T}}\end{array}$$
边界条件：
$$\begin{array}{r}\mathbf{g}(\mathbf{q})=\mathbf{h}(\mathbf{q})-{\mathbf{x}}_{\mathbf{b}}=\{\begin{array}{l}x(s_f)-x_f=0\\ y(s_f)-y_f=0\\ \theta (s_f)-{\theta }_f=0\end{array}\end{array}$$

待续…