# 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。数据融合并不是简单地把多个传感器的读数求平均，而是持续执行“预测、比较、修正”。 ```text 陀螺仪、加速度计 │ ▼ 状态预测 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主要包含： - 陀螺仪：测量机体三个轴的角速度。 - 加速度计：测量机体三个轴的比力。 陀螺仪用于推算姿态变化： ```text 上一时刻姿态 + 角速度积分 = 当前预测姿态 ``` 加速度经过当前姿态转换到地理坐标系，并扣除重力后，用来推算速度和位置： ```text 加速度积分 → 速度 速度积分 → 位置 ``` IMU频率高，可以提供连续、快速的状态预测。但陀螺仪和加速度计都存在噪声及零偏，因此只依靠 IMU 会逐渐漂移。 `Ekf::update()` 的核心调用顺序如下： ```cpp predictCovariance(imu_sample_delayed); predictState(imu_sample_delayed); controlFusionModes(imu_sample_delayed); ``` 含义是： 1. 预测状态的不确定度。 2. 使用 IMU 预测姿态、速度和位置等状态。 3. 使用其他传感器观测修正预测结果。 源码位置：`src/modules/ekf2/EKF/ekf.cpp:137`。 ### 3.1 EKF初始化姿态 启动时，EKF会检查飞机是否基本静止，例如： - 加速度模长是否接近重力加速度。 - 角速度是否低于规定范围。 满足条件后，EKF可以使用重力方向初始化横滚角和俯仰角。因此飞控启动和传感器初始化时应保持机体静止。 --- ## 4. 各传感器在融合中的作用 | 传感器 | 主要作用 | |---|---| | 陀螺仪 | 预测姿态变化 | | 加速度计 | 预测速度、位置，并约束横滚和俯仰 | | 磁力计 | 修正航向角 `yaw` | | GNSS/GPS | 修正水平位置、地速，也可辅助高度和航向 | | 气压计 | 修正高度和垂直位置 | | 空速计 | 提供相对空气速度，辅助估算风速和状态 | | 测距仪 | 提供近地距离和近地高度信息 | | 光流 | 在无 GNSS 环境中提供水平移动信息 | | 外部视觉 | 提供室内位置、姿态或速度观测 | 融合调度位于 `src/modules/ekf2/EKF/control.cpp:46`，其中根据编译配置、参数、传感器健康状态和数据有效性决定是否调用： ```cpp controlMagFusion(); controlGpsFusion(); controlAirDataFusion(); controlHeightFusion(); controlExternalVisionFusion(); ``` “代码被编译进固件”和“运行时正在融合”是两回事。例如固件包含空速融合代码，不代表没有连接空速计时它仍会融合空速数据。 --- ## 5. EKF 如何判断传感器数据是否可信 假设 EKF 根据 IMU 推算飞机速度是 `12 m/s`，GNSS报告速度是 `15 m/s`： ```text 创新量 innovation = 测量值 - 预测值 = 15 - 12 = 3 m/s ``` EKF会结合以下信息判断这个差值是否合理： - 传感器测量噪声。 - 当前状态协方差，即 EKF 对自己预测结果的不确定程度。 - 创新检验门限。 - 数据时间戳和新鲜程度。 - 传感器健康状态。 - 当前是否正在地面、空中、多旋翼或固定翼飞行。 根据判断结果，EKF可能： - 正常融合观测。 - 只进行较小幅度修正。 - 临时拒绝异常观测。 - 停止使用故障传感器。 - 必要时重置位置、速度或航向等状态。 卡尔曼增益决定一次观测应当把预测结果修正多少。因此 GNSS 突然跳变时，估计位置通常不会不加判断地跟着跳变。 --- ## 6. 不同传感器如何进行时间对齐 不同传感器存在不同延迟，例如 GNSS、气压计和空速计的数据到达时间通常不一致。PX4会把 IMU 数据放入环形缓冲区，在一个延迟的融合时刻处理观测： ```text 过去 当前 │-----------------------------│ ↑ ↑ 延迟融合时刻 实时输出时刻 ``` 处理过程为： 1. IMU数据持续进入缓冲区。 2. EKF读取缓冲区中较早的 IMU 样本。 3. 把具有延迟的 GNSS、气压计、空速等观测对齐到对应时刻。 4. 在延迟时刻执行预测和融合修正。 5. 输出预测器把修正后的状态继续预测到当前时刻。 这样既能正确处理传感器延迟，又能避免控制器直接使用明显滞后的姿态数据。 相关源码：`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` 设置了： ```sh param set-default EKF2_FUSE_BETA 0 ``` 这表示尾座式机型默认关闭固定翼零侧滑约束融合，因为当前 PX4 源码明确标记该功能尚不支持 tailsitter。 空速计仍然可以有两个互相独立的用途： 1. EKF2使用空速观测辅助估算风速和状态。 2. `vtol_att_control` 使用有效空速判断能否完成前转换。 第二项用途不等于 EKF 空速融合。即使 EKF没有使用某项空速融合方式，VTOL 状态机仍可能读取 `airspeed_validated`。 --- ## 9. 尾座式 VTOL 的物理转换过程 尾座式转换不是“停止旋翼发动机，再启动固定翼发动机”。对常见尾座式来说： - 悬停时机头朝上，电机推力向上。 - 前转换时整架飞机向前俯转并加速。 - 固定翼状态时机头朝前，同一组电机继续提供前向推力。 - 空速足够后，机翼产生主要升力。 - 后转换时整架飞机重新转回接近竖直姿态。 真正发生变化的是： - 飞机姿态目标。 - 多旋翼和固定翼控制输出的使用方式。 - 推力在不同控制坐标中的映射。 - 电机差动推力和舵面的控制权。 --- ## 10. 尾座式状态机 状态机位于 `src/modules/vtol_att_control/tailsitter.cpp:59`： ```text 多旋翼 MC_MODE │ 请求固定翼 ▼ 前转换 TRANSITION_FRONT_P1 │ 姿态和空速等条件满足 ▼ 固定翼 FW_MODE │ 请求多旋翼 ▼ 后转换 TRANSITION_BACK │ 接近竖直姿态或达到时间限制 ▼ 多旋翼 MC_MODE ``` `VT_TYPE=0` 表示使用 Tailsitter 实现。 VTOL切换请求可以来自遥控器开关、飞行任务、QGroundControl命令或失效保护逻辑。状态机不会因为检测到机体倾斜就自行随意切换模式，它需要相应的转换请求。 --- ## 11. 前转换：多旋翼到固定翼 收到固定翼请求后，状态变化为： ```text MC_MODE → TRANSITION_FRONT_P1 ``` PX4会保存转换开始时间和初始姿态，并生成一个不断向前旋转的姿态目标。 目标旋转速率近似为： ```text 90° / VT_F_TRANS_DUR ``` 参数默认值为： ```text VT_F_TRANS_DUR = 5 s ``` 这表示期望姿态目标大约用 5 秒从竖直方向转到水平飞行方向。它不是对飞机实际转换时间的绝对保证。实际姿态变化还受以下因素影响： - 推重比。 - 机体惯量。 - 多旋翼姿态和角速度参数。 - 电机响应速度。 - 气流和外部扰动。 源码位置：`src/modules/vtol_att_control/tailsitter.cpp:212`。 ### 11.1 转换期间由哪个控制器控制 前转换期间，尾座式主要由多旋翼姿态控制器完成整机姿态旋转： ```text 转换姿态目标 │ ▼ 多旋翼姿态控制器 │ ▼ 多旋翼角速度控制器 │ ▼ 电机力矩和推力 ``` 原因是刚开始转换时空速较低，机翼和舵面的气动控制能力还很弱，不能立即完全依靠固定翼舵面。 在非 `FW_MODE` 状态下，电机推力和力矩主要来自： ```text vehicle_torque_setpoint_virtual_mc vehicle_thrust_setpoint_virtual_mc ``` 相关源码：`src/modules/vtol_att_control/tailsitter.cpp:314`。 --- ## 12. 前转换完成条件 当前尾座式实现首先要求实际俯仰角达到： ```text pitch <= -60° ``` 阈值位于 `src/modules/vtol_att_control/tailsitter.h:52`。 ### 12.1 有有效空速计 当 `airspeed_validated` 中存在有效的真实传感器空速，并且 `FW_USE_AIRSPD` 启用时，完成条件是： ```text 实际俯仰角 <= -60° 并且 校准空速 >= VT_ARSP_TRANS ``` 默认参数： ```text VT_ARSP_TRANS = 10 m/s ``` 两个条件都满足后，状态机才会从 `TRANSITION_FRONT_P1` 进入 `FW_MODE`。 ### 12.2 没有有效空速计 如果 `vtol_att_control` 没有取得有效的传感器空速，尾座式专用判断主要依靠姿态： ```text 实际俯仰角 <= -60° ``` 然后允许完成转换。这意味着系统无法直接确认机翼是否真的获得了足够空速，安全性更依赖推重比、转换时间、机体实际性能和试飞验证。 完成判断位于 `src/modules/vtol_att_control/tailsitter.cpp:337`。 --- ## 13. 固定翼模式下的控制 进入 `FW_MODE` 后，控制链路为： ```text 固定翼位置/模式控制器 │ ▼ 航迹、滚转、俯仰和油门目标 │ ▼ 固定翼姿态与角速度控制器 │ ▼ virtual_fw 力矩和推力 │ ▼ vtol_att_control 坐标映射 │ ▼ 电机、差动推力和舵面 ``` 固定翼控制器发布： ```text vehicle_torque_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_fw ``` 尾座式代码把固定翼 X 轴的前向推力转换到尾座式电机对应的推力轴： ```cpp _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` 设置： ```text VT_ELEV_MC_LOCK = 0 ``` 这表示多旋翼状态下不锁住升降副翼。对应逻辑是： ```cpp if (!VT_ELEV_MC_LOCK || 当前不是MC模式) { 使用固定翼控制器产生的舵面力矩; } ``` 源码位置：`src/modules/vtol_att_control/tailsitter.cpp:328`。 因此当前配置下： - 电机在 MC 和转换阶段主要采用多旋翼控制输出。 - 舵面仍可以接收固定翼控制器输出。 - 低速时舵面气动效果较弱。 - 随空速增加，舵面产生的实际控制力自然增大。 这里的逐渐接管主要来自气动控制能力随空速增大，而不是尾座式代码将 MC 和 FW 电机力矩简单地做线性平均。 --- ## 15. 后转换：固定翼回到多旋翼 请求切回多旋翼后，状态变化为： ```text FW_MODE → TRANSITION_BACK ``` PX4生成逐渐回到竖直方向的姿态目标。目标旋转速率近似为： ```text 90° / VT_B_TRANS_DUR ``` 13200机型把参数默认值设置为： ```text VT_B_TRANS_DUR = 5 s ``` 后转换期间再次由多旋翼姿态控制器负责转动机体，并把固定翼推力平滑连接到多旋翼推力。 后转换完成条件是： ```text pitch >= -15° 或者 后转换时间 > VT_B_TRANS_DUR ``` 相关源码：`src/modules/vtol_att_control/tailsitter.cpp:92`。 --- ## 16. 转换失败与 Quadchute Quadchute 是 VTOL 在固定翼飞行或转换异常时，紧急退回多旋翼模式的保护机制。可能触发的原因包括： - 固定翼飞行高度低于允许值。 - 出现非指令性的持续掉高。 - 前转换掉高超过限制。 - 固定翼俯仰角超过限制。 - 固定翼横滚角超过限制。 - 前转换超时。 - 有空速计，但空速增长过慢。 - 外部命令要求立即退回 MC。 例如，当有有效空速时，满足下面的情况可能判断为前转换失败： ```text 转换时间 > VT_F_TR_OL_TM 并且 空速 < VT_ARSP_BLEND ``` 相关保护判断位于 `src/modules/vtol_att_control/vtol_type.cpp:327`。 需要注意，Quadchute不是降落伞功能。它表示从固定翼控制状态快速退回多旋翼控制状态。 --- ## 17. 完整的数据和控制链路 ```text IMU / GNSS / 气压计 / 磁力计 / 空速计 │ ▼ EKF2 姿态、位置、速度、风速 │ ┌─────────┴─────────┐ ▼ ▼ 多旋翼控制器 固定翼控制器 │ │ virtual_mc 输出 virtual_fw 输出 └─────────┬─────────┘ ▼ vtol_att_control 状态判断、姿态转换、输出选择 │ ▼ control_allocator │ ▼ 电机和舵机 ``` 要点如下： 1. 多旋翼和固定翼控制器共用同一个 EKF2 状态估计。 2. 转换时 EKF2 不会重新初始化。 3. MC 和 FW 控制模块会生成各自的虚拟控制输出。 4. `vtol_att_control` 根据当前状态选择并映射这些输出。 5. 尾座式前后转换期间主要由多旋翼控制器控制整机姿态旋转。 6. 有有效空速时，前转换会同时检查姿态和空速。 7. 同一组电机通常始终工作，变化的是推力方向、控制来源和坐标映射。 8. `control_allocator` 最终把期望推力和力矩分配为各个电机、舵机的具体输出。 --- ## 18. 实机调试时建议观察的消息 可以在 PX4 控制台中使用 `listener`： ```sh 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. 首次实机验证顺序 建议按以下顺序逐级验证，不要第一次试飞就直接测试完整自动转换： 1. 拆除螺旋桨，确认机型、传感器方向和飞控安装方向。 2. 检查每个电机编号、旋转方向以及失控保护。 3. 检查每个舵面的方向、限位和中立位置。 4. 分别确认 MC 与 FW 模式下的执行器响应方向。 5. 完成加速度计、陀螺仪、磁力计、遥控器和空速计校准。 6. 确认 EKF 状态正常，所有所需观测正在融合。 7. 先完成稳定的多旋翼悬停和位置控制测试。 8. 根据机体条件验证固定翼控制参数和执行器方向。 9. 在模拟器或受控环境中验证前后转换状态机。 10. 在足够高度、足够空间和具备人工接管条件时进行实机转换测试。 特别需要确认：`13200` 是通用双电机、双舵面尾座式模板。实际飞机的电机数量、舵面形式和执行器几何必须与控制分配配置一致。仅仅选择该机型并不意味着它天然适配所有尾座式结构。