3. 机械臂运动学基础：坐标系与位姿描述、正运动学、逆运动学

各位同学，咱们今天聊聊机械臂运动学。说实话，这是整个抓取任务的“地基”。你想想看，如果连机器人手在哪儿、怎么动都搞不清楚，后面那些花里胡哨的规划、控制全是空中楼阁。

我个人习惯把运动学分成三块来理解：坐标系与位姿、正运动学、逆运动学。说白了，就是“我在哪”、“我关节怎么动才能到那”、“到了那之后手在什么位置”。咱们一个一个来。

3.1 坐标系与位姿描述

先问个问题：你告诉别人“把杯子拿过来”，对方得知道杯子在哪儿吧？机械臂也一样。它需要一个“参考系”。

在机器人世界里，最常用的就是笛卡尔坐标系（也叫直角坐标系）。一个点用 (x, y, z) 表示。但光有位置不够，机械臂末端还有个“朝向”问题。比如抓一个螺丝刀，你握的方向不对，就抓不住。

所以，我们引入了位姿这个概念。位姿 = 位置 + 姿态。位置用坐标，姿态用旋转矩阵、欧拉角或者四元数来描述。

  核心概念： 一个刚体在空间中的自由度是6个（3个平移 + 3个旋转）。机械臂末端执行器，本质上就是一个刚体。

我记得刚入行时，有个老工程师跟我说：“坐标系搞错了，后面全白干。” 当时我不信，直到有一次在项目现场，因为把工具坐标系（TCP）标定偏了2毫米，结果抓取精密零件时直接撞飞了。嗯，从那以后，我每次部署前都会反复确认坐标系。

常用的坐标系有：

世界坐标系（基坐标系）：固定在机器人底座，是全局参考。
工具坐标系（TCP）：固定在末端执行器上，抓取时用这个。
工件坐标系：固定在目标物体上，方便编程。

小技巧： 我个人习惯在调试时，先用示教器把TCP移动到几个关键点，然后通过“三点法”或“六点法”标定工具坐标系。别偷懒，这一步精度直接决定抓取成功率。

3.2 正运动学

正运动学，说白了就是：你给我每个关节的角度，我告诉你末端在哪儿。

这是一个“确定”的过程。你给一组关节值，末端位姿是唯一确定的。数学上，我们用齐次变换矩阵来描述相邻连杆之间的坐标变换。

对于常见的六轴工业机器人，正运动学推导通常用Denavit-Hartenberg（D-H）参数法。每个关节对应4个参数：

参数	含义
a_i-1	连杆长度
α_i-1	连杆扭转角
d_i	连杆偏距
θ_i	关节角

有了这4个参数，就能写出相邻连杆的变换矩阵。然后从基座一路乘到末端，就得到了末端位姿。

代码实现其实不复杂，我给大家一个简单的Python示例，用的是标准D-H法：

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)
    
    T = np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ]
    ])
    return T

# 假设一个2轴机械臂的D-H参数
a = [1.0, 0.8]   # 连杆长度
alpha = [0, 0]   # 扭转角
d = [0, 0]       # 偏距
theta = [0.5, 1.2]  # 关节角（弧度）

T_01 = dh_transform(a[0], alpha[0], d[0], theta[0])
T_12 = dh_transform(a[1], alpha[1], d[1], theta[1])
T_02 = np.dot(T_01, T_12)  # 末端位姿矩阵

print("末端位姿矩阵：\n", T_02)

注意： 不同品牌的机器人（如ABB、KUKA、发那科）使用的D-H约定可能不同（标准D-H vs 改进D-H）。我曾经因为没注意这个，把库卡机器人的运动学算错了整整两天。一定要先看手册！

3.3 逆运动学

逆运动学就更有意思了。它是正运动学的反问题：你给我末端位姿，我反推出各个关节的角度。

为什么说它“有意思”？因为逆解通常不唯一。同一个位置，机械臂可能有多种“姿势”到达（比如肘部朝上或朝下）。这就是所谓的多解性。

逆运动学的解法主要有两种：

解析法（封闭解）：通过几何或代数推导出公式。速度快，适合实时控制。但只适用于满足Pieper准则的机器人（相邻三个关节轴交于一点）。
数值法（迭代法）：比如牛顿-拉夫森法、雅可比伪逆法。通用性强，但计算慢，且可能陷入局部最优。

在实际项目中，我90%的情况都用解析法。为什么？因为工业机器人大多满足Pieper准则，而且解析法稳定、可预测。数值法我一般只在奇异位形附近或者特殊构型时才用。

举个例子，一个简单的2轴平面机械臂，逆解可以用余弦定理直接算：

import numpy as np

def inverse_kinematics_2r(x, y, l1, l2):
    """2R平面机械臂逆运动学"""
    # 计算腕部到基座的距离
    r = np.sqrt(x**2 + y**2)
    
    # 检查可达性
    if r > (l1 + l2) or r < abs(l1 - l2):
        print("目标点不可达！")
        return None
    
    # 余弦定理求关节2
    cos_theta2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)
    theta2 = np.arccos(cos_theta2)  # 肘部朝下解
    # theta2 = -np.arccos(cos_theta2)  # 肘部朝上解
    
    # 求关节1
    theta1 = np.arctan2(y, x) - np.arctan2(l2 * np.sin(theta2), 
                                            l1 + l2 * np.cos(theta2))
    
    return theta1, theta2

# 测试
x, y = 1.5, 0.5
l1, l2 = 1.0, 0.8
theta1, theta2 = inverse_kinematics_2r(x, y, l1, l2)
print(f"关节角: θ1={theta1:.3f} rad, θ2={theta2:.3f} rad")

  避坑指南： 我曾经在部署一个码垛任务时，逆解选了“肘部朝上”的解，结果机器人运动到一半，肘部撞到了旁边的立柱。从那以后，我每次选解时都会加一个“碰撞检测”的筛选条件。选解不只是数学问题，更是工程问题。

3.4 知识体系总览

为了让大家更直观地理解这三块内容的关系，我画了一张图：

这张图把三者的关系说清楚了。坐标系是基础，正运动学是正向计算，逆运动学是反向求解。三者形成一个闭环，缺一不可。

好了，这一章的内容就到这里。运动学是机械臂编程的“内功”，练好了，后面学轨迹规划、动力学、力控制都会轻松很多。大家回去后，建议拿自己手边的机器人手册，对着D-H参数表手算一遍正运动学，再写个逆解程序试试。纸上得来终觉浅，绝知此事要躬行。

📚 课程目录

3. 机械臂运动学基础：坐标系与位姿描述、正运动学、逆运动学

3.1 坐标系与位姿描述

3.2 正运动学

3.3 逆运动学

3.4 知识体系总览