经典论文翻译--Minimum Snap Trajectory Generation and Control for Quadrotors

这篇论文是无人机路径规划和控制领域的经典文章，这里给出中文译文，翻译是逐渐进行的，不保证正确率，如有错误请指出。

四旋翼最小加加加速度轨迹生成和控制

摘要：我们解决了四旋翼在一个三维空间内的室内紧密约束环境下的控制设计和轨迹生成问题。在这样的设置中，很有必要允许显著的探索从悬停状态和小角度假设对横滚和俯仰角的控制是不够的。我们开发了一种算法，允许通过一组x、y、z、yaw序列实时生成三维最优路径，并且满足速度加速度和输入约束。一个非线性控制器确保了轨迹的跟踪。试验结果证明了这一方法在快速动态轨迹的应用（5-10 机体长度/秒）。

一、引言

略

二、模型

坐标系统包括世界坐标系W和机体坐标系$\Beta $，和旋翼编号规定在图一中显示。因为我们想要控制意味着原理平衡态，为了避免奇异，我们使用旋转矩阵来表示坐标朝向（现在一般用四元数来表示姿态，这一块的思想可以借鉴）。我们也使用Z-X-Y欧拉角定义横滚角、俯仰角、偏航角（r,p,y）作为本地坐标系统。从B至W的旋转矩阵通过 ${}^w{R}_{\mathrm{B}}{=}^w{R}_C{\cdot }^C{R}_{\mathrm{B}}$ 来表示，${}^W{R}_C$代表偏航角旋转至中间坐标系${}^C{R}_{\mathrm{B}}$代表roll和pitch的效果。机器人角速度记做$w_{bW}$，记B在W中的角速度和p、q、r分量在机载坐标系中的分量为：
$$w_{bW}=p{X}_{\mathrm{B}}+q{Y}_{\mathrm{B}}+r{z}_{\mathrm{B}}$$
每个电机有角速度$w_i$，产生一个力$F_i$和$M_i$，关系为：
$$F_i=k_F{w}_i^2$$
$$M_i=k_M{w}_i^2\text{\hspace{1em}(1)}$$
实际上，本文认为电机动力学与刚体动力学和气动学相比是相当快的，在本文的工作中认为其目前可以即刻生效的（电机可以立刻到达指定转速）。因此控制输入记做 u，$u_1$是净升力，$u_2$、$u_3$、$u_4$是机体力矩可以通过电机转速表示为
$$u=\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& -k_{F}L\\ -k_{F}L& 0& k_F& 0\\ k_M& -k_M& k_M& -k_M\end{array}\right]\left[\begin{array}{c}{w}_1^2\\ w_2^2\\ w_3^2\\ w_4^2\end{array}\right]\text{\hspace{1em}(2)}$$
其中L是电机旋转中心到电机中心的距离。
世界坐标系下的质量中心的位置向量记做$r$。系统受力为$-z_w$方向上的重力，和每个电机的合力在$z_{\mathrm{B}}$方向上的$u_1$。牛顿方程为：
$$m\ddot{r}=-mg{z}_W+u_1{z}_{\mathrm{B}}$$
角加速度由欧拉等式确定：
$${\dot{w}}_{\mathrm{B}W}=I^{-1}\left[-w_{\mathrm{B}W}\ast I{w}_{\mathrm{B}W}\left[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]\right]\text{\hspace{1em}(3)}$$
I是关于xyz轴质心的惯性矩阵矩。系统状态通过质心位置和速度给定，朝向【欧拉角本地参数化】和角速度为：
$$x={\left[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},p,q,r\right]}^T\text{\hspace{1em}(4)}$$
或者是无参数化的表达方式，通过质心的位置和速度向量进行表示和旋转矩阵${}^W{R}_{\mathrm{B}}$和角速度$w_{\mathrm{B}W}$

三、微分平坦

这一节中我们将说明四旋翼动力学和四个微分平坦输出。换句话说，状态和输入可以通过四个微分平坦输出与其导数的代数方程决定。这解决了自动生成轨迹的问题，因此任何光滑轨迹（合理有界导数）平坦输出空间中可以被欠驱动的四旋翼跟踪。本文平坦输出的选择是
$$\sigma ={\left[x,y,z,\psi \right]}^T$$
其中 $r={\left[x,y,z\right]}^T$是在世界坐标系下的质心坐标，$\psi$是偏航角。本文定义轨迹$\sigma (t)$为在平坦输出空间中的一条光滑曲线
$$\sigma (t):\left[t_0,t_m\right],->R^{3}xS{O}_3\text{\hspace{1em}(5)}$$
本文现在说明系统状态和控制输入可以被写为$\sigma$与其的n阶导数。
质心位置、速度、加速度可以简单的由$\sigma$，$\dot{\sigma }$，$\ddot{\sigma }$分别表示。为了说明${}^W{R}_{\mathrm{B}}$是平坦输出与其导数的函数，考虑运动方程（3），从（3）可以得到
$$z_B=\frac{t}{\left||t\right||},t=\left[\ddot{{\sigma }_1},\ddot{{\sigma }_2}，\ddot{{\sigma }_3}+y\right]\text{\hspace{1em}(6)}$$
这里定义了机体坐标系下四旋翼的$z$轴。给出偏航角,${\sigma }_4=\psi$，可以写出单位向量
$$X_C=[cos{\sigma }_4,sin{\sigma }_4,0{]}^T$$正如在图1中显示的，可以确定$x_{\mathrm{B}}$和y_{\Beta}如下形式：
$$y_{\mathrm{B}}=\frac{Z_{\mathrm{B}}\ast X_C}{||z_{\mathrm{B}}\ast X_C||},x_{\mathrm{B}}=y_{\mathrm{B}}\ast z_{\mathrm{B}}$$
$X_C\ast z_B!=0$。换句话说，我们可以特别的定义
$${}^W{R}_B=[\begin{array}{ccc}{x}_B& y_B& z_B\end{array}]\text{\hspace{1em}(6)}$$确保我们绝不会碰上$z_B$和$x_C$平行的奇异点情况。
为了展示角速度是平坦输出和他们导数，取(3)的导数
$$m\dot{a}=\dot{u_1}{z}_b{+}^W{b}_W\times u_1{z}_B$$
将表达式沿着$z_B$投影，使用事实${\dot{u}}_1=z_B\cdot m\dot{a}$，我们可以从(7)中替代${\dot{u}}_1$，定义向量$h_{\omega }$为
$$h_{\omega }{=}^w{\mathrm{B}}_W\times z_{\mathrm{B}}=\frac{m}{u_1}(\dot{a}-(z_{\mathrm{B}}\cdot \dot{a})z_{\mathrm{B}})$$
$h_{\omega }$就是$\frac{m}{u_1}\dot{a}$在$x_{\mathrm{B}}-y_{\mathrm{B}}$平面上的投影。如果我们将机体坐标系的角速度分量(1)式中的那样，可以得到分量$p$和$q$：
$$p=-h_w\cdot y_B,q=h_w\cdot x_B$$
第三个分量$r$可以简单地写作$w_{\mathrm{B}W}=w_{BC}+w_{CW}$，发现$w_{BC}$没有$z_{\mathrm{B}}$分量。
$$r=w_{CW}\cdot z_{\mathrm{B}}=\dot{\psi }{z}_W\cdot z_{\mathrm{B}}$$
发现角加速度分量${\alpha }_{\mathrm{B}W}$沿着$x_{\mathrm{B}}$和$y_{\mathrm{B}}$是(3)的二阶导数，接下来的过程和上面的一样。为了找到${\alpha }_{\mathrm{B}W}$的$z\mathrm{B}$分量，使用已知的事实：
$${\alpha }_{\mathrm{B}W}={\alpha }_{\mathrm{B}C}+w_{CW}\times w_{BC}+{\alpha }_{CW}$$
注意到，${\alpha }_{\mathrm{B}C}\cdot z_{\mathrm{B}}=0$且$z_{\mathrm{B}}\cdot w_{CW}{\times }_{BC}=0$。${\alpha }_{\mathrm{B}W}$的$z_B$分量为
$${\alpha }_{\mathrm{B}W}\cdot z_{\mathrm{B}}={\alpha }_{CW}{\dot{z}}_{\mathrm{B}}=\ddot{\psi }{z}_W\cdot z_{\mathrm{B}}$$
从四转子旋翼上来的净升力可以视为平坦输出和他们导数的直接函数（从3,6式)中可以看出，$u_1=m||t||$。给定角速度和角加速度是平坦输出和其导数的函数，我们可以使用欧拉公式(4)来计算，$u_2$，$u_3$，$u_4$。

四、控制

现在我们给出一个控制器用于跟随特殊的轨迹,${\alpha }_T(t)={\left[{\mathbf{r}}_T(t{)}^T,{\psi }_T(t)\right]}^T$。这个控制器和我们之前的工作[15]类似，不一样的地方之后会指出。首先定义位置和速度误差为
$${\mathbf{e}}_p=\mathbf{r}-{\mathbf{r}}_T,{\mathbf{e}}_v=\dot{\mathbf{r}}-{\dot{\mathbf{r}}}_T$$
接下来，计算理想推力向量和理想机体坐标系z轴：
$${\mathbf{F}}_{des}=-K_p{\mathbf{e}}_p-K_v{\mathbf{e}}_v+mg{z}_W+m{\ddot{\mathbf{r}}}_T$$
其中$K_p$，$K_v$是正定增益矩阵。注意，这里假定$||{\mathbf{F}}_{des}\ne 0||$。接下来定义理想力向量到实际的坐标系z轴上，为了计算四旋翼的理想升力和第一个输入：
$$u_1={\mathbf{F}}_{des}\cdot {\mathbf{z}}_{\mathrm{B}}$$
为了确定其他的三个输入，必须考虑旋转误差。首先注意到$z_{\mathrm{B}}$和升力向量是相同方向的：
$${\mathbf{z}}_{B,des}=\frac{{\mathbf{F}}_{des}}{||{\mathbf{F}}_{des}||}$$
因此，如果$e_3={\left[0,0,1\right]}^T$，那么将理想旋转${}^W{R}_{\mathrm{B}}$记作$R_{des}$，由下式给出：
$$R_{des}{\mathbf{e}}_3={\mathbf{z}}_B,des$$。
已知沿着轨迹的特定偏航角，${\psi }_T(t)$，我们可以按照之前的方法计算${\mathbf{x}}_{B,des}$和${\mathbf{y}}_{B,des}$得到
$${\mathbf{x}}_{C,des}={\left[cos{\psi }_T,sin{\psi }_T,0\right]}^T$$
和
${\mathbf{y}}_{C,des}={\left[cos{\psi }_T,sin{\psi }_T,0\right]}^T$，${\mathbf{x}}_{B,des}={\mathbf{y}}_{B,des}\times {\mathbf{z}}_{B,des}$，得到${\mathbf{x}}_{C,des}\times {\mathbf{z}}_{B,des}\ne 0$。这定义了理想旋转矩阵$R_{des}$。数学上来讲这个奇异点在$S{O}_3$上是个单点，计算结果导致单位向量在奇异点领域内的大幅度变化。为了修正这个问题，注意到$-{\mathbf{x}}_{B,des}$和$-{\mathbf{y}}_{B,des}$仍然与偏航角和坐标系z轴共存。实际上，我们为了计算四旋翼朝向$R_{des}$只要简单的检查哪一种解更接近实际的四旋翼朝向就行了。
接下来定义朝向误差：
$${\mathbf{e}}_R=\frac{1}{2}{\left(R_{des}^T{,}^W{R}_B{-}^W{R}_B^T{R}_{des}\right)}^V$$，其中${}^V$代表$so(3)$到$R_3$的vee map。这是和文献【15】的主要差异，文献【15】中角度误差的计算基于小角度偏差。
角速度误差简单的通过实际和理想角速度在机体坐标系下的误差：
$${\mathbf{e}}_w{=}^{\mathrm{B}}\left[w_{\mathrm{B}W}\right]{-}^{\mathrm{B}}\left[w_{\mathrm{B}W,T}\right]$$
现在理想转矩和三个剩余输入可以通过以下公式计算：
$${\left[u_2,u_3,u_4\right]}^T=-K_R{\mathbf{e}}_R-K_w{\mathbf{e}}_w$$
其中$K_R$和K_{w}是对角增益矩阵。允许为r,p,y三个角度的跟踪设置单独的增益。最后，我们计算理想电机转速来实现理想$\mathbf{u}$。实际上，这一工作通过反转（2）中关于$w_i=\sqrt{\frac{u_1}{4k_F}}$的线性化来实现。
注意到和文献【15】中的工作一样，这个控制器在平衡点附近也有线性化假设。这个非线性控制器添加了两个重要的特性。第一，姿态误差不通过有奇异点的欧拉角进行计算。第二，理想力被投影到实际的机体z轴上。文献【16】中的一个相似的控制器给出稳定性和收敛性的证明，除了（a）控制器添加了前向的加速度前馈（b）反馈项的加入抵消了式(8) 中的$w\times Iw$（c）假设所有的增益矩阵都是通过一个标量乘以一个单位矩阵得到（例如$K_R=k_{R}I$）（d）假设电机动态特性不显著，对$m$和$I$的充分辨识。在这些条件下，且满足以下三个条件的，动力学以指数稳定：
（1）
$$tr\left[I-R_{des}^T(0{)}^W{R}_B(0)<2\right]$$
（2）
$$||{\mathbf{e}}_w(0)|{|}^2<\frac{2}{{\lambda }_{min}(I)}{k}_R(1-\frac{1}{2}tr[I-R_{des}^T(0{)}^W{R}_B(0)])$$
（3）在较少约束条件下的完整动力学的近全局指数吸引性。本文提出的控制器的实现是不一样的，并不符合所有上述的假设。但是，从第六节中可以看出，这个控制器保持了对极大横滚角和俯仰角的良好追踪。
这里有些公式打不出来，剩下的点这里查看