3. 运动学基础（二）：齐次变换矩阵、正运动学建模（以简化双足模型为例）

好，咱们接着聊运动学。上一节我们把旋转矩阵、平移向量这些基本概念捋了一遍。这一节，我要把它们串起来——用齐次变换矩阵。

说实话，我刚入行那会儿，觉得齐次变换就是个数学技巧，没啥大不了的。直到有一次，我在调试一个双足机器人的步态，发现脚底的位置算出来总是差那么几毫米。查了半天，原来是坐标系变换的时候，旋转和平移分开处理，顺序搞反了。嗯，从那以后，我再也不敢小看这个「小技巧」了。

3.1 齐次变换矩阵：把旋转和平移装进一个盒子里

你想想看，我们描述一个刚体在空间中的位姿，需要什么？

• 一个旋转矩阵 R（3x3），描述朝向
• 一个平移向量 t（3x1），描述位置

但问题是，如果我们想把这两个操作连续做几次，比如从「髋关节」到「大腿」再到「小腿」，每次都要先旋转再平移，写起来很啰嗦。而且，旋转是乘法，平移是加法，混在一起容易出错。

齐次变换矩阵就是来解决这个问题的。它把旋转和平移统一成一个 4x4 的矩阵：

T = [ R   t ]
    [ 0   1 ]

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

这样一来，一个坐标系到另一个坐标系的变换，就变成了一个矩阵乘法。连续变换就是矩阵连乘。干净利落。

3.2 我习惯的记法：T_ab 表示从 b 到 a

这里有个小细节，不同教材的记法不一样。我个人习惯用 T_ab 表示「从坐标系 {b} 到坐标系 {a} 的变换」。也就是说，一个在 {b} 中描述的向量 p_b，乘以 T_ab，就得到它在 {a} 中的描述 p_a：

p_a = T_ab * p_b

为什么这么记？因为连乘的时候，下标会自然抵消：

T_ac = T_ab * T_bc

你看，中间的 b 消掉了，很直观。我在项目中一直用这套记法，团队里沟通起来基本不会搞混。

3.3 正运动学建模：以简化双足模型为例

好，理论说完了，咱们来点实际的。正运动学，说白了就是：给定关节角度，求末端（比如脚踝、手）的位置和朝向。

我们用一个简化的双足模型。为了讲清楚，我把它拆成一条腿——从骨盆到脚底。这条腿有 3 个自由度：

• 髋关节：绕 Z 轴旋转（偏航），角度记为 θ₁
• 髋关节：绕 Y 轴旋转（俯仰），角度记为 θ₂
• 膝关节：绕 Y 轴旋转（俯仰），角度记为 θ₃

大腿长度 L₁，小腿长度 L₂。坐标系定义如下：

• {0}：骨盆坐标系，原点在髋关节
• {1}：髋关节旋转后的坐标系
• {2}：膝关节坐标系
• {3}：脚踝坐标系（末端）

那么，从骨盆到脚踝的总变换矩阵就是：

T_03 = T_01 * T_12 * T_23

其中：

• T_01：绕 Z 轴转 θ₁，然后沿 Z 轴平移 0（髋关节在原点）
• T_12：绕 Y 轴转 θ₂，然后沿 Z 轴平移 L₁（大腿长度）
• T_23：绕 Y 轴转 θ₃，然后沿 Z 轴平移 L₂（小腿长度）

写成矩阵形式：

T_01 = RotZ(θ₁) * Trans(0, 0, 0)
T_12 = RotY(θ₂) * Trans(0, 0, L₁)
T_23 = RotY(θ₃) * Trans(0, 0, L₂)

把它们乘起来，就得到了脚踝相对于骨盆的位置和朝向。

3.4 代码实现：用 Python 算一遍

光说不练假把式。咱们用 Python 和 numpy 来实现一下。我一般会先定义几个辅助函数：

import numpy as np

def rot_z(theta):
    """绕 Z 轴旋转矩阵"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, 0, 0],
        [s,  c, 0, 0],
        [0,  0, 1, 0],
        [0,  0, 0, 1]
    ])

def rot_y(theta):
    """绕 Y 轴旋转矩阵"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [ c, 0, s, 0],
        [ 0, 1, 0, 0],
        [-s, 0, c, 0],
        [ 0, 0, 0, 1]
    ])

def trans(x, y, z):
    """平移矩阵"""
    return np.array([
        [1, 0, 0, x],
        [0, 1, 0, y],
        [0, 0, 1, z],
        [0, 0, 0, 1]
    ])

然后，给定一组关节角度，计算脚踝位置：

def forward_kinematics(theta1, theta2, theta3, L1, L2):
    T_01 = rot_z(theta1)
    T_12 = rot_y(theta2) @ trans(0, 0, L1)
    T_23 = rot_y(theta3) @ trans(0, 0, L2)
    
    T_03 = T_01 @ T_12 @ T_23
    
    # 脚踝位置在 T_03 的最后一列前三个元素
    position = T_03[:3, 3]
    # 朝向是左上角 3x3 旋转矩阵
    orientation = T_03[:3, :3]
    
    return position, orientation

你看，代码跟数学公式几乎一一对应。这就是齐次变换矩阵的魅力。

3.5 知识体系：一张图说清楚

下面这张 SVG 图，是我梳理的本节核心逻辑。你可以把它当成一个「思维导图」来看：

这张图从左到右，从上到下，把整个正运动学的计算流程串起来了。你写代码的时候，就照着这个流程来，基本不会乱。

3.6 避坑指南：我踩过的几个坑

最后，分享几个我实际项目中踩过的坑，希望能帮你省点时间：

• 矩阵乘法顺序：T_01 * T_12 不等于 T_12 * T_01。矩阵乘法不满足交换律。我刚开始写代码时，经常把顺序搞反，结果脚踝跑到天上去了。
• 角度单位：Python 的 math.cos/sin 默认用弧度。如果你习惯用角度，记得转一下。我见过有人直接传角度进去，算出来的位置完全不对。
• 零位校准：每次搭建新模型，先验证零位。所有关节角度为 0，末端应该在预期位置。如果不对，一定是某个变换矩阵写错了。
• 坐标系方向：Z 轴向上还是 Y 轴向上？不同机器人平台习惯不同。我建议统一用 Z 轴向上，这样跟 ROS 的惯例一致。

好，这一节就到这儿。下一节我们聊逆运动学——给定脚踝位置，怎么反推关节角度。那才是真正有意思的部分。