【移动机器人运动规划】04 ——轨迹生成

前言

本文部分内容参考了深蓝学院的移动机器人运动规划，依此做相关的笔记与整理。

相关代码整理:

1. https://gitee.com/lxyclara/motion-plan-homework/
2. https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master
3. https://gitee.com/aries-wu/Motion-plan/blob/main/
4. 链接: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp 提取码: rhtp

介绍

机器人、无人车经历的轨迹需要满足光滑的特征，同时要符合相应的动力学约束。传统的学院派路线很明显需要在产生路径后生成优化后的轨迹以符合kinodynamic的特征；而已经进行过kinodynamic的路径需要进一步优化轨迹以提高路径的质量。

光滑的轨迹生成通常需要符合以下几点特征：

1. 边界条件：起点、终点的位置（方向等等）
2. 中间条件：中继节点的位置、方向……
3. 光滑的轨迹判断标准：评价函数要合理（比如$\begin{array}{r}\underset{x\to x_0^{+}}{\lim }f(x)\end{array}=\begin{array}{r}\underset{x\to x_0^{-}}{\lim }f(x)\end{array}$）

Minimum Snap Optimization

Differential Flatness(微分平坦)

论文：Minimum Snap Trajectory Generation and Control for Quadrotors, Daniel Mellinger and Vijay Kumar
将全维的状态空间问题$X$转到平坦的空间$\sigma$中进行研究。

四旋翼无人机的状态空间$X$由位置、方向、线速度、角速度组成：
$$X=[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},{\omega }_x,{\omega }_y,{\omega }_z{]}^T$$角速度是在无人机自身坐标系下观测。

四旋翼的状态和输入可以表示为四个平坦空间里的输出变量及其导数的代数函数。
平坦空间中的任何光滑的轨迹（拥有合理的有界导数）可以被欠驱动系统四旋翼跟踪。

在12维的状态空间中选择4维作为平坦空间：$\sigma =[x,y,z,\psi {]}^T$
在$T_0\to T_M$时刻，在平坦空间中的轨迹定义为：${\mathbb{R}}^3\times SO(2)$(${\mathbb{R}}^3$即为$x,y,z,$$SO(2)$为$\psi$)

四旋翼无人机的动力学如下所示，分为两个公式：

${\mathbf{z}}_W$代表了世界坐标系下的$\mathbf{z}$轴，${\mathbf{z}}_B$代表了Body坐标系下的$\mathbf{z}$轴。
从运动等式中可以得到：$${\mathbf{z}}_B=\frac{\mathbf{t}}{\Vert \mathbf{t}\Vert },\mathbf{t}=[{\ddot{\sigma }}_1,{\ddot{\sigma }}_2,{\ddot{\sigma }}_3+g{]}^T$$${\sigma }_1,{\sigma }_2,{\sigma }_3$分别对于$x,y,z$,因此$\mathbf{t}$代表了无人机合加速度的方向。因此，表示了${\mathbf{z}}_B$是$\sigma$的变量代数组合。无人机的姿态可以用${\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B$进行表示，若能确定${\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B$由代数组合，则$\varphi ,\theta ,\psi$也能确定。

接下来定义一个中间坐标系$C$，与world系只差了一个偏航角。$${\mathbf{x}}_C=[\cos {\mathbf{\sigma }}_4,\sin {\mathbf{\sigma }}_4,0{]}^T$$

用${\mathbf{x}}_C$叉乘${\mathbf{z}}_B$可得${\mathbf{y}}_B$
$${\mathbf{y}}_B=\frac{{\mathbf{z}}_B\times {\mathbf{x}}_C}{\Vert {\mathbf{z}}_B\times {\mathbf{x}}_C\Vert },{\mathbf{x}}_B={\mathbf{y}}_B\times {\mathbf{z}}_B{\mathbf{R}}_B=\left[\begin{array}{ccc}{\mathbf{x}}_B& {\mathbf{y}}_B& {\mathbf{z}}_B\end{array}\right]$$

到此，只剩下角速度部分了。
对$m\ddot{\mathbf{p}}=-mg{\mathbf{z}}_W+u_1{\mathbf{z}}_B$求导，得$m\dot{\mathbf{a}}={\dot{u}}_1{\mathbf{z}}_B+({\mathbf{\omega }}_{BW})\times u_1{\mathbf{z}}_B$（这里涉及到刚体运动学的一些知识）$BW$:Body angular velocity
viewed in the world frame
因为无人机只受到垂直方向的推力，则${\dot{u}}_1={\mathbf{z}}_B\cdot m\dot{\mathbf{a}}$
定义一个${\mathbf{h}}_{\omega }$,${\mathbf{h}}_{\omega }={\mathbf{\omega }}_{BW}\times {\mathbf{z}}_B=\frac{m}{u_1}(\dot{\mathbf{a}}-({\mathbf{z}}_B\cdot \dot{\mathbf{a}}){\mathbf{z}}_B)$。${\mathbf{h}}_{\omega }$中的量都可以用平坦空间中表示。
我们知道，${\omega }_{BW}={\omega }_x{\mathbf{x}}_B+{\omega }_y{\mathbf{y}}_B+{\omega }_z{\mathbf{z}}_B$
沿${\mathbf{y}}_B$、${\mathbf{x}}_B$的角速度可以被表示为：${\omega }_x=-{\mathbf{h}}_{\omega }\cdot {\mathbf{y}}_B,{\omega }_y={\mathbf{h}}_{\omega }\cdot {\mathbf{x}}_B$

总结

生成的轨迹是一个多项式，其有以下优点：
•使用多项式阶数轻松确定平滑度标准
•导数的简单闭合形式计算
•三维解耦轨迹生成

Minimum-snap

Smooth 1D Trajectory

Smooth Multi-Segment Trajectory

达到这些点的速度和加速度其实是不好指定的。

Optimization-based Trajectory Generation

最小化jerk相当于最小角速度，可以获得较好的视觉跟踪视野
最小化snap可以节省能量

得到以下公式
$$f(t)=\{\begin{array}{ll}{f}_1(t)\doteq {\sum }_{i=0}^N{p}_{1,i}{t}^i& T_0\le t\le T_1\\ f_2(t)\doteq {\sum }_{i=0}^N{p}_{2,i}{t}^i& T_1\le t\le T_2\\ ⋮& ⋮\\ f_M(t)\doteq {\sum }_{i=0}^N{p}_{M,i}{t}^i& T_{M-1}\le t\le T_M\end{array}$$

• 每一段都是多项式
• 不一定要求每一段轨迹的阶数是一样的，但一样的可以使问题简化。
• 每一段轨迹的时间需要已知

约束：

轨迹光滑、连续以及控制输入最小化是彼此独立的

最小化jerk需要五次轨迹(一段轨迹)
最小化snap需要7次轨迹(一段轨迹)

时间线的设置

一段轨迹目标函数

微分约束，对于起点和终点是k = 0, 1, 2, 3，即要求p，v，a，j；对于waypoint则通常要求k = 0，即只要求p，这两种情况都可以写成矩阵形式的等式约束

连续性约束，对于waypoint虽然不要求v，a，j,但是要求左右两段的轨迹在waypoint处的各阶导数是相等的

Convex Optimization（凸优化）

凸优化相关课程：

1. Boyd斯坦福公开课
2. HKUSThttps://github.com/KellyHwong/convex-hkust
3. 凸优化入门教材与课程推荐与整理

凸函数和凸集

凸函数的数学定义是：若$f(x)$在区间$I$上连续，$I$是实数集上的一个区间，且对于任意$x,y\in I$和任意$\lambda \in [0,1]$，都有$$f(\lambda x+(1-\lambda )y)\le \lambda f(x)+(1-\lambda )f(y)$$则$f(x)$在区间$I$上为凸函数。其中，$\lambda$称为权值或混合比例。该定义是几何意义下的，即对于函数上的任意两点，函数值在这两点间的线段上的每个点都不超过该点对应的线性函数上的值。凸函数也可以用二阶导数的形式定义为$f^{\prime \prime }(x)\ge 0$。
凸集的数学定义：在欧几里得空间中，一个集合$S$为凸集，当且仅当对于任意$x,y\in S$和$0\le \lambda \le 1$，都有
$$\lambda x+(1-\lambda )y\in S$$其中，$\lambda$是一个实数，表示$x_1$和$x_2$之间的比例系数。或者说任意集内的两点所构成的线段均在集内则为凸集。

凸优化问题的标准形式

$$\begin{array}{rl}\mathrm{minimize}& f_0(x)\\ \mathrm{subject}\mathrm{to}& f_i(x)\le 0i=1,\dots ,m\\ & \mathrm{A}x=b\end{array}$$
其中目标函数$f_0$和不等式约束函数$f_i(x)$都是凸函数，$Ax=b$要求是仿射的，$x$的定义域要是一个凸集

Linear Programming (LP)：线性规划问题

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& c^{T}x+d\\ \text{subject to}& Gx\le h\\ & Ax=b\end{array}$$

Quadratic Programming (QP)：二次规划问题

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& \frac{1}{2}{x}^{T}Px+q^{T}x+r\\ \text{subject to}& Gx\le h\\ & Ax=b\end{array}$$

Quadratically Constrained QP (QCQP)：具有二次约束的二次规划问题

QCQP（Quadratically Constrained Quadratic Program）可以用如下形式表示：

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& (1/2)x^T{P}_{0}x+q_0^{T}x+r_0\\ \mathrm{subject}\mathrm{to}& (1/2)x^T{P}_{i}x+q_i^{T}x+r_i\le 0i=1,\dots ,m\\ & Ax=b\end{array}$约束函数也是凸函数

其中，$x\in {\mathbb{R}}^n$ 是优化变量，$P_0\in {\mathbb{S}}^n$, $P_i\in {\mathbb{S}}^n$, $q_0,q_i\in {\mathbb{R}}^n$, $b_i\in \mathbb{R}$, $A\in {\mathbb{R}}^{p\times n}$, $b\in {\mathbb{R}}^p$, $l,u\in {\mathbb{R}}^n$，且满足 $l_i\le u_i$。

其中， ${\mathbb{S}}^n$ 表示所有 $n\times n$ 的实对称矩阵的集合。$\frac{1}{2}{x}^T{P}_{i}x+q_i^{T}x\le b_i$ 表示关于 $x$ 的二次不等式约束，$Ax=b$ 表示线性等式约束，$l_i\le x_i\le u_i$ 表示变量的下界和上界约束。目标是最小化目标函数 $\frac{1}{2}{x}^T{P}_{0}x+q_0^{T}x$。

Second-Order Cone Programming (SOCP)：二阶最优化问题

SOCP是二次锥规划（Second Order Cone Programming）的缩写。二次锥规划是指优化问题的约束条件由二次锥组成的一类凸优化问题。

二次锥规划的一般形式为：

$$\begin{array}{rl}\text{minimize}& c^{T}x\\ \text{subject to}& ||A_{i}x+b_i|{|}_2\le c_i^{T}x+d_i,i=1,...,m\\ & Fx=g\end{array}$$

其中，$x$是优化变量，$c$是一个向量，$A_i$是矩阵，$b_i$是向量，$d_i$和$g$是标量。

主要约束条件是二次锥约束，其中 $||A_{i}x+b_i|{|}_2\le c_i^{T}x+d_i$ 表示向量$A_{i}x+b_i$与向量$c_i^{T}x+d_i$在欧几里得空间中的内积小于等于0。这个约束条件可以写成转化后的形式，即

$$\begin{array}{r}t\ge ||A_{i}x+b_i|{|}_2\\ c_i^{T}x+d_i-t\le 0\end{array}$$

其中$t$是辅助变量，用于确保内积小于等于0。

SOCP可以被视为直接扩展的LP问题。因此，可以使用凸优化进行求解，例如内点法和分支定界法。相较于二次规划，SOCP问题具有更好的凸性。

Semidefinite Programming（SDP）

SDP（Semidefinite Programming）可以用如下形式表示：

$$\begin{array}{rl}\underset{X}{\min }& c^{T}x\\ s.t.& F_0+\sum _{i=1}^n{X}_i{F}_i⪰0\\ & X_i⪰0,i=1,2,\dots ,n\end{array}$$

其中，$X_i\in {\mathbb{S}}^{n_i}$ 是对称半正定矩阵，$c\in {\mathbb{R}}^n$ 是常数向量，$F_i\in {\mathbb{S}}^{n_i}$ 是常数对称矩阵，$F_0\in {\mathbb{S}}^{n_0}$ 是常数对称矩阵，$⪰0$ 表示半正定矩阵。目标是最小化目标函数 $c^{T}x$，满足矩阵不等式约束 $F_0+{\sum }_{i=1}^n{X}_i{F}_i⪰0$ 和半正定约束 $X_i⪰0,i=1,2,\dots ,n$。

不难看出，SDP 可以看做是约束条件为对称半正定矩阵的凸优化问题。其中矩阵不等式约束保证了问题是凸的。

Closed-form Solution to Minimum Snap

论文：Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy
利用QP求解器求解的是Minimum Snap问题的数值解，另一种解法，从代数上直接求出Minimum Snap问题的解析解，即Minimum Snap问题的闭式解法。

Decision variable mapping变量映射

之前的优化问题如下：
J = [ p 1 ⋮ p M ] T [ Q 1 0 0 0 ⋱ 0 0 0 Q M ] [ p 1 ⋮ p M ] J=\begin{bmatrix}\mathbf{p}_1\\\varvdots\\\mathbf{p}_M\end{bmatrix}^T\quad\begin{bmatrix}\mathbf{Q}_1&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\ddots&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{Q}_M\end{bmatrix}\begin{bmatrix}\mathbf{p}_1\\\varvdots\\\mathbf{p}_M\end{bmatrix} J= ​p1​⋮pM​​ ​T ​Q1​00​0⋱0​00QM​​ ​ ​p1​⋮pM​​ ​
可以看到，优化的变量是轨迹多项式的系数$p_0,p_1,...,p_M$,系数没有实际的物理含义，例如$p_{10}{t}^{10}$时，当$t$较大时，$p_{10}$就可能非常小，会带来轨迹规划问题上数值不稳定的问题。因此，我们需要将轨迹优化上系数的问题转化为轨迹点的导数约束(也就是各个点的$p,v,a$，拥有物理含义)。通过映射矩阵$M_j$将待求解的变量$p_j$映射到轨迹点的导数约束$d_j$，即$M_j{p}_j=d_j$。

新的目标函数：

J = [ d 1 ⋮ d M ] T [ M 1 0 0 0 ⋮ 0 0 0 M M ] − T [ Q 1 0 0 0 ⋮ 0 0 0 Q M ] [ M 1 0 0 0 ⋮ 0 0 0 M M ] − 1 [ d 1 ⋮ d M ] J=\begin{bmatrix}\mathbf{d}_1\\\varvdots\\\mathbf{d}_M\end{bmatrix}^T\begin{bmatrix}M_1&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\vdots&\mathbf{0}\\\mathbf{0}&\mathbf{0}&M_M\end{bmatrix}^{-T}\begin{bmatrix}\mathbf{Q}_1&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\vdots&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\mathbf{Q}_M\end{bmatrix}\begin{bmatrix}M_1&\mathbf{0}&\mathbf{0}\\\mathbf{0}&\vdots&\mathbf{0}\\\mathbf{0}&\mathbf{0}&\boldsymbol{M}_M\end{bmatrix}^{-1}\begin{bmatrix}\mathbf{d}_1\\\vdots\\\mathbf{d}_M\end{bmatrix} J= ​d1​⋮dM​​ ​T ​M1​00​0⋮0​00MM​​ ​−T ​Q1​00​0⋮0​00QM​​ ​ ​M1​00​0⋮0​00MM​​ ​−1 ​d1​⋮dM​​ ​

轨迹方程如下：
$$\begin{array}{rl}& x(t)=p_5{t}^5+p_4{t}^4+p_3{t}^3+p_2{t}^2+p_{1}t+p_0\\ & x^{\prime }(t)=5p_5{t}^4+4p_4{t}^3+3p_3{t}^2+2p_{2}t+p_1\\ & \begin{array}{r}{x}^{\prime \prime }(t)=20p_5{t}^3+12p_4{t}^2+6p_{3}t+2p_2\end{array}\end{array}$$

映射矩阵：
M = [ 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 2 0 0 T 5 T 4 T 3 T 2 T 1 5 T 4 4 T 3 3 T 2 2 T 1 0 20 T 3 12 T 2 6 T 2 0 0 ] \boldsymbol{M}=\begin{bmatrix}0&0&0&0&0&1\\0&0&0&0&1&0\\0&0&0&2&0&0\\T^5&T^4&T^3&T^2&T&1\\5T^4&4T^3&3T^2&2T&1&0\\20T^3&12T^2&6T&2&0&0\end{bmatrix} M= ​000T55T420T3​000T44T312T2​000T33T26T​002T22T2​010T10​100100​ ​更清晰一点表示，$M$矩阵前三行将$t=0$时的值代入，后三行将$t=T$代入 M p i = [ 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 2 0 0 T 5 T 4 T 3 T 2 T 1 5 T 4 4 T 3 3 T 2 2 T 1 0 20 T 3 12 T 2 6 T 2 0 0 ] [ p 5 p 4 p 3 p 2 p 1 p 0 ] \boldsymbol{M}\textcolor{red}{p_i}=\begin{bmatrix}0&0&0&0&0&1\\0&0&0&0&1&0\\0&0&0&2&0&0\\T^5&T^4&T^3&T^2&T&1\\5T^4&4T^3&3T^2&2T&1&0\\20T^3&12T^2&6T&2&0&0\end{bmatrix}\textcolor{red}{\begin{bmatrix}p_5\\p_4\\p_3\\p_2\\p_1\\p_0\end{bmatrix}} Mpi​= ​000T55T420T3​000T44T312T2​000T33T26T​002T22T2​010T10​100100​ ​ ​p5​p4​p3​p2​p1​p0​​ ​

Fixed and free variable separation固定变量和自由变量分解

轨迹中起始点、终点的状态变量（$p,v,a,j$）是确定的,中间点(waypoints)的位置$(p)$是确定的，因此可以通过选择矩阵$C$去分离自由变量(free)${\mathbf{d}}_F$以及固定变量(constrained)${\mathbf{d}}_P$。因此，可以得到以下的映射关系：
C T [ d F d P ] = [ d 1 ⋮ d M ] \mathbf{C}^T\begin{bmatrix}\mathbf{d}_F\\\mathbf{d}_P\end{bmatrix}=\begin{bmatrix}\mathbf{d}_1\\\varvdots\\\mathbf{d}_M\end{bmatrix} CT[dF​dP​​]= ​d1​⋮dM​​ ​从而目标函数可以写成：$$J={\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]}^T\underset{\mathbf{R}}{\mathbf{C}{M}^{-T}\mathbf{Q}{M}^{-1}{\mathbf{C}}^T\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]}={\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]}^T\left[\begin{array}{cc}{\mathbf{R}}_{FF}& {\mathbf{R}}_{FP}\\ {\mathbf{R}}_{PF}& {\mathbf{R}}_{PP}\end{array}\right]\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]$$因此目标函数变成一个无约束的二次规划问题，可以闭式求解$$\begin{array}{r}J={\mathbf{d}}_F^T{\mathbf{R}}_{FF}{\mathbf{d}}_F+{\mathbf{d}}_F^T{\mathbf{R}}_{FP}{\mathbf{d}}_P+{\mathbf{d}}_P^T{\mathbf{R}}_{PF}{\mathbf{d}}_F+{\mathbf{d}}_P^T{\mathbf{R}}_{PP}{\mathbf{d}}_P\\ {\mathbf{d}}_P^{\ast }=-{\mathbf{R}}_{PP}^{-1}{\mathbf{R}}_{FP}^T{\mathbf{d}}_F\end{array}$$
PS：为什么连续性约束也没有了?因为可以通过设计选择矩阵$C$将中的一个变量映射到$\mathbf{d}$的两个变量中去，这样就满足了连续性约束

$C$的设计

Hierarchical approach

用RRT*获取waypoints，再minimum snap生成轨迹

Problem: safety issue

利用RRT寻找的路径是无碰撞的，但是经过轨迹优化后会产生超调

解决方案：在碰撞的部分新增一个路点，重新生成轨迹，如果还是碰撞，就继续从插入路点，极端情况生成的轨迹会无限接近于path planning出的绝对安全的路径

更好的方案？

增加bounding box，硬约束下进行轨迹优化（下一讲细讲）。

Implementation Details(实现细节)

Convex Solvers

求解器	地址	优缺点
CVX	http://cvxr.com/cvx/	基于MATLAB实现，较慢
Mosek	https://www.mosek.com/	非常鲁棒，但只能在x86架构上运行
OOQP	http://pages.cs.wisc.edu/~swright/ooqp/	速度快，开源，鲁棒，但代码较老，易读性差
GLPK	https://www.gnu.org/software/glpk/	开源，鲁棒，速度快，解LP有优势

Numerical Stability

Other engineering stuff

Time Allocation时间分配

参考

[1] Minimum Snap Trajectory Generation and Control for Quadrotors, Daniel Mellinger and Vijay Kumar
[2] Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments, Charles Richter, Adam Bry, and Nicholas Roy

Coding Tips

以两段轨迹为例：

闭式求解

ROS:
闭式求解