8.0 KiB
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_strCmd_Kinematics_inverse_pose_str_2PStepsCmd_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
请求示例：
{
"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"
}
}
可选阈值参数：
{
"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 运动学计算内核，不是实时控制器。
尚未支持：
- 与真实机器人控制器通讯。
- 实时伺服控制。
- 关节反馈闭环。
- 安全互锁。
- 急停、安全区、限位开关等控制器级安全能力。
建议后续增强顺序
如果继续按工业机器人实际使用场景增强，建议优先级如下：
- 增加多 IK 解和构型选择。
- 增加关节连续性和最短路径处理。
- 增加速度、加速度和轨迹时间参数化。
- 增加工具坐标系和工件坐标系管理。
- 增加 PTP、LIN、CIRC 等工业运动指令。
- 增加碰撞检测。
- 增加奇异类型分类，例如腕部、肩部、肘部奇异。
- 增加控制器通讯和实时执行相关能力。