4. 飞控基础：PX4/ArduPilot姿态解算与位置控制环路解析
做视觉SLAM和飞控融合，有个坎儿你迟早得迈过去——飞控到底在算什么？
我见过不少做视觉的同学，把SLAM跑得飞起，一接飞控就懵了。为什么？因为飞控里那套姿态解算和控制逻辑，说白了是另一套语言。今天我就带你把这层窗户纸捅破。
4.1 姿态解算：飞控的“感知层”
飞控首先要回答一个问题：我现在是什么姿势？
姿态解算，就是靠IMU（加速度计+陀螺仪）和磁力计，算出飞机的roll、pitch、yaw。嗯，这里有个坑——IMU的原始数据噪声很大，直接积分陀螺仪，几分钟就漂到姥姥家了。
核心思路：用加速度计和磁力计做“长期校准”，用陀螺仪做“短期预测”。两者互补，这就是互补滤波和卡尔曼滤波的底层逻辑。
4.1.1 PX4的EKF2姿态估计
PX4用的是扩展卡尔曼滤波（EKF2）。我个人习惯把它拆成三步理解：
- 预测：用陀螺仪角速度积分，预测下一时刻的姿态
- 观测：用加速度计测重力方向，磁力计测地磁方向
- 更新：把预测和观测加权融合，得到最优估计
我曾经在项目里遇到过，磁力计被机载电机干扰，yaw角直接偏了30度。后来加了硬铁校准和动态阈值，才算稳住。
我的经验：如果你做视觉SLAM和飞控融合，建议把视觉里程计（VO）作为EKF的额外观测源。PX4的EKF2支持外部位置/姿态输入，这就是融合的接口。
4.1.2 ArduPilot的DCM算法
ArduPilot早期用的是方向余弦矩阵（DCM）算法。说白了，就是维护一个旋转矩阵，不断用陀螺仪更新，再用加速度计和磁力计做修正。
DCM的好处是计算量小，适合老款STM32F1。但精度不如EKF。现在ArduPilot也主推EKF了，不过DCM作为备选还在。
| 对比项 | PX4 EKF2 | ArduPilot DCM |
|---|---|---|
| 计算量 | 高（需要浮点运算） | 低（整数运算即可） |
| 精度 | 高，尤其动态场景 | 中等，稳态尚可 |
| 融合外部传感器 | 原生支持（GPS、视觉、激光） | 需额外修改 |
| 适用场景 | 复杂环境、长航时 | 简单飞行、低成本硬件 |
4.2 位置控制环路：飞控的“决策层”
知道姿态之后，飞控要回答第二个问题：怎么飞到目标点？
位置控制，说白了就是一套串级PID。外环算位置误差，输出期望速度；内环算速度误差，输出期望姿态（roll/pitch角）。
串级PID的结构：
位置环（P）→ 速度环（PI）→ 姿态环（PID）→ 角速率环（PID）→ 混控器 → 电机
你想想看，为什么不用单级PID？因为无人机是个欠驱动系统，你没法直接控制位置，只能通过倾斜机身产生水平推力。串级PID把“位置→速度→姿态”层层分解，每个环节的物理意义都很清晰。
4.2.1 PX4的MC位置控制
PX4的多旋翼位置控制代码在 mc_pos_control 模块。核心逻辑是这样的：
// 伪代码示意
// 外环：位置误差 → 期望速度
desired_velocity = P_gain * (target_position - current_position)
// 限幅
desired_velocity = clamp(desired_velocity, max_speed)
// 内环：速度误差 → 期望加速度
desired_acceleration = P_gain * (desired_velocity - current_velocity) + I_gain * integral
// 加上前馈项（比如重力补偿）
desired_acceleration += gravity_vector
// 加速度 → 期望姿态（roll/pitch）
roll_desired = atan2(desired_acceleration.y, gravity)
pitch_desired = atan2(-desired_acceleration.x, gravity)嗯，这里要注意：前馈项很重要。我刚开始做的时候，没加重力补偿，飞机悬停时总是往一边飘。后来才发现，加速度计测到的重力方向，必须显式补偿到控制量里。
避坑指南：我曾经在PX4里直接修改位置环的P增益，结果飞机一推油门就剧烈震荡。后来查了代码才发现，PX4的位置环输出默认做了平滑滤波（Slew Rate Limiter）。你改增益的同时，别忘了调整滤波参数。
4.2.2 ArduPilot的位置控制
ArduPilot的位置控制逻辑类似，但实现上更“工程化”。它把控制拆成了几个模式：
- Loiter模式：位置保持，用GPS或光流
- Guided模式：飞向指定GPS坐标
- Auto模式：按航点飞行
每个模式里，位置环和速度环的参数是独立的。我个人习惯在Loiter模式下把位置环P增益调低一点，因为GPS有漂移，调太高反而会引入抖动。
4.3 视觉SLAM与飞控的融合点
好了，现在你知道了飞控的姿态解算和位置控制。那视觉SLAM怎么接进来？
说白了就两个接口：
- 姿态/位置观测：把SLAM算出的位姿，作为EKF的观测源，修正飞控的估计
- 速度/位置指令：把SLAM规划出的路径，作为位置环的目标输入
我见过最常用的做法是：SLAM输出位置和速度，通过MAVLink的VISION_POSITION_ESTIMATE消息发给飞控。飞控的EKF收到后，会把它和GPS、IMU做融合。
我的建议：刚开始做融合时，别一上来就全权交给视觉。先让飞控用GPS飞，视觉只做“辅助观测”。等视觉的鲁棒性验证好了，再逐步提高视觉的权重。我吃过这个亏——第一次试飞时视觉掉帧，飞机直接乱飘。
4.4 本章知识体系
下面这张图，是我自己总结的飞控与视觉融合的核心逻辑。你把它存下来，以后做系统设计时对照着看：
这张图里，我特意把视觉SLAM画成了“旁路”接入。为什么？因为飞控本身有一套完整的感知-控制闭环，视觉是来“增强”它的，而不是替代它。你想想看，如果视觉挂了，飞控至少还能靠GPS和IMU维持基本飞行——这是工程上的底线思维。