Minimum Snap闭式求解相关公式推导

1 $Minimum$ $Snap$闭式求解的推导

可以看看我的这几篇Blog1，Blog2，Blog3。

1.1 二次规划等式约束构建

闭式法中的 $Q$ 矩阵计算和之前$Minimum$ $Snap$当中的一样，但约束的形式与之前略为不同，在之前的方法中， 等式约束只要构造成 $[\dots ]p=b$ 的形式就可以了，而闭式法中，每段轨迹都构造成如下：
$$A_i{p}_i=d_i,A_i={\left[A_0{A}_t\right]}_i^T,d_i={\left[d_0,d_T\right]}_i$$
其中 $d_0、d_T$ 为第 $i$ 段轨迹的起点和终点的各阶导数组成的向量，比如只考虑PVA: $d_0={\left[p_0,v_0,a_0\right]}^T$ ， 当然也可以把$jerk、snap$等加入到向量。注意：这里是不管每段端点的$PVA$是否已知，都写进来。 块合并各段轨迹的约束方程得到：
A t o t a l [ p 1 ⋮ p k ] = [ d 1 ⋮ d k ] = [ 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 ) ] ⏟ 6 k × 1 A_{total}\left[\begin{array}{c} p_{1} \\ \vdots \\ p_{k} \end{array}\right]=\left[\begin{array}{c} d_{1} \\ \vdots \\ d_{k} \end{array}\right]=\underbrace{\left[\begin{array}{c} p_{1}\left(t_{0}\right) \\ v_{1}\left(t_{0}\right) \\ a_{1}\left(t_{0}\right) \\ p_{1}\left(t_{1}\right) \\ v_{1}\left(t_{1}\right) \\ a_{1}\left(t_{1}\right) \\ \vdots \\ p_{k}\left(t_{k-1}\right) \\ v_{k}\left(t_{k-1}\right) \\ a_{k}\left(t_{k-1}\right) \\ p_{k}\left(t_{k}\right) \\ v_{k}\left(t_{k}\right) \\ a_{k}\left(t_{k}\right) \end{array}\right]}_{6 k \times 1} Atotal​ ​p1​⋮pk​​ ​= ​d1​⋮dk​​ ​=6k×1 ​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​)​ ​​​
$k$ 为轨迹段数， $n$ 为轨迹的阶数，设只考虑pva， $A_{\text{total }}$ 的$size$为 $\left(n_{\text{order }}+1\right)k\times 6k$ 。
由上式可以看到， $A_{\text{total }}$ 是已知的 ，而 $d$ 中只有少部分（起点、终点的$PVA$等）是已知的，其他大部分是未知的。如果能够求出 $\mathbf{d}$ ，那么轨迹参数可以通过 $p=A^{-1}d$ 很容易求得。

1.2 求$d$

闭式法的思路是: 将 $d$ 向量中的变量分成两部分："$d$中所有已知量组成的$Fix$部分 $d_F$ "和”所有末知量组成的$Free$部分 $d_P$ ”。然后通过推导，根据 $d_F$ 求得 $d_P$ ，从而得到 $d$ ，最后求得 $p$ 。 下面介绍整个推导过程。
消除重复变量（连续性约束）
可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。 考虑到连续性 (这里假设PVA连续)， $d$ 向量中很多变量其实重复了，即
$$p_i\left(t_i\right)=p_{i+1}\left(t_i\right),v_i\left(t_i\right)=v_{i+1}\left(t_i\right),a_i\left(t_i\right)=a_{i+1}\left(t_i\right)$$
因此需要一个映射矩阵将一个变量映射到两个重复的变量上，如 $\left[\begin{array}{l}a\\ a\end{array}\right]=\left[\begin{array}{l}1\\ 1\end{array}\right]a$ ，将变量 $a$ 映射到左边向量中的两个变量。
所以构造映射矩阵 $M_{6k\times 3(k+1)}$ ：即 $d=M{d}^{\prime }$ 。
向量元素置换
消除掉重复变量之后，需要调整 $d^{\prime }$ 中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵 $C$ ，使得
$$d^{\prime }=C\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]$$
再来构造$C$矩阵即可，$C$阵的构造参考$Minimum$ $Snap$的构造方法，例如设 d ′ = [ a b c d ] d^{\prime}=\left[\begin{array}{l}a \\ b \\ c \\ d\end{array}\right] d′= ​abcd​ ​, 其中 $a,c,d$ 是已知 $\left(d_F\right)，b$ 末知 $\left(d_P\right)$ ，构造一个 $4\times 4$ 的单位阵，取 $d_F$ 所在的 $(1,3,4)$ 列放到左边，再取 ${\mathbf{d}}_P$ 所在的$(2)$列放到右边，就构造出置换矩阵 $\mathbf{C}$ :
[ a b c d ] = [ 1 0 0 0 0 0 0 1 0 1 0 0 0 0 1 0 ] ⏟ C [ a c d b ] \left[\begin{array}{l} a \\ b \\ c \\ d \end{array}\right]=\underbrace{\left[\begin{array}{llll} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]}_{C}\left[\begin{array}{l} a \\ c \\ d \\ b \end{array}\right] ​abcd​ ​=C ​1000​0010​0001​0100​ ​​​ ​acdb​ ​

1.3 转成无约束优化问题

由上面两步可得
$$\begin{array}{c}d=MC\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]\\ p=A^{-1}d=\underset{K}{A^{-1}MC}\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]=K\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]\end{array}$$
代入优化函数：
$$\begin{array}{rl}\min J& =p^{T}Qp\\ J& ={\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]}^T\underset{R}{K^{T}QK}\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]\\ & ={\left[\begin{array}{l}{d}_F\\ d_P\end{array}\right]}^T\left[\begin{array}{ll}{R}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{l}{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\\ Q_{\text{对称 }\Rightarrow R\text{ 对称 }\Rightarrow }\Rightarrow & =d_F^T{R}_{FF}{d}_F+2d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PP}{d}_P\end{array}$$
令 $J$ 对 $d_P$ 的导数 $\frac{\partial J}{\partial d_P}=0$ 求极值点:
$$\begin{array}{c}\Rightarrow 2d_F^T{R}_{FP}+2d_P^T{R}_{PP}{d}_P=0\text{ (注意 }{R}_{PP}^T=R_{PP}\text{ ) }\\ \Rightarrow d_p=-R_{PP}^{-1}{R}_{FP}^T{d}_F\end{array}$$
至此求得 $d_P$ ，从而求出 $p$ 。