1. 从地图到导航：理解自主导航的完整闭环

上一节我们成功让小车在房间里跑了一圈，用gmapping建出了一张地图。看着Rviz里逐渐清晰的墙壁和走廊，是不是很有成就感？但建图只是第一步，就像你画好了一张藏宝图，下一步是让机器人自己拿着这张图，准确地找到宝藏的位置，并且规划一条安全又高效的路线走过去。这就是我们这一节要做的：让阿克曼小车在已知地图里实现自主导航。

很多新手朋友在做到这一步时会有点懵，感觉要配置的东西一下子多了起来。别担心，咱们一步步来。你可以把自主导航想象成一个“问路-带路”的组合。AMCL（自适应蒙特卡洛定位） 就是那个不断在问“我现在在地图的哪个位置？”的家伙。它通过对比激光雷达实时扫描到的环境特征和我们已经有的地图，来估算小车当前最可能在地图上的哪个点，以及它的朝向（也就是位姿）。而 TEB（Timed Elastic Band）本地规划器 则是那个负责带路的向导。它根据AMCL给出的“当前位置”和我们设定的“目标位置”，实时计算出一条既符合阿克曼小车运动学（比如不能原地转弯），又能避开动态或静态障碍物的平滑轨迹。

所以，整个流程的链条非常清晰：启动仿真环境 -> 加载上一节保存好的地图 -> 启动AMCL进行定位 -> 设置导航目标 -> TEB规划器开始工作，输出控制指令 -> 小车沿着指令移动到目标点。听起来是不是比纯理论好理解多了？接下来，我们就进入实战环节，我会把每个环节的配置细节、我踩过的坑以及调试心得都分享给你。

2. 加载地图与启动AMCL定位

2.1 准备地图与启动文件

首先，确保你上一节保存的地图文件（room_mini.pgm 和 room_mini.yaml）已经放在了正确的位置，比如 ~/catkin_ws/src/racebot/racebot_gazebo/maps/ 目录下。这两个文件是配套的，.pgm是图像数据，.yaml是地图的元信息，比如分辨率、原点坐标等。

我们需要创建一个启动文件来加载地图。在 racebot_gazebo/launch/ 目录下新建一个文件，命名为 amcl_navigation.launch。这个文件将是我们导航演示的总入口。一开始，我们先写加载地图的部分：

<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">
    <!-- 基本帧参数 -->
    <param name="odom_frame_id" value="odom"/>
    <param name="base_frame_id" value="base_footprint"/>
    <param name="global_frame_id" value="map"/>

    <!-- 初始位姿设置（可选的，如果你知道小车大概的起始位置） -->
    <param name="initial_pose_x" value="0.0"/>
    <param name="initial_pose_y" value="0.0"/>
    <param name="initial_pose_a" value="0.0"/>
    <param name="use_map_topic" value="true"/>

    <!-- 激光模型参数 -->
    <param name="laser_model_type" value="likelihood_field"/>
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/>

    <!-- 里程计模型参数（非常重要！） -->
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>
    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="odom_alpha5" value="0.1"/>

    <!-- 粒子滤波器参数 -->
    <param name="min_particles" value="100"/>
    <param name="max_particles" value="3000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="resample_interval" value="1"/>
  </node>
</launch>

这里我解释几个关键参数，它们直接决定了定位的精度和速度。odom_model_type 我设置成了 diff-corrected，这是针对差分驱动模型的，但对于我们这种速度控制的阿克曼小车，经过我的实测，它比 omni（全向）模型表现更稳定。odom_alpha1 到 odom_alpha5 这几个噪声参数需要根据你小车的实际运动精度来调整。如果你的仿真小车在Gazebo里运动时里程计噪声比较大（比如打滑），就需要适当调大这些值，告诉AMCL：“里程计不太准，你多依赖激光数据”。min_particles 和 max_particles 是粒子数量，粒子越多定位越准，但计算量也越大。对于我们的室内小场景，100到3000的区间是足够的。

2.2 在Rviz中验证AMCL定位效果

启动你的仿真环境和这个AMCL启动文件：

roslaunch racebot_gazebo racebot.launch
roslaunch racebot_gazebo amcl_navigation.launch

然后打开Rviz：

rosrun rviz rviz

在Rviz里，你需要添加几个关键的显示项：

1. Map：话题选择 /map，这样你就能看到加载的静态地图。
2. LaserScan：话题选择 /scan，显示小车激光雷达的实时数据。你会看到红色的扫描点覆盖在地图上。
3. PoseArray：话题选择 /particlecloud。这是AMCL定位的核心可视化！你会看到一堆绿色的小箭头（粒子）聚集在小车周围。这些粒子代表了AMCL认为小车可能处于的位置假设。定位良好时，这些粒子会紧密地聚集在小车的真实位置附近。如果粒子散得很开，说明定位不确定或者出错了。

现在，尝试在Rviz里用 2D Pose Estimate 工具（工具栏上的一个绿色箭头图标）给小车一个初始位姿估计。点击工具，然后在地图上小车大概的位置点一下，并拖拽出朝向。你会发现，/particlecloud 的粒子会迅速从分散状态收敛到你指定的位置附近。接着，你可以用键盘控制节点让小车缓慢移动。观察粒子的运动，它们应该紧紧跟随着小车，并且激光扫描点应该和地图的墙壁轮廓基本重合。如果激光点在地图墙壁“外面”或者“穿透”了墙壁，说明定位有漂移，可能需要回头检查里程计数据的准确性，或者调整我上面提到的AMCL噪声参数。

注意：AMCL的初始定位需要你提供一个大概的位姿估计。在真实机器人上，你可能不知道它在地图的哪里，这就需要通过遥控机器人转动，让激光扫描到一些独特的角落或形状，AMCL才能快速收敛。在仿真里，我们通常知道起始点，所以直接指定即可。

3. 为阿克曼小车配置TEB本地规划器

定位搞定了，接下来是规划。ROS最常用的 move_base 框架默认的局部规划器是 DWA，但它主要是为差分驱动机器人设计的，对阿克曼这种前轮转向的模型支持不好，规划出的路径常常要求小车做原地旋转，这阿克曼车根本做不到。所以我们必须换上专为汽车类模型优化的 TEB规划器。

3.1 安装与基础配置

首先，确保安装了TEB规划器。在Melodic下，安装命令是：

sudo apt-get install ros-melodic-teb-local-planner

接下来，我们需要为 move_base 创建配置文件。在 racebot_gazebo 功能包下，新建 config 文件夹，并在里面创建两个YAML文件：costmap_common_params.yaml 和 teb_local_planner_params.yaml。最后，我们还会创建一个 move_base.launch 文件把它们组织起来。

先看 costmap_common_params.yaml，它定义了代价地图的一些通用参数，比如障碍物层、膨胀层等，这些层会同时用于全局和局部代价地图：

obstacle_layer:
  enabled: true
  observation_sources: scan
  scan:
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true
    expected_update_rate: 0.5
    max_obstacle_height: 0.4
    min_obstacle_height: 0.0

inflation_layer:
  enabled: true
  inflation_radius: 0.3 # 膨胀半径，根据小车大小调整
  cost_scaling_factor: 5.0

# 机器人足迹定义（对于阿克曼车，通常用一个矩形近似）
footprint: [[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]]
# 也可以使用圆形，但矩形更符合阿克曼车实际形状
# robot_radius: 0.2

这里的关键是 footprint，它定义了机器人的轮廓。规划器会保证机器人的这个轮廓不与膨胀后的障碍物区域重叠。你需要根据你仿真小车模型的实际尺寸来修改这个多边形坐标。

3.2 详解TEB规划器的阿克曼参数

重头戏是 teb_local_planner_params.yaml。TEB规划器有大量参数，但我们重点关注与阿克曼运动学相关的部分：

TebLocalPlannerROS:
  # 1. 机器人运动学模型 - 必须修改！
  kinematics_type: "car-like" # 设置为汽车模型，这是关键！
  max_vel_x: 0.4           # 最大前进速度 (m/s)
  max_vel_x_backwards: 0.2 # 最大倒车速度
  max_vel_theta: 0.3       # 最大转向角速度 (rad/s)，对于阿克曼，这个参数影响不大
  acc_lim_x: 0.5           # 线加速度限制
  acc_lim_theta: 0.5       # 角加速度限制

  # 2. 阿克曼转向参数 - 核心！
  wheelbase: 0.3           # 轴距 (前后轮中心的距离)，必须与你的URDF模型一致！
  min_turning_radius: 0.5  # 最小转弯半径 (m)，由最大转向角决定。计算方式：轴距 / tan(最大转向角)
  cmd_angle_instead_rotvel: true # 发布转向角指令，而不是角速度。必须为true！
  max_steering_angle: 0.5  # 最大转向角 (rad)，例如0.5 rad ≈ 28.6度

  # 3. 轨迹与时间相关参数
  dt_ref: 0.3              # 期望的轨迹点时间间隔 (秒)
  dt_hysteresis: 0.1
  global_plan_overwrite_orientation: true
  max_global_plan_lookahead_dist: 3.0

  # 4. 障碍物与优化参数
  penalty_epsilon: 0.2
  weight_optimaltime: 5.0
  weight_obstacle: 50.0
  weight_kinematics_nh: 1000.0 # 非完整约束权重，对汽车模型很重要
  weight_kinematics_forward_drive: 10.0 # 鼓励向前行驶

  # 5. 恢复行为参数（当规划失败时）
  oscillation_recovery: true
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 2.0

这里我踩过最大的坑就是 kinematics_type 和 cmd_angle_instead_rotvel。如果你忘了把 kinematics_type 从默认的 diff 改成 car-like，规划器会以为你的小车是差分驱动的，规划出的路径根本没法跟踪。wheelbase（轴距）也必须准确，它直接影响了规划器对转弯几何的计算。min_turning_radius 是一个性能和安全参数，设置得太小，规划器可能会尝试让小车做过于急的转弯，导致控制失败；设置得太大，小车又可能在一些狭窄通道里规划不出路径。

cmd_angle_instead_rotvel: true 这个参数告诉规划器，输出的控制指令是转向角（geometry_msgs/Twist 消息中的 angular.z 字段将被解释为转向角），而不是角速度。这对于需要接收转向角指令的阿克曼控制器来说是必须的。

3.3 整合move_base启动文件

最后，我们创建 move_base.launch 文件，将代价地图、全局规划器（通常用 global_planner 或 navfn）、本地规划器（TEB）整合到一起：

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

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

    <!-- 加载全局规划器参数 (使用navfn) -->
    <rosparam file="$(find racebot_gazebo)/config/global_planner_params.yaml" command="load" />

    <!-- 加载本地规划器参数 (TEB) -->
    <rosparam file="$(find racebot_gazebo)/config/teb_local_planner_params.yaml" command="load" />

    <!-- 指定全局和本地规划器插件 -->
    <param name="base_global_planner" value="navfn/NavfnROS"/>
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>

    <!-- 设置坐标变换容忍时间 -->
    <param name="controller_frequency" value="10.0"/>
    <param name="planner_patience" value="5.0"/>
    <param name="controller_patience" value="15.0"/>
    <param name="recovery_behavior_enabled" value="true"/>
  </node>
</launch>

你还需要创建对应的 global_costmap_params.yaml、local_costmap_params.yaml 和 global_planner_params.yaml。前两个主要定义代价地图的尺寸、更新频率和坐标系，global_planner_params.yaml 可以保持默认或简单设置一下。这里篇幅所限不展开，你可以在ROS的 nav_core 和 teb_local_planner 教程里找到模板。

4. 全系统集成与导航调试实战

4.1 启动全系统并发送导航目标

现在，我们把所有部分整合到最初的 amcl_navigation.launch 文件中：

<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 -->
  <include file="$(find racebot_gazebo)/launch/amcl.launch" />

  <!-- 启动move_base -->
  <include file="$(find racebot_gazebo)/launch/move_base.launch" />

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

用一个命令启动所有节点：

roslaunch racebot_gazebo amcl_navigation.launch

同时，别忘了在另一个终端启动你的Gazebo仿真世界：

roslaunch racebot_gazebo racebot.launch

在Rviz中，确保你能看到地图、激光扫描、粒子云、机器人的足迹（footprint）、全局代价地图（通常是绿色的障碍物膨胀区）和局部代价地图。然后，使用Rviz工具栏上的 2D Nav Goal 工具（一个红色的圆环箭头）。在地图上任意一点点击并拖拽，设定一个目标位置和朝向。这时，你应该会看到：

1. 一条全局路径（通常是蓝色实线）从机器人当前位置连接到目标点。这条路径由全局规划器生成，只考虑静态地图，可能穿过膨胀区，因为它只是粗略参考。
2. 一条局部规划路径（通常是绿色实线或点线）。这是TEB规划器根据全局路径、当前位姿、实时激光数据计算出的、符合阿克曼运动学的实际跟踪轨迹。这条轨迹应该是平滑的弧线，而不是包含尖角的折线。
3. 小车开始自动运动，尝试跟踪绿色轨迹。

4.2 常见问题与调试技巧

事情很少有一次就成功的。下面是我在调试中遇到的几个典型问题及解决方法：

问题一：小车收到目标后不动，或者控制台报错 “Invalid Trajectory”。

• 检查：首先看 rostopic echo /move_base/feedback 和 rostopic echo /move_base/status，查看规划状态。更直接的是看TEB规划器的输出：rostopic echo /move_base/TebLocalPlannerROS/teb_poses。如果这个话题没有数据，或者数据异常，说明规划器内部优化失败了。
• 可能原因1：运动学参数不匹配。再次确认 kinematics_type 是 car-like，wheelbase 是否与模型一致。min_turning_radius 是否设置得过小，导致规划器试图规划一个物理上无法实现的转弯。
• 可能原因2：控制指令话题不匹配。TEB规划器默认发布 cmd_vel 话题。检查你的小车控制器订阅的话题名是什么。如果不一致，需要在 teb_local_planner_params.yaml 中设置 cmd_vel_topic 参数，例如 cmd_vel_topic: /racebot/cmd_vel。

问题二：小车能规划路径，但运动起来非常抖动，或者总是偏离路径。

• 检查：观察绿色局部轨迹是否在频繁、剧烈地跳动。查看 rostopic hz /odom，确保里程计更新频率稳定（比如20Hz）。频率太低会导致规划器基于过时的位姿做决策。
• 调整：尝试降低 max_vel_x 和 acc_lim_x，让运动更平缓。增加 weight_kinematics_nh 和 weight_kinematics_forward_drive 的权重，强化运动学约束。也可以微调 penalty_epsilon 和 weight_obstacle，改变机器人与障碍物之间的距离偏好。

问题三：在狭窄通道或门口，小车卡住不动，规划失败。

• 检查：局部代价地图的膨胀半径 inflation_radius 是否设置过大？机器人的 footprint 定义是否比实际模型大？这会导致规划器认为空间不足。
• 调整：适当减小 inflation_radius。确保 footprint 精确。可以尝试在TEB参数中启用 enable_homotopy_class_planning（设为 true），并设置 max_number_classes（如3或5）。这个功能允许规划器同时考虑多条拓扑路径（比如从左边绕还是右边绕），在复杂环境中更容易找到可行解。

问题四：AMCL定位突然丢失，粒子云散开。

• 检查：激光数据是否正常？/scan 话题有没有发布？地图和实际环境是否严重不符（比如仿真世界里的物体被移动了）？
• 操作：立即用Rviz的 2D Pose Estimate 工具重新给一个位姿估计。如果频繁丢失，考虑增加AMCL的 max_particles 数量，或者检查里程计数据的协方差设置是否合理（上一节里程计代码中的 pose.covariance 和 twist.covariance）。过小的协方差值会让AMCL过于信任里程计，一旦里程计跳变，定位就会跟丢。

调试的过程就是不断观察（Rviz可视化）、分析（查看话题和日志）、调整参数、再验证的过程。不要怕麻烦，每解决一个问题，你对整个导航栈的理解就会加深一层。当你第一次看到阿克曼小车流畅地绕过障碍，平滑地驶向你设定的目标点时，那种感觉绝对值得之前的付出。