论文实现：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}$$

3. 优化问题

minimize：
$$\begin{array}{r}J(\mathbf{q})=\frac{1}{2}{\int }_0^{{\mathbf{s}}_{\mathbf{f}}}[\kappa (\mathbf{q}){]}^2\mathbf{ds}\end{array}$$
subject to:
$$\begin{array}{r}\mathbf{g}(\mathbf{q})=0{\mathbf{s}}_{\mathbf{f}}\mathbf{free}\end{array}$$

4. Hamiltonian

$$\begin{array}{r}\mathbf{H}(\mathbf{q},\lambda )=\mathbf{J}(\mathbf{q})+{\lambda }^{\mathbf{T}}\mathbf{g}(\mathbf{q})\end{array}$$

5. 参数更新方程

看着和牛顿二阶梯度法形式一致：
$$\begin{array}{r}\mathbf{H}\mathbf{\Delta }\mathbf{x}=-{\mathbf{J}}^{\mathbf{T}}\end{array}$$
$$\begin{array}{r}\mathbf{H}=\left[\begin{array}{cc}\frac{{\delta }^2\mathbf{H}}{\delta {\mathbf{q}}^2}(\mathbf{q},\lambda )& \frac{\delta }{\delta \mathbf{q}}\mathbf{g}(\mathbf{q}{)}^{\mathbf{T}}\\ \frac{\delta }{\delta \mathbf{q}}\mathbf{g}(\mathbf{q})& 0\end{array}\right]\end{array}$$
$$\begin{array}{r}\Delta \mathbf{x}=\left[\begin{array}{c}\Delta \mathbf{q}\\ \Delta \lambda \end{array}\right]\end{array}$$
$$\begin{array}{r}{\mathbf{J}}^{\mathbf{T}}=\left[\begin{array}{c}(\frac{\delta }{\delta \mathbf{q}}\mathbf{H}(\mathbf{q},\lambda ){)}^{\mathbf{T}}\\ \mathbf{g}(\mathbf{q})\end{array}\right]\end{array}$$

$$\begin{array}{r}\mathbf{H}=\left[\begin{array}{cc}{\mathbf{H}}_{7x7}& {\mathbf{H}}_{3x7}^{\mathbf{T}}\\ {\mathbf{H}}_{3x7}& 0_{3x3}\end{array}\right]\end{array}$$

$$\begin{array}{rl}{\mathbf{H}}_{7x7}& =\frac{{\delta }^2\mathbf{J}(\mathbf{q})}{\delta {\mathbf{q}}^2}+\frac{{\delta }^2({\lambda }^{\mathbf{T}}\mathbf{g}(\mathbf{q}))}{\delta {\mathbf{q}}^2}\\ & =\frac{{\delta }^2\mathbf{J}(\mathbf{q})}{\delta {\mathbf{q}}^2}+{\lambda }_0\frac{{\delta }^{2}x(s_f)}{\delta {\mathbf{q}}^2}+{\lambda }_1\frac{{\delta }^{2}y(s_f)}{\delta {\mathbf{q}}^2}+{\lambda }_2\frac{{\delta }^2\theta (s_f)}{\delta {\mathbf{q}}^2}\\ & =\left[\begin{array}{ccccccc}{s}_f& \frac{s_f^2}{2}& \frac{s_f^3}{3}& \frac{s_f^4}{4}& \frac{s_f^5}{5}& \frac{s_f^6}{6}& {\kappa }_f\\ \frac{s_f^2}{2}& \frac{s_f^3}{3}& \frac{s_f^4}{4}& \frac{s_f^5}{5}& \frac{s_f^6}{6}& \frac{s_f^7}{7}& s_f{\kappa }_f\\ \frac{s_f^3}{3}& \frac{s_f^4}{4}& \frac{s_f^5}{5}& \frac{s_f^6}{6}& \frac{s_f^7}{7}& \frac{s_f^8}{8}& s_f^2{\kappa }_f\\ \frac{s_f^4}{4}& \frac{s_f^5}{5}& \frac{s_f^6}{6}& \frac{s_f^7}{7}& \frac{s_f^8}{8}& \frac{s_f^9}{9}& s_f^3{\kappa }_f\\ \frac{s_f^5}{5}& \frac{s_f^6}{6}& \frac{s_f^7}{7}& \frac{s_f^8}{8}& \frac{s_f^9}{9}& \frac{s_f^{10}}{10}& s_f^4{\kappa }_f\\ \frac{s_f^6}{6}& \frac{s_f^7}{7}& \frac{s_f^8}{8}& \frac{s_f^9}{9}& \frac{s_f^{10}}{10}& \frac{s_f^{11}}{11}& s_f^5{\kappa }_f\\ {\kappa }_f& s_f{\kappa }_f& s_f^2{\kappa }_f& s_f^3{\kappa }_f& s_f^4{\kappa }_f& s_f^5{\kappa }_f& k_f{k}_f^{\prime }\end{array}\right]\\ & +{\lambda }_0\left[\begin{array}{ccccccc}-C^2(s_f)& -\frac{1}{2}{C}^3(s_f)& -\frac{1}{3}{C}^4(s_f)& -\frac{1}{4}{C}^5(s_f)& -\frac{1}{5}{C}^6(s_f)& -\frac{1}{6}{C}^7(s_f)-& -s_{f}s{\theta }_f\\ -\frac{1}{2}{C}^3(s_f)& -\frac{1}{4}{C}^4(s_f)& -\frac{1}{6}{C}^5(s_f)& -\frac{1}{8}{C}^6(s_f)& -\frac{1}{10}{C}^7(s_f)& -\frac{1}{12}{C}^8(s_f)& -\frac{1}{2}{s}_f^{2}s{\theta }_f\\ -\frac{1}{3}{C}^4(s_f)& -\frac{1}{6}{C}^5(s_f)& -\frac{1}{9}{C}^6(s_f)& -\frac{1}{12}{C}^7(s_f)& -\frac{1}{15}{C}^8(s_f)& -\frac{1}{18}{C}^9(s_f)& -\frac{1}{3}{s}_f^{3}s{\theta }_f\\ -\frac{1}{4}{C}^5(s_f)& -\frac{1}{8}{C}^6(s_f)& -\frac{1}{12}{C}^7(s_f)& -\frac{1}{16}{C}^8(s_f)& -\frac{1}{20}{C}^9(s_f)& -\frac{1}{24}{C}^{10}(s_f)& -\frac{1}{4}{s}_f^{4}s{\theta }_f\\ -\frac{1}{5}{C}^6(s_f)& -\frac{1}{10}{C}^7(s_f)& -\frac{1}{15}{C}^8(s_f)& -\frac{1}{20}{C}^9(s_f)& -\frac{1}{25}{C}^{10}(s_f)& -\frac{1}{30}{C}^{11}(s_f)& -\frac{1}{5}{s}_f^{5}s{\theta }_f\\ -\frac{1}{6}{C}^7(s_f)& -\frac{1}{12}{C}^8(s_f)& -\frac{1}{18}{C}^9(s_f)& -\frac{1}{24}{C}^{10}(s_f)& -\frac{1}{30}{C}^{11}(s_f)& -\frac{1}{36}{C}^{12}(s_f)& -\frac{1}{6}{s}_f^{6}s{\theta }_f\\ -s_{f}s{\theta }_f& -\frac{1}{2}{s}_f^{2}s{\theta }_f& -\frac{1}{3}{s}_f^{3}s{\theta }_f& -\frac{1}{4}{s}_f^{4}s{\theta }_f& -\frac{1}{5}{s}_f^{5}s{\theta }_f& -\frac{1}{6}{s}_f^{6}s{\theta }_f& -{\kappa }_{f}s{\theta }_f\end{array}\right]\\ & +{\lambda }_1\left[\begin{array}{ccccccc}-S^2(s_f)& -\frac{1}{2}{S}^3(s_f)& -\frac{1}{3}{S}^4(s_f)& -\frac{1}{4}{S}^5(s_f)& -\frac{1}{5}{S}^6(s_f)& -\frac{1}{6}{S}^7(s_f)-& s_{f}c{\theta }_f\\ -\frac{1}{2}{S}^3(s_f)& -\frac{1}{4}{S}^4(s_f)& -\frac{1}{6}{S}^5(s_f)& -\frac{1}{8}{S}^6(s_f)& -\frac{1}{10}{S}^7(s_f)& -\frac{1}{12}{S}^8(s_f)& \frac{1}{2}{s}_f^{2}c{\theta }_f\\ -\frac{1}{3}{S}^4(s_f)& -\frac{1}{6}{S}^5(s_f)& -\frac{1}{9}{S}^6(s_f)& -\frac{1}{12}{S}^7(s_f)& -\frac{1}{15}{S}^8(s_f)& -\frac{1}{18}{S}^9(s_f)& \frac{1}{3}{s}_f^{3}c{\theta }_f\\ -\frac{1}{4}{S}^5(s_f)& -\frac{1}{8}{S}^6(s_f)& -\frac{1}{12}{S}^7(s_f)& -\frac{1}{16}{S}^8(s_f)& -\frac{1}{20}{S}^9(s_f)& -\frac{1}{24}{S}^{10}(s_f)& \frac{1}{4}{s}_f^{4}c{\theta }_f\\ -\frac{1}{5}{S}^6(s_f)& -\frac{1}{10}{S}^7(s_f)& -\frac{1}{15}{S}^8(s_f)& -\frac{1}{20}{S}^9(s_f)& -\frac{1}{25}{S}^{10}(s_f)& -\frac{1}{30}{S}^{11}(s_f)& \frac{1}{5}{s}_f^{5}c{\theta }_f\\ -\frac{1}{6}{S}^7(s_f)& -\frac{1}{12}{S}^8(s_f)& -\frac{1}{18}{S}^9(s_f)& -\frac{1}{24}{S}^{10}(s_f)& -\frac{1}{30}{S}^{11}(s_f)& -\frac{1}{36}{S}^{12}(s_f)& \frac{1}{6}{s}_f^{6}c{\theta }_f\\ s_{f}c{\theta }_f& \frac{1}{2}{s}_f^{2}c{\theta }_f& \frac{1}{3}{s}_f^{3}c{\theta }_f& \frac{1}{4}{s}_f^{4}c{\theta }_f& \frac{1}{5}{s}_f^{5}c{\theta }_f& \frac{1}{6}{s}_f^{6}c{\theta }_f& {\kappa }_{f}c{\theta }_f\end{array}\right]\\ & +{\lambda }_2\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& s_f\\ 0& 0& 0& 0& 0& 0& s_f^2\\ 0& 0& 0& 0& 0& 0& s_f^3\\ 0& 0& 0& 0& 0& 0& s_f^4\\ 0& 0& 0& 0& 0& 0& s_f^5\\ 1& s_f& s_f^2& s_f^3& s_f^4& s_f^5& {\kappa }_f^{\prime }\end{array}\right]\\ .& \end{array}$$

$$\begin{array}{rl}{\mathbf{H}}_{3x7}& =\frac{\delta }{\delta \mathbf{q}}\mathbf{g}(\mathbf{q})\\ & =\left[\begin{array}{c}\frac{\delta x(s_f)}{\delta \mathbf{q}}\\ \frac{\delta y(s_f)}{\delta \mathbf{q}}\\ \frac{\delta \theta (s_f)}{\delta \mathbf{q}}\end{array}\right]\\ & =\left[\begin{array}{ccccccc}-S^1(s_f)& -\frac{1}{2}{S}^2(s_f)& -\frac{1}{3}{S}^3(s_f)& -\frac{1}{4}{S}^4(s_f)& -\frac{1}{5}{S}^5(s_f)& -\frac{1}{6}{S}^6(s_f)& c{\theta }_f\\ C^1(s_f)& \frac{1}{2}{C}^2(s_f)& \frac{1}{3}{C}^3(s_f)& \frac{1}{4}{C}^4(s_f)& \frac{1}{5}{C}^5(s_f)& \frac{1}{6}{C}^6(s_f)& s{\theta }_f\\ s_f& \frac{s_f^2}{2}& \frac{s_f^3}{3}& \frac{s_f^4}{4}& \frac{s_f^5}{5}& \frac{s_f^6}{6}& {\kappa }_f\end{array}\right]\end{array}$$

$$\begin{array}{rl}{\mathbf{J}}^{\mathbf{T}}& =\left[\begin{array}{c}(\frac{\delta }{\delta \mathbf{q}}\mathbf{H}(\mathbf{q},\lambda ){)}^{\mathbf{T}}\\ \mathbf{g}(\mathbf{q})\end{array}\right]\\ & =\left[\begin{array}{c}(\frac{\delta \mathbf{J}(\mathbf{q})}{\delta \mathbf{q}}+\frac{\delta {\lambda }^{\mathbf{T}}\mathbf{g}(\mathbf{q})}{\delta \mathbf{q}}{)}^T\\ \mathbf{g}(\mathbf{q})\end{array}\right]\\ & =\left[\begin{array}{c}{K}^0(s_f)\\ K^1(s_f)\\ K^2(s_f)\\ K^3(s_f)\\ K^4(s_f)\\ K^5(s_f)\\ \frac{1}{2}{k}_f^2\\ x(s_f)-x_f\\ y(s_f)-y_f\\ \theta (s_f)-{\theta }_f\end{array}\right]+\left[\begin{array}{c}{\mathbf{H}}_{3x7}^{\mathbf{T}}\lambda \\ 0_{3x1}\end{array}\right]\end{array}$$

其中：
$$\begin{array}{r}{k}_f=a_0+a_1{s}_f+a_2{s}_f^2+a_3{s}_f^3+a_4{s}_f^4+a_5{s}_f^5\end{array}$$
$$\begin{array}{r}{k}_f^{\prime }=a_1+2a_2{s}_f+3a_3{s}_f^2+4a_4{s}_f^3+5a_5{s}_f^4\end{array}$$
$$\begin{array}{r}s{\theta }_f=sin(\theta (s_f)),c{\theta }_f=cos(\theta (s_f))\end{array}$$

将$[0,s_f]$分成m等分，m为偶数，定义：
w = { w k } = [ 1 4 2 4 . . . 4 2 4 1 ] T \begin{align} \bf{w} = \{w_k\} = \begin{bmatrix} 1 & 4 & 2 & 4 &...&4 & 2 & 4 & 1 \end{bmatrix}^T \end{align} w={wk​}=[1​4​2​4​...​4​2​4​1​]T​​

$$\begin{array}{r}{C}^n(s_f)=\left(\frac{\Delta s}{3}\right)\sum _{k=0}^m{w}_k{s}_k^{n}cos(\theta (s_k))with{s}_k=k\Delta s\end{array}$$
$$\begin{array}{r}{S}^n(s_f)=\left(\frac{\Delta s}{3}\right)\sum _{k=0}^m{w}_k{s}_k^{n}sin(\theta (s_k))with{s}_k=k\Delta s\end{array}$$

$$\begin{array}{rl}x(s_f)& =\left(\frac{\Delta s}{3}\right)\sum _{k=0}^m{w}_{k}cos(\theta (s_k))with{s}_k=k\Delta s\end{array}$$

$$\begin{array}{rl}y(s_f)& =\left(\frac{\Delta s}{3}\right)\sum _{k=0}^m{w}_{k}sin(\theta (s_k))with{s}_k=k\Delta s\end{array}$$

$$\begin{array}{r}{\mathbf{K}}^{\mathbf{n}}(\mathbf{s})={\mathbf{a}}_0\frac{{\mathbf{s}}^{\mathbf{n}+1}}{\mathbf{n}+1}+{\mathbf{a}}_1\frac{{\mathbf{s}}^{\mathbf{n}+2}}{\mathbf{n}+2}+{\mathbf{a}}_2\frac{{\mathbf{s}}^{\mathbf{n}+3}}{\mathbf{n}+3}+{\mathbf{a}}_3\frac{{\mathbf{s}}^{\mathbf{n}+4}}{\mathbf{n}+4}+{\mathbf{a}}_4\frac{{\mathbf{s}}^{\mathbf{n}+5}}{\mathbf{n}+5}+{\mathbf{a}}_5\frac{{\mathbf{s}}^{\mathbf{n}+6}}{\mathbf{n}+6}\end{array}$$

待续…