多旋翼无人机控制之完整闭环控制设计

本文主要讲解了多旋翼无人机整个闭环系统的设计流程，对各个控制器的控制输入与输出，控制器的设计要点进行了详细描述。

控制逻辑

Q:要让多旋翼无人机按照预设的航线进行飞行，需要设计哪些控制器呢？

A:位置和姿态控制器。

无人机的位置控制流程用一句话简单来讲就是：位置传感器(通常是GPS、视觉里程计、光流、气压计、超声波、红外传感器等等)实时感知无人机当前位置，位置控制器将当前位置与期望位置做差得到位置误差，位置误差转换至运动坐标系，控制器根据运动系下的位置误差计算期望姿态角，姿态控制器使机体跟踪该姿态角(机体姿态角通常通过磁力计、加速度计和陀螺仪等MEMS传感器通过数据融合得到)从而使无人机向减小位置误差的方向移动。

控制频率

多旋翼无人机控制分为位置环和姿态环两大部分，在实际的控制过程中，姿态和位置控制器工作在不同的控制频率下(姿态环的控制带宽较高，相应地需要较高的控制频率；位置环的控制带宽较低，相应地控制频率较低；而且根据香农采样定理，控制频率必须高于控制带宽的2倍以保证控制信号中的有用频段能作用于系统；通常取位置环控制频率为20hz20hz20hz姿态环控制频率为200hz200hz200hz；更一般地来讲，对响应速度快的控制回路使用较高的控制频率，对相应速度慢的通道使用较低的控制频率。)

姿态控制

姿态控制是无人机位置控制的基石，因为位置误差是通过调整机体姿态角来进行削减的；因此，快速、精确、稳定的姿态控制器是位置稳定控制的前提；在实际的控制器设计过程中，也是先将无人机的姿态控制器调稳再去调位置控制器。

我们知道，姿态控制器的作用是使机体快速跟踪期望姿态角；比较常用的方法是串级PID控制器(原因在之前的博客中有介绍)，即忽略多旋翼无人机的姿态通道的耦合项，滚转、俯仰和偏航通道分别独立设计控制器；我们以俯仰通道的控制为例来简述姿态控制器的设计，具体的系统框图如下：

可以看到,俯仰通道模型简化为二阶积分环节；姿态控制外环只有比例环节PPP，姿态角误差Δθ\Delta \thetaΔθ乘以比例系数PPP生成期望的姿态角速度θ˙d\dot{\theta}_dθ˙d；机体角速度θ˙\dot {\theta}θ˙反馈回前端与期望姿态角速度θ˙d\dot{\theta}_dθ˙d做差得到角速度误差Δθ˙\Delta{\dot{\theta}}Δθ˙，角速度误差通过PID控制器得到期望的俯仰控制量。

姿态控制器的一些设计要点如下：

输入量为期望的姿态角；

输出量为姿态控制量；

控制器作用是使机体快速跟踪期望姿态角；

反馈量为角度和角速度；

P-PID控制器的四个环节均设置限幅函数(因为实际系统的执行量有限，若不设置限幅容易使执行器饱和，控制效果变差);

内环D项需通过低通滤波器(实际调试过程中，若不加低通，增大微分项系数，控制性能改善有限，且很容易使系统震荡发散；这是因为微分项放大了系统中的高频噪声，这对系统的控制性能不利，需引入低通加以抑制，但是低通滤波器的引入会使系统的闭环带宽降低，因此滤波器的截止频率的选择是一个权衡的过程);

位置控制

无人机最终要实现的飞行功能是自主航迹飞行，即按照预设的三维航迹（包括高度和水平位置）自动飞行，并且在飞行过程中能够进行速度控制。

因此位置控制器又分为水平位置回路控制器与高度回路控制器。

接下来介绍水平位置回路的控制器设计：

首先定义基准运动坐标系Sr−OxryrzrS_{r}-O x_{r} y_{r} z_{r}Sr−Oxryrzr，其坐标原点位于无人机质心，zrz_{r}zr轴垂直于地面并指向地心，xrx_{r}xr轴与机体xbx_{b}xb轴在水平面内的投影重合，yry_{r}yr轴与机体yby_{b}yb轴在水平面内的投影重合。无人机滚转产生的水平加速度沿SrS_{r}Sr坐标系yry_{r}yr轴方向，俯仰产生的水平加速度沿SrS_{r}Sr坐标系xrx_{r}xr轴方向，换句话说，沿运动坐标系yry_{r}yr轴方向上的位置误差可以通过滚转角来消除，沿运动坐标系xrx_{r}xr轴方向上的位置误差可以通过俯仰角来消除；而GPS模式下无人机外环的位置和速度信息在NEDNEDNED导航坐标系下进行描述，因此需要先将导航坐标系下的水平位置误差和速度误差分解到SrS_{r}Sr坐标系的xrx_{r}xr轴和yry_{r}yr轴上。转换矩阵如下所示，其中ψ\psiψ表示无人机当前航向角。

Cgr=[cos⁡ψsin⁡ψ0−sin⁡ψcos⁡ψ0001]C_{g}^{r}=\left[ \begin{array}{ccc}{\cos \psi} & {\sin \psi} & {0} \\ {-\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] Cgr=⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤

水平位置环的控制框图如下所示：

其中Δxr\Delta x_rΔxr和Δx˙r\Delta \dot{x}_rΔx˙r分别为地理坐标系下的位置误差和速度在运动坐标系下的投影

沿运动坐标系xrx_rxr方向的位置控制器的设计要点如下：

输入量为期望位置；

输出量为期望俯仰角；

位置误差和速度要从地理系转换到运动坐标系

控制器作用是根据位置误差生成期望姿态角，进而通过姿态控制器跟踪该姿态角，实现减小位置误差的效果；

反馈量为位置和速度；

P-PID控制器限幅;

D项低通;

高度控制器

首先来看高度环的控制模型：

h¨=Fmcos⁡θcos⁡ϕ−g\ddot{h}=\frac{F}{m} \cos \theta \cos \phi-g h¨=mFcosθcosϕ−g

假设进入高度控制模式时，竖直方向上的加速度为0，则有：

h¨=F0cos⁡θ0cos⁡ϕ0−mgm=0\ddot{h}=\frac{F_{0} \cos \theta_{0} \cos \phi_{0}-m g}{m}=0 h¨=mF0cosθ0cosϕ0−mg=0

由期望的高度通道运动加速度到期望升力之间的关系可由下式确定：

h¨d=Fdcos⁡θcos⁡ϕ−F0cos⁡θ0cos⁡ϕ0m\ddot{h}_{d}=\frac{F_{d} \cos \theta \cos \phi-F_{0} \cos \theta_{0} \cos \phi_{0}}{m} h¨d=mFdcosθcosϕ−F0cosθ0cosϕ0

Fd=(mh¨d+F0cos⁡θ0cos⁡ϕ0)/cos⁡θcos⁡ϕF_{d}=\left(m \ddot{h}_{d}+F_{0} \cos \theta_{0} \cos \phi_{0}\right) / \cos \theta \cos \phi Fd=(mh¨d+F0cosθ0cosϕ0)/cosθcosϕ

上图中

1/cos⁡θcos⁡ϕ1 / \cos \theta \cos \phi 1/cosθcosϕ

这一项为油门倾角补偿，因为高度控制计算出的FdF_dFd为竖直方向上的期望升力，需要投影到机体坐标系得到旋翼产生的总升力；

m项为比例项，可以归入PID控制器中；

虚线圈出的地方代表：基准升力与重力不相等时，相当于在高度控制内环引入了常值扰动，PID控制器的具有一定的抗干扰能力，可消除该扰动；（实际上在扰动作用点之前引入积分环节可以消除由阶跃扰动作用产生的稳态误差和稳态输出；而在扰动作用点之后加入积分环节，则对阶跃扰动产生的稳态误差和稳态输出影响不大。）

框图中F0cos⁡θ0cos⁡ϕ0F_{0} \cos \theta_{0} \cos \phi_{0}F0cosθ0cosϕ0表示了基准油门。

遥控器油门通道信号一般为期望的上升速度，在外环P环节后引入。

高度控制器的设计要点如下：

输入量为期望高度；

输出量为期望的升力值；

控制器作用是根据高度误差生成期望升力值，以减小高度误差；

反馈量为高度和上升速度；

P-PID控制器限幅;

D项低通;

高度控制器的控制输出需要叠加在基础升力上；

高度控制器的控制输出需要进行油门倾角补偿。