第1章：坐标变换链——从数学到工程的桥梁

说实话，我刚入行那会儿，觉得坐标变换就是个矩阵乘法的事。直到第一次做多传感器融合项目，IMU和激光雷达的数据怎么都对不上，我才意识到——坐标变换链这东西，看似简单，坑是真不少。

这一章，咱们就聊聊齐次坐标、变换矩阵、链式变换这些核心概念。我会结合自己踩过的坑，帮你把这块彻底吃透。

1.1 齐次坐标：为什么非要多加一维？

你可能会问：三维空间里的点，用(x,y,z)表示不就行了？为什么非要搞成(x,y,z,1)？

嗯，这里有个关键问题：旋转可以用3x3矩阵，但平移不行。你想想看，一个3x3矩阵乘一个三维向量，结果还是三维向量——平移信息根本塞不进去。

齐次坐标的妙处就在于：把平移和旋转统一成一个4x4矩阵。这样，无论多少次变换，都能用矩阵乘法串联起来。

核心公式：齐次变换矩阵 T 的形式

T = [ R   t ]
    [ 0   1 ]

其中 R 是3x3旋转矩阵，t 是3x1平移向量。

我在做手眼标定时，就遇到过因为忘记把点扩展成齐次坐标，导致计算结果完全不对的情况。后来养成了习惯：所有坐标运算，一律用齐次形式。

1.2 变换矩阵：旋转+平移的打包工具

变换矩阵说白了，就是把旋转和平移打包成一个4x4的“黑盒子”。你输入一个点，它输出变换后的点。

常见的变换矩阵有几种：

变换类型	矩阵形式	说明
纯旋转	[R 0; 0 1]	绕原点旋转
纯平移	[I t; 0 1]	只移动位置
刚体变换	[R t; 0 1]	旋转+平移，最常用
缩放	[sI 0; 0 1]	均匀缩放

我个人习惯用 Eigen 库 来处理这些矩阵。C++代码大概长这样：

#include <Eigen/Dense>

Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
// 设置旋转部分（绕Z轴旋转30度）
T.block<3,3>(0,0) = Eigen::AngleAxisd(30*M_PI/180, 
                       Eigen::Vector3d::UnitZ()).toRotationMatrix();
// 设置平移部分
T.block<3,1>(0,3) = Eigen::Vector3d(1.0, 2.0, 0.5);

// 变换一个点
Eigen::Vector4d p(1, 0, 0, 1);
Eigen::Vector4d p_transformed = T * p;

小技巧：写代码时，我习惯把变换矩阵命名为 T_ab，表示从坐标系b到坐标系a的变换。这样链式变换时，下标能自动对齐，不容易出错。

1.3 坐标系之间的链式变换

实际工程中，很少只有一个变换。比如自动驾驶里：

IMU 坐标系 → 车体坐标系
车体坐标系 → 世界坐标系
相机坐标系 → IMU 坐标系

这些变换串起来，就是坐标变换链。

链式变换的数学本质很简单：矩阵乘法。

T_world_point = T_world_vehicle * T_vehicle_imu * T_imu_point

但要注意顺序！我见过太多人搞反了。记住：从右往左读。最右边的矩阵先作用，最左边的最后作用。

我曾经踩过的坑：在做IMU和GPS融合时，我把变换顺序写反了。结果车辆明明在直行，融合出来的轨迹却在画圈。排查了两天才发现，是 T_imu_vehicle 和 T_vehicle_imu 搞混了。

所以我现在每次写链式变换，都会在注释里把坐标系关系画出来。

1.4 从IMU到车体到世界坐标系

这是一个典型的三级变换链。咱们拆开来看：

IMU → 车体：IMU通常安装在车体某个位置，有固定的安装角度和偏移。这个变换是固定的，标定一次就能用。
车体 → 世界：车体在世界中的位置和姿态，由定位系统（如GPS+IMU融合）给出。这个变换是实时变化的。
IMU → 世界：把上面两个串起来。

用代码表示：

// 固定标定结果：IMU在车体中的安装位置
Eigen::Matrix4d T_vehicle_imu = getCalibrationMatrix();

// 实时定位结果：车体在世界中的位姿
Eigen::Matrix4d T_world_vehicle = getVehiclePose();

// 链式变换：IMU测量点转到世界坐标系
Eigen::Vector4d p_imu(ax, ay, az, 1);
Eigen::Vector4d p_world = T_world_vehicle * T_vehicle_imu * p_imu;

关键点：IMU测量的是加速度和角速度，这些量在变换时只旋转，不平移。因为加速度是自由向量，平移不影响它。这一点很多新手会搞错。

1.5 手眼标定中的坐标变换

手眼标定，说白了就是求相机和机械臂末端之间的固定变换。我做过好几个机器人项目，每次标定都像开盲盒——结果对不对，只有跑起来才知道。

手眼标定有两种经典场景：

眼在手上（Eye-in-Hand）：相机固定在机械臂末端。标定目标是求 T_camera_end。
眼在外部（Eye-to-Hand）：相机固定在世界中。标定目标是求 T_camera_base。

标定的核心方程是：

A * X = X * B

其中：

A 是机械臂末端两次运动之间的变换
B 是相机两次观测之间的变换
X 就是我们要标定的固定变换

这个方程的解，网上有很多现成的库。但我建议你理解原理再调库，不然出了问题都不知道怎么排查。

我的经验：手眼标定时，尽量让机械臂做大范围、多姿态的运动。只在小范围内微动，标定结果会很不稳定。我曾经为了省事只拍了5组数据，结果标定误差大到离谱。

1.6 本章知识体系

下面这张图，是我梳理的本章核心逻辑。你可以把它当作一个思维导图来用：

这张图把本章的核心脉络串起来了。你可以看到，齐次坐标是基础，变换矩阵是工具，链式变换是方法，最终都落到手眼标定、多传感器融合这些实际应用上。

1.7 避坑总结

最后，把我这些年踩过的坑总结一下，希望能帮你少走弯路：

坐标系命名要规范：我习惯用 T_from_to 的命名方式，比如 T_vehicle_imu 表示从IMU到车体。
矩阵乘法顺序别搞反：从右往左读，最右边的先作用。
自由向量（加速度、角速度）只旋转，不平移。
手眼标定数据要足够多：至少10组以上，覆盖不同姿态。
写代码时加注释：把坐标系关系画出来，方便以后排查。

嗯，这一章的内容就到这。坐标变换链是后续所有章节的基础，一定要亲手写代码跑一遍。下一章咱们聊聊时间同步——又是一个容易出幺蛾子的地方。

可可精品资料库

📚 课程目录

第1章：坐标变换链——从数学到工程的桥梁

1.1 齐次坐标：为什么非要多加一维？

1.2 变换矩阵：旋转+平移的打包工具

1.3 坐标系之间的链式变换

1.4 从IMU到车体到世界坐标系

1.5 手眼标定中的坐标变换

1.6 本章知识体系

1.7 避坑总结