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

任务目标

给定两连杆平面机械臂的末端位置 (x, y) 及杆长 l1、l2，求解两个关节角 θ1、θ2。本案例采用几何解析法，不依赖数值迭代，直接推导出封闭解。

几何模型与解析推导

机械臂位于平面内，基座在原点。θ1 为第一关节相对 x 轴的角度，θ2 为第二关节相对第一杆的转角。由余弦定理可得：

cos θ2 = (x² + y² − l1² − l2²) / (2·l1·l2)

为保证可达，需满足 |cos θ2| ≤ 1。θ2 取正负两个解（肘上/肘下）。θ1 通过 atan2 计算：

θ1 = atan2(y, x) − atan2(l2·sin θ2, l1 + l2·cos θ2)

完整实现代码

import math

def inverse_kinematics_2link(x, y, l1, l2):
    """
    两连杆平面机械臂逆运动学解析解
    返回: [(θ1_elbow_up, θ2_elbow_up), (θ1_elbow_down, θ2_elbow_down)]
    若目标不可达，返回 None
    """
    r2 = x*x + y*y
    cos_theta2 = (r2 - l1*l1 - l2*l2) / (2.0 * l1 * l2)
    
    # 可达性检查
    if abs(cos_theta2) > 1.0:
        return None
    
    solutions = []
    # 肘上解 (θ2 > 0)
    theta2_up = math.acos(cos_theta2)
    theta1_up = math.atan2(y, x) - math.atan2(l2*math.sin(theta2_up), 
                                               l1 + l2*math.cos(theta2_up))
    solutions.append((theta1_up, theta2_up))
    
    # 肘下解 (θ2 < 0)
    theta2_down = -theta2_up
    theta1_down = math.atan2(y, x) - math.atan2(l2*math.sin(theta2_down), 
                                                 l1 + l2*math.cos(theta2_down))
    solutions.append((theta1_down, theta2_down))
    
    return solutions

# 最小验证示例
if __name__ == "__main__":
    l1, l2 = 1.0, 1.0
    # 目标点：x=1.2, y=0.5（在可达范围内）
    target_x, target_y = 1.2, 0.5
    
    result = inverse_kinematics_2link(target_x, target_y, l1, l2)
    if result is None:
        print("目标点不可达")
    else:
        for i, (t1, t2) in enumerate(result):
            # 正运动学验证
            fx = l1*math.cos(t1) + l2*math.cos(t1+t2)
            fy = l1*math.sin(t1) + l2*math.sin(t1+t2)
            print(f"解{i+1}: θ1={math.degrees(t1):.2f}°, θ2={math.degrees(t2):.2f}°")
            print(f"  正运动学回代: ({fx:.4f}, {fy:.4f})")

运行与验证

直接运行脚本即可验证。程序输出两组关节角，并对每组角度执行正运动学回代，计算出的末端位置应与目标 (1.2, 0.5) 一致（误差在浮点精度范围内）。

通过条件：回代坐标与目标坐标误差小于 1e-6，且两组解分别对应肘上（θ2>0）与肘下（θ2<0）构型。

预期输出

解1: θ1=20.85°, θ2=53.13°
  正运动学回代: (1.2000, 0.5000)
解2: θ1=74.15°, θ2=-53.13°
  正运动学回代: (1.2000, 0.5000)

常见问题

• 返回 None：目标点超出机械臂可达范围（距原点距离大于 l1+l2 或小于 |l1−l2|），此时无实数解。
• 奇异位形：当目标点恰好在半径 l1+l2 的圆上时，两组解退化为同一组（θ2=0），此时机械臂完全伸直。