机械臂入门：自由度与正逆运动学，从关节角到笔尖坐标

你搭好了一只桌面机械臂，几个舵机串起来，能转能动。可问题来了：你想让末端的笔尖精确落在桌面上 (x=120mm, y=80mm) 这个点，你该把每个舵机转到多少度？

手动试？转一下肩关节，再调一下肘关节，眼睛盯着笔尖凑——凑半天还差着十几毫米。这不是控制，这是碰运气。机械臂真正的难点从来不是「让舵机转」，那个在 PCA9685 多舵机驱动 里已经解决了。难的是：给定一个空间坐标，算出每个关节该转多少度。

这就是运动学要回答的问题。本篇不堆数学，只讲两件事——正着算和反着算，然后给你一段能让笔尖走到任意可达坐标的代码。读完你会发现，2 连杆机械臂的逆运动学，就是一道初中三角形题。

前置：本篇默认你已经能用 PCA9685 同时驱动多个舵机、知道怎么把「角度」写进舵机。如果还不熟，先回 robot-servo-pca9685 把多舵机控制跑通，再回来。

---

一、自由度：先数清楚你的臂能动几下

自由度（DOF, Degree of Freedom）= 机械臂能独立运动的关节数量。 一个舵机驱动一个关节，一个关节贡献一个自由度。就这么简单，先把这个数清楚。

• 2DOF：两个关节，能在一个平面内把末端送到一片区域。画画、平面抓取够用。本篇的主角。
• 3DOF：加一个关节。要么加一个让末端能上下抬（进入立体空间），要么加一个腕部让末端能调姿态。
• 4DOF：桌面臂最常见的配置。底座旋转（管左右）+ 大臂（肩）+ 小臂（肘）+ 腕部/夹爪旋转。能在三维空间里把夹爪送到一个位置，并简单调整朝向。
• 6DOF：工业臂标配。三个关节定位置（末端到哪），三个关节定姿态（末端朝哪个方向、怎么倾斜）。机械上能模拟人手臂的全部动作。

为什么强调数自由度？因为自由度决定了你的臂能不能到达某个「位姿」。想让夹爪既到某点、又以某个角度伸进去夹东西，至少要 6 个自由度才能完全自由。4DOF 桌面臂只能保证「到位置」，姿态是受限的——这不是缺陷，是设计取舍，桌面玩够用了。

观点先放这：新手别上来就买 6DOF。 6 个关节意味着逆运动学有多解、有耦合，你会被数学劝退。从 2DOF 平面臂开始，把「正着算」「反着算」吃透，能让笔尖画个圆，你就真懂运动学了。后面 4DOF、6DOF 不过是同一套思路加维度。

---

二、正运动学：已知关节角，算末端在哪（简单）

正运动学（Forward Kinematics）：给每个关节一个角度，算出末端在哪个坐标。 这是「正着推」，简单，纯几何。

看一个 2 连杆平面臂。两段连杆长度 L1、L2，第一关节角 θ₁（大臂相对水平线），第二关节角 θ₂（小臂相对大臂）。肩关节固定在原点 (0,0)。

肘关节（第一段末端）的位置，就是从原点出发、沿 θ₁ 方向走 L1：

肘_x = L1 · cos(θ₁)
肘_y = L1 · sin(θ₁)

末端（笔尖）的位置，是在肘关节基础上，再沿 (θ₁+θ₂) 方向走 L2：

末端_x = L1·cos(θ₁) + L2·cos(θ₁ + θ₂)
末端_y = L1·sin(θ₁) + L2·sin(θ₁ + θ₂)

注意第二段用的是 θ₁ + θ₂，不是 θ₂。因为 θ₂ 是小臂相对大臂的角度，要叠加上大臂自己的角度，才是小臂在世界坐标里的真实方向。这是新手最容易错的一处。

正运动学就这么点东西——已知角度，正弦余弦一加，末端坐标出来。难的是反过来。

---

三、逆运动学：已知目标点，反推关节角（难，但 2 连杆有解析解）

逆运动学（Inverse Kinematics, IK）：给一个目标坐标 (x, y)，反推每个关节该转多少度。 这是机械臂控制真正的核心，也是难点。

为什么难？因为这是「反着解」。正运动学是「输入角度 → 输出坐标」，函数关系明确；逆运动学要从坐标反推角度，可能无解（点太远够不着）、可能多解（同一个点，胳膊「上弯」和「下弯」两种姿势都能到）。对复杂机械臂，逆运动学往往没有公式可写，得靠数值迭代去逼近。

但好消息是：2 连杆平面臂的逆运动学有干净的解析解（闭式解），就是几个三角函数，不用迭代。 这就是我让你从 2 连杆起步的原因。

推导：用余弦定理求 θ₂

把肩、肘、末端三个点连起来，是一个三角形。三边分别是 L1、L2，和从肩到目标点的距离 r = √(x² + y²)。

对这个三角形用余弦定理。肘关节处的夹角对应的对边是 r：

r² = L1² + L2² − 2·L1·L2·cos(肘内角)

解出肘内角的余弦：

cos(肘内角) = (x² + y² − L1² − L2²) / (2·L1·L2)

而 θ₂（小臂相对大臂转过的角）和这个肘内角是补角关系，所以：

cos(θ₂) = (x² + y² − L1² − L2²) / (2·L1·L2)
θ₂ = ±acos( 上式 )

这里的 ± 就是「上弯/下弯」两个解。取正号是一种姿势，取负号是镜像的另一种。

求 θ₁

θ₁ 分两部分：目标点相对原点的方位角，减去（或加上）大臂到目标连线的偏角。

θ₁ = atan2(y, x) − atan2(L2·sin(θ₂), L1 + L2·cos(θ₂))

atan2 是带象限判断的反正切，一定要用它，别用 atan——否则点在二三象限时角度会算错。

可达性判断

在算 acos 之前必须先卡一道：如果目标点比两臂伸直还远（r > L1+L2），或者比两臂折叠还近（r < |L1−L2|），那就够不着，cos(θ₂) 会落到 [-1, 1] 之外，acos 直接返回 NaN。这一步不判，程序就崩或者输出乱角度甩臂。

---

四、完整可跑代码：输入目标坐标，输出关节角并发给舵机

下面是一段完整的 Python，跑在树莓派上。逆运动学函数纯数学、可单独测试；舵机驱动复用 PCA9685 那套，这里不重复讲原理，只调用。

import math
from adafruit_servokit import ServoKit

# ---------- 第一部分：纯数学，逆运动学（可单独测试，不依赖硬件）----------

L1 = 100.0   # 大臂长度 mm，按你实际连杆量
L2 = 80.0    # 小臂长度 mm

def inverse_kinematics(x, y, elbow_up=True):
    """
    输入目标坐标 (x, y)，返回 (theta1, theta2) 角度（度）。
    elbow_up: True 取上弯解，False 取下弯解。
    够不着时返回 None。
    """
    r2 = x * x + y * y
    r = math.sqrt(r2)

    # 可达性判断：太远或太近都算不出
    if r > (L1 + L2) or r < abs(L1 - L2):
        print(f"  目标 ({x},{y}) 不可达：距离 {r:.1f}mm 超出 [{abs(L1-L2):.0f}, {L1+L2:.0f}]")
        return None

    # 余弦定理求 theta2
    cos_t2 = (r2 - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    cos_t2 = max(-1.0, min(1.0, cos_t2))   # 夹紧，防浮点误差导致 acos 出 NaN
    t2 = math.acos(cos_t2)
    if not elbow_up:
        t2 = -t2

    # theta1
    t1 = math.atan2(y, x) - math.atan2(L2 * math.sin(t2), L1 + L2 * math.cos(t2))

    return math.degrees(t1), math.degrees(t2)


# ---------- 第二部分：把角度发给舵机（复用 PCA9685 多舵机驱动）----------

kit = ServoKit(channels=16)

# 舵机零点标定：装配时机械臂摆在「θ1=0、θ2=0」姿态对应的舵机角度
SERVO1_OFFSET = 90    # 大臂舵机，θ1=0 时舵机在 90 度
SERVO2_OFFSET = 90    # 小臂舵机，θ2=0 时舵机在 90 度

def move_to(x, y, elbow_up=True):
    result = inverse_kinematics(x, y, elbow_up)
    if result is None:
        return False
    t1, t2 = result

    servo1 = SERVO1_OFFSET + t1
    servo2 = SERVO2_OFFSET + t2

    # 角度限位：舵机只能转 0~180，超了截断并告警
    if not (0 <= servo1 <= 180 and 0 <= servo2 <= 180):
        print(f"  角度超限：servo1={servo1:.0f}, servo2={servo2:.0f}，跳过")
        return False

    kit.servo[0].angle = servo1
    kit.servo[1].angle = servo2
    print(f"  到达 ({x},{y}) -> θ1={t1:.1f}° θ2={t2:.1f}°")
    return True


if __name__ == "__main__":
    move_to(120, 50)    # 让笔尖走到 (120, 50)
    import time; time.sleep(1)
    move_to(80, 100)
    time.sleep(1)
    move_to(300, 0)     # 故意够不着，看可达性判断生效

SERVO1_OFFSET / SERVO2_OFFSET 是机械装配的零点标定值——你算出的 θ 是数学角度，舵机实际转角和它差一个安装偏置，必须手工量出来填进去。这步偷懒，笔尖坐标和实际位置永远对不上。

---

五、你应该看到什么

代码跑起来，正常的现象是：

• 调用 move_to(120, 50)，机械臂的笔尖（或末端标记点）实际落到桌面坐标 (120mm, 50mm) 附近，误差几毫米属正常（舵机精度+连杆间隙）。
• 终端打印出对应的 θ1、θ2 角度，数值随目标点变化。
• 连续调用不同坐标，臂会逐点移动过去，每个点都停得住。
• 调用够不着的点（如 (300, 0)），终端打印「不可达」，臂不动——而不是乱甩或报错崩溃。

把笔装上，给一串坐标，末端就能在纸上点出/连出你要的图形。这一刻你就把「坐标」和「关节角」打通了。

---

六、故障排查

现象	原因	排查方向
笔尖到不了指定点	L1/L2 填的不是真实连杆长 / 零点偏置没标定	拿尺子量连杆实际长度，重测 OFFSET
角度超限被跳过	目标点对应的解需要舵机转过 0~180 范围	换 elbow_up 取另一个解，或调整安装零点
笔尖抖动停不住	舵机供电不足/共地不良，或目标点贴近工作空间边缘	换独立电源、补共地线；目标点避开极限位置
末端坐标左右/上下反了	舵机安装方向与数学角度增量方向相反	把对应 servo 角度公式的 +t 改成 −t
程序输出 NaN	漏了可达性判断，或 cos 值因浮点误差略超 [-1,1]	确认 max(-1,min(1,...)) 夹紧那行在，且可达性先判

---

七、两个变体

变体 1：让笔尖画一个圆。 圆是参数方程，按角度采样一圈坐标，逐点 move_to 就行：

import math, time
cx, cy, radius = 100, 40, 30   # 圆心和半径
for i in range(0, 360, 10):    # 每 10 度采一个点
    a = math.radians(i)
    x = cx + radius * math.cos(a)
    y = cy + radius * math.sin(a)
    move_to(x, y)
    time.sleep(0.05)

如果圆上有的点画不出来，说明它出了工作空间——把圆心往臂的「舒适区」（离原点约 L1 距离）挪、半径调小。

变体 2：加第三轴变 3DOF。 在底座加一个旋转关节 θ₀，让平面臂能绕竖直轴转，进入三维。做法是把三维目标 (X, Y, Z) 先投影：底座转角 θ0 = atan2(Y, X)，水平距离 d = √(X²+Y²)，再把 (d, Z) 当成 2 连杆平面臂的 (x, y) 喂进 inverse_kinematics。核心的 2 连杆 IK 一行没改——这就是为什么先吃透平面解析解，后面加维度水到渠成。

---

八、动手挑战

让末端沿一条直线匀速移动——比如从 (60, 90) 走到 (140, 90)，画一条水平线。提示：直线插值，把起点到终点等分成 N 段，逐点 move_to。

进阶想想：直线上等距采的点，对应的关节角是不等距变化的（越靠近工作空间边缘，同样的笔尖位移需要的关节角变化越大——这就是「奇异点」附近的现象，臂会在那里突然变慢或变快）。怎么让笔尖匀速而不是关节角匀速？这一步想通了，你就摸到了轨迹规划的门槛。

---

小结与下一步

• 自由度 = 独立关节数，决定能到达什么位姿；桌面臂 4DOF 够玩，新手别上来啃 6DOF。
• 正运动学：已知角度求末端坐标，三角函数一加，简单。
• 逆运动学：已知坐标反推角度，是核心难点；但 2 连杆有干净的解析解（余弦定理 + atan2），不用迭代。
• 算之前必判可达性，算之后必判角度限位，否则 NaN 和堵转甩臂在等你。
• 别一上来啃 DH 参数和雅可比矩阵。把 2 连杆解析解吃透、能让笔尖画个圆，运动学的核心你就懂了——剩下的都是同一套思路加维度。

下一步，你的机械臂现在能精确到点了，但还是「写死的脚本」。要让它接传感器、做任务调度、和其它节点通信，该上一套机器人操作系统了——去 robot-ros 把 ROS 的节点与话题机制搭起来。想直接让臂「看到东西自己决定怎么抓」，跳到 robot-embodied-ai。回 机械臂与机器人专题 看完整路线。