ROS 激光雷达建图导航实战

ROS
4.5k 词

基于 ROS + Gazebo 仿真环境，从零搭建带激光雷达的机器人并实现自主导航。

一、在 URDF 中添加激光雷达

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<link name="sensor_laser">
  <inertial>
    <mass value="1" />
    <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.02" iyz="0" izz="0.02"/>
  </inertial>
  <visual>
    <geometry>
      <cylinder radius="0.05" length="0.1"/>
    </geometry>
    <material name="white" />
  </visual>
  <collision>
    <geometry>
      <cylinder radius="0.05" length="0.1"/>
    </geometry>
  </collision>
</link>

<joint name="joint_sensor_laser" type="fixed">
  <origin xyz="0.15 0 0.05" rpy="0 0 0"/>
  <parent link="link_chassis"/>
  <child link="sensor_laser"/>
</joint>

加载雷达插件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
<gazebo reference="sensor_laser">
  <sensor type="ray" name="head_hokuyo_sensor">
    <update_rate>20</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>720</samples>
          <min_angle>-1.570796</min_angle>
          <max_angle>1.570796</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.10</min>
        <max>10.0</max>
      </range>
      <noise>
        <type>gaussian</type>
        <stddev>0.01</stddev>
      </noise>
    </ray>
    <plugin name="gazebo_ros_head_hokuyo_controller" 
            filename="libgazebo_ros_laser.so">
      <topicName>/m2wr/laser/scan</topicName>
      <frameName>sensor_laser</frameName>
    </plugin>
  </sensor>
</gazebo>

关键参数：samples=720 采样点数、min_angle/max_angle 扫描范围（±90°）、range 测距范围（0.1~10m）。

二、move_base 代价地图

全局 vs 局部代价地图

global_costmap local_costmap
坐标系 map odom
更新方式 导航开始前构建，不随机器人移动改变 以机器人为中心滚动更新
数据来源 静态地图 + 障碍物层 + 膨胀层 障碍物层 + 膨胀层
用途 全局路径规划 局部路径规划 + 避障

local_costmap 只在自己小范围内建图。离开区域后数据会丢失，需要在全局下设置同步更新。

代价地图参数配置

costmap_common_params.yaml：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
footprint: [[-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125]]
transform_tolerance: 0.2

obstacle_layer:
  enabled: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: 
    data_type: LaserScan
    topic: /m2wr/laser/scan
    marking: true
    clearing: true

inflation_layer:
  enabled: true
  inflation_radius: 0.55
  cost_scaling_factor: 2.58

local_costmap_params.yaml：

1
2
3
4
5
6
7
8
9
10
local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 20.0
  publish_frequency: 10.0
  static_map: false
  rolling_window: true        # 以机器人为中心滚动
  width: 5
  height: 5
  resolution: 0.05

rolling_window: true 是关键——让局部代价地图始终以机器人为中心。

三、路径规划算法

Dijkstra

每次从「未求出最短路径」中取出距离起点最近的点，以此为桥梁刷新其他点的距离。

1
2
3
4
5
6
7
8
算法核心：
  result = {A(0)}          已求出最短路径
  notFound = {B(2),C(∞),D(6)}  未求出

  1. notFound 中取最短 B(2) → result
  2. 通过 B 刷新: AD=6 > AB+BD=4 → D 更新为 4
  3. notFound 中取最短 D(4) → result
  4. 最终: result = {A(0),B(2),D(4),C(5)}

Dijkstra 是求一个图中一个点到其他所有点的最短路径。在 ROS 中，global_planner 默认使用 Dijkstra 或其变体 A*。

算法对比

Dijkstra A*
搜索策略 广度优先，均匀向外扩展 启发式搜索，朝目标方向优先
效率 慢，遍历大量无关节点 快，目标导向
最优性 保证最优 启发函数合适时保证最优

A* 在 Dijkstra 基础上加了一个启发函数（通常用曼哈顿距离或欧氏距离），让搜索更有方向性。

四、完整导航参数结构

一份典型的 move_base 参数文件组织：

1
2
3
4
5
config/
├── costmap_common_params.yaml    # 通用代价地图参数
├── global_costmap_params.yaml    # 全局代价地图
├── local_costmap_params.yaml     # 局部代价地图
└── teb_local_planner_params.yaml # 局部规划器参数（DWA/TEB/...）

启动流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
<launch>
  <!-- 启动 map_server 加载静态地图 -->
  <node name="map_server" pkg="map_server" type="map_server" 
        args="$(find my_pkg)/maps/my_map.yaml"/>
  
  <!-- 启动 move_base -->
  <node pkg="move_base" type="move_base" name="move_base">
    <rosparam file="$(find my_pkg)/config/costmap_common_params.yaml" 
              command="load" ns="global_costmap" />
    <rosparam file="$(find my_pkg)/config/costmap_common_params.yaml" 
              command="load" ns="local_costmap" />
    <rosparam file="$(find my_pkg)/config/local_costmap_params.yaml" 
              command="load" />
    <rosparam file="$(find my_pkg)/config/global_costmap_params.yaml" 
              command="load" />
    <rosparam file="$(find my_pkg)/config/teb_local_planner_params.yaml" 
              command="load" />
  </node>
</launch>

注意 costmap_common_params.yaml 对 global 和 local 都要加载一次（通过 ns 切换命名空间）。

关键参数速查

参数 说明 典型值
obstacle_range 障碍物检测最大距离 2.5m
raytrace_range 射线追踪清理距离 3.0m
inflation_radius 膨胀半径 0.55m
cost_scaling_factor 代价衰减系数 2.58
update_frequency 代价地图更新频率 20Hz
rolling_window 是否以机器人为中心 local: true