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

83 阅读 0 评论 55 点赞

我是靠谱客的博主虚幻果汁，最近开发中收集的这篇文章主要介绍Minimum Snap轨迹规划详解（1）轨迹规划入门，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

1. 轨迹规划是什么？

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

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

2. 轨迹是什么？

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

p (t) = p 0 + p 1 t + p 2 t 2 . . . + p n t n = \sum i = 0 n p i t i

$p(t) = p_0+p_1t+p_2t^2...+p_nt^n=sum_{i=0}^{n}p_it^i$
其中

p0,p1,...,pn $p_0,p_1,...,p_n$ 为轨迹参数（n+1个），设参数向量

p=[p0,p1,...,pn]T $p=[p_0,p_1,...,p_n]^T$ ，则轨迹可以写成向量形式，

p (t) = [1, t, t 2, . . ., t n] \cdot p

$p(t) = [1,t,t^2,...,t^n]cdot p$
对于任意时刻

t $t$ ，可以根据参数计算出轨迹的位置P(osition)，速度V(elocity)，加速度A(cceleration)，jerk，snap等。

v (t) = p' (t) = [0, 1, 2 t, 3 t 2, 4 t 3, . . ., n t n - 1] \cdot p a (t) = p'' (t) = [0, 0, 2, 6 t, 12 t 2, . . ., n (n - 1) t n - 2] \cdot p j e r k (t) = p (3) (t) = [0, 0, 0, 6, 24 t, . . ., n ! ( n - 3 ! ) t n - 3] \cdot p s n a p (t) = p (4) (t) = [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] \cdot p

$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}$

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

p (t) = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ [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

$begin{equation} p(t) = begin{cases} [1,t,t^2,...,t^n]cdot p_1~~~t_0leq t<t_1\ [1,t,t^2,...,t^n]cdot p_2~~~t_1leq t<t_2\ ...\ [1,t,t^2,...,t^n]cdot p_k~~~t_{k-1}leq t<t_k\ end{cases} end{equation}$

k $k$ 为轨迹的段数，

pi=[pi0,pi1,...,pin]T $p_i=[p_{i_0},p_{i_1},...,p_{i_n}]^T$ 为第i段轨迹的参数向量。

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

3. Minimum Snap轨迹规划

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

min f (p) s . t . A e q p = b e q, A i e q p \leq b i e q

$begin{equation} min f(p)\ s.t.~~A_{eq}p = b_{eq},\ ~~~~~~~~A_{ieq}p leq b_{ieq} end{equation}$
这样，就可以通过最优化的方法求解出目标轨迹参数

p $p$ 。注意：这里的轨迹参数 $p$ 是多端polynomial组成的大参数向量 $p = [p_1^T,p_2^T,...,p_k^T]^T$ 。
我们要做的就是： 将优化问题中的 $f(p)$ 函数和 $A_{eq},b_{eq},A_{ieq},b_{ieq}$ 参数给列出来，然后丢到优化器中求解轨迹参数p。
Minimum Snap顾名思义，Minimum Snap中的最小化目标函数是Snap（加加加速度），当然你也可以最小化Acceleration（加速度）或者Jerk（加加速度），至于它们之间有什么区别，quora上有讨论。一般不会最小化速度。
$m i n i m u m s n a p : min f (p) = min (p (4) (t)) 2 m i n i m u m j e r k : min f (p) = min (p (3) (t)) 2 m i n i m u m a c c e : min f (p) = min (p (2) (t)) 2$ $begin{equation} minimum~snap:~min f(p)=min (p^{(4)}(t))^2 \ minimum~jerk:~min f(p)=min (p^{(3)}(t))^2 \ minimum~acce:~min f(p)=min (p^{(2)}(t))^2 \ end{equation}$

4. 一个简单的例子

给定包含起点终点在内的k+1个二维路径点 $pt_0,pt_1,...,pt_k，pt_i=(x_i,y_i)$ ，给定起始速度和加速度为 $v_0,a_0$ ，末端加速度为 $v_e,a_e$ ，给定时间T，规划出经过所有路径点的平滑轨迹。

a. 初始轨迹分段与时间分配

根据路径点，将轨迹分为k段，计算每段的距离，按距离平分时间T(匀速时间分配)，得到时间序列 $t_0,t_1,...,t_k$ 。对x,y维度单独规划轨迹。后面只讨论一个维度。
时间分配的方法：匀速分配或梯形分配，假设每段polynomial内速度满足匀速或梯形速度变化，根据每段的距离将总时间T分配到每段。
这里的轨迹分段和时间分配都是初始分配，在迭代算法中，如果corridor check和feasibility check不满足条件，会插点或增大某一段的时间，这个后续细说。

b. 构建优化函数

Minimum Snap的优化函数为：

$min \int T 0 (p (4) (t)) 2 d t = min \sum i = 1 k \int t i t i - 1 (p (4) (t)) 2 d t = min \sum i = 1 k \int t i t i - 1 ([0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] \cdot p) T [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] \cdot p d t = min \sum i = 1 k p T \int t i t i - 1 [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] T [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] d t p = min \sum i = 1 k p T Q i p$ $begin{equation} min int _0^T(p^{(4)}(t))^2 {rm d}t\ =min sum_{i=1}^k int _{t_{i-1}}^{t_i}(p^{(4)}(t))^2 {rm d}t\ =min sum_{i=1}^k int _{t_{i-1}}^{t_i} ([0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]cdot p)^T[0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]cdot p~{rm d}t\ =min sum_{i=1}^k p^Tint _{t_{i-1}}^{t_i}[0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]^T[0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]~{rm d}t~p\ =min sum_{i=1}^k p^TQ_ip end{equation}$
其中，
$Q i = \int t i t i - 1 [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] T [0, 0, 0, 0, 24, . . ., n ! ( n - 4 ! ) t n - 4] d t = ⎡ ⎣ ⎢ 0 4 \times 4 0 (n - 3) \times 4 0 4 \times (n - 3) r ! ( r - 4 ) ! c ! ( c - 4 ) ! 1 ( r - 4 ) + ( c - 4 ) + 1 (t (r + c - 7) i - t (r + c - 7) i - 1) ⎤ ⎦ ⎥$ $begin{equation} Q_i = int _{t_{i-1}}^{t_i}[0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]^T[0,0,0,0,24,...,frac{n!}{(n-4!)}t^{n-4}]~{rm d}t \ =left[begin{matrix} 0_{4times 4} & 0_{4times (n-3)}\ 0_{(n-3) times 4} & frac{r!}{(r-4)!}frac{c!}{(c-4)!}frac{1}{(r-4)+(c-4)+1}(t_{i}^{(r+c-7)}-t_{i-1}^{(r+c-7)}) end{matrix}right] end{equation}$
注意：r,c为矩阵的行索引和列索引， 索引从0开始，即第一行r=0。
$Q = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ Q 1 Q 2 ⋱ Q k ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ min p T Q p$ $begin{equation} Q = left[begin{matrix} Q_1 &&&\ &Q_2&&\ &&ddots &\ &&&Q_k end{matrix}right] \ min p^TQp end{equation}$
可以看到，问题建模成了一个数学上的二次规划（Quadratic Programming，QP）问题。

c. 构建等式约束方程

设定某一个点的位置、速度、加速度或者更高为一个特定的值，可以构成一个等式约束。例如：
$位置约束： [1, t 0, t 20, . . ., t n 0, 0...0      (k - 1) (n + 1)] p = p 0 速度约束： [0, 1, 2 t 0, . . ., n t n - 1 0, 0...0      (k - 1) (n + 1)] p = v 0 加速度约束： [0, 0, 2, . . ., n (n - 1) t n - 2 0, 0...0      (k - 1) (n + 1)] p = a 0$ $begin{equation} 位置约束：[1,t_0,t_0^2,...,t_0^n,underbrace{0...0}_{(k-1)(n+1)}]p = p_0\ 速度约束：[0,1,2t_0,...,nt_0^{n-1},underbrace{0...0}_{(k-1)(n+1)}]p = v_0\ 加速度约束：[0,0,2,...,n(n-1)t_0^{n-2},underbrace{0...0}_{(k-1)(n+1)}]p = a_0 end{equation}$
由于要过中间点，对中间点的位置也构建等式约束，方法同上。
相邻段之间的位置、速度、加速度连续可以构成一个等式约束，例如第i、i+1段的位置连续构成的等式约束为
$[0...0      (i - 1) (n + 1), 1, t i, t 2 i, . . ., t n i, - 1, - t i, - t 2 i, . . ., - t n i, 0...0      (k - i - 1) (n + 1)] p = 0$ $[underbrace{0...0}_{(i-1)(n+1)},1,t_i,t_i^2,...,t_i^n,-1,-t_i,-t_i^2,...,-t_i^n,underbrace{0...0}_{(k-i-1)(n+1)}]p=0$
速度、加速度连续类似，不再罗列。

合并所有等式约束，得到

$⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1, t 0, t 20, . . ., t n 0, 0...0      (k - 1) (n + 1) 0, 1, 2 t 0, . . ., n t n - 1 0, 0...0      (k - 1) (n + 1) 0, 0, 2, . . ., n (n - 1) t n - 2 0, 0...0      (k - 1) (n + 1) ⋮ 0...0      (i - 1) (n + 1), 1, t i, t 2 i, . . ., t n i, 0...0      (k - i) (n + 1) ⋮ 0...0      (k - 1) (n + 1), 1, t k, t 2 k, . . ., t n k 0...0      (k - 1) (n + 1), 0, 1, 2 t k, . . ., n t n - 1 k 0...0      (k - 1) (n + 1), 0, 0, 2, . . ., n (n - 1) t n - 2 k 0...0      (i - 1) (n + 1), 1, t i, t 2 i, . . ., t n i, - 1, - t i, - t 2 i, . . ., - t n i, 0...0      (k - i - 1) (n + 1) 0...0      (i - 1) (n + 1), 0, 1, 2 t i, . . ., n t n - 1 i, - 0, - 1, - 2 t i, . . ., - n t n - 1 i, 0...0      (k - i - 1) (n + 1) 0...0      (i - 1) (n + 1), 0, 0, 2, . . ., n ! ( n - 2 ) ! t n - 2 i, - 0, - 0, - 2, . . ., - n ! ( n - 2 ) ! t n - 2 i, 0...0      (k - i - 1) (n + 1) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ (4 k + 2) \times (n + 1) k p = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ p 0 v 0 a 0 ⋮ p i ⋮ p k v k a k 0 ⋮ 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥$ $begin{equation} left[begin{matrix} 1,t_0,t_0^2,...,t_0^n,underbrace{0...0}_{(k-1)(n+1)}\ 0,1,2t_0,...,nt_0^{n-1},underbrace{0...0}_{(k-1)(n+1)}\ 0,0,2,...,n(n-1)t_0^{n-2},underbrace{0...0}_{(k-1)(n+1)}\ vdots\ underbrace{0...0}_{(i-1)(n+1)} ,1,t_i,t_i^2,...,t_i^n,underbrace{0...0}_{(k-i)(n+1)}\ vdots\ underbrace{0...0}_{(k-1)(n+1)},1,t_k,t_k^2,...,t_k^n\ underbrace{0...0}_{(k-1)(n+1)},0,1,2t_k,...,nt_k^{n-1}\ underbrace{0...0}_{(k-1)(n+1)},0,0,2,...,n(n-1)t_k^{n-2}\ underbrace{0...0}_{(i-1)(n+1)} ,1,t_i,t_i^2,...,t_i^n,-1,-t_i,-t_i^2,...,-t_i^n,underbrace{0...0}_{(k-i-1)(n+1)}\ underbrace{0...0}_{(i-1)(n+1)} ,0,1,2t_i,...,nt_i^{n-1},-0,-1,-2t_i,...,-nt_i^{n-1},underbrace{0...0}_{(k-i-1)(n+1)}\ underbrace{0...0}_{(i-1)(n+1)} ,0,0,2,...,frac{n!}{(n-2)!}t_i^{n-2},-0,-0,-2,...,-frac{n!}{(n-2)!}t_i^{n-2},underbrace{0...0}_{(k-i-1)(n+1)}\ end{matrix}right]_{(4k+2)times (n+1)k}p=\ left[begin{matrix} p_0\ v_0\ a_0\ vdots\ p_i\ vdots\ p_k\ v_k\ a_k\ 0\ vdots\ 0 end{matrix}right] end{equation}$
等式约束个数=3（起始PVA）+k-1（中间点的p）+3（终点pva）+3(k-1)（中间点PVA连续）=4k+2

d. 构建不等式约束

不等式约束与等式约束类似，也是设置某个点的P、V、A小于某一特定值，从而构建 $A_{ieq}p=b_{ieq}$ ，不等式约束一般是在corridor中用的比较多，这里暂时先不使用不等式约束。

e. 求解

利用QP求解器进行求解，在MATLAB中可以使用quadprog() 函数，C++的QP求解器如OOQP，也可以自己去网上找。

实验结果

MATLAB代码在这里。
优化列表：

min：snap 等式约束：起点pva，终点pva，中间点的p，中间点pva连续不等式约束：无

生成x、y两个维度的轨迹，合并后如下图所示。包含起始终止共5个点，用四段poly来描述，中间点也就是poly之间的交界点。

5. 轨迹怎么用？（轨迹跟踪）

至此，我们已经求得了轨迹（很多段高阶多项式的参数），但怎么用来控制机器人运动呢？轨迹跟踪是：根据轨迹和机器人当前状态（当前位置、速度、加速度），输出机器人控制指令（速度、加速度、角速度等），控制机器人沿着轨迹运动。有很多种跟踪方法

最简单的跟踪方法是位置控制：计算轨迹上离当前位置最近的点，以最近点为期望位置做位置控制，即 $v=k_p(p_{nearest}-p_{cur})$
Minimum Snap中的前馈控制：计算轨迹上离最近点的（位置 $p_e$ 、速度 $v_e$ 、加速度 $a_e$ ），
$速度指令： v 加速度前馈： a = v e = a e + k p (p e - p c u r) + k d (v e - v c u r)$ $begin{equation} begin{aligned} 速度指令：v &= v_e\ 加速度前馈：a&=a_e+k_p(p_e-p_{cur})+k_d(v_e-v_{cur}) end{aligned} end{equation}$

6. 小结

轨迹规划问题通常建模成一个带约束的二次规划（QP）问题来求解，优化函数可以是snap、jerk、acceleration及它们的组合或其他任何能够formulate成 $p^TQp$ 形式的函数，约束包括等式约束和不等式约束。
轨迹规划中默认时间t已知，通常根据期望速度和总路程计算一个总时间T，再按照匀速运动和梯形速度曲线分配到每段polynomial上。
上面例子中规划出的轨迹并不是很好，有以下问题：
a) 轨迹与路径相差有点大，而且在第三个waypoint处会有打结的现象；
b) y轴的加速度非常大（接近 $20m/s^2$ ），超过了机器人的最大加速度。实际轨迹需要进行feasibility check（可行性检测），确保满足工程可行性，比如最大速度、最大角速度限制等。
这两个问题的根本原因在于时间给的不合理，时间分配是轨迹规划中比较蛋疼的问题，给的时间太小，速度、加速度自然就很大，两段时间分配不当就会生成打结的轨迹。下一节，专门讨论时间分配问题。

参考文献

Richter C, Bry A, Roy N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[M]//Robotics Research. Springer International Publishing, 2016: 649-666.
Vijay Kumar的一系列论文：Mellinger D, Kumar V. Minimum snap trajectory generation and control for quadrotors[C]//Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 2520-2525.

最后

以上就是虚幻果汁为你收集整理的Minimum Snap轨迹规划详解（1）轨迹规划入门的全部内容，希望文章能够帮你解决Minimum Snap轨迹规划详解（1）轨迹规划入门所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。

本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。

本文分类：MinimunSnap轨迹规划
浏览次数：83 次浏览
发布日期：2023-06-11 21:29:03
本文链接：https://www.kaopuke.com/article/k-p-k_14_ujokf3_13_j_22_z.html

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

概述

1. 轨迹规划是什么？

2. 轨迹是什么？

3. Minimum Snap轨迹规划

4. 一个简单的例子

a. 初始轨迹分段与时间分配

b. 构建优化函数

c. 构建等式约束方程

d. 构建不等式约束

e. 求解

实验结果

5. 轨迹怎么用？（轨迹跟踪）

6. 小结

参考文献

最后

评论列表共有 0 条评论

发表评论取消回复

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

概述

1. 轨迹规划是什么？

2. 轨迹是什么？

3. Minimum Snap轨迹规划

4. 一个简单的例子

a. 初始轨迹分段与时间分配

b. 构建优化函数

c. 构建等式约束方程

d. 构建不等式约束

e. 求解

实验结果

5. 轨迹怎么用？（轨迹跟踪）

6. 小结

参考文献

最后

相关文章

评论列表共有 0 条评论

发表评论 取消回复

发表评论取消回复