PX4-Autopilot代码解析（14）-固定翼控制

固定翼控制系统负责管理 PX4 中固定翼飞机的姿态、能量和飞行路径控制。本文档涵盖总能量控制系统 (TECS)、姿态控制器、速率控制器及其与飞行控制堆栈的集成。

系统架构概述

固定翼控制系统采用级联控制架构，最高级别为能量管理，其次是姿态控制，最后是速率控制。

TECS能源管理

总能量控制系统（TECS）是固定翼纵向控制的核心，管理高度（势能）和空速（动能）之间的能量平衡。

TECS组件

能量平衡控制逻辑

TECS 控制器采用双回路方法，其中：

• 油门控制总能量：管理势能和动能的总和
• 俯仰控制能量平衡：管理高度和空速之间的分配

能源费率计算如下_calcSpecificEnergyRates()：

Specific Potential Energy Rate = altitude_rate * g
Specific Kinetic Energy Rate = airspeed * airspeed_rate  
Total Energy Rate = SPE_rate + SKE_rate
Energy Balance Rate = SPE_rate - SKE_rate (weighted)

姿态控制系统

该FixedwingAttitudeControl模块使用单独的单轴控制器将来自 TECS 和导航的姿态设定点转换为身体速率设定点。

姿态控制器架构

控制器实现细节

每个姿态控制器都实现了一个具有时间常数调节的简单比例控制器：

• 滚动控制器：roll_rate_setpoint = (roll_setpoint - roll_actual) / time_constant
• 俯仰控制器：类似的 P 控制，具有单独的正/负速率限制
• 偏航控制器：根据滚转角和空速实现协调转弯逻辑
• 车轮控制器：用于跑道操作的地面转向PID控制器

控制器使用旋转矩阵雅可比矩阵将欧拉速率设定点转换为机身速率设定点，以解释飞机姿态。

速率控制系统

该FixedwingRateControl模块实现最内层的控制回路，将角速率设定点转换为执行器扭矩和推力命令。

速率控制器架构

空速缩放

固定翼速率控制的一个关键特征是空速缩放，它根据动态压力调整控制面效率：

scaling_factor = trim_airspeed / current_airspeed
control_output = PID_output * scaling_factor²

这补偿了空速和空气动力控制效率之间的二次关系。

手动飞行模式

速率控制器支持多种手动飞行模式：

• 稳定：通过手动速率输入进行姿态控制
• Acro：通过飞行员操纵杆直接控制速率
• 手动：直接执行器控制（直通）

控制分配集成

固定翼速率控制输出由控制分配系统处理以驱动物理执行器：

控制分配为速率控制器提供饱和反馈，以防止在达到执行器极限时积分器饱和。

关键配置参数

TECS参数

范围	描述	典型范围
FW_T_ALT_TC	高度跟踪时间常数	2.0 - 10.0 秒
FW_T_I_GAIN_PIT	音调积分器增益	0.0 - 2.0
FW_T_THR_INTEG	油门积分器增益	0.0 - 1.0
FW_T_PTCH_DAMP	俯仰阻尼增益	0.0 - 2.0
FW_T_THR_DAMPING	油门阻尼增益	0.0 - 1.0

姿态控制参数

范围	描述	典型范围
FW_R_TC	滚动时间常数	0.2 - 1.0 秒
FW_P_TC	俯仰时间常数	0.2 - 1.0 秒
FW_R_RMAX	最大滚转速率	30 - 120 度/秒
FW_P_RMAX_POS/NEG	最大俯仰速率	30 - 90 度/秒

速率控制参数

范围	描述	典型范围
FW_RR_P，，FW_PR_P​FW_YR_P	利率 P 上涨	0.01 - 0.2
FW_RR_I，，FW_PR_I​FW_YR_I	利率 I 上涨	0.0 - 0.5
FW_RR_FF，，FW_PR_FF​FW_YR_FF	速率前馈	0.1 - 1.0
FW_ARSP_SCALE_EN	启用空速缩放	0/1