摘要

本文系统梳理了机械臂逆运动学（Inverse Kinematics, IK）的数值求解过程，从正运动学函数的非线性映射出发，分析了解析解法在通用机械臂中面临的几何复杂性与多解局限。针对这一问题，文章引入数值法（Numerical IK）的核心思想——利用雅可比矩阵（Jacobian）对非线性方程进行线性化近似，并结合一阶泰勒展开建立末端位姿变化与关节变量变化之间的局部映射关系。

在此基础上，文中详细推导了数值法的关键步骤：包括雅可比矩阵的定义与物理含义、李代数形式的姿态误差表示、以及从线性近似到最小二乘与阻尼最小二乘（Damped Least Squares, DLS）算法的迭代求解过程。通过引入李代数，将姿态误差从旋转矩阵空间 $SO(3)$ 映射至向量空间 ${\mathbb{R}}^3$，实现位置与姿态误差的统一建模，使得数值法能够在任意结构机械臂上进行稳定的迭代更新。

文章最后对比了解析法与数值法的适用范围与优缺点：解析法求精确、数值法求通用。前者适用于几何结构规则的标准机械臂，而后者凭借线性化与阻尼机制，能在冗余机构、VR 遥操作、仿真控制及复杂优化任务中实现高鲁棒性和良好收敛性。现代机器人系统（如 ROS2 MoveIt、Isaac Sim、PyBullet）普遍采用基于 DLS 的数值 IK 算法作为通用解算框架

一、问题引出与背景分析

机械臂的逆运动学（Inverse Kinematics, IK） 问题，是整个机器人运动控制中的核心之一。
它的目标是：

已知末端执行器的目标位姿（位置 + 姿态），求出各关节角度，使末端能够到达并保持该目标位姿。

---

1.1 逆运动学与正运动学的关系概述

要想求“逆”，必须先了解“正”。正运动学（Forward Kinematics, FK） 用于描述从关节角度到末端位置的映射关系：
$$\text{关节角度 }\mathbf{q}⟶\text{末端位姿 }\mathbf{x}.$$
而逆运动学则是它的反问题：
$$\text{末端位姿 }{\mathbf{x}}_d⟶\text{关节角度 }\mathbf{q}.$$
综上，正运动学告诉我们“给定关节角，末端在哪里”；逆运动学则要回答“若末端要去某处，关节该怎么动”。

---

1.2 正运动学函数的定义

设机械臂有 $n$ 个关节，每个关节的变量记为：
$$\mathbf{q}=[{\theta }_1,{\theta }_2,\dots ,{\theta }_n{]}^T,$$
其中：

• ${\theta }_i$：第 $i$ 个关节的角度（若为旋转关节）或位移（若为移动关节），单位为 rad 或 m。

则末端执行器的位姿由正运动学函数给出：
$$\boxed{\mathbf{x}=f(\mathbf{q})=\left[\begin{array}{c}\mathbf{p}(\mathbf{q})\\ \mathbf{R}(\mathbf{q})\end{array}\right]}$$
其中：

• $\mathbf{x}$：末端执行器的位姿（位置与姿态的组合）；
• $\mathbf{p}(\mathbf{q})=[x(\mathbf{q}),y(\mathbf{q}),z(\mathbf{q}){]}^T$：末端位置向量；
• $\mathbf{R}(\mathbf{q})\in SO(3)$：末端姿态的旋转矩阵。

也就是通过位移与旋转矩阵，可从坐标系基点求取末端坐标。

该函数 $f(\mathbf{q})$ 是从关节空间（Joint Space） 到 任务空间（Task Space） 的非线性映射：
$$f:{\mathbb{R}}^n\to SE(3).$$

---

1.3 为什么逆运动学困难：非线性与耦合性分析

为了直观理解逆运动学为何难以直接求解，我们先从正运动学的结构特征来看。

机械臂的位姿由多级旋转和平移叠加形成。以一个典型的六自由度机械臂为例，其末端位置可表示为：
$$\{\begin{array}{l}x=a_2\cos {\theta }_1\cos {\theta }_2+a_3\cos {\theta }_1(\cos {\theta }_2\cos {\theta }_3-\sin {\theta }_2\sin {\theta }_3)+d_6[\text{高阶项}],\\ y=a_2\sin {\theta }_1\cos {\theta }_2+a_3\sin {\theta }_1(\cos {\theta }_2\cos {\theta }_3-\sin {\theta }_2\sin {\theta }_3)+d_6[\text{高阶项}],\\ z=d_1+a_2\sin {\theta }_2+a_3(\sin {\theta }_2\cos {\theta }_3+\cos {\theta }_2\sin {\theta }_3)+d_6[\text{高阶项}].\end{array}$$

其中：

• $a_1,a_2,a_3$：连杆长度；
• $d_1,d_6$：连杆偏距（沿 $z$ 轴的平移量）；
• ${\theta }_i$：第 $i$ 个关节角度。

这些方程的共同特点是：

• 各坐标分量由多组 $\sin {\theta }_i$、$\cos {\theta }_i$ 交叉构成；
• 不同关节角度彼此耦合；
• 方程为高阶非线性形式；
• 同一末端位姿可能对应多组解（多解问题）。

---

1.4 从解析法到数值法的必要性

对于结构简单、几何关系明确的机械臂（如 6R 球形腕、SCARA 机械臂），可通过代数或几何手段推导出解析解，即每个关节角的显式表达式。

但对于更通用或冗余结构（如 7R 机械臂、双臂系统、蛇形臂等），上述非线性方程往往无法直接求出解析解，甚至：

• 可能无解（到达不了目标位姿）；
• 可能多解（同一姿态有多种姿势）；
• 可能在某些姿态处出现奇异性（分母趋近于零，计算不稳定）。

此时，解析法将变得复杂且不具通用性。于是引入了数值法（Numerical IK） —— 一种基于雅可比矩阵线性化 + 迭代逼近的通用解法。

---

二、数值法的基本思想与数学基础

数值法的核心思路是：将非线性的逆运动学问题转化为一系列线性近似问题，通过迭代不断修正关节角度，使末端位姿逐步逼近目标。
这种方法不追求一次求出解析解，而是依靠“线性化 + 最小二乘求解 + 迭代更新”实现稳定收敛。

其基本流程可以概括为以下三个步骤：

1. 线性化建模：
   在当前关节状态下，对正运动学函数进行一阶泰勒展开，用雅可比矩阵描述“关节变化量”与“末端位姿变化量”之间的局部线性关系。
2. 误差求解：
   将期望末端位姿与当前末端位姿的差异表示为误差向量，通过最小二乘或阻尼最小二乘方法，求得能最小化该误差的关节增量。
3. 迭代逼近：
   按照求得的关节增量更新当前状态，重复计算，直到末端误差满足收敛条件为止。

通过以上过程，数值法实现了对任意结构机械臂的统一求解框架。
接下来将依次介绍该方法所依托的数学基础——从一阶泰勒展开到雅可比矩阵，再到最小二乘形式的误差求解。

2.1 一阶泰勒展开

为了在线性范围内近似描述末端位姿与关节变量之间的关系，数值法通常对正运动学函数 $f(\mathbf{q})$ 进行一阶泰勒展开。即对于任意可微的多元向量函数 $f:{\mathbb{R}}^n\to {\mathbb{R}}^m$，
其泰勒展开式为：
$$f(\mathbf{q}+\Delta \mathbf{q})=f(\mathbf{q})+\frac{\partial f(\mathbf{q})}{\partial \mathbf{q}}\Delta \mathbf{q}+\frac{1}{2!}\frac{{\partial }^{2}f(\mathbf{q})}{\partial {\mathbf{q}}^2}(\Delta \mathbf{q}{)}^2+\cdots$$
若忽略二阶及以上小量，就得到一阶近似：
$$\boxed{f(\mathbf{q}+\Delta \mathbf{q})\approx f(\mathbf{q})+J(\mathbf{q})\Delta \mathbf{q}}$$
其中：

• $f(\mathbf{q})$：正运动学函数；
• $J(\mathbf{q})=\frac{\partial f(\mathbf{q})}{\partial \mathbf{q}}$：雅可比矩阵；
• $\Delta \mathbf{q}$：关节变量的小变化量。

将两边相减可得：
$$\boxed{\Delta \mathbf{x}\approx J(\mathbf{q})\Delta \mathbf{q}}$$
其中：

• $\Delta \mathbf{x}=f(\mathbf{q}+\Delta \mathbf{q})-f(\mathbf{q})$：末端位姿变化；
• $J(\mathbf{q})$：在当前姿态下描述关节变化对末端影响的线性映射矩阵。

---

线性化的局限性

需要注意的是，上式仅在 小范围关节变化 下成立。
由于高阶项被省略，这种线性化在以下情况下会逐渐失效：

1. 关节变化过大 —— 二阶及更高阶项不可忽略；
2. 非线性强区域 —— 雅可比矩阵变化剧烈，线性假设不再精确；
3. 奇异构型附近 —— 某些方向的运动灵敏度接近零，导致线性关系失真。

因此，数值法的求解过程往往需要 小步长迭代 或 阻尼因子 来保证近似有效与算法稳定。

---

2.2 雅可比矩阵的定义与含义

雅可比矩阵定义为：
$$\boxed{J(\mathbf{q})=\frac{\partial \mathbf{x}}{\partial \mathbf{q}}=\left[\begin{array}{cccc}\frac{\partial x}{\partial {\theta }_1}& \frac{\partial x}{\partial {\theta }_2}& \cdots & \frac{\partial x}{\partial {\theta }_n}\\ \frac{\partial y}{\partial {\theta }_1}& \frac{\partial y}{\partial {\theta }_2}& \cdots & \frac{\partial y}{\partial {\theta }_n}\\ \frac{\partial z}{\partial {\theta }_1}& \frac{\partial z}{\partial {\theta }_2}& \cdots & \frac{\partial z}{\partial {\theta }_n}\end{array}\right]}$$
它表示在给定关节角状态下，每个关节的小幅转动对末端位置分量变化的敏感度。

若考虑线速度与角速度，则有：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]=J(\mathbf{q})\dot{\mathbf{q}},$$
其中：

• $\mathbf{v}$：末端线速度 $[\text{m/s}]$；
• $\mathbf{\omega }$：末端角速度 $[\text{rad/s}]$；
• $\dot{\mathbf{q}}$：关节角速度向量。

---

2.3 二自由度平面臂的例子

考虑二自由度平面机械臂（连杆长度 $L_1,L_2$，关节角 ${\theta }_1,{\theta }_2$）：
$$\{\begin{array}{l}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{array}$$
则雅可比矩阵为：
$$J({\theta }_1,{\theta }_2)=\left[\begin{array}{cc}-L_1\sin {\theta }_1-L_2\sin ({\theta }_1+{\theta }_2)& -L_2\sin ({\theta }_1+{\theta }_2)\\ L_1\cos {\theta }_1+L_2\cos ({\theta }_1+{\theta }_2)& L_2\cos ({\theta }_1+{\theta }_2)\end{array}\right].$$
含义：

• 第一列表示关节 1 对末端运动的贡献；
• 第二列表示关节 2 的贡献；
• 行分别对应 $x$、$y$ 方向。

---

2.4 姿态误差的定义（李代数形式）

在前面的线性化过程中，我们用雅可比矩阵描述了关节变量的微小变化与末端位置变化之间的关系。然而，末端位姿不仅包含位置误差，还包含姿态误差。若仅考虑位置，将导致旋转误差无法参与迭代优化。因此，需要一种方式把旋转误差也转化为可线性处理的向量形式，以便与位置误差统一求解。
为此，数值法通常借助 李代数（Lie Algebra） 表示，将旋转矩阵之间的差异映射为三维向量——这就是姿态误差的李代数形式定义。

设：

• 当前姿态矩阵为 $R$，
• 目标姿态矩阵为 $R_d$。

它们之间的相对旋转可表示为：
$$R_{\text{err}}=R_d{R}^T$$
该矩阵反映了从当前姿态旋转到目标姿态所需的变化。
接着通过对数映射（Rodrigues公式），将矩阵形式的旋转转化为三维向量形式：
$$\mathbf{\varphi }=\text{vee}(\log (R_{\text{err}}))$$
其中 $\mathbf{\varphi }$ 即为姿态误差向量，其方向表示旋转轴，模长表示旋转角。

这样一来，末端误差可统一表示为：
$$\mathbf{e}=\left[\begin{array}{c}{\mathbf{p}}_d-\mathbf{p}\\ \mathbf{\varphi }\end{array}\right]$$
即同时包含位置误差与姿态误差，为后续最小二乘求解提供统一输入。

公式来说可能有些抽象，接着举个实际例子 来进行说明：

---

示例：末端从 X 轴方向旋转到 Y 轴方向

假设：

• 当前末端姿态矩阵（朝向 X 轴）为
  $$R_k=I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

• 目标末端姿态矩阵（朝向 Y 轴）为绕 Z 轴旋转 $90°$：
  $$R_d=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$
  也就是 $R_d=R_z(90°)$。

---

① 计算相对旋转矩阵

$$R_e=R_k^T{R}_d=I^T{R}_d=R_d$$

所以：
$$R_e=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$
这表示一个绕 Z 轴 $+90°$ 的旋转。

---

② 对其取对数（得到旋转误差）

我们知道：
$$\log (R_e)=\hat{\mathbf{\varphi }},$$
其中 $\mathbf{\varphi }$ 为旋转轴角向量。根据旋转矩阵的对数定义：
$$\hat{\mathbf{\varphi }}=\frac{\theta }{2\sin \theta }(R_e-R_e^T),$$
其中 $\theta$ 为旋转角，满足：
$$\cos \theta =\frac{\text{trace}(R_e)-1}{2}.$$

---

③ 代入计算

$$\text{trace}(R_e)=0+0+1=1\Rightarrow \cos \theta =\frac{1-1}{2}=0\Rightarrow \theta =90^{\circ }=\frac{\pi }{2}.$$

---

接着：

$$R_e-R_e^T=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]-\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]=\left[\begin{array}{ccc}0& -2& 0\\ 2& 0& 0\\ 0& 0& 0\end{array}\right]$$

---

于是：

$$\hat{\mathbf{\varphi }}=\frac{\theta }{2\sin \theta }(R_e-R_e^T)=\frac{\pi /2}{2(1)}\left(\begin{array}{ccc}0& -2& 0\\ 2& 0& 0\\ 0& 0& 0\end{array}\right)=\frac{\pi }{2}\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right).$$

---

④ 将反对称矩阵转回向量（vee 运算）

反对称矩阵对应向量规则：
$$\hat{\mathbf{\varphi }}=\left[\begin{array}{ccc}0& -{\varphi }_z& {\varphi }_y\\ {\varphi }_z& 0& -{\varphi }_x\\ -{\varphi }_y& {\varphi }_x& 0\end{array}\right]\Rightarrow \mathbf{\varphi }=\left[\begin{array}{c}{\varphi }_x\\ {\varphi }_y\\ {\varphi }_z\end{array}\right].$$
对应上式可得：
$$\boxed{\mathbf{\varphi }=\left[\begin{array}{c}0\\ 0\\ \pi /2\end{array}\right]}$$

---

⑤ 几何含义

这个结果表示：

要让机械臂末端从当前姿态 $R_k$ 转到目标姿态 $R_d$，
需要 绕 Z 轴旋转 $90°$。

也就是说经过整体处理后，会将矩阵形式的旋转变为角度形式的旋转，在矩阵中全部变为在各个方向直接运算的方式，以便在后续进行处理。

2.5 从线性近似走向“最小二乘”求解

有了线性关系
$$\Delta \mathbf{x}\approx J({\mathbf{q}}_k)\Delta \mathbf{q},$$
此处我们的目标为求取。在一般机械臂中，关节数 $n$ 与任务维度 $m$ 往往不相等（如冗余 $n>m$ 或欠驱动 $n<m$）,或 $J$ 在某些姿态病态/近奇异，直接求解 $\Delta \mathbf{q}$ 不再稳健。
因此把它转化为最小二乘问题：在一阶近似下，找到使残差最小的关节增量
$$\boxed{\underset{\Delta \mathbf{q}}{\min }\Vert J({\mathbf{q}}_k)\Delta \mathbf{q}-\Delta \mathbf{x}{\Vert }_2^2}$$
这将导出标准的正规方程与伪逆解；同时，为抑制奇异性，还可在目标中加入二范数正则得到阻尼最小二乘（DLS）。

---

三、最小二乘与阻尼最小二乘法

3.1 最小二乘解

当方程 $\Delta \mathbf{x}=J\Delta \mathbf{q}$ 无唯一解时，取最小二乘意义下的近似解：
$$\underset{\Delta \mathbf{q}}{\min }\Vert J\Delta \mathbf{q}-\Delta \mathbf{x}{\Vert }^2.$$
求导得：
$$J^{T}J\Delta \mathbf{q}=J^T\Delta \mathbf{x}.$$
若 $J^{T}J$ 可逆，则解为：
$$\boxed{\Delta \mathbf{q}=(J^{T}J{)}^{-1}{J}^T\Delta \mathbf{x}.}$$
其中：

• $\Delta \mathbf{q}$：关节角修正量；
• $\Delta \mathbf{x}$：末端位姿误差；
• $J$：雅可比矩阵。

---

3.2 阻尼最小二乘（Damped Least Squares, DLS）

当 $J^{T}J$ 奇异或病态时，引入阻尼项 ${\lambda }^{2}I$（正则化系数）：
$$\boxed{\Delta \mathbf{q}=(J^{T}J+{\lambda }^{2}I{)}^{-1}{J}^T\Delta \mathbf{x}.}$$
对应优化目标：
$$\underset{\Delta \mathbf{q}}{\min }\left(\Vert J\Delta \mathbf{q}-\Delta \mathbf{x}{\Vert }^2+{\lambda }^2\Vert \Delta \mathbf{q}{\Vert }^2\right),$$
其中：

• $\lambda >0$：阻尼系数，防止在奇异点处求逆不稳定；
• $I$：单位矩阵，确保矩阵正定；
• 第一项控制误差最小化，第二项约束关节变化幅度。

---

3.3 迭代更新公式

更新公式为：
$$\boxed{{\mathbf{q}}_{k+1}={\mathbf{q}}_k+\alpha \Delta \mathbf{q}},$$
其中：

• ${\mathbf{q}}_k$：当前关节角向量；
• ${\mathbf{q}}_{k+1}$：更新后的关节角；
• $\alpha \in (0,1]$：步长（学习率）。

当误差满足：
$$\Vert \Delta \mathbf{x}\Vert <\epsilon ,$$
算法终止，表示收敛。

---

四、数值法的优缺点与适用范围

项目	数值法（伪逆/DLS）	解析法（几何法）
原理	雅可比线性化 + 迭代逼近	几何约束 + 封闭形式
通用性	任意机构均可用	仅限特定结构（如 6R 球形腕）
稳定性	DLS 抗奇异性强	奇异点直接失效
实时性	可迭代更新	快，但推导复杂
实现难度	较低（只需雅可比）	高（需代数推导）
收敛性	依赖初值、步长	精确但固定
适用范围	冗余机械臂、复杂机构、VR遥操、仿真优化	工业标准机械臂（UR、PUMA）

---

五、总结

数值法 IK 的核心思想是：

用雅可比矩阵线性化非线性运动学，再通过最小二乘或阻尼最小二乘求解关节修正量，循环迭代直到误差收敛。

它的优势在于：

• 对任意结构都适用；
• 在奇异点附近依然稳定；
• 可与优化目标（能量、避障、约束）结合。

不足之处是：

• 收敛速度依赖初始角度；
• 阻尼与步长需调节；
• 无法一次性得出封闭解。

现代机器人系统（如 ROS2 MoveIt、PyBullet、Isaac Sim、Gazebo）几乎全部采用基于 DLS 数值法 的逆运动学模块。

一句话总结：
解析法求精确、数值法求通用。
数值法让“无解析解”的机械臂，也能稳定、连续地运动。