(四)路径规划算法---Minimum Snap 轨迹优化问题

Minimum Snap 轨迹优化问题

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^{\ast }、RR{T}^{\ast }$算法提供
• 评价函数：通过评价函数得到轨迹的平滑性指标。

2.2 评价函数

$$f(t)=\{\begin{array}{c}{f}_1(t)={\sum }_{i=0}^N{p}_{1,t}{t}^i{T}_0\le t\le T_1\\ f_2(t)={\sum }_{i=0}^N{p}_{2,t}{t}^i{T}_1\le t\le T_2\\ ...\\ f_M(t)={\sum }_{i=0}^N{p}_{M,t}{t}^i{T}_{M-1}\le t\le T_M\end{array}$$

通过上式可知，每一段轨迹均为多项式表示，为了简化计算过程，每一段轨迹的参数系数一致，即轨迹阶数一致。其次每段轨迹的时间$T$是固定的，并不像OBVP那样可得到最优的时间$T$。

因此目标函数，即最小化$f(t)$。单段轨迹$f(t)$矩阵化简得。其中$f^{(4)}$中的$4$表示Minimum Snap，状态变量为$(p,v,a,j)$ 4个；对$f^{(4)}$进行平方处理，目的是为了防止方程中正负号相消，这点类似最小二乘算法中的平方。
$$\begin{array}{l}f(t)=\sum _i{p}_i{t}^i\\ \Rightarrow f^{(4)}(t)=\sum _{i\ge 4}i(i-1)(i-2)(i-3)t^{i-4}{p}_i\\ \Rightarrow (f^{(4)}(t){)}^2=\sum _{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}$$
单段的目标函数如下，其中$J_j,{\mathbf{p}}_{\mathbf{j}},{\mathbf{Q}}_{\mathbf{j}}$均为轨迹$j$段所对应的目标函数、多项式系数所组成的向量、权重矩阵。
$$\begin{array}{c}J(T)={\int }_{T_{j-1}}^{T_j}{(f^{(4)}(t))}^{2}dt\\ ={\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]}^T\left[\begin{array}{ccc}& ⋮& \\ \cdots & \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}{T}^{i+l-7}& \cdots \\ & ⋮& \end{array}\right]\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]\\ \Rightarrow J_j(T)={\mathbf{p}}_j^T{\mathbf{Q}}_j{\mathbf{p}}_j\end{array}$$
那么整个$M$段的目标函数为
$$J={\mathbf{P}}^T\mathbf{Q}\mathbf{P}={\left[\begin{array}{c}{\mathbf{p}}_1\\ {\mathbf{p}}_2\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]}^T\left[\begin{array}{cccc}{\mathbf{Q}}_1& 0& 0& 0\\ 0& {\mathbf{Q}}_2& 0& 0\\ 0& 0& \ddots & 0\\ 0& 0& 0& {\mathbf{Q}}_M\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_1\\ {\mathbf{p}}_2\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]$$

2.3 约束条件

2.3.1 导数约束(Derivative constraint)

目前已知起点$P_0$、终点$P_N$和中间路标点$P_1、P_2、P_{N-2}、P_{N-1}$的位置，因此导数约束数学表示为
$$f_j^{(k)}(T_j)=x_j^{(k)}$$
其中参数$j$表示第$j$段轨迹，$k$表示导数的阶数，$T_j$表示在$j$段轨迹内从某点到某点所需的时间，$x$表示轨迹的多项式方程。
将上式进行化简
$$\begin{gathered} \begin{array}{c}{f}_j^{(k)}(T_j)=x_j^{(k)}\\ \Rightarrow \sum _{i\ge k}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}=x_{T,j}^{(k)}\\ \Rightarrow \left[\begin{array}{ccc}\dots & \frac{i!}{(i-k)!}{T}_j^{i-k}& \dots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\end{array}\right]=x_{T,j}^{(k)}\end{array} \\ \because \left[\begin{array}{ccc}\dots & \frac{i!}{(i-k)!}{T}_{j-1}^{i-k}& \dots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\end{array}\right]=x_{0,j}^{(k)} \\ \therefore \left[\begin{array}{ccc}\dots & \frac{i!}{(i-k)!}{T}_{j-1}^{i-k}& \dots \\ \dots & \frac{i!}{(i-k)!}{T}_j^{i-k}& \dots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\end{array}\right]=\left[\begin{array}{c}{x}_{0,j}^{(k)}\\ x_{T,j}^{(k)}\end{array}\right] \\ \Rightarrow {\mathbf{A}}_j{\mathbf{p}}_j={\mathbf{d}}_j \end{gathered}$$

2.3.2 连续性约束(Continuity constraint)

要想机器人在中间点之间满足连续性约束，即 使机器人在$j-1$段轨迹的末端的速度、加速度等于$j$段轨迹的初始时刻的速度、加速度。例如**$P_1$点左端的速度、加速度等于右端的速度、加速度**，数学表达式如下
$$f_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)$$
将上式进行化简得
$$\begin{array}{c}l{f}_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)\\ \Rightarrow \sum _{i\ge k}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}-\sum _{i\ge k}\frac{l!}{(l-k)!}{T}_j^{l-k}{p}_{j+1,l}=0\\ \Rightarrow \left[\begin{array}{ccccc}\dots & \frac{i!}{(i-k)!}{T}_j^{i-k}& \dots & -\frac{l!}{(l-k)!}{T}_j^{l-k}& \dots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\\ p_{j+1,i}\\ ⋮\end{array}\right]=0\\ \Rightarrow \left[\begin{array}{cc}{\mathbf{A}}_j& -{\mathbf{A}}_{\mathrm{j}+1}\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_j\\ {\mathbf{p}}_{j+1}\end{array}\right]=0\end{array}$$
联立上述的导数约束和连续性约束，得到最终的约束条件
$${\mathbf{A}}_{eq}\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]={\mathbf{d}}_{eq}$$

2.4 问题求解

联立评价函数和约束条件可得
$$\begin{gathered} minJ={\mathbf{P}}^T\mathbf{Q}\mathbf{P}=min{\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]}^T\left[\begin{array}{ccc}{\mathbf{Q}}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\mathbf{Q}}_M\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right] \\ s.t.{\mathbf{A}}_{eq}\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]={\mathbf{d}}_{eq} \end{gathered}$$
其实上述是一个凸优化（附录1）的问题，可以采用QP进行求解，详情参见附录2。

---

附录

1. 凸函数基本概念

若函数$f$满足
$$\begin{gathered} f(\theta x+(1-\theta )y)\le \theta f(x)+(1-\theta )f(y) \\ 其中0<\theta <1 \end{gathered}$$
则$f$为凸函数，且$-f$为凹函数。

凸集是对于集合内的每一对点，连接该对点的直线段上的每个点也在该集合内,如下图

凸优化一般表达形式如下
$$\begin{gathered} \underset{x}{\mathrm{minimize}}{f}_0(x) \\ \begin{array}{l}\mathrm{s}\mathrm{u}\mathrm{b}\mathrm{j}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{t}\mathrm{o}{f}_i(x)\le 0i=1,...,m\\ Ax=b\end{array} \end{gathered}$$
其中$f_0(x),f_1(x)...f_m(x)$为凸函数，$Ax=b$为反射变换。凸优化的优点：局部最优等于局部最优。

2. QP问题

通过观察，本文Minimum Snap算法的表达式与QP表达式一致，只是将$q^{T}x+r=0$,且不考虑不等式约束$Gx\le h$，仅考虑等式约束$Ax=b$。

注意:上述的图片均来自深蓝学院的路径规划课程，码字不易，转载请注明出处，谢谢