路径规划-Minimum snap轨迹优化

传统的路径规划pipeline包括路径搜索和轨迹优化两部分。

轨迹优化的目的是生成光滑轨迹，其必要性如下：

• 适合移动机器人的自主移动
• 速度和加速度等动力学状态无法突变
• 移动机器人不必在拐角处加速和减速
• 节约能量

1.预备知识

1.1 轨迹优化的一般过程

• 存在边界条件：起始点和终止点
• 中间节点：包括$A^{\ast }$、$RR{T}^{\ast }$寻找到的中继节点
• 平滑规则：给出一个评价函数评价轨迹的光滑程度

1.2 微分平坦(Differential Flatness)

微分平坦对于非线性系统来说，可以类比为线性系统里的能控性。
以无人机的使用为例，其状态空间可以使用4个变量表示：$\sigma =[x,y,z,\Phi ]$，其中$\Phi$为偏航角。

微分平坦的证明：
无人机的状态空间共有12个变量$\sigma =[x,y,z,\varphi ,\theta ,\Phi ,\dot{x},\dot{y},\dot{z},w_x,w_y,w_z]$

其非线性都动力学可以使用下面的平移方程和转动方程表示：

上述方程等价于下述表示形式

主要证明 $[x,y,z,\varphi ,\theta ,\Phi ,\dot{x},\dot{y},\dot{z},w_x,w_y,w_z]$ 12个状态变量可以被 $[x,y,z,\Phi ]$ 4个状态变量通过代数组合表示出来。其中，$\dot{x},\dot{y},\dot{z}$三个变量为$x,y,z$的导数，可以直接被其表示。

如上图所示，无人机机体坐标系为$[x_B,y_B,z_B]$，得到无人机机体坐标系后可以通过欧拉角等计算得到俯仰角、滚转角和偏航角(即$\varphi ,\theta ,\Phi$)。其中，无人机的合加速度方向为(垂直于无人机机体)，即$z_B$可以使用$x,y,z$的二阶导和重力加速度表示：

求取$x_B$和$y_B$，引入中间坐标系$[x_C,y_C,z_W]$（见上图），其中：

${\theta }_4$为偏航角$\Phi$，则$x_B$和$y_B$可以通过以下叉乘计算得到

下面观察$w_x,w_y,w_z$的表示。首先对移动方程求导得到：

$w_{BW}$表示世界坐标系下看到的机体角速度。同时，对于$u_1$有：

代回前式，有：

$w_{BW}$的计算方式为：

代入$h_w$的计算公式得到：
$h_w=-w_x\ast y_B+w_y\ast x_B$
上式同时两边点乘$y_B$得到$h_w\ast y_B=-w_x$,同理两边点乘x_B得到$h_w\ast x_B=w_y$。整理得：

由于 $w_{BW}=w_{BC}+w_{CW}$，且$w_{BC}$不含有$z_B$分量：

无人机的控制回路

通过轨迹规划得到的位置信息($p_{des}$)，速度信息(位置信息的导数)，加速度信息(位置信息的二阶导)和偏航角输入位置控制器，计算出推力$u_1$和姿态信息，将姿态信息输入姿态控制器解算出三个方向的力矩$u_2,u_3,u_4$，$u_1,u_2,u_3,u_4$共同完成对无人机的控制。

2.轨迹生成

令生成的轨迹为多项式，原因如下：

• 多项式满足光滑准则
• 容易计算导数
• 方便在3个维度上生成轨迹

2.1 Minimum-snap的建立

• List item

对于一带有多个节点的轨迹图，可以使用分段多项式进行描述：

Minimum Snap的最小化目标函数为snap（jerk的导数，jerk为加速度的导数），对于一段轨迹，最小化jerk选择的阶数为5(2x3-1，3个未知量分别为位置、速度、加速度)，最小化snap选择的阶数为7(2x4-1，4个未知量分别为位置、速度、加速度、jerk)。实际过程中，考虑最坏情况，k段距离阶数的选择与1段轨迹相同。

对于一段轨迹，目标函数设置如下：

对该优化问题施加约束：

• 等式约束，即一些边界条件：
  
  一个简单的例子表示如下：
  
• 连续性约束(节点两边的n阶导相同)：
  整理得到最终构造的约束问题为：
  
  该问题为一个典型的二次优化问题(quadratic programming,QP)。

2.2 Minimum-snap的求解

决策变量映射
优化问题优化的多项式的系数$p_1$到$p_M$。

该问题将对轨迹系数的优化问题转化为对各个边界(轨迹上的各个节点)的导数约束的优化问题。通过构建映射矩阵$M_j$对多项式系数进行映射，得到$M_j{p}_j=d_j$，即：

给出一个构建M矩阵的例子如下：

可以看出M可以通过将某段轨迹初始时刻t=0和末尾时刻t=T代入上市得到。

利用选择矩阵求解
该问题求解的关键在于通过选择矩阵C将映射后的决策变量分解为已经固定的变量$d_F$和未固定的变量$d_P$，代回J如下：

对$d_P$求偏导，使求导后等于0：