感知算法开发示例

本示例以 FAST-LIVO2（Fast, Direct LiDAR-Inertial-Visual Odometry）为核心，在 Lite3 机器人上实现激光雷达-惯性-视觉融合 SLAM。开发者可以通过该系统获取实时三维点云地图、视觉特征子地图、里程计估计和相机 RGB 图像。

代码资源：DeepRoboticsLab/fast-livo2-deep-robotics

硬件扩展与 FAST-LIVO2 视频或 Bilibili可配合本例阅读。视频演示存在未同步传感器导致的失败；请按当前仓库选择硬件同步或 soft-sync 方案。更多说明见视频教程。

功能定位

感知系统在 Lite3 开发生态中处于高层感知层，其职责包括：

通过 Livox Mid-360/Mid-360s 激光雷达获取三维点云数据
通过雷达内置 IMU 获取高频惯性测量数据
通过 Intel RealSense D435i 获取 RGB 图像数据
运行 FAST-LIVO2 进行 LiDAR-Inertial-Visual 融合里程计估计
输出实时三维点云地图、里程计位姿和视觉特征子地图

上层应用（如导航、建图、路径规划）通过订阅 ROS2 话题获取感知结果。

系统环境

项目	要求
操作系统	Ubuntu 22.04
ROS2 版本	ROS2 Humble
机器人型号	Lite3 Venture（仅此版本需要硬件扩展）
机载计算	AGX Jetson Orin（Jetpack 6.1，CUDA 12.6）
激光雷达	Livox Mid-360 或 Mid-360s
相机	Intel RealSense D435i

硬件安装

AGX 机载计算安装

Lite3 Venture 不自带高性能计算单元，需要额外安装 AGX Jetson Orin 作为机载计算机。

组装 AGX 套件与转接板

所需零件：

AGX 套件
转接板支架
四颗 M2.5 × 55 半螺纹内六角圆柱头螺钉

将 AGX 套件放在转接板支架上，对齐四个安装孔。从转接板底部插入四颗 M2.5 × 55 螺钉并均匀拧紧，确保 AGX 套件平放。

AGX 与转接板组装

将组件安装到 Lite3 上盖

所需零件：

已组装的 AGX + 转接板组件
Lite3 上盖
三颗 M3 × 6 内六角圆柱头螺钉

将组件放置在上盖指定位置，对齐三个安装点，安装 M3 × 6 螺钉并均匀拧紧。

AGX 安装到 Lite3 上盖

激光雷达连接

本仓库支持两种 Livox 雷达变体：

Mid-360（原始版）：使用 MID360 启动文件和 MID360_config.json
Mid-360s（新版）：使用 MID360s 启动文件和 MID360s_config.json

连接方式：

雷达连接线缆

将线缆的 XT30 端插入 Lite3 的 24V XT30 接口
将以太网端连接到 AGX 的以太网口
在主机上打开 Livox Viewer 2，确认雷达已连接

相机连接

Intel RealSense D435i 通过 USB 3.0 连接到 AGX。确保使用原装 USB 线缆以获得最佳带宽。

软件编译与安装

安装 Livox SDK 和驱动

编译 Livox-SDK2

cd fast-livo2-deep-robotics/src/Livox-SDK2
mkdir build && cd build
cmake .. && make -j
sudo make install

编译 livox_ros_driver2

cd fast-livo2-deep-robotics/src/livox_ros_driver2/
source /opt/ros/humble/setup.bash
./build.sh humble

安装 librealsense 和 realsense-ros

安装 librealsense

按照 librealsense 官方文档在 AGX 上安装。

常见问题 — 库版本冲突：

如果 rs-enumerate-devices 报错 bad optional access，说明系统中存在两个版本的 librealsense：

路径	版本	来源	状态
`/usr/local/lib`	librealsense2.so.2.56.5	自编译（支持 RSUSB）	✅
`/opt/ros/humble/lib`	librealsense2.so.2.56.4	ROS2 自动安装（旧版本）	❌

解决方法：

echo "/usr/local/lib" | sudo tee /etc/ld.so.conf.d/99-realsense-local.conf
sudo ldconfig
echo 'export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH' >> ~/.bashrc
source ~/.bashrc

验证：

ldd $(which rs-enumerate-devices) | grep realsense

正确结果应指向 /usr/local/lib/librealsense2.so.2.56.5。

安装 realsense-ros

sudo apt -y install ros-humble-realsense2-*

验证：

ros2 launch realsense2_camera rs_pointcloud_launch.py

编译 FAST-LIVO2

安装 ROS2 依赖：

sudo apt update
sudo apt -y install ros-humble-pcl-ros ros-humble-compressed-image-transport ros-humble-sophus

编译：

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
colcon build --packages-select fast_livo --symlink-install
source install/setup.bash

验证包：

ros2 pkg list | grep livo

预期输出包含 fast_livo 和 livox_ros_driver2。

传感器配置

配置激光雷达网络

以太网适配器需要配置静态 IPv4 地址，与雷达处于同一子网。

# 查找以太网接口
ip -br address
nmcli -t -f DEVICE,TYPE,STATE,CONNECTION device status

# 配置静态 IP（以 192.168.1.45 为例，雷达 IP 为 192.168.1.201）
sudo nmcli connection modify "Wired connection 2" \
  ipv4.method manual \
  ipv4.addresses 192.168.1.45/24 \
  ipv4.gateway "" \
  ipv4.dns "" \
  ipv4.never-default yes
sudo nmcli connection down "Wired connection 2"
sudo nmcli connection up "Wired connection 2"

ipv4.never-default yes 防止雷达连接替换 WiFi 默认路由。

验证：

ip route get 192.168.1.201
ping -c 3 192.168.1.201

配置驱动文件中的 IP 地址：

Mid-360：src/livox_ros_driver2/config/MID360_config.json
Mid-360s：src/livox_ros_driver2/config/MID360s_config.json

关键字段：

"host_ip": "192.168.1.45",
"ip": "192.168.1.201"

配置相机

FAST-LIVO2 要求图像尺寸与相机模型精确匹配。以 D435i 的 RGB 流 640×480×30 为例：

设置图像话题

修改 src/FAST-LIVO2/config/avia.yaml：

common:
  img_topic: "/camera/camera/color/image_raw"
  lid_topic: "/livox/lidar"
  imu_topic: "/livox/imu"

设置相机内参

修改 src/FAST-LIVO2/config/camera_pinhole.yaml：

camera:
  model: Pinhole
  width: 640
  height: 480
  scale: 1.0
  fx: 604.3154296875
  fy: 603.8231811523438
  cx: 325.12115478515625
  cy: 261.0074462890625
  d0: 0.0
  d1: 0.0
  d2: 0.0
  d3: 0.0

获取相机标定参数：

ros2 topic echo --once /camera/camera/color/camera_info

从 K 矩阵提取：fx=K[0]、fy=K[4]、cx=K[2]、cy=K[5]。

配置外参

IMU 到 LiDAR 外参

在 avia.yaml 的 extrin_calib 中配置：

extrin_calib:
  extrinsic_T: [0.04165, 0.02326, -0.0284]  # IMU 到 LiDAR 平移
  extrinsic_R: [1.0, 0.0, 0.0,
                0.0, 1.0, 0.0,
                0.0, 0.0, 1.0]              # IMU 到 LiDAR 旋转

LiDAR 到相机外参

extrin_calib:
  Rcl: [0.00610193,-0.999863,-0.0154172,
        -0.00615449,0.0153796,-0.999863,
        0.999962,0.00619598,-0.0060598]
  Pcl: [0.0194384, 0.104689,-0.0251952]

时间偏移

time_offset:
  imu_time_offset: 0.0    # IMU 时间偏移
  img_time_offset: 0.1    # 相机时间偏移（根据实际硬件调整）
  exposure_time_init: 0.0 # 曝光时间初始值

核心接口与数据流

LIVMapper — 主映射器

LIVMapper 是 FAST-LIVO2 的核心类，负责订阅传感器数据、执行融合 SLAM 并发布结果。

构造与初始化

LIVMapper mapper(nh, "laserMapping", options);
mapper.initializeSubscribersAndPublishers(nh, it_);
mapper.run(nh);

订阅话题

话题	消息类型	说明
`/livox/lidar`	`livox_ros_driver2/msg/CustomMsg`	Livox 点云数据
`/livox/imu`	`sensor_msgs/msg/Imu`	Livox IMU 数据
`/left_camera/image`	`sensor_msgs/msg/Image`	相机 RGB 图像

发布话题

话题	消息类型	说明
`/cloud_registered`	`sensor_msgs/msg/PointCloud2`	注册后的彩色/强度点云
`/aft_mapped_to_init`	`nav_msgs/msg/Odometry`	里程计位姿
`/path`	`nav_msgs/msg/Path`	轨迹路径
`/Laser_map`	`sensor_msgs/msg/PointCloud2`	全局地图
`/cloud_effected`	`sensor_msgs/msg/PointCloud2`	有效特征点
`/rgb_img`	`sensor_msgs/msg/Image`	渲染后的 RGB 图像
`/LIVO2/imu_propagate`	`nav_msgs/msg/Odometry`	IMU 预积分里程计
`/mavros/vision_pose/pose`	`geometry_msgs/msg/PoseStamped`	MAVROS 位姿

运行模式

系统根据配置自动选择运行模式：

模式	条件	说明
`LIVO`	`img_en=1` 且 `lidar_en=1`	激光雷达 + 惯性 + 视觉融合
`ONLY_LIO`	`imu_en=true` 且 `img_en=0`	仅激光雷达 + 惯性
`ONLY_LO`	无 IMU 无相机	仅激光雷达

数据流

graph LR
    A["Livox LiDAR"] -->|/livox/lidar| B["FAST-LIVO2 /laserMapping"]
    C["Livox IMU"] -->|/livox/imu| B
    D["RealSense D435i"] -->|/camera/color/image_raw| B
    B -->|/cloud_registered| E["RViz / 点云地图"]
    B -->|/aft_mapped_to_init| F["里程计"]
    B -->|/path| G["轨迹"]
    B -->|/rgb_img| H["RGB 图像"]

核心处理流程

void LIVMapper::run(rclcpp::Node::SharedPtr &node) {
    rclcpp::Rate rate(5000);
    while (rclcpp::ok()) {
        rclcpp::spin_some(this->node);
        if (!sync_packages(LidarMeasures)) {
            rate.sleep();
            continue;
        }
        handleFirstFrame();    // 首帧初始化
        processImu();          // IMU 预积分
        stateEstimationAndMapping();  // 状态估计与建图
    }
    savePCD();                 // 保存点云
}

每个周期依次执行：

数据同步（sync_packages）：按时间戳对齐 LiDAR、IMU、图像数据
IMU 处理（processImu）：预积分、去偏、状态传播
状态估计（stateEstimationAndMapping）：根据模式分发到 VIO 或 LIO 处理

多传感器时间同步

问题背景

Livox Mid-360s 雷达的 LiDAR 和 IMU 使用不同的硬件时钟，没有共享参考。如果不进行校正，两者之间的时间戳偏移会以约 1 秒/秒的速度增长，导致 FAST-LIVO2 在约 20 秒后停止工作。

硬件同步（PTP、GPIO）在此场景不可行：PTP 可达 Livox 单元但无法到达其内部 IMU 振荡器；RealSense D435i 的 RGB 和深度传感器位于不同 PCB 上，GPIO 同步无法到达 RGB 流。

同步方案

时间同步流水线

graph TB
    A["Livox IMU<br/>自有时钟，会漂移"] --> B["EMA 偏移滤波器<br/>α = 0.01，跟踪漂移"]
    C["Livox LiDAR<br/>参考时钟"] --> D["sync_packages()<br/>互斥锁保护合并"]
    E["RealSense D435i<br/>主机时钟"] --> F["静态偏移<br/>固定 YAML 值"]
    B --> D
    F --> D
    D --> G["FAST-LIVO2 SLAM"]

IMU-LiDAR 偏移校正

在 imu_cbk 中使用指数移动平均（EMA）滤波器（ $\alpha = 0.01$ ）持续重新估计时钟偏移：

// 检测 IMU 与 LiDAR 时间戳偏差
if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0) {
    double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp);
    RCLCPP_INFO(this->node->get_logger(),
        "Self sync IMU and LiDAR, HARD time lag is %.10lf", timediff_imu_wrt_lidar - 0.100);
}

相机偏移校正

RealSense 流通过固定的 img_time_offset 进行校正，因为其驱动已经基于主机时钟打时间戳：

time_offset:
  img_time_offset: 0.1    # 在 YAML 中设置，非运行时计算

非阻塞时间戳处理

将原来检测到时间戳跳变时丢弃 IMU/图像帧的逻辑替换为记录警告日志，避免流水线因瞬时抖动而卡死。

分支选择

分支	适用场景
`main`	传感器已硬件同步，或运行公开数据集（如 `Retail_Street.db3`）
`soft-sync`	IMU、LiDAR 和相机使用独立未同步时钟

切换分支：

git checkout soft-sync

运行示例

通过 ROS2 bag 运行（仿真/回放）

下载 FAST-LIVO2 原始数据集（Retail_Street）：

# 安装 sqlite3 工具
sudo apt update && sudo apt install sqlite3

# 更新数据集中的话题类型（适配 livox_ros_driver2）
sqlite3 Retail_Street.db3 "UPDATE topics SET type = 'livox_ros_driver2/msg/CustomMsg' WHERE name = '/livox/lidar';"

运行算法：

cd fast-livo2-deep-robotics
source install/setup.bash
ros2 launch fast_livo mapping_avia.launch.py use_rviz:=True

播放数据集：

source install/setup.bash
ros2 bag play Retail_Street.db3

在真实 Lite3 机器人上运行

启动激光雷达

终端 1 — LiDAR（Mid-360s）：

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360s_launch.py

Mid-360 使用 msg_MID360_launch.py。

启动相机

终端 2 — D435i RGB 相机：

ros2 launch realsense2_camera rs_launch.py \
  enable_rgbd:=false \
  enable_sync:=false \
  align_depth.enable:=false \
  enable_color:=true \
  enable_depth:=false \
  rgb_camera.color_profile:=640x480x30

确认启动输出显示 Width: 640, Height: 480, FPS: 30。

启动 FAST-LIVO2

终端 3 — FAST-LIVO2 和 RViz：

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch fast_livo mapping_avia.launch.py use_rviz:=True

注意： 启动后保持传感器组件静止数秒以完成 IMU 初始化。初始化完成后，缓慢移动机器人以构建地图。

配置参数说明

通用配置（common）

common:
  img_topic: "/left_camera/image"    # 相机话题
  lid_topic: "/livox/lidar"          # 雷达话题
  imu_topic: "/livox/imu"            # IMU 话题
  img_en: 1                          # 启用相机（0=禁用）
  lidar_en: 1                        # 启用雷达（0=禁用）
  ros_driver_bug_fix: false          # ROS 驱动时间戳修复

预处理配置（preprocess）

preprocess:
  point_filter_num: 1        # 点云降采样因子
  filter_size_surf: 0.1      # 体素滤波尺寸（米）
  lidar_type: 1              # 1=Livox Avia
  scan_line: 6               # 扫描线数
  blind: 0.8                 # 盲区距离（米）

VIO 配置

vio:
  max_iterations: 5          # 最大迭代次数
  outlier_threshold: 1000    # 异常值阈值
  img_point_cov: 100         # 图像点协方差
  patch_size: 8              # 图像块大小
  patch_pyrimid_level: 4     # 金字塔层数
  normal_en: true            # 法线约束
  exposure_estimate_en: true # 曝光估计

LIO 配置

lio:
  max_iterations: 5          # 最大迭代次数
  voxel_size: 0.5            # 体素大小（米）
  max_layer: 2               # 最大体素层数
  max_points_num: 50         # 每体素最大点数

IMU 配置

imu:
  imu_en: true               # 启用 IMU
  imu_int_frame: 30          # IMU 初始化帧数
  acc_cov: 0.5               # 加速度协方差
  gyr_cov: 0.3               # 角速度协方差
  b_acc_cov: 0.0001          # 加速度偏置协方差
  b_gyr_cov: 0.0001          # 角速度偏置协方差

验证与问题排查

验证话题

# 检查话题类型
ros2 topic type /livox/lidar                    # 预期: livox_ros_driver2/msg/CustomMsg
ros2 topic type /livox/imu                      # 预期: sensor_msgs/msg/Imu
ros2 topic type /camera/camera/color/image_raw  # 预期: sensor_msgs/msg/Image

# 检查数据频率
ros2 topic hz /livox/lidar                      # 预期: ~10 Hz
ros2 topic hz /livox/imu                        # 预期: ~200 Hz
ros2 topic hz /camera/camera/color/image_raw    # 预期: ~30 Hz

验证 FAST-LIVO2 输出

ros2 topic hz /cloud_registered       # 注册点云
ros2 topic hz /aft_mapped_to_init     # 里程计
ros2 topic hz /path                   # 轨迹

检查节点

ros2 node list
# 预期包含: /livox_lidar_publisher, /camera/camera, /laserMapping

ros2 topic info -v /livox/lidar
# 预期: 1 个发布者，/laserMapping 为订阅者

常见问题排查

问题	排查步骤
RViz 中无点云	检查 `ros2 node list` 是否包含所有节点
雷达无数据	确认 `msg_MID360s_launch.py` 与物理设备匹配
相机无数据	确认 `camera_pinhole.yaml` 中的分辨率与实际一致
IMU 初始化失败	保持传感器静止数秒，检查 IMU 话题频率
时间戳跳变	检查是否有多个 Livox 驱动实例占用 UDP 端口
驱动 bind 失败	确认 `host_ip` 已分配给本地接口
库版本冲突	检查 `ldd` 输出，确保加载 `/usr/local/lib` 版本
20 秒后停止	切换到 `soft-sync` 分支启用软件时间同步

高级功能

点云保存

pcd_save:
  pcd_save_en: true          # 启用点云保存
  colmap_output_en: false    # Colmap 格式输出
  filter_size_pcd: 0.15      # 保存时体素滤波尺寸
  interval: -1               # -1 = 所有帧保存到一个文件

保存路径：Log/PCD/all_raw_points.pcd 和 Log/PCD/all_downsampled_points.pcd。

Colmap 输出

pcd_save:
  colmap_output_en: true
  interval: -1

输出路径：Log/Colmap/sparse/0/points3D.txt。

轨迹评估

evo:
  seq_name: "CBD_Building_01"  # 序列名称
  pose_output_en: true         # 启用位姿输出

输出路径：Log/result/<seq_name>.txt。

动态物体检测

FAST-LIVO2 支持动态物体检测和移除：

话题	说明
`/dyn_obj`	检测到的动态物体点云
`/dyn_obj_removed`	移除动态物体后的点云
`/dyn_obj_dbg_hist`	动态物体调试历史

IMU 预积分里程计

uav:
  imu_rate_odom: true    # 启用高频 IMU 预积分里程计

输出话题：/LIVO2/imu_propagate（250Hz）。

安全注意事项

硬件安装安全：安装 AGX 套件时确保螺钉均匀拧紧，避免松动。
线缆管理：确保所有线缆固定牢靠，避免机器人运动时线缆脱落。
散热：AGX Jetson Orin 在高负载下会产生大量热量，确保散热良好。
网络隔离：雷达以太网连接设置 ipv4.never-default yes，避免影响正常网络。
传感器保护：激光雷达和相机为精密光学设备，避免碰撞和强光直射。
软件同步局限：软件时间同步是近似方案，急转弯可能导致相机-雷达帧错位，大环境中的累积漂移可能超出滤波器修正能力。