嵌入式知识篇---机械臂的运动学结算（简单2自由度）

机械臂的 “解算” 本质是运动学解算，核心是解决 “关节角度” 和 “末端位置” 的互转问题。下面用最通俗的方式解释，并结合 2 自由度平面机械臂（结构最简单，适合入门）给出 Python 和 ESP32 代码，以及参数细节。

一、机械臂运动学解算的通俗原理

想象你有一条 “简化的手臂”：只有大臂和小臂两个关节（类似人类的上臂和前臂），只能在桌面（X-Y 平面）内运动。

• 正解：知道 “大臂转 30°，小臂转 60°”，算出 “手掌” 的位置（比如在桌面的 x=15cm，y=8cm 处）。
• 逆解：想让 “手掌” 拿到桌面的杯子（位置 x=20cm，y=10cm），算出 “大臂该转多少度，小臂该转多少度”。

二、2 自由度机械臂的结构与参数定义

为了让解算更具体，先定义机械臂的基础参数（单位统一为厘米和度）：

• 关节 1（大臂底座）：绕原点旋转，角度为 θ₁（单位：度，0° 指大臂沿 X 轴正方向，逆时针为正）；
• 关节 2（小臂）：绕大臂末端旋转，角度为 θ₂（单位：度，0° 指小臂与大臂伸直成一条线，向上弯曲为正）；
• 臂长：大臂长度 L₁=10cm，小臂长度 L₂=10cm（可根据实际机械臂调整）；
• 末端位置：用坐标 (x, y) 表示（单位：cm，原点为关节 1 的旋转中心）。

三、正解：已知关节角度→求末端位置（Python 代码）

原理：

大臂和小臂的运动可以分解为 “水平方向（X 轴）” 和 “垂直方向（Y 轴）” 的位移叠加：

• 大臂末端的坐标：(L₁×cosθ₁, L₁×sinθ₁)
• 小臂相对于大臂末端的位移：(L₂×cos (θ₁+θ₂), L₂×sin (θ₁+θ₂))（因为小臂角度是相对于大臂的，总角度为 θ₁+θ₂）
• 末端总坐标：x = 大臂 X 位移 + 小臂 X 位移；y = 大臂 Y 位移 + 小臂 Y 位移

Python 代码（含参数解释）：

import math  

def forward_kinematics(theta1, theta2, L1=10, L2=10):  
    """  
    正运动学解算：已知关节角度，计算末端坐标  
    参数：  
        theta1: 关节1角度（度），范围0~360，逆时针为正  
        theta2: 关节2角度（度），范围0~180，向上弯曲为正  
        L1: 大臂长度（cm），默认10cm  
        L2: 小臂长度（cm），默认10cm  
    返回：  
        (x, y): 末端坐标（cm），保留2位小数  
    """  
    # 角度转弧度（math库的三角函数要求弧度输入）  
    theta1_rad = math.radians(theta1)  
    theta2_rad = math.radians(theta2)  
    total_angle_rad = theta1_rad + theta2_rad  # 小臂相对于X轴的总角度  

    # 计算大臂末端坐标  
    x1 = L1 * math.cos(theta1_rad)  # 大臂在X轴的位移  
    y1 = L1 * math.sin(theta1_rad)  # 大臂在Y轴的位移  

    # 计算小臂相对于大臂末端的位移  
    x2 = L2 * math.cos(total_angle_rad)  # 小臂在X轴的位移  
    y2 = L2 * math.sin(total_angle_rad)  # 小臂在Y轴的位移  

    # 末端总坐标（叠加大臂和小臂的位移）  
    x = x1 + x2  
    y = y1 + y2  

    return round(x, 2), round(y, 2)  


# 测试：关节1=30°，关节2=60°，臂长10cm  
theta1 = 30    # 大臂向右偏30°  
theta2 = 60    # 小臂向上弯60°  
x, y = forward_kinematics(theta1, theta2)  
print(f"正解结果：关节角度(θ₁={theta1}°, θ₂={theta2}°) → 末端坐标(x={x}cm, y={y}cm)")  
# 输出：正解结果：关节角度(θ₁=30°, θ₂=60°) → 末端坐标(x=7.5cm, y=12.99cm)  

四、逆解：已知末端位置→求关节角度（Python 代码）

原理：

已知目标坐标 (x, y)，反推 θ₁和 θ₂：

1. 先算目标点到原点的距离 D（大臂和小臂组成的三角形的第三边）：D = √(x² + y²)；
2. 用余弦定理算大臂与小臂的夹角 α（θ₂ = 180°-α）；
3. 算大臂与 X 轴的夹角 β（θ₁ = β - γ，其中 γ 是大臂与 D 的夹角，用余弦定理计算）。

Python 代码（含参数解释）：

import math  

def inverse_kinematics(x_target, y_target, L1=10, L2=10):  
    """  
    逆运动学解算：已知目标坐标，计算关节角度  
    参数：  
        x_target, y_target: 目标坐标（cm）  
        L1, L2: 大臂和小臂长度（cm），默认10cm  
    返回：  
        (theta1, theta2): 关节角度（度），保留2位小数  
    """  
    # 步骤1：计算目标点到原点的距离D  
    D = math.sqrt(x_target**2 + y_target**2)  

    # 检查目标是否可达（机械臂最大范围：L1+L2；最小范围：|L1-L2|）  
    if D > L1 + L2 or D < abs(L1 - L2):  
        raise ValueError(f"目标点({x_target},{y_target})不可达！最大范围{L1+L2}cm，最小范围{abs(L1-L2)}cm")  

    # 步骤2：用余弦定理算大臂与小臂的夹角α（θ₂ = 180°-α）  
    alpha = math.acos((L1**2 + L2**2 - D**2) / (2 * L1 * L2))  # 弧度  
    theta2 = 180 - math.degrees(alpha)  # 转换为度  

    # 步骤3：算大臂与X轴的夹角θ₁  
    beta = math.atan2(y_target, x_target)  # 目标点与X轴的夹角（弧度）  
    gamma = math.acos((L1**2 + D**2 - L2**2) / (2 * L1 * D))  # 大臂与D的夹角（弧度）  
    theta1 = math.degrees(beta - gamma)  # 转换为度  

    # 确保角度在合理范围（θ₁: -180~180°；θ₂: 0~180°）  
    theta1 = round(theta1 % 360, 2)  # 处理负角度  
    theta2 = round(theta2, 2)  

    return theta1, theta2  


# 测试：目标坐标为正解的结果(x=7.5cm, y=12.99cm)  
x_target, y_target = 7.5, 12.99  
theta1, theta2 = inverse_kinematics(x_target, y_target)  
print(f"逆解结果：目标坐标(x={x_target}cm, y={y_target}cm) → 关节角度(θ₁={theta1}°, θ₂={theta2}°)")  
# 输出：逆解结果：目标坐标(x=7.5cm, y=12.99cm) → 关节角度(θ₁=30.0°, θ₂=60.0°)（误差来自四舍五入）  

五、ESP32 控制实例（Arduino 代码）

ESP32 通过逆解计算关节角度，再控制两个数字舵机转动到目标位置。

硬件准备：

• ESP32 开发板；
• 2 个数字舵机（如 SG90，工作电压 3.3-5V）；
• 杜邦线若干。

ESP32 代码（含参数解释）：

#include   

// 定义舵机对象（控制引脚：D4控制关节1，D5控制关节2）  
Servo servo1;  // 关节1（大臂）  
Servo servo2;  // 关节2（小臂）  

// 机械臂参数（单位：cm）  
const float L1 = 10.0;  // 大臂长度  
const float L2 = 10.0;  // 小臂长度  

// 逆运动学解算函数（返回值：角度，单位：度）  
bool inverse_kinematics(float x_target, float y_target, float &theta1, float &theta2) {  
  float D = sqrt(x_target*x_target + y_target*y_target);  // 目标点到原点的距离  

  // 检查是否可达  
  if (D > L1 + L2 || D < abs(L1 - L2)) {  
    return false;  // 不可达  
  }  

  // 计算关节2角度theta2  
  float alpha = acos((L1*L1 + L2*L2 - D*D) / (2*L1*L2));  // 弧度  
  theta2 = 180 - alpha * 180 / PI;  // 转换为度  

  // 计算关节1角度theta1  
  float beta = atan2(y_target, x_target);  // 目标点与X轴夹角（弧度）  
  float gamma = acos((L1*L1 + D*D - L2*L2) / (2*L1*D));  // 大臂与D的夹角（弧度）  
  theta1 = (beta - gamma) * 180 / PI;  // 转换为度  

  // 确保角度在舵机范围内（舵机通常0°-180°）  
  theta1 = constrain(theta1, 0, 180);  
  theta2 = constrain(theta2, 0, 180);  
  return true;  
}  

void setup() {  
  Serial.begin(115200);  
  servo1.attach(4);  // 关节1接D4  
  servo2.attach(5);  // 关节2接D5  

  // 目标：让末端到达(7.5cm, 12.99cm)（正解的结果）  
  float x_target = 7.5;  
  float y_target = 12.99;  
  float theta1, theta2;  

  if (inverse_kinematics(x_target, y_target, theta1, theta2)) {  
    Serial.printf("逆解成功：theta1=%.2f°, theta2=%.2f°\n", theta1, theta2);  
    servo1.write(theta1);  // 控制关节1转动  
    servo2.write(theta2);  // 控制关节2转动  
  } else {  
    Serial.println("目标点不可达！");  
  }  
}  

void loop() {  
  // 无需循环操作，一次到位  
}  

六、参数与解算细节总结

参数	含义	单位	范围示例
θ₁	关节 1（大臂）角度	度	0°~180°
θ₂	关节 2（小臂）角度	度	0°~180°
L₁, L₂	大臂、小臂长度	cm	自定义（如 10cm）
(x, y)	末端坐标	cm	由臂长决定范围
D	目标点到原点的距离	cm		L₁-L₂	~L₁+L₂

核心结论

• 正解：通过三角函数叠加关节位移，实现 “关节角度→位置” 的计算，用于实时监控末端位置；
• 逆解：通过余弦定理反推关节角度，实现 “位置→关节角度” 的计算，是机械臂控制的核心（比如让机械臂抓取指定位置的物体）。

实际 6 自由度机械臂解算更复杂（需用矩阵），但基础逻辑和 2 自由度一致，都是 “分解运动” 和 “几何关系推导”。