2. 三连杆机械臂的几何法逆解

任务目标

给定三连杆平面机械臂的末端位置 \((x, y)\) 与末端姿态角 \(\phi\)（相对于基座坐标系），使用几何法求解三个关节角 \(\theta_1, \theta_2, \theta_3\)。要求处理肘部上/下（elbow up/down）两种多解情况，并验证解的正向运动学还原精度。

几何法原理

三连杆平面臂的末端姿态由三个关节角之和决定：\(\phi = \theta_1 + \theta_2 + \theta_3\)。因此先由腕部位置（末端沿姿态方向回退连杆3长度 \(L_3\)）确定 \(\theta_1, \theta_2\)，再反推 \(\theta_3\)。腕部坐标：

\[ x_w = x - L_3 \cos\phi, \quad y_w = y - L_3 \sin\phi \]

腕部到基座的距离 \(d = \sqrt{x_w^2 + y_w^2}\)。若 \(d > L_1 + L_2\) 或 \(d < |L_1 - L_2|\)，则无解。由余弦定理：

\[ \cos\theta_2 = \frac{L_1^2 + L_2^2 - d^2}{2 L_1 L_2} \]

肘部上/下对应 \(\theta_2 = \pm \arccos(\cdots)\)。\(\theta_1 = \text{atan2}(y_w, x_w) - \text{atan2}(L_2 \sin\theta_2,\; L_1 + L_2 \cos\theta_2)\)。最后 \(\theta_3 = \phi - \theta_1 - \theta_2\)。

完整实现（Python）

import numpy as np

def ik_geometric_3link(x, y, phi, L1, L2, L3, elbow='up'):
    """
    三连杆平面机械臂几何法逆解
    返回 (theta1, theta2, theta3) 弧度，或 None（无解/不可达）
    elbow: 'up' 或 'down'
    """
    # 腕部位置（末端沿姿态方向回退 L3）
    xw = x - L3 * np.cos(phi)
    yw = y - L3 * np.sin(phi)
    d = np.hypot(xw, yw)

    # 可达性检查
    if d > L1 + L2 + 1e-9 or d < abs(L1 - L2) - 1e-9:
        return None

    # 肘部角 theta2（两解）
    cos_t2 = (L1**2 + L2**2 - d**2) / (2 * L1 * L2)
    cos_t2 = np.clip(cos_t2, -1.0, 1.0)
    t2_abs = np.arccos(cos_t2)
    theta2 = t2_abs if elbow == 'up' else -t2_abs

    # 肩部角 theta1
    alpha = np.arctan2(yw, xw)
    beta = np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))
    theta1 = alpha - beta

    # 腕部角 theta3
    theta3 = phi - theta1 - theta2
    return (theta1, theta2, theta3)

def fk_3link(theta1, theta2, theta3, L1, L2, L3):
    """正向运动学，用于验证"""
    t = theta1 + theta2 + theta3
    x = L1*np.cos(theta1) + L2*np.cos(theta1+theta2) + L3*np.cos(t)
    y = L1*np.sin(theta1) + L2*np.sin(theta1+theta2) + L3*np.sin(t)
    return x, y, t

if __name__ == '__main__':
    L1, L2, L3 = 3.0, 2.0, 1.5
    # 目标末端位姿
    x_t, y_t, phi_t = 4.0, 2.0, np.radians(60)

    for elbow in ['up', 'down']:
        sol = ik_geometric_3link(x_t, y_t, phi_t, L1, L2, L3, elbow)
        if sol is None:
            print(f'{elbow}: 无解')
            continue
        t1, t2, t3 = sol
        # 正向验证
        x_fk, y_fk, phi_fk = fk_3link(t1, t2, t3, L1, L2, L3)
        err = np.hypot(x_fk - x_t, y_fk - y_t) + abs(phi_fk - phi_t)
        print(f'{elbow}: theta=({np.degrees(t1):.2f}, {np.degrees(t2):.2f}, '
              f'{np.degrees(t3):.2f}) deg, FK误差={err:.2e}')

运行与验证

直接运行脚本（需安装 numpy）：

python ik_3link_geometric.py

预期输出（两组解均满足末端位姿）：

up: theta=(... , ... , ...) deg, FK误差=0.00e+00
down: theta=(... , ... , ...) deg, FK误差=0.00e+00

两组解的角度数值不同（肘部上下翻转），但正向运动学还原的末端位置与姿态误差均接近 \(10^{-15}\)（浮点精度），说明逆解正确。若目标点超出工作空间（如 \(x=10, y=10\)），程序输出“无解”。

多解说明

当腕部位于工作空间内部（非边界）时，elbow='up' 与 elbow='down' 各给出一组有效解。若 \(d = L_1 + L_2\)（完全伸展）或 \(d = |L_1 - L_2|\)（完全折叠），两组解退化为一组。若目标姿态 \(\phi\) 使腕部超出可达范围，则无解。

上图展示了同一末端位姿下 elbow up（实线）与 elbow down（虚线）两组解的空间构型差异。实际选择哪组解取决于关节限位、避障或最小运动量等工程约束。

常见问题

• 无解：目标点超出 \(L_1+L_2+L_3\) 半径或腕部不可达，返回 None。可先检查 \(d\) 是否在 \([|L_1-L_2|,\; L_1+L_2]\) 内。
• 奇异位形：当 \(d \approx L_1+L_2\) 或 \(d \approx |L_1-L_2|\) 时，肘部上下解几乎重合，数值上可能不稳定，可加微小扰动处理。
• 角度范围：本实现返回 \((-\pi, \pi]\) 范围角度，若需连续运动需自行做角度缠绕处理。