Minimum Snap轨迹规划详解（3）闭式求解
如果QP问题只有等式约束没有不等式约束，那么是可以闭式求解（close form）的。闭式求解效率要快很多，而且只需要用到矩阵运算，不需要QPsolver。
这里介绍Nicholas Roy文章中闭式求解的方法。
1. QP等式约束构建
闭式法中的 Q 矩阵计算和之前一样（参照文章一），但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成 [...]p=b 的形式就可以了，而闭式法中，每段poly都构造成
Aipi=di, Ai=[A0 At]Ti, di=[d0,dT]i
其中 d0,dT 为第 i 段poly的起点和终点的各阶导数组成的向量，比如只考虑PVA： d0=[p0,v0,a0]T ，当然也可以把jerk，snap等加入到向量。注意：这里是不管每段端点的PVA是否已知，都写进来。
块合并各段轨迹的约束方程得到
Atotal⏟k(n+1)×6k⎡⎣⎢⎢⎢p1⋮pk⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢d1⋮dk⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢p1(t0)v1(t0)a1(t0)p1(t1)v1(t1)a1(t1)⋮pk(tk−1)vk(tk−1)ak(tk−1)pk(tk)vk(tk)ak(tk)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥6k×1
k 为轨迹段数， n 为轨迹的阶数，设只考虑pva， Atotal 的size为 (norder+1)k×6k 。这里为了简化，没有把每段poly的timestamp都改成从0开始，一般，为了避免timestamp太大引起数值问题，每段poly的timestamp都成0开始。
由上式可以看到， Atotal 是已知的（怎么构造可参见文章一种的等式约束构造方法），而 d 中只有少部分（起点、终点的pva等）是已知的，其他大部分是未知的。如果能够求出 d ，那么轨迹参数可以通过 p=A−1d 很容易求得。
2. 如何求d？
闭式法的思路是：将 d 向量中的变量分成两部分：”d中所有已知量组成的Fix部分 dF ”和”所有未知量组成的Free部分 dP ”。然后通过推导，根据 dF 求得 dP ，从而得到 d ，最后求得 p 。
下面介绍整个推导过程，
2.1. 消除重复变量（连续性约束）
可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。考虑到连续性（这里假设PVA连续）， d 向量中很多变量其实重复了，即
pi(ti)=pi+1(ti), vi(ti)=vi+1(ti), ai(ti)=ai+1(ti)
因此需要一个映射矩阵将一个变量映射到两个重复的变量上，怎么映射？
- 如 [aa]=[11]a ，将变量 a 映射到左边向量中的两个变量。
所以构造映射矩阵 M6k×3(k+1) ：
⎡⎣⎢⎢⎢d1⋮dk⎤⎦⎥⎥⎥⏟6k×1=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢111111111111⋱⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥M⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢p(t0)v(t0)a(t0)p(t1)v(t1)a(t1)p(t2)v(t2)a(t2)⋮p(tk)v(tk)a(tk)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⏟3(k+1)×1
即 d=Md′ 。
2.2 向量元素置换
消除掉重复变量之后，需要调整 d′ 中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵 C ，使得
d′=C[dFdP]
。
C矩阵怎么构造？
- 举个例子，设 d′=⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥ ，其中 a,c,d 是已知( dF )， b 未知( dP )，构造一个 4×4 的单位阵，取 dF 所在的(1,3,4)列放到左边，再取 dP 所在的(2)列放到右边，就构造出置换矩阵 C ：
⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢1000001000010100⎤⎦⎥⎥⎥⎥C⎡⎣⎢⎢⎢⎢acdb⎤⎦⎥⎥⎥⎥
2.3 转成无约束优化问题
由上面两步可得
d=MC[dFdP]p=A−1d=A−1MC⏟K[dFdP]=K[dFdP]
代入优化函数
minJJQ对称⇒R对称⇒=pTQp=[dFdP]TKTQK⏟R[dFdP]=[dFdP]T[RFFRPFRFPRPP][dFdP]=dTFRFFdF+dTFRFPdP+dTPRPFdF+dTPRPPdP=dTFRFFdF+2dTFRFPdP+dTPRPPdP
令 J 对 dP 的导数 ∂J∂dP=0 求极值点：
⇒2dTFRFP+2dTPRPPdP=0 (注意RTPP=RPP)⇒dp=−R−1PPRTFPdF
至此求得 dP ，从而求出 p 。
3. 闭式法步骤
总结一下整个闭式法的步骤：
- 先确定轨迹阶数（比如5阶），再确定 d 向量中的约束量（pva），进而根据各段的时间分配求得 Atotal。
- 根据连续性约束构造映射矩阵 M ，并确定 d 向量中哪些量是Fix(比如起点终点pva，中间点的p等)，哪些量是Free，进而构造置换矩阵 C ，并求得 K=A−1MC 。
- 计算QP目标函数中的Q（ minJerk/Snap ）并计算 R=KTQK ，根据fix变量的长度将R拆分成 RFF,RFP,RPF,RPP 四块。
- 填入已知变量得到 dF ，并根据 dp=−R−1PPRTFPdF 计算得到d_P。
- 根据公式 p=K[dFdP]计算得到轨迹参数p。
闭式法主要计算量就在A矩阵的求逆，其他计算基本上是矩阵构造，所以效率比较高，但由于没有不等式约束，所以在中间点只能加强约束，corridor不能直接加到QP问题中，只能是通过压点来实现corridor。
在对计算效率要求比较高或者不想用QPsolver时，可以使用闭式法求解。
代码见这里，由于效果和文章一中的效果一样，这里就不再贴图。
参考文献
- 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.