第3章：正运动学实战：2自由度机械臂正解计算、代码实现与验证

好，咱们直接进入正题。

上一章我们把DH参数法讲透了，现在该真刀真枪地算一把了。我选一个最简单的2自由度机械臂来做演示——为什么选它？因为手算能算清楚，代码能跑通，你也能一眼看出结果对不对。说白了，这是正运动学的“Hello World”。

3.1 2自由度机械臂的几何模型

想象一个肩膀关节和一个肘关节。肩膀固定在底座上，肘关节连着前臂。两个关节都是旋转的，没有滑动。

我习惯把这种臂叫做“RR构型”——两个旋转副。你在项目里见到的桌面小机械臂，十有八九都是这个结构。

参数定义如下：

• L1：大臂长度（从肩膀到肘）
• L2：小臂长度（从肘到腕/末端）
• θ1：肩膀关节角度
• θ2：肘关节角度

嗯，这里要注意：θ2是相对于大臂的夹角，不是绝对角度。很多新手在这里栽跟头，我当年也犯过这个错。

3.2 正解公式推导

正运动学要干什么？给你θ1和θ2，算出末端执行器的(x, y)坐标。说白了就是：

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

为什么会这样？

你想想看，大臂末端的位置是(L1*cosθ1, L1*sinθ1)。然后从小臂末端相对大臂末端，再转一个(θ1+θ2)的角度，走L2的距离。两个向量一加，就是最终位置。

我在项目中遇到过有人把第二项写成L2*cos(θ2)，结果画出来的机械臂像断了似的。记住：角度是累加的！

3.3 代码实现

我习惯用Python写原型，因为调试快。咱们直接上代码：

import numpy as np
import matplotlib.pyplot as plt

def forward_kinematics_2dof(theta1, theta2, L1, L2):
    """
    2自由度机械臂正运动学计算
    theta1, theta2: 弧度制
    L1, L2: 臂长
    返回: (x, y) 末端坐标
    """
    # 大臂末端位置
    x1 = L1 * np.cos(theta1)
    y1 = L1 * np.sin(theta1)
    
    # 小臂末端位置（相对大臂末端）
    x2 = x1 + L2 * np.cos(theta1 + theta2)
    y2 = y1 + L2 * np.sin(theta1 + theta2)
    
    return (x2, y2)

# 测试一下
L1 = 1.0
L2 = 0.8

# 情况1：两个关节都伸直
theta1 = 0
theta2 = 0
x, y = forward_kinematics_2dof(theta1, theta2, L1, L2)
print(f"伸直状态: x={x:.3f}, y={y:.3f}")
# 预期: x=1.8, y=0.0

# 情况2：肩膀45度，肘关节-90度
theta1 = np.pi/4
theta2 = -np.pi/2
x, y = forward_kinematics_2dof(theta1, theta2, L1, L2)
print(f"弯曲状态: x={x:.3f}, y={y:.3f}")

跑一下这段代码，你会看到：

• 伸直时，末端在(1.8, 0.0)——两个臂长加起来，合理
• 弯曲时，末端在某个位置——你可以手算验证一下

3.4 可视化验证

光看数字不够直观。我习惯画出来——眼睛比数字更敏感。

def plot_arm(theta1, theta2, L1, L2):
    # 计算各关节位置
    x0, y0 = 0, 0  # 底座
    x1 = L1 * np.cos(theta1)
    y1 = L1 * np.sin(theta1)
    x2, y2 = forward_kinematics_2dof(theta1, theta2, L1, L2)
    
    # 绘制
    plt.figure(figsize=(6, 6))
    plt.plot([x0, x1], [y0, y1], 'b-o', linewidth=3, label='大臂')
    plt.plot([x1, x2], [y1, y2], 'r-o', linewidth=3, label='小臂')
    plt.scatter(x2, y2, color='green', s=100, label='末端')
    
    plt.xlim(-2, 2)
    plt.ylim(-2, 2)
    plt.grid(True)
    plt.axis('equal')
    plt.legend()
    plt.title(f'θ1={np.degrees(theta1):.0f}°, θ2={np.degrees(theta2):.0f}°')
    plt.show()

# 画几个典型姿态
plot_arm(0, 0, L1, L2)                    # 伸直
plot_arm(np.pi/4, -np.pi/2, L1, L2)       # 弯曲
plot_arm(np.pi/2, np.pi/4, L1, L2)        # 向上举

看到图了吗？大臂蓝色，小臂红色，末端绿色点。一眼就能看出机械臂的姿态对不对。

3.5 验证：用几何法交叉检验

代码写完了，怎么确认它是对的？我推荐用几何法做交叉验证。

举个具体例子：

• L1 = 1.0, L2 = 0.8
• θ1 = 30° (π/6), θ2 = 60° (π/3)

手算步骤：

1. 大臂末端：x1 = 1.0 * cos(30°) = 0.866, y1 = 1.0 * sin(30°) = 0.5
2. 小臂相对角度：θ1+θ2 = 90°
3. 小臂末端：x2 = 0.866 + 0.8 * cos(90°) = 0.866, y2 = 0.5 + 0.8 * sin(90°) = 1.3

用代码算一下：

theta1 = np.pi/6
theta2 = np.pi/3
x, y = forward_kinematics_2dof(theta1, theta2, 1.0, 0.8)
print(f"代码结果: x={x:.3f}, y={y:.3f}")
# 输出: x=0.866, y=1.300

完全吻合。嗯，这就放心了。

3.6 工作空间分析

正运动学还有一个重要应用：看你的机械臂能到达哪些位置。这就是工作空间。

对于2自由度臂，工作空间是一个圆环：

• 外径 = L1 + L2（完全伸直）
• 内径 = |L1 - L2|（完全折叠）

我习惯用蒙特卡洛法来可视化：随机生成一堆角度，算末端位置，然后画点。

# 蒙特卡洛法绘制工作空间
points = []
for _ in range(5000):
    t1 = np.random.uniform(-np.pi, np.pi)
    t2 = np.random.uniform(-np.pi, np.pi)
    x, y = forward_kinematics_2dof(t1, t2, L1, L2)
    points.append((x, y))

xs, ys = zip(*points)
plt.figure(figsize=(6, 6))
plt.scatter(xs, ys, s=1, alpha=0.5)
plt.axis('equal')
plt.title('2自由度机械臂工作空间')
plt.show()

看到那个圆环了吗？中间有个空洞——那是机械臂够不到的区域。这个空洞的大小取决于L1和L2的差值。差值越大，空洞越大。

3.7 本章小结

正运动学说白了就是：给角度，算位置。公式简单，但意义重大——它是所有运动规划的基础。

我建议你亲手敲一遍代码，改几个参数看看效果。把L1改成2.0，或者把θ2范围改成0到π，观察工作空间怎么变。动手试过的东西，才是你自己的。

这张图把本章的知识脉络串起来了。从左到右：输入参数 → 核心公式 → 输出结果。下面两条分支是验证方法和应用场景。你照着这个结构学，思路会很清楚。