【附源码和详细的公式推导】Minimum Snap轨迹生成，闭式求解Minimum Snap问题，机器人轨迹优化，多项式轨迹路径生成与优化

该篇博客内容较多，工作量也很大，难免在理解上表达上有错误，如有发现烦请指教。如有问题在博客中留言，或者github的仓库中提Issues都行，看到后我会尽快回复。

Minimum Snap源代码：Minimum Snap(如果代码有用，别忘记给我点Star呀)

主要内容：

• 介绍Minimum Snap轨迹生成方法的由来以及用处；
• 详细推导Minimum Snap轨迹生成的数学原理；
• 对Minimum Snap问题进行闭式求解；
• 使用C++代码进行算法复现，达到甚至超过论文中的实验结果。

1.什么是Minimum Snap？

考虑一下这样的运动规划问题，上图的两个五角星分别是起点和终点，我们通过某一种路径搜索方式(A star、RRT star等等)，找到了一段轨迹路径，搜索出来的路径是不考虑运动学约束的，因此轨迹上会出现明显的不光滑点，可以想象，机器人不可能在某一点出现运动突变，如果是万向轮的机器人要想按照这个轨迹走，它必须每走完一段路，就要停下来，然后旋转一个角度对着路径然后再加速往前走。这样很明显浪费很多时间和效率。如果是阿克曼的机器人那么它就无法按照这个路径进行运动。

但是如果我们以轨迹中的一些航迹点为基础，画出一条光滑的连接每一个航迹点曲线，那么这条曲线明显就非常的适合机器人去运动。

因此，基于上述这个思路，学者们提出了一些方法去解决这个问题，其中文献【1】提出了一种通过多项式来拟合这个轨迹的方法，可以使得轨迹是光滑并且满足机器人动力学约束。该方法首先在无人机中提出，但是已经被扩展到了很多机器人的运动规划中，后续基于他们工作的研究也非常的多，因此这篇文章的方法十分值得去深入学习。

本篇博客就是总结一些以这篇论文为基础的一些工作，主要关注的是基于Minimum Snap的轨迹生成。

2.一段多项式轨迹

对于空间中的两个点，我们可以使用直线进行拟合，对于三个点我们可以使用二次函数进行拟合。更广义一点来说，一次函数可以拟合两个参数点，二次函数可以拟合三个参数点，$N$次函数也可以拟合$N+1$参数点。

想象一下如果我们知道了一段一维轨迹，并且我们以一个五次多项式去拟合它：
$$P(t)=p_0{t}^0+p_1{t}^1+\cdots +p_5{t}^5\text{\hspace{1em}(1)}$$

上式（1）中有6个未知的系数，如果我们要求解出它，我们至少需要6个方程。一个很明显的思路是我们可以取6个位置点进行拟合。多数情况下我们不光关心机器人的位置，对于它的速度和加速度我们同样很关心，因此我们如果选取轨迹头和尾的位置以及速度和加速度，那么我们也可以唯一确定这个多项式，而且机器人还能以我们想要的速度和加速度开始和结束，这样不是更好吗？而后一种思路正是论文中所采用的。

3.多段多项式轨迹

在第2小节，介绍了一段轨迹使用多项式去拟合的问题，那么如果是多段轨迹，我们又该如何操作呢？

也许你可能会想到多种方式去拟合这整条轨迹,在此我就不做扩展.

直接引用论文中思路进行介绍，论文中作者提出对于一个经过多个中间航迹点的轨迹，分别使用多个轨迹段进行拟合，也就是对于上述$T_0-T_1-T_2-T_3$这条轨迹，我们将其分为三段分别进行轨迹拟合，分别拟合轨迹段$T_0-T_1,T_1-T_2,T_2-T_3$。每一段轨迹的拟合沿用第2小节中对于一段轨迹拟合的思路。

但是与一段轨迹拟合有一些区别，多数情况下，我们很难去给定机器人在经过中间航迹点的速度、加速度、Jerk等信息，于是我们就认为中间点的速度、加速度等信息是一个可以自由变化的状态，那么对于这种中间状态不确定的问题，我们可以构建一个优化问题，使得中间状态取得某一个值时，我们制定的代价函数达到最小值，那么我们一方面就不去设计机器人以什么样的状态通过中间点，另一方面还可以通过优化代价函数得到一个最优的轨迹。

上面的文字读起来可能会有一些难理解，以上图所示的轨迹为例，假设我们已知$T_0$和$T_3$的位置、速度、加速度等运动信息，并且我们还知道$T_1$和$T_2$的位置信息，而对于$T_2$和$T_3$的速度、加速度等信息，我们认为它是一个变量，可以任意变化的量，但是也不是毫无目的的变化，一般我们会设置一个目标代价函数，找到这几个变量在取一定值时使得我们设计的代价函数取得最小值。更通俗的说：就是这几个变量取一定值的时候可以使机器人能量耗费最少，或者机器人运动的转动更少等等。

听起来这个想法非常好，那它是否可行呢？答案是肯定的！

以上就是Minimum Snap算法的最核心思路！

由于四旋翼无人机以及双轮差速轮机器人(不准确的简化)，它们轨迹在各个坐标轴上是独立的，因此我们可以对其分别进行轨迹拟合。也就是说我们可以分别对它们在$x,y,z$轴上进行路径生成，然后直接将三个轴合成就可以得到一个完整的空间轨迹。
因此后续算法细节介绍中，我将不再强调轨迹是一维、二维还是三维，均以一维来进行介绍，你可以很容易将其推广到任意维度。

4. Minimum Snap

后续的数学表达中，有时出现向量和标量字母混淆的问题，但是从文章前后逻辑的角度而言并不会引起歧义，我就不再细致的修改了，哈哈，还请留意！

4.1 Minimum Snap的代价函数

我们已经对基于多项式的轨迹生成有了一些感性的认识，下面我们就从数学的层面对其进行具体建模。

类似于上图的轨迹，它是一个一维的轨迹，横轴是时间$t$，纵轴是位置$P_m(t)$，我们以$N$次多项式来拟合每一个分段轨迹，那么对于第$m$段轨迹:
$$\begin{gathered} P_m(t)=p_{m,0}{t}^0+p_{m,1}{t}^1+\cdots +p_{m,N}{t}^N \\ =\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,N}\end{array}\right]\left[\begin{array}{c}{t}^0\\ t^1\\ ⋮\\ t^N\end{array}\right] \end{gathered}$$

那么，这个多项式就有$N+1$个未知系数。

对其进行一阶求导，相当于得到了它的速度函数：
$$\frac{dP(t)}{dt}=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,N}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ ⋮\\ N{t}^{N-1}\end{array}\right]$$

对其进行二阶求导，相当于得到的了它的加速度函数：
$$\frac{d^{2}P(t)}{d{t}^2}=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,N}\end{array}\right]\left[\begin{array}{c}0\\ 1\ast 0\\ 2\ast 1\\ ⋮\\ N(N-1)t^{N-2}\end{array}\right]$$

三阶求导，得到Jerk函数：
$$\frac{d^{3}P(t)}{d{t}^3}=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,N}\end{array}\right]\left[\begin{array}{c}0\ast 0\ast 0\\ 1\ast 0\ast 0\\ 2\ast 1\ast 0\\ ⋮\\ N(N-1)(N-2)t^{N-3}\end{array}\right]$$

四阶求导，则得到了Snap函数：
$$\frac{d^{4}P(t)}{d{t}^4}=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,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\\ ⋮\\ N(N-1)(N-2)(N-3)t^{N-4}\end{array}\right]$$

以上我们就写出了一段轨迹的多项式函数，以及它们对应的导数。

还记得前面铺垫吗？为了得到中间点的运动状态，我们构建一个代价函数，然后求解最优中间运动状态。

在论文【1】中，对于最优的轨迹生成问题，它构建了一个如下的代价含数$J_m$，对应的是一段多项式轨迹的四阶导数(Snap)平方的积分。

后续的过程中我会交叉使用最小化Jerk平方的积分以及最小化Snap平方的积分，看到后不要感到困惑！你甚至还可以最小化七阶导数平方的积分！

$$\begin{gathered} J_m={\int }_{T_{m-1}}^{T_m}(\frac{d^{4}P(t)}{d{t}^4}{)}^2 \\ ={\int }_{T_{m-1}}^{T_m}{\left[\begin{array}{c}{p}_N\\ p_{N-1}\\ ⋮\\ p_0\end{array}\right]}^T\left[\begin{array}{c}N(N-1)(N-2)(N-3)t^{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}N(N-1)(N-2)(N-3)t^{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]}^T\left[\begin{array}{c}{p}_N\\ p_{N-1}\\ ⋮\\ p_0\end{array}\right] \\ ={\left[\begin{array}{c}{p}_N\\ p_{N-1}\\ ⋮\\ p_i\\ ⋮\\ p_0\end{array}\right]}^T\left[\begin{array}{ccc}\frac{N(N-1)(N-2)(N-3)N(N-1)(N-2)(N-3)(T_m-T_{m-1}{)}^{N+N-7}}{N+N-7}& \frac{N(N-1)(N-2)(N-3)(N-1)(N-2)(N-3)(N-4)(T_m-T_{m-1}{)}^{N+N-1-7}}{N+N-1-7}& \cdots \\ ⋮& ⋮& ⋮\\ \cdots & \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)(T_m-T_{m-1}{)}^{i+l-7}}{i+l-7}& \cdots \\ ⋮& ⋮& ⋮\\ 0& 0& \cdots \end{array}\right]\left[\begin{array}{c}{p}_N\\ p_{N-1}\\ ⋮\\ p_l\\ ⋮\\ p_0\end{array}\right] \\ ={P_m}^T{Q}_m{P}_m \end{gathered}$$

上式推导思路：上式思路：如果两个向量相乘为常数，那么它们乘积等于它们乘积的转置。如$\vec{a}\ast \vec{b}=c$，则$(\vec{a}\ast \vec{b}{)}^T={\vec{b}}^T\ast {\vec{a}}^T=c$

注意：上式中$(T_m-T_{m-1}{)}^{N+N-7}$实际就是这段轨迹运动是所用的时长，为了方便，后续不再采用每段轨迹的绝对时间，而是采用相对时间，也就是对于任意一段轨迹认为它们的时间都是$(0，T_m)$，那么就可以将$(T_m-T_{m-1}{)}^{N+N-7}$简记为$T^{N+N-7}$，这一点请理解并留意。并且假设我们事先已经知道了各段轨迹的时间。

小例子：

为了大家更容易理解，我这里举一个实际的例子向大家展示一下$Q$矩阵的实际结果：论文中给出的是最小Snap，为了形成差异性我这里最小化Jerk，我们使用5次多项式，则$N=5$，它一共有6个未知数，那么它的$Q$矩阵如下：
$$Q=\left[\begin{array}{cccccc}720& 360& 120& 0& 0& 0\\ 360& 192& 72& 0& 0& 0\\ 120& 72& 36& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$$

跳转到生成Q矩阵的代码

总的代价函数
假设我们有$M$段轨迹，那么每一段轨迹都是一个高阶的多项式函数，写出代价函数和如下：
$$\begin{gathered} J=J_0+J_1+\cdots +J_M \\ =\sum _{m=0}^M{\overrightarrow{P_m}}^T{Q}_m\overrightarrow{P_m} \\ ={\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right]}^T\left[\begin{array}{ccccc}{Q}_0& 0& \cdots & \cdots & 0\\ 0& Q_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \cdots & ⋮\\ 0& 0& 0& \cdots & Q_M\end{array}\right]\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right] \end{gathered}$$

到此我们以多项式为基础推导出了代价函数的具体形式，如果我们直接对上式进行优化得到其最小值，看似确实完成了我们的目标，但是这其中忽略了一个问题，那就是可能优化出来的结果是，路标点之间的速度或者加速度不连续，显然这是可能的，因为我们没有限制航迹点在前后两端轨迹中的状态是否一致，也就没有限制要连续。但是我们的机器人只能平滑的运动，前后状态一定是要连续的，因此我们需要加一些额外的约束去限制连续性。

4.2 连续性约束

首先，我们需要限制中间的航迹点在两段轨迹的接合处是左右两边状态要连续。如果我们要最小化的目标是Jerk，那么我们必须保证接合处三阶导数是连续的。因为我们要对Jerk进行积分，那么多项式在求导3次之后必须要连续，所以要确保0阶、1阶、2阶导数都是光滑的，也就是保证求导到加速度函数依然是光滑的，这一点就体现了多项式轨迹的优势，它天然就是一个高阶光滑的函数。

写成数学的形式，那么就是：
$$P_m^{(k)}(T_m)=P_{m+1}^{(k)}(T_m)$$
上式的意义就是轨迹段$P_m$和$P_{m+1}$在$T_m$时刻具有相同的$k$阶导数。

对上式进一步推导：
$$\begin{gathered} P_m^{(k)}(T_m)=P_{m+1}^{(k)}(T_m) \\ \Rightarrow \sum _{i\ge k}\frac{i!}{(i-k)!}{T}_m^{i-k}{p}_{m,i}-\sum _{l\ge k}\frac{l!}{(l-k)!}{T}_m^{l-k}{p}_{m+1,l}=0 \\ \Rightarrow \left[\begin{array}{cc}{A}_m& -A_{m+1}\end{array}\right]\left[\begin{array}{c}{P}_m\\ P_{m+1}\end{array}\right]=0 \end{gathered}$$

4.3 微分约束

连续性约束保证了轨迹的光滑性。我们可以想一下虽然限制了轨迹的光滑性，但是我们并没有限制轨迹必须通过某一些点，或者以一个确定的状态通过某一些中间的航迹点。也就是上述的这些多项式系数中，有一些是已知的。但是大多数情况下，我们都是对轨迹的位置有一些限制的，并不能随意生成，所以还需要给中间的航迹点加一些限制。包括位置、速度、加速度、Jerk、Snap等等的限制。

首先我们考虑很实际的问题，机器人的起点和终点我们肯定事先是知道的，并且机器人以什么样的速度或者加速度开始，我们也是可以设置的，机器人的终点速度和加速度也是可以事先给定的。机器人中间航迹点的位置也是需要事先给定的，但是机器人中间点的速度和加速度等信息，我们没法去约束它，因为很难去确定机器人到达某一个点的速度或者加速度，所以对于中间点的速度和加速度，或者更高阶的Jerk甚至Snap等，我们不对其进行微分限制，让它们只具有连续性约束，然后让优化器给它们分配最优的状态。

那么用数学方法去表述上面的内容，如下：

起始点和终点的位置、速度、加速度等使用确定值限制：
$$\begin{gathered} 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)=确定值 \\ ⋮ \end{gathered}$$

按照之前的思路，如果我们最小化Snap，那我们最少需要给起点终点位置、速度、加速度、Jerk一个确定值，但是实际工程中，对于高于加速度之后的导数，我们都将其设置为0，也就是你最小化Snap，但是我只给位置、速度、加速度设置值，而Jerk就默认设置为0。更高阶的情况也如此。

中间航迹点使用位置限制：
$$\begin{gathered} P_0(T_0)=确定值 \\ {P}_1(T_0)=确定值 \\ {P}_1(T_1)=确定值 \\ ⋮ \end{gathered}$$
画图表示

把上面的过程写成数学等式(以任意一段多项式轨迹为例)：
$$\begin{gathered} \left[\begin{array}{cccc}{T}_{m-1}^N& T_{m-1}^{N-1}& \cdots & T_{m-1}^0\\ T_m^N& T_m^{N-1}& \cdots & T_m^0\\ N{T}_{m-1}^{N-1}& (N-1)T_{m-1}^{N-2}& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮\\ \cdots & \frac{i!}{(i-k)!}{T}_{m-1}^{i-k}& \cdots & \cdots \\ \cdots & \frac{i!}{(i-k)!}{T}_m^{i-k}& \cdots & \cdots \end{array}\right]\left[\begin{array}{c}{p}_N\\ p_{N-1}\\ ⋮\\ p_0\end{array}\right]=\left[\begin{array}{c}{\rho }_{T_{m-1}}\\ {\rho }_{T_m}\\ {\nu }_{T_{m-1}}\\ {\nu }_{T_m}\\ {\alpha }_{T_{m-1}}\\ {\alpha }_{T_m}\\ ⋮\end{array}\right] \\ \Rightarrow A_m{\vec{P}}_m={\vec{d}}_m \end{gathered}$$
以上就是对一段轨迹的头和尾进行微分限制，对于中间的航迹点。通常做法，对于它们高于0阶之后的导数不进行微分限制。

如果将连续性约束和微分约束进行合并，则可以得到如下式子：
$$\left[\begin{array}{cccccc}{A}_0& 0& 0& 0& \cdots & 0\\ A_0& -A_1& 0& 0& \cdots & 0\\ 0& A_1& 0& 0& \cdots & 0\\ 0& A_1& -A_2& 0& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& 0& A_M\end{array}\right]\left[\begin{array}{c}{P}_0\\ P_1\\ P_2\\ P_3\\ ⋮\\ P_M\end{array}\right]=\left[\begin{array}{c}{d}_0\\ 0\\ d_1\\ 0\\ ⋮\\ d_M\end{array}\right]$$
最终，我们构建了一个带约束的QP问题
$$\begin{gathered} \min {\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right]}^T\left[\begin{array}{ccccc}{Q}_0& 0& \cdots & \cdots & 0\\ 0& Q_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & Q_M\end{array}\right]\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right]=P^{T}QP \\ s.t.\left[\begin{array}{cccccc}{A}_0& 0& 0& 0& \cdots & 0\\ A_0& -A_1& 0& 0& \cdots & 0\\ 0& A_1& 0& 0& \cdots & 0\\ 0& A_1& -A_2& 0& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& 0& A_M\end{array}\right]\left[\begin{array}{c}{P}_0\\ P_1\\ P_2\\ P_3\\ ⋮\\ P_M\end{array}\right]=\left[\begin{array}{c}{d}_0\\ 0\\ d_1\\ 0\\ ⋮\\ d_M\end{array}\right] \end{gathered}$$
构建好上述QP问题，然后将其带入第三方求解库就可以求到一个全局最优解。

推荐使用的QP问题求解器：OOQP、Mosek

5. 闭式求解Minimum Snap

对于上面构造的QP问题，它的约束是一个等式约束，实际上可以通过一些数学的带入方法将约束条件转换到代价函数中，那么就变成了无约束的优化问题，那么就可以进行闭式求解。闭式求解的好处的求解速度更快，数值稳定性更高。

上面的代价函数中，我们优化的变量是多项式的系数，我们可以观察一下其中一段多项式的某一项$p_i{t}^i$，由于$p$没有实际的物理意义，所以在优化的时候可以会出现一些特别小的值，此时可能就会计算精度的影响，导致数值不稳定。

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

对于某一段轨迹，它的多项式表示如下：
$$P_m(t)=p_{m,0}{t}^0+p_{m,1}{t}^1+\cdots +p_{m,N}{t}^N=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,N}\end{array}\right]\left[\begin{array}{c}{t}^0\\ t^1\\ ⋮\\ t^N\end{array}\right]$$
根据我们之前说的多项式个数与阶数的关系，我们知道对于上式未知数一共有$N+1$个，对应的两个端点出可以提供$N+1$个微分约束和连续性约束。

以最小化Snap为例，由于我们是对多项式进行了4阶求导，那么可以写出多项式的向量形式如下：
$$P_m(t)=p_{m,0}{t}^0+p_{m,1}{t}^1+\cdots +p_{m,N}{t}^7=\left[\begin{array}{cccc}{p}_{m,0}& p_{m,1}& \cdots & p_{m,7}\end{array}\right]\left[\begin{array}{c}{t}^0\\ t^1\\ ⋮\\ t^7\end{array}\right]$$

每段多项式有8个未知的系数，因此我们需要对轨迹的端点提供位置、速度、加速度、jerk约束(包括微分约束和连续性约束)，那么我们8个未知系数就可以使用两端的位置、速度、加速度、Jerk运动量来表示，每段轨迹可以提供8个运动量，因此我们需要找到某一种方式将系数映射到成运动量(位置、速度、加速度等)。

我们不妨来探索一下，这次为了表述方便和公式写法的简化，我以最小化Jerk为例，那么根据前面的结论，我们需要构建次数为5次的多项式，那么它将有6个未知的系数，我们需要提供轨迹两端的位置、速度、加速度约束。下面我们对轨迹进行多次求导，看看是否可以从中找到这种映射关系：
$$\begin{gathered} \left[\begin{array}{c}{P}_m(t)\\ P_m^{(1)}(t)\\ P_m^{(2)}(t)\end{array}\right]=\left[\begin{array}{cccccc}{p}_{m,5}{t}^5& p_{m,4}{t}^4& p_{m,3}{t}^3& p_{m,2}{t}^2& p_{m,1}{t}^1& p_{m,0}{t}^0\\ 5p_{m,5}{t}^4& 4p_{m,4}{t}^3& 3p_{m,3}{t}^2& 2p_{m,2}{t}^1& p_{m,1}{t}^0& 0\\ 5\ast 4p_{m,5}{t}^3& 4\ast 3p_{m,4}{t}^2& 3\ast 2p_{m,3}{t}^1& 2\ast 1p_{m,2}{t}^0& 0& 0\end{array}\right] \\ =\left[\begin{array}{cccccc}{t}^5& t^4& t^3& t^2& t^1& t^0\\ 5t^4& 4t^3& 3t^2& 2t^1& t^0& 0\\ 5\ast 4t^3& 4\ast 3t^2& 3\ast 2t^1& 2\ast 1t^0& 0& 0\end{array}\right]\left[\begin{array}{c}{p}_{m,5}\\ p_{m,4}\\ p_{m,3}\\ p_{m,2}\\ p_{m,1}\\ p_{m,0}\end{array}\right] \end{gathered}$$

上式中的$P_m(t),P_m^{(1)}(t),P_m^{(2)}(t)$其实分别就是对应轨迹在$t$时刻的位置、速度、加速度

似乎已经很接近答案了，我们只要将分段轨迹两端的时间都带入上式，不就找到了多项式系数到运动微分的映射关系了吗！(与之前一样我们使用相对时间)
$$\begin{gathered} \left[\begin{array}{c}P(0)\\ P^{(1)}(0)\\ P^{(2)}(0)\\ P(T)\\ P^{(1)}(T)\\ P^{(2)}(T)\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\\ T^5& T^4& T^3& T^2& T^1& T^0\\ 5\ast T^4& 4\ast T^3& 3\ast T^2& 2\ast T^1& T^0& 0\\ 5\ast 4\ast T^3& 4\ast 3\ast T^2& 3\ast 2\ast T^1& 2\ast 1\ast T^0& 0& 0\end{array}\right]\left[\begin{array}{c}{p}_5\\ p_4\\ p_3\\ p_2\\ p_1\\ p_0\end{array}\right] \\ \Rightarrow d_m=A_m{P}_m \\ \Rightarrow P_m=A_m^{-1}{d}_m \end{gathered}$$
因此找到了映射矩阵，类似的可以推导任意阶多项式的映射矩阵：
$$A_m=\left[\begin{array}{cccccc}{0}^N& 0^{N-1}& \cdots & 0^2& 0^1& 0^0\\ N\ast 0^{N-1}& (N-1)\ast 0^{N-2}& \cdots & 2\ast 0^1& 0^0& 0\\ N(N-1)\ast 0^{N-2}& (N-1)(N-2)\ast 0^{N-3}& \cdots & 2\ast 1\ast 0^0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ T^N& T^{N-1}& \cdots & T^2& T^1& T^0\\ N\ast T^{N-1}& (N-1)\ast T^{N-2}& \cdots & 2\ast T^1& T^0& 0\\ N(N-1)\ast T^{N-2}& (N-1)(N-2)\ast T^{N-3}& \cdots & 2\ast 1\ast T^0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]$$

跳转到生成A矩阵的代码

将代价函数中的系数向量通过映射矩阵$A_m$进行替换，则得到下式：
$$\begin{gathered} \min {\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right]}^T\left[\begin{array}{ccccc}{Q}_0& 0& \cdots & \cdots & 0\\ 0& Q_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & Q_M\end{array}\right]\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right] \\ \Rightarrow \min {\left[\begin{array}{c}{d}_0\\ d_1\\ ⋮\\ d_M\end{array}\right]}^T{\left[\begin{array}{ccccc}{A}_0& 0& \cdots & \cdots & 0\\ 0& A_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & A_M\end{array}\right]}^{-T}\left[\begin{array}{ccccc}{Q}_0& 0& \cdots & \cdots & 0\\ 0& Q_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & Q_M\end{array}\right]{\left[\begin{array}{ccccc}{A}_0& 0& \cdots & \cdots & 0\\ 0& A_1& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & A_M\end{array}\right]}^{-1}\left[\begin{array}{c}{P}_0\\ P_1\\ ⋮\\ P_M\end{array}\right] \\ \Rightarrow \min d^T{A}^{-T}Q{A}^{-1}d \end{gathered}$$
上式实际上替换，实际上已经把（10）式中微分约束给带入到了代价函数中，但是连续性约束还没有引入到代价函数中。

在论文【1】中，作者引入一个permutation matrix (置换矩阵)$C$来将连续性约束进行引入到代价函数中。$C$矩阵本身是一个只包含0和1的矩阵，它的作用就是给所有$\left[\begin{array}{cccc}{d}_0& d_1& \cdots & d_M\end{array}\right]$进行一个组合，将固定的变量放在头部，将需要就行优化的变量方位尾部，并且对于连续性约束的变量，由于他们的值是相等的，因此选择其中一个来表达连续性约束。说起来有一些抽象，用数学表达就会清晰很多。

根据之前的介绍，那么紫色框中变量的值都是我们事先已经设置好的，浅蓝色框中的变量就是代价函数在优化时分配的最优值，也就是我们需要优化的变量。

咱们以最小化Jerk为例，假设有如下一条轨迹，它有两个分段轨迹，如上图，那么它的$C$矩阵如下：
$$\begin{gathered} \left[\begin{array}{c}{d}_{0,0}^{(0)}\\ d_{0,0}^{(1)}\\ d_{0,0}^{(2)}\\ d_{T,0}^{(0)}\\ d_{T,0}^{(1)}\\ d_{T,0}^{(2)}\\ d_{0,1}^{(0)}\\ d_{0,1}^{(1)}\\ d_{0,1}^{(2)}\\ d_{T,1}^{(0)}\\ d_{T,1}^{(1)}\\ d_{T,1}^{(2)}\end{array}\right]=\left[\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{d}_{0,0}^{(0)}\\ d_{0,0}^{(1)}\\ d_{0,0}^{(2)}\\ d_{T,0}^{(0)}\\ d_{T,1}^{(0)}\\ d_{T,1}^{(1)}\\ d_{T,1}^{(2)}\\ d_{T,0}^{(1)}\\ d_{T,0}^{(2)}\end{array}\right] \\ \Rightarrow d_m=C^T\left[\begin{array}{c}{d}_{m_F}\\ d_{m_P}\end{array}\right] \end{gathered}$$

其中$d_{mF}$表示第m段轨迹中的已经确定的变量(对应紫色框中的变量)，$d_{mP}$表示可以自由变化的变量(浅蓝色框中的变量)。

类似的你可以写出更高阶情况下的置换矩阵$C$.

不知道你是否发现了这其中奥妙，由于使用了置换矩阵$C$，我们可以将连续性约束给引入到代价函数中，因为用了$C$矩阵之后，就把相等的变量给省略了，它们的值相等，所以只需要保留一个进行优化即可。

跳转到生成置换矩阵C的代码

将上式带入代价函数，则最终得到：
$$J=\min {\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^{T}C{A}^{-T}Q{A}^{-1}{C}^T\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$$
最终我们将带约束的QP优化问题，转换成了一个无约束的QP问题，对于上式因为它是一个有全局最小值的凸函数，所以对其求导取极值点就是它的全局最优值：

令
$$C{A}^{-T}Q{A}^{-1}{C}^T=R$$
并对$R$矩阵根据$d_F$和$d_P$的尺寸进行分块，则得到如下变换：
$$\begin{gathered} J=\min {\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^{T}C{A}^{-T}Q{A}^{-1}{C}^T\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]=J=\min {\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^{T}R\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right] \\ =\min {\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\left[\begin{array}{cc}{R}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right] \\ =d_F^T{R}_{FF}{d}_F+d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PF}{d}_F+d_P^T{R}_{PP}{d}_P \end{gathered}$$
对于上式$J$是一个标量，并且$Q$矩阵是对称矩阵，而$C{A}^{-T}Q{A}^{-1}{C}^T=(C{A}^{-T}Q{A}^{-1}{C}^T{)}^T$，所以$R$也是对称矩阵，则$R_{PF}=R_{FP}^T$

上式中：
$$\begin{gathered} J=d_F^T{R}_{FF}{d}_F+d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PF}{d}_F+d_P^T{R}_{PP}{d}_P \\ =d_F^T{R}_{FF}{d}_F+2d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PP}{d}_P \end{gathered}$$
对于上式我们要求的变量是$d_P$，因此将其对$d_P$求导，并零导数为0时取极值点的值对应的就是最优的$d_P^{\ast }$：
$$\begin{gathered} \frac{\partial J}{\partial d_P}=2d_F^T{R}_{FP}+2R_{PP}{d}_P=0 \\ \Rightarrow d_P^{\ast }=-R_{PP}^{-1}{R}_{FP}^T{d}_F \end{gathered}$$
求解出来了$d_P^{\ast }$之后，问题就很简单了，我们只需要将它带入$C$和$A$就能求出最终的各个多项式的系数：

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

6. 从多项式中解算出轨迹

有了多项式的系数之后，我们只要通过离散化时间$t$，就可以得到每一段轨迹。原理比较简单，在此不做深究，直接看代码：跳转到结算轨迹的代码

7.每段多项式的时间分配问题

还记得上面我在推导Minimum Snap时说假设事先已经给定了每段轨迹所用的时间，实际上这并不是一个非常好的做法，因为实际上时间对于最终生成轨迹的好坏有很大影响，下图是来自论文中的结果：

可以看出来，不同的时间轨迹相差还是比较明显的。

在【1】和【2】的论文中都提出了对每段时间也加入到优化的代价函数中，这样固然是好，但是势必会增加优化的时间，并且也可能会导致无法再闭式的求解这个问题。

实际上在工程中，有一个简化的方法去分配每一段轨迹的时间，也就是先假设机器人以最大的加速度加速到最大速度，然后运行一段时间，再以最大的减速度到终点，到达是速度为0。经过我的多次实验，这样的方法分配出的时间基本都能优化出一条非常好的轨迹。在我的代码中就采用了这种思路！跳转到时间分配的代码

8. 如何解决生成的轨迹与障碍物碰撞

这个实际已经超过了本篇博客要介绍的内容，但是为了完整性，我在这里简单地介绍一下在论文【2】中提供的思路：如果发生碰撞，就在两个航迹点的中间再选一个航迹点，然后重新优化生成轨迹，重复这样的操作，直到生成安全的轨迹。如下图：

9. 实验效果

在该项目的仓库中，我提供了两个关于Minimum Snap的应用场景，一个是直接通过rviz中的插件点击空间位置，然后自动生成轨迹。另一个则是编写了一个完整的从前端轨迹搜索(Astar)到后端轨迹优化的全过程的代码。

先来看第一个(GIF图片过大，需要跳转到我的github中观看)：

• 实验效果GIF
• 对应的运行文件

第二个：
该实验，我实现了一个自动随机生成栅格地图，然后使用A star进行路径搜索，搜索到路径之后，使用Minimum Snap进行轨迹拟合。

对应的运行文件

为了方便，我省略了从A star搜索的路径中选择关键位置的步骤，所以A star搜索到的所有节点，都会被输入Minimum Snap进行拟合！

跳转到代码运行方法的文档

另外，我实现的Minimum Snap算法其计算用时比论文中低，当然作者的电脑是接近10年之前的，哈哈，不过实现的效果以及效率基本和论文中是一致的！

在写这篇博客时浙江大学湖州实验室的高飞老师给了我很多帮助，同时也借鉴了他的一些代码和课件，在此对他进行感谢！

【1】: Mellinger D, Kumar V. Minimum snap trajectory generation and control for quadrotors[C]//Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 2520-2525.
【2】：Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy