2. 坐标系与空间变换

各位同学好，我是老张。在机器人领域摸爬滚打了十几年，今天咱们来聊聊最基础、也最容易翻车的一个话题——坐标系与空间变换。

说实话，我刚入行那会儿，觉得坐标系变换不就是个数学问题嘛，有什么难的？直到有一次，我调试一个机械臂抓取程序，明明算好的位置，机械臂就是抓不到目标。折腾了两天，最后发现是欧拉角的万向锁问题。嗯，从那以后，我再也不敢小看坐标系变换了。

1.1 刚体变换：从二维到三维

刚体变换，说白了就是物体在空间中的位置和姿态变化。它包含两部分：平移和旋转。

平移好理解，就是物体从A点挪到B点。旋转呢，就是物体转了个角度。在二维平面里，一个刚体有3个自由度（x, y, θ）。到了三维空间，自由度变成了6个（x, y, z, roll, pitch, yaw）。

我在项目中遇到过一个问题：两个机器人协同工作，各自有自己的坐标系。如果不做刚体变换，它们对同一个物体的描述会完全不一样。你想想看，这多危险。

1.2 齐次坐标：为什么需要它？

为什么要引入齐次坐标？原因很简单——为了用矩阵乘法统一处理平移和旋转。

在普通坐标系下，旋转可以用3x3矩阵表示，平移却要用加法。这就很麻烦。齐次坐标把三维点变成四维向量（x, y, z, 1），这样平移和旋转就能合并成一个4x4矩阵了。

// 齐次坐标示例
// 三维点 (1, 2, 3) 的齐次坐标表示
[1, 2, 3, 1]

// 平移矩阵 (沿x轴移动5个单位)
[1, 0, 0, 5]
[0, 1, 0, 0]
[0, 0, 1, 0]
[0, 0, 0, 1]

// 旋转矩阵 (绕z轴旋转90度)
[0, -1, 0, 0]
[1,  0, 0, 0]
[0,  0, 1, 0]
[0,  0, 0, 1]

1.3 欧拉角：直观但危险

欧拉角用三个角度（roll、pitch、yaw）描述旋转。直观吧？但坑也很多。

最大的坑是万向锁。当pitch接近±90度时，roll和yaw会变得不可区分，丢失一个自由度。我记得有一次调试无人机，飞控突然失控，就是因为欧拉角遇到了万向锁。

另一个问题是表示不唯一。同一个姿态，可以用多组欧拉角表示。这就导致插值计算时，轨迹可能走一条奇怪的路径。

表示方式	优点	缺点
欧拉角	直观，三个角度容易理解	万向锁、表示不唯一、插值困难
旋转矩阵	计算方便，无奇异性	9个参数，冗余，不直观
四元数	无奇异性，插值平滑	不太直观，需要理解四维空间

1.4 四元数：轨迹规划的利器

四元数是我个人最推荐的旋转表示方式。它用四个数（w, x, y, z）表示旋转，没有万向锁问题，插值也特别平滑。

四元数的核心公式：

// 四元数表示绕单位向量 (nx, ny, nz) 旋转 θ 角度
q = [cos(θ/2), nx*sin(θ/2), ny*sin(θ/2), nz*sin(θ/2)]

// 四元数乘法（用于组合旋转）
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
           w1*x2 + x1*w2 + y1*z2 - z1*y2,
           w1*y2 - x1*z2 + y1*w2 + z1*x2,
           w1*z2 + x1*y2 - y1*x2 + z1*w2]

// 球面线性插值（SLERP）
// 用于在两个姿态之间平滑过渡
slerp(q1, q2, t) = (sin((1-t)*Ω) / sin(Ω)) * q1 + (sin(t*Ω) / sin(Ω)) * q2

1.5 ROS中的TF变换

ROS的TF（Transform）库，说白了就是帮你管理坐标系变换的。你只需要发布坐标系之间的变换关系，TF会自动维护一个变换树。

TF的核心概念：

• 广播器（Broadcaster）：发布坐标系之间的变换关系
• 监听器（Listener）：查询任意两个坐标系之间的变换
• 变换树（Transform Tree）：所有坐标系关系的图结构

// C++示例：发布TF变换
#include <tf2_ros/transform_broadcaster.h>

tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::TransformStamped transformStamped;

transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = "robot_base";
transformStamped.transform.translation.x = 1.0;
transformStamped.transform.translation.y = 2.0;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation.x = 0.0;
transformStamped.transform.rotation.y = 0.0;
transformStamped.transform.rotation.z = 0.0;
transformStamped.transform.rotation.w = 1.0;

broadcaster.sendTransform(transformStamped);

1.6 知识体系总览

下面这张图是我自己总结的坐标系与空间变换知识体系。你可以把它当作一个快速索引，遇到问题回来翻翻。

这张图把本章的核心内容串起来了。从刚体变换出发，到齐次坐标和旋转表示，最后落地到ROS的TF实现。你写代码时，可以按这个思路来组织你的变换逻辑。

好了，第一章的内容就到这里。坐标系变换这东西，光看理论是不够的。我建议你打开ROS，搭一个简单的TF树，发布几个坐标系，然后用tf_echo看看效果。实践出真知嘛。

可可精品资料库