机器人运动学基础：正运动学与逆运动学、雅可比矩阵、轨迹规划基础

大家好，我是你们的讲师。今天我们来聊聊机器人运动学。说实话，这部分内容是整个轨迹精度控制的地基。地基不稳，后面盖多高的楼都得塌。我在做第一个工业机器人项目时，就吃过这个亏——正逆解算错了，结果机器人直接撞上了夹具。嗯，从那以后，我对运动学就格外敬畏。

这一节，我会把正运动学、逆运动学、雅可比矩阵，还有梯形/S形速度曲线这些基础概念，用我自己的经验串起来讲。你想想看，搞懂了这些，你才能理解后面为什么模型辨识能提升轨迹精度。

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

正运动学，说白了就是：我知道每个关节转了多少度，求末端执行器在空间中的位置和姿态。这其实是个“正向推导”的过程，相对简单。

我个人习惯用DH参数法（Denavit-Hartenberg）来建模。你只需要四个参数：

θ：关节角（绕Z轴旋转）
d：连杆偏距（沿Z轴平移）
a：连杆长度（沿X轴平移）
α：连杆扭角（绕X轴旋转）

然后，每个关节对应一个齐次变换矩阵。把这些矩阵连乘起来，就得到了末端位姿。公式长这样：

# 举个六轴机器人的例子（简化版）
import numpy as np

def dh_transform(theta, d, a, alpha):
    """计算单个关节的齐次变换矩阵"""
    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

# 假设6个关节角度已知
joint_angles = [0.1, 0.2, -0.3, 0.0, 0.5, 0.1]
# 你的DH参数表（这里只示意）
dh_params = [...]  # 每个元素是 (theta, d, a, alpha)

T_06 = np.eye(4)
for i in range(6):
    theta = joint_angles[i] + dh_params[i][0]  # 注意：theta包含关节变量
    T_i = dh_transform(theta, dh_params[i][1], dh_params[i][2], dh_params[i][3])
    T_06 = T_06 @ T_i

# T_06 就是末端位姿矩阵
print("末端位置:", T_06[:3, 3])

我的小技巧： 写正运动学代码时，一定要先验证单个关节的变换矩阵是否正确。我习惯用单位关节角（全为0）去测试，看末端是否在“零位”姿态。这一步能帮你排除80%的DH参数错误。

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

逆运动学就麻烦多了。你告诉机器人“我要去这里”，它得算出每个关节该转多少度。这玩意儿没有通用解析解，尤其是六轴机器人，经常有多解、无解、奇异点等问题。

我在项目中遇到过最头疼的事：逆解算出来一组关节角，结果机器人走的时候突然“翻腕”——因为选了另一组解。所以，选解策略非常重要。

常用的方法有两种：

解析法（封闭解）：适用于满足Pieper准则的机器人（后三个关节轴线交于一点）。速度快，精度高。
数值法（迭代法）：比如牛顿-拉夫森法。通用性强，但计算慢，且依赖初值。

我个人更推荐解析法，只要你的机器人结构允许。为什么呢？因为数值法在奇异点附近容易发散。我曾经调试一个打磨机器人，逆解迭代死活不收敛，最后发现是末端靠近奇异位形了。

避坑指南： 逆运动学一定要做多解筛选。我一般会按“关节位移最小”原则选解，同时检查是否超限。另外，连续两个控制周期之间，关节角不能跳变太大——否则机器人会抖得像筛糠。

2.3 雅可比矩阵：速度与力的映射

雅可比矩阵，听起来高大上，其实它就是关节速度到末端速度的线性映射。公式很简单：

v = J(q) · q̇

其中v是末端速度（线速度+角速度），q̇是关节速度，J就是雅可比矩阵。

为什么它重要？因为轨迹精度控制里，我们经常需要把笛卡尔空间的误差映射回关节空间去修正。没有雅可比，你都不知道该调哪个关节。

计算雅可比矩阵，我推荐用矢量积法。对于旋转关节i，它对末端线速度和角速度的贡献是：

# 伪代码：计算雅可比矩阵的列
for i in range(6):
    # z_i 是关节i的Z轴方向向量（在基坐标系下）
    # p_i 是关节i的原点位置（在基坐标系下）
    # p_n 是末端位置
    J_v[:, i] = z_i × (p_n - p_i)  # 线速度部分
    J_w[:, i] = z_i                # 角速度部分

嗯，这里要注意：雅可比矩阵是位形相关的。也就是说，机器人姿势不同，J矩阵就不同。所以每次控制周期都得重新算一遍。

    核心应用： 在轨迹精度控制中，我们常用雅可比伪逆（J⁺）来做速度级逆解。比如你希望末端以速度v_d运动，那么关节速度就是 q̇ = J⁺ · v_d。这比位置级逆解更平滑，也更适合实时控制。

2.4 轨迹规划基础：梯形与S形速度曲线

轨迹规划，说白了就是让机器人怎么从A点走到B点。你不能让它突然启动、突然停止——那样会震坏机械结构，精度也别想了。

最常见的两种曲线：

类型	特点	加速度	适用场景
梯形速度曲线	加速-匀速-减速三段	阶跃变化（有冲击）	对精度要求不高的搬运
S形速度曲线	加加速-匀加速-减加速-匀速-加减速-匀减速-减减速七段	连续变化（平滑）	高精度轨迹跟踪、打磨、涂胶

你想想看，梯形曲线在加减速切换点，加速度会突变。这就意味着力会突变，机器人会抖一下。对于轨迹精度来说，这一抖可能就是几毫米的误差。

所以我个人习惯，只要做轨迹精度相关的项目，一律用S形速度曲线。虽然计算复杂一点，但换来的是平滑的运动和更高的重复定位精度。

下面是一个S形曲线的核心逻辑：

def s_curve_profile(t, T, v_max, a_max, j_max):
    """
    简化的S形曲线位置/速度/加速度计算
    t: 当前时间
    T: 总运动时间
    v_max: 最大速度
    a_max: 最大加速度
    j_max: 最大加加速度（Jerk）
    """
    # 这里只展示分段逻辑，实际需要根据时间计算各段
    # 七段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速
    if t < T/7:
        # 加加速段：jerk = j_max
        pass
    elif t < 2*T/7:
        # 匀加速段：jerk = 0, acc = a_max
        pass
    # ... 省略中间段
    else:
        # 减减速段：jerk = -j_max
        pass
    return position, velocity, acceleration

我的经验： 实际工程中，S形曲线的参数（v_max, a_max, j_max）不能设得太激进。我一般会留20%的余量。为什么呢？因为模型辨识出来的参数有误差，留点余量能防止电机过载。另外，加加速度（Jerk）的限制是提升轨迹精度的关键——它直接决定了机器人启停时的振动大小。

知识体系总览

为了让你更直观地理解这一节的内容，我画了一张图。它展示了正逆运动学、雅可比矩阵和轨迹规划之间的关系：

这张图你看懂了吗？从左到右是正运动学，从右到左是逆运动学。雅可比矩阵连接了关节速度和末端速度，而轨迹规划则决定了整个运动过程的“节奏”。这四个部分环环相扣，任何一个环节出问题，轨迹精度都无从谈起。

好了，这一节的内容就到这里。记住：运动学是基础，基础不牢，地动山摇。下一节我们会深入模型辨识，到时候你会发现，今天讲的这些全都会用上。

📚 课程目录

机器人运动学基础：正运动学与逆运动学、雅可比矩阵、轨迹规划基础

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

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

2.3 雅可比矩阵：速度与力的映射

2.4 轨迹规划基础：梯形与S形速度曲线

知识体系总览