1. 整体架构ArduCopter 的 ADS-B 避障功能采用分层设计层次文件职责通用避障库基类libraries/AP_Avoidance/AP_Avoidance.cpp/h障碍物管理、威胁评估、MAVLink GCS 通知飞控特定实现派生类ArduCopter/avoidance_adsb.cpp/hCopter 特有的动作执行状态机切换、速度矢量合成、恢复策略专用飞行模式ArduCopter/mode_avoid_adsb.cppAVOID_ADSB 模式模式号19复用了 GUIDED 模式的底层速度控制器ADSB 射频数据源libraries/AP_ADSB/从机载 ADSB 接收器uAvionix/Sagetech 等读取周围有人机2. 避障主循环update10Hz 调用Copter::avoidance_adsb_update() ├─ adsb.update() // 从硬件轮询 ADSB 报文 └─ avoidance_adsb.update() // AP_Avoidance::update() ├─ get_adsb_samples() // 将每个邻居飞机加入/更新 _obstacles[] 列表 ├─ check_for_threats() // 计算每架飞机的最近接近距离与威胁等级 ├─ handle_avoidance_local() // 对最严重威胁执行策略 └─ handle_threat_gcs_notify() // 向地面站广播 COLLISION 消息3. 威胁判定核心几何算法威胁等级分 LOW / HIGH 两级依据「在 time_horizon 秒内最近接近距离」判定3.1 水平最近接近closest_approach_xy计算本车与障碍物在地面投影线上的最近距离delta_vel_ne obstacle_vel_ne - my_vel_ne // 相对速度 line_segment delta_vel_ne * fail_time_horizon // 在预测窗内的运动线段 closest_xy point_to_radial_distance(line_segment, Δpos_ne)若 closest_xy F_DIST_XY默认 100 m→ HIGH否则用 W_TIME 算一次 W_DIST_XY默认 300 m→ LOW。3.2 垂直最近接近closest_approach_z给定 delta_vel_d 与 delta_pos_dcm → m三条件分支得到最小垂直偏差m。3.3 联合判定threat_level min(horizontal_level, vertical_level) 垂直距离 W_DIST_Z → 直接 NONE 否则水平 HIGH → 仅当垂直也 risk 时才 HIGH否则降为 LOW 超过 MAX_OBSTACLE_AGE_MS 5 s 没更新的障碍物 → NONE4. 动作执行handle_avoidance_local → AP_Avoidance_Copter::handle_avoidance进入条件状态升级NONE → LOW/HIGH 立即切换状态降级必须在 AP_AVOIDANCE_STATE_RECOVERY_TIME_MS 2 s 后才能降级低于 F_ALT_MIN参数 F_ALT_MIN)时强制只上报不机动默认对 HIGH 威胁 F_ACTION REPORT可在参数表中设为 6 种行为之一动作实现逻辑MAV_COLLISION_ACTION_RTL切到 RTL 模式MAV_COLLISION_ACTION_HOVER切到 Loiter 模式ASCEND_OR_DESCENDhandle_avoidance_vertical比较 my_loc.alt 与障碍物高度以 get_default_speed_up/down() 生成纯 z 向速度指令不低于 rtl_altitudeMOVE_HORIZONTALLYhandle_avoidance_horizontal用 perpendicular_xy 计算垂直于对方速度的水平逃逸方向缩放至 default_speed_xyMOVE_PERPENDICULARhandle_avoidance_perpendicular3D 版本x/y 用 default_speed_xyz 用 default_speed_up/down并遵守最小高度限制NONE / REPORT仅上报不机动AVOID_ADSB 飞行模式的工作方式ModeAvoidADSB 实质是一段「借用 GUIDED 模式控制环」的薄封装层ModeAvoidADSB::run() { ModeGuided::run(); } ModeAvoidADSB::init() { return ModeGuided::init(ignore_checks); } ModeAvoidADSB::set_velocity(v_neu) { ModeGuided::set_velocity(v_neu); }这样 AVOID_ADSB 与地面站的 GUIDED 指令互斥只认当前模式的输入源却能复用成熟的 NEU 速度→姿态控制链路。5. 特殊保护Copter 特化逻辑在 LAND / THROW / FLIP 模式不动作无人车已落地、抛飞机等场景避障无意义落地状态下的处理 REPORT 的动作会直接 disarm(“ADSBCOLLISIONACTION”)近地保护垂直机动时 alt get_altitude_minimum() rtl_altitude即禁止下降AVOID_ADSB 进入守卫check_flightmode() 只在 failsafe_state_change 瞬间识别到真正 BEGIN 一次模式切换而每一帧持续推进时不再切模式避免反复进/出的抖动6. 恢复策略handle_recovery由 F_RCVRY 控制消除 HIGH 威胁后且 ModeReason AVOIDANCE 未被用户手动改模式RecoveryAction行为REMAIN_IN_AVOID_ADSB (0)保持在 AVOID_ADSB会无限盘旋RESUME_PREVIOUS_FLIGHTMODE (1)恢复避障前的模式prev_control_modeRTL (2, Copter 默认)立即 RTLRESUME_IF_AUTO_ELSE_LOITER (3)仅当之前是 AUTO 才恢复否则 Loiterset_mode_else_try_RTL_else_LAND 是三级回退期望模式 → RTL → LAND保证任何情况下都有降级安全出口。7. MAVLink 互操作支持 MAVLink 来源的障碍物handle_msg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT) 把友机广播也加入障碍物列表ICAO 地址作为 src_id。通过 mavlink_collision_t 向地面站每秒发送 MAVLINK_MSG_ID_COLLISION含 time/altitude/horizontal minimal deltas 动作 威胁等级并在威胁消除后持续通告 5 秒。8. 关键参数一览Copter 默认值参数默认值含义AVOID_ENABLE0总开关F_ACTION (AVOID_F_ACTION)REPORTHIGH 响应行为编码W_ACTION (AVOID_W_ACTION)REPORTLOW 响应行为编码F_RCVRY (AVOID_F_RCVRY)RTL (2)HIGH 消除后恢复策略OBS_MAX (AVOID_OBS_MAX)20最大跟踪障碍物数F_TIME (AVOID_F_TIME)30 sFAIL 预测视界W_TIME (AVOID_W_TIME)30 sWARN 预测视界F_DIST_XY (AVOID_F_DIST_XY)100 mFAIL 水平阈值W_DIST_XY (AVOID_W_DIST_XY)300 mWARN 水平阈值F_DIST_Z / W_DIST_Z100 / 300 m垂向阈值F_ALT_MIN (AVOID_F_ALT_MIN)0 m最低启动避障的 AMSL 高度9. 总结算法全貌Copter 的 ADS-B 避障是一个「感知 → 几何碰撞预测 → 威胁分级 → 行为选择 → NEU 速度指令 → GUIDED 控制执行 → 状态回收」的闭环。它以点运动学最近接近距离而非传感器障碍物作为核心判据并且做了最小高度保护、悬停模式例外、状态滞后2 s 回收冷却、机载MAVLink 混合障碍物等鲁棒性设计确保这类主要针对合作式有人机的宏观避障不会因误触发或控制抖动而危及自身飞行安全。
