2、导航核心概念：代价地图、规划器与TF树

各位同学，欢迎来到第二章。上一章我们搭好了环境，这一章我们来啃几个硬骨头——代价地图、全局/局部规划器，还有TF树。这三个概念，说白了就是导航系统的“眼睛”、“大脑”和“骨架”。

我在项目里见过不少新手，代码跑通了但一换场景就撞墙，十有八九是没搞懂这三者的协作关系。今天咱们就把它们掰开揉碎，讲清楚。

2.1 代价地图（Costmap）的构成与更新机制

代价地图是什么？简单说，就是一张给机器人看的“风险热力图”。每个格子都标了一个值，告诉机器人：这里能走，那里有坑，那边是悬崖。

2.1.1 两层地图结构

Nav2里维护着两张代价地图：

全局代价地图（Global Costmap）：用于全局路径规划。分辨率可以低一些，范围覆盖整个已知地图。
局部代价地图（Local Costmap）：用于实时避障。分辨率高，范围小，跟着机器人移动。

我个人习惯把全局地图想象成“城市交通图”，局部地图则是“当前路口的实时路况”。两者缺一不可。

2.1.2 代价值的含义

每个栅格的值范围是0~255，但实际我们只关心几个关键阈值：

值范围	含义	颜色（RViz中）
0	自由空间，随便走	黑色
1~49	低风险区域，可通行	深灰
50~97	中等风险，建议绕行	浅灰
98~127	高风险，接近障碍物	白色
128~252	致命障碍物，不可通行	亮白
253~255	未知空间	灰色

避坑指南：我曾经把膨胀半径设得太小，结果机器人贴着墙走，稍微有点定位误差就撞上了。后来我养成了一个习惯：膨胀半径至少是机器人半径的1.2倍。

2.1.3 更新机制：从传感器到代价地图

代价地图不是静态的。它会根据传感器数据不断刷新。流程是这样的：

传感器数据输入：激光雷达、深度相机等发布点云或LaserScan消息。
障碍物标记：将传感器检测到的点投影到地图坐标系，标记为障碍物。
膨胀处理：以障碍物为中心，向外逐层增加代价，形成“安全缓冲区”。
滚动更新：局部地图会随着机器人移动，丢弃远处的旧数据，加载前方的新数据。

嗯，这里要注意：更新频率很关键。我见过有人把更新频率设到100Hz，结果CPU直接拉满。一般10Hz就够用了。

2.1.4 核心参数配置示例

# global_costmap_params.yaml
global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 5.0    # 全局地图更新慢一点没关系
  publish_frequency: 2.0
  resolution: 0.05         # 5cm分辨率
  inflation_radius: 0.5    # 膨胀半径0.5米
  plugins:
    - {name: static_layer, type: "nav2_costmap_2d::StaticLayer"}
    - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"}

# local_costmap_params.yaml
local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 10.0   # 局部地图更新快一些
  publish_frequency: 5.0
  resolution: 0.025        # 2.5cm高分辨率
  inflation_radius: 0.4
  plugins:
    - {name: obstacle_layer, type: "nav2_costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"}

注意：全局地图的global_frame是map，局部地图的global_frame是odom。这个区别很多人会搞混，导致地图错位。

2.2 全局规划器与局部规划器的职责划分

规划器是导航的“大脑”。但一个大脑不够用，我们拆成两个：一个负责“想远路”，一个负责“走好眼前的路”。

2.2.1 全局规划器（Global Planner）

它的任务很简单：给定起点和终点，在全局代价地图上找一条无碰撞的路径。

常用算法：A*、Dijkstra、Theta*
输出：一系列路径点（Path消息）
特点：计算频率低，路径平滑度一般，不考虑动力学约束

你想想看，全局规划器就像你出门前看地图规划路线。它不知道路上会不会突然有辆车停着，但它知道大方向怎么走。

2.2.2 局部规划器（Local Planner）

局部规划器负责“跟着全局路径走，同时避开动态障碍物”。

常用算法：DWA（动态窗口法）、TEB（时间弹性带）、MPC（模型预测控制）
输出：速度指令（Twist消息）
特点：高频计算，考虑机器人运动学约束，实时避障

局部规划器就像你开车时的“实时决策”——前面有车变道了，你打方向盘避让，但大方向还是跟着导航走。

2.2.3 两者的协作流程

我用一张图来说明它们怎么配合：

关键点：当局部规划器发现无法跟随全局路径时（比如被障碍物堵死），它会触发“全局重规划”请求。这个机制在Nav2中通过computePathToPose服务实现。

2.3 TF树在导航中的作用

TF树，很多人觉得它就是个坐标变换工具。其实它更像是导航系统的“骨架”——没有它，所有数据都是散的。

2.3.1 核心TF关系

一个典型的导航机器人，TF树长这样：

map → odom → base_footprint → base_link → laser_link
                                    ↓
                                camera_link

map → odom：由AMCL或SLAM维护，表示机器人在世界坐标系中的位姿估计
odom → base_footprint：由里程计维护，表示机器人相对于起始点的运动
base_footprint → base_link：通常是一个固定变换，表示机器人中心到物理底盘的偏移
base_link → sensor_link：固定变换，表示传感器在机器人上的安装位置

2.3.2 为什么TF这么重要？

我举个例子你就明白了。激光雷达扫描到一堵墙，返回的数据是laser_link坐标系下的点云。但代价地图需要知道这堵墙在map坐标系下的位置。没有TF树，这个转换就做不了。

说白了，TF树就是让所有传感器数据、控制指令、地图信息能在同一个“语言”下对话。

2.3.3 常见TF问题排查

我曾经踩过的坑：有一次机器人导航时突然原地打转，查了半天发现是base_footprint到base_link的变换写反了，导致机器人以为自己一直在原地转圈。用tf2_echo工具一查就现原形了。

排查TF问题，我一般用这几个命令：

# 查看完整TF树
ros2 run tf2_tools view_frames.py

# 查看两个坐标系间的变换
ros2 run tf2_ros tf2_echo map base_footprint

# 实时监控TF延迟
ros2 run tf2_ros tf2_monitor

2.3.4 TF与代价地图的关系

代价地图的更新，每一步都依赖TF：

传感器数据到达，坐标系是laser_link
通过TF查询laser_link → odom的变换
将点云转换到odom坐标系
再通过odom → map变换（如果是全局地图）
最终将障碍物标记到代价地图的对应栅格

每一步的TF延迟都会影响地图的准确性。我建议TF的发布频率至少50Hz，延迟不要超过20ms。

小技巧：如果你发现代价地图上的障碍物位置总是偏的，先别急着调参数。用rviz2同时显示map、odom和base_link坐标系，看看它们之间的相对位置是否合理。很多时候是TF树没搭对。

好了，这一章的内容就到这里。代价地图、规划器、TF树，这三者构成了导航系统的核心骨架。下一章我们会深入AMCL定位，看看机器人是怎么知道自己“在哪里”的。