ROS 2 3D雷达导航系统实现详解

1 实现路径

本文使用的开发板是鲁班猫（ubuntu22.04），通过USB连接鲁班猫并启动ROS 2对应的雷达驱动节点后，系统会产生以下核心数据：

核心数据输出

(1) sensor_msgs/PointCloud2 (3D点云数据)

• 数据来源：雷达驱动底层输出的极其稠密的3D点云
• 数据特性：
  • 包含精确的X、Y、Z三维坐标信息
  • 包含物理反射强度(Intensity)数据
  • 不包含RGB颜色信息
• 数据特点：高密度、高精度，为环境建模提供基础数据支持

(2) sensor_msgs/Imu (惯性数据)

• 数据来源：部分高端3D雷达内置IMU模块
• 数据特性：
  • 高频输出姿态数据（欧拉角或四元数）
  • 高频输出加速度数据
• 应用目的：为导航算法提供运动状态校准，提高定位精度

Cartographer处理流程

系统采用Cartographer算法进行数据处理和地图构建：

• 输入数据：

  • sensor_msgs/PointCloud2点云数据
  • 小车基础阶段的位移数据（里程计信息）

• 处理过程：

  1. 实时接收传感器数据
  2. 进行点云配准和闭环检测
  3. 生成增量式子图
  4. 融合子图构建全局地图

• 输出结果：

  • 2D栅格地图
  • 小车实时坐标信息（位姿估计）

2 Nav2导航实现

实施步骤

步骤1：前期准备

• 获取移动数据（机器移动时候产生的速度，角度数据，发布到/odom中）

步骤2：激光雷达启动

# 启动对应的3D激光雷达驱动
ros2 launch <radar_driver> <radar_config.launch.py>

此时雷达会产生周围环境的高精度3维点云信息。

步骤3：启动Cartographer建图算法

在建图之前需要修改cartographer.lua配置文件，使其可以读取3d数据

#cartographer.lua
#修改以下内容
tracking_frame = "imu_link"        -- 3D导航需要imu坐标系，关于imu的数据发布到/imu话题中，底层会监听此话题
num_laser_scans = 0,               -- 修改点：关闭2D雷达
num_point_clouds = 1,              -- 修改点：开启3D点云

注意：imu数据需为sensor_msgs/msg/Imu格式，移动数据需为nav_msgs/msg/Odometry格式，cartographer会监听/imu和/odom来得到这两个数据。如果数据默认不是发布到/imu和/odom，可以在cartographer.launch.py中设置重映射 Remap。

#cartographer.lua
#关闭2D，开启3D建图
MAP_BUILDER.use_trajectory_builder_2d = false
MAP_BUILDER.use_trajectory_builder_3d = true

#将以前的TRAJECTORY_BUILDER_2D内容都删除掉
#修改点：配置3D建图参数
TRAJECTORY_BUILDER_3D.min_range = 0.3
TRAJECTORY_BUILDER_3D.max_range = 30.0  -- 3D雷达测距较远
TRAJECTORY_BUILDER_3D.use_imu_data = true -- 3D必须使用IMU
TRAJECTORY_BUILDER_3D.voxel_filter_size = 0.15

然后开启cartographer建图

# 使用cartographer启动Cartographer
ros2 launch cartographer cartographer.launch.py

算法内部处理机制：

• Cartographer自动对3D点云进行高度切片处理
• 将3D点云投影生成2D数据
• 生成供Nav2使用的2D全局栅格地图

技术优势：

• 无需在外部ROS架构中额外串联pointcloud_to_laserscan降维节点
• 内部处理提高系统效率和实时性
• 保持数据处理的连续性和准确性

步骤4：地图保存

# 完成地图建模后
ros2 run nav2_map_server map_saver_cli -f <map_name>

# 关闭Cartographer节点
ros2 node kill /cartographer_node

Nav配置

由于Nav2默认是2D导航，需要配置Nav2以支持处理3D点云数据作为障碍物源。主要配置文件包括：

# nav2_params.yaml
observation_sources: pointcloud   # 修改点：将 scan 改为 pointcloud          
pointcloud:
    topic: /lslidar_point_cloud   # ⚠️修改点：雷达实际发出的3D点云话题，镭神通常是这个，请根据实际情况修改
    data_type: "PointCloud2"      # 修改点：数据类型升级为 PointCloud2
    #pointcloud下其他配置不变

# nav2_params.yaml
plugins: ["static_layer", "voxel_layer", "inflation_layer"]  # 修改点：将 obstacle_layer 替换为 voxel_layer
voxel_layer:                         # 修改点：完全重写这一段，替换原来的 obstacle_layer
    plugin: "nav2_costmap_2d::VoxelLayer" 
    enabled: True
    publish_voxel_map: True
    origin_z: 0.0
    z_resolution: 0.05
    z_voxels: 16
    max_obstacle_height: 2.0
    mark_threshold: 0
    observation_sources: pointcloud
    pointcloud:
        topic: /lslidar_point_cloud               # ⚠️修改点：填写实际的3D雷达点云话题
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "PointCloud2"                  # 修改点：指定数据类型为 3D 点云
        raytrace_max_range: 10.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0

2.2.2 启动导航系统

# 使用保存的地图启动Nav2
ros2 launch nav2_bringup navigation.launch.py map:=~/map_navigation.yaml

此时，系统具备了实时避障能力，小车可以根据地图进行自主导航。

3. 自动导航实现机制

3.1 导航指令发送

服务器需要发送巡检的位置坐标和角度信息给小车端。数据格式如下：

# 导航目标数据结构
from nav2_msgs.action import NavigateToPose

goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.pose.position.x = 3.5  # 目标位置X坐标
goal_msg.pose.pose.position.y = 2.0  # 目标位置Y坐标
goal_msg.pose.pose.position.z = 0.0  # Z坐标（2D导航通常为0）
goal_msg.pose.pose.orientation.w = 1.0  # 朝向角度，注意这个角度不能直接使用角度值，需要转换为四元数

3.2 导航流程

1. 数据打包：将位置信息打包为 nav2_msgs/action/NavigateToPose 数据类型
2. 消息发送：将数据包在本地发送到Nav2算法自动监听的动作接口 /navigate_to_pose
3. 路径规划：Nav2收到目标后，自动进行全局路径规划和局部路径规划
4. 实时导航：小车根据规划路径自动向目标方向进行导航，同时实时避障

步骤4：后续导航设置

执行2.1.4节中的步骤6，启动Nav2导航系统。

关键技术要点

3D到2D转换策略

• 内部转换：Cartographer内部自动完成3D到2D的转换
• 保持精度：在转换过程中保持关键的空间信息
• 实时处理：避免外部节点引入的延迟

系统架构优化

• 数据流：传感器→驱动→Cartographer→Nav2
• 节点管理：按需启动，节约系统资源
• 误差控制：通过IMU数据进行运动状态校准

实时性保障

• 高频处理：确保点云数据的实时处理
• 并行计算：利用多线程提高处理效率
• 内存优化：合理管理点云数据的存储和释放

总结

ros2 launch wheeltec_robot_bringup bringup.launch.py  #启动运动底盘

ros2 launch lslidar_driver lslidar_launch.py  #打开对应的雷达驱动

ros2 launch wheeltec_cartographer cartographer.launch.py #初次地图建模
ros2 run teleop_twist_keyboard teleop_twist_keyboard #遥控机器走完全图，进行地图建模
ros2 run nav2_map_server map_saver_cli -f ~/my_map #保存地图， 并关闭cartographer