1 $M inim u m$ $S na p$ 闭式求解的推导

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

1.1 二次规划等式约束构建

闭式法中的 $Q$ 矩阵计算和之前 $M inim u m$ $S na p$ 当中的一样，但约束的形式与之前略为不同，在之前的方法中，等式约束只要构造成 $[\ldots] 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}$ ，当然也可以把 $j er k 、 s na p$ 等加入到向量。注意：这里是不管每段端点的 $P V A$ 是否已知，都写进来。块合并各段轨迹的约束方程得到：
$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}$
$k$ 为轨迹段数， $n$ 为轨迹的阶数，设只考虑pva， $A_{\text {total }}$ 的 $s i ze$ 为 $\left(n_{\text {order }}+1\right) k \times 6 k$ 。
由上式可以看到， $A_{\text {total }}$ 是已知的，而 $d$ 中只有少部分（起点、终点的 $P V A$ 等）是已知的，其他大部分是未知的。如果能够求出 $\boldsymbol{d}$ ，那么轨迹参数可以通过 $p=A^{-1} d$ 很容易求得。

1.2 求 $d$

闭式法的思路是: 将 $d$ 向量中的变量分成两部分：" $d$ 中所有已知量组成的 $F i x$ 部分 $d_{F}$ "和”所有末知量组成的 $F ree$ 部分 $d_{P}$ ”。然后通过推导，根据 $d_{F}$ 求得 $d_{P}$ ，从而得到 $d$ ，最后求得 $p$ 。下面介绍整个推导过程。
消除重复变量（连续性约束）
可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。考虑到连续性 (这里假设PVA连续)， $d$ 向量中很多变量其实重复了，即
$p_{i}\left(t_{i}\right)=p_{i+1}\left(t_{i}\right), \quad v_{i}\left(t_{i}\right)=v_{i+1}\left(t_{i}\right), \quad 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_{6 k \times 3(k+1)}$ ：即 $d^{\prime}$ 。
向量元素置换
消除掉重复变量之后，需要调整 $d^{\prime}$ 中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵 $C$ ，使得
$d^{\prime}=C\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right]$
再来构造 $C$ 矩阵即可， $C$ 阵的构造参考 $M inim u m$ $S na p$ 的构造方法，例如设 $d^{\prime}=\left[\begin{array}{l}a \\ b \\ c \\ d\end{array}\right]$ , 其中 $a, c, d$ 是已知 $\left(d_{F}\right) ， b$ 末知 $\left(d_{P}\right)$ ，构造一个 $\times 4$ 的单位阵，取 $d_{F}$ 所在的 $(1, 3, 4)$ 列放到左边，再取 $\boldsymbol{d}_{P}$ 所在的 $(2)$ 列放到右边，就构造出置换矩阵 $\boldsymbol{C}$ :
$\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]$

1.3 转成无约束优化问题

由上面两步可得
$\begin{gathered} d=M C\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right] \\ p=A^{-1} d=\underbrace{A^{-1} M C}_{K}\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right]=K\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right] \end{gathered}$
代入优化函数：
$\begin{aligned} \min J &=p^{T} Q p \\ J &=\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right]^{T} \underbrace{K^{T} Q K}_{R}\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_{F F} & R_{F P} \\ R_{P F} & R_{P P} \end{array}\right]\left[\begin{array}{l} d_{F} \\ d_{P} \end{array}\right] \\ &=d_{F}^{T} R_{F F} d_{F}+d_{F}^{T} R_{F P} d_{P}+d_{P}^{T} R_{P F} d_{F}+d_{P}^{T} R_{P P} d_{P} \\ Q_{\text {对称 } \Rightarrow R \text { 对称 } \Rightarrow} \Rightarrow &=d_{F}^{T} R_{F F} d_{F}+2 d_{F}^{T} R_{F P} d_{P}+d_{P}^{T} R_{P P} d_{P} \end{aligned}$
令 $J$ 对 $d_{P}$ 的导数 $\frac{\partial J}{\partial d_{P}}=0$ 求极值点:
$\begin{gathered} \Rightarrow 2 d_{F}^{T} R_{F P}+2 d_{P}^{T} R_{P P} d_{P}=0 \text { (注意 } R_{P P}^{T}=R_{P P} \text { ) } \\ \Rightarrow d_{p}=-R_{P P}^{-1} R_{F P}^{T} d_{F} \end{gathered}$
至此求得 $d_{P}$ ，从而求出 $p$ 。