前言

在这里插入图片描述

文章目录

1 FAST-LIO2 快速回顾

1-1 核心思路

x ^ k κ + 1 = x ^ k κ + K κ ( z k − h ( x ^ k κ ) − H κ ( x ^ k κ − x ^ k ) ) \hat{x}_{k}^{\kappa+1} = \hat{x}_{k}^{\kappa} + K^{\kappa}\left( z_k - h(\hat{x}_k^\kappa) - H^\kappa(\hat{x}_k^\kappa - \hat{x}_k) \right) x^kκ+1​=x^kκ​+Kκ(zk​−h(x^kκ​)−Hκ(x^kκ​−x^k​))

  • 其中 K 是卡尔曼增益，H 是观测雅可比，z - h(x) 是点到平面的残差
  • 说人话就是：用 IMU 预测位姿 → 把新来的 LiDAR 点投影到地图上 → 算点到最近平面的距离 → 用这个距离反复修正位姿 → 收敛后把点加入地图

FAST-LIO2 的本质：IMU 做先验、LiDAR 点对面做观测、迭代卡尔曼做融合。不像 LOAM 那样先提特征（边缘点/平面点），而是直接在原始点上算残差

1-2 为什么不用特征提取
  • 传统 LOAM（Lidar Odometry and Mapping）需要从每帧点云中提取"边缘点"和"平面点"，这个过程计算量大且对场景敏感——空旷环境找不到足够平面点时，特征退化会导致里程计发散
  • FAST-LIO2 跳过了特征提取，直接用 ikd-Tree 在全局地图中搜索每个 LiDAR 点的最近邻，拟合局部平面，计算点到平面的距离作为残差
  • 说人话就是：LOAM 是"先把点云分类再匹配"，FAST-LIO2 是"直接拿原始点去地图里找最近的邻居"，省了一步、也少了一个退化来源

请添加图片描述

2 编译 FAST-LIO2

2-1 拉取仓库
  • FAST-LIO2 的官方 ROS2 版本直接维护在 hku-mars/FAST_LIO 仓库的 ROS2 分支上（不是单独的 repo），由社区贡献者 Ericsii 提交的 PR 合并而来
  • 注意分支：必须 clone ROS2 分支，默认的 main 分支只支持 ROS1
cd ~/postgraduate0/px4_ws/src
git clone https://github.com/hku-mars/FAST_LIO.git --branch ROS2 --recursive
  • --recursive 确保把子模块 ikd-Tree（增量 KD 树，FAST-LIO2 的地图数据结构核心）一起拉下来
  • clone 完成后目录结构：
src/FAST_LIO/
├── include/ikd-Tree/    # ikd-Tree 子模块（增量 KD 树实现）
├── src/
│   ├── laserMapping.cpp  # 主节点：ESEKF 迭代 + 地图维护
│   ├── preprocess.cpp    # 点云预处理（去畸变/降采样）
│   └── IMU_Processing.hpp # IMU 前向传播与反向补偿
├── config/               # LiDAR 参数配置（avia/mid360/velodyne/ouster）
├── launch/               # ROS2 launch 文件
└── msg/                  # 自定义消息（Pose6D）
2-2 修改源码 — 去掉 livox_ros_driver2 硬依赖

  • 官方代码硬依赖 livox_ros_driver2（览沃激光雷达的 ROS2 驱动包），但我们的仿真用的是 Velodyne VLP-16——它发布的是标准 sensor_msgs/PointCloud2，根本不需要 CustomMsg

  • livox_ros_driver2 不在 apt 源里，需要从 GitHub 单独安装整个 Livox-SDK2 工具链，只为了一组消息定义拉一整套驱动太不划算

  • 所以我们对源码做了条件编译改造：让 livox_ros_driver2 变成可选的——如果 CMake 检测到就编译 Livox 支持，检测不到就用标准 PointCloud2 路径编译

  • 改动的核心逻辑：

CMake 用 find_package(livox_ros_driver2 QUIET) 尝试查找，找到则定义 USE_LIVOX 宏。源码中所有 CustomMsg 相关的 include、函数、订阅器全部用 #ifdef USE_LIVOX ... #endif 包裹。仿真场景下 CMake 找不到 livox_ros_driver2 → 宏不定义 → 代码编译跳过 Livox 路径 → 只走 PointCloud2

  • 涉及的文件和具体改动：
文件改动
CMakeLists.txtfind_package(livox_ros_driver2 QUIET)，找到则 add_definitions(-DUSE_LIVOX) 并追加到依赖列表
package.xml删除 <depend>livox_ros_driver2</depend>
src/preprocess.h#include <CustomMsg>、process(CustomMsg)、avia_handler(CustomMsg) 全部加 #ifdef USE_LIVOX
src/preprocess.cppprocess(CustomMsg) 重载 + avia_handler() 函数体全部加 #ifdef USE_LIVOX
src/laserMapping.cpp#include <CustomMsg>、livox_pcl_cbk() 回调、AVIA 订阅分支、sub_pcl_livox_ 成员变量全部加 #ifdef USE_LIVOX
  • 关键代码片段（CMakeLists.txt）：
find_package(livox_ros_driver2 QUIET)
if(livox_ros_driver2_FOUND)
  add_definitions(-DUSE_LIVOX)
  message(STATUS "livox_ros_driver2 found, enabling Livox support")
else()
  message(STATUS "livox_ros_driver2 NOT found, building without Livox support")
endif()
  • 关键代码片段（laserMapping.cpp 中的订阅逻辑）：
if (p_pre->lidar_type == AVIA)
{
#ifdef USE_LIVOX
    sub_pcl_livox_ = this->create_subscription<livox_ros_driver2::msg::CustomMsg>(
        lid_topic, 20, livox_pcl_cbk);
#else
    RCLCPP_ERROR(this->get_logger(),
        "AVIA selected but livox_ros_driver2 not available!");
    rclcpp::shutdown();
    return;
#endif
}
else
{
    sub_pcl_pc_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        lid_topic, rclcpp::SensorDataQoS(), standard_pcl_cbk);
}

  • 说人话就是：你选 lidar_type: 2（Velodyne）→ 走 standard_pcl_cbk（标准 PointCloud2 回调）→ 不需要 livox_ros_driver2。你选 lidar_type: 1（AVIA）但没装 livox_ros_driver2 → 直接报错退出，不会出现莫名其妙的链接失败

  • 修改完成后编译：

cd ~/postgraduate0/px4_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select fast_lio
  • 编译输出会有 livox_ros_driver2 NOT found, building without Livox support (simulation mode) 提示，这是预期行为
  • 注意：以后如果要接实物 Livox Mid-360 / Avia，只需安装 livox_ros_driver2，重新 colcon build 即可自动启用 Livox 支持，源码不需要再改

3 启动配置

3-1 TF 树设计

  • 这是最容易踩坑的地方。FAST-LIO2 内部有一套自己的坐标系体系，和我们第一期搭建的 PX4 传感器 TF 树必须对齐，否则 RViz2 里全是 “No transform from [xxx] to [yyy]” 的报错

  • FAST-LIO2 的坐标系定义：

    • camera_init：世界坐标系（原点 = FAST-LIO2 初始化的第一帧 LiDAR 位置）
    • body：IMU 机体坐标系（FAST-LIO2 通过 Odometry 话题动态发布 camera_init → body 的 TF）
    • 所有输出话题（/cloud_registered、/Odometry、/path）都在 camera_init 系下

  • PX4 的坐标系定义：

    • base_link：飞机机体坐标系（根节点）
    • velodyne_link、imu_link、camera_front_link、camera_down_link：传感器子坐标系

  • 问题：FAST-LIO2 只有 camera_init → body，PX4 只有 base_link → 各传感器，两条链之间没有连接——RViz2 设 camera_init 为 Fixed Frame 时看不到 PX4 的 TF，设 base_link 时看不到 FAST-LIO2 的数据

  • 解决方案：加一条 body → base_link 的静态 TF（identity）

camera_init              ← FAST-LIO2 世界系 (Fixed Frame)
  └── body                ← IMU 系 (Odometry TF 动态发布)
       └── base_link       ← PX4 机体系 (static identity)
            ├── velodyne_link
            ├── imu_link
            ├── camera_front_link
            └── camera_down_link
  • 说人话就是：在 FAST-LIO2 的 IMU 坐标系（body）和 PX4 的机身坐标系（base_link）之间加一条"它们就是同一个东西"的声明。因为仿真中 IMU 数据和 LiDAR 数据都属于同一架飞机，body = base_link，identity 就够了

请添加图片描述

3-2 启动脚本
  • 第一期我们用 3_rviz.sh 做纯可视化，本期改成 3_fastlio2.sh：在原有 TF + RViz2 的基础上，加上了 FAST-LIO2 节点的启动
#!/bin/bash
# FAST-LIO2 + RViz2: 点云建图 + 可视化
set -e

cleanup() {
    echo ">>> 正在关闭所有 FAST-LIO2 / RViz2 进程..."
    kill $FASTLIO_PID 2>/dev/null
    kill $TF1 $TF2 $TF3 $TF4 $TF5 2>/dev/null
    pkill -f "fastlio_mapping" 2>/dev/null || true
    echo ">>> 已全部关闭"
    exit 0
}
trap cleanup SIGINT SIGTERM

source /opt/ros/humble/setup.bash
source /home/lzh/postgraduate0/px4_ws/install/setup.bash

# ==================== 1. 传感器静态 TF ====================
# base_link → velodyne_link (VLP-16 在顶部 0.12m)
ros2 run tf2_ros static_transform_publisher \
    0 0 0.12 0 0 0 base_link velodyne_link &
TF1=$!

# base_link → imu_link (IMU 在 base_link 上方 0.05m)
ros2 run tf2_ros static_transform_publisher \
    0 0 0.05 0 0 0 base_link imu_link &
TF2=$!

# base_link → camera_front_link
ros2 run tf2_ros static_transform_publisher \
    0.15 0 0.08 0 0 0 base_link camera_front_link &
TF3=$!

# base_link → camera_down_link
ros2 run tf2_ros static_transform_publisher \
    0 0 -0.02 0 1.5708 0 base_link camera_down_link &
TF4=$!

# body(FAST-LIO2 IMU frame) → base_link (PX4 drone frame)
# FAST-LIO2 发布 camera_init → body 的 odometry TF
# 加上这个静态 TF 后, TF 树连通: camera_init → body → base_link → sensors
ros2 run tf2_ros static_transform_publisher \
    0 0 0 0 0 0 body base_link &
TF5=$!

sleep 1

# ==================== 2. 启动 FAST-LIO2 ====================
echo ">>> 启动 FAST-LIO2 (px4_vlp16 配置)..."
ros2 launch fast_lio mapping_px4.launch.py rviz:=false &
FASTLIO_PID=$!
sleep 2

# ==================== 3. 启动 RViz2 ====================
echo ">>> 启动 RViz2..."
rviz2 -d /home/lzh/postgraduate0/px4_ws/px4_sim.rviz

# 清理
kill $FASTLIO_PID 2>/dev/null
kill $TF1 $TF2 $TF3 $TF4 $TF5 2>/dev/null
  • 启动顺序（需要三个终端）：
终端命令职责
终端 1./1_bringup.shMicroXRCEAgent + PX4 SITL + Gazebo（网笼 + iris_vlp16）
终端 2./2_keyboard.sh键盘 Offboard 控制（等 Ready for takeoff! 后空格解锁起飞）
终端 3./3_fastlio2.shFAST-LIO2 建图 + RViz2 可视化
  • 注意：终端 3 一定要等 Gazebo 完全加载（Ready for takeoff! 出现）后再启动，否则 /velodyne/points 和 /imu/data 话题还没开始发布，FAST-LIO2 会一直等待数据
3-3 话题说明
  • FAST-LIO2 启动后，ros2 topic list 可以看到新增的话题：
lzh@lzh:~/postgraduate0/px4_ws$ ros2 topic list
/Laser_map
/Odometry
/clock
/cloud_effected
/cloud_registered
/cloud_registered_body
/fmu/in/obstacle_distance
/fmu/in/offboard_control_mode
/fmu/in/onboard_computer_status
/fmu/in/sensor_optical_flow
/fmu/in/telemetry_status
/fmu/in/trajectory_setpoint
/fmu/in/vehicle_attitude_setpoint
/fmu/in/vehicle_command
/fmu/in/vehicle_mocap_odometry
/fmu/in/vehicle_rates_setpoint
/fmu/in/vehicle_trajectory_bezier
/fmu/in/vehicle_trajectory_waypoint
/fmu/in/vehicle_visual_odometry
/fmu/out/failsafe_flags
/fmu/out/position_setpoint_triplet
/fmu/out/sensor_combined
/fmu/out/timesync_status
/fmu/out/vehicle_attitude
/fmu/out/vehicle_control_mode
/fmu/out/vehicle_global_position
/fmu/out/vehicle_gps_position
/fmu/out/vehicle_local_position
/fmu/out/vehicle_odometry
/fmu/out/vehicle_status
/imu/data
/parameter_events
/path
/performance_metrics
/rosout
/tf
/tf_static
/velodyne/points
  • FAST-LIO2 发布的 6 个核心话题：
话题类型含义
/cloud_registeredsensor_msgs/PointCloud2最核心：配准后的全局点云（世界系 camera_init），随时间累积形成 3D 地图
/cloud_registered_bodysensor_msgs/PointCloud2当前帧点云在 IMU 机体系（body）下的表示
/cloud_effectedsensor_msgs/PointCloud2当前帧中参与 ESKF 更新的有效特征点
/Odometrynav_msgs/Odometry6-DOF 位姿估计（camera_init → body），同时发布对应的 TF
/pathnav_msgs/Path历史轨迹线（camera_init 系）
/Laser_mapsensor_msgs/PointCloud2降采样后的全局地图点云（低频发布，1Hz）
  • PX4 提供的关键输入话题：
话题类型含义
/velodyne/pointssensor_msgs/PointCloud2VLP-16 3D 激光雷达点云（10Hz，velodyne_link 系）
/imu/datasensor_msgs/Imu原始 IMU 数据（200Hz，imu_link 系），来自 gazebo_ros_imu 模型

FAST-LIO2 只需要两个输入：一个 LiDAR 话题、一个 IMU 话题。其他所有 fmu/* 话题（飞控状态、GPS 等）都不需要——这就是纯 LiDAR-惯性里程计的优雅之处

请添加图片描述

3-4 参数说明
  • FAST-LIO2 的 YAML 配置文件控制预处理、ESKF、发布行为等所有参数。以下是我们为 iris_vlp16 仿真定制的 px4_vlp16.yaml：
/**:
    ros__parameters:
        feature_extract_enable: false   # 关闭特征提取（FAST-LIO2 用直接法）
        point_filter_num: 1             # 每隔 N 个点采样一个（1=不降采样）
        max_iteration: 3                # ESKF 最大迭代次数
        filter_size_surf: 0.5           # 点云降采样体素大小 [m]
        filter_size_map: 0.5            # 地图降采样体素大小 [m]
        cube_side_length: 1000.0        # 地图立方体边长 [m]（远大于网笼 9m）

        common:
            lid_topic:  "/velodyne/points"   # 输入的 LiDAR 话题
            imu_topic:  "/imu/data"          # 输入的 IMU 话题
            time_sync_en: false              # 软同步（仿真不需要）
            time_offset_lidar_to_imu: 0.0    # LiDAR-IMU 时间偏移 [s]

        preprocess:
            lidar_type: 2               # 1=Livox, 2=Velodyne, 3=Ouster, 4=通用
            scan_line: 16               # VLP-16 的线数
            scan_rate: 10               # VLP-16 的扫描频率 [Hz]
            timestamp_unit: 2           # 时间戳单位：0=s, 1=ms, 2=us, 3=ns
            blind: 0.5                  # 盲区距离 [m]（<0.5m 的点丢弃）

        mapping:
            acc_cov: 0.1                # 加速度测量噪声协方差
            gyr_cov: 0.1                # 角速度测量噪声协方差
            b_acc_cov: 0.0001           # 加速度计 bias 随机游走协方差
            b_gyr_cov: 0.0001           # 陀螺仪 bias 随机游走协方差
            fov_degree: 360.0           # LiDAR 水平视场角 [°]
            det_range: 100.0            # 有效探测距离 [m]
            extrinsic_est_en: false     # 是否在线估计 LiDAR-IMU 外参
            extrinsic_T: [ 0., 0., 0.12]  # LiDAR → IMU 平移 [x,y,z]（VLP-16 在顶部 12cm）
            extrinsic_R: [ 1., 0., 0.,    # LiDAR → IMU 旋转（单位矩阵=无旋转）
                        0., 1., 0.,
                        0., 0., 1.]

        publish:
            path_en:  true              # 发布飞行轨迹
            scan_publish_en:  true      # 发布配准点云
            dense_publish_en: true      # 发布稠密点云（false 则只发降采样后的）
            scan_bodyframe_pub_en: false # 发布 body 系下的当前帧点云

        pcd_save:
            pcd_save_en: false          # 是否保存 PCD 地图文件
            interval: -1                # -1=所有帧存到一个文件（可能内存溢出）
  • 重点参数解读：
参数默认值含义为什么要改/不改
lidar_type2LiDAR 类型设为 2（Velodyne），走 velodyne_handler，不去碰 Livox 的 CustomMsg
scan_line16扫描线数VLP-16 就是 16 线，用于去畸变时的线束分配
extrinsic_T[0,0,0.12]LiDAR → IMU 外参平移VLP-16 装在飞机顶部 12cm 处（参考第一期模型中的 pose>0 0 0.12</pose>）
blind0.5盲区距离VLP-16 最短有效距离约 0.5m，小于这个距离的点噪声大，直接丢弃
filter_size_surf0.5降采样体素仿真点云稠密（720×16=11520 pts/frame），0.5m 降采样后约几百个点，够用且不卡
acc_cov / gyr_cov0.1IMU 噪声协方差仿真 IMU 噪声很小，设太大滤波会不信任 IMU，太小会过拟合
extrinsic_est_enfalse在线估计外参仿真中外参是精确已知的（我们自己在 SDF 里设的），不需要在线估计

参数调优的核心原则：仿真里噪声小、外参精确 → 协方差可以设小、外参估计关闭。实物里 IMU 有 bias、外参靠手工量 → 协方差要放大、外参估计打开

3-5 RViz2 可视化
  • 在原有的第一期 px4_sim.rviz 基础上，新增了 3 个 FAST-LIO2 专属显示面板：
显示话题颜色说明
FAST-LIO2 Map/cloud_registered黄色配准后的全局点云地图，随时间累积
FAST-LIO2 Odometry/Odometry橙色箭头实时位姿估计（保留 500 帧历史）
FAST-LIO2 Path/path橙红色线飞行轨迹线
  • 原有的 PX4 Velodyne 点云（彩虹色，/velodyne/points）和 PX4 Odometry（绿色，/fmu/out/vehicle_odometry）继续保留，可以直观对比 PX4 EKF 里程计和 FAST-LIO2 里程计的差异
  • Fixed Frame 从 base_link 改为 camera_init——因为 FAST-LIO2 的所有输出都在 camera_init 系下
  • 完整 RViz 配置文件见 px4_sim.rviz（已在第一期文章中给出，本期在原有基础上增加了三条 FAST-LIO2 的 Display）

请添加图片描述

请添加图片描述

请添加图片描述

3-6 常见警告：No point, skip this scan!
  • 在调试算法时，你可能会在终端频繁看到这条黄色警告：
[fastlio_mapping-1] [WARN] [laser_mapping]: No point, skip this scan!
  • 这条警告在 src/laserMapping.cpp 的 timer_callback 中有 两处触发点
3-6-1 IMU 去畸变后没有有效点（第 1069 行）
p_imu->Process(Measures, kf, feats_undistort);
state_point = kf.get_x();
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;

if (feats_undistort->empty() || (feats_undistort == NULL))
{
    RCLCPP_WARN(this->get_logger(), "No point, skip this scan!\n");
    return;
}
  • p_imu->Process() 做两件事：用 IMU 对 LiDAR 点做运动补偿（去畸变），然后用 ESKF 预测位姿
  • 如果去畸变后 feats_undistort 为空，说明这一帧 LiDAR 的所有点在补偿过程中被干掉了。最常见的原因：
    • 盲区过滤太激进：blind 参数设太大（比如设了 2m，飞机起飞时离地太近）
    • IMU buffer 还没就绪：第一帧 LiDAR 到达时 IMU 数据不够做补偿，Process 直接返回了
    • 点云本来就少：LiDAR 射线全部打在盲区内（比如飞机贴着网笼墙壁起飞）
3-6-2 降采样后有效点太少（第 1107 行）
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body);
feats_down_size = feats_down_body->points.size();

if (feats_down_size < 5)
{
    RCLCPP_WARN(this->get_logger(), "No point, skip this scan!\n");
    return;
}

  • 去畸变后有点，但经过体素降采样（filter_size_surf 默认 0.5m）后，同一个体素格子里只保留一个点

  • 如果剩余点数 < 5，ESKF 观测更新直接跳过——5 个点是 ikd-Tree 搜索和平面拟合的最低门槛：

    • ikd-Tree 对每个点搜 NUM_MATCH_POINTS（默认 5）个最近邻来拟合平面
    • 如果整个 scan 的降采样点都不够 5 个，那一个平面都拟合不出来，ESKF 观测更新无意义

  • 说人话就是：飞机离地 0.3m 起飞、LiDAR 盲区 0.5m → 所有点都被盲区过滤 → 没点。或者你开着飞机顶在天花板上——LiDAR 看出去只有天花板那一个平面，降采样后凑不够 5 个点 → 跳过

  • 如何解决：

    • 把 blind 调小（仿真里可以设 0.1m，实物 Mid-360 盲区约 0.1m）
    • 起飞前确保飞机四周有足够的结构（网笼的纵横杆就是很好的点云来源），不要在完全空旷的 Gazebo 空白世界里飞
    • 如果持续报错，检查 /velodyne/points 是否正常发布：ros2 topic hz /velodyne/points
3-7 实机部署 — livox_ros_driver2 的四个坑
  • 如果你在实物上接 Livox Mid-360（览沃固态激光雷达），会直接遇到 livox_ros_driver2。仿真环节我们刻意绕开了它，但实物绕不过去
  • 以下是实机踩坑实录——问题表现为点云延迟 2000-3300ms、FAST-LIO2 完全不发 odom
3-7-1 PTP 时钟同步不完整
  • Livox Mid-360 使用 PTP（Precision Time Protocol，精密时间协议）做时钟同步，master 端（机载电脑）通过 ptp4l 与雷达 slave 端保持纳秒级一致
  • 出厂默认的 ptp4l.service 只有 ptp4l -i eth0 -S -m 基础参数，缺少 -f automotive-master.cfg 配置文件
  • 没有正确配置的结果：交换机上 PTP 报文在走，但数据时钟 未锁定，雷达以自由运行模式发数据——时间戳以 ~31ms/s 的速率持续漂移
  • 说人话就是：雷达和电脑的"手表"对不上，每过一秒差 31ms，几分钟后差好几秒，点云时间戳全部错乱
# /etc/linuxptp/mid360_master.cfg
[global]
# E2E (End-to-End) 延迟测量模式
delay_mechanism        E2E
# 125ms 快速 Sync 间隔 — Mid-360 支持
logSyncInterval        1
# 高时钟质量声明 (Class 6, Clock Accuracy 0x20)
clockClass             6
clockAccuracy          0x20

# /etc/systemd/system/ptp4l.service
ExecStart=/usr/sbin/ptp4l -f /etc/linuxptp/mid360_master.cfg -i eth0 -S -m
  • 修复后 ptp4l 开机自启，雷达时钟锁定，时间偏移从 2.2 秒降到 60ms 以内
3-7-2 CustomMsg vs PointCloud2
  • livox_ros_driver2 的 xfer_format 参数控制数据输出格式：
    • xfer_format = 1：CustomMsg（Livox 私有格式，包含逐点时间戳、tag、line 等丰富信息）
    • xfer_format = 0：PointCloud2（标准 ROS2 点云格式，pcl::PointXYZI 结构）
  • 实测 CustomMsg 延迟 ~200ms、PointCloud2 延迟 ~60ms
  • CustomMsg 消息体更大、序列化/反序列化开销更高，加上驱动内部的队列调度，高帧率下容易堆积——这是点云延迟的主要来源
  • 说人话就是：CustomMsg 信息量虽大但太重，200Hz 的 IMU 还没啥，200Hz 的点云就扛不住了。实物能跑 PointCloud2 就别用 CustomMsg

关键结论：实物 Mid-360 优先用 xfer_format=0（PointCloud2）。CustomMsg 的逐点时间戳在大多数场景下不是刚需——FAST-LIO2 用 IMU 反向补偿去畸变，不依赖 Livox 私有时间戳

3-7-3 FAST-LIO2 配置 lidar_type 的映射陷阱
  • 设 xfer_format=0 后雷达发的是标准 PointCloud2，但 FAST-LIO2 的 YAML 配置里有一个容易踩的坑：
    • lidar_type: 1 → 走 AVIA 分支 → 订阅 CustomMsg → 收不到 PointCloud2 → 永远没数据 → 不发 odom
    • lidar_type: 4 → 走 mid360_handler → 写死了 reflectivity 字段，但驱动 2.0 用的是 intensity 字段 → 点云强度为 0，但还能跑
  • 最可靠的方案：改用 lidar_type: 5（如果代码已扩展），或将其映射到 default_handler——直接用标准 pcl::PointXYZI 解析，不关心 tag/line/reflectivity 等私有字段
  • FAST-LIO2 的本质是直接法——它不依赖点云的线号来提取特征，所以 default_handler 完全够用
# mid360.yaml — 关键参数
common:
    lid_topic:  "/livox/lidar"
    imu_topic:  "/livox/imu"

preprocess:
    lidar_type: 4         # 走 mid360_handler（或 5 走 default_handler）
    scan_line: 4          # Mid-360 4 线非重复扫描
    blind: 0.1            # Mid-360 盲区 0.1m
    point_filter_num: 1

mapping:
    extrinsic_T: [0.0, 0.0, 0.0]   # Mid-360 内置 IMU，外参平移 ~ 0
    extrinsic_R: [1., 0., 0.,
                  0., 1., 0.,
                  0., 0., 1.]
3-7-4 livox_ros_driver2 版本差异

  • 官方 livox_ros_driver2 最新版是 v1.2.6（支持 Jazzy + MID360s + 修复若干 bug）

  • 本地可能残留旧版 v1.2.4，且 FAST-LIO2 的 CMake 和 package.xml 混在 driver 的 workspace 里，耦合度极高——升级 driver 可能破坏 FAST-LIO2 的编译

  • 建议：driver 和 SLAM 算法放在 不同 workspace，用标准 ROS2 ament_cmake 的 find_package 解耦。driver 作为独立的 system 包安装（sudo make install 或 rosdep），FAST-LIO2 只通过消息类型依赖它

  • 修复结果：

指标修复前修复后
点云延迟2200ms 持续漂移~60ms 稳定
Odometry不发20Hz 正常
ptp4l缺配置开机自启，正确配置
数据格式CustomMsgPointCloud2
  • 附：点云延迟检测脚本（保存为 check_lidar_delay.sh，放在实机上跑）：
#!/bin/bash
source /opt/ros/humble/setup.bash
source /root/livox_ros_driver2/install/setup.bash
source /root/drone_ws/install/setup.bash 2>/dev/null
python3 -c "
import rclpy, time
rclpy.init()
n = rclpy.create_node('dc')

topic_type = None
for name, types in n.get_topic_names_and_types():
    if name == '/livox/lidar':
        topic_type = types[0]
        break

if 'CustomMsg' in topic_type:
    from livox_ros_driver2.msg import CustomMsg
    def cb(msg):
        now = time.time()
        s = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
        print(f'  delay={(now-s)*1000:.0f}ms')
    n.create_subscription(CustomMsg, '/livox/lidar', cb, 1)
else:
    from sensor_msgs.msg import PointCloud2
    def cb(msg):
        now = time.time()
        s = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
        print(f'  delay={(now-s)*1000:.0f}ms')
    n.create_subscription(PointCloud2, '/livox/lidar', cb, 1)

print(f'Topic: {topic_type} | Ctrl+C to stop')
rclpy.spin(n)
"
  • 这个脚本自动检测 /livox/lidar 的消息类型（CustomMsg 还是 PointCloud2），然后打印每条点云消息的延迟 delay = now - stamp
  • 正常值应该 < 100ms。如果持续飙升（每过一秒加 ~30ms），说明 PTP 时钟没锁——回到 3-7-1 检查 ptp4l 配置

4 进阶玩法 — 200Hz IMU 前推

4-1 为什么需要高频里程计
  • 默认的 FAST-LIO2 里程计输出频率 = LiDAR 帧率。我们的 VLP-16 是 10Hz，也就是每秒只有 10 个位姿估计
  • 对于纯建图来说这没问题——点云地图不需要高频更新。但如果你要把里程计喂给 PX4 的 EKF2 做状态估计（替代 GPS），10Hz 远远不够
  • PX4 EKF2 的 IMU 更新是 200Hz+ 的，它期望在每条 IMU 测量之后都能拿到一个对应的里程计位姿来做融合。如果里程计只有 10Hz，EKF2 会在两次里程计之间"裸奔"——纯靠 IMU 积分，发散很快
  • 说人话就是：飞机每 0.005 秒（200Hz）问一次"我在哪？"，但 FAST-LIO2 每 0.1 秒（10Hz）才答一次——其他 19 次都只能猜

这个问题的本质是 IMU 和 LiDAR 的采样频率不匹配：IMU 200Hz、LiDAR 10Hz。解决思路也很直观——IMU 前推：在两次 LiDAR scan 之间，拿最新的 ESKF 状态（位置、速度、姿态、bias）做起点，用每条 IMU 消息做一次中值积分预测一个位姿

  • 本算法参考东北大学 REAL_DRONE_400 开源项目的 fastPredictIMU 实现
4-2 核心思路
  • 整个流程分为两段：
IMU 200Hz ──→ fastPredictIMU() ──→ /Odom_high_freq (200Hz)  ← 每条 IMU 都发一次预测位姿
                   ↑ 拿 latest_* 做起点
LiDAR 10Hz ──→ ESKF 迭代 ──→ updateLatestStates() 缓存 P/Q/V/Ba/Bg  ← 矫正 bias，重置起点
  • 低速环路（10Hz LiDAR）：每来一帧点云，跑一次完整的 ESKF 迭代。ESKF 利用 LiDAR 点对面的观测残差，同时修正位置、速度、姿态、加速度 bias 和陀螺仪 bias。更新完毕后，调用 updateLatestStates() 把最新状态存到全局缓存里
  • 高速环路（200Hz IMU）：在两次 LiDAR scan 之间，每收到一条 IMU 消息就调用一次 fastPredictIMU()。这条函数用 中值积分 从缓存的 ESKF 状态向前推一步，预测当前的 PVQ，发布到 /Odom_high_freq
  • 说人话就是：LiDAR 每 0.1 秒来一次"权威矫正"（修正 bias、修正漂移），IMU 在中间每 0.005 秒做一次"轻量预测"（纯积分，不改 bias）
  • 相比于原本的 10Hz 仅靠 LiDAR 帧之间的纯 IMU 裸推（ESKF 内部的前向传播没暴露出来），现在的 200Hz 显式前推把每一次预测都变成了一帧可直接消费的 /Odom_high_freq，PX4 EKF2 每 5ms 就能拿到一个位姿估计，不再有"两次里程计之间裸奔 19 步"的问题
4-3 中值积分推导
  • 从时间 t_k（上一次预测/LiDAR 更新的时刻）到 t_{k+1}（当前 IMU 消息到达时刻），我们缓存了：

P k ∈ R 3 世界系下的位置 V k ∈ R 3 世界系下的速度 R k w b ∈ S O ( 3 ) IMU body 系到 world 系的旋转矩阵 B a k , B g k ∈ R 3 加速度计和陀螺仪的 bias（由 ESKF 估计） a k m , ω k m ∈ R 3 上一帧的 IMU 原始加速度和角速度测量值 \begin{aligned} \mathbf{P}_k &\in \mathbb{R}^3 &\text{世界系下的位置} \\ \mathbf{V}_k &\in \mathbb{R}^3 &\text{世界系下的速度} \\ \mathbf{R}_k^{wb} &\in SO(3) &\text{IMU body 系到 world 系的旋转矩阵} \\ \mathbf{Ba}_k, \mathbf{Bg}_k &\in \mathbb{R}^3 &\text{加速度计和陀螺仪的 bias（由 ESKF 估计）} \\ \mathbf{a}_k^m, \boldsymbol{\omega}_k^m &\in \mathbb{R}^3 &\text{上一帧的 IMU 原始加速度和角速度测量值} \end{aligned} Pk​Vk​Rkwb​Bak​,Bgk​akm​,ωkm​​∈R3∈R3∈SO(3)∈R3∈R3​世界系下的位置世界系下的速度IMU body 系到 world 系的旋转矩阵加速度计和陀螺仪的 bias（由 ESKF 估计）上一帧的 IMU 原始加速度和角速度测量值​

  • 以防你忘记：中值积分和欧拉积分的区别
    • 欧拉积分（前向欧拉）：只用区间起点的斜率来推终点，一阶精度

y k + 1 = y k + f ( t k ,   y k ) ⋅ Δ t y_{k+1} = y_k + f(t_k, \, y_k) \cdot \Delta t yk+1​=yk​+f(tk​,yk​)⋅Δt

* 中值积分：用区间起点和终点斜率的 __平均值__ 来推，二阶精度

y k + 1 = y k + f  ⁣ ( t k + t k + 1 2 ,    y k + y k + 1 pred 2 ) ⋅ Δ t y_{k+1} = y_k + f\!\left(\frac{t_k + t_{k+1}}{2}, \; \frac{y_k + y_{k+1}^{\text{pred}}}{2}\right) \cdot \Delta t yk+1​=yk​+f(2tk​+tk+1​​,2yk​+yk+1pred​​)⋅Δt
* 说人话就是：欧拉积分是一条笔直射线（斜率靠猜），中值积分是一条折线（先猜后取平均）。对于 200Hz 的 IMU，两者差别不大；但飞机快速偏航时角速度变化剧烈，中值法的旋转积分更稳，不会出现欧拉法的"旋转过冲"

  • 步骤 1：把上一帧的加速度测量值转换到世界系（去掉 bias 和重力）：

a k w = R k ⋅ ( a k m − B a k ) + g a_k^w = R_k \cdot (a_k^m - Ba_k) + g akw​=Rk​⋅(akm​−Bak​)+g

  • 其中 g = [g_x, g_y, g_z] 是世界系下的重力向量（由 ESKF 在线估计，初始值通常为 [0, 0, -9.81]）

  • 步骤 2：对当前帧角速度做中值，更新旋转：

ω mid = ω k m + ω k + 1 m 2 − B g k \omega_{\text{mid}} = \frac{\omega_k^m + \omega_{k+1}^m}{2} - Bg_k ωmid​=2ωkm​+ωk+1m​​−Bgk​

R k + 1 = R k ⋅ exp ⁡ ( ω mid ⋅ Δ t ) R_{k+1} = R_k \cdot \exp(\omega_{\text{mid}} \cdot \Delta t) Rk+1​=Rk​⋅exp(ωmid​⋅Δt)

  • 这里的 exp() 是 SO(3) 的指数映射（将角速度向量转为旋转矩阵），等价于 <Exp> 函数

  • 步骤 3：用新旋转把当前帧加速度也转到世界系做中值：

a k + 1 w = R k + 1 ⋅ ( a k + 1 m − B a k ) + g a_{k+1}^w = R_{k+1} \cdot (a_{k+1}^m - Ba_k) + g ak+1w​=Rk+1​⋅(ak+1m​−Bak​)+g

a mid w = a k w + a k + 1 w 2 a_{\text{mid}}^w = \frac{a_k^w + a_{k+1}^w}{2} amidw​=2akw​+ak+1w​​

  • 步骤 4：中值加速度积分，更新位置和速度：

V k + 1 = V k + a mid w ⋅ Δ t V_{k+1} = V_k + a_{\text{mid}}^w \cdot \Delta t Vk+1​=Vk​+amidw​⋅Δt

P k + 1 = P k + V k ⋅ Δ t + 1 2 a mid w ⋅ ( Δ t ) 2 P_{k+1} = P_k + V_k \cdot \Delta t + \frac{1}{2} a_{\text{mid}}^w \cdot (\Delta t)^2 Pk+1​=Pk​+Vk​⋅Δt+21​amidw​⋅(Δt)2

  • 步骤 5：缓存当前测量值供下一帧使用：

a k m ← a k + 1 m , ω k m ← ω k + 1 m a_k^m \leftarrow a_{k+1}^m,\quad \omega_k^m \leftarrow \omega_{k+1}^m akm​←ak+1m​,ωkm​←ωk+1m​

  • 以上就是一次完整的中值积分前推。对比欧拉积分（只用单边值），中值法用起点和终点加速度/角速度的平均值，二阶精度，对 200Hz 采样率的 IMU 来说误差远小于 0.1 秒内 LiDAR 的累积漂移

中值积分的关键：角速度用中值 = 旋转更准，加速度用中值 = 位置/速度更稳。200Hz 下欧拉和中值差别不大，但中值的数值稳定性更好——特别是角速度剧烈变化时（飞机快速偏航），中值法不会出现欧拉法的"旋转过冲"

4-4 代码实现

  • 代码修改全部集中在 src/laserMapping.cpp，分三块：状态缓存 + 前推函数 + 调用点

  • 状态缓存（全局变量，加在 p_pre 前面）：

/*** 200Hz IMU forward propagation: cache latest ESKF state ***/
double latest_time;
V3D latest_P(Zero3d), latest_V(Zero3d), latest_Ba(Zero3d), latest_Bg(Zero3d);
V3D latest_acc_0(Zero3d), latest_gyr_0(Zero3d);  // 上一帧 IMU 测量值
M3D latest_Q(Eye3d);                               // 当前旋转矩阵
bool init = false;                                 // ESKF 是否已初始化（有了第一组 bias）
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomHighFreq_;
  • updateLatestStates() — ESKF 更新后缓存（加在 SigHandle 函数后面）：
// 每次 ESKF 更新后，缓存最新状态
void updateLatestStates()
{
    latest_time = lidar_end_time;
    latest_P = state_point.pos;
    latest_Q = state_point.rot;
    latest_V = state_point.vel;
    latest_Ba = state_point.ba;
    latest_Bg = state_point.bg;
}

  • 对应 4-3 节公式，这里缓存了中值积分需要的全部起点状态：P_k、V_k、R_k、Ba_k、Bg_k

  • fastPredictIMU() — 核心前推函数：

void fastPredictIMU(double t, V3D acc, V3D gyr)
{
    double dt = t - latest_time;
    latest_time = t;
    // 对应公式 (1): a_k^w = R_k * (a_k^m - Ba_k) + g
    V3D un_acc_0 = latest_Q * (latest_acc_0 - latest_Ba)
                 + V3D(state_point.grav[0], state_point.grav[1], state_point.grav[2]);
    // 对应公式 (2): ω_mid = (ω_k^m + ω_{k+1}^m)/2 - Bg_k
    V3D un_gyr = 0.5 * (latest_gyr_0 + gyr) - latest_Bg;
    // 对应公式 (2): R_{k+1} = R_k * exp(ω_mid * Δt)
    latest_Q = latest_Q * Exp(un_gyr, dt);
    // 对应公式 (3): a_{k+1}^w = R_{k+1} * (a_{k+1}^m - Ba_k) + g
    V3D un_acc_1 = latest_Q * (acc - latest_Ba)
                 + V3D(state_point.grav[0], state_point.grav[1], state_point.grav[2]);
    // 对应公式 (3): a_mid^w = (a_k^w + a_{k+1}^w) / 2
    V3D un_acc = 0.5 * (un_acc_0 + un_acc_1);
    // 对应公式 (4): V_{k+1} = V_k + a_mid * Δt
    //              P_{k+1} = P_k + V_k * Δt + 0.5 * a_mid * Δt^2
    latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc;
    latest_V = latest_V + dt * un_acc;
    // 对应公式 (5): 缓存当前测量值
    latest_acc_0 = acc;
    latest_gyr_0 = gyr;

    // 发布 200Hz 里程计
    nav_msgs::msg::Odometry odomHigh;
    Eigen::Quaterniond quadrotor_Q = Eigen::Quaterniond(latest_Q);
    odomHigh.header.stamp = get_ros_time(t);
    odomHigh.header.frame_id = "camera_init";
    odomHigh.child_frame_id = "body";
    odomHigh.pose.pose.position.x = latest_P.x();
    odomHigh.pose.pose.position.y = latest_P.y();
    odomHigh.pose.pose.position.z = latest_P.z();
    odomHigh.pose.pose.orientation.x = quadrotor_Q.x();
    odomHigh.pose.pose.orientation.y = quadrotor_Q.y();
    odomHigh.pose.pose.orientation.z = quadrotor_Q.z();
    odomHigh.pose.pose.orientation.w = quadrotor_Q.w();
    odomHigh.twist.twist.linear.x = latest_V.x();
    odomHigh.twist.twist.linear.y = latest_V.y();
    odomHigh.twist.twist.linear.z = latest_V.z();
    odomHigh.twist.twist.angular.x = gyr.x() - state_point.bg.x();
    odomHigh.twist.twist.angular.y = gyr.y() - state_point.bg.y();
    odomHigh.twist.twist.angular.z = gyr.z() - state_point.bg.z();

    pubOdomHighFreq_->publish(odomHigh);
}

  • 注意：state_point.grav 是 ESKF 在线估计的重力向量（世界系）。初始化为 [0, 0, -9.81]（NED 世界系，如果实际用的是 ENU 系则是 [0, 0, 9.81]），飞行过程中 ESKF 会动态修正

  • imu_cbk 中加调用（imu_cbk 函数开头，在时间同步逻辑之前）：

// 200Hz forward propagation: ESKF 初始化后, 每次 IMU 消息预测一次高频里程计
if (init)
{
    fastPredictIMU(get_time_sec(msg_in->header.stamp),
        V3D(msg_in->linear_acceleration.x, msg_in->linear_acceleration.y, msg_in->linear_acceleration.z),
        V3D(msg_in->angular_velocity.x, msg_in->angular_velocity.y, msg_in->angular_velocity.z));
}
  • init 标记的设置（在 timer_callback 中，ESKF 更新完毕后）：
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped_, tf_broadcaster_);

/*** Cache latest ESKF state for 200Hz IMU forward propagation ***/
updateLatestStates();
if (!init) init = true;

  • init 在第一条 LiDAR scan 的 ESKF 更新完成后才设为 true——确保 fastPredictIMU 使用的 latest_P/V/Q/Ba/Bg 已经包含了第一组 ESKF bias 估计，而不是全零

  • 说人话就是：bias 没估计好之前不要瞎前推，等 ESKF 说"我准备好了"再开 IMU 高频发表

  • publisher 注册（构造函数中）：

pubOdomHighFreq_ = this->create_publisher<nav_msgs::msg::Odometry>("/Odom_high_freq", 20);
  • 验证：启动仿真后运行：
ros2 topic hz /Odom_high_freq   # 预期 ~200Hz
ros2 topic hz /Odometry         # 原来的 10Hz 不受影响
  • 两者的位姿在 LiDAR 帧时刻（init 前的累积）应该是完全一致的，因为 fastPredictIMU 的积分起点 latest_* 就是 ESKF 的最新估计。差别仅在 LiDAR 帧之间的"裸推"部分——高频是纯 IMU 积分，低频等下一帧 LiDAR 来矫正

5 后续扩展方向

  • FAST-LIO2 只是 SLAM 系统的里程计前端，从这里可以延伸出很多方向：
    • 回环检测 + 全局优化：FAST-LIO2 没有回环，长时间飞行会有累积漂移。可以接 ScanContext（基于点云的场景识别）+ GTSAM（图优化）做后端
    • 3D 占据地图：/cloud_registered 是稠密点云地图，可以直接喂给 OctoMap 或 Voxblox 生成 ESDF（欧几里得符号距离场），给轨迹规划用
    • EGO-Planner 轨迹规划：拿到 ESDF 后就可以在未知环境中实时规划避障轨迹，形成 “FAST-LIO2 → ESDF → EGO-Planner → PX4 Offboard” 的完整自主飞行闭环
    • 实物部署：换用 Livox Mid-360（固态激光雷达，体积小、重量轻、FoV 大），安装 livox_ros_driver2 后重新编译即可。px4_vlp16.yaml 中的参数需要根据 Mid-360 的特性重新调：lidar_type: 1（AVIA 类）、scan_line: 4、fov_degree: 360、blind: 0.1
    • 多机协同：多架飞机各自跑 FAST-LIO2，通过 vehicle_visual_odometry 把里程计发给 PX4 替代 GPS，实现室内编队
  • 考虑后面有一期我们来谈谈 OctoMap + EGO-Planner 的集成

总结

  • 本文在上一期 ROS2-PX4 仿真环境的基础上，完整部署了 FAST-LIO2 LiDAR-惯性里程计
  • 核心工作回顾：
    • 拉取官方 hku-mars/FAST_LIO 的 ROS2 分支
    • 通过 #ifdef USE_LIVOX 条件编译去掉 livox_ros_driver2 硬依赖，仿真用 VLP-16 直接走标准 PointCloud2 路径
    • 设计 camera_init → body → base_link → sensors 的 TF 树，打通 FAST-LIO2 和 PX4 两套坐标系
    • 定制 px4_vlp16.yaml 参数配置（VLP-16、16 线、10Hz、外参 [0,0,0.12]）
    • 一键启动脚本 3_fastlio2.sh：静态 TF + FAST-LIO2 + RViz2 可视化
  • 核心踩坑回顾：
    • FAST-LIO2 必须 clone ROS2 分支，main 分支只支持 ROS1
    • livox_ros_driver2 不在 apt 源里——做仿真不要强行装一整套 Livox SDK，条件编译是最优雅的解法
    • camera_init 和 base_link 之间没有 TF → RViz2 全部报 “No transform” → 加 body → base_link 静态 identity 一步搞定
    • Fixed Frame 要改成 camera_init，否则 FAST-LIO2 的点云/里程计/路径全部不显示
  • 如有错误，欢迎指出！
  • 感谢观看！在这里插入图片描述
# c++ # 无人机
Logo
天工造物局

北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台

更多推荐

graphify 视频音视频转写全流程解析：从 detect 结果到知识图谱文档的 Step 2.5

graphify 的核心能力是把任意语料（代码、文档、PDF、图片）转化为可查询的知识图谱，而视频与音频文件无法被直接阅读，必须先把它们转写成文本。本文围绕 `graphify` 运行手册中的转写参考文档 [transcribe.md](https://link.gitcode.com/i/d3285781588e0f1b84886dfdbc4fe7f6)，完整讲解 "Step 2.5 - Tra

avatar 天工造物局

ML-For-Beginners 终章：经典机器学习在金融、医疗、零售等八大行业中的真实落地场景

本篇技术指南基于开源课程仓库 ML-For-Beginners 的收尾课程《Postscript: Machine learning in the real world》（对应仓库目录 [9-Real-World/1-Applications](https://link.gitcode.com/i/6b23002df80861c20abf34d7db63ef41)）编写。该课此前 11 周的教学带

avatar 天工造物局

graphify 知识图谱查询实战：query / path / explain 与可审计的答案闭环

本指南以 graphify 的 Trae 版查询技能参考（[graphify/skills/trae/references/query.md](https://link.gitcode.com/i/72a7875d46ec6d3706d31791f3cdcbe0)）为主体，系统讲解如何针对**已构建好的知识图谱**执行三类查询：语义相关节点遍历（query）、两概念间最短路径（path）、单节点关

avatar 天工造物局