一、坐标变换基础：为什么机器人规划需要坐标变换？
做机器人规划这些年，我见过太多新手在坐标变换上栽跟头。说实话，这玩意儿看着简单，但真要上手调试，坑多得很。今天咱们就从最基础的东西聊起。
1.1 为什么非得搞坐标变换？
你想想看，机器人身上有那么多传感器——激光雷达、摄像头、IMU，每个都有自己的坐标系。激光雷达看到的是“前方2米有障碍”，但这个“前方”是雷达自己的前方，不是机器人的前方，更不是世界坐标系下的位置。
规划算法要的是什么呢？它要知道障碍物在世界坐标系下的位置，这样才能规划出一条安全的路径。所以，坐标变换说白了就是“翻译官”——把不同传感器看到的东西，统一到同一个坐标系下。
核心观点：没有坐标变换，规划算法就是个瞎子。传感器数据再多，对不上号也是白搭。
我在项目里遇到过这么个事：有一次调试AGV，激光雷达检测到前方有障碍，但规划模块死活不避障。查了半天，发现是雷达坐标系到机器人基坐标系的变换矩阵写反了。嗯，这种低级错误，犯过一次就长记性了。
1.2 世界坐标系 vs 机器人基坐标系
先搞清楚两个基本概念：
- 世界坐标系：固定在地面上的参考系，通常用W表示。地图、目标点、全局路径都在这个坐标系下。
- 机器人基坐标系：固定在机器人底盘上的参考系，通常用B表示。传感器、机械臂末端都相对于这个坐标系定义。
说白了，世界坐标系是“大地图”，机器人基坐标系是“我当前在哪”。规划算法需要把大地图上的目标点，换算成机器人“我该怎么走”的指令。
1.3 变换矩阵推导——从W到B
假设机器人在世界坐标系下的位置是 (x, y, θ)，其中θ是朝向角。那么从世界坐标系到机器人基坐标系的变换，包含两步：
- 旋转：绕Z轴旋转 -θ 角度
- 平移：平移 (-x, -y)
写成齐次变换矩阵就是：
T_W2B = [cosθ sinθ 0 -x*cosθ - y*sinθ]
[-sinθ cosθ 0 x*sinθ - y*cosθ]
[0 0 1 0 ]
[0 0 0 1 ]这个矩阵怎么来的？我习惯这么理解：先把世界坐标系旋转到和机器人朝向一致，再平移到机器人位置。顺序不能乱，先旋转后平移。
小技巧：实际写代码时，我建议用Eigen或tf2库来管理变换，别手写矩阵。手写容易出错，而且不好调试。我曾经手写过一次，结果符号搞反了，机器人原地转圈圈...
1.4 逆变换——从B到W
反过来，从机器人基坐标系到世界坐标系的变换，就是T_W2B的逆矩阵：
T_B2W = [cosθ -sinθ 0 x]
[sinθ cosθ 0 y]
[0 0 1 0]
[0 0 0 1]你看，逆变换其实就是先平移再旋转，和正变换正好相反。这个性质在调试时很有用——你可以用正变换算一遍，再用逆变换算回来，看能不能回到原点。能回来，说明矩阵写对了。
1.5 知识体系总览
下面这张图把本章的核心逻辑串起来了：
1.6 实际调试中的坑
坐标变换看着简单，但调试起来真能让人抓狂。我总结几个常见问题：
| 问题 | 现象 | 原因 | 解决办法 |
|---|---|---|---|
| 机器人原地转圈 | 规划路径时机器人不停旋转 | 旋转矩阵符号反了 | 检查sin/cos符号，用单位圆验证 |
| 位置偏移 | 机器人实际位置和规划位置对不上 | 平移量计算错误 | 用正逆变换互验 |
| 传感器数据错位 | 障碍物显示在错误位置 | 传感器到基坐标的变换缺失 | 检查变换链是否完整 |
避坑指南：我曾经在调试时发现，明明变换矩阵算对了，但机器人就是走不对。后来发现是时间戳没对齐——传感器数据是100ms前的，但变换矩阵用的是当前时刻的位姿。嗯，时间同步这个坑，后面章节会细讲。
1.7 小结
坐标变换是机器人规划的基石。说白了，就是把不同坐标系下的数据统一起来，让规划算法能看懂全局。今天咱们只讲了世界坐标系到机器人基坐标系的基础变换，后面会逐步深入。
记住一句话：变换矩阵写对了，规划就成功了一半。另一半是什么？嗯，是后面的内容了。