PX4固定翼控制器详解(一)——姿态控制器

迅翼SwiftWing

已于 2024-05-25 23:00:18 修改

阅读量6.3k

点赞数 40

分类专栏：固定翼控制器飞行控制系统导航、制导与控制文章标签：无人机

于 2024-05-07 20:06:11 首次发布

本文链接：https://blog.csdn.net/2302_80148937/article/details/138511722

版权

PX4是一个开源飞控软件，广泛应用于无人机和机器人领域，提供稳定且灵活的自动驾驶解决方案，支持多种无人机平台和应用需求。从本期文章起，我们将采用PX4开源飞控平台，向大家系统介绍小型固定翼无人机的控制器设计方法。

PX4的固定翼控制器部分可以大致分为姿态控制器（控制飞行姿态）、TECS控制器（控制飞行速度和高度）、L1控制器（控制飞机的侧向飞行）。本文将聚焦于整个控制系统中最基础的部分——姿态控制器。

姿态控制器

首先，让我们思考一个问题：如果我们要实现飞行器的多种飞行模式，比如爬升、下降或转弯，我们应如何控制舵面和油门？

对于有手动飞行过固定翼航模的朋友来说，这些操作比较熟悉。例如，要使固定翼飞机爬升或下降，飞手通常会调整升降舵，使飞机抬头或低头飞行，从而在特定的俯仰角下完成爬升或下降。升降舵的调整幅度越大，飞机的俯仰角也越大，相应的爬升或下降率（即垂直速度与水平速度的比率）也会增加。

当需要固定翼飞机转弯时，通常通过调整副翼来实现。飞手会打副翼使飞机达到一定的滚转角，之后将副翼归中，手动细调以保持滚转角，使飞机能够沿着一个平滑的圆弧线路转弯。在转弯过程中，为了防止飞机下降，会稍微拉升降舵以维持高度；有时为了更协调的转弯，还会加入一些方向舵的调整。

由此不难得出，飞手控制固定翼航模飞行的整体逻辑是通过舵面控制飞机的姿态角，实现前向飞行的方向控制。根据速度要求，控制前向飞行所需要的油门。油门对速度控制和舵面对姿态控制构成了整个飞控系统。其中”姿态控制器“是更是飞控系统中的重中之重。

控制飞机姿态角可以考虑一个直观简单的控制方法，可以运用自动控制原理中闭环控制的思想。将期望的姿态角与当前飞机的姿态角做误差，通过PID控制舵面来进行调整。

这种单级PID控制，在实现效果上确实可以达到最终能控制到期望姿态角的效果，但是在控制过程中，控制器并没有控制飞机的三轴转动角速度。这意味着根据姿态角误差直接控制舵面，无法保证飞机以一个合理的转动速度达到期望姿态角。

在姿态控制器中，使用串级PID控制较为合理，控制器在控制角速度的基础上，再去控制姿态角。这样的控制器也正是我们所熟知的双回路PID角度控制器。

以下我们将探讨PX4中固定翼飞机的姿态控制器设计。下图所示为PX4官方文档中固定翼姿态控制器的控制原理图。

如果一开始看到这张图有疑惑，请看看下面这张我重新绘制的控制原理图。

下文中，对所涉及的物理量做如下注释：

整体的姿态控制器设计采用双回路串级PID，外环控制为角度控制，内环控制为角速度控制。闭环控制量需要获取状态量做反馈控制，状态量获取需要用到滤波估计算法。下文将从滤波估计、串级PID控制这两个章节进行详细介绍。

闭环控制系统都需要系统当前的状态量做反馈。在双回路姿态控制器中，所需要的状态量有：机体系下三轴角速度 $p, q, r$ 和姿态角 $\phi,\theta,\psi$ 。主要使用到的传感器有陀螺仪、加速度计、磁力计、GPS。

MEMS陀螺仪主要测量角速度 $p, q, r$ ；
加速度计测量飞行器在三个空间轴上的加速度信息，通过和重力加速度的对比，确定倾斜方向，可以得到滚转角 $\phi$ 和俯仰角 $\theta$ ；
磁力计测量地磁场在飞行器上的三个维度的分量，确定飞行器相对于地磁北的方向，用于计算偏航角 $\psi$ ；
GPS本身不直接用于姿态测量，但它提供的位置和速度数据可以辅助改善姿态估计的精确度，尤其是在长距离飞行或动态环境中。

传感器的测量数据往往含有噪声，如果直接用这些数据进行物理公式层面的计算，会放大噪声，导致数据不准确。因此，对这些数据进行处理以降低噪声的影响是必需的，这个过程被称为“估计”。

估计：从带有随机噪声的测量数据中提取有用信息的过程，目的在于滤除或降低随机噪声的影响，提高状态或参数的估计精度。我们给状态量 $X$ 上加个“帽子”变成 $\hat{X}$ 代表其估计值。

状态估计从时间上来分类，可以分为三类情况：

在PX4飞控系统中，状态估计通常采用扩展卡尔曼滤波(EKF)方法。这种方法有效地整合各种传感器数据，提供准确的飞行状态信息。

将期望滚转角 $\phi_{sp}$ 与滚转角估计值 $\hat{\phi}$ 作差得到误差 $\phi_{err}$ ，误差 $\phi_{err}$ 经过比例控制环节，得到期望滚转姿态角速度 $\dot{\phi}_{sp}$ ；
同理，可将期望俯仰角 $\theta_{sp}$ 与俯仰角估计值 $\hat{\theta}$ 作差得到误差 $\theta_{err}$