机器人 / 系统课时

用关节轴推导几何雅可比

用关节轴推导几何雅可比

给定关节轴方向、轴上一点和末端原点位置,就能逐列构造几何雅可比矩阵,不必对整条正运动学链逐项求导。本篇用旋转关节与移动关节串联的平面机械臂,推导两类列向量,并检验它们预测的末端速度。

学习目标

  • 将关节轴方向、轴上一点和末端位置统一表示在基坐标系中。
  • 分别求出旋转关节与移动关节的线速度、角速度雅可比列。
  • 通过 R-P 机械臂算例与位置差分核对计算结果。

统一速度排列和参考坐标系

设有 nn 个关节,关节坐标为 qq,关节速度为 q˙\dot q。本篇采用线速度在前、角速度在后的六维末端速度排列:

νe=[vx,vy,vz,ωx,ωy,ωz]T=Jg(q)q˙\boldsymbol{\nu}_e=[v_x,v_y,v_z,\omega_x,\omega_y,\omega_z]^T=J_g(q)\dot q

因此,几何雅可比 JgJ_g 有 66 行、nn 列;第 ii 列表示仅令第 ii 个关节以单位速度运动时,末端原点的瞬时速度贡献。关节角速度使用 rad/s,移动速度使用 m/s。

推导每列只需三个几何量:末端原点 oeo_e、第 ii 个关节轴上的任一点 oio_i、关节轴单位方向 ziz_i。三者都必须表达在同一个基坐标系,其中位置单位为 m。此处 ziz_i 表示实际第 ii 个关节轴,不预设它在 DH 坐标架中的下标。

分别推导旋转关节和移动关节

**旋转关节 R:**关节绕通过 oio_i、方向为 ziz_i 的轴转动。距离轴越远,末端的切向线速度越大;同时末端获得绕该轴的角速度。因此第 ii 列的上下两部分为:

Jv,i=zi×(oe−oi),Jω,i=ziJ_{v,i}=z_i\times(o_e-o_i),\qquad J_{\omega,i}=z_i

**移动关节 P:**关节沿单位方向 ziz_i 平移,不独立产生角速度。对应列为:

Jv,i=zi,Jω,i=0J_{v,i}=z_i,\qquad J_{\omega,i}=0

将所有关节的 Jv,iJ_{v,i} 放在上三行,Jω,iJ_{\omega,i} 放在下三行,按关节顺序排成 6×n6\times n 矩阵。旋转关节要做叉乘;移动关节不能误用叉乘公式。

旋转关节切向速度与移动关节轴向速度的几何雅可比列

在平面 R-P 机械臂上代入数值

设底座旋转角 θ=30∘\theta=30^\circ,固定导轨长 L=1.0L=1.0 m,滑块向外伸出 d=0.5d=0.5 m。令 u=(cos⁡θ,sin⁡θ,0)Tu=(\cos\theta,\sin\theta,0)^T,那么旋转轴方向为 z1=(0,0,1)Tz_1=(0,0,1)^T,移动轴方向为 z2=uz_2=u,末端原点为 oe=(L+d)uo_e=(L+d)u。

旋转关节的线速度列是 Jv,1=(−(L+d)sin⁡θ,(L+d)cos⁡θ,0)TJ_{v,1}=(-(L+d)\sin\theta,(L+d)\cos\theta,0)^T;移动关节的线速度列是 Jv,2=uJ_{v,2}=u。相应角速度列分别是 (0,0,1)T(0,0,1)^T 和零向量。注意导轨已经随旋转关节转过 30∘30^\circ,所以第二列不能直接填 (1,0,0)T(1,0,0)^T。

取 θ˙=0.2\dot\theta=0.2 rad/s、d˙=0.1\dot d=0.1 m/s。把这两个关节速度乘到各自的雅可比列再相加,就能得到末端的线速度与角速度。

用 Python 验证矩阵和瞬时速度

下面的程序仅依赖 Python 3 标准库。它按上述轴向量拼装 6×26\times2 雅可比,再用极小时间间隔的位置差分独立检查线速度。

python
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")

预期输出:

text
末端位置: [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。前向差分存在 O(dt)O(dt) 的截断误差,因此误差不必恰好为零;本例误差远小于 10−510^{-5} m/s。

避免轴编号和参考点造成错列

使用标准 DH 参数时,第 ii 个关节的实际轴常由前一坐标架的 zi−1z_{i-1} 表示。推导几何雅可比应依据实际关节轴,先将其旋转到基坐标系,再代入列公式;不要直接从 DH 表抄一个未变换的局部 zz 向量。

另外,若末端参考点由法兰原点换成离法兰 0.20 m 的工具尖端,旋转关节的 (oe−oi)(o_e-o_i) 就随之改变,其线速度列通常也会改变;同一机构的移动关节列仍为轴方向。校验时还应确认关节速度顺序与矩阵列顺序一致,并检查 ziz_i 已归一化。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用关节轴推导几何雅可比

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通