机器人 / 系统课时
用 POE 公式表达关节链变换
用 POE 公式表达关节链变换
给定一组关节角,怎样直接由关节轴算出机械臂末端的位置和朝向?指数积公式(POE)只需要各关节在零位时的旋量轴和末端零位构型,就能组合出完整的刚体变换。下面以两转动关节机械臂为例,实际计算并与标准 DH 方法交叉验证。
学习目标
- 根据零位关节轴构造空间旋量 和末端零位矩阵 。
- 理解矩阵指数的含义及空间形式 POE 的乘法顺序。
- 用 Python 求出末端位姿,并与同一零位约定下的标准 DH 结果比较。
用零位关节轴建立模型
空间形式 POE 将每个关节轴都表示在固定的基座坐标系 中。对转动关节,先在零位选取轴上一点 和沿正转方向的单位轴向量 ,再计算 。这里的 是旋量的线速度分量,不是末端线速度。对移动关节,则取 , 为单位移动方向,关节变量改用米表示。
旋量 共有六个分量,约定先角速度、后线速度。对应的 李代数矩阵为:
其中 。一个 关节开链机械臂的末端位姿可写为:
是所有关节变量为零时,末端坐标系 相对基座坐标系 的齐次变换,必须同时包含零位位置和朝向。
考虑平面双转动关节:连杆长度 、,两根转轴都沿 ,零位时两杆沿 。取 、,得到:

从矩阵指数得到关节运动
表示一个关节运动产生的刚体变换,指数作用于整个 矩阵,不是对矩阵各元素分别取指数。对于单位转轴 ,令 ,可直接计算旋转块 和平移块 :
随后组装 。如果是移动关节,指数映射简化为 、。转动关节输入弧度,移动关节输入与模型一致的长度单位。
对本例,计算顺序为 。矩阵右侧的 先作用于末端零位构型;两条 始终使用零位下的基座坐标,不应先把 随第一关节旋转,再重新计算 。
用 Python 计算并与标准 DH 对照
使用 Python 3.10+ 和 NumPy(例如 1.26+)。保存为 poe_2r.py 并运行 python poe_2r.py。示例同时构造标准 DH 变换,其中两关节的 、、,与 POE 采用相同的末端坐标系。
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))预期输出:
POE: x=1.445581 m, y=0.344709 m, yaw=-15.000000°
POE 与标准 DH 一致: True位置也能手工核验: 米, 米;末端朝向为 。标准 DH 连乘对应 ,两法算出的完整 矩阵应在数值容差内一致。
结果不一致时检查什么
先做零位测试: 时必须得到 ,末端位置为 米。再令 、,末端应在 米且朝向 。这两组输入可快速区分零位构型错误和第二关节轴错误。
- 位置偏差但朝向正确:优先检查 的负号、连杆长度以及 的平移列。
- 旋转方向相反:检查转轴正向是否遵循右手定则,以及是否先将角度转成弧度。
- POE 与 DH 不一致:确认两边使用相同的基座系、零位、标准 DH 约定和工具坐标系;不要混用标准 DH 与改进 DH 的矩阵次序。
POE 在更长的串联关节链上仍使用同一形式:为每个关节记录零位下的空间旋量,按关节顺序计算指数积,最后右乘末端零位构型 。