【轨迹规划论文整理(1)】UAV轨迹规划的开山之作Minimum Snap Trajectory

【轨迹规划论文整理(1)】UAV轨迹规划的开山之作Minimum Snap Trajectory Generation and Control for Quadrotors

本系列主要是对精读的一些关于无人机、无人车的轨迹搜索论文的整理，包括了论文所拓展的其他一些算法的改进思路。
这是本系列的第一篇文章，也是UAV轨迹规划的开山之作，是所有学习无人机方向的需要精读的第一篇文章，两个作者来自于宾夕法尼亚大学的GRASP Lab，是全球顶尖的机器人团队，也是UAV方向研究的开山鼻祖之一。

Mellinger D , Kumar V .Minimum snap trajectory generation and control for quadrotors[J].IEEE, 2011.DOI:10.1109/ICRA.2011.5980409.

1. Introduction

1.1 轨迹规划的定义

在机器人导航过程中，如何控制机器人从A点移动到B点，通常称之为运动规划。运动规划一般分为两步：

• 路径搜索：在地图(包括给栅格、八叉树、点云地图等等) 搜索一条从A到B点的路径，这条路径通常是由一系列离散的空间点(waypoint)组成。该部分称之为前端。
• 轨迹规划：由于路径点可能比较稀疏、而且不平滑，为了更好地控制机器人运动，需要将稀疏的路径点变成平滑的曲线或稠密的轨迹点，即轨迹。这部分称之为后端。

本文的主要工作就是一种无人机轨迹生成的方法，额外还包括了建模和控制器的设计。

2. Model

图中是本文定义的坐标关系，利用Z-X-Y欧拉角定义横滚角、俯仰角、偏航角(roll,pitch,yaw)作为本地坐标系统。从B至W的旋转矩阵通过$R_B^W=R_C^W{R}_B^C$来表示，$R_C^W$代表偏航角旋转至中间坐标系(Z轴方向与世界坐标Z轴方向一致)。UAV机体在世界坐标系的角速度计算如下，p、q、r分量在记载坐标系中的分量为：

$${\omega }_B^W=p{X}_B+q{Y}_B+r{Z}_B$$

每个电机有角速度${\omega }_i$，产生一个力$F_i$和$M_i$，关系为:

$$\begin{gathered} F_i=k_F{\omega }_i^2 \\ {M}_i=k_M{\omega }_i^2 \end{gathered}$$

控制输入记为u，$u_1$为净升力，$u_2、u_3、u_4$是机体力矩可以通过电机转速表示为(L是电机旋转中心到电机中心的距离)

$$\mathbf{u}=\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& -k_{F}L\\ -k_{F}L& 0& k_{F}L& 0\\ k_M& -k_M& k_M& -k_M\end{array}\right]\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right]$$

机体的线加速度，其中系统受力为$-z_W$方向上的重力和四个电机的合力为$z_B$方向上的$u_1$。

$$\begin{array}{r}m\ddot{\mathbf{r}}=-mg{\mathbf{z}}_W+u_1{\mathbf{z}}_B\end{array}$$

用欧拉公式计算角速度如下，其中I是惯性矩矩阵：

$${\dot{\omega }}_{\mathcal{B}\mathcal{W}}={\mathcal{I}}^{-1}\left[-{\omega }_{\mathcal{B}\mathcal{W}}\times \mathcal{I}{\omega }_{\mathcal{B}\mathcal{W}}+[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]]$$

UAV系统的状态向量由质心的位置和速度、方向和角速度给出：

$$X=[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},p,q,r]$$

3. 微分平坦性

UAV的平坦输出可以写作：

$$\sigma =[x,y,z,\psi {]}^T$$

其中$r=[x,y,z{]}^T$是世界坐标系中质心的坐标，而ψ是偏航角，这里定义轨迹$\sigma (t)$为平坦输出空间中的平滑曲线：

$$\sigma (t):[t_0,t_m]\to {\mathbb{R}}^3\times SO(2)$$

根据微分平坦性，系统的状态和控制输入可以用$\sigma$及其导数表示，具体推导参考博客。

4. 控制

本文设计了一个控制器去跟踪特定轨迹${\sigma }_T(t)=[r_T(t{)}^T,{\psi }_T(t){]}^T$。首先定义位置和速度误差为

$$e_p=r-r_T,e_v=\dot{r}-{\dot{r}}_T$$

计算控制器所需要的力矢量：

$$F_{des}=-K_p{e}_p-K_v{e}_v+mg{z}_W+m{\ddot{r}}_T$$

将所需的力矢量投影到实际的身体坐标系z轴上，从而计算第一输入的所需力：

$$u_1=F_{des}\cdot z_B$$

理想的$Z_{B,des}$一定沿着理想推力方向：

$$Z_{B,des}=\frac{F_{des}}{||F_{des}||}$$

此时，理想的旋转$R_B^W$为：

$$R_{des}{e}_3=Z_{B,des}$$

其他理想状态量：

$$\begin{gathered} {\mathbf{x}}_{C,des}={\left[\cos {\psi }_T,\sin {\psi }_T,0\right]}^T, \\ \text{and} \\ {\mathbf{y}}_{B,des}=\frac{{\mathbf{z}}_{B,des}\times {\mathbf{x}}_{C,des}}{\Vert {\mathbf{z}}_{B,des}\times {\mathbf{x}}_{C,des}\Vert },{\mathbf{x}}_{B,des}={\mathbf{y}}_{B,des}\times {\mathbf{z}}_{B,des}, \end{gathered}$$

$Z_B,X_B,Y_B$构成了旋转矩阵，从而获得了理想的旋转矩阵

定义旋转误差：

$${\mathbf{e}}_R=\frac{1}{2}(R_{des}^T{R}_B^W-R_B^W{}^T{R}_{des}{)}^{\vee }$$

角速度误差（机身系）：

$$e_{\omega }=\omega -{\omega }_T$$

另外三个输入的计算如下，两个K都是对角增益矩阵：

$$[u_2,u_3,u_4{]}^T=-K_R{e}_R-K_{\omega }{e}_{\omega }$$

5. 轨迹生成(本文最重要部分)

生成的轨迹可以写成m个时间间隔上的n阶分段多项式函数：

$${\sigma }_T(t)=\{\begin{array}{cc}{\sum }_{i=0}^n{\sigma }_{Ti1}{t}^i& t_0\le t<t_1\\ {\sum }_{i=0}^n{\sigma }_{Ti2}{t}^i& t_1\le t<t_2\\ ⋮& \\ {\sum }_{i=0}^n{\sigma }_{Tim}{t}^i& t_{m-1}\le t\le t_m\end{array}$$

根据上一节的微分平坦性，这里的${\sigma }_T=[x_T,y_T,z_T,{\psi }_T{]}^T$，${\sigma }_i=[x_i,y_i,z_i,{\psi }_i{]}^T$。上述中要求相邻两段轨迹相同的时间节点是$r_T$微分连续。

建立二次规划问题，c是一个4nm*1的向量，包含了${\sigma }_{T_{ij}}=[x_{T_{ij}},y_{T_{ij}},z_{T_{ij}},{\psi }_{T_{ij}}{]}^T$及它们的导数：

$$\begin{gathered} \min c^{T}Hc+f^{T}c \\ s.t.Ac\le b \end{gathered}$$

接下来的解析我觉得文章本身不是很清晰，但是结合深蓝学院的课程和其他博客的思路，重新解读一下。

5.1 一段多项式轨迹

N+1个已知参数点可以拟合N次函数，对于首尾位置、速度、加速度已知的条件(相当于6个已知参数)，可以唯一确定一个五次多项式。

5.2 多段多项式轨迹

本文提出了一个经过多个中间航迹点的轨迹，分别使用多个轨迹段进行拟合。但是相对于一段轨迹我们指定了节点的PVA，对于多段轨迹的中间点我们很难给定UAV经过这些航点的VAJ等信息，此时我们将中间速度、加速度等信息看作一个可以自由变化的状态，对于中间状态不确定的问题，可以构建一个优化问题，使得中间状态取得某一个值时，制定的代价函数达到最小值。

Minimum Snap的核心思路就是多端轨迹的中间航点的变量如何取值可以使机器人能量耗费最少。

5.3 Minimum Snap

UAV的轨迹再各个坐标轴上是独立的，因此可以对其分别进行轨迹拟合。即xyz轴上路径生成可以直接将三个轴合成就得到一个完整的空间轨迹。下面的推导均在一维上进行。

第m段轨迹表示如下：

$$\begin{gathered} {\mathrm{P}}_{\mathrm{m}}\left(\mathrm{t}\right)={\mathrm{p}}_{\mathrm{m},0}{\mathrm{t}}^0+{\mathrm{p}}_{\mathrm{m},1}{\mathrm{t}}^1+\cdots +{\mathrm{p}}_{\mathrm{m},\mathrm{N}}{\mathrm{t}}^{\mathrm{N}} \\ =\left[\begin{array}{cccc}{\mathrm{p}}_{\mathrm{m},0}& {\mathrm{p}}_{\mathrm{m},1}& \cdots & {\mathrm{p}}_{\mathrm{m},\mathrm{N}}\end{array}\right]\left[\begin{array}{c}{\mathrm{t}}^0\\ {\mathrm{t}}^1\\ ⋮\\ {\mathrm{t}}^{\mathrm{N}}\end{array}\right] \end{gathered}$$

这个多项式就有N+1个未知系数。

对其分别求一阶、二阶、三阶导，获得它的速度、加速度、Jerk函数函数如下：

$$\begin{array}{rl}\frac{\mathrm{dP}\left(\mathrm{t}\right)}{\mathrm{dt}}& =\left[\begin{array}{cccc}{\mathrm{p}}_{\mathrm{m},0}& {\mathrm{p}}_{\mathrm{m},1}& \cdots & {\mathrm{p}}_{\mathrm{m},\mathrm{N}}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ ⋮\\ \mathrm{N}{\mathrm{t}}^{\mathrm{N}-1}\end{array}\right]\\ & \\ \frac{{\mathrm{d}}^2\mathrm{P}\left(\mathrm{t}\right)}{\mathrm{d}{\mathrm{t}}^2}& =\left[\begin{array}{cccc}{\mathrm{p}}_{\mathrm{m},0}& {\mathrm{p}}_{\mathrm{m},1}& \cdots & {\mathrm{p}}_{\mathrm{m},\mathrm{N}}\end{array}\right]\left[\begin{array}{c}0\\ 1\ast 0\\ 2\ast 1\\ ⋮\\ \mathrm{N}\left(\mathrm{N}-1\right){\mathrm{t}}^{\mathrm{N}-2}\end{array}\right]\end{array}$$

$$\begin{gathered} \frac{{\mathrm{d}}^3\mathrm{P}\left(\mathrm{t}\right)}{\mathrm{d}{\mathrm{t}}^3}=\left[\begin{array}{cccc}{\mathrm{p}}_{\mathrm{m},0}& {\mathrm{p}}_{\mathrm{m},1}& \cdots & {\mathrm{p}}_{\mathrm{m},\mathrm{N}}\end{array}\right]\left[\begin{array}{c}0\ast 0\ast 0\\ 1\ast 0\ast 0\\ 2\ast 1\ast 0\\ ⋮\\ \mathrm{N}(\mathrm{N}-1)(\mathrm{N}-2){\mathrm{t}}^{\mathrm{N}-3}\end{array}\right] \\ \frac{{\mathrm{d}}^4\mathrm{P}(\mathrm{t})}{\mathrm{d}{\mathrm{t}}^4}=\left[\begin{array}{cccc}{\mathrm{p}}_{\mathrm{m},0}& {\mathrm{p}}_{\mathrm{m},1}& \cdots & {\mathrm{p}}_{\mathrm{m},\mathrm{N}}\end{array}\right]\left[\begin{array}{c}0\ast 0\ast 0\ast 0\\ 1\ast 0\ast 0\ast 0\\ 2\ast 1\ast 0\ast 0\\ 3\ast 2\ast 1\ast 0\\ ⋮\\ \mathrm{N}(\mathrm{N}-1)(\mathrm{N}-2)(\mathrm{N}-3){\mathrm{t}}^{\mathrm{N}-4}\end{array}\right] \end{gathered}$$

在论文中提出了一个代价函数$J_m$，对应的是一段多项式轨迹的四阶导数(snap)平方的积分，具体公式如下：

$$\begin{gathered} {\mathrm{J}}_{\mathrm{m}}={\int }_{{\mathrm{T}}_{\mathrm{m}-1}}^{{\mathrm{T}}_{\mathrm{m}}}(\frac{{\mathrm{d}}^4\mathrm{P}\left(\mathrm{t}\right)}{\mathrm{d}{\mathrm{t}}^4}{)}^2 \\ ={\int }_{{\mathrm{T}}_{\mathrm{m}-1}}^{{\mathrm{T}}_{\mathrm{m}}}{\left[\begin{array}{c}{\mathrm{p}}_{\mathrm{N}}\\ {\mathrm{p}}_{\mathrm{N}-1}\\ ⋮\\ {\mathrm{p}}_0\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{N}(\mathrm{N}-1)(\mathrm{N}-2)(\mathrm{N}-3){\mathrm{t}}^{\mathrm{N}-4}\\ ⋮\\ 3\ast 2\ast 1\ast 0\\ 2\ast 1\ast 0\ast 0\\ 1\ast 0\ast 0\ast 0\\ 0\ast 0\ast 0\ast 0\end{array}\right]{\left[\begin{array}{c}\mathrm{N}(\mathrm{N}-1)(\mathrm{N}-2)(\mathrm{N}-3){\mathrm{t}}^{\mathrm{N}-4}\\ ⋮\\ 3\ast 2\ast 1\ast 0\\ 2\ast 1\ast 0\ast 0\\ 1\ast 0\ast 0\ast 0\\ 0\ast 0\ast 0\ast 0\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\mathrm{p}}_{\mathrm{N}}\\ ⋮\\ {\mathrm{p}}_{\mathrm{N}-1}\\ ⋮\\ {\mathrm{p}}_0\end{array}\right] \\ =P_m^T{Q}_m{P}_m \end{gathered}$$

其中$Q_m$如下：

$$\left[\begin{array}{ccc}\frac{\mathrm{N}\left(\mathrm{N}-1\right)\left(\mathrm{N}-2\right)\left(\mathrm{N}-3\right)\mathrm{N}\left(\mathrm{N}-1\right)\left(\mathrm{N}-2\right)\left(\mathrm{N}-3\right){\left({\mathrm{T}}_{\mathrm{m}}-{\mathrm{T}}_{\mathrm{m}-1}\right)}^{\mathrm{N}+\mathrm{N}-7}}{\mathrm{N}+\mathrm{N}-7}& \frac{\mathrm{N}\left(\mathrm{N}-1\right)\left(\mathrm{N}-2\right)\left(\mathrm{N}-3\right)\left(\mathrm{N}-1\right)\left(\mathrm{N}-2\right)\left(\mathrm{N}-3\right)\left(\mathrm{N}-4\right){\left({\mathrm{T}}_{\mathrm{m}}-{\mathrm{T}}_{\mathrm{m}-1}\right)}^{\mathrm{N}+\mathrm{N}-1-7}}{\mathrm{N}+\mathrm{N}-1-7}& \cdots \\ ⋮& ⋮& ⋮\\ \cdots & \frac{\mathrm{i}\left(\mathrm{i}-1\right)\left(\mathrm{i}-2\right)\left(\mathrm{i}-3\right)\left(\mathrm{i}-1\right)\left(\mathrm{i}-2\right){\left({\mathrm{T}}_{\mathrm{m}}-{\mathrm{T}}_{\mathrm{m}-1}\right)}^{\mathrm{i}+1-7}}{\mathrm{i}+1-7}& \cdots \\ ⋮& ⋮& ⋮\\ 0& 0& \cdots \end{array}\right]$$

上式中$T_m-T_{m-1}$实际就是该段轨迹运动所用的时长，为了不让每一段轨迹的时间差过大，这里采用相对时间来表示，对于每一段轨迹认为它们的时间都是$(0,T_m)$。

总的代价函数

假设现在有M段轨迹，那么每段轨迹都是一个高阶的多项式函数，写出代价函数如下：

$$\begin{array}{c}\mathrm{J}={\mathrm{J}}_0+{\mathrm{J}}_1+\cdots +{\mathrm{J}}_{\mathrm{M}}\\ =\sum _{\mathrm{m}=0}^{\mathrm{M}}{\overrightarrow{{\mathrm{P}}_{\mathrm{m}}}}^{\mathrm{T}}{\mathrm{Q}}_{\mathrm{m}}\overrightarrow{{\mathrm{P}}_{\mathrm{m}}}\\ {=\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccccc}{\mathrm{Q}}_0& 0& \cdots & \cdots & 0\\ 0& {\mathrm{Q}}_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \cdots & ⋮\\ 0& 0& 0& \cdots & {\mathrm{Q}}_{\mathrm{M}}\end{array}\right]\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right]\end{array}$$

代价函数完成了，但是还缺少约束，即每个中间航点在前后两段轨迹的速度、加速度或者jerk是连续的，如此运动轨迹才是平滑的。

5.4 约束

连续性约束

限制中间的航迹点在两段轨迹的接合处是左右两边状态连续的，写成数学形式如下：

$$P_m^{(k)}(T_m)=P_{m+1}^{(k)}(T_m)$$

即轨迹段$P_m$和$P_{m+1}$在$T_m$时刻具有相同的k阶导数。

$$\begin{array}{r}{\mathrm{P}}_{\mathrm{m}}^{(\mathrm{k})}({\mathrm{T}}_{\mathrm{m}})={\mathrm{P}}_{\mathrm{m}+1}^{(\mathrm{k})}({\mathrm{T}}_{\mathrm{m}})\\ \Rightarrow \sum _{\mathrm{i}\ge \mathrm{k}}\frac{\mathrm{i}!}{(\mathrm{i}-\mathrm{k})!}{\mathrm{T}}_{\mathrm{m}}^{\mathrm{i}-\mathrm{k}}{\mathrm{p}}_{\mathrm{m},\mathrm{i}}-\sum _{\mathrm{l}\ge \mathrm{k}}\frac{\mathrm{l}!}{(\mathrm{l}-\mathrm{k})!}{\mathrm{T}}_{\mathrm{m}}^{\mathrm{l}-\mathrm{k}}{\mathrm{p}}_{\mathrm{m}+1,\mathrm{l}}=0\\ \Rightarrow \left[\begin{array}{cc}{\mathrm{A}}_{\mathrm{m}}& -{\mathrm{A}}_{\mathrm{m}+1}\end{array}\right]\left[\begin{array}{c}{\mathrm{P}}_{\mathrm{m}}\\ {\mathrm{P}}_{\mathrm{m}+1}\end{array}\right]=0\end{array}$$

微分约束

启动和终点的PVA使用确定值限制，即$P_0(0),P_0^{(1)}(0),P_0^{(2)}(0),P_M(T_M),P_M^{(1)}(T_M),P_M^{(2)}(T_M)$已知。

中间航迹点的位置确定，即$P_m(0)和P_M(T_m)$已知。

融合上述的约束得到：

$$\left[\begin{array}{cccccc}{\mathrm{A}}_0& 0& 0& 0& \cdots & 0\\ {\mathrm{A}}_0& -{\mathrm{A}}_1& 0& 0& \cdots & 0\\ 0& {\mathrm{A}}_1& 0& 0& \cdots & 0\\ 0& {\mathrm{A}}_1& -{\mathrm{A}}_2& 0& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& 0& {\mathrm{A}}_{\mathrm{M}}\end{array}\right]\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ {\mathrm{P}}_2\\ {\mathrm{P}}_3\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{d}}_0\\ 0\\ {\mathrm{d}}_1\\ 0\\ ⋮\\ {\mathrm{d}}_{\mathrm{M}}\end{array}\right]$$

到此，整个QP问题构建完毕。

5.5 求解QP

多项式轨迹中的系数$p$没有具体的物理意义，通过无约束方程求解的p可能会出现一些特别小的值，受计算精度影响，导致数值不稳定。

Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy.

为了解决这个系数不稳定的问题，在上文中提出了将系数通过一个映射矩阵转换成轨迹端点的导数，也就是说我们不再优化轨迹的系数，而是对轨迹在端点处的位置、速度、加速度、Jerk等进行优化，由于这些量都是有实际物理含义，不会出现太离谱的数值，所以在一定程度上是比较稳定的。

以最小化Jerk为例，此时是多项式是一个5次多项式轨迹，那么它有6个未知的系数，需要提供轨迹两端的位置、速度、加速度约束，对轨迹进行过次求导：

$$\begin{array}{rl}\left[\begin{array}{c}{\mathrm{P}}_{\mathrm{m}}\left(\mathrm{t}\right)\\ {\mathrm{P}}_{\mathrm{m}}^{\left(1\right)}\left(\mathrm{t}\right)\\ {\mathrm{P}}_{\mathrm{m}}^{\left(2\right)}\left(\mathrm{t}\right)\end{array}\right]& =\left[\begin{array}{cccccc}{\mathrm{p}}_{\mathrm{m},5}{\mathrm{t}}^5& {\mathrm{p}}_{\mathrm{m},4}{\mathrm{t}}^4& {\mathrm{p}}_{\mathrm{m},3}{\mathrm{t}}^3& {\mathrm{p}}_{\mathrm{m},2}{\mathrm{t}}^2& {\mathrm{p}}_{\mathrm{m},1}{\mathrm{t}}^1& {\mathrm{p}}_{\mathrm{m},0}{\mathrm{t}}^0\\ 5{\mathrm{p}}_{\mathrm{m},5}{\mathrm{t}}^4& 4{\mathrm{p}}_{\mathrm{m},4}{\mathrm{t}}^3& 3{\mathrm{p}}_{\mathrm{m},3}{\mathrm{t}}^2& 2{\mathrm{p}}_{\mathrm{m},2}{\mathrm{t}}^1& {\mathrm{p}}_{\mathrm{m},1}{\mathrm{t}}^0& 0\\ 5\ast 4{\mathrm{p}}_{\mathrm{m},5}{\mathrm{t}}^3& 4\ast 3{\mathrm{p}}_{\mathrm{m},4}{\mathrm{t}}^2& 3\ast 2{\mathrm{p}}_{\mathrm{m},3}{\mathrm{t}}^1& 2\ast 1{\mathrm{p}}_{\mathrm{m},2}{\mathrm{t}}^0& 0& 0\end{array}\right]\\ & =\left[\begin{array}{cccccc}{\mathrm{t}}^5& {\mathrm{t}}^4& {\mathrm{t}}^3& {\mathrm{t}}^2& {\mathrm{t}}^1& {\mathrm{t}}^0\\ 5{\mathrm{t}}^4& 4{\mathrm{t}}^3& 3{\mathrm{t}}^2& 2{\mathrm{t}}^1& {\mathrm{t}}^0& 0\\ 5\ast 4{\mathrm{t}}^3& 4\ast 3{\mathrm{t}}^2& 3\ast 2{\mathrm{t}}^1& 2\ast 1{\mathrm{t}}^0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{p}}_{\mathrm{m},5}\\ {\mathrm{p}}_{\mathrm{m},4}\\ {\mathrm{p}}_{\mathrm{m},3}\\ {\mathrm{p}}_{\mathrm{m},2}\\ {\mathrm{p}}_{\mathrm{m},1}\\ {\mathrm{p}}_{\mathrm{m},0}\end{array}\right]\end{array}$$

带入两端的时间，得到多项式系数到运动微分的映射关系：

$$\begin{array}{rl}\left[\begin{array}{c}\mathrm{P}\left(0\right)\\ {\mathrm{P}}^{\left(1\right)}\left(0\right)\\ {\mathrm{P}}^{\left(2\right)}\left(0\right)\\ \mathrm{P}\left(\mathrm{T}\right)\\ {\mathrm{P}}^{\left(1\right)}\left(\mathrm{T}\right)\\ {\mathrm{P}}^{\left(2\right)}\left(\mathrm{T}\right)\end{array}\right]& =\left[\begin{array}{cccccc}{0}^5& 0^4& 0^3& 0^2& 0^1& 0^0\\ 5\ast 0^4& 4\ast 0^3& 3\ast 0^2& 2\ast 0^1& 0^0& 0\\ 5\ast 4\ast 0^3& 4\ast 3\ast 0^2& 3\ast 2\ast 0^1& 2\ast 1\ast 0^0& 0& 0\\ {\mathrm{T}}^5& {\mathrm{T}}^4& {\mathrm{T}}^3& {\mathrm{T}}^2& {\mathrm{T}}^1& {\mathrm{T}}^0\\ 5\ast {\mathrm{T}}^4& 4\ast {\mathrm{T}}^3& 3\ast {\mathrm{T}}^2& 2\ast {\mathrm{T}}^1& {\mathrm{T}}^0& 0\\ 5\ast 4\ast {\mathrm{T}}^3& 4\ast 3\ast {\mathrm{T}}^2& 3\ast 2\ast {\mathrm{T}}^1& 2\ast 1\ast {\mathrm{T}}^0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{p}}_5\\ {\mathrm{p}}_4\\ {\mathrm{p}}_3\\ {\mathrm{p}}_2\\ {\mathrm{p}}_1\\ {\mathrm{p}}_0\end{array}\right]\\ & \Rightarrow {\mathrm{d}}_{\mathrm{m}}={\mathrm{A}}_{\mathrm{m}}{\mathrm{P}}_{\mathrm{m}}\\ & \Rightarrow {\mathrm{P}}_{\mathrm{m}}={\mathrm{A}}_{\mathrm{m}}^{-1}{\mathrm{d}}_{\mathrm{m}}\end{array}$$

得到映射矩阵：

$${\mathrm{A}}_{\mathrm{m}}=\left[\begin{array}{cccccc}{0}^N& 0^{N-1}& \cdots & 0^2& 0^1& 0^0\\ \mathrm{N}\ast 0^{\mathrm{N}-1}& (\mathrm{N}-1)\ast 0^{N-2}& \cdots & 2\ast 0^1& 0^0& 0\\ \mathrm{N}(\mathrm{N}-1)\ast 0^{\mathrm{N}-2}& (\mathrm{N}-1)(\mathrm{N}-2)\ast 0^{N-3}& \cdots & 2\ast 1\ast 0^0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ {\mathrm{T}}^{\mathrm{N}}& {\mathrm{T}}^{\mathrm{N}-1}& \cdots & {\mathrm{T}}^2& {\mathrm{T}}^1& {\mathrm{T}}^0\\ \mathrm{N}\ast {\mathrm{T}}^{\mathrm{N}-1}& (\mathrm{N}-1)\ast {\mathrm{T}}^{\mathrm{N}-2}& \cdots & 2\ast {\mathrm{T}}^1& {\mathrm{T}}^0& 0\\ \mathrm{N}(\mathrm{N}-1)\ast {\mathrm{T}}^{\mathrm{N}-2}& (\mathrm{N}-1)(\mathrm{N}-2)\ast {\mathrm{T}}^{\mathrm{N}-3}& \cdots & 2\ast 1\ast {\mathrm{T}}^0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]$$

将代价函数中的系数向量通过映射矩阵$A_m$进行替换，得到下式：

$$\begin{gathered} \min {\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cccccc}{\mathrm{Q}}_0& 0& \cdots & \cdots & \cdots & 0\\ 0& {\mathrm{Q}}_1& 0& \cdots & 0& \\ ⋮& ⋮& ⋮& \ddots & ⋮& \\ 0& 0& 0& \cdots & {\mathrm{Q}}_{\mathrm{M}}& \end{array}\right]\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right] \\ \min {\left[\begin{array}{c}{\mathrm{d}}_1\\ {\mathrm{d}}_1\\ ⋮\\ {\mathrm{d}}_{\mathrm{M}}\end{array}\right]}^{\mathrm{T}}{\left[\begin{array}{ccccc}{\mathrm{A}}_0& 0& \cdots & \cdots & 0\\ 0& {\mathrm{A}}_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\mathrm{A}}_{\mathrm{M}}& \end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccccc}{\mathrm{Q}}_0& 0& \cdots & \cdots & 0\\ 0& {\mathrm{Q}}_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & {\mathrm{Q}}_{\mathrm{M}}\end{array}\right]{\left[\begin{array}{ccccc}{\mathrm{A}}_0& 0& \cdots & \cdots & 0\\ 0& {\mathrm{A}}_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & {\mathrm{A}}_{\mathrm{M}}\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{P}}_0\\ {\mathrm{P}}_1\\ ⋮\\ {\mathrm{P}}_{\mathrm{M}}\end{array}\right] \\ \Rightarrow \min {\mathrm{d}}^{\mathrm{T}}{\mathrm{A}}^{-\mathrm{T}}\mathrm{Q}{\mathrm{A}}^{-1}\mathrm{d} \end{gathered}$$

如此一来，微分约束已经融入到了PQ问题当中，但是连续性约束还没有引入到代价函数当中。

本文中引入了一个permutation Matrix(置换矩阵)C来将连续性约束引入到代价函数当中。C矩阵本身是一个只包含0和1的矩阵，它的作用是将向量$[d_0,d_1,...,d_M]$进行一个组合，将固定的变量放在头部，将需要优化的变量放在尾部，并且对于连续性约束的变量值相等，因此只选择其中一个来表达连续性变量。

换言之，首尾状态和中间点航点是已知的设为$d_{mF}$，中间点除位置信息外其他状态是代价函数再优化时需要分配的最优值，即需要优化的变量设为$d_{mP}$。

将置换矩阵带入到代价函数当中：

$$\mathrm{J}=\min {\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]}^{\mathrm{T}}\mathrm{C}{\mathrm{A}}^{-\mathrm{T}}\mathrm{Q}{\mathrm{A}}^{-1}{\mathrm{C}}^{\mathrm{T}}\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]$$

令$R=C{A}^{-T}Q{A}^{-1}{C}^T$。对R矩阵根据$d_F$和$d_P$的尺寸进行分块，得到如下变换：

$$\begin{array}{c}\mathrm{J}=\min {\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]}^{\mathrm{T}}\mathrm{C}{\mathrm{A}}^{-\mathrm{T}}\mathrm{Q}{\mathrm{A}}^{-1}{\mathrm{C}}^{\mathrm{T}}\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]=\mathrm{J}=\min {\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]}^{\mathrm{T}}\mathrm{R}\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]\\ =\min {\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{\mathrm{R}}_{\mathrm{F}}\mathrm{F}& {\mathrm{R}}_{\mathrm{F}}\mathrm{P}\\ {\mathrm{R}}_{\mathrm{P}}\mathrm{F}& {\mathrm{R}}_{\mathrm{P}}\mathrm{P}\end{array}\right]\left[\begin{array}{c}{\mathrm{d}}_{\mathrm{F}}\\ {\mathrm{d}}_{\mathrm{P}}\end{array}\right]\\ ={\mathrm{d}}_{\mathrm{F}}^{\mathrm{T}}{\mathrm{R}}_{\mathrm{FF}}{\mathrm{d}}_{\mathrm{F}}+{\mathrm{d}}_{\mathrm{F}}^{\mathrm{T}}{\mathrm{R}}_{\mathrm{FP}}{\mathrm{d}}_{\mathrm{P}}+{\mathrm{d}}_{\mathrm{P}}^{\mathrm{T}}{\mathrm{R}}_{\mathrm{PF}}{\mathrm{d}}_{\mathrm{F}}+{\mathrm{d}}_{\mathrm{P}}^{\mathrm{T}}{\mathrm{R}}_{\mathrm{PP}}{\mathrm{d}}_{\mathrm{P}}\end{array}$$

对于上式J是一个标量，并且Q矩阵是对称阵，所以R矩阵也是对称阵，则$R_{PF}=R_{FP}^T$。对于上式我们要求的变量是$d_p$，对其求导得到

$$\begin{array}{rl}\frac{\partial \mathcal{J}}{\partial d_{\mathcal{P}}}& =2{\mathrm{d}}_{\mathcal{F}}^{\mathcal{T}}{\mathcal{R}}_{\mathcal{F}\mathcal{P}}+2{\mathrm{R}}_{\mathcal{PP}}{\mathrm{d}}_{\mathcal{P}}=0\\ & \Rightarrow {\mathrm{d}}_{\mathcal{P}}^{\ast }=-{\mathrm{R}}_{\mathcal{PP}}^{-1}{\mathrm{R}}_{\mathcal{F}\mathcal{P}}^{\mathcal{T}}{\mathrm{d}}_{\mathcal{F}}\end{array}$$

然后反求得到多项式系数：

$$\mathrm{P}={\mathrm{A}}^{-1}{\mathrm{C}}^{\mathrm{T}}\left[\begin{array}{c}{d}_F\\ d_P^{\ast }\end{array}\right]$$