尧图网站建设 
ArduPilot定点悬停从位置偏差里把风速估出来再压回姿态环的完整链路【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilotGCS 上卫星数从 12 颗掉到 4 颗、glitch 警告刚弹出20 米外悬停的飞机却几乎没有位移——这就是 ArduPilot 定点悬停PosHold 模式要守住的画面。它不靠测风仪而是从位置控制器为顶住风必须持续输出的加速度反推风场再把补偿角度直接压进姿态环。下面按数据实际流动的顺序走一遍EKF 位置融合 → 风速估计 → 姿态补偿。EKF位置融合GPS、IMU、气压计如何投出一组最优坐标悬停主循环定位在 mode_poshold.cpp它以 100Hz 以上频率运行第一步是向 EKF扩展卡尔曼滤波器通俗说就是用概率公式把多个传感器的读数加权成一个最优估计要数pos_control-get_vel_estimate_NED_ms()拿到的是北东地坐标系下的水平速度。这组速度不是 GPS 原始坐标的直接读数而是 GPS、IMU、气压计三路数据在滤波器里加权后的结果所以单看某一路坏了不代表位置估计立刻崩。GPS跳变保护在哪里触发不在模式代码里而在 AHRS 层。AP_AHRS.cpp 检测到坐标跳变后置位GPS_GLITCHING状态位events.cpp 每个调度周期把它拷进ap.gps_glitching并触发告警起飞检查在 AP_Arming_Copter.cpp 中读到该标志会拒绝解锁而 ekf_check.cpp 负责另一层兜底——一旦 EKF failsafe 激活它会回头复核当前模式是否还依赖 GPS依赖就按降级路径处理。位置估计出来后水平速度m/s就是下一环节判断现在该不该更新风速的原材料。风速估计用位置控制器的出力反推风往哪吹PosHold 没有测风仪思路是利用悬停平衡风推机身的力等于位置控制器必须持续施加的水平加速度。所以它盯的不是速度而是pos_control-get_accel_target_NED_mss()——位置环此刻想要的水平加速度这就是对风推的估计。容易忽略的是这段估计有三道门槛全部通过才更新。第一道在子状态层面只有滚转和俯仰两个轴都处于 LOITER 子状态机身真的停住了时update_wind_comp_estimate()才会被调用你手上一有输入它就停更。第二道是时间门槛切入悬停后要等 1500msPOSHOLD_WIND_COMP_START_TIME_MS编译期常量不是参数才开始积累避免起飞余速污染估计。第三道是速度门槛if (pos_control-get_vel_estimate_NED_ms().xy().length() POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS) { return; // 水平速度超过 0.1 m/s 就拒绝更新 }这几行保证只有机身近似静止时位置环的出力才归因于风而不是归因于还没刹住。通过门槛后估计值走一阶低通滤波首次采样直接赋值之后每步按系数TC_WIND_COMP0.0025向新值收敛wind_comp_ne_mss.x (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x TC_WIND_COMP * accel_target_ned_mss.x;在 100Hz 循环率下这对应约 4 秒的时间常数——风是秒级缓慢变化的这个滤波速度刚好滤掉刹停过程的瞬态只留下稳态风推。最后还有一道硬限幅 ⚠️估计值被限制在最大倾角的三分之二POSHOLD_WIND_COMP_LEAN_PCT_MAX 0.6666以内把剩下三分之一留给飞手动盘防止补偿把姿态顶到极限、飞手反而拉不回来。姿态补偿补偿角度叠加进姿态环的时机与顺序补偿量算好之后get_wind_comp_lean_angles_rad()把地系北、东加速度向量按当前偏航角旋到机体系用反正切换算成滚转/俯仰补偿角roll_angle_rad atanf((-wind_comp_ne_mss.x * ahrs.sin_yaw() wind_comp_ne_mss.y * ahrs.cos_yaw()) / GRAVITY_MSS);这两行就是把风向量翻译成机身该朝反方向倾斜多少度俯仰轴是同构的公式。角度拿到手后进入 mode_poshold.cpp 的滚转/俯仰子状态机。这个状态机按飞手输入 → 刹车 → 刹车就绪 → 定点逐级推进飞手松杆且输入量接近零时从 PILOT_OVERRIDE 切到 BRAKEBRAKE 按速度形状计算刹停角受PHLD_BRK_ANGLE限幅速度足够低且超时后进入 BRAKE_READY_TO_LOITER最后两个轴都就绪才切入 LOITER。补偿角叠加的姿态控制指令就发生在这个交界处在 PILOT_OVERRIDE 子状态里最终送给姿态控制器的是飞手倾角加上滚转轴wind_comp_roll_rad的一行加法LOITER 子状态里则由定点控制器直接出角。执行器拿到这个角度后再统一被ATT_LIM_TILT_MAX限幅送入姿态环整个链路闭环。刹车阶段的响应由PHLD_BRK_RATE决定固件还做了保护若该参数低于 4mode_poshold.cpp 会直接把参数改回最小值所以调优时看到改不动不是 bug。可调参数汇总参数名作用默认值调优方向偏大/偏小的影响PHLD_BRK_RATE刹车子状态的最大倾角变化率deg/s同时决定刹停增益与超时估计8偏大顶风刹停更快过大则刹车过程易振荡偏小刹停拖沓、停点过冲PHLD_BRK_ANGLE刹车阶段允许的最大倾角30°偏大抗风更硬、机身摆幅更明显偏小姿态保守阵风下停不住ATT_LIM_TILT_MAX整机滚转/俯仰总限幅风补偿只能用其 2/335°偏大风补偿空间增大但飞手修正余量同步变小偏小抗风上限被压低如果你正在遇到 PosHold 悬停点持续单向漂移先跑gps status命令确认卫星数 ≥8 且无 glitch 告警再检查PHLD_BRK_RATE当前值——刹车率被调过低是刹停追不上风推最常见的原因。【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 ArduPilot定点悬停：从“位置偏差“里把风速估出来，再压回姿态环的完整链路
ArduPilot定点悬停从位置偏差里把风速估出来再压回姿态环的完整链路【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilotGCS 上卫星数从 12 颗掉到 4 颗、glitch 警告刚弹出20 米外悬停的飞机却几乎没有位移——这就是 ArduPilot 定点悬停PosHold 模式要守住的画面。它不靠测风仪而是从位置控制器为顶住风必须持续输出的加速度反推风场再把补偿角度直接压进姿态环。下面按数据实际流动的顺序走一遍EKF 位置融合 → 风速估计 → 姿态补偿。EKF位置融合GPS、IMU、气压计如何投出一组最优坐标悬停主循环定位在 mode_poshold.cpp它以 100Hz 以上频率运行第一步是向 EKF扩展卡尔曼滤波器通俗说就是用概率公式把多个传感器的读数加权成一个最优估计要数pos_control-get_vel_estimate_NED_ms()拿到的是北东地坐标系下的水平速度。这组速度不是 GPS 原始坐标的直接读数而是 GPS、IMU、气压计三路数据在滤波器里加权后的结果所以单看某一路坏了不代表位置估计立刻崩。GPS跳变保护在哪里触发不在模式代码里而在 AHRS 层。AP_AHRS.cpp 检测到坐标跳变后置位GPS_GLITCHING状态位events.cpp 每个调度周期把它拷进ap.gps_glitching并触发告警起飞检查在 AP_Arming_Copter.cpp 中读到该标志会拒绝解锁而 ekf_check.cpp 负责另一层兜底——一旦 EKF failsafe 激活它会回头复核当前模式是否还依赖 GPS依赖就按降级路径处理。位置估计出来后水平速度m/s就是下一环节判断现在该不该更新风速的原材料。风速估计用位置控制器的出力反推风往哪吹PosHold 没有测风仪思路是利用悬停平衡风推机身的力等于位置控制器必须持续施加的水平加速度。所以它盯的不是速度而是pos_control-get_accel_target_NED_mss()——位置环此刻想要的水平加速度这就是对风推的估计。容易忽略的是这段估计有三道门槛全部通过才更新。第一道在子状态层面只有滚转和俯仰两个轴都处于 LOITER 子状态机身真的停住了时update_wind_comp_estimate()才会被调用你手上一有输入它就停更。第二道是时间门槛切入悬停后要等 1500msPOSHOLD_WIND_COMP_START_TIME_MS编译期常量不是参数才开始积累避免起飞余速污染估计。第三道是速度门槛if (pos_control-get_vel_estimate_NED_ms().xy().length() POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS) { return; // 水平速度超过 0.1 m/s 就拒绝更新 }这几行保证只有机身近似静止时位置环的出力才归因于风而不是归因于还没刹住。通过门槛后估计值走一阶低通滤波首次采样直接赋值之后每步按系数TC_WIND_COMP0.0025向新值收敛wind_comp_ne_mss.x (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x TC_WIND_COMP * accel_target_ned_mss.x;在 100Hz 循环率下这对应约 4 秒的时间常数——风是秒级缓慢变化的这个滤波速度刚好滤掉刹停过程的瞬态只留下稳态风推。最后还有一道硬限幅 ⚠️估计值被限制在最大倾角的三分之二POSHOLD_WIND_COMP_LEAN_PCT_MAX 0.6666以内把剩下三分之一留给飞手动盘防止补偿把姿态顶到极限、飞手反而拉不回来。姿态补偿补偿角度叠加进姿态环的时机与顺序补偿量算好之后get_wind_comp_lean_angles_rad()把地系北、东加速度向量按当前偏航角旋到机体系用反正切换算成滚转/俯仰补偿角roll_angle_rad atanf((-wind_comp_ne_mss.x * ahrs.sin_yaw() wind_comp_ne_mss.y * ahrs.cos_yaw()) / GRAVITY_MSS);这两行就是把风向量翻译成机身该朝反方向倾斜多少度俯仰轴是同构的公式。角度拿到手后进入 mode_poshold.cpp 的滚转/俯仰子状态机。这个状态机按飞手输入 → 刹车 → 刹车就绪 → 定点逐级推进飞手松杆且输入量接近零时从 PILOT_OVERRIDE 切到 BRAKEBRAKE 按速度形状计算刹停角受PHLD_BRK_ANGLE限幅速度足够低且超时后进入 BRAKE_READY_TO_LOITER最后两个轴都就绪才切入 LOITER。补偿角叠加的姿态控制指令就发生在这个交界处在 PILOT_OVERRIDE 子状态里最终送给姿态控制器的是飞手倾角加上滚转轴wind_comp_roll_rad的一行加法LOITER 子状态里则由定点控制器直接出角。执行器拿到这个角度后再统一被ATT_LIM_TILT_MAX限幅送入姿态环整个链路闭环。刹车阶段的响应由PHLD_BRK_RATE决定固件还做了保护若该参数低于 4mode_poshold.cpp 会直接把参数改回最小值所以调优时看到改不动不是 bug。可调参数汇总参数名作用默认值调优方向偏大/偏小的影响PHLD_BRK_RATE刹车子状态的最大倾角变化率deg/s同时决定刹停增益与超时估计8偏大顶风刹停更快过大则刹车过程易振荡偏小刹停拖沓、停点过冲PHLD_BRK_ANGLE刹车阶段允许的最大倾角30°偏大抗风更硬、机身摆幅更明显偏小姿态保守阵风下停不住ATT_LIM_TILT_MAX整机滚转/俯仰总限幅风补偿只能用其 2/335°偏大风补偿空间增大但飞手修正余量同步变小偏小抗风上限被压低如果你正在遇到 PosHold 悬停点持续单向漂移先跑gps status命令确认卫星数 ≥8 且无 glitch 告警再检查PHLD_BRK_RATE当前值——刹车率被调过低是刹停追不上风推最常见的原因。【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考