**Closed-formed **solution to Minimum snap

之前优化的都是分段多项式每一段的系数，这里存在一些问题，求解p都是没有物理意义的值，可能会带来轨迹优化数值上的不稳定性，所以通过M映射矩阵，把p转化为d(轨迹路径点的v，a)这样数值稳定性会好很多

以minimum snap为例，写出多项式的向量形式：

​

每段多项式有8个未知数，因此需要对轨迹的端点提供位置、速度、加速度，jerk约束（包括微分约束和连续性约束），那么我们8个未知数系数就可以使用两端的位置、速度、加速度、jerk运动量来表示，每段轨迹可以提供8个运动量，因此我们需要找到某一种方式将系数映射到成运动量(位置、速度、加速度等)。

以最小化jerk为例：

​

将分段轨迹两端的时间都带入上式，就得到了多项式系数到运动微分的映射关系。注意在程序实现时多项式系数的排列可能和下图不一样

使用选择矩阵C，可以将连续性约束引入到代价函数中。

• 等式左边是每一段轨迹首尾带入时间的各阶导数，即首尾处的pvaj等。
• 等式右边为df和dp，其中df为起始点的pvaj+k-1个中间航点+结束点的pvaj。dp为从第二段开始到k-1段的自由变量vaj

参考

[Minimum Snap轨迹规划详解(二)- 闭式求解-CSDN博客](https://blog.csdn.net/liuerin/article/details/117329722?ops_request_misc=%7B%22request%5Fid%22%3A%22170720739716800182157767%22%2C%22scm%22%3A%2220140713.130102334..%22%7D&request_id=170720739716800182157767&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-2-117329722-null-null.142^v99^pc_search_result_base1&utm_term=minimum snap闭式解&spm=1018.2226.3001.4187)

深蓝学院资料