机器人手臂运动学入门——正向运动学、逆向运动学和雅可比矩阵

机械臂的电机只能旋转每个关节的角度，然而我们实际想要的，几乎总是任务空间中定义的动作：“将机械臂移动到这个位置和方向”。运动学是连接这两个世界——“关节的运动状态”和“我们希望机械臂执行的动作”——的数学，也是所有控制机械臂的软件的基础。本文将系统地讲解正向运动学（通过关节角度确定机械臂的姿态）、逆向运动学（解决相反的问题）以及雅可比矩阵（连接两个空间中的“速度”）——全程遵循方程式。

现场协作机器人手臂

图片：UR16e 机器人手臂 (Auledas, CC BY-SA 4.0), Wikimedia Commons.本文方程所描述的关节连杆结构具有代表性，而非特定型号或品牌的运动学参数。

0. 本文内容

• 机器人手臂运动学解决的问题及其必要性

• 输入（关节角度、目标姿态）与输出（手部姿态、关节角度）之间的关系

• 如何使用齐次变换矩阵和DH参数构建正向运动学

• 逆运动学“不一定有唯一解”的原因，以及解析方法和数值方法的区别

• 雅可比矩阵是什么，以及它如何将关节速度与手部速度联系起来

• 奇异点处会发生什么，以及冗余自由度的作用

• 代表性逆运动学算法之间的区别——CCD、FABRIK、阻尼最小二乘法等

• 运动学在工业机器人、人形机器人和CG动画中的应用差异

1.首先，总结一下：什么是机械臂运动学

一句话概括：运动学是一个数学框架，它利用纯粹的几何关系，在机械臂的两种不同表示形式之间进行转换——关节角度（关节空间）和机械臂的位置/姿态（任务空间），而不考虑力或电机特性。

“纯粹的几何关系”是一个重要的限定条件。运动学从不涉及动力学——例如机械臂能承受多大的重量，电机能以多快的速度运转。它只处理纯粹的几何问题：“当关节角度为θ时，机械臂在哪里？”以及“要将机械臂置于此位置，关节角度应该是多少？”只有在几何基础建立之后，我们才能在此基础上讨论力、扭矩和轨迹跟踪等问题。

2. 为什么需要运动学？

机械臂的电机只能旋转（或伸缩）每个单独的关节。但人类想要向机器人发出的指令，几乎在所有情况下，都是以机械臂（末端执行器）的位置和姿态来表述的——“把这个零件放在这里”、“把这个杯子拿到那里”。关节角度（关节空间）和机械臂的位置和姿态（任务空间）乍一看似乎是完全不同的量：对于一个六轴机械臂，关节角度是一组六个数字，而机械臂的姿态则表示为一个三维位置（x、y、z）加上一个三维方向（旋转）——总共六个自由度。

如果没有一个连接这两个空间的转换规则，就无法计算“每个关节需要旋转多少才能使机械臂到达目标位置”，机械臂也就无法执行我们想要的运动。运动学的作用就是用显式方程来表达关节空间和任务空间之间的这种几何对应关系——这种对应关系是由机器人的结构（连杆长度、关节布局）决定的。

3. 输入是什么？

运动学处理的输入取决于我们求解的方向。

• 正向运动学 (FK) 的输入：关节向量 \boldsymbol{\theta} = (\theta_1, \theta_2, \ldots, \theta_n)，包含每个关节的角度（对于旋转关节）或滑动量（对于移动关节）。此外，机器人的结构信息（每个连杆的长度、每个关节轴的布局）作为一组预先设定的常量给出，用于构建运动学模型。

• 逆向运动学 (IK) 的输入：手部应达到的目标位姿 \mathbf{x}_d。这是一个目标位置 \mathbf{p}_d \in \mathbb{R}^3 和目标方向（以旋转矩阵或四元数表示）R_d 的组合；在许多情况下，尤其是在使用数值方法时，当前关节角度 \boldsymbol{\theta}_0 也作为初始值输入包含在内。机器人的结构信息是一个“模型”，并非每次计算都需要提供——但如果它不准确，正向运动学 (FK) 和反向运动学 (IK) 都会产生与实际机器不符的结果。在处理机器人手臂的运动学之前，必须首先正确校准该结构模型。

4. 我们求解的是什么？输出是什么？

正向运动学 (FK) 的输出 是在给定关节角度 \boldsymbol{\theta} 的情况下，手部姿态 \mathbf{x} = (\mathbf{p}, R)。这是唯一确定的——一旦每个关节角度确定，连杆长度和关节布局也随之确定，因此手部的最终位置由几何关系确定，只有一个答案。

反向运动学 (IK) 的输出 是实现目标姿态 \mathbf{x}_d 的关节角度 \boldsymbol{\theta}^{*}。这正是反向运动学 (IK) 与正向运动学 (FK) 的根本区别所在：反向运动学 (IK) 的解通常不唯一。可能存在多种弯曲肘关节的方式可以达到相同的手部位置（多解），或者目标位置可能位于机器人的运动范围之外（超出可达工作空间），在这种情况下，根本不存在解。这种非唯一性正是逆运动学（IK）在数学上比正运动学（FK）更难处理的原因。

此外，在许多情况下，我们想要的并非位置或姿态本身，而是速度之间的关系——“如果我将当前的关节角度移动这么多，手部会移动多少？” 这就是雅可比矩阵的作用，我们将在第 6 节中详细讨论。

5. 基本架构

运动学处理可以围绕三种变换——正运动学、逆运动学和雅可比矩阵——如何连接关节空间和任务空间的结构来组织。

图 1 — 正向运动学正向运动学 (FK) 可以根据关节角度唯一确定手部姿态。逆运动学 (IK) 求解相反方向的姿态，但可能存在多个解，也可能没有解。雅可比矩阵通过线性关系连接两个空间中的“速度”。

顶行表示 FK，中间行表示 IK，底行表示通过雅可比矩阵建立的速度关系。FK 总是可以唯一确定（从左到右），而 IK（从右到左）是一个几何逆问题，通常需要多种求解方法。由于雅可比矩阵连接的是“变化率”（速度），而不是位置或姿态本身，因此在线性代数框架内，它比 FK 或 IK 更容易处理——这也是为什么大多数数值 IK 方法都建立在它之上的原因。

6. 代表性算法

齐次变换矩阵和DH参数 — 构建正向运动学

正向运动学的基本操作是使用齐次变换矩阵连接相邻连杆的坐标系，该矩阵结合了旋转和平移。

R_i 表示从连杆i的坐标系到连杆i-1的坐标系的旋转，\mathbf{d}_i 表示平移。对于n关节臂，从基坐标系到手坐标系的变换是所有单个关节变换的乘积。

如果这些n齐次变换矩阵是为每个连杆单独定义后从头开始编写的，那么每次机器人发生变化时，整套方程组都必须重新构建。这就是为什么标准化的符号——Denavit-Hartenberg (DH) 参数——被广泛使用的原因。Jacques Denavit 和 Richard Hartenberg 在他们 1955 年发表于 ASME 应用力学杂志的论文“基于矩阵的低副机构运动学符号”中提出了这种符号。它仅使用四个参数——连杆长度a_i、连杆扭转角\alpha_i、关节偏移量d_i和关节角度\theta_i——来表达两个相邻关节轴之间的位置关系，从而可以将每个关节的变换矩阵组装成统一的形式。

根据此DH参数表——每个关节的三个常数a_i, \alpha_i, d_i，加上变量\theta_i——任何串联机器人的正向运动学方程都可以使用相同的步骤进行机械组装。这就是为什么工业机器人手册和许多仿真器都以这种DH参数形式提供机器人规格的原因。

解析逆运动学：以双连杆平面机械臂为例

让我们具体地看一下解析逆运动学的概念，它通过反向求解由DH参数构建的正向运动学方程来实现，并使用一个最简单的例子：一个连杆长度为l_1, l_2的双连杆平面机械臂。

Diagram 2 · Use the button to switch views

图2 — 双连杆平面机械臂。手E的位置(x, y)由肩关节O和肘关节J的角度θ₁、θ₂以及连杆长度l₁、l₂决定（正向运动学）。反之，从(x, y)求θ₁、θ₂即为逆向运动学。

正向运动学 可以简单地通过将两个连杆向量相加来表示。

逆向运动学 从目标位置求出\theta_1, \theta_2。首先，利用余弦定理求出肘关节角度 (x, y)。

当等式右侧落在 [-1, 1] 范围内时，方程组存在解。\theta_2 = \pm\arccos(\cdots) 给出两个解，仅符号不同（肘关节向上或向下弯曲）。一旦 \theta_2 确定，\theta_1 即可根据几何关系唯一确定（对于给定的 \theta_2）。此过程——“通过纯代数运算推导反三角函数”——属于解析逆运动学；对于具有 2-3 个自由度的简单机构，它可以写成闭合形式的表达式，但随着自由度的增加或关节布局的复杂化，找到闭合形式的表达式通常变得非常困难。

数值方法逆运动学 (IK) — 基于雅可比矩阵的迭代求解

对于具有多个自由度的机器人或没有闭式解的机构，数值逆运动学——一种从当前关节角度逐步逼近目标姿态的迭代计算方法——被广泛应用。该方法基于雅可比矩阵。

对关节角度求手部姿态 \mathbf{x}(\boldsymbol{\theta}) 的导数，即可得到关节速度与手部速度之间的线性关系。

J(\boldsymbol{\theta}) 是机械臂雅可比矩阵，它是一个 m \times n 矩阵（m 是任务空间的自由度，n 是关节数）。可以通过雅可比矩阵的逆矩阵（或伪逆矩阵）来确定更新关节角度以减小与目标姿态误差 \mathbf{e} = \mathbf{x}_d - \mathbf{x}(\boldsymbol{\theta}) 的方向。

这种利用雅可比伪逆进行迭代更新的思路可以追溯到 Daniel E. Whitney 于 1969 年发表的论文《机械臂和人体假肢的解析运动速率控制》。该论文提出了一种速度控制框架，其中包括解析冗余自由度。然而，当机器人接近奇异构型（详见第 7 节）时，J J^{\top} 几乎变为奇异（逆矩阵发散），导致更新不稳定。Charles Wampler 在 1986 年的论文中提出的阻尼最小二乘法 (DLS)通过在逆矩阵计算中添加阻尼项 \lambda^2 I 来抑制这种不稳定性。

这里存在一个权衡：\lambda 越大，在奇异点附近数值稳定性越高，但……收敛速度变慢（每次迭代误差只能减少较小的量）。由于该公式允许您使用单个参数 \lambda 来调整收敛速度和稳定性，因此它因其实用性而被广泛应用于工业机器人逆运动学求解器中。

CCD 和 FABRIK — 迭代几何启发式算法

还有一些方法通过重复更直观的几何运算来求解逆运动学，完全不使用雅可比矩阵。CCD（循环坐标下降法）逐个关节地进行运算，从离手最近的关节向根关节移动，重复“仅旋转该关节以使手尽可能靠近目标”的简单操作。它易于实现且计算成本低，但由于每次只移动一个关节，因此收敛速度慢，并且往往会产生看起来不自然的轨迹。

FABRIK（前后向到达逆运动学），Andreas 提出的一种方法Aristidou 和 Joan Lasenby 于 2011 年在《图形模型》（Graphical Models）期刊上发表的算法完全不处理旋转角度——它通过反复将每个连杆的关节位置重新定位为“指向目标的直线上的一个点，该点与目标点的距离保持连杆长度不变”，从而解决逆运动学（IK）问题。该算法从手部移动到根部，然后再从根部返回手部。由于避免了角度计算，因此计算量很小，并且往往只需几次迭代就能收敛到自然的姿态，这也是它被广泛用于 CG 动画和电子游戏角色手臂和腿部运动的原因。

7. 算法差异

解析逆运动学在可求解时“速度最快、精度最高”，但随着自由度的增加，推导方程本身就变得困难。数值方法（基于雅可比矩阵、CCD、FABRIK）可以通用，不受自由度或机构的限制，但由于是迭代方法，它们无法避免收敛速度、局部最小值和计算成本之间的权衡。

8. 难点/复杂环境

运动学固有的困难大致可以分为三类。

奇异点：在某些关节构型下，雅可比矩阵 J 的秩会下降，导致手部无论如何都无法朝某个方向移动。例如，在手臂完全伸展时，没有任何关节速度组合能够使手部进一步向外伸展。在这种状态下，J J^{\top} 接近奇异矩阵，使用伪逆的控制律会导致指令关节速度发散。由吉川恒夫于 1985 年提出的可操作性指标 w = \sqrt{\det(J J^{\top})} 被广泛用于量化接近奇异性的程度——w 越接近于零，构型就越接近奇异性。

多解性和可达性：逆运动学通常可以有多个解（例如，弯曲肘关节的不同方式），并且在某些情况下，由于机器人的连杆长度和运动范围的限制，目标姿态在物理上是无法达到的。除非数值方法明确地为这种“无解”情况设计了停止条件，否则它将不断迭代而永远不会收敛。

冗余度自由度：对于关节数量多于任务空间自由度（通常为 6 个）的机器人——例如 7 轴机械臂——存在无数种关节角度组合可以实现相同的手部姿态。这种多余的自由度并非“令人头疼的歧义”，而是一种可以积极用于次要目标**的资源，例如避障、避免奇异点或使关节保持在运动范围的中间位置附近。冗余机器人的逆运动学设计是通过将次要目标的梯度投影到雅可比矩阵的零空间（即对机械臂姿态没有影响的关节速度方向）来实现的。

9. 实际选择

如何实现运动学取决于机器人的自由度、应用以及实时性要求。

• 工业机器人手臂（焊接、装配等，通常为 6 轴）：对于 6 个自由度的机器人，许多机构都允许解析逆运动学解，并且在实践中，许多制造商的控制器直接在固件中实现解析逆运动学（IK）。无需迭代计算，这种方法速度快且行为可预测。

• 具有冗余自由度的机器人手臂（7轴以上、协作机器人或人形手臂）：解析求解要么困难，要么会产生多个难以处理的解，因此基于雅可比矩阵的数值IK（例如阻尼最小二乘法）较为常见。它还可以通过零空间更轻松地同时设计避障和奇异点规避算法。

• CG动画、视频游戏角色、VR/AR虚拟形象：视觉自然度和低计算成本通常优先于物理精度，因此倾向于使用轻量级几何启发式算法，例如CCD或FABRIK。

• 移动机械臂（移动基座+机械臂）或全身人形机器人：不仅需要以集成的方式处理机械臂自身的IK，还需要处理整个身体（包括腿部和躯干）的冗余自由度——通常需要采用数值方法。使用一个将基于雅可比矩阵的方法扩展到全身运动学（全身逆运动学）的框架。

在任何应用中，运动学都不是一个独立的技术——机器人手臂只有与实际生成关节速度和力矩指令的控制回路（LQR入门、MPC入门）以及轨迹生成入门结合使用，才能实现其预期的运动。轨迹生成入门设计了手臂沿时间轴的运动路径。

10. 总结（三行概括）

• 正向运动学可以根据关节角度唯一确定手臂的姿态；逆向运动学解决相反的问题，但可能存在多个解，也可能没有解。

• 雅可比矩阵将关节速度与手臂速度线性关联起来，它是基础。通过伪逆或阻尼最小二乘法进行数值逆运动学求解。

• 如何处理奇异性、多解和冗余自由度这三个难题，决定了解析逆运动学还是数值逆运动学更适合你。

参考资料

• MoveIt — 运动学配置

• Orocos 运动学和动力学库 (KDL)

• Lynch & Park，《现代机器人学：力学、规划和控制》（免费）预印本)](https://hades.mech.northwestern.edu/index.php/Modern_Robotics)