(四)路径规划算法---Minimum Snap 轨迹优化问题
时间：2023-08-05 10:37:00
Minimum Snap 轨迹优化问题
文章目录
- Minimum Snap 轨迹优化问题
-
- 1. 前景
- 2. 基本原理
-
- 2.1 轨迹生成条件
- 2.2 评价函数
- 2.3 约束条件
-
- 2.3.1 导数约束(Derivative constraint)
- 2.3.2 连续性约束(Continuity constraint)
- 2.4 问题求解
- 附录
-
- 1. 凸函数的基本概念
- 2. QP问题
1. 前景
介绍了上一章OBVP但是OBVP更关心如何从起点到终点规划最佳路径，但如果中间有许多必要的节点（如检查工作），则需要多次OBVP计算。这种情况可以通过本章的算法来解决。
一般最小化Jerk(acceleration的导数)，目的用于最小化角速度，可用于视觉跟踪(最小化视觉变化率)。最小化snap(jerk导数)可以节约能源。实际上，需要从时间的准确性来综合衡量哪一种最小化。
| 导数次数 | 位置 | 姿态 |
|---|---|---|
| 0 | Position | |
| 1 | Velocity | |
| 2 | Acceleration | Rotation |
| 3 | Jerk | Angular Velocity |
| 4 | Snap | Angular Acceleration |
2. 基本原理
2.1 轨迹生成条件
- 边界条件：起点、终点状态（位置、速度、加速度等）
- 中点：可有 A ? 、 R R T ? A^*、RRT^* A?、RRT?算法提供
- 评价函数：通过评价函数获得轨迹的平滑度指标。
2.2 评价函数
f ( t ) = { f 1 ( t ) = ∑ i = 0 N p 1 , t t i T 0 ≤ t ≤ T 1 f 2 ( t ) = ∑ i = 0 N p 2 , t t i T 1 ≤ t ≤ T 2 . . . f M ( t ) = ∑ i = 0 N p M , t t i T M ? 1 ≤ t ≤ T M f(t) = \left\{ {\begin{matrix} { {f_1}(t) = \sum\nolimits_{i = 0}^N { {p_{1,t}}{t^i} \quad \quad{T_0} \le t \le {T_1}} }\\ { {f_2}(t) = \sum\nolimits_{i = 0}^N { {p_{2,t}}{t^i} \quad \quad{T_1} \le t \le {T_2}} }\\ {...}\\ { {f_M}(t) = \sum\nolimits_{i = 0}^N { {p_{M,t}}{t^i} \quad \quad {T_{M - 1}} \le t \le {T_M}} } \end{matrix}} \right. f(t)=⎩⎪⎪⎨⎪⎪⎧f1(t)=∑i=0Np1,ttiT0≤t≤T1f2(t)=∑i=0Np2,ttiT1≤t≤T2...fM(t)=∑i=0NpM,ttiTM−1≤t≤TM
通过上式可知，每一段轨迹均为多项式表示，为了简化计算过程，每一段轨迹的参数系数一致，即轨迹阶数一致。其次每段轨迹的时间 T T T是固定的，并不像OBVP那样可得到最优的时间 T T T。
因此目标函数，即最小化 f ( t ) f(t) f(t)。单段轨迹 f ( t ) f(t) f(t)矩阵化简得。其中 f ( 4 ) f^{(4)} f(4)中的 4 4 4表示Minimum Snap，状态变量为 ( p , v , a , j ) (p,v,a,j) (p,v,a,j) 4个；对 f ( 4 ) f^{(4)} f(4)进行平方处理，目的是为了防止方程中正负号相消，这点类似最小二乘算法中的平方。
f ( t ) = ∑ i p i t i ⇒ f ( 4 ) ( t ) = ∑ i ≥ 4 i ( i − 1 ) ( i − 2 ) ( i − 3 ) t i − 4 p i ⇒ ( f ( 4 ) ( t ) ) 2 = ∑ i ≥ 4 , l ≥ 4 i ( i − 1 ) ( i − 2 ) ( i − 3 ) l ( l − 1 ) ( l − 2 ) ( l − 3 ) t i + l − 8 p i p l \begin{array}{l} f(t) = \sum\limits_i {
{p_i}{t^i}} \\ \Rightarrow {f^{(4)}}(t) = \sum\limits_{i \ge 4} {i(i - 1)(i - 2)(i - 3){t^{i - 4}}{p_i}} \\ \Rightarrow {({f^{(4)}}(t))^2} = \sum\limits_{i \ge 4,l \ge 4} {i(i - 1)(i - 2)(i - 3)l(l - 1)(l - 2)(l - 3){t^{i + l - 8}}{p_i}{p_l}} \end{array} f(t)=i∑piti⇒f(4)(t)=i≥4∑i(i−1)(i−2)(i−3)ti−4pi⇒(f(4)(t))2=i≥4,l≥4∑i(i−1)(i−2)(i−3)l(l−1)(l−2)(l−3)ti+l−8pipl
单段的目标函数如下，其中 J j , p j , Q j J_j,\bf p_j, \bf Q_j Jj,pj,Qj均为轨迹 j j j段所对应的目标函数、多项式系数所组成的向量、权重矩阵。
J ( T ) = ∫ T j − 1 T j ( f ( 4 ) ( t ) ) 2 d t = [ ⋮ p i ⋮ ] T [ ⋮ ⋯ i ( i − 1 ) ( i − 2 ) ( i − 3 ) l ( l − 1 ) ( l − 2 ) ( l − 3 ) i + l − 7 T i + l − 7 ⋯ ⋮ ] [ ⋮ p i ⋮ ] ⇒ J j ( T ) = p j T Q j p j \begin{matrix} J(T) = \int_{
{T_{j - 1}}}^{
{T_j}} {
{
{({f^{(4)}}(t))}^2}dt} \\ = {\left[ {\begin{matrix} \vdots \\ {
{p_i}}\\ \vdots \end{matrix}} \right]^T}\left[ {\begin{matrix} {}& \vdots &{}\\ \cdots &{\frac{
{i(i - 1)(i - 2)(i - 3)l(l - 1)(l - 2)(l - 3)}}{
{i + l - 7}}{T^{i + l - 7}}}& \cdots \\ {}& \vdots &{} \end{matrix}} \right]\left[ {\begin{matrix} \vdots \\ {
{p_i}}\\ \vdots \end{matrix}} \right]\\ \Rightarrow {J_j}(T) = {\bf{p}}_j^T{
{\bf{Q}}_j}{
{\bf{p}}_j} \end{matrix} J(T)=∫Tj−1Tj(f(4)(t))2dt=⎣⎢⎢⎡⋮pi⋮⎦⎥⎥⎤T⎣⎢⎢⎡⋯⋮i+l−7i(i−1)(i−2)(i−3)l(l−1)(l−2)(l−3)Ti+l−7⋮⋯⎦⎥⎥⎤⎣⎢⎢⎡⋮pi元器件数据手册、IC替代型号，打造电子元器件IC百科大全！
