【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署，200HZ IMU前推的无限魅力

前言

• 上一期我们花了很大篇幅，从零搭建了一套完整的 ROS2 Humble + PX4 SITL + Gazebo Classic 仿真环境：
  • 【10天速通ROS2-PX4无人机】(一) 从零搭建仿真环境，妈妈再也不用担心我炸机了！
• 现在仿真有了、VLP-16 3D 激光雷达有了、IMU 有了、网笼测试场有了，但飞机还不知道自己在哪里——PX4 自带的 EKF 依赖 GPS，在室内/网笼这种 GPS 拒止环境下直接抓瞎
• 前几期我们刚好也完整推导了 FAST-LIO2 和 Point-LIO 的算法原理与源码：
  • 【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照
  • 【Point-LIO 源码解读】逐点更新取代逐帧更新 — 公式与代码逐行对照
• 本期我们就把 FAST-LIO2 部署到仿真环境中，让飞机在网笼里靠 LiDAR + IMU 实时建图定位，打通"真机部署前的最后一公里"
• 在进阶部分，我们会参考东北大学 REAL_DRONE_400 开源项目，给 FAST-LIO2 加上 200Hz IMU 前推——用中值积分在每两条 IMU 消息之间预测一次位姿，把里程计输出频率从 LiDAR 帧率（10Hz）拉到 IMU 帧率（200Hz）。这对 PX4 的 Offboard 控制至关重要——EKF2 需要高频里程计输入才能稳定飞行
• 核心链路：hku-mars/FAST_LIO ROS2 分支 + 条件编译去掉 livox_ros_driver2 + VLP-16 仿真配置 + camera_init TF 树 + 200Hz IMU 前推 + RViz2 可视化

文章目录

• • 前言
  • 1 FAST-LIO2 快速回顾
  • • • 1-1 核心思路
      • 1-2 为什么不用特征提取
  • 2 编译 FAST-LIO2
  • • • 2-1 拉取仓库
      • 2-2 修改源码 — 去掉 livox_ros_driver2 硬依赖
  • 3 启动配置
  • • • 3-1 TF 树设计
      • 3-2 启动脚本
      • 3-3 话题说明
      • 3-4 参数说明
      • 3-5 RViz2 可视化
      • 3-6 常见警告：`No point, skip this scan!`
      • • 3-6-1 IMU 去畸变后没有有效点（第 1069 行）
        • 3-6-2 降采样后有效点太少（第 1107 行）
      • 3-7 实机部署 — livox_ros_driver2 的四个坑
      • • 3-7-1 PTP 时钟同步不完整
        • 3-7-2 CustomMsg vs PointCloud2
        • 3-7-3 FAST-LIO2 配置 lidar_type 的映射陷阱
        • 3-7-4 livox_ros_driver2 版本差异
  • 4 进阶玩法 — 200Hz IMU 前推
  • • • 4-1 为什么需要高频里程计
      • 4-2 核心思路
      • 4-3 中值积分推导
      • 4-4 代码实现
  • 5 后续扩展方向
  • 总结

1 FAST-LIO2 快速回顾

1-1 核心思路

• 我们在 【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照 中已经完整推导过，这里只做最小回顾
• FAST-LIO2 做一件事：把 LiDAR 点云和 IMU 数据融合，输出实时 6-DOF 位姿 + 3D 地图
• 核心公式是迭代误差状态卡尔曼滤波（ESEKF）：

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.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

• 启动顺序（需要三个终端）：

• 注意：终端 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 个核心话题：

• PX4 提供的关键输入话题：

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=所有帧存到一个文件（可能内存溢出）

• 重点参数解读：

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

3-5 RViz2 可视化

• 在原有的第一期 px4_sim.rviz 基础上，新增了 3 个 FAST-LIO2 专属显示面板：

• 原有的 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 只通过消息类型依赖它

• 修复结果：

• 附：点云延迟检测脚本（保存为 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 的点云/里程计/路径全部不显示
• 如有错误，欢迎指出！
• 感谢观看！