3. 齐次变换矩阵：平移与旋转的复合、链式变换规则、逆变换
各位同学，今天我们聊一个在机器人学里绕不开的核心工具——齐次变换矩阵。说白了，它就是用来描述一个坐标系相对于另一个坐标系的位置和姿态的。你想想看，人形机器人的脑袋、躯干、手臂、腿，每个关节都有自己的坐标系，怎么把这些坐标系串起来？靠的就是这个矩阵。
我个人习惯把齐次变换矩阵看作一个“打包盒”。它把旋转矩阵（3x3）和平移向量（3x1）塞进一个4x4的矩阵里，再加一行(0,0,0,1)收尾。这样，一次矩阵乘法就能同时搞定旋转和平移，非常优雅。
3.1 平移与旋转的复合
先看一个简单的例子。假设机器人手部坐标系 {H} 相对于基坐标系 {B} 先绕 Z 轴转了 30°，再沿着 X 轴平移了 10 厘米。这两个操作怎么合并？
在数学上，我们先把旋转写成齐次形式，再把平移写成齐次形式，然后相乘。注意顺序：先旋转后平移，矩阵乘法是左乘。
# 旋转矩阵 R (绕Z轴30°)
R = [
[cos30, -sin30, 0],
[sin30, cos30, 0],
[0, 0, 1]
]
# 平移向量 t = [10, 0, 0]^T
# 齐次变换矩阵 T = [R t; 0 0 0 1]
T = [
[cos30, -sin30, 0, 10],
[sin30, cos30, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
]这个 T 矩阵就是复合结果。它表示：先旋转坐标系，再平移坐标系的原点。我在项目中遇到过不少新手搞反顺序，结果机器人手臂直接飞到天上去。嗯，这里要注意：矩阵乘法不满足交换律，顺序错了，结果全错。
from_rotation_translation() 和 compose() 方法。这样不容易搞混顺序。
3.2 链式变换规则
人形机器人有几十个关节，每个关节都是一个坐标系。从基座到末端执行器，中间经过一串变换。这就是链式变换。
假设我们有三个坐标系：{A}、{B}、{C}。已知 {B} 相对于 {A} 的变换矩阵 T_AB，以及 {C} 相对于 {B} 的变换矩阵 T_BC。那么 {C} 相对于 {A} 的变换矩阵就是：
T_AC = T_AB * T_BC
注意，这里是左乘。从右往左读：先经过 T_BC 从 {C} 到 {B}，再经过 T_AB 从 {B} 到 {A}。我经常跟团队说：链式变换就是“从右向左读，从左向右乘”。
举个例子，人形机器人的左腿：
- 骨盆坐标系 {P} 到髋关节坐标系 {H}：T_PH
- 髋关节到膝关节 {K}：T_HK
- 膝关节到踝关节 {A}：T_KA
- 踝关节到脚底 {F}：T_AF
那么脚底相对于骨盆的变换就是：
T_PF = T_PH * T_HK * T_KA * T_AF
我曾经在调试双足行走算法时，发现脚底位置算出来总是偏了 5 厘米。查了半天，原来是链式变换里少乘了一个关节偏移矩阵。从那以后，我写链式变换代码时一定会先画一张坐标系树图，再逐级验证。
3.3 逆变换
逆变换，顾名思义，就是求变换矩阵的逆。给定 T_AB，我们想求 T_BA。T_BA 就是 T_AB 的逆矩阵。
齐次变换矩阵的逆有一个非常简洁的形式。假设：
T = [R t; 0 0 0 1]
则 T^(-1) = [R^T -R^T * t; 0 0 0 1]你看，不需要真的去求 4x4 矩阵的逆。只需要把旋转矩阵转置，再把平移向量旋转回去取负。这个公式我建议你背下来，因为在实际代码中，求逆操作非常频繁。
举个例子，在运动学正解中，我们已知关节角度，求末端位置。但在运动学逆解中，我们已知末端位置，反求关节角度。逆变换就是逆解的基础工具之一。
3.4 知识体系图
下面这张 SVG 图总结了本章的核心逻辑。你可以把它当作一个快速参考。
3.5 实战中的避坑指南
最后，分享几个我在实际项目中踩过的坑：
- 坐标系定义要统一： 我见过一个团队，有人用右手系，有人用左手系，结果数据合在一起时乱成一锅粥。建议项目一开始就约定好坐标系规则。
- 浮点数精度问题： 链式变换乘多了，误差会累积。我一般每 10 次变换后做一次归一化，保证旋转矩阵的正交性。
- 逆变换的数值稳定性： 如果旋转矩阵接近奇异（比如两个关节轴平行），逆变换可能不稳定。这时候要用奇异值分解（SVD）来处理。
好了，这一章的内容就到这里。齐次变换矩阵是机器人运动学的基石，理解透了，后面的正解、逆解、雅可比矩阵都会轻松很多。动手写代码试试吧，把今天讲的公式实现一遍，你会记得更牢。