2. 机械臂运动学基础（上）：刚体变换与齐次坐标、旋转矩阵与欧拉角、四元数基础

各位同学，欢迎来到机械臂运动学基础的第一部分。

做机器人抓取，说白了就是回答两个问题：「我的机械臂末端现在在哪儿？」 和 「它该怎么动才能抓到目标？」。这两个问题的答案，都绕不开一个核心工具——刚体变换。

我个人习惯把这一章看作是整个位姿解算的「地基」。地基没打牢，后面盖多高的楼都悬。今天我们就来把这套工具彻底搞明白。

2.1 刚体变换与齐次坐标

先说说「刚体」是什么。你想想看，一个铁块，你把它从桌子左边挪到右边，它的形状、大小变了吗？没有。这种只改变位置和朝向，不改变自身形状的物体，就叫刚体。机械臂的每个连杆，都可以近似看作刚体。

刚体变换，就是描述这种运动——旋转 + 平移。

2.1.1 从欧氏变换到齐次坐标

在三维空间中，一个点 P 在世界坐标系下的坐标是 P_w。当它跟着机械臂末端移动后，我们想知道它在末端坐标系下的坐标 P_e。这通常分两步：先旋转，再平移。

用公式表达就是：

P_e = R * P_w + t

这里 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。

这个公式本身没问题，但用起来有点麻烦。比如你要连续变换多次：

P_tool = R2 * (R1 * P_w + t1) + t2

看着就乱，对吧？而且没法写成统一的矩阵乘法形式。

齐次变换矩阵长这样：

| R   t |
| 0   1 |

其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量，最后一行是 [0, 0, 0, 1]。

这样一来，连续变换就变成了矩阵连乘：

P_tool = T2 * T1 * P_w

简洁、优雅，而且计算机处理矩阵乘法非常高效。

2.2 旋转矩阵与欧拉角

旋转矩阵是描述姿态最直接的方式。一个 3x3 的矩阵，9 个元素，但只有 3 个自由度。它有两个重要性质：

• 正交性： R^TR = I，矩阵的逆等于它的转置。
• 行列式为 +1： 保证是纯旋转，没有镜像。

我在项目中遇到过一个问题：直接用旋转矩阵做插值，结果矩阵不再满足正交性，导致物体变形。嗯，这里要注意，旋转矩阵不适合直接做平滑过渡。

2.2.1 欧拉角：直观但危险

欧拉角用三个角度表示旋转：绕 X 轴（滚转 Roll）、绕 Y 轴（俯仰 Pitch）、绕 Z 轴（偏航 Yaw）。

它的优点是直观。你给操作员说「绕 Z 轴转 30 度」，他立刻能理解。但欧拉角有个臭名昭著的问题——万向锁。

我曾经在一个视觉伺服项目里踩过这个坑。目标物体的姿态恰好让机械臂进入了万向锁区域，结果末端执行器在那疯狂抖动，就是转不过去。后来我果断换成了四元数，问题立刻解决。

三种常见欧拉角顺序对比：

2.3 四元数基础

四元数，说白了就是复数的升级版。一个复数有两个分量 (a + bi)，四元数有四个分量：

q = w + xi + yj + zk

其中 w 是实部，(x, y, z) 是虚部，且 i² = j² = k² = ijk = -1。

为什么用四元数？三个理由：

1. 无万向锁： 这是最大的优势。四元数通过一个旋转轴和一个旋转角来描述姿态，永远不会丢失自由度。
2. 插值平滑： 可以用球面线性插值（SLERP）在两个姿态之间生成平滑过渡。这在机械臂轨迹规划中太重要了。
3. 计算高效： 组合两个旋转只需要一次四元数乘法，比矩阵乘法快。

2.3.1 四元数的核心操作

我建议你记住这几个操作，写代码时经常用到：

• 单位四元数： ||q|| = 1，表示纯旋转。任何旋转都可以用一个单位四元数表示。
• 共轭： q* = (w, -x, -y, -z)，表示反向旋转。
• 乘法： 组合两个旋转。注意四元数乘法不满足交换律——先转 A 再转 B 和先转 B 再转 A 结果不同。
• 旋转向量： 用四元数旋转一个三维向量 v：v' = q * v * q*（这里 v 要写成纯四元数 (0, v)）。

2.3.2 四元数与旋转矩阵的转换

给你一个实用的代码片段，Python 版：

import numpy as np

def quaternion_to_rotation_matrix(q):
    """四元数转旋转矩阵"""
    w, x, y, z = q
    return np.array([
        [1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
        [2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
        [2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
    ])

def rotation_matrix_to_quaternion(R):
    """旋转矩阵转四元数"""
    tr = np.trace(R)
    if tr > 0:
        S = np.sqrt(tr + 1.0) * 2
        w = 0.25 * S
        x = (R[2,1] - R[1,2]) / S
        y = (R[0,2] - R[2,0]) / S
        z = (R[1,0] - R[0,1]) / S
    # 其他情况处理略...
    return np.array([w, x, y, z])

2.4 本章小结

我们这一章把刚体变换的三种主流表示方法都过了一遍：

• 齐次变换矩阵： 统一表示位置和姿态，适合做连续变换和坐标系转换。
• 欧拉角： 直观，但小心万向锁。适合人机交互和简单调试。
• 四元数： 无奇点、插值平滑，是机器人领域的首选。建议你在核心算法中优先使用。