第1章：坐标系统与空间变换

大家好，我是你们这门课的老朋友。今天咱们聊聊多传感器融合里最基础、也最绕不开的一个话题——坐标系统与空间变换。

说实话，我刚开始做机器人那会儿，觉得坐标变换不就是个矩阵乘法嘛，有啥难的？结果第一次做激光雷达和摄像头融合，数据怎么都对不上。折腾了两天，最后发现是坐标系定义反了。嗯，从那以后，我再也不敢小看这个“基础”了。

1.1 为什么需要多个坐标系？

你想想看，一个机器人身上挂着激光雷达、摄像头、IMU、轮式里程计。每个传感器都有自己的“视角”。

• 激光雷达：以自己为中心，测量周围物体的距离和角度
• 摄像头：以镜头光心为原点，把三维世界投影到二维图像上
• IMU：测量自身加速度和角速度，坐标系跟着芯片走
• 机器人底盘：关心的是“我往哪走”，坐标系通常在前方

说白了，每个传感器都在用自己的“语言”描述世界。我们要做的，就是把这些语言统一翻译成机器人能理解的“世界语”。

1.2 三大核心坐标系

我个人习惯把坐标系分成三层：

1.2.1 世界坐标系

这是全局的、固定不动的参考系。通常我们定义：

• X轴指向东
• Y轴指向北
• Z轴指向天（右手系）

或者更常见的，在室内场景里，我们以房间的一个角落为原点。我在做仓储机器人项目时，就把充电桩位置设为世界坐标系原点，这样机器人每次回充都知道自己在哪。

1.2.2 机器人坐标系

也叫车体坐标系。原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝上。这个坐标系是跟着机器人动的。

为什么要单独搞一个？因为机器人控制指令（“往前走1米”）是在这个坐标系下发的。你不可能让机器人“在世界坐标系下往东走”，万一它转了个身呢？

1.2.3 传感器坐标系

每个传感器都有自己的小天地。比如摄像头：

• 原点在光心
• Z轴沿光轴向前
• X轴向右，Y轴向下（图像坐标系就是这么定义的）

我记得第一次标定摄像头时，被这个Y轴向下搞晕了。后来养成了习惯：拿到传感器第一件事，先看它的坐标系定义文档。

1.3 坐标变换的数学工具

好了，有了坐标系，怎么把一个点从传感器坐标系变到世界坐标系？

1.3.1 欧拉角

最直观的旋转表示方法。绕X轴转叫横滚角（Roll），绕Y轴转叫俯仰角（Pitch），绕Z轴转叫偏航角（Yaw）。

1.3.2 四元数

四元数用四个数表示旋转：q = w + xi + yj + zk。它没有万向锁，插值平滑，计算效率高。

说实话，四元数刚接触时挺抽象的。我有个小技巧：把它想象成“绕着某个轴转多少度”。轴是(x,y,z)，角度由w决定。这样就好理解多了。

// 四元数转旋转矩阵（C++示例）
Eigen::Quaterniond q(0.707, 0.0, 0.707, 0.0); // 绕Y轴转90°
Eigen::Matrix3d R = q.toRotationMatrix();
// R = [0, 0, 1; 0, 1, 0; -1, 0, 0]

1.3.3 齐次变换矩阵

把旋转和平移写到一个4×4矩阵里：

T = [R  t]
    [0  1]

其中R是3×3旋转矩阵，t是3×1平移向量。用齐次坐标，一个矩阵就能搞定旋转+平移，还能连续变换。

1.4 手眼标定基础

这是传感器融合里最实操的一步。说白了，就是求传感器坐标系和机器人坐标系之间的变换关系。

分两种情况：

• 眼在手上（Eye-in-Hand）：摄像头装在机械臂末端，跟着臂走
• 眼在手外（Eye-to-Hand）：摄像头固定，看机械臂运动

我做过一个焊接机器人项目，用的是眼在手上。标定过程就是解一个AX=XB的方程。听起来简单，但实际做的时候，数据采集、噪声处理、奇异值分解，每一步都有坑。

1.5 知识体系总览

下面这张图，是我自己梳理的本章知识结构。建议你保存下来，后面学完再回来看，会有更深的理解。

1.6 实战中的小建议

最后，分享几个我踩过的坑：

1. 坐标系定义一定要写在代码注释里。我见过一个项目，三个人用了三种坐标系定义，最后融合出来的轨迹像喝醉了酒。
2. 变换顺序很重要。先旋转再平移，还是先平移再旋转？结果完全不同。齐次变换矩阵里，顺序是：先旋转，再平移。
3. 验证变换是否正确。我习惯用一个已知点做测试：把传感器坐标系下的点(1,0,0)变换到机器人坐标系，看看结果是不是符合预期。

---