[运动规划算法]Minimum Snap轨迹规划

---

简介

1.为什么需要平滑轨迹？

• 为了更好地自主移动。
• 速度不能立即改变。
• 机器人不应该在转弯时停止。
• 节省能量，经常地加减速会减少能量。
  

2.平滑轨迹生成

• 边界条件：开始、目标位置（方向）
• 中间条件：航路点位置（方向） •可通过路径规划（A*、RRT*等）找到航路点
• 平滑度的标准
  • 通常是转化为最小化“输入”的变化率

---

一、微分平坦

系统状态可以被平坦空间的状态以及他们的导数所组成的代数组合所表示。

无人机一般认为有12个维度：

---

二、Minimum-snap

1.平滑的一维轨迹

简单的BVP问题（两点边界值问题），已知始末状态，通过参数化来调整平滑度。

使用五次多项式来描述这段轨迹：

---

每个分段的一维轨迹

---

2.基于优化的轨迹生成

• 显式最小化平面输出空间中的某些导数
• 四旋翼动力学
  
• Minimum jerk: 最小化角速度，有利于视觉跟踪
• Minimum snap: 最小化差速推力，节省能量

---

3.Minimum Snap轨迹生成

• 每个段都是一个多项式；
• 每个分段的多项式都是相同的阶次，这样对于问题的求解比较简单；
• 每一段的时间间隔必须是已知的。

Minimum snap问题的限制条件：初始条件以及结束条件的限制（包含有位置，速度，加速度等）；中间节点连续（包含有位置，速度，加速度）。

针对minimum jerk，则需要提供位置，速度，角速度3个状态量。所以N = 2 * 3(jerk) - 1 = 5
针对minimum snap，则需要提供位置，速度，加速度，以及角速度，4个状态量。所以N = 2 * 4-1 = 7

如果考虑多段的情况，例如K段，则minimum jerk ，需要 (k-1) +3 +3= k+5 , 这里只要求能够连续的到达中间点，至于以怎样的速度，怎样的加速度到达这个点，是优化出来的，不属于约束 。

假设每一段的阶数为N ,则每一段轨迹所能提供的自由度为为N+1。N阶多项式可以提供N次导数，加上原多项式，即为N+1。 所以，总计 （N+1）*k。
(N+1)*k>= k+5. 则 N = 5 / k。表明轨迹段数越多，则提供的阶次越低。

构建约束方程

一个多项式分段的导数约束
•还对路径点约束建模（0阶导数）

两段之间的连续性约束：

• 在没有给出特定导数的情况下确保轨迹段之间的连续性
  
  带约束的二次规划（QP）公式：
  

---

三、凸优化

1.凸函数和凸集定义

凸集

2.轨迹优化经常遇到的问题

---

四、闭式求解解决Minimum Snap轨迹规划

在轨迹生成中，解析解中的限制条件没有物理意义，而且数值会在高次产生不稳定性，所以要把对系数（无物理意义）的优化变为对va的优化。

通过映射矩阵M将P与限制条件d相连产生关系。d又可以通过选择矩阵Ct把d中的fix条件和free条件分开为dF dP.所以P就可以换成与条件有关的矩阵，通过qp得到闭式解。

---

参考资料

【1】深蓝学院运动规划课程；
【2】

• Minimum Snap轨迹规划详解（1）轨迹规划入门
  https://blog.youkuaiyun.com/q597967420/article/details/76099491
  

• Minimum Snap轨迹规划详解（2）corridor与时间分配
  https://blog.youkuaiyun.com/q597967420/article/details/77623235
  

• Minimum Snap轨迹规划详解（3）闭式求解
  https://blog.youkuaiyun.com/q597967420/article/details/79031791