如何在 ROS2 中构建机器人手臂 IK 解算器

一种具有连续零空间控制且无需雅可比矩阵迭代的 7 自由度逆运动学求解器的封闭形式解。我们为 NERO 7 自由度机械臂实现了一个实时的分析型逆运动学求解器，该求解器采用了关节角度参数化和几何冗余解决方法。

与基于雅可比矩阵的传统 IK 解算器不同，这种方法具有以下优势：

•连续的肘部运动

•稳定零空间控制

•没有迭代优化

•没有局部最小值

•实时求解性能

整个机械臂的冗余度被简化为一个单一的臂角参数 Ψ，从而实现了平稳的自主运动，同时保持末端执行器的姿势不变。

本教程涵盖：

•几何法的 IK 推导

•SWE 三角形建模

•分析题 1 - 7 的解答

•关节限制优化

•零空间运动生成

•ROS2 集成

•MoveIt2 模拟工作流程

1. 从多种解决方案到可控解决方案：一种用于 7 自由度 逆运动学的参数化方法

为什么需要参数化的 IK 呢?

7 自由度机械臂与 6 自由度机械臂的主要区别并不仅仅在于多了一个关节。真正的区别在于一个末端执行器的姿势可以对应多种关节配置。

当末端执行器的位置固定后，关节角度仍不唯一——存在无数种有效的解决方案。这种情况下，末端执行器保持不动，而手臂则自行重新调整姿态，这种运动方式被称为“零空间运动”。

这意味着 IK 不再是关于寻找一个独特的解决方案，问题变成了：我们应该选择哪一个解决方案呢?

为了解决这个问题，多余的自由度被分离出来，并通过一个单一的参数 ψ 来表示，该参数控制着肘部的姿态。

IK 中的肘部角度参数化

•S：肩部中心(三个关节轴线的交点)

•E：肘部中心(关节 4 的位置)

•W：腕部中心(最后三个关节轴线的交点)

•点 S、点 E、点 W 构成一个边长固定的三角形。

•肘部角度 决定了点 E 在圆上的位置。

当末端执行器的姿态固定后，S 和 W 在空间中位置固定，而 E 则沿着三维空间中的一个圆周运动。

这种圆周运动的角度被定义为肘部角度 ψ 。

请用一句话进行翻译：

ψ → 肘部姿势变化 → 关节角度变化 → 末端执行器保持不变

此方法与传统数值 IK 解决方案之间的差异

2.快速入门指南

3.参数化 IK 求解器的完整工作流程

这种 IK 解算器遵循一个参数化的分析流程：

•从目标姿态中提取 S、W 和 4 。

•根据肘部角度 ψ 计算肘部点 E，并通过解析方法求解 1⁢3 和 5⁢7

•计算在所有关节限制条件下肘部角度的可行区域

•利用加权二次目标函数在可行区域内优化肘部角度

以下各部分与论文中的方程式以及代码中的实现内容直接对应。

步骤 0：代码结构概述(简洁版)

ik_solver.py 中的核心功能

步骤 1：提取几何锚点

1.目标

鉴于目标末端执行器的位姿为 T07 ，我们首先提取出分析式 IK 算法所需的几何锚点。

2.几何定义

如下图所示：

•B：基坐标系

•S：肩部中心点

•W：手腕中心

•E：肘部中心点

已知末端执行器的姿态为 Τ07 ，我们首先求解以下内容：

S：肩部点

W：手腕点(通过将末端执行器框架向后偏移距离 6 而获得)

θ4：肘关节角度(通过 S—E—W 三角形并依据余弦定理唯一确定)

在三角形 S–E–W 中，肘角 θ4 由边长决定：

3.代码实现

参数计算：_compute_swe_from_target

辅助函数：_从三角形中求解θ4

关键见解

在这个阶段，问题被简化为从完整的姿态 IK 任务转变为对 S-E-W 三角形的纯几何计算。

步骤 2：根据手臂角度 (核心几何参数)求解肘部点 E 的位置

1. 目标

一旦 S 和 W 的位置确定，肘部点 E 就会在三维空间中沿着一个圆周运动。

手臂角度 ψ 用于在该圆圈上选定特定的肘部姿势。

2. 几何解释

肘部位置位于一个圆上，该圆的弦是由 S 到 W 这条线段所确定的：

地点：

•C：圆心

•r：圆的半径

•e1,e2：构成圆平面的正交基向量

3. 代码实现

点 E 计算：_肘部角度(由手臂角度计算得出)

这是整个算法的几何核心部分。

第 3 步：通过 S-E-W 方向分析求解所有关节角度

3.1.1 肩关节：q1、q2、q3

该论文通过几何投影方法得出了一个直接的闭式解：

•q1是由点 E 在底面的投影所得到的。

•q2由 E 的高度所决定

•q3是從手腕相对于肘部的方位來進行求解的

3.1.2 代码实现

配置解决方案：_从 SWE 中解决 q123 问题

第 4 步：计算可行的肘部角度范围

1. 目标

并非每一个肘部角度 ψ 都能形成有效的关节结构。

即使末端执行器的姿势是固定的，某些 ψ 值仍会使一个或多个关节超出其机械限制范围。

所以在选定最终解决方案之前，我们首先会确定可行的弯头角度集合。

2.代码实现

可行区域计算：_get_theta0_feasible_region

在内部，该函数会调用 _ik_one_arm_angle 这个函数，其执行以下步骤：

•将手臂角度Ψ代入

•解决完整的关节配置问题

•检查所有接头是否均未超出其极限范围

•如果有效 → 将手臂角度纳入可行区域

第 5 步：最优臂角度选择(加权二次目标函数)

1.目标

在确定了可行的肘部角度范围之后，我们还需要从所有有效的候选方案中选出最佳方案。

目标函数定义如下：

wi：权重系数，其值会随着对应关节接近其机械极限而增大。

目标：在保证所有关节尽可能远离其极限位置的前提下，尽量减少整体关节的活动范围。

联合限制惩罚设计：加权函数(论文中的公式 20)

哪里

a = 2.28

b = 2.28

2.代码实现

重量计算：_重量限制

最优臂角搜索：_optimal_theta0

这是该论文中提出的最优解决方案选择策略。

从本质上讲，它将问题转化为：

一维二次函数最小化 → 全局最优解 → 无需迭代求解且不存在局部最小值。

4.零空间运动原理(自然嵌入式)

对于一个 7 自由度的机械臂而言，零空间是由手臂角度Ψ直接控制的。

其原则很简单：

•末端执行器的姿态 Τ07 保持不变

•只有手臂的角度 ψ 被改变

•机器人的关节会自动进行自我重新配置，同时保持末端执行器的位置不变。

这被称为零空间运动。

在实际应用中，通过改变手臂的角度就能轻松实现空转运动：

无需使用雅可比矩阵。

无需投影算子。

而且该运动依然平稳且稳定，没有出现振荡现象。

5.总结

该方法为一个具有 7 个自由度的 S-R-S 机器人机械臂设计了一种封闭形式的逆运动学求解器，并结合了对机械臂角度零空间的 1 维二次优化。

本文编译自hackster.io