在这一章里，我们将探索四旋翼无人机在二维平面以及三维空间中的运动控制，基于姿态与位置的PD控制器。

上一章链接：

Robotics: Aerial Robotics（空中机器人）笔记（四）:无人机动力学

二维四旋翼无人机控制（2-D Quadrotor Control）

在上一章的内容中，我们已经知道四旋翼无人机的运动方程，接下来我们先从平面四旋翼无人机模型开始，讲一下其PD控制。我们在之前对平面四旋翼无人机的运动分析就能得到如下与控制输入有关的表达式，那么我们就能写出相应的状态空间。通过选择合适的u1和u2，我们可以改变四旋翼的状态并且能让它运动到我们想要它运动到的地方：

如果我们看系统的运动方程，会注意到这个系统是非线性的，系统的非线性仅仅来自于等式中含有的正弦和余弦函数：

我们来考虑四旋翼在悬停状态下的运动方程，定义在y=0, z=0, v=0的悬停状态下其稳态的条件是四旋翼无人机系统的推力向量 $u_1$ 和重力等大反向，所以在悬停时需要输入的实际值必须等于mg并且 $u_2$ 的值必须等于0，否则无人机将会尝试加速运动：

之后我们对这一模型进行线性化，我们看到系统的非线性仅仅来自于等式中含有的正余弦函数，所以我们就要想办法替换掉这些函数。我们注意到无人机在接近悬停状态时 $\phi$ 会趋于0，而 $sin\phi$ 也会趋于0，而我们知道 $\lim_{\phi \to 0}{\frac{sin\phi}{\phi}}=1$ ，即 $sin\phi$ 和 $\phi$ 互为等价无穷小。所以我们就可以在接近悬停状态时拿 $\phi$ 替换掉 $sin\phi$ ，并且这时 $cos\phi=1$ 即：

扫描二维码关注公众号，回复： 14739669 查看本文章

接下来我们来看如何控制一个四旋翼无人机去跟踪一条特定的轨迹 $\textbf{r}_T$ ，其表示位置坐标向量，并且随时间而变化，所以是一个以时间 $t$ 为自变量的函数 $\textbf{r}_T(t)=\begin{bmatrix} y(t)\\ z(t) \end{bmatrix}$ 。

那么我们跟踪一条特定的轨迹 $\textbf{r}_T$ ，也就是尽可能地让当前时刻位置与轨迹相近，也就是误差尽可能地小，并且我们希望这个误差能以指数的速度减小到0。那么通过本专栏第二部分的内容的学习我们就可以得到相应的公式如下，我们通过给予满足下列二阶微分方程的四旋翼无人机加速度命令来进行二维平面的无人机轨迹跟踪控制：

这个想法可以扩展并提供一个层次化的控制结构，接下来我们来看其平面四旋翼无人机模型的控制结构：

首先我们要注意其中的姿态控制内环，我们知道在这个模型中需要控制其姿态来让它跟踪一条特定的轨迹（因为轨迹中的 y 的二次微分与无人机的翻滚角 $\phi$ 有关，而 $\phi$ 与无人机的控制 $u_2$ 有关），所以我们就要通过PD控制器控制其姿态角（在平面四旋翼无人机模型只有翻滚角 $\phi$ ）到我们的指令翻滚角 $\phi_c$ ，这时就有：

注意这里第二个增益应当是 $k_{p,\phi}$ ，注意到这里其实 $I_{xx}$ 是一个常数，我们可以用后面的两个增益把它吸收进去，并且我们近似 $\ddot{\phi}=0$ ，就能得到一个更简洁的形式：

$\large u_2 = k_{v,\phi}(\dot{\phi}_T{(t)-\dot{\phi}})+k_{p,\phi}(\phi_T(t)-\phi)$

完成上一步后，我们就能控制我们的四旋翼无人机到达任意的翻滚角 $\phi$ ，又因为我们的y方向的指令加速度与翻滚角 $\phi$ 有关，那么我们就能进一步通过PD控制器控制我们的无人机在 y 方向上的运动：

所以，综上所述，内环设计保证滚转角 $\phi$ 收敛于参考滚转角 $\phi_c$ ，指令滚转角是通过理想轨迹的 y 方向上的分量得到的。

最后，无人机 z 方向上的位移是通过输入 $u_1$ 来进行控制的，那么我们就可以通过PD控制器控制无人机当前 z 方向上的分量尽可能接近理想轨迹的 z 方向上的分量：

因此我们就得到了在接近悬停状态的平面四旋翼无人机模型下，驱动一个四旋翼无人机的三个控制方程。剩下要做的唯一的事是不断调整这些增益（ $k_{v,\phi}, k_{p,\phi}$ 等6个），让无人机稳定地飞行。

三维四旋翼无人机控制（3-D Quadrotor Control）

现在我们将目光转向三维空间下的四旋翼无人机控制，我们在之前的学习中引入了两个坐标系，惯性系和刚体系，惯性系可以理解成一个固定在空间中某个位置的坐标系，而刚体系固定在无人机上，其坐标轴随着无人机旋转平移也发生相应改变。

四旋翼无人机的状态向量由位置和姿态角组合而成，位置是指惯性系原点指向重心的向量，而翻滚角，俯仰角，偏航角则告诉你机体的偏转情况。当然状态向量除了这6个量以外还包括了这6个量的微分，也就是其变化率：

我们再回顾下四旋翼无人机的角速度与欧拉角的关系，具体怎么来的我们已经在之前的部分中推导出来了：

有了这些方程组，我们就可以来考虑如何跟踪一条随意给定的三维路径了，在三维空间中其轨迹的表示如下，不仅包含了期望的位置，还包含了期望的偏航角：

$\textbf{r}_T(t)=\begin{bmatrix} x(t) \\y(t)\\ z(t) \\ \psi(t) \end{bmatrix}$

那与平面无人机模型同理，我们要尽可能地让当前时刻四旋翼无人机位置与轨迹相近，也就是误差尽可能地小，并且我们希望这个误差能以指数的速度减小到0：

就像在平面情况下一样，我们的控制结构也有内环的姿态控制回路和外环的位置控制回路。在内环利用旋转矩阵或者三个姿态角来描述机体旋转，通过实际的姿态角和角速度的反馈，我们可以由此计算出输入值 $u_2$ ，其是关于电机速度产生的推力和力矩的函数。对于外环的位置控制回路（期望位置和期望偏航角控制），通过相应的反馈可以计算出输入值 $u_1$ ，其是关于电机速度产生的推力合力的函数。具体整个回路怎么计算的我们会在后面说明。

现在我们考虑一个特殊的情况，也就是无人机处于悬停状态的情况，这时 $u_2$ 必须是 0，p,q,r以及它们的微分也都必须是0，推力的合力 $u_1$ 需要补偿重力，但偏航角不需要等于0，只要保持一个常数就可以了，接下来我们对动力学方程进行线性化，假设 $u_1$ 约等于重力 $mg$ ，翻滚角和俯仰角是0，而且偏航角是固定或接近固定于值 $\phi_0$ ：

那么我们通过线性化方程（也就是）就会得到关于x和y二阶导数的方程，利用这个式子：

其中旋转矩阵R对应ZXY欧拉角：

联立两个方程我们就能得到 $\ddot{\textbf{r}}_1$ 和 $\ddot{\textbf{r}}_2$ ， $\ddot{\textbf{r}}_1$ 表示的是 $\ddot{\textbf{r}}$ 的第一行元素，也就是 $\ddot{\textbf{x}}$ ， $\ddot{\textbf{r}}_2$ 同理对应的是 $\ddot{\textbf{y}}$ ：