第四节：坐标系与空间建模
做机器人避障，说白了就是回答三个问题：我在哪？障碍物在哪？怎么过去？ 这三个问题都绕不开坐标系。我见过不少新手，代码写得挺漂亮，结果机器人一跑就撞墙——查到最后，往往是坐标系搞反了。
这一节，咱们就把坐标系这件事彻底聊透。
一、机器人坐标系定义
机器人身上有多个坐标系，最常用的就三个：
- 世界坐标系（world）：固定在地面上的全局参考系。通常以机器人启动位置为原点，X轴向前，Y轴向左，Z轴向上（右手系）。
- 机器人本体坐标系（base_link）：固定在机器人底盘中心。X轴指向机器人正前方，Y轴向左，Z轴向上。这个坐标系会随着机器人移动而移动。
- 传感器坐标系（laser_frame / camera_frame）：固定在激光雷达或摄像头等传感器上。注意，传感器安装位置不同，坐标系原点也不同。
核心原则：所有传感器数据最终都要转换到 base_link 或 world 坐标系下，才能用于避障决策。
我在项目中遇到过最典型的错误：激光雷达装在机器人后方，有人直接拿原始数据做避障，结果机器人一直往后倒。嗯，这就是没做坐标系转换的后果。
二、障碍物坐标转换
坐标转换其实就一个公式：
P_world = T_world_base * P_base其中 T 是 4x4 的齐次变换矩阵，包含旋转和平移。ROS 里用 tf2 库帮我们搞定了这件事。
举个例子，激光雷达检测到前方 1 米处有个障碍物，但雷达装在机器人左前方 0.3 米处。你想想看，这个障碍物相对于机器人中心的位置是多少？
# Python 示例：使用 tf2 进行坐标转换
import tf2_ros
import tf2_geometry_msgs
# 假设已经获取到激光雷达坐标系下的点
point_laser = PointStamped()
point_laser.header.frame_id = "laser_frame"
point_laser.point.x = 1.0
point_laser.point.y = 0.0
point_laser.point.z = 0.0
# 转换到 base_link 坐标系
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
try:
point_base = tf_buffer.transform(point_laser, "base_link", timeout=rospy.Duration(1.0))
print(f"障碍物在机器人坐标系下的位置: ({point_base.point.x:.2f}, {point_base.point.y:.2f})")
except Exception as e:
rospy.logerr("坐标转换失败: %s", str(e))我的习惯：每次做坐标转换前，先打印一下当前 tf 树，确认所有坐标系都在线。用 tf2_echo 命令可以实时查看。
三、栅格地图（Grid Map）基础
栅格地图，说白了就是把环境切成一个个小格子。每个格子有三种状态：
- 空闲（0）：可以通行
- 占用（100）：有障碍物
- 未知（-1）：还没探测到
为什么用栅格？因为简单、直观、好计算。我最早做避障时试过用连续空间建模，结果碰撞检测算到死机。换成栅格地图后，问题迎刃而解。
栅格地图的核心参数：
| 参数 | 说明 | 典型值 |
|---|---|---|
| 分辨率 | 每个格子代表的实际尺寸 | 0.05 m（5 cm） |
| 宽度 | 地图的格子列数 | 1000 |
| 高度 | 地图的格子行数 | 1000 |
| 原点 | 地图左下角对应的世界坐标 | (-25, -25) |
我曾经踩过的坑：分辨率设得太高（比如 0.01 m），地图太大导致内存爆掉。后来我总结了一个经验：对于室内机器人，0.05 m 分辨率足够；对于室外，0.1 m 就挺好。
四、从传感器数据到栅格地图
这个过程其实就三步：
- 获取传感器数据：激光雷达返回一系列距离值
- 坐标转换：把每个激光点从 sensor frame 转到 world frame
- 更新栅格：根据激光点位置，更新对应格子的占用概率
ROS 里的 costmap_2d 包帮我们封装好了这一切。你只需要配置好参数，它就能自动维护一张栅格地图。
# costmap_2d 的典型配置
global_costmap:
global_frame: map
robot_base_frame: base_link
resolution: 0.05
width: 40.0
height: 40.0
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}这里有个关键点：膨胀层（inflation_layer）。它会把障碍物周围的格子也标记为不可通行，相当于给障碍物穿了一层「防护服」。膨胀半径设多大？我一般设成机器人半径的 1.2 倍，这样既安全又不会太保守。
五、知识体系总览
下面这张图，是我自己梳理的坐标系与空间建模的核心逻辑。你把它理解了，后面做避障就顺了。
六、实战中的几个要点
最后，分享几个我这些年总结出来的经验：
- 坐标系命名要规范：ROS 社区约定俗成的名字（map, odom, base_link, base_footprint）别乱改，否则别人看不懂你的代码。
- 先验证再集成：每次改完坐标转换逻辑，先用
rviz可视化看看激光点云是否对齐。我见过有人调了一整天参数，结果发现是雷达装歪了。 - 栅格地图不是万能的：对于动态障碍物（比如行人），栅格地图更新速度可能跟不上。这时候需要结合局部代价地图做实时避障。
一个小技巧：调试时把 costmap_2d 的 publish_frequency 设高一点（比如 10 Hz），这样在 rviz 里能看到地图实时更新，方便排查问题。
好了，坐标系和栅格地图就聊到这儿。这些东西看着基础，但真到了实战中，80% 的避障问题都出在这里。你把它搞扎实了，后面学路径规划、动态避障都会轻松很多。
可可精品资料库