控制器图解

本节包含PX4主要控制器的图示说明。

这些图示采用标准的PX4符号表示法（且每个图示都有带注释的图例）。

多旋翼的控制架构

• 这是一个标准的级联控制架构。
• 控制器采用比例（P）和比例积分微分（PID）控制相结合的方式。
• 估计值来自EKF2。
• 在某些模式下，外环（位置回路）可能会被绕过（在图中以位置回路之后添加一个多路开关表示）。只有在位置保持模式或某轴无速度请求时，位置回路才会起作用。

多旋翼角速率控制器

• 采用K - PID控制器。更多信息请参考速率控制器。

• 为防止积分饱和，积分环节的权重是受限的。

• 输出是受限的（在控制分配模块中），通常限制在 -1 到 1 之间。

• 在微分路径上使用低通滤波器（LPF）来降低噪声（陀螺仪驱动会向控制器提供经过滤波的微分值）。

  INFO

  IMU数据处理流程如下： 陀螺仪数据 > 应用校准参数 > 去除估计偏差 > 陷波滤波器（IMU_GYRO_NF0_BW 和 IMU_GYRO_NF0_FRQ）> 低通滤波器（IMU_GYRO_CUTOFF）> vehicle_angular_velocity（P和I控制器使用的滤波后的角速率）> 微分 -> 低通滤波器（IMU_DGYRO_CUTOFF）> vehicle_angular_acceleration（D控制器使用的滤波后的角加速度）

:::

多旋翼姿态控制器

• 姿态控制器利用四元数。
• 该控制器依据此文章实现。
• 当调整此控制器时，唯一需要考虑的参数是比例增益P。
• 输出的角速率指令是饱和的。

多旋翼加速度到推力和姿态设定点的转换

• 速度控制器生成的加速度设定点将被转换为推力和姿态设定点。
• 转换后的加速度设定点将在垂直和水平推力上进行饱和处理并设定优先级。
• 推力饱和处理在计算出相应推力后进行：
  1. 计算所需的垂直推力（thrust_z）
  2. 用 MPC_THR_MAX 对 thrust_z 进行饱和处理
  3. 用 (MPC_THR_MAX^2 - thrust_z^2)^0.5 对 thrust_xy 进行饱和处理

实现细节可在 PositionControl.cpp 和 ControlMath.cpp 中找到。

多旋翼速度控制器

• 采用PID控制器来稳定速度，该控制器输出的指令是加速度。
• 积分器包含一种采用钳制方法的反复位饱和措施。
• 指令加速度不饱和，饱和处理将结合最大倾斜角应用于转换后的推力设定点。
• 水平增益通过参数 MPC_XY_VEL_P_ACC、MPC_XY_VEL_I_ACC 和 MPC_XY_VEL_D_ACC 设置。
• 垂直增益通过参数 MPC_Z_VEL_P_ACC、MPC_Z_VEL_I_ACC 和 MPC_Z_VEL_D_ACC 设置。

多旋翼位置控制器

• 采用简单的比例（P）控制器来控制速度。
• 输出的速度指令是饱和的，目的是维持一定的速度限制。参考参数 MPC_XY_VEL_MAX，此参数设置了最大可能的水平速度。这与最大期望速度 MPC_XY_CRUISE（自主模式）和 MPC_VEL_MANUAL（手动位置控制模式）不同。
• 水平比例增益通过参数 MPC_XY_P 设置。
• 垂直比例增益通过参数 MPC_Z_P 设置。

静态力矩（PI）缩放补偿

• 与模式相关的前馈（ff） - 例如，任务模式轨迹生成器（加加速度受限轨迹）计算位置、速度和加速度设定点。
• 加速度设定点（惯性坐标系）将通过偏航设定值转换为姿态设定点（四元数）和总推力设定点。

固定翼位置控制器

总能量控制系统（TECS）

PX4中总能量控制系统（TECS）的实现，能够同时控制固定翼飞机的真空速和高度。该代码作为一个库实现，用于固定翼位置控制模块。

从上述图示可知，TECS接收空速和高度需求，然后输出油门和俯仰角控制量。这两个输出被发送到固定翼姿态控制器，该控制器实现姿态控制解决方案。然而，如果油门设定点是有限值且未检测到发动机故障，则直接传递该设定点。因此，务必理解TECS的性能直接受俯仰控制回路性能的影响。空速和高度跟踪不佳，通常是由飞机俯仰角跟踪不良导致的。

INFO

在尝试调整TECS之前，请确保先调整姿态控制器。

同时控制飞机的空速和高度并非易事。增加飞行器的俯仰角会使高度上升，同时导致空速下降。推力（通过油门控制）增加飞机的总能量。因此，俯仰角和油门这两个输入量都会对空速和高度产生影响，从而使控制问题变得复杂。

TECS提供了一种解决方案，即从能量角度而非初始设定值来考量问题。飞行器的总能量是动能与势能之和。推力（通过油门控制）可增加飞机的总能量。给定的总能量状态可通过势能和动能的任意组合来实现。换句话说，飞行器在高海拔以低空速飞行与在低海拔以高空速飞行时的总能量是等效的。我们称这种情况为比能量平衡，它是根据当前高度和真实空速设定值计算得出的。通过控制俯仰角可控制飞行器的比能量平衡。俯仰角增加会将动能转化为势能，俯仰角减小则反之。如此，通过将初始空速和海拔设定值转化为能量值（空速和海拔相互耦合，而能量值可独立控制），就能将控制问题解耦。我们利用油门调节飞行器的特定总能量，利用俯仰角维持势能（高度）和动能（真空速）的特定平衡点。

总能量控制回路

比能量控制回路

飞行器的总能量包含动能和势能。

$$E_T=\frac{1}{2}m{V}_T^2+mgh$$

对时间求导，可得到能量变化率：

$$\dot{E_T}=m{V}_T\dot{V_T}+mg\dot{h}$$

由此，我们定义能量变化率：

$$\dot{E}=\frac{\dot{E_T}}{mg{V}_T}=\frac{\dot{V_T}}{g}+\frac{\dot{h}}{V_T}=\frac{\dot{V_T}}{g}+sin(\gamma )$$

其中，$\gamma$ 是飞行器纵平面的速度角。当 $\gamma$ 较小时，可近似认为 $sin(\gamma )=\gamma$，所以可得：

$$\dot{E}\approx \frac{\dot{V_T}}{g}+\gamma$$

列出飞行器的动力学方程，可得：

$$T-D=mg(\frac{\dot{V_T}}{g}+sin(\gamma ))\approx mg(\frac{\dot{V_T}}{g}+\gamma )$$

这里的 $T$ 和 $D$ 分别是飞行器的推力和阻力。在水平飞行中，推力和阻力应相等，所以推力的变化会导致：

$$\mathrm{\Delta }T=mg(\frac{\dot{V_T}}{g}+\gamma )$$

如所见，$\mathrm{\Delta }T$ 与 $\dot{E}$ 成正比，因此推力设定点应用于控制总能量。

另一方面，对升降舵的控制是能量守恒的，因此用于能量转换，反之亦然。控制升降舵可将势能转换为动能，反之亦然。为此，定义特定的能量平衡变化率为：

$$\dot{B}=\gamma -\frac{\dot{V_T}}{g}$$

固定翼姿态控制器

姿态控制器采用级联环路的方式工作。外环计算姿态设定值与估计值的误差，并将误差乘以增益（比例控制器），生成角速率设定值。内环计算角速率误差，并采用（比例 + 积分）控制器生成所需的角加速度。

然后，利用此期望角加速度以及通过控制分配（也称为混控）获得的系统先验知识，计算控制舵面（副翼、升降舵、方向舵等）的角位置。此外，由于控制面在高速时更有效，在低速时效率较低，因此使用根据巡航速度调整的控制器，并利用空速测量值进行缩放（若使用此类传感器）。

INFO

若未使用空速传感器，则固定翼姿态控制器的增益调度功能将被禁用（处于开环状态）；TECS中无法使用空速反馈进行校正。

前馈增益用于补偿空气动力阻尼。基本上，绕机体轴的两个主要力矩分量分别来自：控制翼面（副翼、水平尾翼、垂直尾翼 - 驱动机体转动）和空气动力阻尼（与机体角速率成正比 - 阻止机体转动）。为保持恒定的角速率，可在角速率回路中使用前馈来补偿这种气动阻尼。

转弯协调

滚转和俯仰控制器结构相同，且假设纵向和横向动力学解耦充分，可独立工作。然而，为使飞机侧滑产生的侧向加速度最小化，偏航控制器利用转弯协调约束生成偏航速率设定值。转弯协调算法仅基于协调转弯几何计算。

$${\dot{\mathrm{\Psi }}}_{sp}=\frac{g}{V_T}\tan {\varphi }_{sp}\cos {\theta }_{sp}$$

偏航速率控制器还通过提供额外的方向阻尼，帮助抵消不利偏航效应并抑制荷兰滚模式。

VTOL飞行控制器

本节简要概述垂直起降（VTOL）飞机的控制结构。VTOL飞行控制器包含多旋翼和固定翼控制器，它们在相应的VTOL模式下单独运行，或在过渡阶段共同运行。上图展示了一个简化的控制示意图。请注意VTOL姿态控制器模块，它主要为不同的VTOL模式提供必要的切换和混合逻辑，以及过渡阶段特定于VTOL类型的控制动作（例如，在向前过渡期间提升标准VTOL的推进电机）。输入到此模块的信号被称为“虚拟”信号，因为根据当前VTOL模式，控制器会忽略其中一些信号。

对于标准和倾转旋翼VTOL，在过渡阶段，固定翼姿态控制器生成速率设定点，然后将其输入到单独的速率控制器中，从而为多旋翼和固定翼执行器生成扭矩指令。对于尾坐式VTOL，在过渡阶段运行的是多旋翼姿态控制器。

VTOL姿态模块的输出是分别针对多旋翼和固定翼执行器的扭矩和力指令（vehicle_torque_setpoint 和 vehicle_thrust_setpoint 各有两个实例）。这些在特定飞行器架构的控制分配类中进行处理。

有关VTOL模块内过渡逻辑调整的更多信息，请参阅VTOL配置。

空速缩放

本节旨在借助公式解释为何以及如何使用空速对速率PI和前馈（FF）控制器的输出进行缩放，以提高控制性能。我们首先给出滚转轴上简化的线性有量纲力矩方程，然后展示空速对直接力矩生成的影响，最后展示恒滚转过程中空速的影响。

如上述固定翼姿态控制器所示，速率控制器为控制分配器（此处称为“混控器”）生成角加速度设定点。为产生这些期望的角加速度，混控器利用可用的气动控制面（例如：标准飞机通常有两个副翼、两个升降舵和一个方向舵）产生扭矩。这些控制面产生的扭矩受相对空速和空气密度的影响很大，更准确地说，受动压影响很大。如果不进行空速缩放，为特定巡航空速紧密调整的控制器会使飞机在更高空速时振荡，或在低空速时跟踪性能不佳。

读者应注意真空速（TAS）与指示空速（IAS）之间的差异，因为在非海平面飞行时，它们的值会有显著不同。

动压的定义为：

$$\overline{q}=\frac{1}{2}\rho V_T^2$$

其中，$\rho$ 是空气密度，$V_T$ 是真空速（TAS）。

在本节余下部分以滚转轴为例，有量纲的滚转力矩可写为：

$$\ell =\frac{1}{2}\rho V_T^{2}Sb{C}_{\ell }=\overline{q}Sb{C}_{\ell }$$

其中，$\ell$ 是滚转力矩，$b$ 是翼展，$S$ 是参考面积。

无量纲滚转力矩导数 $C_{\ell }$ 可使用副翼效率导数 $C_{{\ell }_{{\delta }_a}}$、滚转阻尼导数 $C_{{\ell }_p}$ 和上反角导数 $C_{{\ell }_{\beta }}$ 建模：

$$C_{\ell }=C_{{\ell }_0}+C_{{\ell }_{\beta }}\beta +C_{{\ell }_p}\frac{b}{2V_T}p+C_{{\ell }_{{\delta }_a}}{\delta }_a$$

其中，$\beta$ 是侧滑角，$p$ 是机体滚转速率，${\delta }_a$ 是副翼偏转角。

假设飞机对称（$C_{{\ell }_0}=0$）且协调飞行（$\beta =0$），该方程可仅使用滚转速率阻尼和副翼产生的滚转力矩进行简化：

$$\ell =\frac{1}{2}\rho V_T^{2}Sb[C_{{\ell }_{{\delta }_a}}{\delta }_a+C_{{\ell }_p}\frac{b}{2V_T}p]$$

此最终方程将作为接下来两个小节的基准，以确定PI和FF控制器所需的空速缩放表达式。

静态力矩（PI）缩放补偿

在零速率条件（$p=0$）下，阻尼项消失，可通过以下方式产生恒定的 - 瞬时的 - 扭矩：

$$\ell =\frac{1}{2}\rho V_T^{2}Sb{C}_{{\ell }_{{\delta }_a}}{\delta }_a=\overline{q}Sb{C}_{{\ell }_{{\delta }_a}}{\delta }_a$$

提取 ${\delta }_a$ 可得：

$${\delta }_a=\frac{2bS}{C_{{\ell }_{{\delta }_a}}}\frac{1}{\rho V_T^2}$$