机器人 / 系统课时
用关节轴推导几何雅可比
用关节轴推导几何雅可比
给定关节轴方向、轴上一点和末端原点位置,就能逐列构造几何雅可比矩阵,不必对整条正运动学链逐项求导。本篇用旋转关节与移动关节串联的平面机械臂,推导两类列向量,并检验它们预测的末端速度。
学习目标
- 将关节轴方向、轴上一点和末端位置统一表示在基坐标系中。
- 分别求出旋转关节与移动关节的线速度、角速度雅可比列。
- 通过 R-P 机械臂算例与位置差分核对计算结果。
统一速度排列和参考坐标系
设有 个关节,关节坐标为 ,关节速度为 。本篇采用线速度在前、角速度在后的六维末端速度排列:
因此,几何雅可比 有 行、 列;第 列表示仅令第 个关节以单位速度运动时,末端原点的瞬时速度贡献。关节角速度使用 rad/s,移动速度使用 m/s。
推导每列只需三个几何量:末端原点 、第 个关节轴上的任一点 、关节轴单位方向 。三者都必须表达在同一个基坐标系,其中位置单位为 m。此处 表示实际第 个关节轴,不预设它在 DH 坐标架中的下标。
分别推导旋转关节和移动关节
**旋转关节 R:**关节绕通过 、方向为 的轴转动。距离轴越远,末端的切向线速度越大;同时末端获得绕该轴的角速度。因此第 列的上下两部分为:
**移动关节 P:**关节沿单位方向 平移,不独立产生角速度。对应列为:
将所有关节的 放在上三行, 放在下三行,按关节顺序排成 矩阵。旋转关节要做叉乘;移动关节不能误用叉乘公式。

在平面 R-P 机械臂上代入数值
设底座旋转角 ,固定导轨长 m,滑块向外伸出 m。令 ,那么旋转轴方向为 ,移动轴方向为 ,末端原点为 。
旋转关节的线速度列是 ;移动关节的线速度列是 。相应角速度列分别是 和零向量。注意导轨已经随旋转关节转过 ,所以第二列不能直接填 。
取 rad/s、 m/s。把这两个关节速度乘到各自的雅可比列再相加,就能得到末端的线速度与角速度。
用 Python 验证矩阵和瞬时速度
下面的程序仅依赖 Python 3 标准库。它按上述轴向量拼装 雅可比,再用极小时间间隔的位置差分独立检查线速度。
import math
def cross(a, b):
return (
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0],
)
theta = math.radians(30)
length, slide = 1.0, 0.5
rate_theta, rate_slide = 0.2, 0.1
u = (math.cos(theta), math.sin(theta), 0.0)
origin_r = (0.0, 0.0, 0.0)
origin_p = tuple(length * a for a in u)
tip = tuple(origin_p[i] + slide * u[i] for i in range(3))
# R 轴垂直于运动平面,P 轴随 R 旋转后沿导轨方向。
axis_r, axis_p = (0.0, 0.0, 1.0), u
radius = tuple(tip[i] - origin_r[i] for i in range(3))
jv_r, jv_p = cross(axis_r, radius), axis_p
jw_r, jw_p = axis_r, (0.0, 0.0, 0.0)
# 上三行是线速度,下三行是角速度。
jacobian = [[jv_r[i], jv_p[i]] for i in range(3)]
jacobian += [[jw_r[i], jw_p[i]] for i in range(3)]
twist = [row[0] * rate_theta + row[1] * rate_slide for row in jacobian]
def fmt(values):
return "[" + ", ".join(f"{v:.4f}" for v in values) + "]"
print("末端位置:", fmt(tip))
print("雅可比矩阵(列顺序为 R、P):")
for row in jacobian:
print(fmt(row))
print("末端速度:", fmt(twist))
# 用前向差分独立校验雅可比预测的线速度。
dt = 1e-5
next_theta = theta + rate_theta * dt
next_slide = slide + rate_slide * dt
next_tip = (
(length + next_slide) * math.cos(next_theta),
(length + next_slide) * math.sin(next_theta),
0.0,
)
v_fd = [(next_tip[i] - tip[i]) / dt for i in range(3)]
err = max(abs(v_fd[i] - twist[i]) for i in range(3))
print(f"差分速度误差: {err:.8f} m/s")预期输出:
末端位置: [1.2990, 0.7500, 0.0000]
雅可比矩阵(列顺序为 R、P):
[-0.7500, 0.8660]
[1.2990, 0.5000]
[0.0000, 0.0000]
[0.0000, 0.0000]
[0.0000, 0.0000]
[1.0000, 0.0000]
末端速度: [-0.0634, 0.3098, 0.0000, 0.0000, 0.0000, 0.2000]
差分速度误差: 0.00000036 m/s线速度前三项的单位为 m/s,后三项角速度为 rad/s。前向差分存在 的截断误差,因此误差不必恰好为零;本例误差远小于 m/s。
避免轴编号和参考点造成错列
使用标准 DH 参数时,第 个关节的实际轴常由前一坐标架的 表示。推导几何雅可比应依据实际关节轴,先将其旋转到基坐标系,再代入列公式;不要直接从 DH 表抄一个未变换的局部 向量。
另外,若末端参考点由法兰原点换成离法兰 0.20 m 的工具尖端,旋转关节的 就随之改变,其线速度列通常也会改变;同一机构的移动关节列仍为轴方向。校验时还应确认关节速度顺序与矩阵列顺序一致,并检查 已归一化。