PX4 Navigator 围栏安全与精确降落

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 919 阅读时长 ≈ 3 分钟
深入解析 PX4 Navigator 的安全与验证层，包括 Geofence 围栏检测、GeofenceBreachAvoidance 预测规避、任务合法性检查 MissionFeasibilityChecker，以及精确降落 PrecLand 的多状态机流程。

一、安全与验证层的职责分工

安全层由四个组件构成，各司其职：

组件 职责
Geofence 加载围栏数据，判断某点是否越界（当下）
GeofenceBreachAvoidance 基于运动学预测未来位置，提前规避
MissionFeasibilityChecker 执行前验证任务合法性（不在飞行过程中）
FeasibilityChecker 纯规则检查：命令合法、机型支持、距离合理

二、Geofence — 围栏检测

2.1 围栏类型

围栏数据存储在 dataman 中，支持以下类型：

类型 描述
包含多边形（Inclusion Polygon） 飞行必须在此多边形内
排除多边形（Exclusion Polygon） 飞行必须在此多边形外
包含圆形（Inclusion Circle） 飞行必须在圆形区域内
排除圆形（Exclusion Circle） 飞行必须在圆形区域外
最大水平距离 不超过 Home 点的水平距离（GF_MAX_HOR_DIST）
最大高度 不超过设定海拔（GF_MAX_VER_DIST）

2.2 检测函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool Geofence::checkPointAgainstAllGeofences(double lat, double lon, float alt) {
    // 1. 检查是否超过最大 Home 距离
    if (isCloserThanMaxDistToHome(lat, lon, alt) == false) return false;

    // 2. 检查是否超过最大高度
    if (isBelowMaxAltitude(alt) == false) return false;

    // 3. 遍历所有 dataman 围栏项，检查多边形/圆形
    for (int i = 0; i < num_fence_points; i++) {
        // 射线法判断是否在多边形内/外
        // 圆形则用 Haversine 距离公式
    }
    return true;
}

2.3 围栏源选择（GF_SOURCE 参数）

值 说明
0 优先使用 GPS（vehicle_gps_position），更新频率高
1 使用 EKF2 全局位置（vehicle_global_position），与控制器一致

三、GeofenceBreachAvoidance — 预测规避

3.1 预测逻辑

GF_PREDICT 参数开启后，不仅检测当前位置是否越界，还要预测未来位置：

1
2
预测测试点 = 当前位置 + 刹车距离
刹车距离 = v² / (2 * a_max)  （运动学公式）

对于多旋翼：

1
2
float braking_distance_xy = sq(vel_xy) / (2.f * _param_mpc_acc_hor.get());
float braking_distance_z  = sq(vel_z)  / (2.f * _param_mpc_acc_down_max.get());

对于固定翼：

  • 使用盘旋半径估算预警距离
  • 考虑当前航向角，计算切线方向的越界风险

3.2 规避点计算

当预测点越界时，计算安全 loiter 点：

1
2
3
4
5
6
7
8
9
10
11
struct position_setpoint_s GeofenceBreachAvoidance::getFenceAvoidanceLoiterPoint() {
    // 在围栏边界内侧一定距离处设置 loiter 点
    // 方向：从当前位置到越界点的射线，在边界前 1 m 设安全点
    position_setpoint_s sp;
    sp.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
    sp.lat  = safe_lat;
    sp.lon  = safe_lon;
    sp.alt  = safe_alt;  // 若超高，强制降至 GF_MAX_VER_DIST
    sp.loiter_radius = _param_nav_loiter_rad.get();
    return sp;
}

3.3 触发动作（GF_ACTION 参数）

值 动作
0 无（仅记录日志）
1 发出警告
2 Loiter（悬停在安全点）
3 返航（RTL）
4 就地降落
5 Loiter 并持续等待

四、任务合法性检查

MissionFeasibilityChecker::checkMissionFeasible() 在任务上传后或模式切换前调用，逐项扫描任务：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
flowchart TD
    A["checkMissionFeasible"] --> B["遍历所有 mission_item_s"]
    B --> C["FeasibilityChecker::processNextItem"]
    C --> D{"命令是否合法?"}
    D -->|否| FAIL["返回 false<br/>GCS 提示错误"]
    D -->|是| E{"机型是否支持?"}
    E -->|否| FAIL
    E -->|是| F{"起降要求是否满足?"}
    F -->|否| FAIL
    F -->|是| G["继续下一项"]
    G --> B
    B --> H["checkMissionAgainstGeofence<br/>检查所有航点是否在围栏内"]
    H --> I{"全部合法?"}
    I -->|是| OK["返回 true<br/>任务可以执行"]
    I -->|否| FAIL

关键检查项：

  • 首个有效航点距 Home 是否过远（MIS_DIST_1WP）
  • 固定翼降落进近角度是否合理
  • VTOL 相关命令是否在 VTOL 机型上使用
  • 所有带位置的航点是否在当前围栏内

五、精确降落 PrecLand

精确降落是一个独立的复杂状态机，不是普通 Land 的扩展，需要 landing_target_pose 话题提供目标位置（来自视觉/红外标记系统）：

5.1 状态机

1
2
3
4
5
6
7
8
9
10
stateDiagram-v2
    [*] --> Start
    Start --> HorizontalApproach : 初始化投影
    HorizontalApproach --> DescendAboveTarget : 到达目标上方
    HorizontalApproach --> Search : 目标丢失 > PLD_BTOUT
    DescendAboveTarget --> FinalApproach : 高度 <= PLD_FAPPR_ALT
    FinalApproach --> Done : 落地检测
    Search --> HorizontalApproach : 目标重新发现
    Search --> Fallback : 搜索超时 > PLD_SRCH_TOUT
    Fallback --> Done : 执行普通降落

5.2 各状态说明

状态 动作 完成条件
Start 初始化地图投影，建立参考坐标系 立即进入 HorizontalApproach
HorizontalApproach 水平移动使无人机对准目标上方 XY 误差 < PLD_HACC_RAD
DescendAboveTarget 保持 XY 对准的同时缓慢下降 高度 <= PLD_FAPPR_ALT
FinalApproach 最终精确降落 落地检测（vehicle_land_detected.landed）
Search 目标不可见时，在 PLD_SRCH_ALT 高度盘旋搜索 目标重新出现
Fallback 搜索超时，执行普通降落 落地

5.3 目标可见性处理

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void PrecLand::on_active() {
    // 检测 landing_target_pose 是否更新
    if (_landing_target_pose_sub.updated()) {
        _landing_target_pose_sub.copy(&_landing_target_pose);
        const bool target_visible = _landing_target_pose.is_static ||
                                    (hrt_elapsed_time(&_landing_target_pose.timestamp) < 2_s);

        if (!target_visible) {
            // 目标丢失超过 PLD_BTOUT 阈值
            if (hrt_elapsed_time(&_last_target_visible) > _param_pld_btout.get()) {
                // 切换到 Search 状态
                _state = PrecLandState::Search;
            }
        }
    }
}

5.4 关键参数

参数 默认值 说明
PLD_BTOUT 5 s 目标丢失多久后进入搜索
PLD_HACC_RAD 0.2 m 水平接近阶段的对准精度
PLD_FAPPR_ALT 0.1 m 最终进近阶段开始的高度
PLD_SRCH_ALT 10 m 搜索阶段的盘旋高度
PLD_SRCH_TOUT 10 s 搜索超时阈值
PLD_MAX_SRCH 3 次 最大搜索次数

六、小结

Navigator 安全层的设计原则：

  1. 分离判定与执行：Geofence 只判定是否越界，GeofenceBreachAvoidance 才计算规避方案
  2. 预测优先于响应：GF_PREDICT 让围栏检查从被动响应变为主动预判
  3. 执行前与运行时双重验证：FeasibilityChecker 在任务上传时检查，geofence_breach_check() 在飞行中持续监控
  4. PrecLand 独立自治：精确降落状态机完全独立，不依赖 Mission 框架，可以单独激活

上一篇：Navigator 主循环与模式调度
系列总目录：PX4 v1.16 源码解读总目录
下一章：mc_pos_control 位置控制总览