参考:
Inverse Kinematics Techniques in Computer Graphics: A Survey
逆运动学-知乎
逆运动学-知乎2
https://www.cnblogs.com/21207-iHome/p/9452896.html
https://zhuanlan.zhihu.com/p/326387013

通过这篇综述学习一下解逆运动学的主要方法.

综述中主要介绍了 4 个类别的方法, 分别是解析法 (analytical), 数值法 (numerical), 数据驱动方法 (data-driven) 和混合方法 (hybrid).

1. Intro

逆运动学简单来说就是已知末端执行器的位姿, 求解各个关节的变量 $q$ , 在 DH 参数的介绍中, 我们知道了末端执行器的位姿和各个关节变量这件的关系可以抽象成:

$$ r = f(\theta) $$

其中 $r$ 表示末端执行器的的变量 $r = [r_1, r_2, r_3, ..., r_m]$ 其中 $m$ 表示末端执行器的自由度, 例如只考虑空间中的位置, 那么 $m=3$ ,如果加上转动 $m=6$ .

$\theta$ 表示所有关节变量组成的向量. 逆运动学就是求解 $\theta$ 的过程.

衡量逆运动学方法主要从三个方面:

1. 运动的平滑度（smoothness of the produced motion）：IK 求解器应该能够生成平滑的运动轨迹，避免突兀或不自然的动作。
2. 可扩展性（scalability）：IK 求解器应该能够处理规模较大的问题，例如更复杂的机械臂或动画角色。
3. 计算成本（computational cost）：IK 计算所需的时间和资源应尽可能少，以提高实时性和效率。

可扩展性意味着更高的计算成本：当 IK 需要处理更复杂的约束条件或更长的运动学链（如具有大量关节和自由度的系统）时，计算量会增加。

相比于正运动学, IK 可能存在多解, 单一解或者无解的情况.
解的数量取决于目标的位置和/或运动链的自由度（DoFs）。

• 没有解的情况（Over-constrained）如果目标位置超出了运动链的可到达范围，则该目标是无法实现的。或者，当存在多个目标且这些目标间存在矛盾时，也会出现无法解的问题。这类问题被称为过约束问题（over-constrained problems）
• 有多解的情况（Under-constrained 或 Redundant）当目标是可到达的时，可能存在多个解。例如：一个有两个或更多关节的链条可以用多种不同的姿态实现同一个目标。这种情况下，IK 问题被认为是欠约束的（under-constrained）或冗余的（redundant），可能存在无限多种解满足目标位置。

在实际问题中, 需要根据运动链 (Kinematic Chain) 的构型, 解法的平滑度, 求解的计算成本, 给定应用的具体需求等综合考虑逆运动学的解法。

1.1 可达空间

目标无法达到（unreachable target）：当目标在运动链的能力范围之外时，IK 问题的解不存在。这种情况称为“目标不可达”。

Reachable Workspace（可达工作空间）：指运动链末端（例如机械臂的末端执行器）可以到达的目标空间范围。

Dexterous Space（灵巧空间）：这是机器人运动学中的一个扩展概念。它指的是机器人可以在某一点生成的所有速度矢量所覆盖的空间（即切空间的全部范围）。这意味着机器人不仅可以到达某点，还可以在该点灵活地调整方向和速度。

在求解 IK 问题之前，检查目标是否在可达空间内非常重要。如果目标不可达，那么继续尝试寻找解是浪费计算资源的。因此，预先判断目标的可达性可以显著减少处理时间，提升系统效率。

在不考虑关节约束和目标方向的情况下，可以应用简单的可达性检查:
先计算目标点与基座的距离 $d$ 。目标点的距离大于所有关节连杆长度的总和（即整个运动链的最大伸展长度），此时目标超出最大范围，无法到达; 目标点的距离小于一个特定下限值（即某些关节重叠或卷曲时的最小收缩长度），此时目标位于一个过小的范围内，也无法到达.

2. 解析解（Analytic Solutions）

先简单概述一下优缺点：
优点：速度快（us级），精度高，能得到所有解；
缺点：通用性差，适用性弱（仅针对特定结构可解，例如：满足Pieper准则）

Pieper 准则论文
[1]. Pieper, Donald Lee. The kinematics of manipulators under computer control. Stanford University, 1969.

计算机器人运动学逆解首先要考虑可解性(solvability)，即考虑无解、多解等情况。在机器人工作空间外的目标点显然是无解的。

串联机械臂有逆运动学解析解的充分条件是满足Pieper准则。即如果机器人满足两个充分条件中的一个，就会得到封闭解，这两个条件是：

• 三个相邻关节轴相交于一点；
• 三个相邻关节轴相互平行。

现在的大多数商品化的工业机器人在设计构型时，都会尽可能满足满足Pieper准则，因为解析法求解能够很快的使用较少的算力，使用较低成本的控制器就能求解，之后随着芯片算力的提升，感觉在未来，机器人公司也会在是否采用满足解析解的构型和采用特定构型并开发对应的逆解算法之间找一个平衡。

3. 数值法

以下是来自知乎的一段: 看不太懂..

特点：通用性高，但是求解速度较慢（ms级）。
除了一些特殊的机械臂构型外，机械臂逆运动学问题很难用解析解求解，因此在许多情况下会使用数值解求解。
Newton-Raphson（NR）是数值解的一种方法。它需要基本的雅可比矩阵。然而，当且仅当原始方程的函数具有逆函数，且原始方程可解时，NR方法才会成功。从运动学的角度来看，前一个条件意味着机器人需要非冗余，机器人在从初始配置到最终配置的运动过程中不通过奇异点。后一个条件意味着机械臂的期望位置和方向需要在机器人的工作空间内，是可解的。由于这些限制，NR方法不能保证全局收敛性，因此它在很大程度上取决于初始值。

奇异性问题与基本雅可比矩阵的性质密切相关，这主要在微分逆运动学（differential inverse kinematics）相关领域 中进行了讨论，Whitney提出了使用 Moore–Penrose 的广义逆矩阵来解决这个问题。无论原始雅可比矩阵的秩如何，都通过构型的最小偏差使约束方程的残差最小化。Nakamura and Hanafusa指出，Whitney的方法不能解决构型在奇异点附近抖动的问题，并提出了引入阻尼因子的奇异鲁棒逆矩阵（singularity-robust inverse matrix）。Wampler 也提出了一种类似的方法，并提到它涉及到 Levenberg–Marquardt（LM）方法的框架。

可解性的问题和奇异点的问题一样需要考虑。大多数情况下，很难提前知道方程是否是可解的。一个合理的思想是用残差极小化代替逆运动学中的根查找问题。基于这一想法，有使用了最陡下降（ steepest descent，SD) 和变量度量（Variable metric，VM）的方法。但前者的收敛速度较慢，而后者的可靠性较低，因此经常处于局部极小值。合理快速的解决方案是一类在每一步迭代中利用 DIK 的梯度方法。LM 方法在其中具有较高的计算稳定性。虽然 LM 方法的收敛性能取决于阻尼因子的选择，但这个问题到目前为止还没有得到充分的讨论。Tomomichi Sugihara 提出了一种选择 LM 方法的阻尼因子的方法，该方法对奇异性、可解性和快速收敛问题具有鲁棒性。通过一种相当简单的利用残差平方范数作为阻尼因子的方法来实现鲁棒性和收敛性，并通过对阻尼因子略有偏置来解决在奇异点附近，计算不稳定的问题。

数值方法涵盖了需要一组迭代才能获得满意解决方案的方法。迭代方法使用要最小化的成本函数来制定问题。数值方法族一般可分为三类：雅可比法(Jacobian)、牛顿法(Newton)和启发式(Heuristic)方法。

3.1 Jacobian inverse methods

Jacobian $J$ 是一个偏导数矩阵，它描述了整个链条系统（机器人关节或机械臂）中关节参数（例如关节角度 $\theta$ ）与末端执行器位置和方向 之间的关系。

简单来说， $J$ 用于表示关节的微小变化如何影响末端执行器的变化。
Jacobian 提供了一种线性近似的方法，逐步调整关节的角度，使末端执行器的实际位置和方向逐渐接近目标位置和方向。
线性近似： Jacobian 方法不直接给出最终解，而是通过一系列迭代来逼近目标位置。在每一步中，根据 Jacobian 的线性近似，计算需要调整的关节角度变化。
公式:

$$ \dot{s}=J(θ) \dot{θ} $$

这个公式表示 前向动力学方程（Forward Dynamics Equation）：

• $\dot{s}$ 表示末端执行器的速度（位置和方向的时间导数）。
• $J(\theta)$ 雅可比矩阵，它依赖于当前的关节角度 θ。
• $\dot{\theta}$ 关节角度的变化速率（时间导数）。

迭代过程:

• 初始状态： 从机械臂的初始关节角度配置开始，计算其末端执行器位置和方向。
• 误差计算： 比较末端执行器的当前位置与目标位置，计算误差。
• 关节调整： 根据 Jacobian 计算出需要调整的关节角度增量，使误差逐步减少。
• 重复迭代： 不断调整关节角度，直到误差足够小，末端执行器到达目标位置和方向。

k：末端执行器（end effectors）的数量。
n：机械臂的关节（joints）的数量。
因此，Jacobian 矩阵 $J$ 的理论维度是 k×n .
在实践中，每个末端执行器的位置和方向通常用 3 个标量（x, y, z）表示。
为了便于计算，JJJ 被展开为 $3k * n$ 的标量矩阵, 如果再考虑上姿态, 那么就会扩展成 $6k * n$

Jacobian 矩阵 $J$ 的元素 $J_{ij}$ ​ 表示末端执行器位置 $s_i$ 对关节角度 $\theta_j$ ​ 的偏导数：

$$ J(\theta)_{ij} = \frac{\partial s_i}{\partial \theta_j}. $$

对于旋转关节，使用公式：

$$ \frac{\partial s_i}{\partial \theta_j} = \mathbf{v}_j \times (\mathbf{s}_i - \mathbf{p}_j) $$

其中:

• $v_j$ 是关节 $j$ 的旋转轴方向单位向量。
• $s_i$ ​末端执行器 i 的位置。
• $p_j$ 关节 $j$ 的位置

误差计算:
给定目标末端执行器位置 $t_i$ ​ 和当前末端执行器位置 $s_i$ ​，误差定义为：

$$ e_i = t_i - s_i(\theta) $$

过对关节角度的微小变化 $\Delta \theta$ ，末端执行器的位置变化 $\Delta s$ 可以近似为：

$$ \Delta s \approx J(\theta) \Delta \theta $$

为了使位置误差最小化，计算关节角度变化：

$$ \Delta \theta \approx J^{-1} e $$

其中 $e$ 是目标位置和当前实际位置的误差向量。通过计算 Δθ，调整关节角度 θ，使末端执行器的位置和姿态更接近目标。

Jacobian 的潜在问题:
矩阵非方针, 不可逆->

Jacobian 的一阶近似:

• 独立性假设：
  • Jacobian 只考虑了每个关节的独立影响，忽略了关节之间的复杂依赖关系（例如，一个关节的变化可能会影响到其“子关节”）。
  • 这种假设使得 Jacobian 是一种一阶近似（first-order approximation），在大范围运动中可能不够准确。
• 刚体模型：
  • 在一阶近似中，所有子关节的连杆被简化为一个单一的刚体，这可能无法完全反映机械臂的复杂运动行为。

3.1.1 Jacobian transpose

未完