📚 课程目录第2章三连杆机械臂逆运动学几何法

2. 三连杆机械臂逆运动学几何法

任务目标

本案例使用几何法求解三连杆平面机械臂的逆运动学问题：已知末端执行器的目标位置 \((x, y)\) 及末端姿态角 \(\phi\)，求解三个关节角 \(\theta_1, \theta_2, \theta_3\)。三连杆长度分别为 \(L_1, L_2, L_3\)，所有关节均为旋转关节，运动平面为二维平面。

几何法原理与多解处理

将三连杆机械臂的末端位置分解为两部分：前三连杆中前两杆决定腕部位置，第三杆决定末端姿态。设腕部位置为 \((x_w, y_w)\)，则有：

\(x_w = x - L_3 \cos\phi,\quad y_w = y - L_3 \sin\phi\)

此时问题退化为两连杆机械臂的逆运动学（求 \(\theta_1, \theta_2\)），再加上 \(\theta_3 = \phi - (\theta_1 + \theta_2)\)。两连杆部分存在两种典型构型：肘上（elbow-up）和肘下（elbow-down），对应 \(\theta_2\) 的正负两种取值，由此产生多解。此外，当腕部位置超出可达范围时无解，恰好位于边界时仅有一解。

完整实现代码

import numpy as np

def inverse_kinematics_3link(x, y, phi, L1, L2, L3, elbow='up'):
    """
    几何法求解三连杆平面机械臂逆运动学。
    参数:
        x, y   : 末端目标位置
        phi    : 末端姿态角（弧度）
        L1,L2,L3: 三杆长度
        elbow  : 'up' 或 'down'，指定肘部构型
    返回:
        (theta1, theta2, theta3) 或 None（无解）
    """
    # 1. 计算腕部位置
    xw = x - L3 * np.cos(phi)
    yw = y - L3 * np.sin(phi)

    # 2. 腕部可达性检查
    r = np.hypot(xw, yw)
    if r > L1 + L2 or r < abs(L1 - L2):
        return None  # 不可达

    # 3. 两连杆逆解（余弦定理）
    cos_theta2 = (xw**2 + yw**2 - L1**2 - L2**2) / (2 * L1 * L2)
    cos_theta2 = np.clip(cos_theta2, -1.0, 1.0)  # 数值保护

    if elbow == 'up':
        theta2 = np.arccos(cos_theta2)          # 肘上：theta2 为正
    else:
        theta2 = -np.arccos(cos_theta2)         # 肘下：theta2 为负

    # 4. 求 theta1（atan2 自动处理象限）
    alpha = np.arctan2(yw, xw)
    beta = np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))
    theta1 = alpha - beta

    # 5. 求 theta3
    theta3 = phi - theta1 - theta2

    return (theta1, theta2, theta3)

# ===== 最小验证示例 =====
if __name__ == "__main__":
    L1, L2, L3 = 3.0, 2.0, 1.0

    # 测试1：肘上构型
    sol_up = inverse_kinematics_3link(4.0, 2.0, 0.5, L1, L2, L3, 'up')
    print("肘上解:", sol_up)

    # 测试2：肘下构型（同一目标，不同构型）
    sol_down = inverse_kinematics_3link(4.0, 2.0, 0.5, L1, L2, L3, 'down')
    print("肘下解:", sol_down)

    # 测试3：不可达位置
    sol_invalid = inverse_kinematics_3link(10.0, 10.0, 0.0, L1, L2, L3, 'up')
    print("不可达位置:", sol_invalid)

    # 验证：正解回代（确认解的正确性）
    def forward_kinematics(theta1, theta2, theta3):
        x = L1*np.cos(theta1) + L2*np.cos(theta1+theta2) + L3*np.cos(theta1+theta2+theta3)
        y = L1*np.sin(theta1) + L2*np.sin(theta1+theta2) + L3*np.sin(theta1+theta2+theta3)
        phi = theta1 + theta2 + theta3
        return x, y, phi

    if sol_up:
        xf, yf, pf = forward_kinematics(*sol_up)
        print(f"肘上回代误差: x={xf-4.0:.2e}, y={yf-2.0:.2e}, phi={pf-0.5:.2e}")
    if sol_down:
        xf, yf, pf = forward_kinematics(*sol_down)
        print(f"肘下回代误差: x={xf-4.0:.2e}, y={yf-2.0:.2e}, phi={pf-0.5:.2e}")

运行与验证方法

运行方式：将代码保存为 ik_3link.py，在装有 NumPy 的 Python 环境中执行 python ik_3link.py。

验证路径：

多解验证：同一目标位置 \((4.0, 2.0)\) 和姿态 \(0.5\text{ rad}\)，肘上与肘下应输出两组不同的关节角，且两组角度均满足正解回代误差接近 \(10^{-15}\) 量级（浮点精度内）。
无解验证：目标 \((10.0, 10.0)\) 超出最大可达距离 \(L_1+L_2+L_3=6.0\)，函数应返回 None。
姿态一致性：回代时 \(\theta_1+\theta_2+\theta_3\) 应等于目标 \(\phi\)，误差在浮点精度内。

通过条件：肘上与肘下解均能通过正解回代验证（误差 < \(10^{-10}\)），且不可达位置正确返回 None。

常见问题：当目标位置恰好位于工作空间边界（\(r = L_1+L_2\) 或 \(r = |L_1-L_2|\)）时，肘上与肘下解重合，此时两种构型返回相同结果，属正常现象。数值计算中若 \(\cos\theta_2\) 因浮点误差略超出 \([-1,1]\)，代码中已用 np.clip 处理。