3、运动学基础：正运动学、逆运动学、雅可比矩阵、速度与加速度约束
各位同学，欢迎来到运动学基础这一节。
说实话，运动学是整个轨迹规划的基石。你想想看，如果连机器人末端在哪儿、怎么动都搞不清楚，后面谈轨迹跟踪就是空中楼阁。我当年刚入行时，就吃过这个亏——上来就调轨迹跟踪参数，结果机器人乱跑，查了半天才发现是运动学模型写错了。
所以，咱们今天把这块地基打牢。我会把正运动学、逆运动学、雅可比矩阵，还有速度加速度约束，掰开了揉碎了讲清楚。
3.1 正运动学：从关节空间到任务空间
正运动学，说白了就是：给定每个关节的角度，算出末端执行器在空间中的位置和姿态。
这就像你抬起胳膊——你知道肩膀转了30度，肘弯了45度，手腕扭了10度，然后你自然知道手在哪儿。机器人也一样，只不过它需要数学公式。
核心公式：
T = T₁ · T₂ · T₃ · … · Tₙ
其中 T 是4×4的齐次变换矩阵，包含旋转和平移信息。
我个人习惯用DH参数法（Denavit-Hartenberg）来建立模型。为什么？因为它标准化，不容易出错。每个关节只需要4个参数：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | a | 相邻关节轴之间的公垂线长度 |
| 连杆扭角 | α | 相邻关节轴之间的夹角 |
| 关节距离 | d | 沿关节轴方向的偏移 |
| 关节角度 | θ | 绕关节轴的旋转角度 |
举个例子，一个6轴工业机器人，正运动学就是6个变换矩阵连乘。代码实现其实很直接：
import numpy as np
def dh_transform(a, alpha, d, theta):
"""计算单个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
# 假设6个关节角度
joint_angles = [0.1, 0.2, -0.3, 0.4, 0.5, 0.6]
# 假设DH参数表（a, alpha, d, theta）
dh_params = [...] # 实际项目中从机器人手册获取
T_total = np.eye(4)
for i in range(6):
a, alpha, d, _ = dh_params[i]
T_i = dh_transform(a, alpha, d, joint_angles[i])
T_total = T_total @ T_i
print("末端位姿矩阵:\n", T_total)我的经验：写正运动学代码时，一定要先验证单个关节的变换矩阵是否正确。我曾经因为一个符号写反，排查了整整两天。建议用已知的简单构型（比如只动一个关节）来测试。
3.2 逆运动学：从任务空间回到关节空间
逆运动学正好反过来：给定末端的目标位姿，反算出每个关节的角度。
这比正运动学难多了。为什么？因为解不唯一，甚至可能无解。你想想看，让你把手放到桌面上某个点，你可以肘朝外，也可以肘朝内——这就是多解。
逆运动学求解方法主要有两种：
- 解析法：针对特定构型（如6轴机器人有封闭解的条件），速度快，精度高。但只适用于满足Pieper准则的机器人。
- 数值法：通用性强，任何构型都能用。常用的是牛顿-拉夫森迭代法，或者基于雅可比矩阵的伪逆法。
我在项目中遇到最多的是6轴工业机器人。这类机器人通常有8组逆解。怎么选？我一般遵循三个原则：
- 关节限位优先——先排除超出物理限位的解
- 运动连续性——选离当前关节角度最近的那组解
- 避障需求——如果附近有障碍物，选能避开的那组
注意：数值法求解逆运动学时，初始值的选择非常关键。选不好，迭代可能发散，或者收敛到错误的解。我建议先用解析法（如果可行）算一个近似解，再用数值法精调。
3.3 雅可比矩阵：速度的桥梁
雅可比矩阵，说白了就是关节速度到末端速度的线性映射。
公式很简单：
ẋ = J(q) · q̇
其中ẋ是末端速度（线速度+角速度，共6维），q̇是关节速度（n维），J是6×n的雅可比矩阵。
雅可比矩阵的每一列，物理意义是：某个关节单位速度对末端速度的贡献。
计算雅可比矩阵有两种方法：
- 解析法：对正运动学方程求偏导。适合简单构型。
- 几何法：利用每个关节的旋转轴和位置向量直接构造。更直观，也更容易编程实现。
几何法的核心思想：
def compute_jacobian(joint_angles, dh_params):
"""
几何法计算雅可比矩阵
返回6×n的矩阵
"""
n = len(joint_angles)
J = np.zeros((6, n))
# 先计算所有变换矩阵
T_list = [np.eye(4)]
for i in range(n):
a, alpha, d, _ = dh_params[i]
T_i = dh_transform(a, alpha, d, joint_angles[i])
T_list.append(T_list[-1] @ T_i)
# 末端位置
p_end = T_list[-1][:3, 3]
for i in range(n):
# 第i个关节的旋转轴（在基坐标系下）
z_i = T_list[i][:3, 2]
# 第i个关节到末端的位置向量
p_i = T_list[i][:3, 3]
r = p_end - p_i
# 旋转关节：线速度 = z × r，角速度 = z
J[:3, i] = np.cross(z_i, r)
J[3:, i] = z_i
return J关键理解：雅可比矩阵的秩反映了机器人在当前位形下的运动能力。如果秩小于6，说明存在奇异位形——某些方向上的运动能力会丧失。这是轨迹规划时必须避开的。
3.4 速度与加速度约束：别让机器人"散架"
好了，现在我们知道怎么算位置和速度了。但实际机器人是有物理限制的——电机不能无限快，也不能无限加速。
速度约束和加速度约束，说白了就是：给关节的运动加上"天花板"。
常见的约束形式：
| 约束类型 | 数学表达 | 物理含义 |
|---|---|---|
| 关节速度上限 | |q̇ᵢ| ≤ v_max,i | 电机最大转速 |
| 关节加速度上限 | |q̈ᵢ| ≤ a_max,i | 电机最大转矩/惯性限制 |
| 末端速度上限 | ‖ẋ‖ ≤ V_max | 工艺要求或安全限制 |
| 加加速度上限 | |q̇̈ᵢ| ≤ j_max,i | 减少振动，延长寿命 |
我曾经在一个焊接项目中，只考虑了速度约束，没管加速度。结果机器人启动时猛地一冲，焊丝直接断了。嗯，从那以后我再也不敢忽略加速度约束了。
怎么把这些约束融入轨迹规划？我常用的方法是S形速度规划：
def s_curve_planning(q_start, q_end, v_max, a_max, dt):
"""
S形速度规划
返回位置、速度、加速度序列
"""
# 计算总位移
delta_q = q_end - q_start
# 计算加速段、匀速段、减速段的时间
# 这里简化处理，实际需要根据v_max和a_max计算
t_acc = v_max / a_max
t_const = abs(delta_q) / v_max - t_acc
t_dec = t_acc
# 生成轨迹点
t_total = t_acc + t_const + t_dec
t = np.arange(0, t_total, dt)
q = np.zeros_like(t)
qd = np.zeros_like(t)
qdd = np.zeros_like(t)
for i, ti in enumerate(t):
if ti < t_acc:
# 加速段：加加速度恒定
q[i] = 0.5 * a_max * ti**2
qd[i] = a_max * ti
qdd[i] = a_max
elif ti < t_acc + t_const:
# 匀速段
q[i] = v_max * (ti - t_acc/2)
qd[i] = v_max
qdd[i] = 0
else:
# 减速段
t_rem = ti - t_acc - t_const
q[i] = v_max * (t_const + t_acc/2) + v_max*t_rem - 0.5*a_max*t_rem**2
qd[i] = v_max - a_max * t_rem
qdd[i] = -a_max
# 缩放到实际位移
q = q_start + (q / q[-1]) * delta_q
return q, qd, qdd避坑指南：加速度约束一定要留余量。电机标称的a_max通常是理想情况，实际运行时温度、负载都会影响。我一般取标称值的70%~80%作为规划上限。
3.5 知识体系总览
说了这么多，咱们用一张图把运动学基础的核心逻辑串起来：
这张图把咱们今天讲的内容串起来了。正运动学和逆运动学解决"位置"问题，雅可比矩阵解决"速度"问题，而速度和加速度约束给所有运动加上"安全锁"。四者缺一不可。
好了，运动学基础就讲到这里。记住，这些公式和代码不是背出来的，是用出来的。下次你写轨迹规划代码时，不妨先从正运动学验证开始，一步步来，稳扎稳打。