3、坐标与地图表示：全局坐标系与局部坐标系、栅格地图、拓扑地图、代价地图的构建
做路径规划这么多年，我越来越觉得一个道理：地图是算法的灵魂。你算法再牛，地图没建好，机器人照样撞墙。今天咱们就把地图这块彻底聊透。
3.1 全局坐标系与局部坐标系
先问个问题：你让机器人去厨房拿杯水，它怎么知道厨房在哪？
这里就涉及两个坐标系的概念。全局坐标系，说白了就是整个世界的"大地图"。比如你家客厅的东北角是原点，往东是X轴，往北是Y轴。厨房的坐标是(5.2, 3.8)。这个坐标是固定的，机器人重启了也不会变。
局部坐标系呢？它是跟着机器人跑的。机器人说"我前方1米有堵墙"，这个"前方"就是局部坐标系。原点在机器人中心，X轴朝前，Y轴朝左。
关键点：全局坐标系是"世界怎么看机器人"，局部坐标系是"机器人怎么看世界"。两者之间靠一个变换矩阵来转换。
我记得刚做第一个AGV项目时，就栽在这个坑里。传感器数据明明显示障碍物在左边0.5米，结果机器人直接撞上去了。查了半天，原来是局部坐标系下的数据没转成全局坐标。嗯，从那以后我每次写代码都会先确认坐标系转换对不对。
ROS里怎么处理？很简单，用tf库。你只需要发布一个transform，系统自动帮你算。
// C++示例：从局部坐标转到全局坐标
geometry_msgs::PointStamped local_point;
local_point.header.frame_id = "base_link"; // 局部坐标系
local_point.point.x = 1.0;
local_point.point.y = 0.5;
geometry_msgs::PointStamped global_point;
try {
tf_listener.transformPoint("map", local_point, global_point);
// 现在global_point里就是全局坐标了
} catch (tf::TransformException &ex) {
ROS_ERROR("坐标转换失败: %s", ex.what());
}我的习惯：所有传感器数据一进来，立刻转成全局坐标。这样后面所有模块都不用操心坐标系的事，省心很多。
3.2 栅格地图
栅格地图，你想想看，就是把真实世界切成一个个小格子。每个格子要么是空地（0），要么是障碍物（100），要么是未知（-1）。
为什么用栅格？因为简单、直观。路径规划算法（比如A*、Dijkstra）直接在上面跑，每个格子就是一个节点。
| 栅格值 | 含义 | 颜色 |
|---|---|---|
| 0 | 空闲区域 | 白色 |
| 100 | 障碍物 | 黑色 |
| -1 | 未知区域 | 灰色 |
栅格大小怎么选？这是个学问。我做过一个仓储机器人项目，一开始用5cm的栅格，地图精细是精细了，但计算量太大，CPU直接飙到90%。后来改成20cm，路径虽然粗糙了点，但实时性上去了。
避坑指南：我曾经因为栅格分辨率设得太高，导致机器人卡在走廊里出不来。原因是栅格太小，把门框的微小凸起都当成障碍物了。建议室内场景用10-20cm，室外场景可以放宽到50cm。
ROS里用nav_msgs/OccupancyGrid来表示栅格地图。发布一个地图话题，导航栈自动订阅。
// 创建一张10x10的栅格地图，分辨率0.1米
nav_msgs::OccupancyGrid grid;
grid.header.frame_id = "map";
grid.info.resolution = 0.1; // 每个格子0.1米
grid.info.width = 10;
grid.info.height = 10;
grid.info.origin.position.x = 0;
grid.info.origin.position.y = 0;
// 初始化所有格子为未知
grid.data.resize(10 * 10, -1);
// 设置障碍物：第5行第5列
grid.data[5 * 10 + 5] = 100;3.3 拓扑地图
栅格地图虽然好，但有个问题：太大了。一个100平米的房子，用10cm栅格，就是10000个格子。如果做全局路径规划，计算量不小。
拓扑地图就是来解决这个问题的。它只保留关键节点和连接关系。比如你家客厅是一个节点，走廊是一个节点，厨房是一个节点。节点之间用边连接，边上可以带长度信息。
说白了，拓扑地图就是一张"地铁线路图"。它不关心具体怎么走，只关心"从A站到B站要经过哪些站"。
实际应用：我在做商场导览机器人时，就用拓扑地图做全局规划。先找到从入口到目标店铺的节点序列，再用局部规划器处理每个节点间的细节路径。这样既快又准。
拓扑地图的数据结构很简单，就是一个图（Graph）。
// 拓扑地图节点
struct TopoNode {
int id; // 节点ID
double x, y; // 全局坐标
string name; // 节点名称，比如"客厅"
};
// 拓扑地图边
struct TopoEdge {
int from_id; // 起点
int to_id; // 终点
double cost; // 代价（距离或时间）
};
// 拓扑地图
class TopoMap {
vector<TopoNode> nodes;
vector<TopoEdge> edges;
// 用Dijkstra找最短路径
vector<int> findPath(int start, int goal);
};3.4 代价地图的构建
代价地图，这是路径规划里最核心的东西。它不是一个独立的地图类型，而是在栅格地图基础上加了"代价"信息。
什么叫代价？就是机器人经过某个格子时，要付出的"代价"。比如：
- 空地：代价0，随便走
- 靠近障碍物：代价高，尽量避开
- 障碍物本身：代价无穷大，不能走
- 地毯区域：代价低，但比空地高一点（因为摩擦力大）
ROS里的costmap_2d包就是干这个的。它把原始栅格地图膨胀一下，给障碍物周围加上"安全距离"。
我的经验：膨胀半径设多少合适？我一般设成机器人半径的一半。比如机器人半径0.3米，膨胀半径就设0.15米。这样既保证安全，又不会把路堵死。
代价地图的构建流程：
- 从传感器（激光雷达、深度相机）获取障碍物信息
- 更新到栅格地图中
- 对障碍物进行膨胀处理
- 生成最终的代价地图
// 在ROS中配置代价地图
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: true
inflation_layer:
enabled: true
inflation_radius: 0.15 # 膨胀半径
cost_scaling_factor: 10.0 # 代价衰减因子
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 10.0
publish_frequency: 5.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05这里要注意，全局代价地图和局部代价地图是分开的。全局的用静态地图，负责长期规划；局部的用滚动窗口，负责实时避障。
我曾经踩过的坑：有一次局部代价地图的更新频率设得太低（1Hz），机器人快速移动时，障碍物信息更新不及时，直接撞上了突然出现的行人。后来我把更新频率提到10Hz，问题就解决了。记住：速度越快，更新频率就要越高。
最后总结一下这四种地图的关系：
| 地图类型 | 用途 | 精度 | 计算量 |
|---|---|---|---|
| 全局坐标系 | 统一坐标基准 | 高 | 低 |
| 栅格地图 | 环境建模 | 中高 | 中 |
| 拓扑地图 | 全局路径规划 | 低 | 低 |
| 代价地图 | 路径搜索与避障 | 中 | 中高 |
嗯，地图这块内容就这些。你想想看，其实核心就一句话：用什么地图，取决于你要解决什么问题。做全局规划用拓扑，做局部避障用代价地图，做建图用栅格地图。搞清楚这个，你就入门了。
下一章我们聊路径搜索算法，A*和Dijkstra，到时候你会看到地图是怎么被算法"吃"进去的。