机器人运动学是机器人控制的理论基础。本文用通俗的语言解释正运动学和逆运动学的概念，并用Python实现一个简单的2关节机械臂运动学。

一、基本概念

　　正运动学：已知各关节角度，求末端执行器的位置和姿态。比如你知道机械臂每个关节转了多少度，就可以算出机械手抓在空间中的位置。

　　逆运动学：已知末端执行器要到达的目标位置，反推各关节需要转多少度。这是实际控制中更常用的——你告诉机器人"把手伸到那个点"，它自己算关节角度。

二、2关节机械臂正运动学

　　一个2关节机械臂由两个连杆组成，长度分别为L1和L2，关节角度为θ1和θ2：

　　末端位置 x = L1*cos(θ1) + L2*cos(θ1+θ2)

　　末端位置 y = L1*sin(θ1) + L2*sin(θ1+θ2)

import math

def forward_kinematics(L1, L2, theta1, theta2):
    """2关节机械臂正运动学"""
    x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
    y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)
    return x, y

# 示例：连杆10cm和8cm，关节角30度和45度
x, y = forward_kinematics(10, 8, math.radians(30), math.radians(45))
print(f"末端位置: ({x:.2f}, {y:.2f}) cm")

三、2关节机械臂逆运动学

　　给定目标点(x,y)，解算两个关节角θ1和θ2：

　　cos(θ2) = (x² + y² - L1² - L2²) / (2 * L1 * L2)

　　θ2 = arccos(cosθ2) (注意有两个解：肘部向上和向下)

　　θ1 = arctan(y/x) - arctan(L2*sin(θ2) / (L1 + L2*cos(θ2)))

def inverse_kinematics(L1, L2, x, y, elbow_up=True):
    """2关节机械臂逆运动学"""
    d = math.sqrt(x*x + y*y)
    if d > L1 + L2:
        return None  # 目标超出可达范围
    
    cos_theta2 = (x*x + y*y - L1*L1 - L2*L2) / (2 * L1 * L2)
    cos_theta2 = max(-1, min(1, cos_theta2))  # 限幅
    
    theta2 = math.acos(cos_theta2)
    if not elbow_up:
        theta2 = -theta2
    
    theta1 = math.atan2(y, x) - math.atan2(
        L2 * math.sin(theta2), L1 + L2 * math.cos(theta2))
    
    return theta1, theta2

# 测试
angles = inverse_kinematics(10, 8, 15, 5)
if angles:
    t1, t2 = angles
    print(f"关节角度: θ1={math.degrees(t1):.1f}°, θ2={math.degrees(t2):.1f}°")
    # 验证正解
    x2, y2 = forward_kinematics(10, 8, t1, t2)
    print(f"验证位置: ({x2:.2f}, {y2:.2f})")

四、在Arduino上实现

　　将上述算法移植到Arduino，用Servo库控制舵机实现2关节臂。关键在于使用float类型和math.h中的三角函数。

五、常见问题

　　Q：末端到达不了目标点？ A：检查目标点是否在工作空间内（以L1+L2为半径的圆范围）。

　　Q：运动过程中抖动？ A：计算出的角度变化太大，需要插值平滑运动。把路径分成多个小段逐段运动。