1. 从地图到导航：AMCL定位与TEB规划器登场

上一节我们费了老大劲，终于让阿克曼小车在 room_mini 仿真世界里跑起来，并且用 gmapping 建好了一张地图。看着 map_server 保存下来的 room_mini.pgm 和 room_mini.yaml，是不是有种“装修完房子拿到房产证”的感觉？但这只是第一步。有了地图，我们的机器人还是个“路痴”，它不知道自己在地图里的哪个位置，更不知道怎么从A点走到B点。这就好比给你一张城市地图，但蒙上你的眼睛把你扔在某个十字路口，你依然寸步难行。

这一节，我们要解决的就是这两个核心问题：定位和路径规划。目标很明确，就是让我们的阿克曼小车在已经构建好的地图里，实现全自动的导航——我们只需要在 Rviz 里用鼠标点一个目标点，小车就能自己规划路径、避开障碍、稳稳当当地开过去。听起来很酷对吧？这背后主要依赖两个ROS中的“明星”包：amcl 和 teb_local_planner。

amcl（自适应蒙特卡洛定位）是ROS里最常用、最经典的定位算法。你可以把它想象成机器人的“内心地图匹配器”。它不知道自己的绝对位置，但它身上带着传感器（比如我们仿真里的激光雷达）。amcl 会不断地将实时激光扫描数据与已有的全局地图进行匹配，通过一套巧妙的概率粒子滤波算法，来推算“我当前最可能在地图上的哪个位置”。这个过程是动态且自适应的，即使机器人初始位置有点偏差，或者在运动过程中产生了一些里程计累积误差，amcl 也能逐渐修正，把机器人“拉”回正确的位置上。

而 teb_local_planner（Timed Elastic Band，时间弹性带本地规划器）则是负责“怎么走”的。全局规划器（比如 global_planner）会给出一条从起点到终点的粗略路径，就像导航软件给你画出的那条灰色路线。但这条路径可能不够平滑，没有考虑机器人具体的运动学约束（比如我们阿克曼车的转弯半径限制），也无法应对动态出现的障碍物。teb_local_planner 的厉害之处在于，它会以这条全局路径为参考，生成一条考虑时间、动力学、避障的局部优化轨迹。它特别适合像汽车、阿克曼转向这类非全向移动的机器人，能生成非常自然、符合车辆运动学的转弯和行进路线。

所以，这一节的流程就是：启动仿真世界和机器人 -> 加载上一节建好的地图 -> 启动 amcl 让机器人“找到自己” -> 配置 move_base 导航框架，并指定使用 teb_local_planner -> 最后在 Rviz 中下达导航目标，看着小车自动驾驶。我会带你一步步配置，并分享我在调试参数时踩过的那些坑，比如粒子莫名其妙发散、小车在目标点来回震荡就是停不下来等问题，以及怎么解决它们。准备好了吗？我们开始吧。

2. 搭建自主导航的核心框架：move_base与配置文件

要让ROS的导航栈 move_base 为我们工作，我们需要提供一整套配置文件，告诉它我们的机器人长什么样、能跑多快、用什么算法定位和规划。这就像给一个新司机一本详细的车辆说明书和交规手册。这些配置文件通常放在功能包的 config 文件夹下，我们为阿克曼小车创建一个 racebot_navigation 功能包来管理它们。

首先，我们需要定义机器人的代价地图。导航过程中，move_base 会维护两张代价地图：全局代价地图和局部代价地图。全局地图用于全局路径规划，基于我们加载的静态地图；局部地图用于局部避障和轨迹优化，基于实时传感器数据。创建 costmap_common_params.yaml，这里定义一些通用参数：

# racebot_navigation/config/costmap_common_params.yaml
obstacle_range: 3.0        # 传感器最大障碍物检测范围
raytrace_range: 3.5         # 用于清理（清除）障碍物的传感器范围
footprint: [[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]] # 机器人轮廓（矩形，单位：米）
inflation_radius: 0.3       # 膨胀半径，障碍物向外膨胀的距离，保证安全

# 激光雷达传感器配置
laser_scan_sensor:
  sensor_frame: laser_link           # 传感器坐标系
  data_type: LaserScan
  topic: /scan                      # 激光话题
  marking: true                     # 用于标记障碍物
  clearing: true                    # 用于清除已通过区域的障碍物

这里的关键是 footprint，它定义了机器人在地面上的投影轮廓，是一个多边形。move_base 会确保整个轮廓都不与障碍物重叠。我们的阿克曼小车模型大概长0.5米，宽0.3米，这里我设置得稍微保守一点。inflation_radius 是安全边际，即使没碰到障碍物轮廓，靠近到这个距离也会产生代价。

接下来，分别配置全局和局部代价地图。创建 global_costmap_params.yaml：

# racebot_navigation/config/global_costmap_params.yaml
global_costmap:
  global_frame: map          # 全局代价地图的坐标系
  robot_base_frame: base_footprint # 机器人基坐标系
  update_frequency: 1.0     # 地图更新频率（Hz），全局地图不需要太高
  publish_frequency: 1.0
  static_map: true          # 使用静态地图（即我们加载的pgm地图）
  rolling_window: false     # 不使用滚动窗口（全局地图是固定的）
  width: 100                # 地图尺寸（单位：米），应覆盖整个静态地图
  height: 100
  resolution: 0.05          # 分辨率，与建图时保持一致（0.05米/像素）
  origin_x: -50.0           # 原点，与建图时设置的xmin, ymin对应
  origin_y: -50.0
  plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

然后是 local_costmap_params.yaml。局部地图是围绕机器人移动的，关注附近的环境。

# racebot_navigation/config/local_costmap_params.yaml
local_costmap:
  global_frame: odom         # **注意：局部代价地图通常用odom坐标系**
  robot_base_frame: base_footprint
  update_frequency: 5.0     # 局部地图需要更高更新频率，以应对动态障碍物
  publish_frequency: 2.0
  static_map: false         # 局部地图不使用静态地图
  rolling_window: true      # 启用滚动窗口，地图中心始终跟随机器人
  width: 6.0               # 滚动窗口的宽度（米），覆盖机器人周围区域
  height: 6.0
  resolution: 0.05
  origin_x: -3.0            # 原点相对于机器人中心偏移
  origin_y: -3.0
  plugins:
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

这里有个非常重要的细节：global_frame 在局部代价地图中通常设置为 odom。这是因为局部规划需要在短时间内有稳定、连续的坐标系来评估机器人的运动，而 odom（里程计）坐标系虽然会有漂移，但在短时间、短距离内是平滑且连续的，非常适合局部规划。map 坐标系是全局固定的，但 amcl 对机器人位姿的修正可能不是完全平滑的，直接用于局部规划可能导致控制指令抖动。

最后，我们需要配置 move_base 本身的核心参数，比如机器人的速度限制、恢复行为等。创建 base_local_planner_params.yaml（虽然名字叫这个，但它包含 move_base 的通用参数）：

# racebot_navigation/config/base_local_planner_params.yaml
controller_frequency: 5.0   # 控制循环频率（Hz）
planner_patience: 5.0       # 规划器尝试寻找路径的耐心时间（秒）
controller_patience: 15.0   # 控制器尝试到达局部目标的耐心时间
conservative_reset_dist: 3.0 # 重置代价地图前机器人需要移动的距离（米）
recovery_behavior_enabled: true # 启用恢复行为（如旋转、后退）
clearing_rotation_allowed: true # 允许在清理障碍物时旋转
shutdown_costmaps: false    # 不关闭代价地图

# 机器人速度限制
max_vel_x: 0.8              # 最大前进速度（米/秒）
min_vel_x: -0.2             # 最大后退速度
max_vel_theta: 1.0          # 最大旋转角速度（弧度/秒）
min_vel_theta: -1.0
min_in_place_vel_theta: 0.4 # 原地旋转的最小角速度

# 目标容差
xy_goal_tolerance: 0.15     # 到达目标点的XY位置容差（米）
yaw_goal_tolerance: 0.1     # 到达目标点的朝向角容差（弧度）

这些参数需要根据你的机器人实际性能来调整。比如我们的仿真小车，电机扭矩和转向速度都是有限的，所以最大速度不能设得太高，否则规划器会给出无法执行的指令。xy_goal_tolerance 和 yaw_goal_tolerance 决定了机器人何时认为“我已到达目标”。设得太小，小车可能永远在目标点附近微调无法停止；设得太大，停车位置就不够精确。我给出的值是经过几次测试后比较稳定的值。

3. 让机器人“找到自己”：AMCL定位配置与启动

有了导航框架，接下来就要解决“我在哪”的问题。amcl 的配置相对复杂，参数众多，但大部分可以使用默认值，我们只需要关注几个关键参数。创建 amcl_params.yaml 文件：

# racebot_navigation/config/amcl_params.yaml
# 滤波器基本参数
min_particles: 100          # 最小粒子数
max_particles: 3000         # 最大粒子数
kld_err: 0.01               # KLD误差上限，影响粒子数自适应
kld_z: 0.99                 # KLD分位数
update_min_d: 0.2           # 平移运动超过此距离（米）才更新滤波器
update_min_a: 0.5           # 旋转运动超过此角度（弧度）才更新滤波器

# 激光模型参数（影响定位精度关键！）
laser_model_type: likelihood_field # 使用似然场模型，比beam模型更高效稳定
laser_likelihood_max_dist: 2.0     # 似然场最大作用距离
laser_max_range: 12.0              # 激光最大有效距离，与gmapping设置一致
laser_min_range: 0.1
laser_z_hit: 0.95                  # 测量噪声（命中）的权重
laser_z_rand: 0.05                 # 随机噪声的权重

# 里程计运动模型参数（影响粒子传播）
odom_model_type: diff              # 差分驱动模型，对于阿克曼车，在转弯半径较大时可近似
odom_alpha1: 0.2                   # 旋转到平移的噪声
odom_alpha2: 0.2                   # 平移到旋转的噪声
odom_alpha3: 0.2                   # 平移噪声
odom_alpha4: 0.2                   # 旋转噪声
odom_frame_id: odom
base_frame_id: base_footprint
global_frame_id: map

# 初始位姿估计（可以大致指定，也可以在Rviz中手动设置）
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0

这里我踩过一个很大的坑：粒子发散。有一次启动后，amcl 的粒子云（在 Rviz 中显示为一堆小箭头）没有收敛到机器人身上，而是散落在整个地图，甚至跑到地图外面。机器人完全“迷失”了。这通常有几个原因：一是初始位姿 (initial_pose_x/y/a) 设得离机器人真实位置太远；二是里程计噪声参数 (odom_alpha1-4) 设得太小，导致粒子扩散不够，无法覆盖真实位置；三是激光参数 (laser_z_hit, laser_z_rand) 设置不合理，导致测量更新权重失衡。

我的经验是，如果机器人启动位置比较确定（比如每次都在仿真世界的原点），可以把初始位姿设准。如果不确定，更好的方法是在 Rviz 中使用 2D Pose Estimate 工具，手动点击地图告诉机器人“你大概在这里”。对于噪声参数，如果机器人运动模型比较精确（比如仿真环境里程计很准），可以适当调小（如0.05）；如果运动噪声大，就调大。我上面给的0.2是一个比较保守的、容错性较高的值。

现在，我们可以编写启动文件，把地图服务器、amcl 和 rviz 整合起来。创建 amcl_demo.launch：

<!-- racebot_navigation/launch/amcl_demo.launch -->
<launch>
  <!-- 启动Gazebo仿真世界和机器人 -->
  <include file="$(find racebot_gazebo)/launch/racebot.launch"/>

  <!-- 启动地图服务器，加载之前保存的地图 -->
  <arg name="map_file" default="$(find racebot_gazebo)/maps/room_mini.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- 启动AMCL节点 -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <rosparam file="$(find racebot_navigation)/config/amcl_params.yaml" command="load"/>
    <!-- 如果使用差分驱动近似，可以尝试使用diff模型，否则对于阿克曼，可以考虑使用omni模型，但需调整参数 -->
  </node>

  <!-- 启动rviz，并加载预设的导航配置 -->
  <node pkg="rviz" type="rviz" name="rviz" required="true"
        args="-d $(find racebot_navigation)/rviz/navigation.rviz"/>
</launch>

运行这个launch文件：roslaunch racebot_navigation amcl_demo.launch。打开 Rviz 后，你应该能看到地图、激光扫描点、以及 amcl 的粒子云。如果粒子云没有收敛，就用 2D Pose Estimate 工具在地图上机器人实际所在位置点一下，并拖出方向。你会看到粒子云迅速向你点击的位置聚集、收敛，最终紧紧包裹住机器人模型。这时，Rviz 中机器人在地图上的位姿应该和它在Gazebo世界中的真实位姿基本一致了。恭喜，定位成功了！

4. 为阿克曼转向量身定制：TEB本地规划器详解与配置

定位搞定，接下来是规划。全局规划我们使用ROS默认的 global_planner（或者 navfn）即可，它负责给出一条从起点到终点的最短路径（通常是基于地图的A*或Dijkstra算法）。重头戏在本地规划器，也就是真正控制机器人轮子怎么转的部分。对于阿克曼转向的汽车模型，teb_local_planner 是不二之选。

首先，确保你已经安装了 teb_local_planner。在Melodic中，可以使用命令安装：sudo apt-get install ros-melodic-teb-local-planner。它的核心思想是将机器人的运动轨迹表示为一连串带时间戳的位姿序列（即“弹性带”），然后通过优化算法，使这条带子满足机器人的运动学约束、动力学约束，同时避开障碍物，并且时间最优。

为TEB规划器创建专门的配置文件 teb_local_planner_params.yaml：

# racebot_navigation/config/teb_local_planner_params.yaml
TebLocalPlannerROS:

  # ##########################
  # 机器人运动学模型 (关键！)
  # ##########################
  robot_type: carlike          # 指定为汽车类型（阿克曼）
  footprint_model: # 轮廓模型，与代价地图中的footprint对应
    type: "polygon"
    vertices: [[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]]

  # 阿克曼转向参数
  wheelbase: 0.28              # 轴距（前轮中心到后轮中心的距离），根据你的URDF模型设定
  cmd_angle_instead_rotvel: true # 发布转向角指令，而不是旋转速度
  min_turning_radius: 0.5      # 最小转弯半径（米），由最大转向角决定

  # ##########################
  # 轨迹规划参数
  # ##########################
  dt_ref: 0.3                  # 期望的轨迹点时间间隔（秒）
  dt_hysteresis: 0.1
  global_plan_overwrite_orientation: true # 覆盖全局路径的朝向，使朝向与运动方向一致
  max_global_plan_lookahead_dist: 3.0    # 前瞻全局路径的最大距离
  feasibility_check_no_poses: 5           # 可行性检查的位姿数量

  # ##########################
  # 优化目标权重
  # ##########################
  weight_optimaltime: 5.0      # 时间最优权重（希望尽快到达）
  weight_obstacle: 50.0        # 避障权重
  weight_inflation: 0.2         # 膨胀层权重
  weight_dynamic_obstacle: 10.0 # 动态障碍物权重（仿真中暂不重要）
  weight_velocity: 1.0         # 速度平滑权重
  weight_acceleration: 2.0      # 加速度平滑权重

  # ##########################
  # 速度与加速度限制
  # ##########################
  max_vel_x: 0.7               # 最大前进速度，略低于move_base的全局限制
  max_vel_x_backwards: 0.2     # 最大倒车速度
  max_vel_theta: 0.5           # 最大转向角速度
  acc_lim_x: 0.5               # 线加速度限制
  acc_lim_theta: 0.6           # 角加速度限制

  # ##########################
  # 目标点容差
  # ##########################
  xy_goal_tolerance: 0.15
  yaw_goal_tolerance: 0.1
  free_goal_vel: true          # 允许以非零速度到达目标点（更自然）

这份配置文件中，最核心的是 robot_type: carlike 和 wheelbase 参数。carlike 模式让TEB知道我们的机器人是汽车模型，优化时会考虑非完整约束（即不能横向移动）。wheelbase（轴距）必须与你的URDF模型里前后轮的距离一致，这是计算转弯半径和转向角关系的关键。min_turning_radius 则是由最大转向角计算出来的物理极限，规划出的路径转弯半径不能小于这个值。

weight_optimaltime 和 weight_obstacle 是两个需要权衡的权重。如果你希望机器人尽快到达，可以增大 weight_optimaltime；如果环境比较拥挤，需要更保守的避障，就增大 weight_obstacle。我给出的值是一个比较均衡的起点。

另一个容易出问题的是 free_goal_vel。如果设为 false，TEB会强制规划一条使机器人在目标点速度为零的轨迹，这有时会导致机器人在接近目标时产生不必要的倒车调整。设为 true 后，只要位置和朝向在容差范围内，即使还有一点速度也认为到达目标，行为更自然，但停车可能没那么“准”。

5. 最终整合与实战：启动完整导航并调试

万事俱备，只欠东风。现在我们把所有部分整合到一个最终的导航启动文件中。创建 racebot_navigation.launch：

<!-- racebot_navigation/launch/racebot_navigation.launch -->
<launch>
  <!-- 仿真环境与机器人 -->
  <include file="$(find racebot_gazebo)/launch/racebot.launch"/>

  <!-- 地图服务器 -->
  <arg name="map_file" default="$(find racebot_gazebo)/maps/room_mini.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- AMCL 定位 -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <rosparam file="$(find racebot_navigation)/config/amcl_params.yaml" command="load"/>
  </node>

  <!-- Move Base 导航栈 -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!-- 加载通用代价地图参数 -->
    <rosparam file="$(find racebot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find racebot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />

    <!-- 加载全局/局部代价地图专属参数 -->
    <rosparam file="$(find racebot_navigation)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find racebot_navigation)/config/local_costmap_params.yaml" command="load" />

    <!-- 加载move_base基础参数 -->
    <rosparam file="$(find racebot_navigation)/config/base_local_planner_params.yaml" command="load" />

    <!-- 指定全局规划器 (默认使用navfn) -->
    <param name="base_global_planner" value="navfn/NavfnROS"/>

    <!-- 指定本地规划器为TEB，并加载其参数 -->
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>
    <rosparam file="$(find racebot_navigation)/config/teb_local_planner_params.yaml" command="load" />

    <!-- 重映射话题，确保与我们的机器人控制接口匹配 -->
    <remap from="cmd_vel" to="/racebot/cmd_vel"/> <!-- 发布到我们的控制器 -->
    <remap from="odom" to="/odom"/>               <!-- 订阅里程计话题 -->
  </node>

  <!-- Rviz -->
  <node pkg="rviz" type="rviz" name="rviz" required="true"
        args="-d $(find racebot_navigation)/rviz/navigation.rviz"/>
</launch>

现在，激动人心的时刻到了。在终端运行：roslaunch racebot_navigation racebot_navigation.launch。等待所有节点启动完毕，Gazebo和Rviz界面都正常显示。

第一步，初始化定位。如果粒子云没有自动收敛，在Rviz顶部工具栏点击 2D Pose Estimate，然后在地图上小车实际所在的大概位置点击并拖拽，给出一个初始朝向。观察粒子云是否快速收敛到机器人身上。

第二步，发送导航目标。点击Rviz工具栏的 2D Nav Goal，然后在地图上任意你想让机器人去的位置点一下，并拖拽出机器人到达时的最终朝向（箭头方向）。你会看到：

1. 一条绿色的全局路径（navfn 规划出的粗略路径）从机器人脚下延伸到目标点。
2. 一条红色的局部轨迹（TEB 规划出的优化轨迹）紧贴着绿色路径，并且在靠近障碍物时会自动弯曲绕开。
3. 机器人开始自动运动！转向轮会根据轨迹自动调整角度，后轮驱动前进。

常见问题与调试技巧：

1. 机器人不运动，或者规划失败：首先检查 Rviz 中 MoveBase 的状态。在 Rviz 左侧 Displays 面板，添加一个 RobotModel，并确保能看到 /move_base/global_costmap 和 /move_base/local_costmap。查看终端是否有红色错误信息。最常见的原因是话题不对应，检查 launch 文件中的 remap 是否正确，特别是 cmd_vel 是否发布到了你机器人控制节点订阅的话题上（我们之前用的是 /racebot/cmd_vel）。

2. 轨迹很奇怪，比如原地转圈或画弧线：这通常是TEB参数问题。检查 robot_type 和 wheelbase 是否正确。如果 min_turning_radius 设置得太大，机器人可能无法执行某些小弯，导致规划失败。可以尝试在Rviz中发布一个离机器人较近的目标，观察局部轨迹的曲率。

3. 在目标点来回震荡，无法停止：调整 xy_goal_tolerance 和 yaw_goal_tolerance，适当增大容差。同时检查 TebLocalPlannerROS 中的 weight_optimaltime 是否过高，导致机器人过于“急躁”而无法精确停车。也可以尝试将 free_goal_vel 设为 false，强制零速到达。

4. 遇到障碍物不绕行，或者撞上虚拟障碍物：检查局部代价地图是否正常更新。在 Rviz 中确保 LaserScan 的显示是正常的，并且 local_costmap 中的障碍物层能正确地将激光数据转换为障碍物。调整 inflation_radius（膨胀半径）和TEB的 weight_obstacle（避障权重）。

调试过程可能需要反复进行。我的习惯是，先在一个开阔区域测试点到点导航是否顺畅，然后再让机器人尝试通过狭窄的通道或绕过角落。每次只修改一两个参数，观察机器人的行为变化。Rviz 是强大的调试工具，多利用它显示各种可视化信息，比如代价地图、规划轨迹、粒子云等，能帮你快速定位问题所在。

当你的阿克曼小车能够流畅地从房间一角自动驾驶到另一角，并优雅地绕过桌椅（仿真世界里的障碍物）时，那种成就感是非常棒的。这标志着你的仿真机器人已经具备了最核心的自主移动能力。从键盘控制到建图，再到如今的完全自主导航，我们一步步搭建了一个完整的移动机器人软件栈。这不仅仅是Gazebo仿真的胜利，更是你对ROS导航生态理解的一次飞跃。接下来，你可以尝试更换不同的仿真环境地图，或者添加动态障碍物，看看你的小车能否应对更复杂的场景。