第1章：运动学模型——机器人运动学基础
各位同学，大家好。我是你们这门课的老朋友。
今天咱们聊点硬核的——运动学模型。说白了，就是研究机器人怎么动、动到哪、能不能动到的问题。我做了这么多年机器人抓取，发现很多同学一上来就调PID、调轨迹，结果机器人要么撞墙，要么抓不住东西。为什么？因为运动学没吃透。
1.1 正运动学：从关节到末端
正运动学，就是已知每个关节的角度，求末端执行器在空间中的位置和姿态。嗯，这个方向是确定的，没有歧义。
我记得刚入行时，带我的老工程师说：“正解是送分题，逆解才是送命题。” 后来我发现，正解虽然简单，但它是所有上层算法的基础。你连末端在哪都不知道，还谈什么轨迹跟踪？
常用的方法就是DH参数法。我习惯用改进型DH，因为它在处理树形结构机器人时更直观。来看一个简单的六轴机器人正解示例：
import numpy as np
def dh_transform(theta, d, a, alpha):
"""
标准DH参数变换矩阵
"""
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
# 六轴机器人正解示例
def forward_kinematics(joint_angles, dh_params):
T = np.eye(4)
for i in range(len(joint_angles)):
theta = joint_angles[i] + dh_params[i]['offset']
T = T @ dh_transform(theta, dh_params[i]['d'],
dh_params[i]['a'], dh_params[i]['alpha'])
return T
小技巧： 我建议你在调试时，先用一个已知的关节角度组合（比如所有关节归零），手动算一遍末端位置。这样能快速验证你的DH参数有没有写错。我曾经因为一个符号搞反，排查了整整两天。
1.2 逆运动学：从末端到关节
逆运动学就复杂多了。给定末端位姿，求关节角度。说白了，就是“反着算”。
为什么难？因为解不唯一。一个六轴机器人，同一个末端位置，可能有8组甚至更多解。你想想看，你的手臂够到同一个点，肘关节可以朝上也可以朝下，对吧？
我常用的方法分两种：
- 解析法： 针对特定构型（比如有腕部偏置的机器人），可以推导出封闭解。速度快，适合实时控制。
- 数值法： 比如牛顿-拉夫森迭代。通用性强，但需要好的初值，否则容易发散。
这里有个坑，我踩过很多次——奇异点。当机器人处于奇异位形时，逆解会失效。比如肘关节完全伸直时，你没法通过微调关节来改变末端位置。嗯，这时候雅可比矩阵就派上用场了。
1.3 雅可比矩阵：速度与力的桥梁
雅可比矩阵，说白了就是关节速度到末端速度的线性映射。它把关节空间和操作空间联系起来了。
我个人习惯把雅可比矩阵分成两部分：
- 线性速度部分： 关节转动如何影响末端位置变化
- 角速度部分： 关节转动如何影响末端姿态变化
来看一个简单的2自由度平面臂的雅可比矩阵推导：
def jacobian_2dof(theta1, theta2, l1, l2):
"""
2自由度平面臂的雅可比矩阵
"""
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
J = np.array([
[-l1*s1 - l2*s12, -l2*s12],
[ l1*c1 + l2*c12, l2*c12]
])
return J
重点： 雅可比矩阵的秩决定了机器人的灵巧度。当秩下降时，机器人就进入了奇异位形。我在做抓取规划时，一定会避开这些区域，否则末端速度会突然变得很大，导致抓取失败。
1.4 速度与加速度约束
光有运动学还不够，你还得考虑机器人的物理限制。每个关节都有最大速度、最大加速度。你想想看，如果轨迹规划出来的速度超过了电机能承受的范围，那结果就是——电机过载、报警、停机。
我一般会在轨迹生成阶段就加入这些约束：
- 关节速度约束： 每个关节的角速度不能超过额定值
- 关节加速度约束： 防止冲击和振动
- 末端速度约束： 根据抓取任务调整，比如抓鸡蛋时末端速度不能超过0.1m/s
这里有个经验值：我通常把加速度限制在额定值的70%左右，留出余量。为什么？因为实际负载和摩擦会消耗一部分能力。我曾经在产线上遇到过，理论计算没问题，但实际跑起来电机发烫严重。后来发现是加速度设得太极限了。
1.5 运动学模型在轨迹跟踪中的作用
好了，前面讲了这么多基础，现在说说它们怎么用在轨迹跟踪上。
轨迹跟踪的核心，就是让机器人末端按照预定的路径运动。但控制器是在关节空间里工作的，而路径是在操作空间里定义的。怎么办？
答案就是：通过运动学模型来回转换。
- 正向映射： 把期望的关节轨迹映射到操作空间，验证是否满足末端路径要求
- 逆向映射： 把操作空间的路径误差映射回关节空间，生成关节补偿量
- 雅可比转置： 把操作空间的力映射为关节力矩，实现力控抓取
我常用的一个框架是这样的：
# 轨迹跟踪中的运动学应用示例
def trajectory_control(joint_angles, desired_pose, Kp, dt):
# 1. 正解：计算当前末端位姿
current_pose = forward_kinematics(joint_angles, dh_params)
# 2. 计算操作空间误差
error = desired_pose - current_pose
# 3. 通过雅可比逆映射到关节空间
J = compute_jacobian(joint_angles)
joint_velocity = np.linalg.pinv(J) @ (Kp * error)
# 4. 施加速度约束
joint_velocity = np.clip(joint_velocity, -max_vel, max_vel)
# 5. 更新关节角度
new_joint_angles = joint_angles + joint_velocity * dt
return new_joint_angles
注意： 使用伪逆时，一定要检查雅可比矩阵的条件数。条件数过大说明接近奇异点，这时候伪逆的结果会很不稳定。我建议设置一个阈值，当条件数超过100时，切换到阻尼最小二乘法。
最后，我想说一句：运动学模型不是万能的，但没有它是万万不能的。你后面学动力学、学控制、学抓取规划，每一步都离不开今天讲的这些基础。所以，别急着跳过去，多花点时间把正解、逆解、雅可比矩阵吃透。
嗯，今天就到这里。记住，理论是死的，但应用是活的。多动手写代码，多在实际机器人上验证，你才能真正理解这些概念。