19 KiB

Raw Blame History

PX4 EKF2 数据融合与尾座式 VTOL 切换逻辑

1. 当前源码与机型背景

本文针对当前工程中的配置进行说明：

PX4 源码版本：v1.17.0
PX4 源码目录：simulation_ws/deps/PX4-Autopilot
飞控：雷迅/CUAV 7-Nano V2
固件目标：cuav_7-nano_minimal_vtol
机型：Generic VTOL Tailsitter
SYS_AUTOSTART=13200
VT_TYPE=0，表示尾座式 VTOL

需要先区分两个概念：

EKF2 负责数据融合，回答“飞机当前姿态、位置、速度和风速是多少”。
vtol_att_control 负责 VTOL 状态管理，决定使用多旋翼还是固定翼控制逻辑。

它们会相互使用数据，但不是同一个模块。

2. EKF2 数据融合的总体过程

PX4 使用扩展卡尔曼滤波器 EKF2。数据融合并不是简单地把多个传感器的读数求平均，而是持续执行“预测、比较、修正”。

陀螺仪、加速度计
        │
        ▼
   状态预测 Predict
        │
        ├── 磁力计
        ├── GNSS/GPS
        ├── 气压计
        ├── 空速计
        ├── 测距仪
        ├── 光流
        └── 外部视觉
                │
                ▼
       计算创新量和可信度
                │
                ▼
       状态修正 Correct
                │
                ▼
姿态、位置、速度、风速、传感器偏置

核心源码：

src/modules/ekf2/EKF2.cpp
src/modules/ekf2/EKF/ekf.cpp
src/modules/ekf2/EKF/control.cpp
src/modules/ekf2/EKF/estimator_interface.cpp

3. IMU 如何进行状态预测

IMU主要包含：

陀螺仪：测量机体三个轴的角速度。
加速度计：测量机体三个轴的比力。

陀螺仪用于推算姿态变化：

上一时刻姿态 + 角速度积分 = 当前预测姿态

加速度经过当前姿态转换到地理坐标系，并扣除重力后，用来推算速度和位置：

加速度积分 → 速度
速度积分   → 位置

IMU频率高，可以提供连续、快速的状态预测。但陀螺仪和加速度计都存在噪声及零偏，因此只依靠 IMU 会逐渐漂移。

Ekf::update() 的核心调用顺序如下：

predictCovariance(imu_sample_delayed);
predictState(imu_sample_delayed);
controlFusionModes(imu_sample_delayed);

含义是：

预测状态的不确定度。
使用 IMU 预测姿态、速度和位置等状态。
使用其他传感器观测修正预测结果。

源码位置：src/modules/ekf2/EKF/ekf.cpp:137。

3.1 EKF初始化姿态

启动时，EKF会检查飞机是否基本静止，例如：

加速度模长是否接近重力加速度。
角速度是否低于规定范围。

满足条件后，EKF可以使用重力方向初始化横滚角和俯仰角。因此飞控启动和传感器初始化时应保持机体静止。

4. 各传感器在融合中的作用

传感器	主要作用
陀螺仪	预测姿态变化
加速度计	预测速度、位置，并约束横滚和俯仰
磁力计	修正航向角 `yaw`
GNSS/GPS	修正水平位置、地速，也可辅助高度和航向
气压计	修正高度和垂直位置
空速计	提供相对空气速度，辅助估算风速和状态
测距仪	提供近地距离和近地高度信息
光流	在无 GNSS 环境中提供水平移动信息
外部视觉	提供室内位置、姿态或速度观测

融合调度位于 src/modules/ekf2/EKF/control.cpp:46，其中根据编译配置、参数、传感器健康状态和数据有效性决定是否调用：

controlMagFusion();
controlGpsFusion();
controlAirDataFusion();
controlHeightFusion();
controlExternalVisionFusion();

“代码被编译进固件”和“运行时正在融合”是两回事。例如固件包含空速融合代码，不代表没有连接空速计时它仍会融合空速数据。

5. EKF 如何判断传感器数据是否可信

假设 EKF 根据 IMU 推算飞机速度是 12 m/s，GNSS报告速度是 15 m/s：

创新量 innovation = 测量值 - 预测值
                  = 15 - 12
                  = 3 m/s

EKF会结合以下信息判断这个差值是否合理：

传感器测量噪声。
当前状态协方差，即 EKF 对自己预测结果的不确定程度。
创新检验门限。
数据时间戳和新鲜程度。
传感器健康状态。
当前是否正在地面、空中、多旋翼或固定翼飞行。

根据判断结果，EKF可能：

正常融合观测。
只进行较小幅度修正。
临时拒绝异常观测。
停止使用故障传感器。
必要时重置位置、速度或航向等状态。

卡尔曼增益决定一次观测应当把预测结果修正多少。因此 GNSS 突然跳变时，估计位置通常不会不加判断地跟着跳变。

6. 不同传感器如何进行时间对齐

不同传感器存在不同延迟，例如 GNSS、气压计和空速计的数据到达时间通常不一致。PX4会把 IMU 数据放入环形缓冲区，在一个延迟的融合时刻处理观测：

过去                         当前
│-----------------------------│
       ↑                      ↑
  延迟融合时刻             实时输出时刻

处理过程为：

IMU数据持续进入缓冲区。
EKF读取缓冲区中较早的 IMU 样本。
把具有延迟的 GNSS、气压计、空速等观测对齐到对应时刻。
在延迟时刻执行预测和融合修正。
输出预测器把修正后的状态继续预测到当前时刻。

这样既能正确处理传感器延迟，又能避免控制器直接使用明显滞后的姿态数据。

相关源码：src/modules/ekf2/EKF/estimator_interface.cpp:78。

7. EKF2 输出哪些数据

EKF2通过 uORB 发布的主要消息包括：

vehicle_attitude：当前姿态四元数。
vehicle_local_position：本地坐标位置、速度、高度。
vehicle_global_position：经纬度和海拔。
vehicle_odometry：完整位姿和速度。
wind：估算风速。
estimator_status：估计器状态。
estimator_status_flags：融合状态标志。
estimator_innovations：各观测创新量。

多旋翼控制器和固定翼控制器使用同一套 EKF 状态。切换到固定翼模式时不会停止或重新启动 EKF。尾座式机体转过约 90 度后，EKF只是用四元数继续表示新的机体姿态。

8. 当前尾座式机型的融合特殊项

13200_generic_vtol_tailsitter 设置了：

param set-default EKF2_FUSE_BETA 0

这表示尾座式机型默认关闭固定翼零侧滑约束融合，因为当前 PX4 源码明确标记该功能尚不支持 tailsitter。

空速计仍然可以有两个互相独立的用途：

EKF2使用空速观测辅助估算风速和状态。
vtol_att_control 使用有效空速判断能否完成前转换。

第二项用途不等于 EKF 空速融合。即使 EKF没有使用某项空速融合方式，VTOL 状态机仍可能读取 airspeed_validated。

9. 尾座式 VTOL 的物理转换过程

尾座式转换不是“停止旋翼发动机，再启动固定翼发动机”。对常见尾座式来说：

悬停时机头朝上，电机推力向上。
前转换时整架飞机向前俯转并加速。
固定翼状态时机头朝前，同一组电机继续提供前向推力。
空速足够后，机翼产生主要升力。
后转换时整架飞机重新转回接近竖直姿态。

真正发生变化的是：

飞机姿态目标。
多旋翼和固定翼控制输出的使用方式。
推力在不同控制坐标中的映射。
电机差动推力和舵面的控制权。

10. 尾座式状态机

状态机位于 src/modules/vtol_att_control/tailsitter.cpp:59：

多旋翼 MC_MODE
      │ 请求固定翼
      ▼
前转换 TRANSITION_FRONT_P1
      │ 姿态和空速等条件满足
      ▼
固定翼 FW_MODE
      │ 请求多旋翼
      ▼
后转换 TRANSITION_BACK
      │ 接近竖直姿态或达到时间限制
      ▼
多旋翼 MC_MODE

VT_TYPE=0 表示使用 Tailsitter 实现。

VTOL切换请求可以来自遥控器开关、飞行任务、QGroundControl命令或失效保护逻辑。状态机不会因为检测到机体倾斜就自行随意切换模式，它需要相应的转换请求。

11. 前转换：多旋翼到固定翼

收到固定翼请求后，状态变化为：

MC_MODE → TRANSITION_FRONT_P1

PX4会保存转换开始时间和初始姿态，并生成一个不断向前旋转的姿态目标。

目标旋转速率近似为：

90° / VT_F_TRANS_DUR

参数默认值为：

VT_F_TRANS_DUR = 5 s

这表示期望姿态目标大约用 5 秒从竖直方向转到水平飞行方向。它不是对飞机实际转换时间的绝对保证。实际姿态变化还受以下因素影响：

推重比。
机体惯量。
多旋翼姿态和角速度参数。
电机响应速度。
气流和外部扰动。

源码位置：src/modules/vtol_att_control/tailsitter.cpp:212。

11.1 转换期间由哪个控制器控制

前转换期间，尾座式主要由多旋翼姿态控制器完成整机姿态旋转：

转换姿态目标
      │
      ▼
多旋翼姿态控制器
      │
      ▼
多旋翼角速度控制器
      │
      ▼
电机力矩和推力

原因是刚开始转换时空速较低，机翼和舵面的气动控制能力还很弱，不能立即完全依靠固定翼舵面。

在非 FW_MODE 状态下，电机推力和力矩主要来自：

vehicle_torque_setpoint_virtual_mc
vehicle_thrust_setpoint_virtual_mc

相关源码：src/modules/vtol_att_control/tailsitter.cpp:314。

12. 前转换完成条件

当前尾座式实现首先要求实际俯仰角达到：

pitch <= -60°

阈值位于 src/modules/vtol_att_control/tailsitter.h:52。

12.1 有有效空速计

当 airspeed_validated 中存在有效的真实传感器空速，并且 FW_USE_AIRSPD 启用时，完成条件是：

实际俯仰角 <= -60°
并且
校准空速 >= VT_ARSP_TRANS

默认参数：

VT_ARSP_TRANS = 10 m/s

两个条件都满足后，状态机才会从 TRANSITION_FRONT_P1 进入 FW_MODE。

12.2 没有有效空速计

如果 vtol_att_control 没有取得有效的传感器空速，尾座式专用判断主要依靠姿态：

实际俯仰角 <= -60°

然后允许完成转换。这意味着系统无法直接确认机翼是否真的获得了足够空速，安全性更依赖推重比、转换时间、机体实际性能和试飞验证。

完成判断位于 src/modules/vtol_att_control/tailsitter.cpp:337。

13. 固定翼模式下的控制

进入 FW_MODE 后，控制链路为：

固定翼位置/模式控制器
          │
          ▼
航迹、滚转、俯仰和油门目标
          │
          ▼
固定翼姿态与角速度控制器
          │
          ▼
virtual_fw 力矩和推力
          │
          ▼
vtol_att_control 坐标映射
          │
          ▼
电机、差动推力和舵面

固定翼控制器发布：

vehicle_torque_setpoint_virtual_fw
vehicle_thrust_setpoint_virtual_fw

尾座式代码把固定翼 X 轴的前向推力转换到尾座式电机对应的推力轴：

_thrust_setpoint_0->xyz[2] =
    -_vehicle_thrust_setpoint_virtual_fw->xyz[0];

源码位置：src/modules/vtol_att_control/tailsitter.cpp:287。

这不是电机停止或重新启动，而是控制输出来源和控制坐标发生变化。

如果启用了相关参数，固定翼控制器还可以通过电机差动推力产生滚转、俯仰或偏航力矩。

14. 舵面什么时候参与控制

当前 13200_generic_vtol_tailsitter 设置：

VT_ELEV_MC_LOCK = 0

这表示多旋翼状态下不锁住升降副翼。对应逻辑是：

if (!VT_ELEV_MC_LOCK || 当前不是MC模式) {
    使用固定翼控制器产生的舵面力矩;
}

源码位置：src/modules/vtol_att_control/tailsitter.cpp:328。

因此当前配置下：

电机在 MC 和转换阶段主要采用多旋翼控制输出。
舵面仍可以接收固定翼控制器输出。
低速时舵面气动效果较弱。
随空速增加，舵面产生的实际控制力自然增大。

这里的逐渐接管主要来自气动控制能力随空速增大，而不是尾座式代码将 MC 和 FW 电机力矩简单地做线性平均。

15. 后转换：固定翼回到多旋翼

请求切回多旋翼后，状态变化为：

FW_MODE → TRANSITION_BACK

PX4生成逐渐回到竖直方向的姿态目标。目标旋转速率近似为：

90° / VT_B_TRANS_DUR

13200机型把参数默认值设置为：

VT_B_TRANS_DUR = 5 s

后转换期间再次由多旋翼姿态控制器负责转动机体，并把固定翼推力平滑连接到多旋翼推力。

后转换完成条件是：

pitch >= -15°
或者
后转换时间 > VT_B_TRANS_DUR

相关源码：src/modules/vtol_att_control/tailsitter.cpp:92。

16. 转换失败与 Quadchute

Quadchute 是 VTOL 在固定翼飞行或转换异常时，紧急退回多旋翼模式的保护机制。可能触发的原因包括：

固定翼飞行高度低于允许值。
出现非指令性的持续掉高。
前转换掉高超过限制。
固定翼俯仰角超过限制。
固定翼横滚角超过限制。
前转换超时。
有空速计，但空速增长过慢。
外部命令要求立即退回 MC。

例如，当有有效空速时，满足下面的情况可能判断为前转换失败：

转换时间 > VT_F_TR_OL_TM
并且
空速 < VT_ARSP_BLEND

相关保护判断位于 src/modules/vtol_att_control/vtol_type.cpp:327。

需要注意，Quadchute不是降落伞功能。它表示从固定翼控制状态快速退回多旋翼控制状态。

17. 完整的数据和控制链路

IMU / GNSS / 气压计 / 磁力计 / 空速计
                    │
                    ▼
                  EKF2
         姿态、位置、速度、风速
                    │
          ┌─────────┴─────────┐
          ▼                   ▼
     多旋翼控制器          固定翼控制器
          │                   │
 virtual_mc 输出        virtual_fw 输出
          └─────────┬─────────┘
                    ▼
            vtol_att_control
       状态判断、姿态转换、输出选择
                    │
                    ▼
             control_allocator
                    │
                    ▼
                电机和舵机

要点如下：

多旋翼和固定翼控制器共用同一个 EKF2 状态估计。
转换时 EKF2 不会重新初始化。
MC 和 FW 控制模块会生成各自的虚拟控制输出。
vtol_att_control 根据当前状态选择并映射这些输出。
尾座式前后转换期间主要由多旋翼控制器控制整机姿态旋转。
有有效空速时，前转换会同时检查姿态和空速。
同一组电机通常始终工作，变化的是推力方向、控制来源和坐标映射。
control_allocator 最终把期望推力和力矩分配为各个电机、舵机的具体输出。

18. 实机调试时建议观察的消息

可以在 PX4 控制台中使用 listener：

listener estimator_status
listener estimator_status_flags
listener estimator_innovations
listener vehicle_attitude
listener vehicle_local_position
listener airspeed_validated
listener vtol_vehicle_status
listener vehicle_torque_setpoint_virtual_mc
listener vehicle_torque_setpoint_virtual_fw
listener vehicle_thrust_setpoint_virtual_mc
listener vehicle_thrust_setpoint_virtual_fw

其中：

estimator_status：检查 EKF 整体状态和故障信息。
estimator_status_flags：确认 GNSS、磁力计、气压、空速等是否正在融合。
estimator_innovations：检查传感器观测与 EKF 预测的差异。
airspeed_validated：检查空速来源、校准空速和数据是否有效。
vtol_vehicle_status：检查当前处于 MC、前转换、FW 还是后转换。
virtual_mc/fw 消息：比较两套控制器分别产生了什么输出。

实飞问题应优先提供 .ulg 日志。分析转换问题时，至少关注：

实际姿态与姿态目标。
空速、地速和高度。
VTOL 状态变化时间点。
MC/FW 虚拟推力和力矩。
电机及舵机最终输出。
EKF创新量和融合状态。
Quadchute原因及相关事件消息。

19. 当前参数的参考值

以下是当前源码中的主要默认值或 13200 机型覆盖值：

参数	参考值	含义
`VT_TYPE`	`0`	尾座式 VTOL
`VT_F_TRANS_DUR`	`5 s`	前转换目标姿态旋转时长
`VT_B_TRANS_DUR`	`5 s`	后转换目标姿态旋转时长，13200覆盖值
`VT_ARSP_BLEND`	`8 m/s`	转换检查使用的空速参考值
`VT_ARSP_TRANS`	`10 m/s`	有效空速条件下完成前转换的空速
`VT_TRANS_TIMEOUT`	`15 s`	前转换总超时
`VT_TRANS_MIN_TM`	`2 s`	通用最短前转换时间参数
`VT_F_TR_OL_TM`	`6 s`	前转换开环时间/检查参考时间
`VT_ELEV_MC_LOCK`	`0`	MC模式下不锁住尾座式舵面
`EKF2_FUSE_BETA`	`0`	尾座式关闭零侧滑约束融合

不要仅根据默认值直接进行实机转换。转换参数必须结合以下实际数据确定：

机体重量和重心。
推重比和电机响应。
翼载荷及失速速度。
空速计安装方向与校准结果。
多旋翼和固定翼两套控制参数。
执行器几何和控制分配。

20. 首次实机验证顺序

建议按以下顺序逐级验证，不要第一次试飞就直接测试完整自动转换：

拆除螺旋桨，确认机型、传感器方向和飞控安装方向。
检查每个电机编号、旋转方向以及失控保护。
检查每个舵面的方向、限位和中立位置。
分别确认 MC 与 FW 模式下的执行器响应方向。
完成加速度计、陀螺仪、磁力计、遥控器和空速计校准。
确认 EKF 状态正常，所有所需观测正在融合。
先完成稳定的多旋翼悬停和位置控制测试。
根据机体条件验证固定翼控制参数和执行器方向。
在模拟器或受控环境中验证前后转换状态机。
在足够高度、足够空间和具备人工接管条件时进行实机转换测试。

特别需要确认：13200 是通用双电机、双舵面尾座式模板。实际飞机的电机数量、舵面形式和执行器几何必须与控制分配配置一致。仅仅选择该机型并不意味着它天然适配所有尾座式结构。

19 KiB Raw Blame History