- Authors
- Name
- Youngju Kim
- @fjvbn20031
- 引言 —— 输入角度就能告诉你末端在哪里的函数
- 坐标系、旋转矩阵与平移
- 齐次变换矩阵为什么是 4×4
- 为什么串接坐标系就变成了乘法
- 手工推导两连杆手臂的正运动学
- DH 参数 —— 用四个数字描述一根连杆
- 旋转表示法的取舍 —— 欧拉角、旋转矩阵、四元数
- 一个符号错了,实际会发生什么
- 结语 —— 把坐标系精确地写下来,就完成了一半
引言 —— 输入角度就能告诉你末端在哪里的函数
假设已经定好了机械臂的结构并装上了舵机。现在可以读出每个关节的角度:肩部 30 度,肘部 45 度。
那么,末端执行器在哪里?
回答这个问题就是正运动学,它是机械臂上所有计算的起点。逆运动学、轨迹规划、碰撞检测,以及把相机看到的物体转换到手臂坐标系,全都建立在这一步计算之上。
好在正运动学很简单。输入角度就会得到一个位置,而且总是能得到,只需要几次乘法就能算完。 不管是六个关节还是十个关节,原理都一样。
问题在于,连杆一多,三角函数表达式就会长得没法处理。两连杆能手写,三连杆勉强也能应付,但轴线在三维空间中各自朝向不同的六连杆,就没法手写了。所以需要一种人能实际使用的记法,那就是齐次变换矩阵和 DH 参数。
这篇文章要用数字说明这两个工具为什么长成这个样子。
坐标系、旋转矩阵与平移
在机械臂里,每根连杆都会挂一个坐标系:肩部一个,上臂一个,前臂一个,末端一个。每个坐标系都固定在自己所属的连杆上,跟着连杆一起运动。
这么做的原因是:在同一根连杆内部,一切都是常数。 末端在腕部坐标系里的位置,不管关节怎么动都不会变。变的只是坐标系之间的相对关系。于是问题就从「一个复杂的形状」缩小成了「坐标系之间的几次变换」。
坐标系之间的关系由两件事构成:转了多少,离了多远。
旋转是矩阵。在二维中,逆时针转 θ 的旋转是:
R(θ) = [ cos θ -sin θ ]
[ sin θ cos θ ]
在三维中,绕 z 轴的旋转是:
Rz(θ) = [ cos θ -sin θ 0 ]
[ sin θ cos θ 0 ]
[ 0 0 1 ]
一列一列地读这个矩阵,含义就清楚了。第一列是旋转后坐标系的 x 轴在原坐标系里指向哪个方向,第二列是 y 轴,第三列是 z 轴。旋转矩阵不过是把三个轴的方向并排写在一起而已。
平移是向量加法。如果坐标系平移了 t,就把 t 加到点的坐标上。
这里有个不方便的地方:旋转是乘法,平移是加法。把两者写在一起,
p_world = R · p_local + t
这种形式一旦串接多个阶段就会变得杂乱。串接三段:
p = R1·(R2·(R3·p + t3) + t2) + t1
= R1·R2·R3·p + R1·R2·t3 + R1·t2 + t1
关节一到六个,项数就涨到六项,而且每一项都带着前面所有旋转的乘积。手工处理很吃力,搬进代码也很麻烦。
齐次变换矩阵为什么是 4×4
解法出乎意料地简单。给坐标多加一个 1。
把三维点 (x, y, z) 写成 (x, y, z, 1),再构造一个这样的 4×4 矩阵:
T = [ R11 R12 R13 tx ]
[ R21 R22 R23 ty ]
[ R31 R32 R33 tz ]
[ 0 0 0 1 ]
左上角的 3×3 是旋转矩阵,右上角的 3×1 是平移,最下面一行永远是 0 0 0 1。
用这个矩阵去乘 (x, y, z, 1),只算第一行看看会发生什么。
R11·x + R12·y + R13·z + tx·1
旋转的第一行,加上平移的第一个分量,正好就是 R·p + t 的第一个分量。最后一行算出来是 0·x + 0·y + 0·z + 1·1 = 1,所以补上去的那个 1 原样保留。
正因为被乘的对象里带着一个常数 1,那一列的分量才会表现得像加法一样。 这就是 4×4 矩阵的全部秘密。最后一行的 0 0 0 1,只是用来保证结果依然是一个有效点的装置。
用数字确认一下。坐标系 2 相对坐标系 1 绕 z 轴转了 45 度,沿 x 方向偏移了 0.20 米。在坐标系 2 里,末端位于自己 x 轴上 0.15 米处。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def transform_z(theta, tx):
"""绕z轴旋转theta、再沿x轴平移tx的4×4齐次变换。"""
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s, 0, tx],
[s, c, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
T = transform_z(np.radians(45), 0.20)
print("T (坐标系2 -> 坐标系1):")
print(T)
p_local = np.array([0.15, 0.0, 0.0, 1.0]) # 坐标系2中看到的末端
p_world = T @ p_local
print("坐标系2中的末端:", p_local[:3])
print("坐标系1中的末端:", np.round(p_world[:3], 6))
print("手算得到的值 :", np.round([0.20 + 0.15*np.cos(np.radians(45)),
0.15*np.sin(np.radians(45)), 0.0], 6))
运行结果如下。
T (坐标系2 -> 坐标系1):
[[ 0.707107 -0.707107 0. 0.2 ]
[ 0.707107 0.707107 0. 0. ]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
坐标系2中的末端: [0.15 0. 0. ]
坐标系1中的末端: [0.306066 0.106066 0. ]
手算得到的值 : [0.306066 0.106066 0. ]
手工计算:
x = 0.20 + 0.15 × cos(45°) = 0.20 + 0.106066 = 0.306066
y = 0 + 0.15 × sin(45°) = 0 + 0.106066 = 0.106066
和矩阵算出的值一致。
齐次变换矩阵还有一个方便的性质:求逆很便宜。一般 4×4 矩阵求逆计算量很大,但齐次变换矩阵可以利用其结构写成:
T⁻¹ = [ Rᵀ -Rᵀ·t ]
[ 0 1 ]
因为旋转矩阵的逆就是它的转置(正交矩阵的性质),所以只需要几次乘法。把末端坐标系里看到的物体位置转换到基座坐标系、以及反过来做这件事,都很便宜,而手腕上装了相机的手臂,每一帧要做的正是这个计算。
为什么串接坐标系就变成了乘法
现在就能看出这种表示法为什么和机械臂如此契合了。
把从基座到连杆 1 的变换记作 T01,连杆 1 到连杆 2 的变换记作 T12。要把连杆 2 坐标系里的点 p 转换到基座坐标系,
p_1 = T12 · p
p_0 = T01 · p_1 = T01 · T12 · p
串接变换,和矩阵相乘完全是一回事。 前面那个杂乱的展开式消失了。
关节有六个的话,
T06 = T01 · T12 · T23 · T34 · T45 · T56
这就是正运动学的全部内容。每个变换里带着一个关节角度,乘起来就得到末端的位置和姿态。位置是结果矩阵右上角的 3×1,姿态是左上角的 3×3。
要留意乘法的顺序。矩阵乘法不满足交换律,所以 T01·T12 和 T12·T01 是不同的。记顺序的窍门是看下标:内侧的下标要对上。T01·T12 里,两个 1 挨在一起,剩下 0 和 2,所以结果是 T02。
手工推导两连杆手臂的正运动学
在用矩阵之前,先用三角函数把最简单的情形解出来,后面就有了核对的依据。
平面上有两根连杆。上臂 L1 = 0.20 m,前臂 L2 = 0.15 m。肩部角度 θ1 是从 x 轴量起的绝对角,肘部角度 θ2 是相对上臂的相对角。
先求肘部的位置。从肩部沿 θ1 方向走 L1,
肘部 = (L1·cos θ1, L1·sin θ1)
末端再从肘部继续走 L2。此时前臂所指的绝对方向是 θ1 + θ2。把相对角转换成绝对角的这一行,是关键所在。
x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)
代入 θ1 = 30°、θ2 = 45°,
x = 0.20 × cos(30°) + 0.15 × cos(75°)
= 0.20 × 0.8660254 + 0.15 × 0.2588190
= 0.1732051 + 0.0388229
= 0.2120279
y = 0.20 × sin(30°) + 0.15 × sin(75°)
= 0.20 × 0.5000000 + 0.15 × 0.9659258
= 0.1000000 + 0.1448889
= 0.2448889
末端在 (0.212028, 0.244889)。
再加一根连杆,同样的规律会重复出现。取 L3 = 0.10 m、θ3 = -60°,第三根连杆的绝对方向就是 30 + 45 - 60 = 15°,
x = 0.2120279 + 0.10 × cos(15°) = 0.2120279 + 0.0965926 = 0.3086205
y = 0.2448889 + 0.10 × sin(15°) = 0.2448889 + 0.0258819 = 0.2707708
规律很明显了。每根连杆都沿着「自己之前所有角度之和」这个方向,前进自己的长度。 平面手臂用这个就够了,而且不管关节数量是多少,实际上用的都是同一个式子。
但在关节轴线各自指向不同方向的三维手臂上,这套方法就不成立了。肩部第一根轴可能是竖直的,第二根轴却是水平的,手腕的三根轴又互相正交。角度没法简单相加。从这里开始,就必须请出矩阵了。
DH 参数 —— 用四个数字描述一根连杆
连接一根连杆本来需要六个自由度:三个旋转加三个平移。可是机器人的连杆并不是随便接在一起的,存在关节轴线,连杆连接的正是这些轴线。利用这个结构、按规则去摆放坐标系,六个自由度就能减少到四个。
德纳维特-哈滕伯格约定(DH 约定)就是这套规则,四个数字分别是:
| 符号 | 名称 | 含义 |
|---|---|---|
a | 连杆长度 | 相邻两条关节轴之间公垂线的长度 |
α | 连杆扭角 | 绕公垂线量出的两条关节轴之间的夹角 |
d | 连杆偏距 | 沿关节轴量出的两条公垂线之间的距离 |
θ | 关节角 | 绕关节轴量出的两条公垂线之间的夹角 |
在转动关节里,θ 是变量,其余三个是常数。在移动关节里,d 是变量,其余三个是常数。一个关节对应一个变量,这种对应关系保持得很干净。
由这四个数字构成的变换,是四个基本变换串接而成的:
A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
展开之后是这样:
A = [ cos θ -sin θ·cos α sin θ·sin α a·cos θ ]
[ sin θ cos θ·cos α -cos θ·sin α a·sin θ ]
[ 0 sin α cos α d ]
[ 0 0 0 1 ]
这里有个必须知道的陷阱。DH 约定不止一种。
上面这套是标准(经典)DH,克雷格教材使用的改进 DH,乘法顺序不同。
标准DH: A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
改进DH: A_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)
两种约定放置坐标系的位置不同,所以同一台机器人的参数表在两种约定下也不一样。改进 DH 里,a 和 α 会带上 i-1 这个下标,这是判断一张表用的是哪种约定时最快的线索。把从论文里抄来的表原样塞进按标准 DH 写的代码里,手臂就会摆出奇怪的形状,而且往往要花好几天才能找到原因。
顺带一提,标准 DH 的乘法顺序也常被写成 Trans_z(d)·Rot_z(θ)·Trans_x(a)·Rot_x(α)。这不是错的,而是同一回事。Rot_z 和 Trans_z 作用在同一根轴上,所以可以交换顺序,Trans_x 和 Rot_x 同理。
来写一份三连杆平面手臂的 DH 表。因为所有关节轴都垂直于平面、彼此平行,所以扭角和偏距全部为零。
| i | a_i | α_i | d_i | θ_i |
|---|---|---|---|---|
| 1 | 0.20 | 0 | 0 | 变量θ1 |
| 2 | 0.15 | 0 | 0 | 变量θ2 |
| 3 | 0.10 | 0 | 0 | 变量θ3 |
用代码确认一下。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def dh_transform(a, alpha, d, theta):
"""标准(经典)DH约定下的连杆变换矩阵。
是Rot_z(theta) · Trans_z(d) · Trans_x(a) · Rot_x(alpha)展开后的形式。"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0., sa, ca, d],
[0., 0., 0., 1.]])
def forward_kinematics(dh_rows, q):
"""DH表的每一行是(a, alpha, d, theta_offset)。"""
T = np.eye(4)
for (a, alpha, d, offset), theta in zip(dh_rows, q):
T = T @ dh_transform(a, alpha, d, offset + theta)
return T
PLANAR_3R = [(0.20, 0.0, 0.0, 0.0),
(0.15, 0.0, 0.0, 0.0),
(0.10, 0.0, 0.0, 0.0)]
q = np.radians([30, 45, -60])
T = forward_kinematics(PLANAR_3R, q)
print("DH链给出的变换矩阵:")
print(T)
# 与手工推导出的闭式解做对比。
a = np.cumsum(q)
x = 0.20 * np.cos(a[0]) + 0.15 * np.cos(a[1]) + 0.10 * np.cos(a[2])
y = 0.20 * np.sin(a[0]) + 0.15 * np.sin(a[1]) + 0.10 * np.sin(a[2])
print(f"闭式解: x={x:.6f} y={y:.6f}")
print(f"DH结果: x={T[0,3]:.6f} y={T[1,3]:.6f}")
print("两个结果是否相同:", np.allclose([x, y], T[:2, 3]))
print(f"末端方向(三个角度之和) = {np.degrees(a[2]):.4f}度,"
f"从矩阵读到的值 = {np.degrees(np.arctan2(T[1,0], T[0,0])):.4f}度")
运行结果如下。
DH链给出的变换矩阵:
[[ 0.965926 -0.258819 0. 0.308621]
[ 0.258819 0.965926 0. 0.270771]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
闭式解: x=0.308621 y=0.270771
DH结果: x=0.308621 y=0.270771
两个结果是否相同: True
末端方向(三个角度之和) = 15.0000度,从矩阵读到的值 = 15.0000度
手工推导出的 (0.308621, 0.270771) 和矩阵链算出的值一致。而且从结果矩阵左上角 3×3 读出的末端方向,和三个角度之和 15 度相等。位置和姿态一次性都能算出来,这正是矩阵方法的优势。
用现代工具能直接搬用这张表。彼得·科克(Peter Corke)的 Robotics Toolbox for Python(PyPI 上的 roboticstoolbox-python,截至 2026 年 7 月为 1.3.1 版本,要求 Python 3.10 以上)把标准 DH 连杆定义为 RevoluteDH、改进 DH 连杆定义为 RevoluteMDH,而且这两者不能混用在同一台机器人上。约定用类而不是标志位来区分,反而成了一种安全保障。用 DHRobot 把它们组合起来,调用 .fkine(q),上面那段代码就浓缩成了一行。
ROS 生态用的是 URDF,而不是 DH。连杆和关节写在 XML 里,每个关节都要明确写出相对父连杆的位置和轴线。
<joint name="elbow" type="revolute">
<parent link="upper_arm"/>
<child link="forearm"/>
<origin xyz="0.20 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-2.6" upper="2.6" effort="1.5" velocity="3.0"/>
</joint>
URDF 里关节类型能用的字符串精确地只有六个:revolute、continuous、prismatic、fixed、floating、planar。而在 limit 元素里,effort 和 velocity 是必填项,lower 和 upper 是可选的。如果是旋转范围无限的关节,就用 continuous 并省略上下限。
URDF 比 DH 好的地方在于不需要按规则去摆放坐标系,原点放在方便的地方、轴的方向写对就行。代价是每个关节的参数增加到六个。对于要从 CAD 里照抄尺寸的人来说,URDF 方便得多;而要手工推导公式时,DH 更方便。
旋转表示法的取舍 —— 欧拉角、旋转矩阵、四元数
位置用三个数字就能搞定,但姿态不是这样。三维旋转的自由度是三,但怎么写这三个自由度有好几种选择,各自的代价也不同。
| 表示法 | 存储的数值个数 | 奇异点 | 插值 | 合成成本 | 主要用在哪里 |
|---|---|---|---|---|---|
| 旋转矩阵 | 9(6个约束) | 无 | 不能直接做 | 27次乘法 | 内部计算、运动学 |
| 欧拉角(rpy) | 3 | 万向锁 | 自然但危险 | 需转换成矩阵 | 人读写的场合、URDF |
| 轴角 | 4(或3) | 角度为0时轴未定义 | 中等 | 中等 | 旋转向量、微小旋转 |
| 四元数 | 4(1个约束) | 无 | slerp下很自然 | 16次乘法 | 姿态估计、插值、存储 |
来逐一看看每一项的代价。
旋转矩阵在计算上最方便。旋转一个向量只需一次乘法,合成也只需一次乘法。代价是用九个数表示三个自由度,因此带来六个约束:各列必须互相正交,且长度为 1。浮点误差累积后,这个约束会逐渐被打破,放任不管,物体就会微妙地拉伸或扭曲。所以需要定期做重新正交化。
欧拉角对人最友好。说横滚 30 度、俯仰 0 度、偏航 90 度,谁都能想象出来。URDF 的 rpy 属性就是这个,约定是按固定轴 x、y、z 顺序旋转,写成矩阵就是 Rz(yaw)·Ry(pitch)·Rx(roll)。
问题是万向锁。当第二个角度到达 90 度时,第一根轴和第三根轴会指向同一个方向,于是不同的角度组合会产生同一个旋转。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def rotation_rpy(roll, pitch, yaw):
"""与URDF的rpy相同的约定。按固定轴x、y、z顺序旋转,
写成矩阵就是Rz(yaw) · Ry(pitch) · Rx(roll)。"""
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def rpy_from_rotation(R):
"""从旋转矩阵反推rpy。关键在于分母是cos(pitch)。"""
pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
roll = np.arctan2(R[2, 1], R[2, 2])
yaw = np.arctan2(R[1, 0], R[0, 0])
return np.degrees([roll, pitch, yaw])
A = rotation_rpy(np.radians(0), np.radians(90), np.radians(30))
B = rotation_rpy(np.radians(-30), np.radians(90), np.radians(0))
print("roll=0, pitch=90, yaw=30 与 roll=-30, pitch=90, yaw=0")
print(" 两个矩阵是否相同:", np.allclose(A, B), " 最大差异:", np.abs(A - B).max())
# 在万向锁的边缘,看看极小的姿态变化会把rpy晃动多少。
base = rotation_rpy(np.radians(0), np.radians(89.9), np.radians(30))
print("\n反推(0, 89.9, 30)得到:", np.round(rpy_from_rotation(base), 4))
for eps in (0.001, 0.01, 0.1):
perturbed = rotation_rpy(np.radians(eps), 0.0, 0.0) @ base
print(f" 绕x轴晃动{eps:>5}度,rpy = {np.round(rpy_from_rotation(perturbed), 4)}"
f" (矩阵最大差异 {np.abs(perturbed - base).max():.6f})")
运行结果如下。
roll=0, pitch=90, yaw=30 与 roll=-30, pitch=90, yaw=0
两个矩阵是否相同: True 最大差异: 3.0616169978683824e-17
反推(0, 89.9, 30)得到: [ 0. 89.9 30. ]
绕x轴晃动0.001度,rpy = [ 0.4937 89.8995 30.4937] (矩阵最大差异 0.000017)
绕x轴晃动 0.01度,rpy = [ 4.715 89.8946 34.715 ] (矩阵最大差异 0.000175)
绕x轴晃动 0.1度,rpy = [30. 89.8268 60. ] (矩阵最大差异 0.001745)
前两行正是万向锁本身。两组完全不同的角度组合是同一个旋转,差异是 3×10⁻¹⁷,在浮点精度这个量级上等同于零。
后三行是实务中更痛的部分。在俯仰角为 89.9 度的姿态下,把手臂绕 x 轴晃动了区区 0.01 度,反推出来的横滚角却从 0 度变成了 4.715 度,偏航角从 30 度变成了 34.715 度。输入 0.01 度,输出 4.7 度,放大了 470 倍。 晃动 0.1 度,横滚角就变成 30 度,偏航角变成 60 度。
原因出在反推公式的分母上。用来除开横滚角和偏航角的量是 cos(pitch),而俯仰角越接近 90 度,这个量就越趋近于零。用接近零的数去除,会把输入中很小的噪声放大成很大的输出。
实际的症状是这样的:手臂经过这个姿态附近时,哪怕只是编码器噪声或浮点误差,横滚角和偏航角的读数也会剧烈跳动;如果直接拿这些值去插值,手腕就会转上一整圈。物理上手臂几乎没动,控制器却被下达了移动 60 度的指令。手腕关节被缠绕得死死的事故,相当一部分就是这么来的。
四元数没有万向锁。四个数 (w, x, y, z) 只带一个约束(长度为 1),正好对应三个自由度,而且表示是连续的。两个姿态之间的插值(slerp)很自然,合成旋转也比矩阵便宜。
代价有两个。一是人没法只看数字就想象出姿态。二是双重覆盖:q 和 -q 表示的是同一个旋转。在两个四元数之间插值时如果不对齐符号,就会绕远路而不是走近路。
实务中的答案大致是这样:存储和插值用四元数,计算用旋转矩阵,只有给人看的时候才用欧拉角。 三种都用,只是各自待在自己擅长的那个位置上。
一个符号错了,实际会发生什么
正运动学代码里最常见的 bug 并不复杂,就是一个符号,一个角度约定。
来看一个关节角度方向反了的情形。组装舵机时如果齿轮从反方向咬合,指令角度增大时关节反而会往相反方向转。代码本身没变,相当于 θ2 的符号被反转了。
import numpy as np
L1, L2 = 0.20, 0.15
def fk(theta1, theta2):
return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])
good = fk(np.radians(30), np.radians(45))
bad = fk(np.radians(30), np.radians(-45)) # 只反转了θ2的符号
print(f"正确的末端 : ({good[0]:.6f}, {good[1]:.6f})")
print(f"符号反转 : ({bad[0]:.6f}, {bad[1]:.6f})")
print(f"偏差距离 : {np.linalg.norm(good - bad) * 1000:.3f} mm")
print(f"理论值 2·L2·sin(45°) = {2 * L2 * np.sin(np.radians(45)) * 1000:.3f} mm")
print("\n在原点附近很难看出来:")
for d2 in (1, 5, 15, 45, 90):
g, b = fk(np.radians(30), np.radians(d2)), fk(np.radians(30), np.radians(-d2))
print(f" θ2={d2:3}° -> 误差 {np.linalg.norm(g - b) * 1000:7.3f} mm")
运行结果如下。
正确的末端 : (0.212028, 0.244889)
符号反转 : (0.318094, 0.061177)
偏差距离 : 212.132 mm
理论值 2·L2·sin(45°) = 212.132 mm
在原点附近很难看出来:
θ2= 1° -> 误差 5.236 mm
θ2= 5° -> 误差 26.147 mm
θ2= 15° -> 误差 77.646 mm
θ2= 45° -> 误差 212.132 mm
θ2= 90° -> 误差 300.000 mm
在 45 度处偏差了 212 毫米——而整条手臂的总长度也才 350 毫米。而且误差的大小正好等于 2·L2·sin(θ2),因为末端是相对上臂的延长线被镜像反射了过去。
最后那张表道出了这个 bug 的性格。在零点附近几乎看不出来。 在 θ2 = 1° 处误差只有 5 毫米,很容易被误认为是装配公差或者背隙。所以如果只在原点姿态附近测试,能顺利通过;一旦到了实际工作角度,就会完全跑偏。
还有几个性格相似的 bug。
弧度和角度的混淆。 numpy 的三角函数一律接收弧度。直接把 30 传进去,会被解读成 30 弧度,也就是 1719 度,算出的是手臂转了四圈多的姿态。好在这个 bug 的结果太离谱了,通常一下就能发现。
绝对角和相对角的混淆。 上面式子里的 θ2 是相对上臂的相对角。如果编码器给出的是绝对角,直接把那个值代进去,θ1 就会被多加一次。当 θ1 = 0 时,两种约定给出的值相同,所以这个 bug 在原点姿态下同样看不出来。
零点偏移。 DH 表里的 θ 是按约定所定的基准量出来的角度,而舵机报告的角度是从自己的机械零点量起的。两者不一致时会相差一个常数,这个常数需要填进 DH 表的偏移项里。上面代码中写成 offset + theta 的地方,正是这个位置。
有一种验证方法可以一次性抓住这四种情况。一次只动一个关节。 把其余关节都保持在零位,把一个关节转到已知角度,再对比计算预测的末端位置和用尺子量出来的实际位置。要是六个关节同时动、再想找原因,就分不清是哪个关节的问题了。
结语 —— 把坐标系精确地写下来,就完成了一半
正运动学的数学并不难。给旋转矩阵加一行一列,把平移也变成乘法,再按顺序把它们乘起来。就这些。
难的不是数学,而是约定。角度从哪里量起,哪个方向是正,是绝对角还是相对角,用的是哪种 DH 约定,零点在哪里。这五件事不写进文档里,几周后的自己一定会弄错。
而且一旦正运动学出错,建立在它之上的一切都会悄悄跟着出错。 逆运动学是正运动学的逆,会直接继承同样的符号错误;雅可比矩阵是正运动学的偏导数,同样会继承。相机标定、碰撞检测、轨迹规划都是一样。追着一个奇怪的症状查了好几天,最后发现只是 DH 表里的一个符号,这种情况并不少见。
所以正运动学写完之后,一定要用尺子实测一遍。一个关节一个关节地,用已知角度,和真实距离对比。只要计算和尺子量出的结果在 5 毫米以内吻合,在这个基础上不管往上叠加什么都行。
下一步是把这个函数反过来。逆运动学那篇会讲清楚为什么那要难得多,以及这里搭出来的这些矩阵是怎么一路通向雅可比矩阵的。这篇文章里出现的线性代数和三角学,该学到什么程度,已经按顺序整理在机器人学所需的数学那篇里了。