第二章：运动学基础——刚体运动描述、齐次变换矩阵与正运动学建模

各位同学，大家好。欢迎来到《人形机器人轨迹规划精讲》的第二讲。

今天我们要聊的，是整个轨迹规划的“地基”——运动学基础。说白了，就是搞清楚机器人身上每个关节、每个连杆，到底在空间里是怎么动的。你想想看，如果连自己的胳膊在哪儿都不知道，还谈什么规划轨迹呢？

我个人习惯，在开始任何复杂的规划算法之前，先把运动学模型搞得清清楚楚。这一步要是出了岔子，后面所有的优化、避障、控制，全都是空中楼阁。我在项目里见过太多人，一上来就调PID、跑MPC，结果机器人动起来像个喝醉的章鱼，最后发现是运动学建模时一个符号搞反了。嗯，咱们今天就把这个坑填上。

2.1 刚体运动的描述：位置与姿态

首先，我们要描述一个刚体（比如机器人的大腿）在空间中的状态。这需要两样东西：位置和姿态。

位置：很简单，就是刚体上某个参考点（比如关节中心）在全局坐标系下的坐标。用向量 p = [x, y, z]^T 表示。
姿态：描述这个刚体“朝哪个方向”。这稍微复杂一点，我们通常用一个3x3的旋转矩阵 R 来表示。

旋转矩阵的每一列，其实就是刚体自身坐标系的三个轴，在全局坐标系下的投影。所以，一个完整的刚体位姿，可以用一个4x4的齐次变换矩阵来打包：

T = [ R   p ]
    [ 0   1 ]

这个矩阵，就是咱们今天的主角。它把旋转和平移统一到了一个框架里，非常优雅。

我的小经验： 刚开始学的时候，总觉得旋转矩阵的9个元素记不住。后来我总结了一个笨办法：你想象一下，把机器人关节上的坐标系画出来，然后看它的X轴、Y轴、Z轴分别指向世界坐标系的哪个方向。一列一列地填进去，准没错。

2.2 齐次变换矩阵：连接世界的桥梁

齐次变换矩阵，说白了就是用来“搬家”的。它可以把一个点从一个坐标系，搬到另一个坐标系。

假设我们在机器人脚踝上有一个点 P，它在脚踝坐标系下的坐标是 ^AP。如果我们知道脚踝坐标系相对于大腿坐标系的变换矩阵 ^B_A T，那么点 P 在大腿坐标系下的坐标 ^BP 就是：

^BP = ^B_A T * ^AP

这就是齐次变换最核心的用法——链式法则。你可以把机器人从基座到末端执行器，一连串的变换矩阵乘起来，就能得到末端在基座坐标系下的位姿。

我曾经在一个六轴协作臂的项目里，需要计算末端夹爪的精确位置。当时我手动推导了所有变换矩阵，结果发现乘出来的位置总是差那么几毫米。排查了半天，原来是其中一个关节的旋转顺序搞反了——先平移后旋转，和先旋转后平移，结果完全不同。所以，顺序很重要！

避坑指南： 矩阵乘法不满足交换律。也就是说，T1 * T2 和 T2 * T1 代表完全不同的运动。在写代码时，一定要搞清楚当前变换是相对于哪个坐标系的。

2.3 正运动学建模：以D-H参数法为例

好了，有了齐次变换这个工具，我们就可以来建立机器人的正运动学模型了。正运动学，就是已知每个关节的角度，求末端执行器的位姿。

最经典的方法，就是 D-H参数法。我个人非常喜欢这个方法，因为它把复杂的连杆关系，抽象成了四个参数，非常简洁。

这四个参数分别是：

a_i-1：连杆长度。沿着X_i-1轴，从Z_i-1轴移动到Z_i轴的距离。
α_i-1：连杆扭转角。绕着X_i-1轴，从Z_i-1轴旋转到Z_i轴的角度。
d_i：连杆偏距。沿着Z_i轴，从X_i-1轴移动到X_i轴的距离。
θ_i：关节角。绕着Z_i轴，从X_i-1轴旋转到X_i轴的角度。

对于旋转关节，θ_i是变量；对于移动关节，d_i是变量。

有了这四个参数，相邻两个连杆之间的变换矩阵就可以写成：

i-1T_i = Rot(X, α_i-1) * Trans(X, a_i-1) * Rot(Z, θ_i) * Trans(Z, d_i)

展开后就是：

i-1T_i = [ cosθ_i   -sinθ_i   0    a_i-1 ]
         [ sinθ_i * cosα_i-1   cosθ_i * cosα_i-1   -sinα_i-1   -d_i * sinα_i-1 ]
         [ sinθ_i * sinα_i-1   cosθ_i * sinα_i-1    cosα_i-1    d_i * cosα_i-1 ]
         [ 0              0               0           1         ]

看着有点复杂，对吧？别怕，实际用起来，大部分参数都是0或者±90度，会简化很多。

下面，我用一个简单的两连杆人形机器人腿部模型来演示一下。假设大腿长L1，小腿长L2，髋关节和膝关节都是旋转关节。

首先，建立D-H参数表：

连杆 i	a_i-1	α_i-1	d_i	θ_i
1 (大腿)	0	0	0	θ₁
2 (小腿)	L1	0	0	θ₂

然后，写出每个连杆的变换矩阵：

0T1 = [ cosθ₁  -sinθ₁  0  0 ]
      [ sinθ₁   cosθ₁  0  0 ]
      [ 0        0       1  0 ]
      [ 0        0       0  1 ]

1T2 = [ cosθ₂  -sinθ₂  0  L1 ]
      [ sinθ₂   cosθ₂  0  0 ]
      [ 0        0       1  0 ]
      [ 0        0       0  1 ]

最后，末端（脚踝）相对于基座（髋关节）的变换矩阵就是：

0T2 = 0T1 * 1T2

乘出来之后，矩阵的最后一列就是脚踝的位置坐标。你看，是不是很简单？

    核心要点： D-H参数法的精髓在于，它把复杂的空间几何关系，转化成了四个参数的查表和矩阵乘法。只要参数表没错，正运动学就一定是对的。

为了让大家更直观地理解整个知识体系，我画了一张图：

这张图把咱们今天讲的内容串起来了。从描述刚体运动，到用齐次变换矩阵做坐标系变换，再到用D-H参数法建立正运动学模型，每一步都是环环相扣的。

最后，我想强调一点：正运动学是轨迹规划的“眼睛”。没有它，你根本不知道机器人现在在哪儿，更别提规划它下一步去哪儿了。所以，花时间把D-H参数表建好，把矩阵乘法算对，值得。

好了，今天就到这儿。希望大家回去之后，能拿自己的机器人模型练练手，把D-H参数表列出来，然后写个小程序算一下末端位置。相信我，当你看到计算结果和实际测量值完全吻合时，那种成就感，比打游戏爽多了。

可可精品资料库

📚 课程目录

第二章：运动学基础——刚体运动描述、齐次变换矩阵与正运动学建模

2.1 刚体运动的描述：位置与姿态

2.2 齐次变换矩阵：连接世界的桥梁

2.3 正运动学建模：以D-H参数法为例