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

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

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

姿态控制器

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

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

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

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

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

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

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

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

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

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

• $X_{sp}$：物理量$X$的期望控制值
• $\hat{X}$：物理量$X$的滤波估计值
• $\dot{X}$：物理量$X$的导数值
• $X_{err}$：$X_{err}=X_{sp}-\hat{X}$
• $X^b$：机体系$(body)$下物理量$X$

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

滤波估计

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

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

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

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

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

• 平滑：$t<t_i$，状态估计在最后测量时间点前，$\widehat{X(t)}$称为状态的平滑；
• 滤波：$t=t_i$，状态估计在最后测量时间点上，$\widehat{X(t)}$称为状态的滤波；
• 预测：$t>t_i$，状态估计在最后测量时间之后，$\widehat{X(t)}$称为状态的预测。

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

串级PID

外环角度控制

• 将期望滚转角${\varphi }_{sp}$与滚转角估计值$\hat{\varphi }$作差得到误差${\varphi }_{err}$，误差${\varphi }_{err}$经过比例控制环节，得到期望滚转姿态角速度${\dot{\varphi }}_{sp}$；
• 同理，可将期望俯仰角${\theta }_{sp}$与俯仰角估计值$\hat{\theta }$作差得到误差${\theta }_{err}$，误差${\theta }_{err}$经过比例控制环节，得到期望俯仰姿态角速度${\dot{\theta }}_{sp}$；
• 期望偏航角速率${\dot{\psi }}_{sp}$获取采用协调转弯控制率。其计算公式为${\dot{\psi }}_{sp}=\frac{g}{V_T}\tan {\varphi }_{sp}\cos {\theta }_{sp}$；
• 综上可得期望的姿态角速度三轴矢量${\dot{\Psi }}_{sp}=[{\dot{\varphi }}_{sp}{\dot{\theta }}_{sp}{\dot{\psi }}_{sp}{]}^T$。

值得一提的是，这里所得到的${\dot{\Psi }}_{sp}$是姿态角的角度变化率，而非机体系下的三轴滚转角速度${\omega }_{sp}^b=[p_{sp}{q}_{sp}{r}_{sp}{]}^T$。考虑到舵偏直接产生力矩的对象是飞机本体，所以此处合理的中间控制量应该是机体系下的三轴滚转角速度${\omega }_{sp}^b=[p_{sp}{q}_{sp}{r}_{sp}{]}^T$。

这里需要用到一个转换矩阵，根据期望的${\dot{\Psi }}_{sp}$得到期望的${\omega }_{sp}^b$，具体转换公式如下，此公式在之前的文章《固定翼气动模型（三）——六自由度动力学》有介绍。

$$\left[\begin{array}{c}{p}_{sp}\\ q_{sp}\\ r_{sp}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \cos \theta \sin \varphi \\ 0& -\sin \varphi & \cos \theta \cos \varphi \end{array}\right]\left[\begin{array}{c}{\dot{\varphi }}_{sp}\\ {\dot{\theta }}_{sp}\\ {\dot{\psi }}_{sp}\end{array}\right]$$

内环角速度控制

为了得到期望角加速度$\dot{{\omega }_{sp}^b}$，这里使用了期望值前馈$FF$和误差值$PI$控制。

• 将${\omega }_{sp}^b=[p_{sp}{q}_{sp}{r}_{sp}{]}^T$作为前馈输入，可得到前馈控制输出量。
• 将${\omega }_{sp}^b=[p_{sp}{q}_{sp}{r}_{sp}{]}^T$与$\widehat{{\omega }_{sp}^b}=[\widehat{p_{sp}}\widehat{q_{sp}}\widehat{r_{sp}}{]}^T$作差，可得到误差量${\omega }_{err}^b=[p_{err}{q}_{err}{r}_{err}{]}^T$，这部分误差作为$PI$控制器输入。这里仅使用了$PI$而非$PID$控制器的原因在于固定翼飞机本身存在一定的气动阻尼，可以不加$D$控制。
• 期望角加速度为$\dot{{\omega }_{sp}^b}={\dot{{\omega }_{sp}^b}}_{FF}+{\dot{{\omega }_{sp}^b}}_{PI}$

高速飞行和低速飞行状态下，同样的舵偏量会产生不一样大小的力矩。如何根据空速，实现高速飞行下输出较小的舵偏，低速飞行下输出较大的舵偏成为了一个值得思考的问题。

在这个环节基础上，前馈控制输出量和$PI$控制输出量后都乘上了一个$Scaler$系数。这个设计被称为空速缩放。这里提出一种名为“空速缩放”的设计方法，在前馈控制输出量和$PI$控制输出量后乘上一个$Scaler$缩放系数。

首先给出这两个空速缩放系数的计算表达式分别为：

$$F{F}_{Scaler}=\frac{V_{T_0}}{V_T},P{I}_{Scaler}=\frac{{V_{I_0}}^2}{{V_I}^2}$$

式中，$V_T$为真实空速，$V_I$为指示空速；$V_{T_0}$和$V_{I_0}$为配平条件下设定下的真实空速和指示空速。

这里的$V_{T_0}$和$V_{I_0}$均为设定的参数值，一般设为巡航速度或最小飞行速度与最大飞行速度的中位数值；$V_T$和$V_I$为传感器数据处理后所得到的值。

我们思考一下当飞行速度较大，大于设定的配平值，所得到的$Scaler<1$ ；而低速情况下，$Scaler>1$。

这样处理后，期望角加速度$\dot{{\omega }_{sp}^b}$在高速飞行时的值较小，在低速飞行时的值较大。之后混控器$Mixer$根据$\dot{{\omega }_{sp}^b}$控制舵面时，能够有效抑制高空速下飞行振荡，低空速下跟踪性能差的问题。

最终我们得到的期望角加速度为
$$\dot{{\omega }_{sp}^b}={\dot{{\omega }_{sp}^b}}_{FF}\times F{F}_{Scaler}+{\dot{{\omega }_{sp}^b}}_{PI}\times P{I}_{Scaler}$$
得到的$\dot{{\omega }_{sp}^b}$经过混控器$Mixer$后，输出PWM信号使得舵机偏转，从而控制舵面。

这里你可能会问，为什么$F{F}_{Scaler}$计算是线性比例计算，而$P{I}_{Scaler}$则是平方比例计算？只是为了算一个比例，为什么不直接都用线性比例？甚至有没有可能存在其他计算方式？

由于篇幅原因，这个问题我们留到下一期文章进行讲解。

混控器

混控器$Mixer$负责将飞行控制器的控制命令($\dot{{\omega }_{sp}^b}$)转换为电机或舵机的实际输出信号。

这里如果说的简单直白一点，就是同样的控制命令，不同机型的舵面偏转需要不同的配置。
例如同样的$\dot{{\omega }_{sp}^b}$指令，常规的水平尾翼加垂直尾翼的舵偏与V尾的舵偏存在差异，而V尾和倒V尾的舵偏则完全相反。

混控器功能是为了实现在同一个控制指令输入下，多种不同布局机型的适配。具体设计方式不在本文讨论范围内。

总结

本期文章介绍了固定翼飞控中姿态控制器设计方法，下一期文章会带来协调转弯控制率、空速缩放设计原理介绍以及如何将结合姿态控制器完成姿态自稳飞行模式(Attitude Mode)设计。
后续会陆续带来规划、控制相关的文章、仿真分享。

END
迅翼SwiftWing致力于固定翼技术共享，汇聚固定翼领域技术极客，推动固定翼技术持续创新！