机械臂正运动学入门：从旋转矩阵到DH参数解析

机械臂这个东西，入门时最容易迷惑的不是电机怎么转，而是“我要它去的点”和“它实际到的点”之间到底隔着什么。我刚开始折腾机械臂时，以为把每个舵机角度调到某个值就行，结果想让它末端走一条直线，它硬是在空中画了个大圆弧，后来才意识到，问题出在机械臂运动学这套“空间翻译”上没搞通。

这篇是机械臂运动学系列的第一篇，主要讲正运动学：已知每个关节的角度，精准求出末端执行器在三维空间中的位置和姿态。我会以自己调一台两连杆平面机械臂的真实经历为线，把旋转矩阵、齐次变换、DH参数这些抽象概念落到具体数字上。内容既适合准备入坑机器人控制的同学，也适合已经写过代码但一直对坐标变换似懂非懂的工程师。读完你至少能亲手写出一个二连杆机械臂的正解，并知道如何验证它和物理形态是否一致。

1. 先想清楚定义：运动学是“算位姿”，动力学才是“用力气”

1.1 正解、逆解、积分这些名词，别被吓住

我刚入行的时候，被“正运动学”“逆运动学”“速度运动学”“雅可比矩阵”一堆名词砸得晕头转向。后来在一台六轴样机上调试，我才真正把这两个方向分清楚。

正运动学就是“给定关节角度，求末端在哪、朝哪看”。这就好比你看一个人的手，只要知道他肩膀和胳膊每个关节的角度，就能判断手掌大概落在什么位置。逆运动学则反过来，“给定一个目标位置，求每个关节该转多少度”，这是控制机械臂去抓东西的核心。

运动学里还有个容易混淆的学名叫“动力学”。动力学才关心电机输出多少力矩、负载有多重、惯性补偿怎么做。很多刚入门的同学一上来就陷入PID和力矩控制，结果连自己末端坐标都算不对，这就本末倒置了。做运动学不需要知道关节有多重，只需要一把尺子和一个量角器。

1.2 关节空间与笛卡尔空间的落差

人的直觉是站在笛卡尔空间里的，我们习惯用X、Y、Z去描述一个点。但机械臂的电机和你最终控制的量，是关节空间里的角度。

举个例子，你想要末端点从（0.3，0.4）直线走到（0.5，0.4），如果不知道正运动学公式，你只能去试每个关节的角速度，往往试到一半，末端已经飘到一个奇怪的位置去了。关节空间到笛卡尔空间的那座桥，正是运动学本身。理解这一点之后，我做路径规划时就不再只盯着每个电机的角速度了，而是先在想去的空间点列一串坐标，再反推角度。这算是第一篇里最值得刻在脑子里的思维转变。

2. 坐标系与旋转矩阵基础：描述一条手臂该具备的最小语言

2.1 从二维旋转看穿三维旋转的本质

所有三维姿态变换，起点都藏在二维旋转里。假设有一个点在平面上坐标是（x，y），它绕原点逆时针旋转一个角度θ后变成（x'，y'），推导用三角函数展开就能得到：

x' = x·cosθ - y·sinθ y' = x·sinθ + y·cosθ

如果把上式写成矩阵形式，就是绕Z轴旋转矩阵：

Rz(θ) = [ cosθ -sinθ 0 ] [ sinθ cosθ 0 ] [ 0 0 1 ]

刚接触的时候，我总觉得这个矩阵很抽象。后来自己画了个坐标纸，把（1，0）点绕着原点转90°，用上面的公式算一遍，发现确实变成了（0，1），这才对旋转矩阵有了实感。三维里绕X轴、绕Y轴的矩阵和绕Z轴的形状完全一样，只不过把sin和cos放到了不同的行列位置。描述机械臂末端姿态，本质上就是在描述“末端坐标系”相对“基座坐标系”的三个轴方向。

2.2 为什么偏偏是4x4的齐次变换矩阵？

一个3x3的旋转矩阵只能描述姿态，不能描述位置。要把“末端坐标系原点在哪”也放进去，我们就需要一个平移量t_x、t_y、t_z。把旋转矩阵R和平移向量t拼在一起，再补上数学上很好用的最后一行的[0 0 0 1]，就得到了4x4的齐次变换矩阵：

T = [ r11 r12 r13 t_x ] [ r21 r22 r23 t_y ] [ r31 r32 r33 t_z ] [ 0 0 0 1 ]

很多人会问，为什么要费劲保留那个“1”？因为只有把坐标点写成（x，y，z，1）的形式，用4x4矩阵去做乘法时，平移量才会被自动加入。如果不用齐次形式，平移和旋转就要分开计算，每次写代码都要多判断一次，特别容易出错。用4x4变换矩阵，多连杆机械臂就能不停“左乘”下去，像套娃一样把整个链串起来。

需要特别注意的是矩阵乘法顺序。矩阵乘法不满足交换律，先转A轴再平移B，和先平移B再转A轴完全不是同一回事。我做过一次很惨的调试，就是误以为可以把两个旋转矩阵交换位置，结果末端直接倒到了桌面底下。现在我的习惯是，每一次写连乘前，都要先在纸上把相邻两个坐标系的关系画出来。

3. 两连杆机械臂正解实战：从三角几何到矩阵连乘

3.1 先画图，再代数：二连杆是如何变成两个三角和的

我们拿一个最简单的平面两连杆机械臂开刀。底座固定在坐标系原点，第一根杆长度L1=1.0米，第二根杆长度L2=0.8米。第一关节转角记为θ1，第二关节转角记为θ2。

正常情况下，第二根杆相对水平面的角度并不是θ2，而是θ1+θ2，因为第二个关节坐标系继承了第一个关节的旋转。所以末端点的平面坐标是：

x = L1·cosθ1 + L2·cos(θ1+θ2) y = L1·sinθ1 + L2·sin(θ1+θ2) φ = θ1+θ2

这里φ是末端坐标系相对基座的总姿态角。看到这个式子，很多人会觉得简单得不像机器人学。但正是这个简单的几何关系，直接决定了一条六轴机械臂的末端位姿，只是它要多累加几个关节角，还要考虑连杆在空间里的扭转而已。

3.2 一个具体算例：θ1=30°，θ2=60°时末端在哪

把数值代进去算一遍。L1=1.0，L2=0.8，θ1=30°（即0.5236弧度），θ2=60°（即1.0472弧度）。

先查三角函数值：

• cos30°≈0.866，sin30°≈0.5
• cos90°=0，sin90°=1

所以末端点坐标就是：

x = 1.0×0.866 + 0.8×0 = 0.866m y = 1.0×0.5 + 0.8×1 = 1.3m φ = 90°

如果实物机械臂校准好了零点，你把关节1转到30°、关节2转到60°，用尺子和量角器去量末端，得到的结果应该就是大约（0.866，1.3）。我第一次量的时候误差有5毫米，后来排查发现是关节减速器的传动间隙，这是另一个话题了。

3.3 用代码验证正解公式：30秒写一个两连杆求解器

写一个Python函数验证这个结果，顺便为后面逆运动学做铺垫。

import math def forward_kinematics_2dof(theta1_deg, theta2_deg, L1=1.0, L2=0.8): theta1 = math.radians(theta1_deg) theta2 = math.radians(theta2_deg) x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2) y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2) phi = theta1 + theta2 return x, y, phi print(forward_kinematics_2dof(30, 60)) # 输出近似： (0.866025, 1.3, 1.57079)

这个函数的输出就是正运动学的结果。如果你更想体会矩阵连乘的味道，可以用两个齐次变换矩阵相乘来算，效果完全一样：

import numpy as np def rot_z(theta): c, s = np.cos(theta), np.sin(theta) return np.array([ [c, -s, 0], [s, c, 0], [0, 0, 1] ]) def T_2link(L1, L2, theta1, theta2): T01 = np.eye(4) T01[:3, :3] = rot_z(theta1) T01[:3, 3] = [L1 * np.cos(theta1), L1 * np.sin(theta1), 0] T12 = np.eye(4) T12[:3, :3] = rot_z(theta2) T12[:3, 3] = [L2 * np.cos(theta2), L2 * np.sin(theta2), 0] return T01 @ T12 T02 = T_2link(1.0, 0.8, math.radians(30), math.radians(60)) print(T02) print(T02[0, 3], T02[1, 3]) # 同样输出 0.866, 1.3

矩阵连乘的好处是：不管你的机械臂是2个关节还是6个关节，代码结构都不会变，只需要往链条里继续乘下一个变换矩阵就行。

4. 看懂DH表：四个参数就是把每个关节的“户口”一笔一笔登记

4.1 连杆长度、连杆转角、连杆偏距、关节角到底代表什么

如果你去看工业机械臂的说明书或ROS里导出的urdf，经常会出现一张DH参数表。初学者看到一列列a、alpha、d、theta，很容易直接放弃。我当时的突破点在于，把这四个参数当成“两个关节轴之间的交接”来理解。

• a（连杆长度）：上一关节轴和当前关节轴之间的公垂线长度，可以理解为两轴之间的最短距离。
• alpha（连杆转角）：上一关节轴绕公垂线转到当前关节轴时扫过的角度。
• d（连杆偏距）：沿着当前关节轴方向，从公垂线交点到当前坐标系原点的距离。
• theta（关节角）：绕当前关节轴，从上一X轴转到当前X轴的角度。

套到人身上，肘关节和腕关节之间的前臂，长度就是a，手臂上两根骨骼在空间里可能有个扭转角，那就对应alpha，关节本身有个沿轴线方向的偏移，对应d，最后才是真正转动的theta。每个DH行，描述的就是一根连杆加一个关节的完整“户口信息”。

4.2 给一台北漂的六轴机械臂填DH表

假设现在某项目组手里有一台常见构型的六轴串联机械臂，底座到末端各关节DH参数可能长这样：

这张表不是靠猜出来的，而是拿着游标卡尺和角度尺在机械臂上一个个量出来的。量完以后，把它代入标准的DH变换公式：

T = Rot(z, θ) · Trans(z, d) · Trans(x, a) · Rot(x, α)

就能写出通用变换函数。以后不管是哪个型号的六轴臂，只要把DH表替换进去，正解代码完全不用重新写。

写代码时我踩过一个很经典的坑，就是角度单位。DH表里alpha往往写着π/2，theta则是变量，但Python的math库默认用弧度，调试遥控器返回的角度却是度数的。每次没转换直接把度数丢进sin/cos里，末端位置都是天女散花。现在我的代码都会统一在入口处做一次math.radians()，把单位问题一次性扼杀在参数层。

5. 落地实测与常见坑：我的机械臂在调零点时飘去天边的那次事故

5.1 实测检查清单：先用正解给系统“照镜子”

写完正解代码，下一步就是拿实物验证。调试那天，我把关节1固定在0°，关节2固定在0°，理论上末端应该指向正前方X轴方向。结果机器人一上电，末端坐标显示在Y轴负方向，整个坐标系像被掰歪了。

后来逐项排查，发现问题的根源是基座坐标系的定义和电机编码器的零点对不上。也就是说，我在代码里假设关节在0°时X轴朝右，但实际机械臂装配时，第一关节的零点被装到了相对于Y轴负方向的位置。这个对不齐，会让所有后续坐标全乱掉。

我自己总结出一套“照镜子”检查顺序：

1. 先把所有关节手动转到机械限位处，记录编码器读数，作为零点偏移。
2. 把每个关节单独转到0°、90°、180°，对照DH表，确认旋转方向的正负号。
3. 让机械臂摆出一组简单姿态，比如肘部伸直，用尺子量末端坐标，和正解输出比较。
4. 如果位置对但姿态不对，多半是某个alpha符号搞反了。

这套检查做完以后，后面写逆运动学才有可靠的地基。

5.2 一个经典工程坑：同一个末端位置，可能有两副完全不同的“锁骨”

我还遇到过一件很有意思的事。当时做一台两连杆平面臂的点到点运动，目标点是（0.866，1.3）。我按θ1=30°、θ2=60°解出来了，上电一跑，末端确实到了那个点。但同事在旁边说，机械臂明明也可以这样摆：大臂抬高一点、小臂折下来，末端同样落在这个点。这就是运动学里的多解现象。

同一个末端位置，对应好几组关节角度。工程上处理这类问题，通常要加一个“肘部上下”的偏好选择，比如让大臂尽量贴近水平，或者避开奇异点。这个判断逻辑放到逆运动学里非常重要，不过那是第二篇要展开的内容了。

调试下来，我的一个深刻体会是：正运动学虽然看起来只是“把角度换成坐标”，但它是整套控制系统里最诚实的标尺。如果你连自己的正解都不确定，那后面做轨迹规划、速度解耦、力控，全都像是在黑屋子里闭眼走路。至少我有一次在做圆弧插补时，定位误差几毫米查了半天，最后发现不是控制器算法问题，而是起初的DH表里alpha符号写反了。从那以后，我每次改完正解代码，都会先在调试台上把关节转到几个特殊角度，拿尺子量末端，确认点坐标和姿态都对得上，再做下一层的控制。这个习惯，算是这篇运动学（一）里最值得带走的实操经验。