机器人 / 系统课时

用 POE 公式表达关节链变换

用 POE 公式表达关节链变换

给定一组关节角,怎样直接由关节轴算出机械臂末端的位置和朝向?指数积公式(POE)只需要各关节在零位时的旋量轴和末端零位构型,就能组合出完整的刚体变换。下面以两转动关节机械臂为例,实际计算并与标准 DH 方法交叉验证。

学习目标

  • 根据零位关节轴构造空间旋量 S=(ω,v)S=(\omega,v) 和末端零位矩阵 MM。
  • 理解矩阵指数的含义及空间形式 POE 的乘法顺序。
  • 用 Python 求出末端位姿,并与同一零位约定下的标准 DH 结果比较。

用零位关节轴建立模型

空间形式 POE 将每个关节轴都表示在固定的基座坐标系 {s}\{s\} 中。对转动关节,先在零位选取轴上一点 qiq_i 和沿正转方向的单位轴向量 ωi\omega_i,再计算 vi=−ωi×qiv_i=-\omega_i\times q_i。这里的 viv_i 是旋量的线速度分量,不是末端线速度。对移动关节,则取 ωi=0\omega_i=0,viv_i 为单位移动方向,关节变量改用米表示。

旋量 Si=(ωi,vi)S_i=(\omega_i,v_i) 共有六个分量,约定先角速度、后线速度。对应的 4×44\times4 李代数矩阵为:

[Si]=[[ωi]×vi00][S_i]=\begin{bmatrix}[\omega_i]_{\times}&v_i\\0&0\end{bmatrix}

其中 [ω]×p=ω×p[\omega]_{\times}p=\omega\times p。一个 nn 关节开链机械臂的末端位姿可写为:

Tsb(θ)=e[S1]θ1e[S2]θ2⋯e[Sn]θnMT_{sb}(\boldsymbol{\theta})=e^{[S_1]\theta_1}e^{[S_2]\theta_2}\cdots e^{[S_n]\theta_n}M

MM 是所有关节变量为零时,末端坐标系 {b}\{b\} 相对基座坐标系 {s}\{s\} 的齐次变换,必须同时包含零位位置和朝向。

考虑平面双转动关节:连杆长度 L1=1.0 mL_1=1.0\,\mathrm m、L2=0.6 mL_2=0.6\,\mathrm m,两根转轴都沿 +z+z,零位时两杆沿 +x+x。取 q1=(0,0,0)q_1=(0,0,0)、q2=(1,0,0)q_2=(1,0,0),得到:

S1=(0,0,1, 0,0,0),S2=(0,0,1, 0,−1,0)S_1=(0,0,1,\,0,0,0),\quad S_2=(0,0,1,\,0,-1,0)
M=[1001.6010000100001]M=\begin{bmatrix}1&0&0&1.6\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}

双转动关节的零位旋量轴与运动后末端位姿

从矩阵指数得到关节运动

e[Si]θie^{[S_i]\theta_i} 表示一个关节运动产生的刚体变换,指数作用于整个 4×44\times4 矩阵,不是对矩阵各元素分别取指数。对于单位转轴 ω\omega,令 W=[ω]×W=[\omega]_{\times},可直接计算旋转块 RR 和平移块 pp:

R=I+sin⁡θ W+(1−cos⁡θ)W2,p=(θI+(1−cos⁡θ)W+(θ−sin⁡θ)W2)v\begin{aligned} R&=I+\sin\theta\,W+(1-\cos\theta)W^2,\\ p&=\bigl(\theta I+(1-\cos\theta)W+(\theta-\sin\theta)W^2\bigr)v \end{aligned}

随后组装 T=[Rp01]T=\begin{bmatrix}R&p\\0&1\end{bmatrix}。如果是移动关节,指数映射简化为 R=IR=I、p=vθp=v\theta。转动关节输入弧度,移动关节输入与模型一致的长度单位。

对本例,计算顺序为 T=e[S1]θ1e[S2]θ2MT=e^{[S_1]\theta_1}e^{[S_2]\theta_2}M。矩阵右侧的 MM 先作用于末端零位构型;两条 SiS_i 始终使用零位下的基座坐标,不应先把 q2q_2 随第一关节旋转,再重新计算 S2S_2。

用 Python 计算并与标准 DH 对照

使用 Python 3.10+ 和 NumPy(例如 1.26+)。保存为 poe_2r.py 并运行 python poe_2r.py。示例同时构造标准 DH 变换,其中两关节的 αi=di=0\alpha_i=d_i=0、a1=1.0a_1=1.0、a2=0.6a_2=0.6,与 POE 采用相同的末端坐标系。

python
import numpy as np

def hat(w):
    return np.array([[0., -w[2], w[1]],
                     [w[2], 0., -w[0]],
                     [-w[1], w[0], 0.]])

def exp_twist(S, theta):
    w, v = S[:3], S[3:]
    T = np.eye(4)
    if np.linalg.norm(w) < 1e-12:  # 移动关节没有旋转块
        T[:3, 3] = v * theta
        return T
    W = hat(w)  # 转动关节的 w 必须已归一化
    T[:3, :3] = np.eye(3) + np.sin(theta)*W + (1-np.cos(theta))*(W @ W)
    V = theta*np.eye(3) + (1-np.cos(theta))*W + (theta-np.sin(theta))*(W @ W)
    T[:3, 3] = V @ v
    return T

def dh_planar(theta, a):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0, a*c],
                     [s,  c, 0, a*s],
                     [0,  0, 1,   0],
                     [0,  0, 0,   1]])

L1, L2 = 1.0, 0.6
omega = np.array([0., 0., 1.])
q1, q2 = np.array([0., 0., 0.]), np.array([L1, 0., 0.])
S1 = np.r_[omega, -np.cross(omega, q1)]
S2 = np.r_[omega, -np.cross(omega, q2)]
M = np.eye(4)
M[:3, 3] = [L1 + L2, 0., 0.]  # 末端在零位沿 +x
t1, t2 = np.deg2rad([30., -45.])

T_poe = exp_twist(S1, t1) @ exp_twist(S2, t2) @ M
T_dh = dh_planar(t1, L1) @ dh_planar(t2, L2)
yaw = np.rad2deg(np.arctan2(T_poe[1, 0], T_poe[0, 0]))
print(f"POE: x={T_poe[0, 3]:.6f} m, y={T_poe[1, 3]:.6f} m, yaw={yaw:.6f}°")
print("POE 与标准 DH 一致:", np.allclose(T_poe, T_dh, atol=1e-10))

预期输出:

text
POE: x=1.445581 m, y=0.344709 m, yaw=-15.000000°
POE 与标准 DH 一致: True

位置也能手工核验:x=1.0cos⁡30∘+0.6cos⁡(−15∘)≈1.445581x=1.0\cos30^\circ+0.6\cos(-15^\circ)\approx1.445581 米,y=1.0sin⁡30∘+0.6sin⁡(−15∘)≈0.344709y=1.0\sin30^\circ+0.6\sin(-15^\circ)\approx0.344709 米;末端朝向为 30∘−45∘=−15∘30^\circ-45^\circ=-15^\circ。标准 DH 连乘对应 Rz(θ1)Tx(1.0)Rz(θ2)Tx(0.6)R_z(\theta_1)T_x(1.0)R_z(\theta_2)T_x(0.6),两法算出的完整 4×44\times4 矩阵应在数值容差内一致。

结果不一致时检查什么

先做零位测试:θ1=θ2=0\theta_1=\theta_2=0 时必须得到 T=MT=M,末端位置为 (1.6,0)(1.6,0) 米。再令 θ1=0\theta_1=0、θ2=90∘\theta_2=90^\circ,末端应在 (1.0,0.6)(1.0,0.6) 米且朝向 90∘90^\circ。这两组输入可快速区分零位构型错误和第二关节轴错误。

  • 位置偏差但朝向正确:优先检查 vi=−ωi×qiv_i=-\omega_i\times q_i 的负号、连杆长度以及 MM 的平移列。
  • 旋转方向相反:检查转轴正向是否遵循右手定则,以及是否先将角度转成弧度。
  • POE 与 DH 不一致:确认两边使用相同的基座系、零位、标准 DH 约定和工具坐标系;不要混用标准 DH 与改进 DH 的矩阵次序。

POE 在更长的串联关节链上仍使用同一形式:为每个关节记录零位下的空间旋量,按关节顺序计算指数积,最后右乘末端零位构型 MM。

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

目录与阅读设置

阅读设置

17

记录一个想法

用 POE 公式表达关节链变换

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

登录哪都通