之前是接触IKFast了解到了这篇文章，据说IKFast在７自由度以下的机器人都能生成其解析解，感觉很神奇，所以来阅读一下。
其中有关于IKFast为第4.1章，不禁高山仰止。
其中比较简单的地方就不翻译了。
摘要
随着机器人开始在事实上执行大多数任务，开始在家庭环境和办公室环境中提供帮助，社会正变得越来越自动化。机器人最重要的功能之一是能够操纵环境中的对象。**由于可能的机器人设计、传感器模式和目标任务的空间很大，研究人员最终不得不手动创建许多模型、数据库和程序来完成其特定任务，而每当任务发生更改时，这些工作都会重复。给定机器人和任务的规范，提出的框架会自动构建机器人可靠地执行操作任务所需的数据库和亲公词。**它包括对操作任务至关重要的三个主要组件的贡献
第一种是基于几何的规划系统，它分析所有必要的操作规划模式，并提供有效的算法来制定和解决它们。这允许识别任务和机器人特定设备所需的必要信息。使用这组，我们建立了一个规划知识库，允许对场景结构和机器人目标进行翔实的参数推理。我们展示如何有效地生成和查询规划人员的信息
第二种是一组高效的算法，在选择操作目标时考虑摄像机中对象的可见性。我们使用抓握器摄像机展示多个机器人平台的结果，以提高检测到的 obiect 的精度并可靠地完成任务。此外，我们使用介绍的规划和可见性基础设施开发一种完全自动化的外在摄像机校准方法和一种检测全自动校准数据的方法。
第三种是一个以视觉为中心的数据库，它可以分析刚性对象的表面，以便用于姿势提取程序的稳定和可区分的特征。此外，我们展示一种新的基于投票的对象姿势提取算法，该算法不依赖于 2D/3D 特征对应关系，从而减少了困扰传统基于视觉的姿势提取算法普遍性的早期承诺问题。
为了通过扎实的实施基础加强我们理论的贡献，我们讨论了开源规划环境 OpenRAVE。开始和演变作为本文的工作的结果。我们分析其体系结构，并为成功的机器人软件环境提供见解。
Manipulation Planning Knowledge-base
我们从图 4.1 中所有组件的动机开始。快速规划需要每个手臂的分析反向运动学方程，并需要以下许多几何分析。在第 4.1 节中，我们引入了一种名为 ikfast 的算法方法，用于查找最健壮的闭式解决方案。反向运动学允许精确计算用于许多规划启发式工作的手臂可伸缩空间（第 4.3 节）。将可到达性反转并投射到 2D 平面移动使我们能够开始计算基本分布（第 4.4 节）。从目标对象的角度来看，我们生成所有根据任务规范处理对象的抓握。在第 4.2 节中，我们涵盖了计算抓握的两种算法：力闭合抓握和截合抓。我们展示这些把握对广泛任务的有用性。对于根据对象规范训练的每个姿势识别程序，我们收集所有摄像机位置的地图，这些摄像机位置可以有力地检测对象（第 4.7 节）。这与抓握的预阴影相结合，使我们能够计算机器人的传感器可见性图。通过结合抓握集和反向可到达性，我们可以根据目标对象的位置开始推理机器人基座如何分布;我们称此地图为可到达性，并在第 4.5 节中介绍其生成和使用。我们还讨论了对机器人碰撞几何使用凸分解的优点（第 4.6 节）。凸分解为我们提供了更简单的几何形状，使我们能够垫上机器人，并近似其每个链接的体积。一旦我们对链接的体积进行估计，我们就可以计算每个关节的扫描体积，并从那里计算更好的配置空间距离指标（第 4.6.3 节）
4.1 Inverse Kinematics
通过运动学来表示机器人传感器的工作空间运动与抓握器与机器人配置之间的关系。对于规划算法和其他工作空间分析，从机器人所需的参考帧到关节角度的反面问题同样重要。在这篇论文中，我们找出了反运动学的几个问题，并为它们提供了自动化分析和解决方案。最受欢迎的逆运动学问题是计算将链接移动到指定平移和方向的所有关节角度。分析解决一般问题的纯数学尝试无法处理奇点，因此生成的程序也不总是最优的[Low and Dubey（1987）]。因此，已经开发出许多类型的数值方法，包括使用δT进行数值梯度下降。实际上，大多数关于广义逆运动学解决方案的文献都围绕数值方法，而忽略了解析问题，因为它太难解决了。尽管已经提出了对该问题的一般解析解[Manocha and Zhu（1994）]，但是由于诸如本征分解之类的沉重的数学机制，这些方法在数值上可能变得不稳定。在本节中，我们着眼于使用基于算法搜索的方法自动生成最小的，数值稳定的解析逆运动学求解器。我们注意到，很难用算法方法证明解决方案的存在。 但是，我们将证明，通过牺牲一点点通用性，本文提出的分析可以为当今可用的大多数常见机械臂提供更加稳定的封闭形式解决方案（所以说论文中没有经过验证的机械臂的稳定性应该如何验证和保证？） 封闭式解决方案对于运动计划至关重要，其原因有二：
- 逆运动学解算器总是比封闭形式解慢得多。但是，由于计划人员要求每秒能够处理数千个配置，因此拥有快速的IK解算器至关重要。通过我们提出的方法生成的闭式代码可以产生大约6微秒的解。作为比较，大多数数值解的数量级为10毫秒，并且必须担心收敛。
- 由于可以计算所有解，因此可以探索解集的空空间。这为计划算法提供了更多自由空间来移动机器人。
在本文中，我们开发了ikfast算法来执行此生成过程。 ikfast作为程序，会生成一个直接编译为求解器的C ++文件。尽管逆运动学在闭合链中会变得任意复杂，但我们仅考虑采用铰链和棱柱形一自由度关节的运动学链。
我们确定了对机器人技术重要的几个逆运动学参数化：
- 转换IK（6DOF平移+旋转）-逆向运动学的最常见形式，用于将所需的工作空间移动到一组配置。此IK对于快速抓取计划至关重要，其中每个抓取都指定一个末端执行器转换。此外，此IK可用于将传感器定向到所需位置以确保可见性。
- 平移IK（3DOF）-用于获得3D位置而无需担心方向。机器人技术中的许多问题（例如按钮和其他对象）仅涉及点接触。
- 旋转IK（3DOF）-用于实现特定方向而无需考虑平移组件。在计算机视觉中存在可见性问题，必须从各个方向观察零件以建立良好的外观/感知模型。这样的任务对传感器的位置没有严格的限制。
- Look-at Ray（4DOF）-这是另一个参数设置，用于摄像机可见性计算世界上物体的姿势。可以将相机视为具有特定位置和方向的平截头体。当我们需要移动相机传感器来观察物体时，可以将问题参数化为将相机放置在沿指向物体的光线的任何位置。尽管较近的物体可以提供更好的测量结果，但对物体的距离或绕射线轴的滚动没有严格的限制。
图4.2概述了四个参数化以及解决每个参数化的一般方法。在以下各节中，我们将详细讨论ikfast实现，并在最后评估其性能。
4.1.1 Basic Formulation of Inverse Kinematics
在后面我亲自推导了一遍PR2的运动学链路，发现这里用的T0和Tn其实可以理解为“没有用”。
J0T1为关节0的旋转，从这里也可以看出作者用的是标准DH（关节0的旋转体现在轴1上），所以可以知道当作者没有特别强调DH是什么的时候都为标准DH。
过去，有四种解决IK问题的方法。 第一种是代数方法，其中问题可以解决为高阶单变量多项式。 在寻找根源时，这种方法很容易变得病态。 第二种方法是基于分析多项式系统解集的结构[Wampler and Morgan（1991）]，但存在数值不适的问题。 第三种方法是基于线性代数，其中问题可以表达为特征分解[Raghavan and B.Roth（1990）; Manocha and Zhu（1994）]。 尽管线性代数方法在所有提出的方法中是最快的，但由于48x48矩阵的特征分解，它仍然存在数值问题。 我们的方法与过去的方法不同之处在于，我们强调数值稳定性胜于解决方案一般性。
其中pi，j + qi，j + ri，j <=1。与c2 + s1 − 1 = 0三角约束结合
对于所有旋转关节，我们都有一个最多2n个未知变量的系统。 建立稳定的IK解算器的挑战是，首先从所有可以用低阶多项式求解的变量开始，以尽可能地利用问题的结构。 在本文中，我们涵盖了运动学上的复杂性，直至解决了圆锥曲线的交点。 当耦合成对的联合变量时，问题可能会变得更加复杂，在这种情况下，必须有二次曲面的交点[Dupont et al（2007）]。 尽管在当今的商用机器人中极为罕见，但是如果存在更多的关节耦合，我们将退回到一般的求解器，例如[Manocha and Zhu（1994）]。
4.1.2 Evaluating Equation Complexity
第一个挑战是使用正向运动学方程式来设置变量的所有可能约束。解决方案的搜索者首先搜索最简单和最明确的解决方案。变量可以通过多种不同的方式求解，每种方式都有其自己的简并案例集。我们考虑两个级别的复杂性：
- 解决方案的复杂性涉及一个联合值可以采用的离散解决方案的数量。引入平方根和反正弦/余弦方法引入了两种可能的解决方案。有时这是不可避免的，因为基本的运动学决定了几种可能的解决方案，但是大多数情况下，其中一种解决方案可能与其余的运动学不一致。
- 数值复杂度涉及计算特定方程所需的运算次数。具有相同解决方案复杂度的方程式可能没有不同的数值复杂度。由于可能会发生简并的情况，因此数值复杂性还会不利于划分。
在确定要求解的变量时，将为每个变量评估解决方案的复杂度，并选择一组具有相同最小复杂度的解决方案。在这些方程式的每个内部，选择具有最小数值复杂度的方程式。
给定当前已解决的一组已知变量，我们以以下优先级搜索方程：
- cos jd和sin jd可以通过cos jd和sin jd中的两个线性独立方程多项式求解。 度数大于1的多项式将受到惩罚。
- 如果不存在线性独立方程，则搜索形式为cos jd +
b sin jd = c的方程。 这将使我们能够解决π弧度以内的正确角度。 - 最后解析为（cos jd）2 = a形式的方程，最多可以包含四个答案：（4.6）jd = {cos−1√a，− cos−1√a，cos−1 −√a，− cos−1 −√a}
- 可以以封闭形式解决的任何解决方案。
如果存在诸如零除之类的退化情况，则在生成的代码中会出现一个显式分支，该分支将条件的所有实例设置为零并重新分析方程式。 除法分为两类。 一个条件可以直接评估为联合变量的值，例如cos jd = 0； 另一个无法轻易解决，或者与p2 + p2之类的联合解决方案无关。 前者允许我们为jd =π和jd =-π显式创建分支，下面将显示这对于获得正确的解是必要的。 后者除以零类别将使传播更改时的更改更加困难，因此会受到更多的惩罚。
例如，当出现以下等式时：
- atan2函数对零和无穷极强，并且将在整个圆[0，2π]中返回一个解。 它在所有数学库中都可用。
- cos-1和sin-1的域为[-1，1]，由于数值不精确，导致函数不稳定。
还有两种解决方案，其中一种可能与机器人运动学不一致。
因此，应优先于公式4.11之类的解决方案，而不是公式4.10。 实际上，当没有其他可用方法时，仅应将sin-1 x，cos-1 x和√x形式的方程式用作最后的手段。
为了在实际的IK解算器中应用解决方案，需要保证解决方案将始终产生正确的答案。 开发一个可以在100％的时间内工作的IK解算器很困难，因为会出现许多退化的情况。 例如，研究人员在解决sj1和cj1时不考虑退化情况a1a5-a2a4 = 0时照原样应用公式4.11是一个普遍的错误。 除以零是一个严重的问题，必须完全改变优先级方程。 正如我们将在下面的示例中看到的那样，这实际上会影响首先解决的变量。不幸的是，我们不能为了消除零而仅仅消除分母，因为
c的符号会影响返回角度的象限，因此有可能返回正确结果为π弧度的角度。 因此，正确的方法是首先评估潜在的被零除的条件，然后转移到一组不同的解决方案中，在这些解决方案中，除以零的时间被延迟，其他变量被优先处理。 最终将为原始变量找到不同的解决方案。
在以下各节中，我们将使用Kuka KR5 R850工业机械手（图4.4和PR2个人服务机器人4.3）来解释逆向运动学的产生。PR2服务机器人具有七个关节，这增加了运动学的冗余性。 将在下面进一步详细讨论，但是现在我们假设第一关节j0的值由用户设置。
4.1.3 Solving 3D Translation IK
我们展示了如何解决PR2关节的一部分以进行翻译。 我们将最后三个关节相交的手腕位置视为平移目标。 PR2运动学方程为
经过验证除了t4和t6运算顺序有问题外，其他均正确
没有从1-6都列出是因为有些关节之间其实是等价的。
我们将 j0 视为自由参数，因此 c0 和 s0 是已知的。找到的第一个解决方案是 j3，其中我们得到约束：
此方程有两个解，并且无法计算对 j3 的其他约束，因此我们添加两个解决方案作为可能的解决方案。几何思维，j3是机器人的肘部，在计算底座到臂尖的长度时，通常有两个解决方案。
将j3视为已知值，下一个可解变量是j2：
没有其他约束可以形成，因此我们从sin j2计算出j2的两个解。 从几何上讲，这是有道理的，因为两个角度可以通过两种不同的方式达到相同的方向。 一旦设置了一个，另一个角度就被唯一确定。 但是，您会注意到，sin j3存在一个潜在的被零除的问题。 因为这是唯一可能的解决方案，所以我们必须计算给定j3∈0，π的一组新约束。 例如，当设置j3 = 0时，我们得到以下新方程式：
我们还向Kuka机械手提供了这些方程，以显示分析的相似性。 首先，毫无用处的是j1和j2的关节轴对齐，这暗示j0可以与计算解耦。
Conic Sections
公式4.15中的约束可能不会那么宽容，以致始终保证有一个联合变量被选出来。机械手将两个关节变量耦合在一起会引起以下方程，这很常见：
其中Aj，k是根据公式4.15的所有参考帧计算出的第j个约束的常数集。将c0，s0，c1和s1视为独立的未知变量，很难找到该方程组的闭合形式解。但是，通过利用c2 + s2 = 1的性质，我们可以求解公式4.20。首先，我们将方程组相对于成对变量c0c1，c0s1，s0c1和s0s1视为线性，然后使用基本线性代数技术对其求解。这使我们可以像这样挑选出成对变量：
尽管此处有四个方程，但存在两个潜在的自由度，这意味着可以将两个方程公式化为其他两个方程的组合。为了从左侧删除耦合变量，我们选择两个方程式，将双方平方，然后将它们加在一起。例如，（方程4.21）^2 +（方程4.22）^2得出左边。将转换应用于所有有效组合可得出：
其中Dj，k对于k <4是常数，而Dj，4是余数。如果Dj，4中的一个是常数，则可以通过将问题表示为两个圆锥截面的交点，设置x = cos jk和y = sin jk来求解单个变量的cos和sin：
此处就涉及到了圆锥曲线的知识
目标是产生第三个圆锥C2 = C0 +λC1，以使C2退化。退化圆锥曲线的解决方案是：点，线或两条相交的线。这样的几何图元更易于使用，我们可以立即找到它们与C1表示的单位圆的交点。当C2的行列式为0时，C 2退化。这等于求解λ中的三次方程：
可以使用[Weisstein（1999-至今）]中的方法进行解析。
对于λ的每个实际解，我们计算简并圆锥C2并找到其零空间NS（C2），我们可以使用它来计算线
与单位圆相交可得到以下二次方程式：
一旦求解了第一个关节变量，我们就可以再次从公式4.15开始继续寻找其他变量。在这些方程式中，sin j2的除法很清楚，因此我们在j2∈{0，π}时专门测试IK。例如，设置j2 = 0并重新求解可以使我们立即求解j0：
4.1.4 Solving 3D Rotation IK
其中j1和j2索引旋转矩阵中9个元素之一。与平移不同，旋转解决方案无需更改参考系即可解决。但是，退化的情况更加频繁，并且被零除，这使得最终解决方案变得复杂。对问题的几何分析将显示，三个角度可以为每个旋转矩阵提供至少2个唯一解。如果有两个以上的唯一解，则是因为两个关节轴已对齐，并且可以计算出无数个唯一解。
在这里，我们使用PR2和Kuka机器人的最后三个轴来说明如何解决3D旋转。 PR2旋转运动学是：
这里最明显的方程是c5 = r00，因为它几乎可以计算j5。实际上，如果r00∈{±1}，我们可以得出s5 = 0，则可以直接计算j5。因为我们从公式4.32得出的约束之一涉及除以R的元素，所以一个非常危险的可能性是c4，s4，c5，s5，c6或s6中的任何一个都可能出现在分母中。如果为零，将使方程退化。因此，根据经验法则，在生成方程式时会充分利用对联合变量的所有计算约束。根据上述约束，如果r00∈{±1}，那么我们知道s5 = 0并且可以求解j5，因此我们在生成的代码中将j5设置为0和π来创建一个额外的分支。这给出了j5 = 0评估的以下新矩阵：
这种类型的分析表明，ikfast还可以通过显式存储关节值之间的关系来返回无限数量的解。测试j5 =π将得出相反的关系j4 − j6 = atan2（r21，r11）。这些在这里毫无价值如果我们不遵循c5 = r00约束，那么这些关系将被完全忽略。
两者都涉及s5的乘法，如果s5 = 0则毫无意义。但是，根据以上讨论，我们保证s5ƒ= 0，因此等式是有效的。因为atan2是一个除法，所以我们可以从计算中忽略s5的绝对值，而只关心其符号。但是，这根本无法计算。进一步分析R8将得出解与s5是正还是负一致。因此，我们为j4计算两个解决方案：
4.1.5 Solving 6D Transformation IK
通过假设平移和旋转分量是可分离的，可以使用上述分析作为子模块来解决两类运动链。 可分离性使得要求解的最复杂的多项式是二次多项式，因此可以采用简单得多的解决方案。 第一种是最后三个关节轴在同一点相交（图4.5a），这意味着:
其中t6是常数。 这使我们可以通过对所有i <= 6使用公式4.15建立仅针对j0，j1，j2变量的约束。在求解了前三个联合值之后，我们可以将Ree，6（j0，j1，j2）视为 作为已知的常数矩阵，并使用公式4.32建立的约束条件直接求解R6（j3，j4，j5）。
第二种类型是前三个关节在同一点处相交（图4.5b），从而产生以下分离：
4.1.6 Solving 4D Ray IK
射线的参数方程
本章没有细细研究
其中p是沿射线的任何位置，d是总共四个自由度的单位方向。 给定目标射线rn（s）= pn + s dn在最后一条链接的坐标系中，射线的逆运动学是问题将rn匹配到全局指定的射线ree（s）= pee + s dee（图 4.6）：
在这里，我们针对以下情况提出了反射线运动学问题的解决方案：
•最后两个关节在同一点相交。
•问题得到了解决，因此射线在最后一个链接的坐标系中的位置位于原点。 如果不是这种情况，则可以修改Tn而不失一般性以满足此条件。
这些约束使我们能够将射线位置与射线方向分开：
射线IK与3D平移/ 3D旋转IK之间的根本区别在于，平移分量的射线方程式不由等号表示。 公式4.43是3维，但是只有两个实际约束；第二个约束是3维。 忽略任何方程式可能会导致完全错误的解决方案。 这些方程仅适用于s的一个值，这对于将输入射线位置pee与射线正向运动学的值标准化是必需的。 但是，显式处理s会引入新的依赖关系：j0和j1的解决方案取决于s，j2和j3的解决方案取决于j0和j1。
我们通过在每个参考帧处取与方向的叉积来消除公式4.43中的自由参数s：
我们找到了j0和j1的方程组。 由于存在叉积，因此存在约束，约束系统是一个圆锥截面，可以使用第4.1.3节中的技术求解。 对于方向，方程变为
这是解决完整3D旋转的子问题。
我们展示了一个使用巴雷特WAM臂的子链表示射线来解决射线IK的示例（图4.7。根据方程式4.26，我们仅在c1中计算二次方程式，求解该方程式将产生：
在这里，我们检测到另一种被零除的情况，并通过将j1设置为0，π进行处理。 在几何上，这使轴j0和j2对齐，从而允许无数个解。 产生以下解决方案：
4.1.7 Handling Redundancies
在求解IK时，机器人可能比IK参数设置具有更大的自由度。例如，Barrett WAM具有7个自由度，但是IK仅需要6个自由度。在这种情况下，我们选择一个重要性最低的关节，并假定它是由用户在运行IK之前设置的。我们称这些关节为自由关节，IK解决的关节称为活动关节。在计划期间，将自由接缝的范围离散化，并测试所有值，直到找到满足接缝极限，碰撞和计划约束的解决方案。 此自由空间的离散化取决于关节对IK解决方案的重要性。为了使搜索尽可能快，离散值应尽可能高而又不牺牲解的可行性。一个好的经验法则是，更靠近底部的关节更重要。
但是，我们不能仅仅选择距离基地最远的关节；有些关节不能自由选择，因为它们会破坏平移和旋转分量之间的独立性。例如，此处介绍的6D IK解决方案要求前3个或后3个活动关节在同一点相交。我们用于自动选择自由接点的一般启发式方法是从距底座最远的接点开始，然后继续选择更接近的接点，直到找到解决方案为止。
即使用户设置了自由关节，由于其值而导致的退化情况仍然会发生。 例如，Barrett WAM有4个关节用于求解平移分量，并将自由关节选择为j2，我们可以看到，当j2∈0，π，pi，3π时，关节轴可以对齐并生成简并的情况。 我们为每个简并值创建一个特例，并从头开始重新求解IK方程。 找到这些简并值有时可能非常具有挑战性。 但是，即使不是全部，大多数（如果不是全部）机器人的关节轴彼此正交，因此可以安全地假设在所有四个π边界上的配置都退化了。 实际上，即使不是必需的，这样的检查也只会使解决方案更加健壮，但会花费计算时间。
4.1.8 IKFast Results
图4.8显示了ikfast可以成功解决解决方案的几种类型的机械手。对于每个机器人，我们仅测试其运行ikfast求解器及其成功率（表4.1）。首先通过对随机配置进行1000-10000次采样，然后将机器人操纵器的目的地输入到IK功能来衡量成功。如果机器人具有自由关节值，则将这些值离散化并采样。故障分为返回错误的解决方案和找不到解决方案。前者真的很危险；从实验来看，ikfast永远不会返回错误的解决方案，因此这不是问题。从桌子上看，带有6个关节的机械手一直在成功。具有7个关节的机械手的自由关节离散度为0.1弧度，因此失败是因为离散度不够。设置较小的离散化可以解决问题，但不切实际。 WAM，PA10和HRP2机械手的自由关节是第三关节。 WAM和PA10的关节范围都非常大，因此总能找到解决方案。 HRP2接头范围很小，因此有时找不到解决方案。 PR2的自由接头是第一个接头，这极大地影响了溶液空间。因此，我们看到，当离散值相同时，PR2的成功率要小于其对应的成功率。
尽管每个ikfast解决方案都可以计算出精确的结果，但是计算中涉及的运算数量会极大地影响由于浮点运算而引起的数值不精确性。我们试图优先考虑解决方案的搜索，以便选择最稳定，最自信的解决方案。即使并非总是找到最佳解决方案，我们也证明了这些解决方案非常简单，不需要高级数学计算，甚至可以将必须完成的除法次数降至最低。因此，它们比以前提出的用于解析逆运动学的一般线性代数方法要鲁棒得多。
表4.2显示了ikfast算法的参数。我们已经讨论了如何自动设置自由连接和离散值。默认情况下，我们将所有值计算为10个有效数字，并在10-7处具有默认的零舍入。
ikfast从根本上改变了研究人员处理操纵计划问题的方式-
障碍。所有研究人员都可以依靠分析性IK安全地将IK集成到他们的计划器中，并开始搜索所有解决方案，而不必依赖于困扰数值误差和计算速度慢的基于梯度的方法。
结论
其实可以大概看出，IKFast其实就是通过解等式来实现解析解的计算的，但是论文中并没有写出他是如何同过各种判断用一个通用的方法解出来解析解的，这可能需要去看源码了。