机器人运动学基础：正运动学与逆运动学，DH参数法，坐标变换与齐次矩阵

各位同学，欢迎来到第二章。这一章是机器人学的硬核基础，也是后续关节标定和任务执行的数学根基。说实话，很多做强化学习的同学一开始会忽略运动学，觉得“反正有神经网络去学”。但我在实际项目中吃过亏——模型学了半天，结果机械臂末端位置算错了，整个策略全废。所以，这一章我们得把底子打牢。

2.1 坐标变换与齐次矩阵：机器人的“语言”

机器人运动学，说白了就是描述各个关节和末端执行器之间的位置和姿态关系。你想想看，一个六轴机械臂，每个关节都在动，你怎么知道末端到底在哪儿？答案就是坐标变换。

我习惯把坐标变换比作“问路”。你在A点，想知道B点怎么走，就需要知道从A到B的位移和方向。在机器人里，我们用齐次矩阵（Homogeneous Transformation Matrix）来干这件事。

| R   t |
| 0   1 |

为什么非得用4x4矩阵？因为这样可以把旋转和平移统一成一次矩阵乘法。否则你得先旋转再平移，写代码麻烦，还容易出错。我记得第一次手写运动学代码时，忘了加齐次坐标，结果算出来的位置全是错的，排查了半天……嗯，从那以后我再也不敢省这一步了。

2.2 DH参数法：标准化你的机器人模型

有了齐次矩阵，我们怎么描述相邻两个关节之间的关系？Denavit-Hartenberg参数法，也就是DH参数，就是干这个的标准化工具。

DH参数用四个值来描述一个连杆和关节：

这里有个坑，我必须要提醒你：DH参数有两种主流约定——标准DH和改进DH。标准DH把坐标系建在连杆末端，改进DH建在连杆起始端。我在项目中遇到过，团队里两个人用了不同约定，结果仿真和实机对不上，折腾了两天。所以，一开始就要统一约定，并在代码里写清楚。

2.3 正运动学：从关节空间到任务空间

正运动学，就是已知每个关节的角度（或位移），求末端执行器的位姿。说白了，你给机器人一组关节角度，它告诉你手在哪儿、朝哪个方向。

计算过程很简单：把每个相邻关节的齐次矩阵连乘起来。

# Python伪代码：正运动学计算
def forward_kinematics(joint_angles, dh_params):
    # dh_params: 每行 [a, alpha, d, theta_offset]
    T = np.eye(4)  # 初始为单位矩阵
    for i in range(len(joint_angles)):
        a, alpha, d, theta_offset = dh_params[i]
        theta = joint_angles[i] + theta_offset
        
        # 构建当前关节的齐次矩阵
        Ti = np.array([
            [np.cos(theta), -np.sin(theta)*np.cos(alpha),  np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
            [np.sin(theta),  np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
            [0,             np.sin(alpha),                 np.cos(alpha),               d],
            [0,             0,                             0,                            1]
        ])
        T = T @ Ti  # 矩阵连乘
    return T  # 末端位姿

正运动学是唯一的、确定的。你给一组关节角，末端位姿只有一个答案。这在强化学习中非常有用——你可以精确地知道当前状态对应的末端位置，从而设计更准确的奖励函数。

2.4 逆运动学：从任务空间回到关节空间

逆运动学就麻烦多了。已知末端位姿，求关节角度。这玩意儿在数学上是个多解问题，甚至可能无解。

为什么会这样？你想想看，一个六轴机械臂，末端在空间里只有6个自由度（3个位置+3个姿态），但关节可能有6个甚至7个。多出来的自由度就是冗余，导致同一个末端位姿对应无数种关节构型。

我常用的逆运动学解法有几种：

• 解析法：针对特定构型（如PUMA 560）推导封闭解。速度快，但通用性差。
• 数值法（如牛顿-拉夫森）：迭代求解，通用性强，但可能陷入局部最优。
• 优化法：把IK当成一个优化问题，用梯度下降或进化算法求解。

2.5 知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张图。它把坐标变换、DH参数、正运动学和逆运动学串在了一起。

2.6 实战中的几个关键点

最后，我分享几个在强化学习项目中用运动学的实战经验：

• 坐标系统一：所有传感器、执行器、仿真环境的坐标系必须对齐。我见过最离谱的bug，是相机坐标系和机器人基坐标系差了90度，模型学了三天都没发现。
• 奇异性处理：当机械臂处于奇异位形时，逆运动学的雅可比矩阵会降秩，导致求解失败。在训练中，我通常会在奖励函数里加一个“远离奇异位形”的惩罚项。
• 精度与速度的权衡：正运动学计算很快，但逆运动学如果迭代求解，可能会拖慢训练速度。我的做法是：在仿真中预计算IK查找表，训练时直接查表，省时省力。