机器人运动学：从坐标变换到正逆解

运动学要回答两个问题：给定各关节角度，末端在哪（正运动学）； 想让末端到某个位姿，各关节该转多少（逆运动学）。前者是计算，后者往往是解方程。

这篇笔记用两连杆平面机械臂把这两件事完整走一遍，公式尽量给出推导思路而不只是结论。

位姿与坐标系

一个刚体在平面上的位姿用 $(x, y, \theta)$ 描述，在空间中则需要位置加姿态。姿态最常用的表示有三种：

旋转矩阵 $R$：3×3 正交矩阵，$R^T R = I$，直观但占 9 个数；
欧拉角 / RPY：人类友好，但有万向锁；
四元数 $q = (w, x, y, z)$：紧凑、无奇异，工程上最常用。

三者可以互相转换，ROS 2 里用 tf2 统一处理，不要自己手写转换。

齐次变换

把旋转和平移写进一个 4×4 矩阵，连续变换就变成矩阵连乘：

$$ T = \begin{bmatrix} R & p \ 0 & 1 \end{bmatrix}, \qquad T_1 T_2 = \begin{bmatrix} R_1 R_2 & R_1 p_2 + p_1 \ 0 & 1 \end{bmatrix} $$

这个形式的好处是：从基座到末端的整条链，就是把每段变换按顺序乘起来。

$$ T_{\text{base}}^{\text{end}} = T_0^1 T_1^2 \cdots T_{n-1}^n $$

注意顺序不能交换——矩阵乘法不满足交换律，这正是"先转再走"和"先走再转"结果不同的原因。

两连杆平面臂的正解

设连杆长度 $l_1, l_2$，关节角 $\theta_1, \theta_2$（相对前一连杆），末端位置：

$$ \begin{aligned} x &= l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) \ y &= l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2) \end{aligned} $$

用代码验证一下：

python

import math


def fk(l1, l2, t1, t2):
    x = l1 * math.cos(t1) + l2 * math.cos(t1 + t2)
    y = l1 * math.sin(t1) + l2 * math.sin(t1 + t2)
    return x, y


print(fk(1.0, 1.0, 0.0, math.pi / 2))  # (1.0, 1.0)

当 $\theta_1 = 0, \theta_2 = 90°$ 时，第一段沿 x 轴走 1，第二段向上走 1，结果 $(1, 1)$ 与直觉一致。

逆解：几何法

已知目标 $(x, y)$，先算到原点的距离：

$$ r^2 = x^2 + y^2 $$

由余弦定理：

$$ \cos\theta_2 = \frac{r^2 - l_1^2 - l_2^2}{2 l_1 l_2} $$

$$ \theta_2 = \pm \arccos(\cos\theta_2) $$

正负号对应肘部朝上/朝下两组解，这是冗余的一种最简形式。 $\theta_1$ 则用两段角相加得到：

$$ \theta_1 = \operatorname{atan2}(y, x) - \operatorname{atan2}\big(l_2 \sin\theta_2,; l_1 + l_2 \cos\theta_2\big) $$

python

import math


def ik(l1, l2, x, y, elbow_up=True):
    r2 = x * x + y * y
    c2 = (r2 - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    if c2 < -1 or c2 > 1:
        raise ValueError('目标点超出工作空间')
    t2 = math.acos(max(-1.0, min(1.0, c2)))
    if not elbow_up:
        t2 = -t2
    t1 = math.atan2(y, x) - math.atan2(l2 * math.sin(t2), l1 + l2 * math.cos(t2))
    return t1, t2

两个细节值得注意：

工作空间判断：$|c_2| > 1$ 时目标不可达，必须提前拦截，否则 acos 会抛异常；
解的选择：多解时通常选离当前关节角最近的一组，避免机械臂大范围甩动。

雅可比矩阵与奇异位形

雅可比 $J$ 描述关节速度到末端速度的映射：

$$ \dot{x} = J(\theta),\dot{\theta} $$

对两连杆臂求偏导可得：

$$ J = \begin{bmatrix} -l_1 s_1 - l_2 s_{12} & -l_2 s_{12} \ l_1 c_1 + l_2 c_{12} & l_2 c_{12} \end{bmatrix} $$

当 $\det J = 0$ 时进入奇异位形，此时某些方向上的速度无法实现，逆解也会变得极不稳定。平面两连杆臂的奇异位形出现在 $\theta_2 = 0$ 或 $\pi$，也就是手臂完全伸直或完全折叠的位置——实际调试时机械臂在伸直状态下最容易"抖"。

工程上的处理办法：

路径规划时避开奇异点附近；
用阻尼最小二乘（DLS）代替直接求逆；
限制单步关节增量，超限就减速而不是硬跟。

从平面到空间

真实机械臂还需要考虑：

DH 参数：用四个参数描述相邻连杆关系，标准 DH 与改进 DH 的坐标系定义不同，混用是最常见的错误来源；
关节限位：逆解结果必须落在物理限位内，否则会撞限位或触发保护；
标定误差：连杆长度和零位误差会让"算出来"和"走到"差几毫米，视觉抓取时这点误差就够抓空。

小结

平面两连杆虽然简单，但把正解、逆解、多解、奇异这几个核心问题都包含了。把这套推导在纸上写一遍、再用代码验证一次，比看十篇综述更有用。

机器人运动学：从坐标变换到正逆解 ​

位姿与坐标系 ​

齐次变换 ​

两连杆平面臂的正解 ​

逆解：几何法 ​

雅可比矩阵与奇异位形 ​

从平面到空间 ​

小结 ​