路径规划-Minimum snap轨迹优化

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

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

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

1.预备知识

1.1 轨迹优化的一般过程

存在边界条件：起始点和终止点
中间节点：包括A∗A^*A∗、RRT∗RRT^*RRT∗寻找到的中继节点
平滑规则：给出一个评价函数评价轨迹的光滑程度

1.2 微分平坦(Differential Flatness)

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

微分平坦的证明：
无人机的状态空间共有12个变量σ=[x,y,z,ϕ,θ,Φ,x˙,y˙,z˙,wx,wy,wz]\sigma=[x,y,z,\phi,\theta,\Phi,\dot{x},\dot{y},\dot{z},w_x,w_y,w_z]σ=[x,y,z,ϕ,θ,Φ,x˙,y˙,z˙,wx,wy,wz]

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

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

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

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

求取xBx_BxB和yBy_ByB，引入中间坐标系[xC,yC,zW][x_C,y_C,z_W][xC,yC,zW]（见上图），其中：

θ4\theta_4θ4为偏航角Φ\PhiΦ，则xBx_BxB和yBy_ByB可以通过以下叉乘计算得到

下面观察wx,wy,wzw_x,w_y,w_zwx,wy,wz的表示。首先对移动方程求导得到：

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

代回前式，有：

wBWw_{BW}wBW的计算方式为：

代入hwh_whw的计算公式得到：
hw=−wx∗yB+wy∗xBh_w=-w_x*y_B+w_y*x_Bhw=−wx∗yB+wy∗xB
上式同时两边点乘yBy_ByB得到hw∗yB=−wxh_w*y_B=-w_xhw∗yB=−wx,同理两边点乘x_B得到hw∗xB=wyh_w*x_B=w_yhw∗xB=wy。整理得：

由于 wBW=wBC+wCWw_{BW}=w_{BC}+w_{CW}wBW=wBC+wCW，且wBCw_{BC}wBC不含有zBz_BzB分量：

无人机的控制回路

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

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的求解

决策变量映射
优化问题优化的多项式的系数p1p_1p1到pMp_MpM。

该问题将对轨迹系数的优化问题转化为对各个边界(轨迹上的各个节点)的导数约束的优化问题。通过构建映射矩阵MjM_jMj对多项式系数进行映射，得到Mjpj=djM_jp_j=d_jMjpj=dj，即：

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

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

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

对dPd_PdP求偏导，使求导后等于0：