机器人运动学：从 DH 参数法到数值逆解#

为什么要从运动学开始#

任何一个机械臂项目——无论是六轴工业机械臂还是四轴 SCARA——运动学都是最底层的数学模型。它回答了机器人最基本的两个问题：

正运动学（FK）：给定各关节角度，末端执行器在哪里？
逆运动学（IK）：想让末端到某个位置，各关节应该转多少度？

DH 参数法#

Denavit-Hartenberg（DH）参数法用 4 个参数 描述相邻关节间的空间关系：

参数	含义	类型
a	连杆长度	常量
α	连杆扭转角	常量
d	关节偏移	旋转关节为变量
θ	关节角度	旋转关节为变量

齐次变换矩阵#

^{i-1}T_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}

对于六轴机械臂，末端位姿就是 6 个矩阵连乘：

$^0T_6 = ^0T_1 \cdot ^1T_2 \cdot ^2T_3 \cdot ^3T_4 \cdot ^4T_5 \cdot ^5T_6$

Python 实现正运动学#

1
import numpy as np
2

3
def dh_transform(theta, d, a, alpha):
4
    """返回 4x4 DH 齐次变换矩阵"""
5
    ct, st = np.cos(theta), np.sin(theta)
6
    ca, sa = np.cos(alpha), np.sin(alpha)
7
    return np.array([
8
        [ct, -st*ca,  st*sa, a*ct],
9
        [st,  ct*ca, -ct*sa, a*st],
10
        [0,      sa,     ca,    d],
11
        [0,       0,      0,    1]
12
    ])
13

14
def forward_kinematics(dh_params, joint_angles):
15
    """计算末端位姿"""
16
    T = np.eye(4)
17
    for i, (d, a, alpha) in enumerate(dh_params):
18
        T = T @ dh_transform(joint_angles[i], d, a, alpha)
19
    return T

逆运动学：数值解法#

解析法只适用于特定构型的机械臂（如 Pieper 准则下的六轴臂）。更通用的方案是基于雅可比伪逆的数值迭代：

1
def inverse_kinematics_numeric(dh_params, target_pose, initial_guess, max_iter=100, tolerance=1e-6):
2
    """数值 IK：雅可比伪逆迭代法"""
3
    q = np.array(initial_guess, dtype=float)
4

5
    for iteration in range(max_iter):
6
        # 正运动学
7
        T_current = forward_kinematics(dh_params, q)
8

9
        # 位置误差和姿态误差 → 6维误差向量
10
        pos_error = target_pose[:3, 3] - T_current[:3, 3]
11
        rot_error = 0.5 * np.cross(T_current[:3,:3].T @ target_pose[:3,:3]).diagonal()
12
        error = np.concatenate([pos_error, rot_error])
13

14
        if np.linalg.norm(error) < tolerance:
15
            return q
16

17
        # 计算雅可比矩阵（数值微分）
18
        J = compute_jacobian(dh_params, q)
19

20
        # 阻尼最小二乘法（避免奇异）
21
        lambda_damp = 0.01
22
        delta_q = J.T @ np.linalg.solve(
23
            J @ J.T + lambda_damp**2 * np.eye(6), error
24
        )
25
        q += delta_q
26

27
    raise RuntimeError("IK did not converge")