Minimum Snap轨迹规划详解(二)- 闭式求解

转载自https://blog.youkuaiyun.com/q597967420/article/details/79031791
如果QP问题只有等式约束没有不等式约束，那么是可以闭式求解（close form）的。闭式求解效率要快很多，而且只需要用到矩阵运算，不需要QPsolver。
这里介绍Nicholas Roy文章中闭式求解的方法。

1.QP等式约束构建

闭式法中的$Q$矩阵计算和之前一样，但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成$[...]p=b$的形式就可以了，而闭式法中，每段poly都构造成
$$\begin{gathered} M_i{p}_i=d_i \\ {M}_i=[M_0{M}_t{]}_i^T, \\ {d}_i=[d_0,d_T{]}_i \end{gathered}$$
其中 $d_0,d_T$为第 $i$段poly的起点和终点的各阶导数组成的向量，比如只考虑PVA：$d_0=[p_0,v_0,a_0{]}^T]$，当然也可以把jerk，snap等加入到向量。注意：这里是不管每段端点的PVA是否已知，都写进来。
块合并各段轨迹的约束方程得到
$$\begin{array}{r}\underset{k(n+1)\times 6k}{\underbrace{M_{total}}}\left[\begin{array}{c}{p}_1\\ ⋮\\ p_k\end{array}\right]=\left[\begin{array}{c}{d}_1\\ ⋮\\ d_k\end{array}\right]={\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]}_{6k\times 1}\end{array}$$
$k$为轨迹段数，$n$为轨迹的阶数，设只考虑pva， $M_{total}$的size为 $(n_{order}+1)k\times 6k$。这里为了简化，没有把每段poly的timestamp都改成从0开始，一般，为了避免timestamp太大引起数值问题，每段poly的timestamp都成0开始。

由上式可以看到，$M_{total}$是已知的（怎么构造可参见文章一种的等式约束构造方法），而 $d$中只有少部分（起点、终点的pva等）是已知的，其他大部分是未知的。如果能够求出$d$，那么轨迹参数可以通过 $p=M^{-1}d$很容易求得。

2. 如何求d？

闭式法的思路是：将$d$向量中的变量分成两部分：”d中所有已知量组成的Fix部分$d_F$”和”所有未知量组成的Free部分$d_P$”。然后通过推导，根据$d_F$求得$d_P$，从而得到$d$，最后求得$p$。
使得$d=C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$
下面介绍整个推导过程，

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

可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。考虑到连续性（这里假设PVA连续），$d$向量中很多变量其实重复了，即
$$\begin{array}{rl}{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)\end{array}$$
因此需要一个映射矩阵将一个变量映射到两个重复的变量上，怎么映射？

• 如$\left[\begin{array}{c}a\\ a\end{array}\right]=\left[\begin{array}{c}1\\ 1\end{array}\right]a$，将变量$a$映射到左边向量中的两个变量。
  所以构造映射矩阵$C_{1(6k\times 3(k+1))}$：
  $${\left[\begin{array}{c}{d}_1\\ ⋮\\ d_k\end{array}\right]}_{6k\times 1}=\left[\begin{array}{ccccccc}1& & & & & & \\ & 1& & & & & \\ & & 1& & & & \\ & & & 1& & & \\ & & & & 1& & \\ & & & & & 1& \\ & & & 1& & & \\ & & & & 1& & \\ & & & & & 1& \\ & & & & & & \ddots \end{array}\right]{\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]}_{3(k+1)\times 1}$$

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

2.2 向量元素置换

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

• 举个例子，设$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$所在的(2)列放到右边，就构造出置换矩阵$C_2$：
  $$\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]=\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]$$

2.3 转成无约束优化问题

由上面两步可得
$$\begin{array}{rl}d& =C_1{C}_2\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]=C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\\ p& =M^{-1}d=M^{-1}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\end{array}$$
代入优化函数
$$\begin{array}{rl}J& =p^{T}Qp={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T{C}^T{M}^{-T}Q{M}^{-1}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\\ & ={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^{T}R\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{array}$$
令$J$对$d_P$的导数$\frac{\partial J}{\partial d_P}=0$求极值点：
$$\begin{array}{r}2d_F^T{R}_{FP}+2d_P^T{R}_{PP}=0\\ d_P=-R_{PP}^{-1}{R}_{FP}^T{d}_F\end{array}$$

至此求得$d_P$，从而求出$p$。

3. 闭式法步骤

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

• 先确定轨迹阶数（比如5阶），再确定$d$向量中的约束量（pva），进而根据各段的时间分配求得$M_{total}$。
• 根据连续性约束构造映射矩阵$C_1$，并确定$d$向量中哪些量是Fix(比如起点终点pva，中间点的p等)，哪些量是Free，进而构造置换矩阵$C_2$，并求得$C=C_1{C}_2$。
• 计算QP目标函数中的Q（$\min Jerk/Snap$）并计算$R=C^T{M}^{-T}Q{M}^{-1}C$，根据fix变量的长度将R拆分成$R_{FF},R_{FP},R_{PF},R_{PP}$四块。
• 填入已知变量得到$d_F$，并根据$d_p=-R_{PP}^{-1}{R}_{FP}^T{d}_F$计算得到$d_P$。
• 根据公式 $p=M^{-1}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$计算得到轨迹参数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.