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

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

控制逻辑

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

A:位置和姿态控制器。

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

控制频率

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

姿态控制

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

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

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

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

输入量为期望的姿态角；

输出量为姿态控制量；

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

反馈量为角度和角速度；

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

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

位置控制

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

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

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

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

$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ψ0​sinψcosψ0​001​⎦⎤​

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

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

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

输入量为期望位置；

输出量为期望俯仰角；

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

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

反馈量为位置和速度；

P-PID控制器限幅;

D项低通;

高度控制器

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

$\ddot{h}=\frac{F}{m} \cos \theta \cos \phi-g$h¨=mF​cosθcosϕ−g

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

$\ddot{h}=\frac{F_{0} \cos \theta_{0} \cos \phi_{0}-m g}{m}=0$h¨=mF0​cosθ0​cosϕ0​−mg​=0

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

$\ddot{h}_{d}=\frac{F_{d} \cos \theta \cos \phi-F_{0} \cos \theta_{0} \cos \phi_{0}}{m}$h¨d​=mFd​cosθcosϕ−F0​cosθ0​cosϕ0​​

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

上图中

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

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

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

虚线圈出的地方代表：基准升力与重力不相等时，相当于在高度控制内环引入了常值扰动，PID控制器的具有一定的抗干扰能力，可消除该扰动；

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

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

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

输入量为期望高度；

输出量为期望的升力值；

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

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

P-PID控制器限幅;

D项低通;

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

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

标签：控制,控制器,误差,位置,cos,闭环控制,无人机,theta,旋翼
来源： https://blog.csdn.net/qq_39554681/article/details/90214610