第二章：运动学基础——刚体变换、旋转矩阵、欧拉角、四元数、齐次坐标

大家好，我是你们的老朋友。今天咱们聊聊运动学基础。说实话，这部分内容我当年学的时候也觉得有点绕，但后来做项目才发现，这玩意儿就是机器人关节标定的“地基”。地基不牢，后面全白搭。

你想想看，人形机器人要动起来，每个关节都得知道自己在哪、要往哪转。这背后就是刚体变换在撑腰。说白了，就是研究一个物体在空间里怎么“挪”和“转”。

2.1 刚体变换：位置+姿态

刚体变换，我习惯把它拆成两件事：位置和姿态。位置好理解，就是坐标(x,y,z)。姿态呢？就是物体朝哪个方向摆着。

举个例子。你手里拿个螺丝刀，螺丝刀头的位置是(x,y,z)，但你是横着拿还是竖着拿，这就是姿态。刚体变换就是同时描述这两样东西。

我在项目中遇到过一个问题：两个机器人手臂要协同工作，但它们的基座坐标系不一样。这时候就得用刚体变换把其中一个手臂的位姿换算到另一个的坐标系下。嗯，这一步做错了，后面全乱套。

2.2 旋转矩阵：最直观的旋转表达

旋转矩阵，说白了就是一个3x3的方阵。它描述了坐标系A相对于坐标系B的朝向。

它的性质很有意思：

• 行列式等于1（纯旋转，不包含缩放）
• 逆矩阵等于转置矩阵（正交矩阵）
• 每一列都是单位向量，且互相垂直

我记得刚入行时，总记不住旋转矩阵的乘法顺序。后来我总结了一个口诀：“左乘坐标系，右乘向量”。什么意思呢？如果你想把一个向量从一个坐标系转到另一个，就用旋转矩阵左乘这个向量。

2.3 欧拉角：直观但容易踩坑

欧拉角，就是用三个角度来描述旋转。比如绕X轴转α，绕Y轴转β，绕Z轴转γ。听起来很直观对吧？

但这里有个大坑——万向锁。我曾经在调试一个六轴机械臂时，发现某个姿态下关节突然“卡住”了。查了半天，原来是欧拉角遇到了万向锁，丢失了一个自由度。

欧拉角的顺序也很重要。常见的顺序有ZYX、ZYZ、XYZ等。不同顺序得到的角度值完全不同。我建议你在项目一开始就定好顺序，并在文档里写清楚。

2.4 四元数：旋转的“瑞士军刀”

四元数，听起来很玄乎，其实就是一个四维向量(qw, qx, qy, qz)。其中qw是实部，qx,qy,qz是虚部。它用来表示旋转，没有万向锁问题，而且插值非常平滑。

我个人习惯用四元数来做所有内部计算。为什么？因为：

• 没有奇点（不像欧拉角）
• 计算效率高（比旋转矩阵少几个乘法）
• 插值平滑（用Slerp算法）

四元数转旋转矩阵的公式，我建议你直接背下来：

// 四元数 q = (w, x, y, z) 转旋转矩阵 R
R[0][0] = 1 - 2*y*y - 2*z*z
R[0][1] = 2*x*y - 2*w*z
R[0][2] = 2*x*z + 2*w*y
R[1][0] = 2*x*y + 2*w*z
R[1][1] = 1 - 2*x*x - 2*z*z
R[1][2] = 2*y*z - 2*w*x
R[2][0] = 2*x*z - 2*w*y
R[2][1] = 2*y*z + 2*w*x
R[2][2] = 1 - 2*x*x - 2*y*y

2.5 齐次坐标：把旋转和平移统一起来

齐次坐标，就是把三维坐标(x,y,z)变成四维坐标(x,y,z,1)。为什么要这么做？因为这样我们就可以用一个4x4矩阵同时表示旋转和平移了。

你想想看，如果没有齐次坐标，你要先做旋转（3x3矩阵乘法），再做平移（向量加法）。有了齐次坐标，一步到位：

// 齐次变换矩阵 T
T = | R   t |
    | 0   1 |

// 其中 R 是3x3旋转矩阵，t 是3x1平移向量
// 变换后的点 p' = T * p

我在做机器人运动学正解时，就是用一个又一个的齐次变换矩阵连乘，从基座一直算到末端执行器。每个关节对应一个变换矩阵，乘起来就是末端位姿。

2.6 知识体系总览

下面这张图，是我自己总结的运动学基础知识体系。你看一眼，心里就有谱了。

2.7 各表达方式对比

为了让你看得更清楚，我把这几种表达方式放在一起对比一下：

表达方式	维度	优点	缺点	适用场景
旋转矩阵	3x3	直观，容易理解	9个参数，冗余	理论推导、坐标变换
欧拉角	3个角度	参数少，直观	万向锁，插值不平滑	人机交互、姿态显示
四元数	4个参数	无奇点，插值平滑	不够直观	内部计算、运动规划
齐次坐标	4x4	统一旋转+平移	计算量稍大	运动学正解、逆解

2.8 实战建议

说了这么多，到底怎么用？我给出几条实战建议：

1. 存储用四元数：内部计算和存储，一律用四元数。避免万向锁，插值也方便。
2. 显示用欧拉角：给用户看或者调试时，转成欧拉角。人脑更容易理解三个角度。
3. 计算用齐次矩阵：做运动学正解、逆解时，用4x4齐次变换矩阵。一步到位，不容易出错。
4. 转换要熟练：四元数↔旋转矩阵↔欧拉角，这三种转换的代码，建议你背下来或者写成工具函数。