机器人 / 系统课时

从雅可比计算末端线角速度

从雅可比计算末端线角速度

给定各关节的转动速度,怎样同时求出机械臂末端沿三个坐标轴的移动速度与绕三个坐标轴的旋转速度?本课从关节轴几何关系构造雅可比矩阵,用平面双关节机械臂完成数值计算,并核对维度、单位和速度所属的坐标系。

学习目标

  • 写出关节速度到末端线速度、角速度的映射,并识别矩阵的行列含义。
  • 利用关节轴方向及末端位置构造转动关节的雅可比列。
  • 复算一组双关节速度,检查单位、表达坐标系和参考点。

速度映射与表达约定

设机械臂有 nn 个自由度,关节速度为 q˙\dot{\mathbf q}。将末端参考点 EE 的线速度 vE\mathbf v_E 与末端刚体的角速度 ω\boldsymbol\omega 按顺序叠成六维向量,定义几何雅可比:

tE=J(q)q˙,J∈R6×n\mathbf t_E=J(\mathbf q)\dot{\mathbf q},\qquad J\in\mathbb R^{6\times n}

本课约定 tE=[vx,vy,vz,ωx,ωy,ωz]T\mathbf t_E=[v_x,v_y,v_z,\omega_x,\omega_y,\omega_z]^T,即前三行线速度、后三行角速度。这些分量全部在基座坐标系 {0}\{0\} 下表达;部分机器人库采用角速度在前的顺序,调用接口时必须先核对。

对于转动关节,q˙i\dot q_i 的单位是 rad/s,末端线速度为 m/s,角速度为 rad/s;若含移动关节,其关节速度单位改为 m/s。雅可比映射的是瞬时速度,不是位置增量或欧拉角变化量。

从关节轴建立雅可比

对第 ii 个转动关节,取其轴线上一点 pi\mathbf p_i、单位轴向量 zi\mathbf z_i,以及末端参考点 pE\mathbf p_E。三者都必须先变换到同一坐标系,雅可比这一列满足:

Jv,i=zi×(pE−pi),Jω,i=ziJ_{v,i}=\mathbf z_i\times(\mathbf p_E-\mathbf p_i),\qquad J_{\omega,i}=\mathbf z_i

叉积描述关节转动造成的末端切向线速度;角速度分量沿该关节轴。若第 ii 个关节是移动关节,则改为 Jv,i=ziJ_{v,i}=\mathbf z_i、Jω,i=0J_{\omega,i}=\mathbf 0。依关节顺序拼成 6×n6\times n 矩阵,不要把轴方向、轴上一点和末端点混用不同坐标系。

双关节数值演算

设两根连杆位于基座 XYXY 平面,长度分别为 L1=1.0L_1=1.0 m、L2=0.5L_2=0.5 m;两关节绕基座 +Z+Z 轴旋转。令 q1=30∘q_1=30^\circ、q2=60∘q_2=60^\circ(第二关节角相对第一根连杆),第二根连杆的绝对方向为 90∘90^\circ。此时关节轴位置为 O=(0,0,0)O=(0,0,0)、P1=(0.8660,0.5,0)P_1=(0.8660,0.5,0),末端 E=(0.8660,1.0,0)E=(0.8660,1.0,0)。

平面双关节的关节轴、速度矢量及雅可比映射

代入上述关节轴公式,非零的雅可比分量为:

Jxy=[−1−0.50.86600],Jωz=[11]J_{xy}=\begin{bmatrix}-1&-0.5\\0.8660&0\end{bmatrix},\qquad J_{\omega_z}=\begin{bmatrix}1&1\end{bmatrix}

其余线速度 zz 行和角速度 x,yx,y 行均为零,因此完整 JJ 是 6×26\times2,不是 2×22\times2。取关节速度 q˙=[0.2,−0.1]T\dot{\mathbf q}=[0.2,-0.1]^T rad/s,可算得 vx=−1(0.2)−0.5(−0.1)=−0.15v_x=-1(0.2)-0.5(-0.1)=-0.15 m/s,vy=0.8660(0.2)=0.1732v_y=0.8660(0.2)=0.1732 m/s,ωz=0.2−0.1=0.1\omega_z=0.2-0.1=0.1 rad/s。末端此刻向左上方移动,同时绕正 ZZ 轴旋转。

用 Python 复算

在已安装 NumPy 的 Python 3 环境运行下列完整程序。代码从轴和点计算雅可比,避免只抄最终矩阵而漏掉坐标系约定。

python
import numpy as np

L1, L2 = 1.0, 0.5
q1, q2 = np.deg2rad([30.0, 60.0])
qdot = np.array([0.2, -0.1])  # 两个转动关节的速度,单位 rad/s

# 位置和关节轴均在基座坐标系下表达,长度单位 m
p0 = np.zeros(3)
p1 = np.array([L1 * np.cos(q1), L1 * np.sin(q1), 0.0])
pe = p1 + np.array([L2 * np.cos(q1 + q2), L2 * np.sin(q1 + q2), 0.0])
z = np.array([0.0, 0.0, 1.0])

# 每列上半部是叉积线速度系数,下半部是关节角速度轴
J = np.column_stack([
    np.r_[np.cross(z, pe - p0), z],
    np.r_[np.cross(z, pe - p1), z],
])
twist = J @ qdot
print("J shape:", J.shape)
print("v (m/s):", np.round(twist[:3], 4))
print("omega (rad/s):", np.round(twist[3:], 4))

预期输出:

text
J shape: (6, 2)
v (m/s): [-0.15    0.1732  0.    ]
omega (rad/s): [0.  0.  0.1]

单位、参考点与坐标系核查

工程中发现结果方向错误时,先核对以下三项,而不是直接调整速度符号:

  1. 角度单位:三角函数输入应为弧度。此例把 30∘30^\circ 和 60∘60^\circ 转成弧度;若误把数值 30、60 直接传入 np.sin,位置和速度都会错。
  2. 参考坐标系:轴向量、关节轴上一点、末端点必须同系;若想把结果改在另一仅旋转坐标轴的坐标系中表达,线速度与角速度都需分别旋转,不能只转换前三项。
  3. 参考点:改用工具尖端 E′E' 后,即使关节角不变,vE′=vE+ω×(pE′−pE)\mathbf v_{E'}=\mathbf v_E+\boldsymbol\omega\times(\mathbf p_{E'}-\mathbf p_E),必须重新计算线速度对应的雅可比;同一刚体的角速度保持不变。

还要区分几何角速度和欧拉角导数:一般三维姿态下,[ωx,ωy,ωz]T[\omega_x,\omega_y,\omega_z]^T 并不直接等于三个欧拉角的变化率。只要关节位置变化,就应在新的 q\mathbf q 处重新求 J(q)J(\mathbf q),再乘当前关节速度。

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

目录与阅读设置

阅读设置

17

记录一个想法

从雅可比计算末端线角速度

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

登录哪都通