1. 机械臂运动学基础：从DH参数到正逆解

机械臂的运动学，说白了就是研究“关节怎么动，末端去哪”的问题。你不需要关心力有多大、电机有多烫，只关心角度和位置的关系。我刚开始接触这个领域时，总觉得这不过是几何题，直到第一次调试六轴臂时发现末端死活对不准目标点……嗯，从那以后我再也不敢小看运动学了。

1.1 什么是运动学？为什么它这么重要？

运动学是机械臂的“骨架”与“关节”之间的数学契约。它回答两个核心问题：

• 正运动学：已知每个关节的角度，求末端在空间中的位置和姿态。
• 逆运动学：已知末端的目标位姿，反推每个关节应该转多少度。

你想想看，实际项目中90%的调试时间都花在逆解上。我记得有一次在产线上，六轴臂抓取一个倾斜放置的工件，正解算出来完全正确，但逆解死活跳不出奇异点——最后发现是DH参数表里一个符号写反了。

1.2 DH参数：机械臂的“身份证”

Denavit-Hartenberg参数，简称DH参数，是描述相邻两个关节之间相对位置的四个值。我个人习惯用改进DH（Modified DH），因为它在处理树形结构时更直观。

参数	符号	含义	单位
连杆长度	ai-1	从Zi-1到Zi沿Xi-1的距离	mm
连杆扭角	αi-1	从Zi-1到Zi绕Xi-1的转角	rad
连杆偏距	di	从Xi-1到Xi沿Zi的距离	mm
关节角	θi	从Xi-1到Xi绕Zi的转角	rad

举个例子，一个常见的六轴工业机器人（比如UR5）的DH参数表长这样：

# UR5 改进DH参数表（单位：mm, rad）
a = [0, -0.425, -0.392, 0, 0, 0]
alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]
d = [0.089, 0, 0, 0.109, 0.094, 0.082]
theta = [q1, q2, q3, q4, q5, q6]  # 变量

这里每个关节的theta是变量，其他参数都是固定值。我曾经犯过一个低级错误：把a和d的单位搞混了，结果正解算出来的末端位置差了20多厘米——嗯，那次排查花了我整整一个下午。

1.3 正运动学：从关节空间到笛卡尔空间

正运动学的核心就是矩阵连乘。每个关节对应一个齐次变换矩阵T_i，把所有T_i乘起来，就得到末端相对于基座的变换矩阵。

公式其实不复杂：

T_i = Rot_z(theta_i) * Trans_z(d_i) * Trans_x(a_i) * Rot_x(alpha_i)

写成代码就是：

import numpy as np

def dh_transform(a, alpha, d, theta):
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = 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]
    ])

# 正解：连乘所有关节
T_06 = np.eye(4)
for i in range(6):
    T_06 = T_06 @ dh_transform(a[i], alpha[i], d[i], theta[i])

我个人习惯在调试时把每个中间矩阵都打印出来看一眼。为什么？因为一旦某个关节的符号反了，中间矩阵的某个元素会突然“跳变”，一眼就能发现。

1.4 逆运动学：从笛卡尔空间回到关节空间

逆解才是真正的难点。六轴臂的逆解通常有两种方法：

• 解析法：通过几何关系直接推导出封闭解。速度快，但只适用于满足Pieper准则的机器人（后三个关节轴线交于一点）。
• 数值法：用迭代优化（比如牛顿-拉夫森法）逼近解。通用性强，但可能陷入局部最优或奇异点。

我建议优先用解析法。为什么？因为数值法在奇异点附近会“抖”，而且你很难判断它收敛到的是不是你要的那个构型。

以UR5为例，它的后三个关节轴线交于一点（腕部），所以可以用解析法。核心思路是：

1. 根据末端位姿反推腕部中心位置（前三个关节决定）。
2. 用几何法解出q1、q2、q3。
3. 根据末端姿态反推q4、q5、q6。

代码片段（只展示q1的求解）：

def inverse_ur5(T_06):
    # 提取末端位置和姿态
    p = T_06[:3, 3]
    R = T_06[:3, :3]
    
    # 腕部中心位置（假设后三个关节交点在末端坐标系下的偏移为d6）
    d6 = 0.082
    p_wrist = p - d6 * R[:, 2]
    
    # 求解q1（有两个可能解：肘部朝左/朝右）
    q1_1 = np.arctan2(p_wrist[1], p_wrist[0])
    q1_2 = q1_1 + np.pi
    # ... 后续求解q2, q3, q4, q5, q6

1.5 知识体系总览

下面这张图概括了本章的核心逻辑：从DH参数出发，经过正解得到末端位姿，再通过逆解回到关节角度。整个过程构成了运动学的闭环。

1.6 工程中的常见坑与应对

最后分享几个我踩过的坑，希望能帮你省点时间：

• DH参数符号搞反：尤其是α的正负号，不同厂家定义可能不同。拿到机器人手册后，先用一个已知姿态验证一下。
• 逆解多解选择：不要随便选第一个解。我习惯按“关节运动量最小”来选，同时避开关节限位和奇异区域。
• 奇异点处理：当机器人接近奇异位形时，逆解会变得不稳定。我的做法是在奇异点附近切换到数值法，并加入阻尼项。
• 浮点误差：矩阵连乘16次后，误差会累积。建议在关键位置用double精度，并定期做正交化修正。