3. 坐标系与空间变换：机器人坐标系定义与变换
做机器人控制，说白了就是跟坐标系打交道。我见过不少新手，算法写得漂亮，但一上真机就翻车，十有八九是坐标系搞混了。今天咱们就把这个基础打牢。
3.1 三大坐标系：世界、机体、传感器
一个机器人系统里，至少有三个坐标系在同时工作。你得清楚每个时刻数据来自哪个坐标系，又要变换到哪个坐标系去。
| 坐标系 | 原点位置 | 典型用途 |
|---|---|---|
| 世界坐标系 (World Frame) | 固定于环境某点（如起点、墙角） | 全局路径规划、地图构建、位置 |
| 机体坐标系 (Body Frame) | 机器人质心或几何中心 | 运动控制、IMU 姿态、底盘速度 |
| 传感器坐标系 (Sensor Frame) | 传感器自身安装位置 | 激光雷达点云、相机图像、力传感器读数 |
核心原则：所有传感器数据最终都要统一到机体坐标系或世界坐标系，才能用于决策和控制。否则就是鸡同鸭讲。
举个例子。你车上的激光雷达装在右前方，它测到一个障碍物在它自己的坐标系里是 (1.0, 0.5)。如果你直接用这个值去避障，车会往左打方向——但实际障碍物在车的正前方偏右。为什么？因为没做坐标变换。
3.2 姿态表示：欧拉角与四元数
描述一个坐标系相对于另一个坐标系的旋转，主要有两种方式。我个人的习惯是：人脑用欧拉角，代码用四元数。
3.2.1 欧拉角
欧拉角用三个角度表示旋转：绕 X 轴（横滚 Roll）、绕 Y 轴（俯仰 Pitch）、绕 Z 轴（偏航 Yaw）。直观，但有个大坑——万向锁。
我曾经在调试一个六轴机械臂时，用欧拉角做逆解，结果在 Pitch 接近 90° 时，Roll 和 Yaw 突然耦合，末端执行器直接飞了。那次之后，只要涉及连续旋转，我必用四元数。
欧拉角的旋转顺序也很关键。常见的顺序有 ZYX、ZYZ 等。不同顺序得到的结果完全不同。你想想看，先绕 Z 转 30° 再绕 Y 转 45°，跟先绕 Y 再绕 Z，能一样吗？
3.2.2 四元数
四元数用四个数表示旋转：一个实部 + 三个虚部。形式是 q = w + xi + yj + zk，其中 w² + x² + y² + z² = 1。
它没有万向锁问题，插值平滑，计算效率高。嗯，这里要注意：四元数虽然好，但人眼直接看四个数根本看不出姿态。所以我一般在调试界面里把四元数转成欧拉角显示。
小技巧：ROS 里用 tf::Quaternion 或 Eigen::Quaterniond。转换函数随手可得，千万别自己手写乘法——我见过太多人把四元数乘法顺序搞反了。
3.3 坐标变换矩阵
坐标变换的核心就是 4×4 齐次变换矩阵。它把旋转和平移打包在一起：
| R t |
| 0 1 |其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。一个点从坐标系 A 变换到坐标系 B，就是左乘这个矩阵。
我记得在做一个 AGV 项目时，需要把两个激光雷达的数据融合。一个装在车头左前，一个装在车尾右后。我写了个变换链：
// 传感器数据 → 机体坐标系
T_body_lidar1 = T_body_base * T_base_lidar1
T_body_lidar2 = T_body_base * T_base_lidar2
// 机体坐标系 → 世界坐标系
T_world_lidar1 = T_world_body * T_body_lidar1你看，这就是典型的链式变换。每个环节的矩阵都是通过标定或测量得到的固定值。
3.4 知识体系总览
下面这张图把本章的核心逻辑串起来了。建议你保存下来，每次做坐标变换前瞄一眼。
3.5 实战中的坐标系管理
在实际项目中，我建议你遵循几条铁律：
- 每个传感器数据都带上坐标系标签。比如在 ROS 里用
frame_id字段。我见过有人偷懒不写，结果数据回放时根本不知道是谁的数据。 - 变换树（TF Tree）要维护好。所有坐标系之间的变换关系，用树状结构管理。不要出现循环依赖。
- 定期标定传感器外参。机器人用久了，螺丝松动、碰撞都会导致安装位置偏移。我曾经因为一个激光雷达歪了 2°，导致 AGV 在狭窄通道里反复撞墙。
调试技巧：在 Rviz 里把各个坐标系显示出来，用箭头或坐标轴可视化。一眼就能看出变换对不对。如果传感器数据出现在莫名其妙的位置，十有八九是变换矩阵错了。