2. 坐标系与空间建模
各位同学，今天我们来聊聊避障算法里最基础、也最容易被忽视的一环——坐标系与空间建模。
说实话，我刚开始做机器人那会儿，觉得坐标系这东西太简单了，不就是个x、y、z嘛。结果第一次做多传感器融合，激光雷达的数据怎么都对不上里程计，折腾了两天才发现——嗯，坐标系搞反了。从那以后，我每次写代码第一件事就是确认坐标系。
世界坐标系：机器人的“绝对参考系”
世界坐标系，说白了就是给整个环境定一个“绝对原点”。
你可以把它想象成一张地图的经纬度。不管机器人怎么动，世界坐标系是固定不动的。通常我们用 W 或 world 来表示。
关键点：世界坐标系的原点一般选在机器人启动位置，或者环境中的某个固定点（比如房间的角落）。
我个人习惯把世界坐标系的Z轴朝上，这样符合直觉。但有些项目里，Z轴朝下也有——比如无人机，Z轴朝下更方便计算高度。
举个例子：
// 世界坐标系下的一个障碍物位置
double obstacle_x_world = 5.2; // 米
double obstacle_y_world = 3.8; // 米
double obstacle_z_world = 0.0; // 地面高度机器人坐标系：跟着机器人跑的“移动参考系”
机器人坐标系，也叫本体坐标系，通常用 B 或 body 表示。
它的原点就在机器人中心，X轴朝前，Y轴朝左（或者朝右，看你的习惯），Z轴朝上。
为什么需要这个坐标系？因为传感器都装在机器人身上。激光雷达测出来的距离，天然就是机器人坐标系下的数据。
我的经验：在写避障算法时，我建议先把所有传感器数据统一到机器人坐标系下，再转换到世界坐标系。这样调试起来更清晰。
来看一个实际转换的例子：
// 机器人坐标系下的障碍物位置（激光雷达测得）
double obstacle_x_body = 2.0; // 前方2米
double obstacle_y_body = 0.5; // 右侧0.5米
// 机器人当前在世界坐标系中的位姿
double robot_x = 1.0;
double robot_y = 2.0;
double robot_theta = 0.3; // 弧度
// 转换到世界坐标系
double obstacle_x_world = robot_x + obstacle_x_body * cos(robot_theta) - obstacle_y_body * sin(robot_theta);
double obstacle_y_world = robot_y + obstacle_x_body * sin(robot_theta) + obstacle_y_body * cos(robot_theta);这段代码我写过无数遍。你想想看，如果坐标系搞反了，sin和cos用错，那障碍物位置就全乱了。
障碍物表示方法：从点到多边形
障碍物怎么表示？这取决于你的传感器和算法需求。
我遇到过三种最常见的表示方式：
- 点云表示：激光雷达扫出来的一堆点。精度高，但数据量大。
- 包围盒表示：用矩形或立方体框住障碍物。计算快，适合实时避障。
- 多边形表示：用多个顶点描述障碍物轮廓。精度和计算量的折中。
| 表示方法 | 精度 | 计算量 | 适用场景 |
|---|---|---|---|
| 点云 | 高 | 大 | 建图、SLAM |
| 包围盒 | 低 | 小 | 实时避障 |
| 多边形 | 中 | 中 | 路径规划 |
我曾经踩过的坑：用包围盒表示障碍物时，如果障碍物是斜着的，包围盒会多出很多空白区域。后来我改用旋转包围盒（OBB），效果好了不少。
坐标系转换：避障算法的“基本功”
坐标系转换说白了就是矩阵乘法。但实际写代码时，我建议你封装成函数，别每次都手算。
// 一个简单的2D坐标转换函数
void transformPoint2D(double x_body, double y_body,
double robot_x, double robot_y, double robot_theta,
double &x_world, double &y_world) {
x_world = robot_x + x_body * cos(robot_theta) - y_body * sin(robot_theta);
y_world = robot_y + x_body * sin(robot_theta) + y_body * cos(robot_theta);
}嗯，这里要注意：角度单位一定要统一。我见过有人混用度和弧度，结果障碍物飞到天上去。
知识体系总览
下面这张图，是我自己总结的坐标系与空间建模的核心逻辑：
这张图我画了好几次才满意。你看，从左到右，从世界坐标系到机器人坐标系，再到障碍物表示，整个流程是单向的。但实际项目中，我们经常需要来回转换——比如做路径规划时，要在世界坐标系下规划，然后转换到机器人坐标系下执行。
一个小技巧：写代码时，我习惯在变量名里带上坐标系后缀。比如 obs_x_body、obs_y_world。这样一眼就能看出数据在哪个坐标系下，不容易搞混。
好了，这一讲的内容就到这里。坐标系和空间建模是避障算法的地基，地基不稳，上层算法再牛也白搭。下一讲我们会聊聊传感器数据处理，到时候你会更深刻地体会到坐标系的重要性。
可可精品资料库