4、传感器驱动与EKF融合：PX4的驱动框架与ArduPilot的AP_InertialNav
传感器融合，说白了就是飞控的「大脑皮层」。你硬件再好，IMU数据再干净，融合算法不行，飞机照样晃成筛子。我这些年调试过的炸机案例，少说有三分之一跟传感器融合参数没调对有关。
PX4和ArduPilot在这块走了两条完全不同的路。一个偏模块化、偏学术范儿；一个偏工程化、偏实战派。咱们今天就把它们的驱动框架和EKF核心掰开揉碎看看。
4.1 传感器驱动架构：模块化 vs 单体化
先看驱动层。你想想看，飞控要接IMU、磁力计、气压计、GPS、光流……这些传感器的数据怎么进到飞控核心？
4.1.1 PX4的uORB驱动模型
PX4用了一个叫uORB的发布-订阅总线。每个传感器驱动都是一个独立进程，拿到原始数据后，通过uORB广播出去。谁想用这个数据，订阅就行。
// PX4传感器数据发布示例（伪代码）
// gyro驱动进程
int gyro_fd = orb_advertise(ORB_ID(sensor_gyro), &gyro_data);
while (1) {
read_gyro_hw(&gyro_data);
orb_publish(ORB_ID(sensor_gyro), gyro_fd, &gyro_data);
usleep(1000); // 1kHz
}这样做的好处很明显：
- 解耦彻底——驱动挂了不影响其他模块
- 调试方便——我可以单独拉一个驱动进程出来测数据
- 扩展性强——加一个新传感器，写个驱动，发布到uORB就行
4.1.2 ArduPilot的AP_InertialSensor层
ArduPilot这边，驱动层叫AP_InertialSensor。它把所有IMU相关的传感器驱动都塞进了一个统一的抽象层里。你不需要关心底层是SPI还是I2C，AP_InertialSensor帮你搞定。
// ArduPilot IMU数据读取示例（伪代码）
// AP_InertialSensor统一接口
AP_InertialSensor ins;
void loop() {
ins.update(); // 一次调用，更新所有IMU
Vector3f gyro = ins.get_gyro(0); // 获取主IMU陀螺仪
Vector3f accel = ins.get_accel(0);
}ArduPilot的做法更「嵌入式」：
- 代码紧凑——一个函数搞定所有IMU读取
- 实时性好——没有进程间通信开销
- 调试集中——所有IMU数据在一个地方处理
4.2 EKF融合算法：EKF2 vs AP_NavEKF
驱动层只是前菜。真正的硬骨头，是EKF融合算法。这部分决定了你的飞机在GPS丢星、磁力计受干扰时，还能不能稳住。
4.2.1 PX4的EKF2
PX4用的是EKF2，一个24状态的扩展卡尔曼滤波器。它把IMU、GPS、磁力计、气压计、光流的数据全部融合在一起。
EKF2的核心思路是：
- 预测步——用IMU的角速度和加速度，预测下一时刻的状态
- 更新步——用GPS、磁力计等观测数据，修正预测误差
- 健康监测——每个传感器都有独立的创新序列监测，发现异常自动降权
我记得有一次在强磁干扰环境下试飞，磁力计数据完全乱了。EKF2的健康监测自动把磁力计的权重降到了接近零，飞机靠IMU和GPS继续飞，愣是没翻。
EKF2_GPS_POS_X：GPS天线相对于IMU的位置偏移EKF2_MAG_TYPE：磁力计融合模式（0=自动，1=3D，2=2D）EKF2_IMU_POS_X：IMU相对于重心的位置
这些参数调不好，EKF2再牛也白搭。我见过有人把GPS位置偏移设错了，结果飞机悬停时一直在原地画圈。
4.2.2 ArduPilot的AP_NavEKF
ArduPilot这边，用的是AP_NavEKF。它其实有两个版本：EKF2（老版）和EKF3（新版）。现在主流是EKF3，一个28状态的滤波器。
AP_NavEKF3比PX4的EKF2多了几个状态：
- 地磁扰动估计——能在线估计磁力计的硬铁和软铁干扰
- 风场估计——固定翼模式下，能估计风速和风向
- 双天线GPS偏航——支持双天线GPS直接测量航向
// ArduPilot EKF3参数配置示例
// 启用EKF3
AHRS_EKF_TYPE = 3
// 磁力计在线校准
EK3_MAG_CAL = 1 // 启用磁力计在线校准
EK3_MAG_I = 0.01 // 磁力计积分增益
// 风场估计（固定翼）
EK3_WIND_P_NOISE = 0.1 // 风场过程噪声我个人觉得，ArduPilot的EKF3在磁力计处理上更激进。它那个在线校准功能，在无人机靠近铁塔时，能自动修正磁力计偏差。PX4的EKF2虽然也有磁力计融合，但不会主动去估计硬铁干扰。
EK3_MAG_I从0.01改到0.003，问题解决。调参这事儿，真得看场景。
4.3 传感器融合对比表
| 对比项 | PX4 EKF2 | ArduPilot EKF3 |
|---|---|---|
| 状态维度 | 24状态 | 28状态 |
| 磁力计处理 | 固定偏差补偿 | 在线硬铁/软铁估计 |
| 风场估计 | 不支持 | 支持（固定翼） |
| 双天线GPS | 支持（需配置） | 原生支持 |
| 传感器健康监测 | 创新序列监测 | 创新序列 + 方差监测 |
| 参数数量 | 约50个 | 约80个 |
4.4 核心逻辑流程图
下面这张图，是我自己总结的传感器融合核心流程。两个飞控虽然实现不同，但逻辑骨架是一样的。
4.5 选型建议
说了这么多，到底选哪个？我个人的建议是这样的：
- 如果你做科研、做算法验证——选PX4。uORB的模块化架构让你能轻松替换某个传感器驱动或融合算法模块。我见过有人在PX4上把EKF2换成因子图优化，uORB的接口几乎不用改。
- 如果你做产品、做行业应用——选ArduPilot。AP_NavEKF3的在线校准和风场估计，在真实场景下更稳定。特别是磁力计在线校准，在电力巡检、桥梁检测这种强干扰场景下，能省你很多事。
- 如果你做固定翼——强烈推荐ArduPilot。EKF3的风场估计是固定翼的刚需，PX4目前还没有这个功能。