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

文章目录

• 1 闭式求解的推导
• • 1.1 二次规划等式约束构建
  • 1.2 求
  • 1.3 转成无约束优化问题

1 闭式求解的推导

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

1.1 二次规划等式约束构建

闭式法中的 矩阵计算和之前 当中的一样，但约束的形式与之前略为不同，在之前的方法中， 等式约束只要构造成 的形式就可以了，而闭式法中，每段轨迹都构造成如下：

其中 为第 段轨迹的起点和终点的各阶导数组成的向量，比如只考虑PVA: ， 当然也可以把等加入到向量。注意：这里是不管每段端点的是否已知，都写进来。 块合并各段轨迹的约束方程得到：

为轨迹段数， 为轨迹的阶数，设只考虑pva， 的为 。
由上式可以看到， 是已知的 ，而 中只有少部分（起点、终点的等）是已知的，其他大部分是未知的。如果能够求出 ，那么轨迹参数可以通过 很容易求得。

1.2 求

闭式法的思路是: 将 向量中的变量分成两部分：”中所有已知量组成的部分 “和”所有末知量组成的部分 ”。然后通过推导，根据 求得 ，从而得到 ，最后求得 。 下面介绍整个推导过程。
消除重复变量（连续性约束）
可以会发现，上面构造等式约束时，并没有加入连续性约束，连续性约束并不是直接加到等式约束中。 考虑到连续性 (这里假设PVA连续)， 向量中很多变量其实重复了，即

因此需要一个映射矩阵将一个变量映射到两个重复的变量上，如 ，将变量 映射到左边向量中的两个变量。
所以构造映射矩阵 ：即 。
向量元素置换
消除掉重复变量之后，需要调整 中的变量，把fix部分和free部分分开排列，可以左成一个置换矩阵 ，使得

再来构造矩阵即可，阵的构造参考 的构造方法，例如设 , 其中 是已知 末知 ，构造一个 的单位阵，取 所在的 列放到左边，再取 所在的列放到右边，就构造出置换矩阵 :

1.3 转成无约束优化问题

由上面两步可得

代入优化函数：

令 对 的导数 求极值点:

至此求得 ，从而求出 。