机器人 / 系统课时

分离位置与姿态求球腕逆解

分离位置与姿态求球腕逆解

给定六轴机械臂的目标末端位置和方向,直接联立六个关节角往往难以求解。对后三根关节轴交于一点的球腕结构,可以先回推腕心、求前三轴位置,再求后三轴姿态。本篇给出可运行的八候选求解与回代检查示例。

学习目标

  • 判断球腕分解的几何前提,并从目标位姿计算腕心。
  • 用平面双连杆几何和 ZYZ 旋转分解求出关节角候选解。
  • 按关节限位、当前姿态距离与正运动学误差筛选结果。

确认球腕条件并回推腕心

球腕要求第 4、5、6 轴的转动轴线交于同一固定点 WW。这些关节改变工具方向,却不改变 WW 在基座坐标系中的位置。这里进一步约定:第 6 坐标系原点 P6P_6 位于腕心沿其局部正 z6z_6 轴距离 d6d_6 处,且没有其他横向偏置。

设目标位姿为位置 pd\mathbf p_d 和旋转矩阵 RdR_d,则:

pW=pd−d6Rd[001]\mathbf p_W=\mathbf p_d-d_6R_d\begin{bmatrix}0\\0\\1\end{bmatrix}

这一步必须用目标方向旋转偏移向量,不能直接从世界坐标的 zz 分量减去 d6d_6。如果给定的是工具中心点(TCP)位姿,应先用已标定的法兰到 TCP 变换换算出第 6 坐标系目标,再回推腕心。

球腕三轴交点与目标位姿的腕心回推

用腕心位置求前三个关节角

采用一个明确的示例机型:第 1 轴绕基座 zz 轴旋转,第 2、3 轴绕各自局部负 yy 轴转动;肩部高度 d1=0.40d_1=0.40 m,两段连杆长度 a2=0.50a_2=0.50 m、a3=0.40a_3=0.40 m。对腕心 (xW,yW,zW)(x_W,y_W,z_W),先计算 ρ=xW2+yW2\rho=\sqrt{x_W^2+y_W^2}、h=zW−d1h=z_W-d_1。

基座角有两个候选:q1=atan2⁡(yW,xW)q_1=\operatorname{atan2}(y_W,x_W) 时取有符号半径 r=ρr=\rho;q1q_1 增加 180∘180^\circ 时取 r=−ρr=-\rho。每种基座方向再用余弦定理求肘部双解:

C3=r2+h2−a22−a322a2a3,q3=±arccos⁡(C3)C_3=\frac{r^2+h^2-a_2^2-a_3^2}{2a_2a_3},\qquad q_3=\pm\arccos(C_3)
q2=atan2⁡(h,r)−atan2⁡(a3sin⁡q3,a2+a3cos⁡q3)q_2=\operatorname{atan2}(h,r)-\operatorname{atan2}(a_3\sin q_3,a_2+a_3\cos q_3)

若 ∣C3∣>1|C_3|>1 且超过数值容差,说明腕心在该两连杆模型中不可达;在容差范围内才可把 C3C_3 截断到 [−1,1][-1,1]。后面的示例腕心约为 (0.7324,0.4229,0.4342)(0.7324,0.4229,0.4342) m,对应 ρ=0.8457\rho=0.8457 m、h=0.0342h=0.0342 m、C3=0.7660C_3=0.7660,故肘角为约 ±40∘\pm40^\circ。

同一腕心的肘部双解与腕部翻转

利用剩余旋转求腕部三轴

对前三轴的每个位置解,计算腕心处第 3 坐标系方向。本模型采用 R03=Rz(q1)Ry(−(q2+q3))R_{03}=R_z(q_1)R_y(-(q_2+q_3)),球腕采用 ZYZ 顺序:

R36=R03TRd=Rz(q4)Ry(q5)Rz(q6)R_{36}=R_{03}^{\mathsf T}R_d=R_z(q_4)R_y(q_5)R_z(q_6)

将 R36R_{36} 的矩阵元素记为 rijr_{ij}。对非奇异腕部,取:

q5=atan2⁡(r132+r232,r33)q_5=\operatorname{atan2}(\sqrt{r_{13}^2+r_{23}^2},r_{33})
q4=atan2⁡(r23,r13),q6=atan2⁡(r32,−r31)q_4=\operatorname{atan2}(r_{23},r_{13}),\qquad q_6=\operatorname{atan2}(r_{32},-r_{31})

另一组等效姿态解为 (q4+180∘,−q5,q6+180∘)(q_4+180^\circ,-q_5,q_6+180^\circ),需将角度归一化并检查限位。因此一般位置会形成基座两支、肘部两支、腕部两支,最多八组代数候选;具体机型存在肩偏置时,前三轴公式要按它的真实运动学重推。

当 ∣sin⁡q5∣|\sin q_5| 接近零时发生腕部奇异:q5≈0q_5\approx0 仅约束 q4+q6q_4+q_6,q5≈180∘q_5\approx180^\circ 仅约束 q4−q6q_4-q_6。不能继续使用上述两个 atan2 独立恢复角度,应固定一个腕角(例如取接近当前值)再求另一个。

运行完整的候选解算例

环境为 Python 3.10+、NumPy;可通过 python -m pip install numpy 安装。下面固定连杆长度和球腕转轴顺序,先由已知关节角生成一个合法目标位姿,再完全只用目标位置、旋转矩阵求逆解,以便进行闭环验算。保存为 wrist_ik.py 后运行 python wrist_ik.py。

python
import numpy as np

D1, A2, A3, D6 = 0.4, 0.5, 0.4, 0.1

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

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

def forward(q):
    q1, q2, q3, q4, q5, q6 = q
    radial = A2 * np.cos(q2) + A3 * np.cos(q2 + q3)
    pw = np.array([radial * np.cos(q1), radial * np.sin(q1),
                   D1 + A2 * np.sin(q2) + A3 * np.sin(q2 + q3)])
    r03 = rz(q1) @ ry(-(q2 + q3))
    r06 = r03 @ rz(q4) @ ry(q5) @ rz(q6)
    return pw + D6 * r06[:, 2], r06

def inverse(p, r06):
    # 先沿目标工具轴回推腕心,再枚举基座与肘部支路。
    pw = p - D6 * r06[:, 2]
    rho = np.hypot(pw[0], pw[1])
    h = pw[2] - D1
    c3 = (rho**2 + h**2 - A2**2 - A3**2) / (2 * A2 * A3)
    if abs(c3) > 1 + 1e-9:
        return []
    c3 = np.clip(c3, -1, 1)
    solutions = []
    for base_sign in (1, -1):
        q1 = np.arctan2(pw[1], pw[0]) + (0 if base_sign == 1 else np.pi)
        r = base_sign * rho
        for elbow_sign in (1, -1):
            q3 = elbow_sign * np.arccos(c3)
            q2 = np.arctan2(h, r) - np.arctan2(
                A3 * np.sin(q3), A2 + A3 * np.cos(q3))
            r03 = rz(q1) @ ry(-(q2 + q3))
            r36 = r03.T @ r06
            sy = np.hypot(r36[0, 2], r36[1, 2])
            # 奇异腕支路需另外求耦合角,此简例不强行拆分。
            if sy < 1e-8:
                continue
            q4 = np.arctan2(r36[1, 2], r36[0, 2])
            q5 = np.arctan2(sy, r36[2, 2])
            q6 = np.arctan2(r36[2, 1], -r36[2, 0])
            for flip in (False, True):
                q = np.array([q1, q2, q3, q4 + (np.pi if flip else 0),
                              -q5 if flip else q5,
                              q6 + (np.pi if flip else 0)])
                solutions.append((q + np.pi) % (2 * np.pi) - np.pi)
    return solutions

# 用正运动学生成可复核的目标,逆解过程不读取原关节角。
target_q = np.deg2rad([30, 20, -40, 40, 50, -30])
p, r06 = forward(target_q)
solutions = inverse(p, r06)
limits = np.array([[-170, 170], [-120, 120], [-150, 150],
                   [-170, 170], [-120, 120], [-175, 175]])
valid = [q for q in solutions if np.all(
    (np.rad2deg(q) >= limits[:, 0]) & (np.rad2deg(q) <= limits[:, 1]))]
if not valid:
    raise RuntimeError("没有符合关节限位的非奇异候选解")

# 用相对于当前关节角的最短角差选解,最后回代检查。
seed = np.deg2rad([28, 18, -35, 38, 48, -25])
angle_diff = lambda a, b: (a - b + np.pi) % (2 * np.pi) - np.pi
best = min(valid, key=lambda q: np.linalg.norm(angle_diff(q, seed)))
p_check, r_check = forward(best)

print("腕心位置(m):", np.round(p - D6 * r06[:, 2], 4))
print("代数候选数:", len(solutions))
print("关节限位内候选数:", len(valid))
print("选中关节角(度):", np.round(np.rad2deg(best), 1))
print("位置误差小于1e-9m:", np.linalg.norm(p_check - p) < 1e-9)
print("旋转矩阵误差小于1e-9:", np.linalg.norm(r_check - r06) < 1e-9)

预期输出:

text
腕心位置(m): [0.7324 0.4229 0.4342]
代数候选数: 8
关节限位内候选数: 3
选中关节角(度): [ 30.  20. -40.  40.  50. -30.]
位置误差小于1e-9m: True
旋转矩阵误差小于1e-9: True

在真机执行前筛选与复核

八组是几何上的候选数,不代表八组都可以执行。本例按各轴限位留下三组,再选与当前关节配置最接近的一组。回代误差用目标末端位置的欧氏距离和目标旋转矩阵的差异检验;示例中的 10−910^{-9} 仅是数值闭环测试阈值,不是机器人实际标定精度。

实际集成时,按以下顺序执行:先验证目标旋转矩阵正交且行列式接近 11,再用真实连杆尺寸求候选并排除超限、奇异或碰撞配置;随后用机器人真实正运动学回代位置和姿态,最后结合当前关节角及规划器检查运动路径。对多圈关节还要保留允许的旋转圈数,不能直接照搬示例的 [−180∘,180∘)[-180^\circ,180^\circ) 归一化方式。

这套闭式解依赖特定球腕结构、坐标轴定义和准确的 TCP 标定。如果第 4 至第 6 轴并非共点,或前臂带有与示例不同的偏置,就应使用该机器人的完整运动学模型求解,不能只修改连杆长度后沿用这些三角公式。

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

目录与阅读设置

阅读设置

17

记录一个想法

分离位置与姿态求球腕逆解

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

登录哪都通