第1章：环境建模与坐标变换

做路径规划，第一步不是写算法，而是搞清楚「我在哪儿，要去哪儿」。

这话听起来像废话，但我刚入行时真在这上面栽过跟头。有一次在ROS里做导航，地图明明建好了，机器人就是往墙上撞。排查了半天，发现是栅格坐标和物理坐标的转换矩阵写反了。嗯，坐标变换这事，看似基础，但坑不少。

1.1 全局坐标系与局部坐标系

先说说坐标系。路径规划里，我们至少需要两套坐标系：

• 全局坐标系：固定在地面上的世界坐标系。通常用 map 或 odom 表示。机器人走到哪儿，这个坐标系都不动。
• 局部坐标系：绑定在机器人身上的坐标系。通常叫 base_link 或 base_footprint。机器人转个身，这个坐标系也跟着转。

你想想看，传感器（比如激光雷达）测到的障碍物距离，是在局部坐标系下的数据。但路径规划需要在全局坐标系下做决策。所以，必须把局部数据转换到全局坐标系下。

我个人习惯把全局坐标系想象成「地图上的经纬度」，局部坐标系想象成「我手机上的指南针」。手机转个方向，指南针跟着转，但经纬度不变。

1.2 坐标变换矩阵

坐标变换靠什么？靠矩阵乘法。

一个二维坐标点 (x, y) 从局部坐标系变换到全局坐标系，需要知道两件事：

• 局部坐标系原点在全局坐标系中的位置 (tx, ty)
• 局部坐标系相对于全局坐标系的旋转角度 θ

变换公式长这样：

[x_global]   [cosθ  -sinθ  tx] [x_local]
[y_global] = [sinθ   cosθ  ty] [y_local]
[1       ]   [0      0     1] [1       ]

这就是齐次坐标变换矩阵。为什么最后加个1？为了把平移和旋转统一成一次矩阵乘法。我在项目中遇到过，有人忘了加这个1，结果平移量死活算不对。

1.3 栅格坐标与物理坐标的转换

栅格地图说白了就是一张二维数组。数组的索引 (row, col) 是栅格坐标，而机器人实际位置是物理坐标 (x, y)。两者怎么转换？

假设栅格地图的分辨率是 resolution（单位：米/像素），地图原点在物理坐标系中的位置是 (origin_x, origin_y)。那么：

# 栅格坐标 → 物理坐标
x_physical = col * resolution + origin_x
y_physical = row * resolution + origin_y

# 物理坐标 → 栅格坐标
col = int((x_physical - origin_x) / resolution)
row = int((y_physical - origin_y) / resolution)

注意，这里的 row 对应的是 y 轴，col 对应的是 x 轴。我见过有人搞反了，结果路径规划出来的轨迹歪歪扭扭。

另外，地图原点 origin 通常不是 (0,0)。ROS 的 nav_msgs/OccupancyGrid 消息里，origin 字段表示地图左下角在物理坐标系中的位置。这个值可能是负的，别奇怪。

1.4 ROS中的坐标变换

ROS 里坐标变换有专门的工具——tf2 库。说白了，它维护了一棵坐标变换树，你只需要告诉它「我要从 A 坐标系变换到 B 坐标系」，它自动帮你查矩阵、做乘法。

举个例子，获取激光雷达数据在全局坐标系下的位置：

import tf2_ros
import tf2_geometry_msgs

# 初始化 tf2 监听器
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)

# 假设有一个激光点 point_laser，在 laser_frame 坐标系下
point_laser = PointStamped()
point_laser.header.frame_id = "laser_frame"
point_laser.point.x = 1.0
point_laser.point.y = 0.5

# 变换到 map 坐标系
try:
    point_map = tf_buffer.transform(point_laser, "map", timeout=rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
    rospy.logerr("坐标变换失败: %s", e)

你看，代码里不需要手动算矩阵。但有个前提——你的 tf 树必须完整。我曾经调试一个机器人，激光雷达数据死活变换不到 map 坐标系，后来发现是 base_link 到 laser_frame 的变换没发布。

1.5 实战中的常见问题

最后总结几个我踩过的坑：

1. 坐标系搞混：全局坐标系和局部坐标系的名字别乱起。ROS 里约定 map 是全局静态坐标系，odom 是里程计坐标系，base_link 是机器人本体坐标系。别自己发明名字。
2. 分辨率单位搞错：栅格地图的分辨率单位是米/像素。有人写成厘米/像素，结果地图缩放了100倍。
3. 原点偏移忘算：地图原点不一定在 (0,0)。转换时一定要加上 origin 偏移量。
4. tf 树不完整：用 tf2 前，先用 tf2_echo 命令行工具检查一下变换关系是否都存在。

嗯，这一章的内容就这些。坐标变换是路径规划的基石，基础打牢了，后面学 A*、Dijkstra 才不慌。下一章我们聊聊栅格地图的数据结构和读写方式。