315 lines
8.0 KiB
Markdown
315 lines
8.0 KiB
Markdown
# URDF/Orocos KDL 工业机器人运动学能力说明
|
||
|
||
本文档说明当前 URDF/Orocos KDL 机器人运动学模块已经支持的能力，以及距离完整工业级机器人控制器还需要补齐的能力。
|
||
|
||
## 当前已支持能力
|
||
|
||
### 多机器人 URDF 注册
|
||
|
||
模块支持通过 `Cmd_InitRobot` 动态注册 URDF，并用 `robot_uuid` 管理不同机器人实例。当前也支持查询机器人、列出机器人和删除机器人。
|
||
|
||
相关能力：
|
||
|
||
- `Cmd_InitRobot`：注册或更新机器人 URDF。
|
||
- `Cmd_GetRobot`：查询指定机器人实例。
|
||
- `Cmd_ListRobots`：列出当前机器人实例。
|
||
- `Cmd_RemoveRobot`：删除机器人实例。
|
||
|
||
### 自动推导运动学链
|
||
|
||
初始化时不再固定依赖 `base`、`tool0`、`base_link` 等特定 link 名称。模块会基于 KDL Tree 自动推导 6 轴串联运动学链。
|
||
|
||
当前策略：
|
||
|
||
- 优先兼容旧命名链路 `base -> tool0`。
|
||
- 如果旧链路不存在，则从 URDF 的真实根节点出发，扫描叶子 link。
|
||
- 选择一条正好包含 6 个活动关节的串联链。
|
||
|
||
### 正运动学 FK
|
||
|
||
模块支持输入 6 个关节角，计算 TCP 位姿。
|
||
|
||
输出内容：
|
||
|
||
- TCP 位置：`x, y, z`
|
||
- TCP 姿态：四元数 `qx, qy, qz, qw`
|
||
|
||
对应接口：
|
||
|
||
- `Cmd_Kinematics_forward_pose_str`
|
||
|
||
### 全关节位姿 FK
|
||
|
||
模块支持计算每个关节节点的位姿，并按前端需要的 `OPERATION.frames.objStates` 格式输出。
|
||
|
||
该能力适用于：
|
||
|
||
- three.js 机器人显示。
|
||
- 关节动画回放。
|
||
- 数字孪生场景姿态同步。
|
||
- 简单离线仿真预览。
|
||
|
||
对应接口：
|
||
|
||
- `Cmd_Kinematics_forward_all_joints`
|
||
|
||
### 逆运动学 IK
|
||
|
||
模块支持输入目标 TCP 位姿和初始关节角，计算对应关节解。
|
||
|
||
当前主路径使用 KDL LMA 求解器，另保留 NR 求解接口。
|
||
|
||
对应接口：
|
||
|
||
- `Cmd_Kinematics_inverse_pose_str`
|
||
- `Cmd_Kinematics_inverse_pose_str_2PSteps`
|
||
- `Cmd_Kinematics_inverse_pose_str_NoDifference`
|
||
|
||
### 姿态轨迹插补和逐点 IK
|
||
|
||
模块支持对两个姿态点之间进行插补，然后逐点执行 IK，生成一串关节解。
|
||
|
||
当前支持两种方式：
|
||
|
||
- 自动估算插补步数。
|
||
- 由调用方指定插补步数。
|
||
|
||
适用场景：
|
||
|
||
- 前端拖动目标位姿后的简单路径预览。
|
||
- 离线验证 TCP 从起点到终点的可达性。
|
||
- 生成用于动画显示的关节序列。
|
||
|
||
### 不插补的多点 IK
|
||
|
||
模块支持直接对输入的多个目标姿态点逐点逆解，不在相邻点之间插补。
|
||
|
||
适用场景：
|
||
|
||
- 已有外部轨迹点。
|
||
- 只需要验证离散点位是否可达。
|
||
- 调试 IK 解算稳定性。
|
||
|
||
对应接口：
|
||
|
||
- `Cmd_Kinematics_inverse_pose_str_NoDifference`
|
||
|
||
### 关节上下限检查
|
||
|
||
模块已支持从 URDF 的 `<limit lower="..." upper="...">` 中读取关节上下限。
|
||
|
||
当前检查范围：
|
||
|
||
- FK 输入关节值。
|
||
- 全关节 FK 输入关节值。
|
||
- IK 初始关节值。
|
||
- IK 求解结果。
|
||
|
||
如果关节值超出 URDF 定义范围，计算会返回失败，不会继续输出不可用结果。
|
||
|
||
### three.js 前端测试页面
|
||
|
||
当前已有 three.js 测试页面，用于验证 URDF 注册、机器人显示、FK、IK 和姿态回放。
|
||
|
||
页面能力包括：
|
||
|
||
- 加载并注册 URDF。
|
||
- 显示 3D 机器人模型。
|
||
- 测试 FK。
|
||
- 测试 IK。
|
||
- 应用 IK 结果到关节状态。
|
||
- 查看 JSON 请求和响应。
|
||
|
||
## 当前定位
|
||
|
||
当前模块更接近“工业机器人运动学仿真和前端验证内核”。
|
||
|
||
它已经具备工业机器人常见的基础运动学能力：
|
||
|
||
- URDF 模型注册。
|
||
- 运动学链自动推导。
|
||
- FK。
|
||
- 全关节 FK。
|
||
- IK。
|
||
- 简单轨迹插补。
|
||
- 关节上下限约束。
|
||
- 前端 3D 可视化测试。
|
||
|
||
这些能力适合用于：
|
||
|
||
- 机器人模型可用性检查。
|
||
- 前端 3D 调试。
|
||
- 数字孪生姿态同步。
|
||
- 离线点位可达性验证。
|
||
- 简单路径和动画预览。
|
||
|
||
## 尚未支持的完整工业级能力
|
||
|
||
以下能力目前尚未完整支持，如果要接近真实工业机器人控制器，需要继续补齐。
|
||
|
||
### 碰撞检测
|
||
|
||
当前不处理碰撞相关内容。
|
||
|
||
尚未支持：
|
||
|
||
- 机器人自碰撞检测。
|
||
- 机器人与环境碰撞检测。
|
||
- 工具与工件碰撞检测。
|
||
- 基于 URDF collision 几何的碰撞模型构建。
|
||
|
||
### 奇异点检测
|
||
|
||
模块已支持基于 TCP 雅可比矩阵的奇异点检测。
|
||
|
||
当前支持：
|
||
|
||
- 计算 6x6 TCP 雅可比矩阵。
|
||
- 计算雅可比矩阵奇异值。
|
||
- 输出最小奇异值、最大奇异值、条件数和 manipulability。
|
||
- 输出 rank。
|
||
- 根据阈值给出 `normal`、`warning`、`singular` 风险等级。
|
||
- 在 three.js 测试页面中通过“奇异点”按钮直接查看当前关节姿态风险。
|
||
|
||
对应接口：
|
||
|
||
- `Cmd_Kinematics_check_singularity`
|
||
|
||
请求示例：
|
||
|
||
```json
|
||
{
|
||
"msg": "singularity check",
|
||
"req_code": "CHECK_SINGULARITY_001",
|
||
"req_from": "client",
|
||
"req_cmd": "Cmd_Kinematics_check_singularity",
|
||
"req_param": {
|
||
"robot_uuid": "abb_irb120_3_58",
|
||
"joints_str": "0.15,-0.25,0.35,0.1,-0.2,0.3"
|
||
}
|
||
}
|
||
```
|
||
|
||
可选阈值参数：
|
||
|
||
```json
|
||
{
|
||
"singular_threshold": 0.0001,
|
||
"warning_threshold": 0.01,
|
||
"condition_threshold": 1000000,
|
||
"condition_warning_threshold": 10000
|
||
}
|
||
```
|
||
|
||
响应中的关键字段：
|
||
|
||
- `risk_level`：`normal`、`warning` 或 `singular`。
|
||
- `is_singular`：是否已判定为奇异。
|
||
- `is_near_singular`：是否接近奇异。
|
||
- `rank`：雅可比矩阵秩。
|
||
- `singular_values`：奇异值数组。
|
||
- `min_singular_value`：最小奇异值。
|
||
- `condition_number`：条件数。
|
||
- `manipulability`：可操作度指标。
|
||
- `jacobian`：6x6 TCP 雅可比矩阵。
|
||
|
||
注意：当前检测给出数值风险等级，尚未进一步分类为腕部奇异、肩部奇异或肘部奇异。
|
||
|
||
### 速度、加速度和 jerk 约束
|
||
|
||
当前主要约束位置上下限，尚未完整处理工业轨迹中的速度、加速度和 jerk 约束。
|
||
|
||
尚未支持：
|
||
|
||
- 关节速度限制。
|
||
- 关节加速度限制。
|
||
- 笛卡尔速度限制。
|
||
- 笛卡尔加速度限制。
|
||
- jerk 限制。
|
||
- 时间最优轨迹规划。
|
||
|
||
### 多 IK 解管理
|
||
|
||
工业机器人通常存在多个 IK 分支。当前模块没有显式管理多解分支。
|
||
|
||
尚未支持：
|
||
|
||
- 肘上/肘下选择。
|
||
- 腕翻转/不翻转选择。
|
||
- 肩部左右构型选择。
|
||
- 最接近当前姿态的解选择。
|
||
- 解连续性筛选。
|
||
|
||
### 轨迹连续性和最短路径
|
||
|
||
当前插补和 IK 可以生成关节序列，但还不是完整工业级轨迹规划。
|
||
|
||
尚未支持：
|
||
|
||
- 关节空间最短路径选择。
|
||
- 关节跨越 `+-pi` 时的连续性处理。
|
||
- 多点轨迹平滑。
|
||
- 速度连续和加速度连续约束。
|
||
- 轨迹失败点定位和恢复。
|
||
|
||
### 工具坐标系和工件坐标系
|
||
|
||
当前主要使用 URDF 链路末端作为 TCP，尚未形成完整坐标系管理能力。
|
||
|
||
尚未支持：
|
||
|
||
- 工具坐标系 TCP 管理。
|
||
- 工件坐标系/用户坐标系管理。
|
||
- 基坐标、世界坐标、工具坐标之间的统一转换。
|
||
- 多 TCP 切换。
|
||
|
||
### 工业机器人指令语义
|
||
|
||
当前支持基础运动学计算，但还没有完整工业机器人指令层。
|
||
|
||
尚未支持：
|
||
|
||
- PTP 指令。
|
||
- LIN 指令。
|
||
- CIRC 指令。
|
||
- Blend/Zone 过渡。
|
||
- 等待、IO、夹具动作与轨迹同步。
|
||
- 程序段级离线编程。
|
||
|
||
### 动力学和负载
|
||
|
||
当前模块不处理动力学。
|
||
|
||
尚未支持：
|
||
|
||
- 质量和惯量参与计算。
|
||
- 关节力矩计算。
|
||
- 重力补偿。
|
||
- 负载模型。
|
||
- 动态可达性判断。
|
||
|
||
### 控制器通讯和实时控制
|
||
|
||
当前模块是 WASM 运动学计算内核，不是实时控制器。
|
||
|
||
尚未支持：
|
||
|
||
- 与真实机器人控制器通讯。
|
||
- 实时伺服控制。
|
||
- 关节反馈闭环。
|
||
- 安全互锁。
|
||
- 急停、安全区、限位开关等控制器级安全能力。
|
||
|
||
## 建议后续增强顺序
|
||
|
||
如果继续按工业机器人实际使用场景增强，建议优先级如下：
|
||
|
||
1. 增加多 IK 解和构型选择。
|
||
2. 增加关节连续性和最短路径处理。
|
||
3. 增加速度、加速度和轨迹时间参数化。
|
||
4. 增加工具坐标系和工件坐标系管理。
|
||
5. 增加 PTP、LIN、CIRC 等工业运动指令。
|
||
6. 增加碰撞检测。
|
||
7. 增加奇异类型分类，例如腕部、肩部、肘部奇异。
|
||
8. 增加控制器通讯和实时执行相关能力。
|