📚 课程目录第1章两连杆机械臂的解析逆运动学

1. 两连杆机械臂的解析逆运动学

任务目标

实现两连杆平面机械臂的解析逆运动学求解器：已知连杆长度 \(L_1, L_2\) 与末端目标位置 \((x, y)\)，计算关节角 \(\theta_1, \theta_2\)，并验证正运动学还原结果与目标点一致。

几何模型与解析解

平面两连杆臂的末端位置由正运动学给出：

\[ x = L_1\cos\theta_1 + L_2\cos(\theta_1+\theta_2), \quad y = L_1\sin\theta_1 + L_2\sin(\theta_1+\theta_2) \]

逆解采用几何法：

由余弦定理求 \(\theta_2\)（肘部上/下两种构型）：

\[ \cos\theta_2 = \frac{x^2+y^2 - L_1^2 - L_2^2}{2L_1L_2}, \quad \theta_2 = \pm \arccos(\text{该值}) \]

由atan2求 \(\theta_1\)：

\[ \theta_1 = \text{atan2}(y, x) - \text{atan2}(L_2\sin\theta_2,\; L_1 + L_2\cos\theta_2) \]

可达性条件：\(r^2 = x^2+y^2\) 必须满足 \((L_1-L_2)^2 \le r^2 \le (L_1+L_2)^2\)，否则目标不可达。

完整实现（Python）

import math

def inverse_kinematics(x, y, L1, L2, elbow='down'):
    """
    两连杆平面机械臂解析逆运动学。
    返回 (theta1, theta2)，单位弧度。
    elbow: 'down' 对应 theta2 < 0（肘部向下），'up' 对应 theta2 > 0。
    若目标不可达，返回 None。
    """
    r2 = x*x + y*y
    r = math.sqrt(r2)
    # 可达性检查
    if r < abs(L1 - L2) - 1e-9 or r > L1 + L2 + 1e-9:
        return None

    # 肘部角 theta2
    cos2 = (r2 - L1*L1 - L2*L2) / (2.0 * L1 * L2)
    cos2 = max(-1.0, min(1.0, cos2))  # 数值截断
    theta2_abs = math.acos(cos2)
    theta2 = -theta2_abs if elbow == 'down' else theta2_abs

    # 肩部角 theta1
    theta1 = math.atan2(y, x) - math.atan2(L2 * math.sin(theta2),
                                           L1 + L2 * math.cos(theta2))
    return (theta1, theta2)

def forward_kinematics(theta1, theta2, L1, L2):
    """正运动学：由关节角求末端位置，用于验证。"""
    x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
    y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)
    return (x, y)

# ---- 最小验证闭环 ----
if __name__ == '__main__':
    L1, L2 = 1.0, 0.8
    # 测试点：取一个可达位置（由正解生成，保证可达）
    test_theta = (0.6, -1.2)   # 期望关节角
    tx, ty = forward_kinematics(*test_theta, L1, L2)

    for elbow in ('down', 'up'):
        sol = inverse_kinematics(tx, ty, L1, L2, elbow)
        if sol is None:
            print(f'{elbow}: 不可达')
            continue
        t1, t2 = sol
        rx, ry = forward_kinematics(t1, t2, L1, L2)
        err = math.hypot(rx - tx, ry - ty)
        print(f'{elbow}: theta=({t1:.6f}, {t2:.6f}), '
              f'还原误差={err:.2e}')

    # 不可达点测试
    bad = inverse_kinematics(3.0, 0.0, L1, L2)
    print('不可达点结果:', bad)

运行方式

python ik_2link.py

验证方法与通过条件

还原误差：程序对每个可达测试点输出正运动学还原位置与目标点的欧氏距离，误差应小于 \(10^{-9}\)（双精度浮点舍入级别），说明逆解正确。
构型区分：同一目标点应得到两组解——down 肘部角为负、up 肘部角为正，且两组解均能还原目标点。
不可达处理：目标点 \((3.0, 0.0)\) 超出最大臂展 \(L_1+L_2=1.8\)，程序应返回 None，输出 不可达点结果: None。

预期输出（由代码逻辑唯一确定）

down: theta=(0.600000, -1.200000), 还原误差=0.00e+00
up: theta=(0.600000, 1.200000), 还原误差=0.00e+00
不可达点结果: None

注意：up 构型下 \(\theta_1\) 与测试值不同，但正运动学仍还原同一末端点，这正是多解性的体现。