Apollo学习笔记（15）Mini-Snap

老规矩首先上大神的链接 https://blog.csdn.net/q597967420/article/details/76099491。学习了深蓝学院的规划课程，以及拜读了大佬的博客后，做出的以下整理。

1.背景知识

机器人导航系统，是期望机器人从A点运动到B点，根据当前的物理环境规划出一条合适的路线，可以使得机器人可以移动到目标点的过程，就是运动规划。无人车其实也是机器人的一种，有着其独特的运动特性。

无人车的运动规划一般分为两个步骤：

1. routing：在地图（栅格地图，RRT地图，四叉树、八叉树等）中使用搜索的方式找出一条能从A点到B点的可行路径，此时路径为一系列离散的点组成；
2. planning：由于 routing 出的路点可能比较稀疏，并且路点之间的连线不够平滑，并不是很符合车辆的运动学特性，因此为了使得规划出的路线更加平滑并且符合车辆的运动学特性的平滑曲线或者更加稠密轨迹点。

一般来说，轨迹输出的方式都是用多项式来表示（五阶就可以了，有个大佬已经证明了这个结论，太高的阶增加了很大的计算量，对处理的结果改善却没有太大的作用）：

$$p(t)=p_0+p_{1}t+p_2{t}^2\dots +p_n{t}^n=\sum _{i=0}^n{p}_i{t}^i$$
其中，$p_0,p_1,\dots ,p_n$为轨迹参数，共有$n+1$个，设参数向量$p=[p_0,p_1,\dots ,p_n{]}^T$，则表达输出轨迹的多项式可以写为向量形式，
$$p(t)=[1,t,t^2,\dots ,t^n]\cdot p$$

基于上式，可以根据参数计算出轨迹的位置$Positon$，速度$Velocity$，加速度$Acceleration$，加加速度$Jerk$，加加加速度$Snap$，如下，
$$\begin{gathered} p(t)=[1,t,t^2,t^3,t^4,\dots ,t^n]\cdot p \\ v(t)=p^{\prime }(t)=[0,1,2t,3t^2,4t^3,\dots ,n{t}^{n-1}]\cdot p \\ a(t)=p^{(3)}(t)=[0,0,2,6t,12t^2,\dots ,n(n-1)t^{n-2}]\cdot p \\ jerk(t)=p^{(4)}(t)=[0,0,0,6,24t,\dots ,n(n-1)(n-2)t^{n-3}]\cdot p \\ snap(t)=p^{(5)}(t)=[0,0,0,0,24,\dots ,n(n-1)(n-2)(n-3)t^{n-4}]\cdot p \end{gathered}$$

由于routing出的路点有很多个，很难找到一条多项式表示，为了保证多项式尽量能涵盖所有的路点（起码能最大层度上能拟合到所有的路点），所以将轨迹按时间分为很多段，每段都用一个多项式曲线来表示，如下，
$$p(t)=\{\begin{array}{ll}[1,t,t^2,\dots ,t^n]\cdot p_1& {\text{t}}_0\le t\le t_1\\ [1,t,t^2,\dots ,t^n]\cdot p_2& {\text{t}}_1\le t\le t_2\\ \dots & \\ [1,t,t^2,\dots ,t^n]\cdot p_k& {\text{t}}_{k-1}\le t\le t_k\end{array}$$
$k$为轨迹的段数，$p_i=[p_{i0},p_{i1},\dots ,p_{in}{]}^T$为第$i$段轨迹的参数向量。

此外，实际工程问题中的轨迹往往是多维度的，通常的做法是单独求每个维度的轨迹。

可以看出，轨迹规划的最终目标就是求解出不同时间段内的轨迹多项式的参数$p_1,p_2,\dots ,p_k$，同时，希望轨迹能满足一系列的约束条件，比如：设定起点和终点的位置、加速度、速度等，希望相邻的轨迹连接处足够平滑（位置连续，速度连续等），希望轨迹经过某些特定的路点，设定最大速度，最大加速度等，为了壁障还会希望轨迹在规定的空间内（corridor）。

通常，满足预先设定的约束的估计会有无数多个，而实际的工程问题中，往往需要满足特定条件的一条最优解，因此又会构建一个cost function，从而在可行的轨迹中找到一条最合适的特定轨迹。

因此，综上所述，我们就可以将目标问题就建立成了一个有约束的优化问题，如下：

$$minf(p),s.t.A_{eq}=b_{eq},A_{ieq}=b_{ieq}$$
式中，$p=[p_1^T,p_2^T,\dots ,p_k^T{]}^T$。

看到这个公式，是不是有点激动，这不是又到了最优化控制的东西了吗？是的，没错，只要解出来这个方程，就可以求解出目标轨迹参数$p$。剩下的就是将优化问题中的$A_{eq}$，$b_{eq}$，$A_{ieq}$，$b_{ieq}$详细参数列出来，使用优化求解器求解即可。

至于cost function的选择，既然文章名字叫做 Mini-Snap，那最小化的目标函数 snap（加加加速度）肯定是可以的，也有人使用acceleration（加速度）和jerk（加加速度），至于速度和位置这两个为什么不可以做cost目标，你可以想想一下车子蜗牛速度行驶的场景。。。。。。

2.实际应用

首先给定包含起点在内的$k+1$个二维路点$(x_i,y_i)$，同时给定起始点的速度和加速度$v_0,a_0$，末端速度和加速度为$v_e,a_e$，给定了时间T，然后需要根据以上的条件规划出一条经过所有点的平滑轨迹。

2.1初始轨迹分段与时间分配

根据路径点，将轨迹分为k段，计算每段的距离，按距离平分时间T（匀速时间分配）,得到时间序列$t_0,t_1,\dots ,t_k$。然后对$x,y$维度单独规划轨迹。（后面都是单独讨论单一的维度）

在时间如何进行分配这一点上，这里稍微讨论一下，如果两个点之间的时间间隔太短，车辆可能由于自身的物理特性，导致在设置的间隔时间内无法到达下一个路点，而如果时间间隔设置的太大的话，很可能规划出的曲线会出现绕弯的情况，这两中情况都是我们不希望看到的。为了避免这个问题，时间分配的方法目前主流的大致有两种方式：
第一种，匀速分配时间。假设车辆在此段路径上按照一个固定的速度匀速行驶，然后根据每段的距离将总时间T分配到每段；
第二种，假设车辆速度变化为梯形的形式，车辆从当前速度按固定加速度加速到设定的速度，匀速行驶一段时间后，再匀减速到一个速度，也有可能是只有匀加速到固定速度后，匀速行驶，或者匀速行驶一段时间后，匀减速减速，然后根据每段的距离将总时间T分配到每段。

注意，这里的轨迹分段和时间分配都是初始分配，在迭代算法中，如果corridor check 和 feasiblility check 不满足条件，会插点或增大某一段时间，具体在后面有描述。

2.2构建优化函数

Mini-Snap的优化函数为：

$$\begin{gathered} \min {\int }_0^T(p^{(4)}(t){)}^{2}dt=\min \sum _{i=1}^k{\int }_{t_i-1}^{t_i}(p^{(4)}(t){)}^{2}dt \\ =\min \sum _{i=1}^k{\int }_{t_i-1}^{t_i}([0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}]\cdot p{)}^T([0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}]\cdot p)dt \\ =\min \sum _{i=1}^k{p}^T{\int }_{t_i-1}^{t_i}[0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}{]}^T[0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}]dtp \\ =\min \sum _{i=1}^k{p}^T{Q}_{i}p \end{gathered}$$

式中，
$$\begin{gathered} Q_i={\int }_{t_i-1}^{t_i}[0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}{]}^T[0,0,0,0,24,\dots ,\frac{n!}{(n-4)!}{t}^{n-4}]dt \\ =\left[\begin{array}{cc}{0}_{4\times 4}& 0_{4\times (n-3)}\\ 0_{(n-3)\times 4}& \frac{r!}{(r-4)!}\frac{c!}{(c-4)!}\frac{1}{(r-4)+(c-4)+1}(t_i^{(r+c-7)}-t_{i-1}^{(r+c-7)})\end{array}\right] \end{gathered}$$

注意：式中r为行索引，c为列索引，索引从0开始，第一行和第一列均为0。

因此有，
$$Q=\left[\begin{array}{cccc}{Q}_1& & & \\ & Q_2& & \\ & & \ddots & \\ & & & Q_k\end{array}\right]$$

所以优化目标方程为，
$$\min p^{T}Qp$$
这样就把Mini-Snap问题构建成了一个数学上的二次规划问题。

2.3构建等式约束

1. 设定某一点的位置、速度、加速度或者更高阶的值为一个特定的值，可以构成一个等式约束。如，
   位置约束： $$[1,t_0,t_0^2,\dots ,t_0^n,\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}]p=p_0$$
   速度约束： $$[0,1,2t_0,\dots ,n{t}_0^{n-1},\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}]p=v_0$$

   加速度约束： $$[0,0,2,\dots ,n(n-1)t_0^{n-2},\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}]p=a_0$$

   由于要过中间点，对中间点的位置也构建等式约束，方法同上。

2. 相邻段之间的位置、速度、加速度连续可以构成一个等式约束，例如第$i,i+1$段的位置连续构成的等式约束为： $$[\underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},1,t_i,t_i^2,\dots ,t_i^n,-1,-t_{i+1},-t_{i+1}^2,\dots ,-t_{i+1}^n,\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}]p=0$$
   速度连续构成的等式约束为： $$[\underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},0,1,2t_i,\dots ,n{t}_i^{(n-1)},0,-1,-2t_{i+1},\dots ,-n{t}_{i+1}^{n-1},\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}]p=0$$
   加速度连续构成的等式约束为： $$[\underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},0,0,2,\dots ,n(n-1)t_i^{n-2},0,0,-2,\dots ,-n(n-1)t_{i+1}^{n-2},\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}]p=0$$

合并所有的等式约束，有
$$\begin{gathered} {\left[\begin{array}{c}1,t_0,t_0^2,\dots ,t_0^n,\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}\\ 0,1,2t_0,\dots ,n{t}_0^{n-1},\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}\\ 0,0,2,\dots ,n(n-1)t_0^{n-2},\underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}}\\ ⋮\\ \underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},1,t_i,t_i^2,\dots ,t_i^n,\underset{\text{(k-i)(n+1)}}{\underbrace{0\dots 0}}\\ ⋮\\ \underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}},1,t_k,t_k^2,\dots ,t_k^n\\ \underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}},0,1,2t_k,\dots ,n{t}_k^{n-1}\\ \underset{\text{(k-1)(n+1)}}{\underbrace{0\dots 0}},0,0,2,\dots ,n(n-1)t_k^{n-2}\\ \underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},1,t_i,t_i^2,\dots ,t_i^n,-1,-t_{i+1},-t_{i+1}^2,\dots ,-t_{i+1}^n,\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}\\ \underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},0,1,2t_i,\dots ,n{t}_i^{(n-1)},0,-1,-2t_{i+1},\dots ,-n{t}_{i+1}^{n-1},\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}\\ \underset{\text{(i-1)(n+1)}}{\underbrace{0\dots 0}},0,0,2,\dots ,n(n-1)t_i^{n-2},0,0,-2,\dots ,-n(n-1)t_{i+1}^{n-2},\underset{\text{(k-i-1)(n+1)}}{\underbrace{0\dots 0}}\end{array}\right]}_{(4k+2)\times [(n+1)k]}\cdot p \\ =\left[\begin{array}{c}{p}_0\\ v_0\\ a_0\\ ⋮\\ p_i\\ ⋮\\ p_k\\ v_k\\ a_k\\ 0\\ ⋮\\ 0\end{array}\right] \end{gathered}$$

上述等式约束的个数设定为：

3（起始位置pva）+（k-1）（中间点的p）+3（终点pva）+3（k-1）（中间点pva连续）=4k+2

这里需要注意的是，上述公式中$i$的范围是从$1$到$k-1$，所以中间点的项和最后三个有$i$的公式并不是一项。

2.4构建不等式约束

不等式的约束和等式约束的设置方式类似，也是设置某个点的$p,v,a$小于某一特定的值，从而构建$A_{ieq}p\le b_{ieq}$，不等式约束一般是在corridor中使用的比较多，后面会有详细的说明，这里就不赘述了。

2.5求解

不等式建立之后，可以直接丢给非线性求解器求解，比如说Ipopt，OOQP等，具体的自己查一下非线性求解器。

2.6Matlab 例子

这里还是借用大佬的代码，项目地址。

初始条件：
min：snap
等式约束：起点pva，终点pva，中间点的p，中间点pva连续
不等式约束：无

根据上述条件，生成$x,y$两个维度的轨迹，合并后如下图所示（包含起始终止共5个点，用四段poly来描述，中间点也就是poly之间的交界点）。

从上图中可以看出，上图规划出的轨迹并不是很好，第二个和第三个路点之间还出现了打结的现象，另外y轴的最大加速度甚至到了20m/s，这明显超过了车辆的物理极限。造成这种现象的最主要的原因，是因为时间分配的不合理，至于时间分配的问题，下面会有详细的讨论。

因此实际轨迹需要进行 feasibility check （可行性检测），确保满足车辆的实际物理可行性，比如前轮偏角，加速度，jerk等。

3.corridor 与时间分配

为了解决上述合理分配时间的问题，现在主要有两个四路：

1. 把这些“期望”加入到优化问题中；
2. 调整时间分配，避免出现上述问题。

3.1corridor

为了限制规划出的轨迹的形状，引入了corridor的概念，就是人为的添加一条可行通道，如下图，使得规划出的轨迹必须在corridor内。其实也就是给QP问题加入了一个新的条件，这样就可以保证求解出的轨迹在corridor内了。

3.2corridor 如何添加

很容易想到，把之前的等式约束$Ap=d$改成不等式约束$Ap\le d_1$和$Ap\ge d_2$。然而，不管是等式约束还是不等式约束,都是针对一个特定时刻，然而实际希望的是对所有的时刻$t\in [0,T]$，都需要在corridor中。

一个简单粗暴的思路：在路径上多采样一些中间点，每个中间点都加corridor约束。尽管这种方法理论上只能保证采样点在corridor中，但实际过程中，如果corridor大小和采样步长设置得合理，而且不Fix waypoints，能得到比较好效果。这里不固定中间点的位置，是为了去掉中间点的强约束，尽量避免轨迹打结，实际中，我们也是不一样要求轨迹完全经过中间点，只要在那附近（corridor）内就行。

3.3构造corridor不等式约束

为了方便构造，对于每一个采样点$p_t$，我们施加一个矩形的corridor，即，
$$x_{min}\le p_{tx}\ge x_{max},y_{min}\le p_{ty}\ge y_{max}$$

这里用矩形corridor是因为斜线corridor（平行四边形）不太好构造，另外我们也只是需要大致放在corridor里面就可以了，对corridor的要求并不是非常的严格，没必要设置一个严格的斜线corridor。

对于每个采样点，设矩形corridor的边长为2r，增加两个位置不等式约束：

$$\begin{gathered} [1,t_i,t_i^2,\dots ,t_i^n]\cdot p\le p(t_i)+r \\ [-1,-t_i,-t_i^2,\dots ,-t_i^n]\cdot p\le -[p(t_i)+r] \end{gathered}$$

大佬的代码在这里：https://github.com/symao/minimum_snap_trajectory_generation

初始条件：

• minmin：snap
• 等式约束：起点$pva$ 3 + 终点$pva$ 3 + 中间点 $pva$ 连续 $3\times (n_{poly}-1)$个
• 不等式约束：中间点位置 corridor $2\ast (n_{poly}-1)$ 个

优化结果如下图所示，绿色的点为按固定步长新采样的中间点，每个中间点都有一个矩形corridor，红色为优化后的轨迹。

可以看到，去掉中间点的位置强约束(等式约束)改成弱约束(corridor不等式约束)后，轨迹比之前要好很多，而且尽管轨迹有小部分超出了corridor（因为只加了采样时刻的位置corridor约束），但基本不会超出太多。

但这带来的一个问题是，为了保证轨迹不超出corridor，需要采样比较密，但这样轨迹的段数就上来的（轨迹段数=waypoint数量-1），优化参数变多了，计算效率会下降。

针对这个问题，

• 一种思路是通过corridor合并来减少中间点、轨迹段数，从而提高计算效率；
• 另一种思路是，不采样中间点，直接规划输出轨迹之后，找打轨迹的极值点（波峰和波谷），若极值点位置超过corridor，则在极值点上添加一个corridor或者位置强约束（“压点”，把极值点压到corridor以内），以此迭代。实验证明，压有限个点（不超过路径点总数），就能把整段轨迹压到corridor内。

3.4广义corridor

上面介绍了位置corridor，也就是位置的不等式约束，同样的，corridor也适用于速度corridor、加速度
corridor甚至更高阶。

这样是不是就会想，通过这种方式来限制车辆行驶的速度和加速度了。不过，实际情况是，这么做并不合适。

为什么呢？举个简单的例子，假设一维空间规划一条轨迹从0到10，给定总时间T=1s，要求最大限速2m/s，满足要求的轨迹根本不存在，即使你加很多中间点并加速度corridor，得到的轨迹在采样点中间肯定会速度蹭的巨高。

速度、加速度之所以很大，是因为时间给的不合理（时间太小），所以，这种情况光靠加corridor是行不通的，还是要从时间分配的层面来解决。

4.时间分配

时间分配是轨迹规划中很关键的一个问题，会直接影响到规划结果的好坏。

4.1初始时间分配

• 总时间给定：通常给定一个平均速度，根据总路程计算得到总时间。
• 各段时间分配：
  匀速分配：假设每段 polynomial 内速度恒定不变，这样各段 poly 的时间比就等于路程比；
  梯形分配：假设每段polynomial内速度曲线为：以恒定加速度$a$从0加速到最大速度$v_{max}$，再以$-a$减速到0。$a$和$v_{max}$事先给定，就能得出每一段所需要的时间。

设各段的时间分别为$t_1^{{}^{\prime }},t_2^{{}^{\prime }},\dots ,t_n^{{}^{\prime }}$，前$k$项之和就可以得到轨迹分段的时刻向量：
$$\begin{gathered} t_0=0 \\ {t}_1=t_1^{{}^{\prime }} \\ {t}_2=t_1+t_2^{{}^{\prime }} \\ \dots \\ {t}_n=t_{n-1}+t_n^{{}^{\prime }} \end{gathered}$$

4.2feasibility check

有了时间分配，就可以规划得到轨迹参数，对于轨迹参数，求速度和加速度曲线的极值，判断是否超过最大限幅，如果所有极值都小于最大限幅，则得到可行轨迹。如果不满足，则需要调整该段的时间。

调整的方法是：增大unfeasible段poly的时间，显然，增大时间会使得速度和加速度都变小。通常以一个固定的比例来调整时间如（k=1.2~1.5），并进行迭代，每调整一次，重新规划轨迹并check feasibility，如若不满足，再次加大时间，直至满足为止。

4.3corridor与时间分配的关联

不等式corridor约束实际上也有时间分配的效果，可以这么想，polynomial的分段点（轨迹中间点）在corridor内移动，实际上就相当于分段点的时刻在移动，中间点在corridor内移动，也就变相调整了前后两段poly分配的时间。

举个简单的例子，如下图所示，两段轨迹的分段点为$a$，初始时间分配为$t1,t2$，比如说想减少前一段的时间$t1$，加大后一段的时间$t2$，其实就相当于把分段点$a$向右挪。也就是说，还是前段poly仍然分配$t1$时间，但走的路程更多了（相当于原来路程所用的时间减少了），后一段poly在$t2$时间走的路程更少了（相当于原来的路程所用时间增加了）。

所以，如果corridor比较大的话，时间调整的空间就会更大，规划出的轨迹会更加的平滑。

4.4归一化时间参数

轨迹规划中容易出现一些数值问题，比如需要求t的高阶指数，如果t比较大，计算数值就会很大，而且最终轨迹的参数会很小，特别是高阶项系数，比如很有可能小于float的精度，所以一般计算过程中最好用double。

但还有一种办法，是把轨迹参数做时间归一化，将时间归一化到$[0,1]$之间。

设未归一化的轨迹 $p(t)=p_0+p_{1}t+\dots +p_n{t}^n$，令时间 $\overline{t}=t/T$，则
$$\overline{p}(\overline{t})=p(\overline{t}T)=p_0+p_1\overline{t}T+\dots +p_n{\overline{t}}^n{T}^n=\sum _{i=0}^n{\overline{p}}_i{\overline{t}}^i$$

其中，${\overline{p}}_i=p_i\times T^i$，$T$为轨迹总时间，$[{\overline{p}}_i{]}_{i=0\to n}$就是归一化时间的轨迹参数，它们不像非归一化参数那么小，便于参数传递。

对于任意时刻$t$，先除以总时间$T$得到归一化时间$\overline{t}$，
$$\begin{gathered} p(t)=\overline{p}(\overline{t})=\sum _{i=0}^n{\overline{p}}_i{\overline{t}}^i \\ v(t)=\frac{\overline{p}(\overline{t})}{dt}=\frac{d\overline{p}(\overline{t})}{d\overline{t}}\frac{d\overline{t}}{dt}={\overline{p}}_i^{{}^{\prime }}(\overline{t})/T \\ a(t)={\overline{p}}_i^{{}^{\prime \prime }}(\overline{t})/T^2 \end{gathered}$$

注意：归一化事件参数后，位置计算和原来相同，但速度计算需要除以 $T$，加速度需要除以$T^2$，即每求一阶导需要除以一个$T$。

5.闭式求解

如果QP问题只有等式约束没有不等式约束，那么是可以闭式求解（close form）的。闭式求解效率要快很多，而且只需要用到矩阵运算，不需要QPsolver。

5.1QP等式约束构建

闭式求解的$Q$矩阵和之前的一样，请看上面的文章，但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成$A_{eq}\cdot p=b$的形式就可以了，而闭式求解中，每段轨迹都可以构造成
$$A_i\cdot p_i=d_i,A_i=[A_0{A}_t{]}_i^T,d_i=[d_0{d}_T{]}_i$$

其中，$d_0$,$d_T$为第$i$段poly的起点和终点的各阶导数组成的向量，比如只考虑$pva$时：$d_0=[p_0,v_0,a_0{]}^T$，当然也可以把更高阶的jerk，snap加入到向量。

注意：不管每段的$pva$是否已知，都要写到向量中。

合并各段轨迹的约束方程得到
$$\underset{k(n+1)\times 6k}{\underbrace{A_{total}}}\left[\begin{array}{c}{p}_1\\ ⋮\\ p_k\end{array}\right]=\left[\begin{array}{c}{d}_1\\ ⋮\\ d_k\end{array}\right]=\underset{6k\times 1}{\underbrace{\left[\begin{array}{c}{p}_1(t_0)\\ v_1(t_0)\\ a_1(t_0)\\ p_1(t_1)\\ v_1(t_1)\\ a_1(t_1)\\ ⋮\\ p_k(t_{k-1})\\ v_k(t_{k-1})\\ a_k(t_{k-1})\\ p_k(t_k)\\ v_k(t_k)\\ a_k(t_k)\end{array}\right]}}$$

上式中，$k$为轨迹段数，$n$为轨迹阶数，设值考虑$pva$，这里的$A_{total}$的阶数为$(n+1)k\times 6k$。

这里为了简化，没有把每段poly的时间戳都改成从0开始，但是实际使用中，为了避免时间戳太大引起的数值问题，每段poly 的时间戳都是设置成从0开始的。

从上式可以看出，$A_{total}$是已知的，而$d$中只有少部分（起点、终点的$pva$）是已知的，其他的大部分是未知的。如果可以求出$d$，那么轨迹的参数$p$可以通过$p=A^{-1}d$直接求出。

5.2求解d

闭式法的思路是：将$d$向量中的变量分成两部分：”$d$中所有已知量组成的Fix部分$d_F$”和”所有未知量组成的Free部分$d_P$”。然后通过推导，根据$d_F$求得$d_P$，从而得到$d$，最后求得$p$。

下面介绍整个推导过程。

5.2.1消除重复变量（连续性约束）

可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。
考虑到连续性（这里假设$pva$连续），$d$向量中很多变量其实重复了，即
$$p_i(t_i)=p_{i+1}(t_i),v_i(t_i)=v_{i+1}(t_i),a_i(t_i)=a_{i+1}(t_i)$$

因此需要一个映射矩阵将一个变量映射到两个重复的变量上，怎么映射？

• 如$\left[\begin{array}{c}a\\ a\end{array}\right]=\left[\begin{array}{c}1\\ 1\end{array}\right]a$，将变量$a$映射到左边向量中的两个变量。

因此，构造映射矩阵$M_{6k\times 3(k+1)}$：
$$\underset{6k\times 1}{\underbrace{\left[\begin{array}{c}{d}_1\\ ⋮\\ d_k\end{array}\right]}}=\underset{M}{\underbrace{\left[\begin{array}{cccccccccc}1& & & & & & & & & \\ & 1& & & & & & & & \\ & & 1& & & & & & & \\ & & & 1& & & & & & \\ & & & & 1& & & & & \\ & & & & & 1& & & & \\ & & & 1& & & & & & \\ & & & & 1& & & & & \\ & & & & & 1& & & & \\ & & & & & & 1& & & \\ & & & & & & & 1& & \\ & & & & & & & & 1& \\ & & & & & & & & & \ddots \end{array}\right]}}\underset{3(k+1)\times 1}{\underbrace{\left[\begin{array}{c}p(t_0)\\ v(t_0)\\ a(t_0)\\ p(t_1)\\ v(t_1)\\ a(t_1)\\ p(t_2)\\ v(t_2)\\ a(t_2)\\ ⋮\\ p(t_k)\\ v(t_k)\\ a(t_k)\end{array}\right]}}$$

即$d=M{d}^{{}^{\prime }}$。

5.2.2向量元素置换

消除掉重复变量之后，需要调整$d^{{}^{\prime }}$中的变量，把fix部分和free部分分开排列，可以构成一个置换矩阵$C$，使得：
$$d^{{}^{\prime }}=C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$$

那么$C$矩阵如何构造？

• 举个例子，设$d^{{}^{\prime }}=\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]$，其中$a,c,d$是已知的（$d_F$），$b$是未知的（$d_P$），构造一个$4\times 4$的单位阵，取$d_F$所在的$(1,3,4)$列放在左边，再取$d_P$所在的第二列放在右边，就构造出置换矩阵$C$。

$$\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]=\underset{C}{\underbrace{\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]}}\left[\begin{array}{c}a\\ c\\ d\\ b\end{array}\right]$$

5.2.3 转成无约束优化问题

由上面两步可得
$$\begin{gathered} d=MC\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right] \\ p=A^{-1}d=\underset{K}{\underbrace{A^{-1}MC}}\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]=K\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right] \end{gathered}$$

代入优化函数
$$\begin{gathered} minJ=p^{T}Qp \\ J={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\underset{R}{\underbrace{K^{T}QK}}\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]={\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}$$

$$Q对称\Rightarrow R对称\Rightarrow =d_F^T{R}_{FF}{d}_F+2d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PP}{d}_P$$

令$J$对$d_P$的倒数$\frac{\partial J}{\partial d_P}=0$，则有极值点，

$$\begin{gathered} 2d_F^T{R}_{FP}+d_P^T{R}_{PP}{d}_P=0 \\ {d}_P=-R_{PP}^{-1}{R}_{FP}^T{d}_F \end{gathered}$$

5.3闭式求解步骤

现在做一下总结：

1. 先确定轨迹阶数（比如5阶），再确定dd向量中的约束量（$pva$），进而根据各段的时间分配求得$A_{total}$；
2. 根据连续性约束构造映射矩阵$M$，并确定$d$向量中哪些量是Fix(比如起点终点$pva$，中间点的$p$等)，哪些量是Free，进而构造置换矩阵$C$，并求得$K=A^{-1}MC$；
3. 计算QP目标函数中的Q（$minJerk/Snap$）并计算$R=K^{T}QK$，根据fix变量的长度将R拆分成$R_{FF},R_{FP},R_{PF},R_{PP}$四块；
4. 填入已知变量得到$d_F$，并根据$d_P=-R_{PP}^{-1}{R}_{FP}^T{d}_F$计算得到$d_P$；
5. 根据公式 $p=K\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$计算得到轨迹参数$p$；
6. 闭式法主要计算量就在A矩阵的求逆，其他计算基本上是矩阵构造，所以效率比较高，但由于没有不等式约束，所以在中间点只能加强约束，corridor不能直接加到QP问题中，只能是通过压点来实现corridor。

在对计算效率要求比较高或者不想用QPsolver时，可以使用闭式法求解。最后附上大神的代码。

参考文献：

1. Richter C, Bry A, Roy N. Polynomial trajectory planning for
   aggressive quadrotor flight in dense indoor
   environments[M]//Robotics Research. Springer International
   Publishing, 2016: 649-666.
2. Vijay Kumar的一系列论文：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.
3. HKUST 沈劭劼老师的工作：Chen J, Liu T, Shen S. Online generation of
   collision-free trajectories for quadrotor flight in unknown
   cluttered environments[C]//Robotics and Automation (ICRA), 2016 IEEE
   International Conference on. IEEE, 2016: 1476-1483.