Minimum Snap轨迹规划详解（1）轨迹规划入门

1. 轨迹规划是什么？

在机器人导航过程中，如何控制机器人从A点移动到B点，通常称之为运动规划。运动规划一般又分为两步：

1. 路径规划：在地图（栅格地图、四\八叉树、RRT地图等）中搜索一条从A点到B点的路径，由一系列离散的空间点（waypoint）组成。
2. 轨迹规划：由于路径点可能比较稀疏、而且不平滑，为了能更好的控制机器人运动，需要将稀疏的路径点变成平滑的曲线或稠密的轨迹点，也就是轨迹。

2. 轨迹是什么？

轨迹一般用n阶多项式(polynomial)来表示，即

$$p(t) = p_0+p_1t+p_2t^2...+p_nt^n=\sum_{i=0}^{n}p_it^i$$
其中 $p_0,p_1,...,p_n$为轨迹参数（n+1个），设参数向量 $p=[p_0,p_1,...,p_n]^T$，则轨迹可以写成向量形式，
$$p(t) = [1,t,t^2,...,t^n]\cdot p$$
对于任意时刻 $t$，可以根据参数计算出轨迹的位置P(osition)，速度V(elocity)，加速度A(cceleration)，jerk，snap等。
$$\begin{equation} v(t) = p'(t) = [0,1,2t,3t^2,4t^3,...,nt^{n-1}]\cdot p\\ a(t) = p''(t)=[0,0,2,6t,12t^2,...,n(n-1)t^{n-2}]\cdot p\\ jerk(t)=p^{(3)}(t) =[0,0,0,6,24t,...,\frac{n!}{(n-3!)}t^{n-3}]\cdot p \\ snap(t)=p^{(4)}(t) =[0,0,0,0,24,...,\frac{n!}{(n-4!)}t^{n-4}]\cdot p \end{equation}$$

一个多项式曲线过于简单，一段复杂的轨迹很难用一个多项式表示，所以将轨迹按时间分成多段，每段各用一条多项式曲线表示，形如：

$$\begin{equation} p(t) = \begin{cases} [1,t,t^2,...,t^n]\cdot p_1~~~t_0\leq t<t_1\\ [1,t,t^2,...,t^n]\cdot p_2~~~t_1\leq t<t_2\\ ...\\ [1,t,t^2,...,t^n]\cdot p_k~~~t_{k-1}\leq t<t_k\\ \end{cases} \end{equation}$$
$k$为轨迹的段数， $p_i=[p_{i_0},p_{i_1},...,p_{i_n}]^T$为第i段轨迹的参数向量。

此外，实际问题中的轨迹往往是二维、三维甚至更高维，通常每个维度单独求解轨迹。

3. Minimum Snap轨迹规划

轨迹规划的目的：求轨迹的多项式参数$p_1,...,p_k$。
我们可能希望轨迹满足一系列的约束条件，比如：希望设定起点和终点的位置、速度或加速度，希望相邻轨迹连接处平滑（位置连续、速度连续等），希望轨迹经过某些路径点，设定最大速度、最大加速度等，甚至是希望轨迹在规定空间内（corridor）等等。
通常满足约束条件的轨迹有无数条，而实际问题中，往往需要一条特定的轨迹，所以又需要构建一个最优的函数，在可行的轨迹中找出“最优”的那条特定的轨迹。
所以，我们将问题建模（fomulate）成一个约束优化问题，形如：

$$\begin{equation} \min f(p)$$