Minimum Snap轨迹规划详解（3）闭式求解

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

1. QP等式约束构建

闭式法中的QQ矩阵计算和之前一样（参照文章一），但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成[...]p=b[...]p=b的形式就可以了，而闭式法中，每段poly都构造成

Aipi=di, Ai=[A0 At]Ti, di=[d0,dT]i

A_ip_i=d_i,~A_i=[A_0~A_t]^T_i,~d_i=[d_0,d_T]_i
其中 d0,dTd_0,d_T为第 ii段poly的起点和终点的各阶导数组成的向量，比如只考虑PVA：d0=[p0,v0,a0]Td_0=[p_0,v_0,a_0]^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

\underbrace{A_{total}}_{k(n+1) \times 6k} \left[ \begin{matrix} p_1 \\ \vdots \\ p_k \\ \end{matrix} \right] = \left[ \begin{matrix} d_1 \\ \vdots \\ d_k \\ \end{matrix} \right] =\underbrace{ \left[ \begin{matrix} p_1(t_0)\\ v_1(t_0)\\ a_1(t_0)\\ p_1(t_1)\\ v_1(t_1)\\ a_1(t_1)\\ \vdots \\ 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{matrix} \right]}_{6k \times 1}
kk为轨迹段数，nn为轨迹的阶数，设只考虑pva， AtotalA_{total}的size为 (norder+1)k×6k(n_{order}+1)k \times 6k。这里为了简化，没有把每段poly的timestamp都改成从0开始，一般，为了避免timestamp太大引起数值问题，每段poly的timestamp都成0开始。
由上式可以看到， AtotalA_{total}是已知的（怎么构造可参见文章一种的等式约束构造方法），而 dd中只有少部分（起点、终点的pva等）是已知的，其他大部分是未知的。如果能够求出dd，那么轨迹参数可以通过 p=A−1dp=A^{-1}d很容易求得。

2. 如何求d？

闭式法的思路是：将dd向量中的变量分成两部分：”d中所有已知量组成的Fix部分dFd_F”和”所有未知量组成的Free部分dPd_P”。然后通过推导，根据dFd_F求得dPd_P，从而得到dd，最后求得pp。
下面介绍整个推导过程，

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

可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。考虑到连续性（这里假设PVA连续），dd向量中很多变量其实重复了，即

pi(ti)=pi+1(ti), vi(ti)=vi+1(ti), ai(ti)=ai+1(ti)

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)
因此需要一个映射矩阵将一个变量映射到两个重复的变量上，怎么映射？

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

所以构造映射矩阵M6k×3(k+1)M_{6k\times 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

\underbrace{ \left[ \begin{matrix} d_1 \\ \vdots \\ d_k \\ \end{matrix} \right]}_{6k\times1}= \underbrace{ \left[ \begin{matrix} 1\\ &1\\ &&1\\ &&&1\\ &&&&1\\ &&&&&1\\ &&&1\\ &&&&1\\ &&&&&1\\ &&&&&&1\\ &&&&&&&1\\ &&&&&&&&1\\ &&&&&&&&&\ddots \end{matrix} \right]}_{M} \underbrace{ \left[\begin{matrix} 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)\\ \vdots\\ p(t_k)\\ v(t_k)\\ a(t_k)\\ \end{matrix} \right]}_{3(k+1)\times 1}
即d=Md′d=Md'。

2.2 向量元素置换

消除掉重复变量之后，需要调整d′d'中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵CC，使得

d′=C[dFdP]

d' = C\left[\begin{matrix}d_F\\d_P\end{matrix}\right]。
C矩阵怎么构造？

举个例子，设d′=⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥d' = \left[\begin{matrix}a\\b\\c\\d\end{matrix}\right]，其中a,c,da,c,d是已知(dFd_F)，bb未知(dPd_P)，构造一个4×44\times4的单位阵，取dFd_F所在的(1,3,4)列放到左边，再取dPd_P所在的(2)列放到右边，就构造出置换矩阵CC：

⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢1000001000010100⎤⎦⎥⎥⎥⎥C⎡⎣⎢⎢⎢⎢acdb⎤⎦⎥⎥⎥⎥

\left[ \begin{matrix} a\\b\\c\\d \end{matrix}\right] = \underbrace{\left[\begin{matrix}1&0&0&0\\0&0&0&1\\0&1&0&0\\0&0&1&0 \end{matrix}\right]}_C\left[\begin{matrix}a\\c\\d\\b\end{matrix}\right]

2.3 转成无约束优化问题

由上面两步可得

d=MC[dFdP]p=A−1d=A−1MC⏟K[dFdP]=K[dFdP]

\begin{equation} d=MC\left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ p=A^{-1}d=\underbrace{A^{-1}MC}_K\left[\begin{matrix}d_F\\d_P\end{matrix}\right] = K\left[\begin{matrix}d_F\\d_P\end{matrix}\right] \end{equation}

代入优化函数

minJJQ对称⇒R对称⇒=pTQp=[dFdP]TKTQK⏟R[dFdP]=[dFdP]T[RFFRPFRFPRPP][dFdP]=dTFRFFdF+dTFRFPdP+dTPRPFdF+dTPRPPdP=dTFRFFdF+2dTFRFPdP+dTPRPPdP

\begin{equation} \begin{aligned} \min J&=p^TQp\\ J&= \left[\begin{matrix}d_F\\d_P\end{matrix}\right]^T\underbrace{K^TQK}_R\left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ &=\left[\begin{matrix}d_F\\d_P\end{matrix}\right]^T \left[\begin{matrix}R_{FF} & R_{FP}\\R_{PF}&R_{PP}\end{matrix}\right] \left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ &=d_F^TR_{FF}d_F+d_F^TR_{FP}d_P+d_P^TR_{PF}d_F+d_P^TR_{PP}d_P\\ Q对称 \Rightarrow R对称\Rightarrow &=d_F^TR_{FF}d_F+2d_F^TR_{FP}d_P+d_P^TR_{PP}d_P \end{aligned}\\ \end{equation}
令JJ对dPd_P的导数∂J∂dP=0\frac{\partial J}{\partial{d_P}}=0求极值点：

⇒2dTFRFP+2dTPRPPdP=0 (注意RTPP=RPP)⇒dp=−R−1PPRTFPdF

\begin{equation} \Rightarrow 2d_F^TR_{FP}+2d_P^TR_{PP}d_P=0~~(注意R_{PP}^T=R_{PP})\\ \Rightarrow d_p = -R_{PP}^{-1}R_{FP}^Td_F \end{equation}
至此求得dPd_P，从而求出pp。

3. 闭式法步骤

总结一下整个闭式法的步骤：

先确定轨迹阶数（比如5阶），再确定dd向量中的约束量（pva），进而根据各段的时间分配求得Atotal。A_{total}。
根据连续性约束构造映射矩阵MM，并确定dd向量中哪些量是Fix(比如起点终点pva，中间点的p等)，哪些量是Free，进而构造置换矩阵CC，并求得K=A−1MCK=A^{-1}MC。
计算QP目标函数中的Q（minJerk/Snap\min Jerk/Snap）并计算R=KTQKR=K^TQK，根据fix变量的长度将R拆分成RFF,RFP,RPF,RPPR_{FF},R_{FP},R_{PF},R_{PP}四块。
填入已知变量得到dFd_F，并根据dp=−R−1PPRTFPdFd_p = -R_{PP}^{-1}R_{FP}^Td_F计算得到d_P。
根据公式 p=K[dFdP]计算得到轨迹参数p。p=K\left[\begin{matrix}d_F\\d_P\end{matrix}\right]计算得到轨迹参数p。
闭式法主要计算量就在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.