深入 ArduPilot 高度控制：从原理到代码实战
在多旋翼、垂直起降飞行器（VTOL）甚至固定翼无人机的飞控系统中，高度控制是决定飞行品质的核心环节。无论是悬停稳定性、自动爬升下降，还是复杂地形下的精准降落，背后都离不开一套高效且鲁棒的高度闭环控制系统。
作为开源飞控领域的标杆项目，ArduPilot的高度控制模块经过十余年迭代，已形成结构清晰、参数丰富、适应性强的设计体系。然而，面对海量参数和交错的代码逻辑，许多开发者在调试过程中常陷入“调参靠猜、问题难定位”的困境。
本文将带你深入 ArduPilot 源码层级，逐层拆解其高度控制的实现机制——不只是告诉你“怎么调”，更要讲清楚“为什么这么设计”。我们将结合核心算法、关键函数与典型问题排查，还原一个真实可理解、可修改、可优化的高度控制器全貌。
一、高度控制的本质：不是直接控高，而是“以速代高”
初学者常误以为高度控制器会“直接输出油门来拉到目标高度”。实际上，现代飞控采用的是串级控制（Cascade Control）架构：外环管“去哪里”，内环管“怎么去”。
在 ArduPilot 中，这一思想体现得尤为彻底：
- 外环（Altitude Loop）：根据当前高度与目标高度之差，计算出一个理想的垂直速度指令（target climb rate）；
- 内环（Speed Loop）：接收这个目标速度，通过 PID 控制器调节油门，使实际爬升速率趋近于目标值。
这就像开车上坡时，你不会猛踩油门试图“一步到位”到山顶，而是先设定一个合理的车速，再由油门自动维持该速度平稳前进。
这种分层设计的好处非常明显：
- 外环负责宏观规划，响应慢但稳定；
- 内环专注动态跟踪，响应快并能抑制扰动；
- 整体系统具备更好的抗干扰能力和过渡过程平滑性。
二、外环解析：如何把“高度误差”变成“目标爬升率”？
外环的核心任务是将空间上的位置偏差转化为时间上的运动指令。在 ArduPilot 中，这部分主要由AC_PosControl类中的update_pos_z()函数完成。
我们来看一段精简后的关键代码，并逐行解读其含义：
// 文件: libraries/AC_AttitudeControl/AC_PosControl.cpp void AC_PosControl::update_pos_z() { float curr_pos_z = pos_target.z; // 当前高度（z轴向下为正） float target_pos_z = _pos_target.z; // 目标高度 float error = target_pos_z - curr_pos_z; // 高度误差 float target_vel_z = error * _kp_z; // P控制：误差 × 增益 target_vel_z = constrain_float(target_vel_z, -_max_speed_down, _max_speed_up); set_velocity_z(target_vel_z); // 下发给内环 }关键点剖析：
- 坐标系约定
ArduPilot 使用 NED 坐标系（北-东-下），因此 z 轴向下为正。这意味着：
- 当前高度越高，pos.z数值越小；
- 上升对应负的垂直速度（velocity.z < 0）；
这一点如果不注意，很容易在读日志或写扩展功能时搞反方向！
纯比例控制为主
外环默认仅使用 P 控制（即_kp_z），不引入积分项。原因很实际：
- 引入 I 项可能导致过度累积，在阶跃响应中引发超调振荡；
- 内环已经具备积分能力，外环再加容易双重积分导致不稳定。增益参数对应关系
_kp_z实际上由参数MOT_THST_ALT_P控制（多旋翼场景）。数值越大，相同高度误差产生的目标爬升率越强，响应越快。速度限幅至关重要
constrain_float(...)将目标速度限制在合理范围内（如 ±5 m/s）。否则一个小的高度跳变就可能命令飞行器“瞬间上升10米”，不仅危险，还会让内环失控。
📌调参建议：
MOT_THST_ALT_P初始可设为 1.0～2.0。若发现爬升缓慢，则适当增大；若出现明显过冲或震荡，则应减小。
三、内环揭秘：PID 如何驱动油门输出？
如果说外环是“大脑”决定要去哪里，那么内环就是“肌肉”执行具体的动力输出。它接收来自外环的目标垂直速度（target_climb_rate），并与 EKF 提供的实际垂直速度比较，进行 PID 运算，最终生成归一化的油门增量。
核心函数位于AC_AttitudeControl::get_throttle_rate_pid()：
float AC_AttitudeControl::get_throttle_rate_pid(float target_rate, float current_rate, float dt) { float error = target_rate - current_rate; // P项：比例反馈 float p_out = _thr_pid.kP() * error; // I项：积分消除稳态误差（带抗饱和） _thr_pid.integrator += _thr_pid.kI() * error * dt; _thr_pid.integrator = constrain_float(_thr_pid.integrator, 0.0f, _thr_pid.imax); float i_out = _thr_pid.integrator; // D项：微分抑制变化率 float deriv = (error - _thr_pid.last_error) / dt; float d_out = _thr_pid.kD() * deriv; _thr_pid.last_error = error; // 总输出 float output = p_out + i_out + d_out; return constrain_float(output, 0.0f, 1.0f); // 归一化至 [0,1] }精细设计亮点：
✅ 正向积分限制（只增不减）
注意到积分项被约束在[0.0, imax]范围内，且初始为 0。这意味着：
- 只有当需要增加油门以维持高度时，积分才会累加；
- 在下降过程中即使存在负误差，也不会积累负积分。
这是一种巧妙的安全机制：防止因长时间低油门运行后突然需要爬升时，积分项仍处于深度负值而导致响应延迟。
✅ 微分项防噪处理
虽然代码未显式加入滤波器，但在实际应用中，ArduPilot 对垂直速度信号进行了低通滤波预处理（通常截止频率 ~5Hz），避免 D 项放大高频噪声。
此外，微分使用的是一阶后向差分法，计算简单、延迟小，适合嵌入式实时系统。
✅ 参数映射清晰
| 参数名 | 功能说明 |
|---|---|
MOT_SPD_P | 控制响应灵敏度。过高易振荡，过低则迟钝 |
MOT_SPD_I | 消除长期漂移，尤其对气压计漂移补偿有效 |
MOT_SPD_D | 抑制加速度突变引起的过冲，提升动态性能 |
MOT_SPD_IMAX | 积分上限，防止积分饱和 |
🛠️调参口诀：先调 P 看响应 → 加 I 消漂移 → 补 D 抑振荡
推荐顺序：P=1.0 → I=0.5 → D=0.3作为起点，逐步微调。
四、传感器融合：没有准确感知，就没有可靠控制
再好的控制器也架不住“瞎指挥”。高度控制的前提是有一个可信的高度与速度估计源。ArduPilot 并非依赖单一传感器，而是通过EKF（扩展卡尔曼滤波）实现多源融合。
主要输入源包括：
| 传感器 | 特点 |
|---|---|
| 气压计 | 连续输出、低成本，但受温度、风速影响大，存在漂移 |
| GPS | 绝对高度（AMSL），更新率低（1~5Hz）、噪声大 |
| 下视测距仪（Lidar/ToF） | 精确 AGL 高度，适用于近地飞行 |
| IMU 积分 | 短期可用，长期发散严重 |
| 视觉/光流 | 辅助速度估计，增强低空稳定性 |
这些数据统一送入 EKF 引擎（如 EKF3），在状态空间模型中联合估计最优的位置、速度、姿态及传感器偏置。
例如，在起飞阶段：
- 初始高度由气压计设定；
- 加速度积分提供短期速度趋势；
- 随着 Lidar 数据接入，EKF 自动提升其权重，修正地面距离；
- 若 GPS 信号良好，后期可用于全局高度校准。
开发者须知：
- 启用 EKF 是必须的：设置
AHRS_EKF_TYPE=2或3，并确保EK3_ENABLE=1； - 日志中查看
POS.Z,VFR_HUD.climb字段，它们才是控制器真正使用的输入； - 定期执行“水平校准”和“气压清零”，特别是在不同海拔地区作业前。
五、实战排错：那些年我们都遇到过的“定高怪病”
理论懂了，代码看了，但真机一飞，问题接踵而至。以下是两个典型故障案例及其解决思路。
❌ 问题一：飞行器持续缓慢下降，仿佛“漏气”
现象描述：
进入定高模式后，飞行器无法完全悬停，以约 0.1m/s 的速度缓慢下沉，即便 PID 参数正常。
排查路径：
- 检查悬停油门是否匹配
查看日志中的MOT_THST_HOVER参数。理想情况下，它应等于实际悬停所需的油门百分比（通常在 45%~65% 之间）。
如果设置偏低（如 30%），控制器会认为“我还很轻”，从而减少推力输出。
🔧解决方案：
手动飞行至稳定悬停，记录THR_Out平均值，将其填入MOT_THST_HOVER。
- 确认气压计是否受热干扰
电机运转产生的热气流会上升并包围机体，导致气压计读数虚低（误判为“正在升高”），进而触发降油门动作。
🔧解决方案：
- 在气压计周围加装隔热棉；
- 启用BARO_WIND_COMP=1，开启动态风压补偿；
- 使用底部测距仪辅助 AGL 修正。
- 电池电压衰减未补偿
随着电量下降，电调在同一油门指令下输出的推力降低。
🔧启用推力电压补偿：
设置BATT_VOLT_SCALING=1，系统将根据电压动态调整油门输出曲线。
❌ 问题二：爬升无力、响应迟缓
现象描述：
下达爬升指令后，飞行器反应迟钝，最大油门也无法快速上升。
排查清单：
| 检查项 | 是否满足 |
|---|---|
MOT_THST_MAX是否为 100？ | 否则限制了最大推力 |
日志中THR_Out是否达到 100%？ | 若未达上限，说明控制未饱和 |
| 螺旋桨安装方向正确？ | 反装会导致推力不足 |
| 电调协议是否支持双向通信（如 DShot150+）？ | 获取转速反馈有助于提升响应 |
| 是否启用了“最小油门保护”？ | MOT_THST_MIN不宜过高（推荐 13~15） |
💡 小技巧：可在 Mission Planner 的“Data”页面实时监控
CTUN.Climb（目标爬升率）、VFR_HUD.climb（实际爬升率）和CTUN.ThROut（油门输出），对比三者差异即可判断瓶颈所在。
六、高级玩法：基于现有架构拓展新功能
一旦掌握底层机制，便可在此基础上实现更智能的行为。
示例1：地形跟随飞行（Terrain Following）
利用下方 Lidar 实时获取离地高度，动态调整目标海拔：
if (rangefinder.has_data()) { float terrain_alt = ins.get_position_z() - rangefinder.distance; float target_agl = 2.0; // 期望离地2米 float new_target_z = terrain_alt + target_agl; pos_control->set_target_z(new_target_z); }只要不断更新_pos_target.z，外环自然会驱动飞行器贴地飞行。
示例2：负载自适应悬停油门
在物流无人机中，载重变化会影响悬停油门需求。可通过学习机制在线更新MOT_THST_HOVER：
if (flightmode == LOITER && abs(vert_speed) < 0.1) { float avg_throttle = get_smoothed_throttle(); g2.hover_throttle.learn(avg_throttle); // 平滑学习新值 }类似逻辑已在 ArduPilot 的HoverLearn模块中部分实现。
七、总结与延伸思考
ArduPilot 的高度控制并非神秘黑盒，而是一个工程权衡的艺术品：
- 外环简洁（P控制）是为了避免叠加不稳定；
- 内环完整（PID）是为了兼顾动态与稳态；
- 传感器融合强大，但也要求硬件布局合理；
- 参数体系开放，但需遵循科学调参流程。
当你下次面对“定高抖动”或“爬升缓慢”等问题时，不要再盲目调MOT_SPD_P。请回到控制链路本身，问自己几个问题：
“目标速度生成合理吗？”
“EKF 输出可信吗？”
“积分有没有饱和？”
“物理极限达到了吗？”
只有这样，才能真正做到从现象看到本质，从调试走向设计。
如果你正在做二次开发，不妨尝试：
- 修改外环为 PI 控制测试效果；
- 添加前馈项（feedforward）提升阶跃响应；
- 结合 GPS 高程数据库实现全自动 AGL 飞行。
欢迎在评论区分享你的调参经验或遇到的奇葩问题，我们一起深挖 ArduPilot 的每一行代码。
