【10天速通ROS2-PX4无人机】(四) 关掉GPS和气压计，纯激光定位还能飞吗

zh路西法2026-07-28 18:55

前言

文章目录

    • 前言
    • [1 背景分析](#1 背景分析)
        • [1-1 当前主流方案](#1-1 当前主流方案)
        • [1-2 GPS、光流、气压计为什么失效](#1-2 GPS、光流、气压计为什么失效)
        • [1-3 PX4 EKF2 能接受什么替代输入](#1-3 PX4 EKF2 能接受什么替代输入)
        • [1-4 为什么不用 200Hz 前推作为输入](#1-4 为什么不用 200Hz 前推作为输入)
        • [1-5 为什么 FAST-LIO2 的 Z 和 Vz 不可靠](#1-5 为什么 FAST-LIO2 的 Z 和 Vz 不可靠)
    • [2 PX4 EKF2 定位原理](#2 PX4 EKF2 定位原理)
        • [2-1 EKF2 是什么](#2-1 EKF2 是什么)
        • [2-2 融合调度：controlFusionModes](#2-2 融合调度：controlFusionModes)
        • [2-3 实际的 EKF 更新流程：从 innovation 到 fuse](#2-3 实际的 EKF 更新流程：从 innovation 到 fuse)
        • [2-4 EV 通道入口：controlExternalVisionFusion](#2-4 EV 通道入口：controlExternalVisionFusion)
        • [2-5 EV_CTRL 的代码实现](#2-5 EV_CTRL 的代码实现)
        • [2-6 innovation 门控------TOF 跳变时 EKF2 如何处理](#2-6 innovation 门控——TOF 跳变时 EKF2 如何处理)
        • [2-7 高度源优先级](#2-7 高度源优先级)
        • [2-8 EKF2 参数总结](#2-8 EKF2 参数总结)
    • [3 PX4 位置/速度指令的执行链路](#3 PX4 位置/速度指令的执行链路)
        • [3-1 整体架构：串级 P-PID](#3-1 整体架构：串级 P-PID)
        • [3-2 外环：位置 → 速度（P 控制器）](#3-2 外环：位置 → 速度（P 控制器）)
        • [3-3 内环：速度 → 加速度（PID 控制器）](#3-3 内环：速度 → 加速度（PID 控制器）)
        • [3-4 加速度 → 推力向量 + 目标姿态](#3-4 加速度 → 推力向量 + 目标姿态)
        • [3-5 下游：姿态 → 角速度 → 电机](#3-5 下游：姿态 → 角速度 → 电机)
        • [3-6 Offboard 到底发了什么？](#3-6 Offboard 到底发了什么？)
        • [3-7 参数与实际调参](#3-7 参数与实际调参)
    • [4 无 GPS、无气压计、光流失效方案](#4 无 GPS、无气压计、光流失效方案)
        • [4-1 TOF 传感器配置](#4-1 TOF 传感器配置)
        • [4-2 为什么不用 RNG 通道](#4-2 为什么不用 RNG 通道)
        • [4-3 为什么不融合 FAST-LIO2 速度](#4-3 为什么不融合 FAST-LIO2 速度)
        • [4-4 融合节点 odom_to_px4](#4-4 融合节点 odom_to_px4)
    • [5 新的测试环境](#5 新的测试环境)
        • [5-1 cage_with_table 生成](#5-1 cage_with_table 生成)
        • [5-2 项目结构与启动顺序](#5-2 项目结构与启动顺序)
        • 启动脚本
        • [5-3 飞过桌子------TOF 验证](#5-3 飞过桌子——TOF 验证)
    • 总结

1 背景分析

1-1 当前主流方案
  • PX4 默认的定位方案依赖三个传感器各司其职：
    • GPS：提供全局 XY 位置（1-5Hz），绝对的经纬度坐标，室外精度 2-5m
    • 气压计：提供绝对高度，通过大气压力换算，精度 1-2m，受天气和空调影响
    • 光流：朝下摄像头追踪地面纹理移动，提供 XY 速度，室内悬停用
  • EKF2 把这三个传感器的数据和 IMU（200Hz+ 加速度+角速度）融合，输出一个统一的 24 维状态估计
1-2 GPS、光流、气压计为什么失效
  • GPS 在室内和城市峡谷中直接抓瞎------建筑物遮挡卫星信号，定位精度从 2m 退化到 20m 甚至完全无信号。无人机仓库巡检、地下管道检测、室内物流这些场景，GPS 就是废的
  • 光流依赖地面纹理------纯色地板、水泥地面、水面这些弱纹理场景下，特征点一个都检测不到，输出要么全零（飞机以为自己悬停，实际在飘），要么随机噪声（飞机疯狂摇摆试图修正不存在的速度）。
  • 气压计在室内并非完全失效，但精度只有 1-2m，且受空调风、开门关窗引起的气压波动影响。对于需要贴地飞行或穿过狭窄通道的场景，1m 的高度误差足以导致撞墙
  • 说人话就是：室内飞无人机，GPS 没信号（看不见天），光流看地板（纯色 = 瞎），气压计听天由命（风一吹就飘）。三个传感器各有各的死法，没有一个能独当一面
1-3 PX4 EKF2 能接受什么替代输入
  • PX4 的 EKF2 并非只能吃 GPS------它留了多种外部输入的接口：
传感器 ROS2 话题 提供
GPS /fmu/out/vehicle_gps_position（PX4 内部接收） 全局经纬度
外部视觉里程计 /fmu/in/vehicle_visual_odometry 6-DOF 位姿 + 速度
外部动捕里程计 /fmu/in/vehicle_mocap_odometry 6-DOF 位姿 + 速度
激光测距仪 /fmu/in/distance_sensor 单点高度（RNG 通道）
光流 /fmu/in/sensor_optical_flow XY 速度 + 距离
  • EV 是什么 ：EV = External Vision（外部视觉里程计），对应 ROS2 话题 /fmu/in/vehicle_visual_odometry。
    • 名字里虽带"视觉"二字，但在 PX4 的语境下它是一个通用的外部 6-DOF 位姿输入通道------激光里程计、视觉 SLAM、动捕系统都可以往这里灌，PX4 不关心数据来源
  • 替代 GPS 的核心思路：用 vehicle_visual_odometry（EV 通道）灌入激光里程计（FAST-LIO2 的 XY + 姿态），用 distance_sensor（RNG 通道）灌入 TOF 测距仪的高度
  • 关键参数：EKF2_EV_CTRL 控制 EV 通道融合哪些分量（bit0=水平位置, bit1=垂直位置, bit2=速度, bit3=偏航），EKF2_AID_MASK=2 将外部视觉设为主要位置源
1-4 为什么不用 200Hz 前推作为输入
  • 第二期我们实现了 FAST-LIO2 的 200Hz IMU 前推------在两次 LiDAR 矫正（10Hz）之间，用 IMU 做中值积分裸推位姿，给 ego_planner 用。这个 200Hz 信号如果直接灌给 PX4 EKF2，会导致严重问题
    • 原因：200Hz 裸推本质是纯 IMU 积分，没有 LiDAR 矫正，位置噪声在每步（5ms）累积 0.5-2mm，连推 19 步就是厘米级噪声
  • 更致命的是：PX4 EKF2 自己也在做 IMU 积分------它内部以 200Hz+ 的频率用 IMU 推位置。如果你再给它灌一个 200Hz 的 FAST-LIO2 裸推位姿，等于 叠了两层 IMU 积分 ------FAST-LIO2 裸推一遍，PX4 又裸推一遍，噪声被硬生生推了两次
    • 正确做法：FAST-LIO2 只喂 10Hz 的 ESKF 矫正后的位姿（/Odometry，已经包含 LiDAR 点云矫正），中间那 19 步 IMU 前推只给 ego_planner 用（它需要高频位姿来初始化 B 样条）
1-5 为什么 FAST-LIO2 的 Z 和 Vz 不可靠
  • FAST-LIO2 的 XY 位置和姿态是 ESKF（迭代误差状态卡尔曼滤波）通过 point-to-plane 残差迭代 估计出来的------每一帧 LiDAR 点云到来时，ESKF 用 IMU 预测一个初始位姿，把每个 LiDAR 点投影到 ikd-Tree 维护的全局地图上，找到该点周围最近的几个地图点拟合一个局部平面，计算点到平面的距离作为残差，反复迭代修正位姿直到收敛。
  • 因为 point-to-plane 残差对水平方向的平移和旋转极其敏感（你在地面上滑动，点云的横向匹配会给出强烈的纠正信号），所以 FAST-LIO2 的 XY 位置和姿态估计非常准，漂移率可以做到 <0.5% 里程
    • Z 轴的情况完全不同：地面是一个大平面，飞机平行于地面平移时，点到平面的残差对 Z 几乎不敏感（你沿着地板滑动，高度读数不变）。ESKF 对 Z 的可观测性极弱，Z 基本靠 IMU 裸推，漂移比 XY 大一个数量级
    • Vz（Z 方向速度）更不可靠------速度估计依赖位置差分或状态估计，既然 Z 位置本身就不可观，Vz 的噪声只会更大。实测中 FAST-LIO2 的 Z 速度差分噪声可以达到 0.2 m/s，这个噪声灌进 EKF2 会直接让飞机上下抖动
  • 说人话就是：FAST-LIO2 告诉你"飞机在往哪飘、飘了多远"很准（XY），但告诉你"飞机离地多高"不太准（Z），告诉你"飞机在往上升还是降"基本是猜（Vz）。XY 位置和姿态可以信，Z 和 Vz 不能信。那速度怎么办？1-4 讲了 200Hz 前推叠了两层 IMU 积分------连 XY 速度都不能信。速度让 PX4 自己的 IMU 推，外部只给位置和姿态。具体做法见 3-3

2 PX4 EKF2 定位原理

2-1 EKF2 是什么
  • EKF2 是 PX4 的第二代扩展卡尔曼滤波器（v1.13 起全面替代初代 EKF），负责融合所有传感器数据，输出一个统一的 24 维状态估计

x = p v q b a b g b m w T \mathbf{x} = \begin{bmatrix} \mathbf{p} & \mathbf{v} & \mathbf{q} & \mathbf{b}_a & \mathbf{b}_g & \mathbf{b}_m & \mathbf{w} \end{bmatrix}^T x=pvqbabgbmwT

  • 其中 p = 位置(3), v = 速度(3), q = 姿态四元数(4), b_a = 加速度计 bias(3), b_g = 陀螺仪 bias(3), b_m = 磁力计 bias(3), w = 风速(2)

  • PX4 是完全开源的（BSD 许可证），EKF2 源码目录：src/modules/ekf2/EKF/。可以直接在浏览器看：

    https://github.com/PX4/PX4-Autopilot/tree/main/src/modules/ekf2/EKF

  • 核心文件清单：

文件 职责
control.cpp 融合调度主循环------每轮决定该调用哪个传感器的融合函数
ev_control.cpp EV 通道入口------检查数据有效性，分发到位置/速度/高度/偏航四个子模块
ev_pos_control.cpp EV 水平位置融合------将 position[0,1] 作为 EKF 观测更新
ev_vel_control.cpp EV 速度融合------将 velocity[0,1,2] 作为 EKF 观测更新
ev_height_control.cpp EV 高度融合------将 position[2] 作为 EKF 观测更新
ev_yaw_control.cpp EV 偏航融合------将 q[4] 解析的 yaw 作为 EKF 观测更新
2-2 融合调度：controlFusionModes
  • 每一轮 EKF 迭代（~200Hz），control.cpp 中的 controlFusionModes() 被调用一次。这个函数依次检查各个传感器是否有新数据，有就调用对应的融合函数
  • 截取核心代码（第 106-148 行）：
cpp 复制代码 
// control.cpp --- controlFusionModes() 传感器融合调度
void Ekf::controlFusionModes(const imuSample &imu_delayed)
{
    // ... 姿态对齐检查 ...

    controlMagFusion();           // 磁力计 → 偏航
    controlOpticalFlowFusion();   // 光流 → XY 速度 (可选)
    controlGpsFusion();           // GPS → XY+Z 位置+速度
    controlAirDataFusion();       // 空速 → 风速 (可选)
    controlHeightFusion();        // 高度融合 (气压计/RNG/EV/GPS 四选一)
    controlGravityFusion();       // 重力方向融合

#if defined(CONFIG_EKF2_EXTERNAL_VISION)
    controlExternalVisionFusion(); // EV → 位置/速度/偏航/高度 (可选)
#endif

    // 零速更新、假位置约束、判断是否进入航位推算...
    controlZeroVelocityUpdate();
    controlFakePosFusion();
    updateDeadReckoningStatus();
}
  • 注意 controlExternalVisionFusion() 被 #if defined(CONFIG_EKF2_EXTERNAL_VISION) 包裹------这是编译期决定的。PX4 默认编译包含了这个 feature，所以 EV 通道开箱即用
  • 说人话就是：每一轮滤波迭代，EKF2 把能用的传感器都问一遍"你有没有新数据？"，有就吃进去做一次观测更新。你关了 GPS、关了气压计，它就会自动多依赖 EV 和 RNG
2-3 实际的 EKF 更新流程：从 innovation 到 fuse
  • 调度循环决定"能用哪个传感器"之后，每个传感器的融合都遵循同一个三步公式。以 EV 水平位置融合为例------ev_pos_control.cpp 中的 updateEvPosFusion()：
cpp 复制代码 
// ev_pos_control.cpp --- updateEvPosFusion() 测量更新
void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, ...)
{
    // Step 1: 计算 innovation (观测值 - 预测值)
    _aid_src_ev_pos.observation[0] = measurement(0) - _state.pos(0);
    _aid_src_ev_pos.observation[1] = measurement(1) - _state.pos(1);
    _aid_src_ev_pos.innovation[0] = _aid_src_ev_pos.observation[0] - _state.pos(0);
    _aid_src_ev_pos.innovation[1] = _aid_src_ev_pos.observation[1] - _state.pos(1);

    // Step 2: 马氏距离门控 (Mahalanobis gate, outlier rejection)
    // innovation² > gate² × variance → 判定为异常值, 丢弃本帧
    const float innov_gate = _params.ev_pos_innov_gate;  // 默认 5.0
    if (sq(_aid_src_ev_pos.innovation[0]) > sq(innov_gate) * _aid_src_ev_pos.innovation_variance[0]
     || sq(_aid_src_ev_pos.innovation[1]) > sq(innov_gate) * _aid_src_ev_pos.innovation_variance[1]) {
        _aid_src_ev_pos.fusion_enabled = false;  // 丢弃这帧
        return;
    }
    _aid_src_ev_pos.fusion_enabled = true;

    // Step 3: 计算 Kalman gain 并更新状态
}
  • 门控的数学依据是 马氏距离（Mahalanobis distance）------考虑了不确定性的"归一化距离"：

ν 2 σ ν 2 > g 2 ⇒ outlier, reject \frac{\nu^2}{\sigma_\nu^2} > g^2 \quad \Rightarrow \quad \text{outlier, reject} σν2ν2>g2⇒outlier, reject

  • 其中 ν = z - Hx 是 innovation（观测值减预测值），σ²_ν 是 innovation variance（预测协方差 + 观测噪声），g 是门控系数（默认 5.0 = 5σ）。同样的 0.25m 高度跳变------传感器噪声设 0.01m 时 0.25m 远超 5×0.01=0.05m 门槛被丢弃；传感器噪声设 0.5m 时 0.25m 在 5×0.5=2.5m 门槛之内正常融合

  • 对于一维观测（如单轴位置），Kalman gain K 的计算在 vel_pos_fusion.cpp 的 fuseVelPosHeight() 中：

cpp 复制代码 
// vel_pos_fusion.cpp --- fuseVelPosHeight()
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
    Vector24f Kfusion;  // Kalman gain vector
    const unsigned state_index = obs_index + 4;  // H = [0...1...0], 1 at state_index

    // K = P * H^T / innovation_variance
    // 因为 H 只有一个 1, P * H^T 就是 P 的第 state_index 列
    for (int row = 0; row < _k_num_states; row++) {
        Kfusion(row) = P(row, state_index) / innov_var;
    }

    // 协方差更新: P = P - K * H * P
    SquareMatrix24f KHP;
    for (unsigned row = 0; row < _k_num_states; row++) {
        for (unsigned column = 0; column < _k_num_states; column++) {
            KHP(row, column) = Kfusion(row) * P(state_index, column);
        }
    }
    P -= KHP;  // 修正协方差矩阵

    fixCovarianceErrors(true);

    // 调用核心 fuse() 更新状态向量
    fuse(Kfusion, innov);
    return true;
}

  • 标准 EKF 的多维观测更新需要矩阵求逆：

    • K = P H^T (H P H^T + R)^{-1}，对一个 24×24 的协方差矩阵求逆计算量是 O(n³)≈14000 次运算。PX4 没有这样做------它用了 序贯融合（sequential fusion） ，把 XY 两个维度的 EV 位置观测拆成两次一维标量更新，两次都调用同一个 fuseVelPosHeight()。每次只需要除一个标量 innov_var，遍历一次 24 维列向量，O(n²)

  • 为什么要拆？200Hz 的滤波频率下，一次矩阵求逆 14000 次运算还能接受，但加上协方差递推（P -= KHP），批量更新的总计算量可以轻松超过 IMU 预测的耗时，拖慢整个滤波循环。

    • 序贯法每次一维，虽然要跑两遍 fuseVelPosHeight()，但每次只做 24 次除法和 24×24 次乘法------两遍加起来计算量连批量求逆的十分之一都不到

  • 唯一的代价：如果两维观测之间有相关性（比如同一个传感器同时给出 x 和 y，且它们的噪声相关），序贯融合的理论最优性略差于批量融合。但 EV 位置 x 和 y 由 FAST-LIO2 独立估计，噪声几乎不相关，拆不拆结果基本一致

  • 说人话就是：K 的计算其实很简单------因为 EKF2 把多维拆成一维，观测矩阵 H 退化成只有一个 1，

    • K = P列 / innov_var。innov_var 越大（传感器噪声大）→ K 越小 → 修正越保守；innov_var 越小（传感器精确）→ K 越大 → 信传感器更多。这和 2-3 最后一段的原理完全吻合

  • 核心的 fuse() 函数 ------ekf_helper.cpp 第 789 行，所有传感器融合最终都调用这同一个函数来更新 24 维状态向量：

cpp 复制代码 
// ekf_helper.cpp --- Ekf::fuse()
void Ekf::fuse(const Vector24f &K, float innovation)
{
    // 每个状态分量 = 原值 - K分量 × innovation
    _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;    // 姿态修正
    _state.quat_nominal.normalize();
    _R_to_earth = Dcmf(_state.quat_nominal);

    _state.vel       -= K.slice<3, 1>(4, 0) * innovation;      // 速度修正
    _state.pos       -= K.slice<3, 1>(7, 0) * innovation;      // 位置修正
    _state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation; // 陀螺 bias 修正
    _state.delta_vel_bias -= K.slice<3, 1>(13, 0) * innovation; // 加速度 bias 修正
    _state.mag_I     -= K.slice<3, 1>(16, 0) * innovation;      // 磁力计修正
    _state.mag_B     -= K.slice<3, 1>(19, 0) * innovation;
    _state.wind_vel  -= K.slice<2, 1>(22, 0) * innovation;      // 风速修正
}
  • K 是一个 24×1 的卡尔曼增益向量------每个元素控制 innovation 对对应状态分量的修正力度。比如 K.slice<3,1>(7,0) 这三个元素决定了"位置 innovation"对位置估计的修正量
  • 方程角度看，这是标准 EKF 测量更新：x = x - K * innovation，其中 innovation = z - h(x_pred)（观测值减去预测观测）
  • 说人话就是：传感器告诉你"飞机其实在(x,y)"，EKF2 自己猜的是(x',y')，innovation = (x-x', y-y') 就是"你猜错的量"。
    • Kalman gain K 决定了"信传感器多一点还是信自己的预测多一点"------它的值由 innovation_variance（传感器噪声 + 预测不确定性）决定。
    • 你把 EKF2_EV_POS_NOISE 设大，K 就变小，EKF2 就更信自己的预测；
    • 设小，K 变大，EKF2 就更信传感器
2-4 EV 通道入口：controlExternalVisionFusion
  • ev_control.cpp（仅 87 行）是 EV 数据的总入口。核心逻辑：
cpp 复制代码 
// ev_control.cpp --- controlExternalVisionFusion()
void Ekf::controlExternalVisionFusion()
{
    _ev_pos_b_est.predict(_dt_ekf_avg);  // 预测当前 EV 位置偏置

    extVisionSample ev_sample;
    if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(
            _time_delayed_us, &ev_sample)) {

        // 质量检查：quality >= ev_quality_minimum 才允许融合
        bool quality_sufficient = (_params.ev_quality_minimum <= 0)
            || (ev_sample.quality >= _params.ev_quality_minimum);

        // 分别控制位置/速度/偏航/高度四个维度的融合
        controlEvYawFusion(ev_sample, ...);    // ← 受 ev_ctrl bit3 控制
        controlEvVelFusion(ev_sample, ...);    // ← 受 ev_ctrl bit2 控制
        controlEvPosFusion(ev_sample, ...);    // ← 受 ev_ctrl bit0 控制
        controlEvHeightFusion(ev_sample, ...); // ← 受 ev_ctrl bit1 控制

        // 超时 2×EV_MAX_INTERVAL 无新数据 → 自动关闭 EV 融合
    } else if (isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {
        stopEvPosFusion(); stopEvVelFusion();
        stopEvYawFusion(); stopEvHgtFusion();
        ECL_WARN("vision data stopped");
    }
}
  • 关键细节：
  • quality 字段：
    * VehicleOdometry.quality 是一个 0-100 的置信度百分比------值越大表示你对这个里程计数据越有信心。PX4 用 EKF2_EV_QUALITY_MINIMUM 参数设一个门槛（默认 0 = 来者不拒），低于门槛的数据帧直接被丢弃。我们在 odom_to_px4 中设 quality=100，确保 EKF2 不会因为质量不够而拒绝 FAST-LIO2 的定位数据
    • EV_MAX_INTERVAL：默认为 200ms。如果超过 400ms 没有新 EV 数据，EKF2 自动关闭 EV 融合，防止用过期数据
    • 四个 controlEv*Fusion() 函数内部各自检查 ev_ctrl 的对应 bit------bit 为 0 的分量直接 return，不做任何融合。这就是为什么 EKF2_EV_CTRL=11 只融位置+偏航不融速度
2-5 EV_CTRL 的代码实现
  • EKF2_EV_CTRL 是一个位掩码参数，在 EKF2 初始化时从参数系统读取，存储在 _params.ev_ctrl 中。各个 fusion 函数通过检查对应的 bit 来决定是否执行
  • 以位置融合为例------ev_pos_control.cpp 的开头：
cpp 复制代码 
// ev_pos_control.cpp --- controlEvPosFusion()
void Ekf::controlEvPosFusion(extVisionSample &ev_sample, ...)
{
    // bit0 控制水平位置融合
    if (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS)) {
        // ... 检查 innovation、更新状态 ...
        _control_status.flags.ev_pos = true;   // 标记: EV 位置融合已激活
    } else {
        stopEvPosFusion();  // bit0 未置位 → 停用位置融合
        return;
    }
}
  • EvCtrl 枚举定义了各位的含义（common.h）：
cpp 复制代码 
enum class EvCtrl : int32_t {
    HPOS = 1,  // bit0: 水平位置
    VPOS = 2,  // bit1: 垂直位置
    VEL  = 4,  // bit2: 速度
    YAW  = 8,  // bit3: 偏航
};
  • 说人话就是：EKF2_EV_CTRL 的每个 bit 就是 4 个独立开关------bit0 管 XY 位置、bit1 管高度、bit2 管速度、bit3 管偏航。关掉哪个，EKF2 就用 IMU 裸推 + 其他传感器兜底
2-6 innovation 门控------TOF 跳变时 EKF2 如何处理
  • 门控公式和代码在 2-3 节已详细给出，这里讲实际表现。纯 TOF 控制下（无气压计兜底），飞过桌子时飞机 会缓慢上升------因为桌面比地面高 25cm，TOF 测量值确实变短了，EKF2 判断这帧数据在门控阈值内（25cm ≈ 0.05m 噪声 × gate 5.0 = 0.25m，刚好擦边），正常融合了
  • 飞机上升恰好证明了 TOF 已成功接入 EKF2 并参与高度控制------这正是我们要验证的目标
  • 门控真正发挥作用是在更大尺度的异常跳变上------比如 TOF 突然读到 0.01m（传感器被遮挡）、或 30m（打到远处的墙）。这些跳变会超过门控阈值，被判定为 outlier 直接丢弃
  • 说人话就是：桌子 25cm 这个量级，EKF2 认为"虽然有点意外但还在误差范围内，我信你"。TOF 说地面变近了 → EKF2 调整高度估计 → 飞机爬升保持离地距离。这是 正常工作状态 ，不是 bug
2-7 高度源优先级
  • PX4 EKF2 对高度源有一套明确的优先级------controlHeightFusion() 中按以下顺序选择：
    • baro_hgt（气压计，参数 EKF2_BARO_CTRL 控制）------默认最高优先
    • ev_hgt（EV 通道，参数 EKF2_EV_CTRL bit1 控制）------我们用的这个
    • gps_hgt（GPS 高度，参数 EKF2_GPS_CTRL 控制）
    • rng_hgt（测距仪，参数 EKF2_RNG_CTRL 控制）------TOF 最应该走的通道，但 uXRCE-DDS 不桥接
  • 关掉气压计（EKF2_BARO_CTRL=0）并开启 EV Z（EKF2_EV_CTRL bit1=1）后，EKF2 自动切到 ev_hgt 作为高度源。这就是我们当前的配置：EV 管的不仅是 XY，还包括 Z
2-8 EKF2 参数总结
  • 本节涉及的 EKF2 参数汇总：
参数 我们的值 含义
EKF2_EV_CTRL 11 bit0=XY位 + bit1=Z位 + bit3=偏航, 不融速
EKF2_EV_POS_NOISE 0.1 (XY), 0.05 (Z) 位置观测噪声, Z 设小因为 TOF 更准
EKF2_EV_VEL_NOISE 1.0 速度噪声极大 → 等价于不融速
EKF2_AID_MASK 2 bit1=1: EV 为主要位置源
EKF2_BARO_CTRL 0 关气压计
SYS_HAS_GPS 0 关 GPS
EKF2_GPS_CTRL 0 不融 GPS

3 PX4 位置/速度指令的执行链路

  • 第二章讲了 EKF2 怎么估计当前位姿------那是"我在哪"。这一章讲另一个核心问题：Offboard 模式下，PX4 收到目标位置或速度指令后，是怎么一步步变成电机转速的？

  • 整个过程是一条 串级 PID 控制链，从位置到姿态到电机，五层递进。每一层都有具体的源码文件和参数可以验证

  • PX4 多旋翼控制的完整调用链：

    Offboard 节点
    ↓ 发布 trajectory_setpoint (uORB)
    mc_pos_control (MulticopterPositionControl.cpp)
    ↓ 位置 P + 速度 PID → 推力向量
    推力向量 → 目标姿态 (旋转矩阵) + 油门
    ↓ 发布 vehicle_attitude_setpoint (uORB)
    mc_att_control (MulticopterAttitudeControl.cpp)
    ↓ 姿态四元数误差 → 目标角速度
    ↓ 发布 vehicle_rates_setpoint (uORB)
    mc_rate_control (MulticopterRateControl.cpp)
    ↓ 角速度 PID → 力矩指令
    ↓ 发布 actuator_motors (uORB)
    Mixer (mixer_multirotor.cpp)
    ↓ 推力+力矩 → 各电机 PWM
    电机 (actuator)

  • 下游的部分（att_control → rate_control → mixer）是 PX4 内部固定链路，不属于本文范围，但本节会用源码展示位置控制如何输出姿态和推力

3-1 整体架构：串级 P-PID
  • PX4 多旋翼位置控制的数学本质是 串级 P-PID------两个串级的控制器，外环只用 P，内环用完整的 PID：

#mermaid-svg-7Mq9XfEEGGaaw0N7{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .error-icon{fill:#552222;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .marker{fill:#333333;stroke:#333333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .marker.cross{stroke:#333333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 p{margin:0;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster-label text{fill:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster-label span{color:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster-label span p{background-color:transparent;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .label text,#mermaid-svg-7Mq9XfEEGGaaw0N7 span{fill:#333;color:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .node rect,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node circle,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node ellipse,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node polygon,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .rough-node .label text,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node .label text,#mermaid-svg-7Mq9XfEEGGaaw0N7 .image-shape .label,#mermaid-svg-7Mq9XfEEGGaaw0N7 .icon-shape .label{text-anchor:middle;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .rough-node .label,#mermaid-svg-7Mq9XfEEGGaaw0N7 .node .label,#mermaid-svg-7Mq9XfEEGGaaw0N7 .image-shape .label,#mermaid-svg-7Mq9XfEEGGaaw0N7 .icon-shape .label{text-align:center;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .node.clickable{cursor:pointer;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .arrowheadPath{fill:#333333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-7Mq9XfEEGGaaw0N7 .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-7Mq9XfEEGGaaw0N7 .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster text{fill:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .cluster span{color:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-7Mq9XfEEGGaaw0N7 rect.text{fill:none;stroke-width:0;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .icon-shape,#mermaid-svg-7Mq9XfEEGGaaw0N7 .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .icon-shape p,#mermaid-svg-7Mq9XfEEGGaaw0N7 .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .icon-shape .label rect,#mermaid-svg-7Mq9XfEEGGaaw0N7 .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-7Mq9XfEEGGaaw0N7 .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-7Mq9XfEEGGaaw0N7 .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-7Mq9XfEEGGaaw0N7 :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 下游固定链路
几何分解
内环: 速度 PID 控制
外环: 位置 P 控制
pos_sp
pos_err
vel_sp_position
若 != NaN 则 += P 环输出
vel_sp
vel_sp
vel_err
acc_sp_velocity
若 != NaN 则 += PID 输出

(前馈加法)
acc_sp
body_z = acc - g
pos (反馈)
vel (反馈)
trajectory_setpoint

position3
⊕
× Kp

(MPC_XY_P / MPC_Z_P)
addIfNotNanVector3f
external velocity
限幅

XY_VEL_MAX / Z_VEL_MAX
⊕
PID 控制器

P·err + I·∫err + D·vel_dot
addIfNotNanVector3f
external acceleration
3D 加速度向量

(NED 坐标系)
倾角限制

MPC_TILTMAX_AIR
推力标量

collective_thrust
目标姿态

rotation matrix
mc_att_control

姿态 → 角速度 (P)
mc_rate_control

角速度 → 力矩 (PID, ~8kHz)
Mixer

力矩 → PWM
Motors

4 个电机
EKF2 位姿估计

  • 关键：外部值和本环输出做加法前馈 --- addIfNotNan 的逻辑是：两者都有效则 setpoint += addition（加法），只有外部值是 NaN 时才把本环输出填进去。EGO-Planner 只发 position（vel/acc=NaN）→ 本环独立工作无前馈
  • PX4 不直接控制位置，而是用位置误差算出速度指令，再用速度误差算出加速度向量，最后把加速度向量分解成"机身该朝哪"（姿态）+ "油门该多大"（推力标量）
  • 核心源码文件：src/modules/mc_pos_control/PositionControl/PositionControl.cpp
  • 关键参数：
参数 含义 默认值
MPC_XY_P 位置环 P 增益（水平） 0.95
MPC_Z_P 位置环 P 增益（垂直） 1.0
MPC_XY_VEL_P 速度环 P 增益（水平） 0.09
MPC_XY_VEL_I 速度环 I 增益（水平） 0.02
MPC_XY_VEL_D 速度环 D 增益（水平） 0.01
MPC_Z_VEL_P 速度环 P 增益（垂直） 0.4
MPC_Z_VEL_I 速度环 I 增益（垂直） 0.05
MPC_Z_VEL_D 速度环 D 增益（垂直） 0.0
3-2 外环：位置 → 速度（P 控制器）
  • 文件：PositionControl.cpp → _positionControl()
  • 这是外环。为什么只用 P 不用 I？因为内环（速度环）已经有 I 来消除静差，外环再加 I 会导致两级积分互相打架（windup）
cpp 复制代码 
// PositionControl.cpp --- _positionControl()
void PositionControl::_positionControl(const float dt)
{
    // Step 1: 位置误差 × P 增益 → 速度指令
    Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
    //                          ↑ 位置误差     ↑ P 增益 (MPC_XY_P / MPC_Z_P)

    // Step 2: 如果外部给了 velocity (trajectory_setpoint.velocity != NaN)
    //          则外部值 += P 环输出 (前馈加法)
    ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);

    // Step 3: 限幅 --- 水平速度不超过 MPC_XY_VEL_MAX, 垂直不超过 MPC_Z_VEL_MAX
    ControlMath::constrainXY(vel_sp_position, _lim_vel_horizontal);
    vel_sp_position(2) = math::constrain(vel_sp_position(2),
                                          -_lim_vel_up, _lim_vel_down);
    _vel_sp = vel_sp_position;
}
  • 说人话：EKF2 说飞机在位置 P，setpoint 说你应该在位置 P*。(P* - P) × MPC_XY_P 就是"你应该以多快的速度飞过去"------距离越远速度越快，越近越慢，到达后速度为 0
3-3 内环：速度 → 加速度（PID 控制器）
  • 文件：PositionControl.cpp → _velocityControl()
  • 这是内环，速度控制的主力。完整的 PID + 积分抗饱和（anti-windup）
cpp 复制代码 
// PositionControl.cpp --- _velocityControl()
void PositionControl::_velocityControl(const float dt)
{
    // Step 1: 速度误差
    Vector3f vel_error = _vel_sp - _vel;

    // Step 2: PID 三项
    Vector3f acc_sp_velocity =
        vel_error.emult(_gain_vel_p)           // P: 比例项
      + _vel_int                                // I: 积分项 (累积的)
      - _vel_dot.emult(_gain_vel_d);           // D: 微分项 (用加速度估计，非差分)

    // Step 3: 如果外部给了 acceleration (trajectory_setpoint.acceleration != NaN)
    //         则外部值 += PID 输出 (前馈加法)
    ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);

    // Step 4: 积分更新 (带 anti-windup)
    //   Z 轴: 如果推力饱和且误差同号 → 停止积分 (防止 windup)
    //   XY 轴: ARW gain = 2 / P_gain, 在加速度饱和时弱化积分
    _vel_int += vel_error.emult(_gain_vel_i) * dt;

    _acc_sp = acc_sp_velocity;
}
  • D 项为什么用 _vel_dot（加速度估计）而不是速度差分？差分放大高频噪声，PX4 用独立的低通滤波器从速度估计中提取加速度，比直接差分平滑得多
  • 积分抗饱和 是关键：你在悬停时突然打杆（大速度指令），P 项瞬间给一个巨大的加速度，可能超过飞机的物理极限（推力饱和）。此时如果 I 项继续累积"我还没达到目标速度"的误差，积分会越来越大------等你松杆时飞机还在狂飞。anti-windup 检测到推力饱和就停止积分
3-4 加速度 → 推力向量 + 目标姿态
  • 文件：PositionControl.cpp → _accelerationControl()
  • 速度环输出的 _acc_sp 是一个三维向量（NED 坐标系），含义是"飞机需要多大的加速度"。但多旋翼只有两个控制量：推力大小 和 推力方向（即机身朝向）。这需要一次几何分解
cpp 复制代码 
// PositionControl.cpp --- _accelerationControl()
void PositionControl::_accelerationControl(const float dt)
{
    // Step 1: 加速度向量 → body_z 轴方向
    // 推力方向必须抵消重力 + 提供水平加速度
    // body_z = normalize(acc_sp + [0, 0, -g])
    Vector3f body_z = _acc_sp - Vector3f(0, 0, CONSTANTS_ONE_G);
    // 限制最大倾角 (MPC_TILTMAX_AIR, 默认 45°)
    // ... tilt limiting ...

    // Step 2: 推力标量
    // 垂直方向的加速度需求 → 油门
    float collective_thrust =
        (-_acc_sp(2) * _hover_thrust / CONSTANTS_ONE_G) + _hover_thrust;
    // 考虑倾角: cos(θ) 越大 → 油门越大 (倾角分走了一部分推力)
    collective_thrust /= cos_tilt;

    // Step 3: 输出推力 + 姿态
    _thr_sp = body_z * collective_thrust;  // 推力向量
    // 推力向量 → 旋转矩阵 (目标姿态) → 发布给 att_control
}
  • 关键物理直觉：如果飞机要往前飞（N 方向），需要的加速度向量是 [a_x, 0, -g]------前向加速度 a_x 靠机身前倾产生，-g 靠油门抵消重力。这个向量归一化后就是机身 Z 轴的目标方向
  • 水平推力留有余量（MPC_THR_XY_MARGIN，默认 0.3）------最大推力的 30% 恒留给垂直方向，保证飞机不会因为水平加速过大而掉高度
3-5 下游：姿态 → 角速度 → 电机

  • 位置控制输出了 vehicle_attitude_setpoint（目标姿态 + 油门），剩下的链路是：

    vehicle_attitude_setpoint
    ↓
    mc_att_control: 四元数误差 → P 控制 → 目标角速度 (vehicle_rates_setpoint)
    ↓
    mc_rate_control: 角速度误差 → PID 控制 → 力矩指令 (actuator_motors)
    ↓
    mixer_multirotor: 推力 + X/Y/Z 力矩 → 4 个电机 PWM (十字/叉型混控矩阵)
    ↓
    电机输出

  • att_control 的姿态误差计算用了四元数乘法：q_error = q^{-1}_current * q_target，提取出 roll/pitch/yaw 三个轴的误差角。只做 P 控制（没有 I/D），因为下游 rate_control 有完整的 PID

  • rate_control 是 PX4 控制栈中频率最高的闭环（~8kHz），也是唯一用了陀螺仪原始数据（200Hz+）作为反馈的控制环。它直接决定了飞机的抗风能力和手感------参数 MC_ROLL_P、MC_ROLLRATE_P/I/D 等

  • mixer 是一个简单的线性变换矩阵------把 [thrust, roll_moment, pitch_moment, yaw_moment] 映射到 [motor1, motor2, motor3, motor4]。对于 Quad X 构型：

    motor1 = thrust - roll - pitch + yaw
    motor2 = thrust + roll + pitch + yaw
    motor3 = thrust + roll - pitch - yaw
    motor4 = thrust - roll + pitch - yaw

3-6 Offboard 到底发了什么？
  • 回到我们的 EGO-Planner 场景------ego_to_px4 桥接节点发布的是 trajectory_setpoint uORB 话题。这个消息里同时包含位置、速度和加速度：
字段 值 含义
position[3] EGO-Planner 轨迹点 (NED) 目标位置
velocity[3] NaN 不控制速度（让 PX4 自己算）
acceleration[3] NaN 不控制加速度
yaw 当前偏航角 保持航向

  • mc_pos_control 收到后：position 非 NaN → 走位置 P-PID 链路 → 算出速度和姿态；velocity 是 NaN → P 控制器自动忽略外部速度，自己根据位置误差算速度指令

  • 如果同时给 position + velocity + acceleration 呢？外部值和本环输出做加法（前馈），不是替代：

    给了 position + velocity + acceleration：

    复制代码 
      position ──→ P 环算 vel_sp_position ──→ addIfNotNan
  velocity ──→ _vel_sp = 外部值 ──────────→ 两者都有效 → vel_sp += vel_sp_position
                                                          (前馈：外部基础 + P 环修正)
                    ↓
  acceleration ──→ _acc_sp = 外部值 ──→ addIfNotNan
  PID 环 ──→ acc_sp_velocity ───────────→ 两者都有效 → acc_sp += acc_sp_velocity
                                                          (前馈：外部基础 + PID 修正)
                    ↓
               推力 + 姿态 (几何分解，始终执行)

  • addIfNotNan 的行为总结：

外部值 本环输出 结果
有效值 有效值 外部值 += 本环输出（前馈加法）
NaN 有效值 外部值 = 本环输出（本环独立算，EGO-Planner 模式）
有效值 NaN 外部值不变
NaN NaN 不变
cpp 复制代码 
// ControlMath.cpp
void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition)
{
    for (int i = 0; i < 3; i++) {
        addIfNotNan(setpoint(i), addition(i));
    }
}

void addIfNotNan(float &setpoint, const float addition)
{
    if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) {
        // 情况1: 外部和本环都给出了有效值 → 加法叠加（前馈！）
        setpoint += addition;
    } else if (!PX4_ISFINITE(setpoint)) {
        // 情况2: 外部没给 (NaN) → 用本环算的结果
        setpoint = addition;
    }
    // 情况3: addition 是 NaN → 什么都不做
}

  • 这意味着：外部给了 velocity 时，P 环结果不是被丢弃，而是 加到外部值上。有了速度前馈，位置 P 环只负责修正误差。加速度同理

  • 我们 EGO-Planner 只发 position（velocity=NaN, acceleration=NaN）→ 走情况2，P 环和速度 PID 各自独立算，没有外部前馈叠加

bash 复制代码 
1. 位置 P 环    →  vel_sp = 外部vel + (pos_sp − pos) × Kp
2. 速度 PID 环  →  acc_sp = 外部acc + (vel_sp − vel) 的 PID
3. 加速度 → 推力+姿态  ← 这不是PID，是纯几何分解，没有反馈
cpp 复制代码 
// _positionControl(): 
//   _vel_sp 初值 = 外部 setpoint.velocity
//   addIfNotNanVector3f(_vel_sp, vel_sp_position)
//   → 外部 vel 有效? vel_sp += vel_sp_position (前馈加法)
//   → 外部 vel 是 NaN? vel_sp = vel_sp_position (P 环独立算)

// _velocityControl():
//   _acc_sp 初值 = 外部 setpoint.acceleration
//   addIfNotNanVector3f(_acc_sp, acc_sp_velocity)
//   → 外部 acc 有效? acc_sp += acc_sp_velocity (前馈加法)
//   → 外部 acc 是 NaN? acc_sp = acc_sp_velocity (PID 独立算)

  • 两个 addIfNotNanVector3f 的行为一模一样：谁给就用谁，没给就自己算。 velocity 和 acceleration 的逻辑没有任何区别

  • 三种常见组合及其行为（关键：P 环需要 position 才能算误差，position=NaN → P 环输出也是 NaN → addIfNotNan 什么都不做）：

给了哪些 P 环输出 addIfNotNan 结果 行为
只给 position 有效 (Kp×误差) vel_sp = P 环结果（外部 vel=NaN） PX4 自己算全部速度------EGO-Planner 模式
只给 velocity NaN（position=NaN，误差无意义） vel_sp = 外部值（P 环输出 NaN 被忽略） 外部速度直接当作 vel_sp，没有 P 环前馈
position + velocity 有效 外部值 += P 环输出（两者都有效→加法！） 外部速度打底 + P 环修正误差
全部给 有效 vel_sp += P 环, acc_sp += PID 环 外部全权打底 + 本环微调
  • 说人话版：addIfNotNan 的名字就是一切------"如果（addition）不是 NaN，就加"。它不是"替代"也不是"跳过"，是标准的 前馈 + 闭环修正 架构。外部规划器给大方向（前馈），PX4 自己跑 P/PID 做小修正（闭环）。EGO-Planner 只给 position 的特殊之处在于：vel 和 acc 都是 NaN → addIfNotNan 落入"外部NaN"分支 → P 和 PID 独立算全部值
3-7 参数与实际调参

  • 位置响应太慢 ：增大 MPC_XY_P（默认 0.95）。位置误差被更大倍数放大为速度指令，飞机到达目标更快。副作用是速度指令变大可能导致急加速再急刹车

  • 悬停漂移 ：增大 MPC_XY_VEL_I（默认 0.02）。积分项累积的力增强，长周期静差减小。副作用是积分过大会导致低频抖动（slow oscillation）

  • 急停抖动 ：增大 MPC_XY_VEL_D（默认 0.01）。D 项提前预判速度变化趋势，抑制震荡。副作用是噪声放大------D 对加速度噪声极其敏感

  • 上升下降太慢 ：增大 MPC_Z_VEL_P（默认 0.4）。Z 轴通常比 XY 更难调------重力方向需要更强的 P 增益，但又不能太大导致撞地反弹

  • 说人话版：P 是飞机的"反应速度"------越大反应越快但容易刹不住；I 是飞机的"记忆力"------越大越不飘但容易低频晃；D 是飞机的"预判力"------越大刹车越稳但噪声敏感

4 无 GPS、无气压计、光流失效方案

4-1 TOF 传感器配置
  • Gazebo Classic 中模拟单线激光 TOF，使用 libgazebo_ros_ray_sensor.so------这是 ROS2 官方 gazebo_ros_pkgs 自带的 ray sensor 插件，支持 type="ray"（CPU 射线），不会卡 Gazebo
  • 在 iris_vlp16/model.sdf 中添加一个 <link>，挂载 ray sensor：
xml 复制代码 
<link name="tof_link">
  <pose>0.25 0 0.02 0 1.5708 0</pose>  <!-- 右臂外侧 25cm, pitch 90° 朝下 -->
  <inertial><mass>0.001</mass><inertia><ixx>1e-9</ixx><iyy>1e-9</iyy><izz>1e-9</izz></inertia></inertial>
  <collision name="c"><geometry><box><size>0.01 0.01 0.01</size></box></geometry></collision>
  <visual name="v"><geometry><box><size>0.01 0.01 0.01</size></box></geometry></visual>
  <sensor name="tof" type="ray">
    <update_rate>20</update_rate>
    <ray>
      <scan>
        <horizontal><samples>5</samples><min_angle>-0.1</min_angle><max_angle>0.1</max_angle></horizontal>
      </scan>
      <range><min>0.05</min><max>30</max></range>
    </ray>
    <plugin name="tof_ros" filename="libgazebo_ros_ray_sensor.so">
      <ros><namespace>/tof</namespace></ros>
      <output_type>sensor_msgs/PointCloud2</output_type>
    </plugin>
  </sensor>
</link>
<joint name="tof_joint" type="fixed">
  <parent>iris::base_link</parent><child>tof_link</child>
</joint>

  • 关键参数解读：

  • <pose>0.25 0 0.02 0 1.5708 0</pose>：传感器装在右臂外侧 25cm，pitch=1.5708（90°）指向地面。

    * 装在外侧是为了避开机身遮挡------如果装在机身正下方（z=-0.04），射线会被起落架挡住；

    * 装在机身上方（z=0.06），射线要穿透机身才能打到地面，实测读数是固定值 0.115m（打在机身上而不是地面上）

  • <samples>5</samples> + <min_angle>-0.1</min_angle><max_angle>0.1</max_angle>：5 条采样射线，±0.1rad（约 ±5.7°）的微小视场。

    * 必须给非零角度，否则 Gazebo 生不出有效射线（width: 0）

    • <min>0.05</min>：最短测距 5cm。安装高度必须 > 5cm，否则地面时无数据
    • <namespace>/tof</namespace>：ROS2 话题输出到 /tof/tof_ros/out，类型 sensor_msgs/PointCloud2

  • 重要警告 ：Gazebo 里 ray sensor 沿 sensor 的 +X 轴发射，不是 +Z。我们通过 pitch=1.5708 将 sensor 朝下，此时 +X 指向地面。所以 PointCloud2 的 x 字段是离地距离，y 和 z 接近 0

  • 如果使用+Z,就会出现如下的爆炸实况:(喜)

  • 验证 TOF 数据：

bash 复制代码 
source ~/postgraduate0/px4_ws/install/setup.bash
ros2 topic echo /tof/tof_ros/out --once | grep -E "width|x:"
4-2 为什么不用 RNG 通道
  • PX4 EKF2 有专门的激光测距仪融合通道 EKF2_RNG_CTRL，通过 /fmu/in/distance_sensor 话题接收。这是理论上最合适的通道------天然处理单点距离，有角度补偿和地面跟踪逻辑
  • 但是，uXRCE-DDS 默认不桥接 distance_sensor 这个 uORB 话题。我们尝试通过 ROS2 发布到 /fmu/in/distance_sensor，PX4 端的订阅数为 0------数据根本进不去
  • 要打通 RNG 通道需要修改 PX4 的 uXRCE-DDS 配置文件，或者在 PX4 源码中注册新的 bridge 主题。这两条路都超出了当前的范围
  • 因此我们选择了替代方案：
    • 让 TOF 搭 FAST-LIO2 的 EV 顺风车 ------把 TOF 的 Z 高度填进 VehicleOdometry 消息的 position[2] 字段，和 FAST-LIO2 的 XY+姿态 共用同一个 EV 话题。这样不增加新通道，复用已有的 vehicle_visual_odometry 桥接
4-3 为什么不融合 FAST-LIO2 速度
  • 1-5 分析了为什么 Z 和 Vz 不可靠------物理原因，point-to-plane 不可观测。但这里要说的是另一个问题：连 Vx 和 Vy 也不应该融
  • XY 位置为什么可靠：ESKF 的 point-to-plane 残差对水平位移极其敏感，LiDAR 点云的横向匹配直接约束了位置。每帧 LiDAR（10Hz）做一次完整的观测更新，位置被反复矫正
  • Vx 和 Vy 为什么不可靠：速度在 ESKF 中 不是直接观测量------LiDAR 点云只约束位置，不约束速度。ESKF 对速度的估计来自 IMU 积分（预测步）+ 卡尔曼滤波对位置变化率的平滑（更新步）。后者本质是"位置差分经过滤波器"，引入了滞后和噪声
  • 实测表现：即使喂 10Hz 的 ESKF 矫正后的速度（/Odometry.twist），EKF2_EV_CTRL=7（融速）时飞机仍然左右晃、上下抖。不是因为频率不够，而是 FAST-LIO2 的速度估计精度远不如它的位置估计精度
  • 为什么 PX4 自己的 IMU 推速度更好：PX4 EKF2 内部以 200Hz+ 对原始 IMU 做紧耦合积分，没有中间加工环节的精度损失。FAST-LIO2 的速度走了"IMU→ESKF 预测→卡尔曼平滑"这一整条链路，噪声和滞后已经被放大了
  • 结论：EKF2_EV_CTRL=11，关 bit2（不融速），velocity[3] 全部 NaN。位置给 FAST-LIO2（它算得准），速度给 PX4 自己的 IMU（更干净）。1-5 解决了"Z 和 Vz 靠不靠谱"，3-3 解决了"就算 Vx Vy 看似该靠谱，也别用"
4-4 融合节点 odom_to_px4
  • 核心桥接节点------同时订阅 FAST-LIO2 的 /Odometry 和 TOF 的 /tof/tof_ros/out，融合后发布 VehicleOdometry 到 /fmu/in/vehicle_visual_odometry
  • 完整代码 px4_ws/src/px4_bridge/src/odom_to_px4.cpp：
cpp 复制代码 
/**
 * FAST-LIO2 XY+姿态 + TOF Z → PX4 VehicleOdometry (NED)
 * EKF2_EV_CTRL=11 (位姿, 不融速)
 */
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <cmath>
#include <cstring>

static constexpr int POSE_FRAME_NED     = 1;
static constexpr int VELOCITY_FRAME_NED = 1;

class OdomToPX4 : public rclcpp::Node
{
public:
    OdomToPX4() : Node("odom_to_px4"), tof_z_(NAN), tof_base_(NAN)
    {
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/Odometry", 10,
            std::bind(&OdomToPX4::odomCallback, this, std::placeholders::_1));

        tof_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/tof/tof_ros/out", 10,
            std::bind(&OdomToPX4::tofCallback, this, std::placeholders::_1));

        vis_odom_pub_ = this->create_publisher<px4_msgs::msg::VehicleOdometry>(
            "/fmu/in/vehicle_visual_odometry", 10);

        RCLCPP_INFO(this->get_logger(),
            "odom_to_px4: FAST-LIO2 XY + TOF Z → EV (CTL=11, no vel)");
    }

private:
    double tof_z_, tof_base_;

    void tofCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        if (msg->width == 0) return;  // 射线没打到东西, 跳过
        for (const auto &f : msg->fields) {
            if (f.name == "x") {       // ray sensor +X=朝下, x=距离
                float x;
                memcpy(&x, &msg->data[f.offset], 4);
                tof_z_ = x;
                if (std::isnan(tof_base_)) {
                    tof_base_ = x;     // 记录地面基线
                    RCLCPP_INFO(this->get_logger(), "TOF baseline: %.3fm", x);
                }
                break;
            }
        }
    }

    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        px4_msgs::msg::VehicleOdometry out;
        out.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        out.timestamp_sample = out.timestamp;

        out.pose_frame = POSE_FRAME_NED;
        out.position[0] = msg->pose.pose.position.y;   // north (FAST-LIO2)
        out.position[1] = msg->pose.pose.position.x;   // east  (FAST-LIO2)
        // Z = TOF高度 - 基线 = 离地高度 (NED: down = -高度)
        out.position[2] = (!std::isnan(tof_z_) && !std::isnan(tof_base_))
            ? -(tof_z_ - tof_base_) : NAN;

        // FAST-LIO2 姿态 → NED 四元数
        auto &q = msg->pose.pose.orientation;
        out.q[0] = q.w; out.q[1] = q.y; out.q[2] = q.x; out.q[3] = -q.z;

        out.velocity_frame = VELOCITY_FRAME_NED;
        out.velocity[0] = NAN; out.velocity[1] = NAN; out.velocity[2] = NAN;

        // 位置方差: XY宽松, Z精确(TOF厘米级)
        out.position_variance[0] = 0.1f;
        out.position_variance[1] = 0.1f;
        out.position_variance[2] = 0.05f;
        // 速度方差极大 → EKF2不信任, 用IMU自己推
        out.velocity_variance[0] = 1.0f;
        out.velocity_variance[1] = 1.0f;
        out.velocity_variance[2] = 1.0f;

        out.reset_counter = 0;
        out.quality = 100;
        vis_odom_pub_->publish(out);

        static int cnt = 0;
        if (++cnt % 10 == 0)
            RCLCPP_INFO(this->get_logger(), "TOF: %.2fm", tof_z_ - tof_base_);
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr tof_sub_;
    rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr vis_odom_pub_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<OdomToPX4>());
    rclcpp::shutdown();
    return 0;
}

  • 设计要点：

    • TOF 初始化时记录地面基线（首次有效读数的 x 值），后续高度 = 当前读数 - 基线
    • XY 位置和姿态来自 FAST-LIO2 10Hz ESKF 矫正后的 /Odometry
    • 速度全部设为 NaN------EKF2 看到 NaN 自动跳过速度融合，用 IMU 积分
    • 位置方差：XY 设 0.1（适度信任），Z 设 0.05（TOF 厘米级，更信任）
    • 速度方差：全部 1.0（极不信任，等价于关掉速度融合）
    • 每秒打印一次 TOF 高度，方便飞行中观察

  • PX4 airframe 关键参数（11015_gazebo-classic_iris_vlp16）：

bash 复制代码 
param set-default SYS_HAS_GPS 0         # 关 GPS
param set-default EKF2_GPS_CTRL 0       # 不融 GPS
param set-default EKF2_EV_CTRL 11       # 融XY+Z(bit0+1)+yaw(bit3), 不融速
param set-default EKF2_BARO_CTRL 0      # 关气压计
param set-default EKF2_AID_MASK 2       # 外部视觉为唯一位置源
  • 完整 airframe 文件（含旋翼布局、电机分配、电池/RC 仿真设置）：
bash 复制代码 
#!/bin/sh
# @name 3DR Iris Quadrotor SITL
# @type Quadrotor Wide

. ${R}etc/init.d/rc.mc_defaults

param set-default CA_AIRFRAME 0
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.1515
param set-default CA_ROTOR0_PY 0.245
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR1_PX -0.1515
param set-default CA_ROTOR1_PY -0.1875
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR2_PX 0.1515
param set-default CA_ROTOR2_PY -0.245
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.1515
param set-default CA_ROTOR3_PY 0.1875
param set-default CA_ROTOR3_KM -0.05

param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

# 仿真环境: 禁用电池故障保护 + 允许无RC进入Offboard
param set-default COM_LOW_BAT_ACT -1
param set-default COM_RCL_EXCEPT 4

# 关 GPS, 用 FAST-LIO2 10Hz ESKF 位姿 + TOF 高度
param set-default SYS_HAS_GPS 0
param set-default EKF2_GPS_CTRL 0
param set-default EKF2_EV_CTRL 11       # 融XY+Z(bit0+1)+yaw(bit3), 不融速
param set-default EKF2_BARO_CTRL 0      # 关气压计
param set-default EKF2_EV_POS_NOISE 0.5
param set-default EKF2_EV_VEL_NOISE 0.3
param set-default EKF2_AID_MASK 2       # EV 为唯一位置源

# Yaw 起飞前不稳定, 允许无稳定航向解锁
param set-default COM_ARM_WO_GPS 1

5 新的测试环境

5-1 cage_with_table 生成
  • 为了验证 TOF 真实测到了地面高度变化，在网笼四角放了 4 张矮桌（高 25cm），用 Python 生成 SDF World 文件
  • 桌子位置选在四角，避开飞机在原点生成的位置，防止起飞时撞到
python 复制代码 
import random; random.seed(42)

with open('cage.world', 'r') as f:
    orig = f.read()

tables = [(3.0, 1.5, 0.0), (-3.0, 1.5, 0.0),
          (3.0, -1.5, 0.0), (-3.0, -1.5, 0.0)]

def table_model(name, x, y, z):
    t = f'<model name="{name}"><static>true</static><pose>{x} {y} {z} 0 0 0</pose>'
    # 桌面 1.2x0.8x0.03, 装在高 0.25m
    t += '<link name="top"><pose>0 0 0.25 0 0 0</pose>'
    t += '<collision name="c"><geometry><box><size>1.2 0.8 0.03</size></box></geometry></collision>'
    t += '<visual name="v"><geometry><box><size>1.2 0.8 0.03</size></box></geometry>'
    t += '<material><ambient>0.6 0.4 0.2 1</ambient></material></visual></link>'
    # 4 条短桌腿
    for lx, ly in [(-0.5,-0.3),(0.5,-0.3),(-0.5,0.3),(0.5,0.3)]:
        t += f'<link name="leg_{lx}_{ly}"><pose>{lx} {ly} 0.12 0 0 0</pose>'
        t += '<collision name="c"><geometry><cylinder><radius>0.03</radius><length>0.24</length></cylinder></geometry></collision>'
        t += '<visual name="v"><geometry><cylinder><radius>0.03</radius><length>0.24</length></cylinder></geometry>'
        t += '<material><ambient>0.4 0.3 0.2 1</ambient></material></visual></link>'
    t += '</model>'
    return t

tables_xml = ''.join(table_model(f'tab{i}', x, y, z) for i, (x,y,z) in enumerate(tables))
new_world = orig.replace('</world>', tables_xml + '</world>')
with open('cage_with_table.world', 'w') as f: f.write(new_world)
  • 在 1_bringup.sh 中通过 PX4_SITL_WORLD 环境变量切换到新场地：
bash 复制代码 
export PX4_SITL_WORLD="$WORKSPACE_DIR/cage_with_table.world"
5-2 项目结构与启动顺序
复制代码 
~/postgraduate0/px4_ws/
├── 1_bringup.sh          # PX4 + Gazebo (cage_with_table.world)
├── 2_keyboard.sh          # 键盘 Offboard 控制
├── 3_fastlio2.sh          # FAST-LIO2 + odom_to_px4 (含 TOF 融合)
├── 4_ego_planner.sh       # EGO-Planner (无 RViz2)
├── 5_rviz.sh              # RViz2 可视化
├── 6_killall.sh           # 一键全杀
├── goal_send.sh           # 发送目标点
├── cage_with_table.world  # 网笼+桌子测试场地
├── src/
│   ├── px4_msgs/
│   ├── FAST_LIO/           # 含 200Hz IMU 前推
│   ├── ego-planner-swarm/  # 只编译 planner 核心包
│   └── px4_bridge/
│       ├── src/
│       │   ├── ego_to_px4.cpp    # ENU→NED 桥接
│       │   └── odom_to_px4.cpp   # FAST-LIO2+TOF → EV 融合
│       └── scripts/
│           └── vis_ego_planner.py
└── ego_planner.rviz
启动脚本
  • 1_bringup.sh --- PX4 + Gazebo 仿真环境（含 cage_with_table.world）：
bash 复制代码 
#!/bin/bash
set -e
cleanup() {
    kill $AGENT_PID 2>/dev/null
    pkill -f "gzserver" 2>/dev/null; pkill -f "gzclient" 2>/dev/null; pkill -f "px4" 2>/dev/null
    pkill -9 -f "gz model" 2>/dev/null || true; pkill -9 -f "gzserver" 2>/dev/null || true
    rm -f /tmp/gazebo-*/client-* /tmp/gazebo-*/server-* 2>/dev/null || true
    rm -f /tmp/px4-sock-* /tmp/.gazebo/lock* 2>/dev/null || true
    exit 0
}
trap cleanup SIGINT SIGTERM
pkill -9 -f "gzserver" 2>/dev/null || true; pkill -9 -f "gzclient" 2>/dev/null || true
pkill -9 -f "gz model" 2>/dev/null || true; pkill -9 -f "px4" 2>/dev/null || true
pkill -9 -f "MicroXRCEAgent" 2>/dev/null || true; pkill -9 -f "sitl_run" 2>/dev/null || true
rm -f /tmp/px4-sock-* /tmp/.gazebo/lock* 2>/dev/null || true; sleep 1
WORKSPACE_DIR="/home/lzh/postgraduate0/px4_ws"; PX4_DIR="$HOME/PX4-Autopilot"
AGENT_BIN="$HOME/microros_agent/bin/MicroXRCEAgent"; AGENT_LIB="$HOME/microros_agent/lib"
export LD_LIBRARY_PATH="$AGENT_LIB:$LD_LIBRARY_PATH"
$AGENT_BIN udp4 -p 8888 & AGENT_PID=$!; sleep 2
export PX4_SITL_WORLD="$WORKSPACE_DIR/cage_with_table.world"
cd $PX4_DIR; make px4_sitl gazebo-classic_iris_vlp16 & PX4_PID=$!; sleep 8
echo "仿真环境已启动！按 Ctrl+C 退出"; wait
  • 2_keyboard.sh --- 键盘 Offboard 控制：
bash 复制代码 
#!/bin/bash
source /opt/ros/humble/setup.bash
source /home/lzh/postgraduate0/px4_ws/install/setup.bash
exec python3 /home/lzh/postgraduate0/px4_ws/2_keyboard_control.py
  • 3_fastlio2.sh --- FAST-LIO2 + odom_to_px4（TOF 融合）：
bash 复制代码 
#!/bin/bash
set -e
cleanup() {
    kill $FASTLIO_PID 2>/dev/null; kill $ODOM_PID 2>/dev/null
    kill $TF1 $TF2 $TF3 $TF4 $TF5 2>/dev/null
    pkill -f "fastlio_mapping" 2>/dev/null || true; pkill -f "odom_to_px4" 2>/dev/null || true
    exit 0
}
trap cleanup SIGINT SIGTERM
source /opt/ros/humble/setup.bash; source /home/lzh/postgraduate0/px4_ws/install/setup.bash
ros2 run tf2_ros static_transform_publisher 0 0 0.12 0 0 0 base_link velodyne_link & TF1=$!
ros2 run tf2_ros static_transform_publisher 0 0 0.05 0 0 0 base_link imu_link & TF2=$!
ros2 run tf2_ros static_transform_publisher 0.15 0 0.08 0 0 0 base_link camera_front_link & TF3=$!
ros2 run tf2_ros static_transform_publisher 0 0 -0.02 0 1.5708 0 base_link camera_down_link & TF4=$!
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 body base_link & TF5=$!
sleep 1
ros2 launch fast_lio mapping_px4.launch.py rviz:=false & FASTLIO_PID=$!; sleep 3
ros2 run px4_bridge odom_to_px4 & ODOM_PID=$!
echo "FAST-LIO2 + odom→PX4 已启动"; wait
  • 4_ego_planner.sh --- EGO-Planner 自主避障：
bash 复制代码 
#!/bin/bash
set -e
cleanup() {
    pkill -f "ego_planner_node" 2>/dev/null || true; pkill -f "traj_server" 2>/dev/null || true
    pkill -f "ego_to_px4" 2>/dev/null || true; exit 0
}
trap cleanup SIGINT SIGTERM
source /opt/ros/humble/setup.bash; source /home/lzh/postgraduate0/px4_ws/install/setup.bash
ros2 launch ego_planner px4_single.launch.py \
    odom_topic:=/Odom_high_freq cloud_topic:=/cloud_registered &
PLANNER_PID=$!; sleep 3
echo "EGO-Planner + Bridge 已启动"; wait $PLANNER_PID 2>/dev/null
  • 5_rviz.sh --- RViz2 可视化：
bash 复制代码 
#!/bin/bash
source /opt/ros/humble/setup.bash
source /home/lzh/postgraduate0/px4_ws/install/setup.bash
rviz2 -d /home/lzh/postgraduate0/px4_ws/ego_planner.rviz
  • 6_killall.sh --- 一键全杀：
bash 复制代码 
#!/bin/bash
pkill -9 -f "gzserver" 2>/dev/null || true; pkill -9 -f "gzclient" 2>/dev/null || true
pkill -9 -f "gz model" 2>/dev/null || true; pkill -9 -f "px4" 2>/dev/null || true
pkill -9 -f "sitl_run" 2>/dev/null || true; pkill -9 -f "MicroXRCEAgent" 2>/dev/null || true
pkill -9 -f "fastlio_mapping" 2>/dev/null || true
pkill -9 -f "static_transform_publisher" 2>/dev/null || true
pkill -9 -f "ego_planner_node" 2>/dev/null || true; pkill -9 -f "traj_server" 2>/dev/null || true
pkill -9 -f "ego_to_px4" 2>/dev/null || true; pkill -9 -f "odom_to_px4" 2>/dev/null || true
pkill -9 -f "vis_ego_planner" 2>/dev/null || true
pkill -9 -f "keyboard_control" 2>/dev/null || true; pkill -9 -f "rviz2" 2>/dev/null || true
rm -rf /tmp/gazebo-*/ 2>/dev/null || true; rm -f /tmp/px4-sock-* 2>/dev/null || true
rm -f /tmp/.gazebo/lock* 2>/dev/null || true; ros2 daemon stop 2>/dev/null || true
echo "已全部清理"
  • 严格启动顺序：./1 → ./3 → ./4 → ./2 解锁起飞
5-3 飞过桌子------TOF 验证
  • 起飞后，odom_to_px4 每秒打印一次 TOF 高度。正常飞行高度 ~1.5m，飞过桌子时 TOF 读数会突然从 ~1.50 跳到 ~1.25（减少了 25cm------桌子高度）
  • 这是正常的，说明我们的 TOF 融合进去了------飞机检测到"地面"突然变近了（桌面比地面高 25cm），PX4 EKF2 认为飞机高度增加了 25cm，会短暂降低油门来补偿
  • 这个现象恰好验证了 TOF 单点测距的特点------它对地面高度变化极其敏感，如果是真实应用场景（地不平、有台阶），这种敏感度会导致飞机跟着地形起伏。解决方法是融合多传感器（气压计兜底 + TOF 精修），以后有机会再加上更多传感器（气压计、深度相机等）做多源融合，进一步平滑地形跳变

总结

  • 本文成功用 FAST-LIO2 + TOF 完全替代了 GPS 和气压计，在仿真中实现了纯 LiDAR+IMU 定位
  • 核心工作回顾：
    • 分析了 GPS、光流、气压计在室内的失效原因
    • 在 Gazebo 中用 libgazebo_ros_ray_sensor.so 模拟了单线激光 TOF
    • 将 TOF 的 Z 高度搭上 FAST-LIO2 的 EV 顺风车，避开了 uXRCE-DDS 不桥接 RNG 通道的限制
    • EKF2_EV_CTRL=11 实现只融位姿、不融速度的方案，避免了 FAST-LIO2 速度噪声导致的飞机抖动
  • 核心踩坑回顾：
    • TOF 装在机身上方（z=0.06）→ 射线打在自己身上，读数固定 0.115m。必须装在外侧悬空位置（x=0.25）
    • min_angle=0, max_angle=0 → Gazebo 生不出射线，width: 0。必须给非零角度
    • uXRCE-DDS 不桥接 distance_sensor → RNG 通道走不通，改走 EV 通道共用
    • EKF2_EV_CTRL=7（融速度）→ 飞机抖动。改为 11（不融速）后稳定
  • 如有错误，欢迎指出！
  • 感谢观看！
相关推荐
yyds_yyd_100862 小时前
1464. 数组中两元素的最大乘积（2026.07.27）
数据结构·c++·算法·leetcode
库克克4 小时前
【C++】set 与multiset
开发语言·c++
Mortalbreeze4 小时前
深入 Linux Socket 编程：端口号、网络字节序与 struct sockaddr 详解
linux·服务器·网络·c++
繁星蓝雨6 小时前
C++设计原理——异常处理
java·c++·异常处理·noexcept·throw·try catch
Kurisu_红莉栖6 小时前
关于相关问题的自我回答
c++
hansang_IR6 小时前
【题解】[AGC020E] Encoding Subsets
c++·算法·dp
我不管我就要叫小猪7 小时前
C/C++----命名空间
c语言·开发语言·c++
无忧.芙桃8 小时前
数据结构之堆
c语言·数据结构·c++·算法·堆
2301_777998348 小时前
Linux线程控制——从线程创建到线程分离（第三部分：线程等待 pthread_join）
linux·运维·服务器·c语言·c++
热门推荐
01GitHub 镜像站点02如何新建文件夹？ 电脑新建文件夹的4种方法032026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片04国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）05AI科技热点日报 | 2026年07月01日06AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路072026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？08微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新09CC-Switch & Claude 基于 Linux 服务器安装使用指南102026 年 AI 大模型 & AI 编程工具实战全总结