ArduPilot 定点悬停深度解析：8级风中不漂移的3套底层机制

ArduPilot 定点悬停深度解析：8级风中不漂移的3套底层机制

【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot

ArduPilot 悬停模式如何做到强风中仍能把无人机钉在一个位置？本文以源码为线索，拆解动态风场对抗、EKF 定位融合与刹车控制三条链路，帮你掌握 ArduPilot 风力补偿参数与定点悬停调参的实际入口。

悬停控制闭环：一张图看懂数据怎么流动

PosHold（位置保持）模式的控制主循环在 mode_poshold.cpp 中，主频不低于 100Hz。它从三个方向收集输入：EKF 给出的机体位置与速度估计、飞手的姿态杆输入、位置环算出的目标加速度。三者各自经过限幅、平滑后混合成一个滚转/俯仰倾斜角，最终交给内层角速率环驱动电机。

风对抗并不单独成一个"模式"，而是以增量方式叠在悬停指令之上——系统持续估计"位置环为了压住漂移而额外付出的加速度"，并把它当作风速的代理量。定位链路则藏在 AP_AHRS 里，负责把 GPS、IMU、气压计的输出合成一份可信的位姿估计。

风场对抗主链路：从采样到姿态叠加

整条链路按数据流分四步，全部位于 mode_poshold.cpp。

第1步：低速采样，只信准了的数据

进入悬停后先等待 1500ms 延迟（常量POSHOLD_WIND_COMP_START_TIME_MS），并且只有水平速度低于 0.10 m/s 时才允许更新风估计。这一步的意义是：飞机在快速机动时，位置环的加速度输出里混着大量机动分量，此时采样会把"自己制造的漂移"当成风。

第2步：低通滤波提取风向量

取位置环的北向/东向目标加速度，逐通道做一阶低通滤波，得到地平面系下的风向量估计（单位 m/s²）：

wind_comp_ne_mss.x = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x + TC_WIND_COMP * accel_target_ned_mss.x;

这段代码在做什么：把每一帧的目标加速度按 0.25% 的比例混入历史估计，时间常数由TC_WIND_COMP（当前 0.0025f）决定——系数越小，风向量越平滑、越滞后；调大则响应快但会带入抖动。

估计值随后被限幅，防止叠加的倾斜角吃掉飞手的操纵余量：

if (!is_zero(accel_lim_mss) && (wind_comp_ef_len > accel_lim_mss)) { wind_comp_ne_mss *= accel_lim_mss / wind_comp_ef_len; }

上限取最大倾斜角的 2/3（POSHOLD_WIND_COMP_LEAN_PCT_MAX= 0.6666f），确保任何风况下飞手都能用杆量压过补偿量。

第3步：坐标旋转 + 姿态叠加

风向量在地面系（北/东），而倾斜角必须表达在机体系（滚/俯），因此用当前航向的 sin/cos 做一次旋转：

roll_angle_rad = atanf((-wind_comp_ne_mss.x * ahrs.sin_yaw() + wind_comp_ne_mss.y * ahrs.cos_yaw()) / GRAVITY_MSS);

公式在做什么：以"重力与风的合力方向"反解出需要压住的倾斜角，再用航向角把地面坐标转到机体系。算出的wind_comp_roll_rad / pitch_rad直接加在悬停控制器输出上，飞手杆量与刹车输出也都与它同向叠加，四路信号最终汇入姿态指令。

定位与容错：EKF 加权裁决与 GPS 丢星兜底

可以把 EKF 定位融合（扩展卡尔曼滤波，一种对传感器数据做加权预测/修正的估计算法）理解成三方投票：GPS 报位置、IMU 报加速度与角速度、气压计报高度，滤波器根据各自的噪声水平做加权裁决。GPS 短暂失效时，系统退回航位推算（DR）——只靠 IMU 积分外推位置，误差会随时间累积，所以属于"续命"手段而非长期方案。

故障检测逻辑在 ekf_check.cpp：它周期性比对 EKF 报告与 GPS 原始观测量，GPS 出现跳变或位置源失效会被记录并触发相应保护动作；G_FENCE_TYPE与FS_POSITION_CHECK等参数决定检测到坏位置后执行 RTL、降落还是保持悬停，这是 GPS 丢星应对的主要配置面。

实操调参：ArduPilot 风力补偿参数一览

📌 风补偿的三个核心量在当前版本是编译期常量，位于 mode_poshold.cpp 头部；可调参数为刹车相关项。

遇到现象时的排查路径：

1. 悬停漂移、风大时明显被推：先确认 PHLD_BRK_RATE 未被调低（代码里有下限保护，低于 4 会被强制改回），再考虑调小TC_WIND_COMP让风向量更快跟上阵风变化。
2. 风补偿起效后飞机"呼吸"式摆动：说明滤波系数偏大、估计跟着杆量抖动，应调小TC_WIND_COMP，或确认桨/机架无机械松动。
3. GPS 跳变频繁触发保护：检查天线是否远离电机走线，确认FS_POSITION_CHECK的保护动作设置符合预期，而不是简单放宽检测阈值。

无 GPS 场景：光流与地形感知的扩展

GPS 完全不可用（室内、峡谷）时，mode_flowhold.cpp 提供光流悬停：用光流传感器测视差角速度推算水平位移，配合测距仪保持高度，把"GPS 报位置"替换为"光流积分位置"，风对抗逻辑仍然生效，只是位置估计源的精度随高度差与纹理质量下降。地形飞行场景下，terrain.cpp 会把地形高度数据喂给悬停与返航逻辑，避免恒定 AGL 假设在斜坡上出错。

本文覆盖了 PosHold 风对抗的完整数据流（采样门限 → 低通滤波 → 坐标旋转 → 姿态叠加）、EKF 定位裁决与 DR 兜底机制，以及 PHLD_BRK_RATE、TC_WIND_COMP 等关键量的取值位置，可作为后续定点悬停调参与源码走读的起点。

【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot