416 lines
15 KiB
Markdown
416 lines
15 KiB
Markdown
# RM75 三种逆运动学方法离线对比实验设计
|
||
|
||
## 1. 目标
|
||
|
||
基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节
|
||
状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法：
|
||
|
||
1. Jacobian Moore-Penrose 伪逆法；
|
||
2. 阻尼最小二乘法（Damped Least Squares，DLS）；
|
||
3. 当前项目中的优化 Placo QP 方法。
|
||
|
||
实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和
|
||
可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码
|
||
“QP 更优”的结论。
|
||
|
||
## 2. 现有上下文
|
||
|
||
### 2.1 报告要求
|
||
|
||
中期报告 2.3.4 节已经确定：
|
||
|
||
- 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹；
|
||
- 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和
|
||
求解成功率；
|
||
- 章节末尾预留三张对比图。
|
||
|
||
现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。
|
||
|
||
### 2.2 当前 QP 与报告文字的差异
|
||
|
||
报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的
|
||
`PlacoIkSolver` 还包含：
|
||
|
||
- J3 初始构型软引导；
|
||
- J4 硬下限和预警区软缓冲；
|
||
- 接近奇异区时动态启用的六维可操作度任务；
|
||
- QP 失败时恢复实际关节状态并保持上一安全输出。
|
||
|
||
本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段
|
||
方法补充文字，避免报告方法描述与对比对象不一致。
|
||
|
||
## 3. 范围与安全边界
|
||
|
||
### 3.1 本次包含
|
||
|
||
- 只读加载一个现有右臂 episode；
|
||
- 离线重采样目标位姿；
|
||
- 在同一 URDF 上运行三种逆运动学方法；
|
||
- 复用当前 QP 代码和右臂 YAML 参数；
|
||
- 对三种方法使用相同的输出端安全处理；
|
||
- 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。
|
||
|
||
### 3.2 本次不包含
|
||
|
||
- 不连接真机，不移动机械臂，不操作夹爪；
|
||
- 不启动新的 PICO 录制；
|
||
- 不修改生产遥操作节点、launch、YAML 默认值或公开 API；
|
||
- 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果；
|
||
- 不模拟电机、通信和接触动力学；
|
||
- 不直接编辑用户提供的 PDF。
|
||
|
||
实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目
|
||
依赖。
|
||
|
||
因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机
|
||
在线控制对比。
|
||
|
||
## 4. 数据源与质量基线
|
||
|
||
实验固定使用：
|
||
|
||
```text
|
||
/home/robot/ACT_Data/tomato_pick/episode_0.hdf5
|
||
```
|
||
|
||
该文件的已核对属性如下：
|
||
|
||
- 机械臂：`right_rm75`；
|
||
- 样本数：484；
|
||
- 有效时长：约 16.1 s；
|
||
- 保存采样率：约 30 Hz；
|
||
- 位姿顺序：`x,y,z,qx,qy,qz,qw`；
|
||
- 所有目标位姿、当前位姿和关节状态均为有限值；
|
||
- 目标和当前四元数范数接近 1；
|
||
- 483 帧为遥操作激活且已发送命令；
|
||
- 记录时 QP 尝试 483 次并成功 483 次；
|
||
- 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。
|
||
|
||
只使用满足以下条件的最长连续区间：
|
||
|
||
```text
|
||
teleop_active && action_valid && command_sent
|
||
```
|
||
|
||
共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标
|
||
平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角
|
||
取有效区间第一帧的 `observations/qpos[:7]`。
|
||
|
||
episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于
|
||
数据质量核对，不替代任何方法在本实验中的离线计算结果。
|
||
|
||
## 5. 统一复放架构
|
||
|
||
数据流为：
|
||
|
||
```text
|
||
episode_0.hdf5
|
||
-> 有效区间与共同初始状态
|
||
-> 30 Hz 目标位姿重采样到 90 Hz
|
||
-> 伪逆 / DLS / 当前优化 QP 三路独立复放
|
||
-> 共同输出安全层
|
||
-> 正向运动学和逐采样指标
|
||
-> CSV / JSON / 三张图 / 中文分析
|
||
```
|
||
|
||
三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法
|
||
本周期的安全输出推进，三路之间不共享可变状态。
|
||
|
||
离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz
|
||
周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。
|
||
|
||
## 6. 目标轨迹重采样
|
||
|
||
原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的
|
||
`debug/timestamps/control_monotonic_ns`，目标时间轴保持原始起止时刻并以 1/90 s
|
||
采样：
|
||
|
||
- 位置使用分段线性插值；
|
||
- 姿态使用归一化四元数的最短弧 SLERP；
|
||
- 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值；
|
||
- 第一个和最后一个重采样位姿必须与原始有效区间端点一致；
|
||
- 不对目标轨迹额外放大、延长或人工加入困难片段。
|
||
|
||
## 7. 三种逆运动学方法
|
||
|
||
### 7.1 共同任务定义
|
||
|
||
当前关节状态为 `q`，正向运动学得到当前 TCP 位姿 `(p, R)`，目标位姿为
|
||
`(p_d, R_d)`。位置误差和姿态误差分别为：
|
||
|
||
```text
|
||
e_p = p_d - p
|
||
e_R = Log(R^T R_d)
|
||
```
|
||
|
||
求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致；
|
||
姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的
|
||
位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。
|
||
|
||
三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛：
|
||
|
||
```text
|
||
位置误差 <= 0.002 m
|
||
姿态误差 <= 0.005 rad
|
||
```
|
||
|
||
### 7.2 Jacobian 伪逆法
|
||
|
||
伪逆法按报告公式计算：
|
||
|
||
```text
|
||
q_dot = pinv(J) * v_d
|
||
```
|
||
|
||
其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose
|
||
伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。
|
||
|
||
### 7.3 DLS 方法
|
||
|
||
DLS 按报告公式计算：
|
||
|
||
```text
|
||
q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d
|
||
```
|
||
|
||
公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。
|
||
|
||
只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合：
|
||
|
||
```text
|
||
0.001, 0.003, 0.01, 0.03, 0.1, 0.3
|
||
```
|
||
|
||
每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中
|
||
最小化：
|
||
|
||
```text
|
||
位置 RMSE / 0.002 + 姿态 RMSE / 0.005
|
||
```
|
||
|
||
若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字
|
||
必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能
|
||
将其解释为跨轨迹最优参数。
|
||
|
||
阻尼扫描耗时不计入三种方法的在线求解时间对比。
|
||
|
||
### 7.4 当前优化 QP
|
||
|
||
QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、
|
||
当前双臂 URDF 和右臂配置中的参数：
|
||
|
||
```text
|
||
qp_j3_reference_deg: -89.57
|
||
qp_j3_weight: 0.0001
|
||
qp_j4_min_deg: 10.0
|
||
qp_j4_warn_deg: 25.0
|
||
qp_j4_weight: 0.0001
|
||
qp_manipulability_sigma_stop: 0.01
|
||
qp_manipulability_sigma_warn: 0.04
|
||
qp_manipulability_weight: 0.0001
|
||
```
|
||
|
||
保留现有六维末端软任务、`1e-6` 动能正则化、URDF 关节位置和速度限制、30 次迭代、
|
||
收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。
|
||
|
||
## 8. 共同输出安全层与失败处理
|
||
|
||
三种方法使用同一安全口径：
|
||
|
||
1. 每次数值迭代结果必须为 7 个有限关节值；
|
||
2. 数值迭代的关节状态不得超出 URDF 位置范围；
|
||
3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 `1/90 s`；
|
||
4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑；
|
||
5. 输出端最大关节速度为 `180 deg/s`，最大关节加速度为 `300 deg/s^2`；
|
||
6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全
|
||
关节状态；
|
||
7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。
|
||
|
||
QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。
|
||
共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化
|
||
过程中找到满足约束的解。
|
||
|
||
## 9. 评价指标
|
||
|
||
### 9.1 末端跟踪
|
||
|
||
- 逐采样位置误差 `||p_d - p||`，单位为 mm；
|
||
- 逐采样姿态夹角误差，单位为 degree；
|
||
- 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm；
|
||
- 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。
|
||
|
||
### 9.2 关节运动
|
||
|
||
- 每个采样时刻七关节绝对速度的最大值，单位为 `deg/s`；
|
||
- 全轨迹最大关节速度；
|
||
- 超过或触发共同速度/加速度限制器的周期数；
|
||
- 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度；
|
||
- 全轨迹最小归一化关节安全裕度。
|
||
|
||
速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 `3.14 rad/s`
|
||
（约 `180 deg/s`），直接展示实际速度更直观且与报告文字一致。
|
||
|
||
### 9.3 求解性能
|
||
|
||
- 收敛成功周期数和成功率；
|
||
- 失败周期数和最长连续失败长度；
|
||
- 单周期 IK 求解平均时间和最大时间，单位为 ms。
|
||
|
||
耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整
|
||
预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间
|
||
保持确定；平均和最大耗时从 10 次计时复放汇总。
|
||
|
||
## 10. 三张图设计
|
||
|
||
### 10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比
|
||
|
||
使用上下两个共享时间轴的子图：
|
||
|
||
- `(a)` 位置误差时序，单位 mm；
|
||
- `(b)` 姿态误差时序，单位 degree。
|
||
|
||
三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制
|
||
episode 原始 QP 误差曲线。
|
||
|
||
### 10.2 图 2-12 三种逆运动学方法关节运动约束对比
|
||
|
||
使用两个共享时间轴的子图：
|
||
|
||
- `(a)` 每个时刻的最大关节速度，单位 `deg/s`，并绘制 `180 deg/s` 虚线；
|
||
- `(b)` 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。
|
||
|
||
### 10.3 图 2-13 三种逆运动学方法综合性能指标对比
|
||
|
||
使用 `2 x 3` 六个小型分组柱状图，避免不同量纲共用坐标轴：
|
||
|
||
1. 位置 RMSE；
|
||
2. 姿态 RMSE；
|
||
3. 最大关节速度；
|
||
4. 最小归一化关节安全裕度；
|
||
5. 平均和最大求解时间；
|
||
6. 求解成功率。
|
||
|
||
柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和
|
||
图例区分方法。
|
||
|
||
## 11. 文件与产物
|
||
|
||
实验实现优先保持最小范围：
|
||
|
||
```text
|
||
xr_rm_teleop/test/ik_method_comparison.py
|
||
xr_rm_teleop/test/test_ik_method_comparison.py
|
||
```
|
||
|
||
前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖
|
||
无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。
|
||
|
||
默认输出目录为：
|
||
|
||
```text
|
||
output/ik_comparison/episode_0/
|
||
```
|
||
|
||
产物包括：
|
||
|
||
```text
|
||
samples.csv
|
||
summary.json
|
||
figure_2_11_tracking_error.svg
|
||
figure_2_11_tracking_error.png
|
||
figure_2_12_joint_constraints.svg
|
||
figure_2_12_joint_constraints.png
|
||
figure_2_13_summary.svg
|
||
figure_2_13_summary.png
|
||
analysis_2.3.4.md
|
||
```
|
||
|
||
`samples.csv` 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、
|
||
位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制
|
||
触发标志。`summary.json` 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径，
|
||
保证结果可追溯。
|
||
|
||
`analysis_2.3.4.md` 使用中文撰写，包含：
|
||
|
||
- 数据来源和离线实验口径；
|
||
- DLS 最终阻尼和选择规则；
|
||
- 三张图的建议图题与图注；
|
||
- 与式 (2-25) 至式 (2-28) 对应的数值结果；
|
||
- 对优势、代价和异常结果的客观分析；
|
||
- 当前优化 QP 相对报告 2.3.2 节的补充方法说明。
|
||
|
||
## 12. 错误处理
|
||
|
||
以下情况在生成任何正式图前立即报错：
|
||
|
||
- episode 路径不存在或不是 HDF5；
|
||
- 必需字段或属性缺失；
|
||
- 数组长度不一致；
|
||
- 找不到至少包含两个样本的连续有效遥操作区间；
|
||
- 时间戳非严格递增；
|
||
- 位姿、关节角或四元数含 NaN/Inf；
|
||
- 四元数无法正规化；
|
||
- episode 机械臂不是 `right_rm75`；
|
||
- URDF 或当前右臂配置不存在；
|
||
- Placo 版本不是项目固定的 0.9.4；
|
||
- 任一方法没有生成与统一时间轴等长的结果；
|
||
- CSV、JSON 和绘图使用的汇总数值不一致。
|
||
|
||
单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据
|
||
结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。
|
||
|
||
## 13. 测试与验证
|
||
|
||
### 13.1 聚焦测试
|
||
|
||
最小测试至少覆盖：
|
||
|
||
- 30 Hz 到 90 Hz 重采样保持首尾位置和姿态；
|
||
- SLERP 选择最短弧并输出单位四元数；
|
||
- 姿态夹角误差在单位旋转和已知小角度下正确；
|
||
- 关节安全裕度与式 (2-28) 一致；
|
||
- 无效候选触发失败保持而不是推进状态；
|
||
- DLS 选择规则按成功率、归一化误差和最大速度依次决策；
|
||
- 汇总指标与逐采样数据一致。
|
||
|
||
### 13.2 真实模型冒烟验证
|
||
|
||
使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认：
|
||
|
||
- 输出始终为有限 7 维关节值；
|
||
- 没有输出越过 URDF 关节位置边界；
|
||
- 失败时保持上一安全状态；
|
||
- 当前 QP 直接走现有 `PlacoIkSolver`，没有本地复制实现。
|
||
|
||
### 13.3 项目级验证
|
||
|
||
从工作空间根目录 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
colcon build --symlink-install
|
||
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||
```
|
||
|
||
随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查：
|
||
|
||
- 三张 PNG 无裁切、重叠、乱码或不可辨识曲线；
|
||
- SVG 可编辑且文字完整；
|
||
- PNG 为 300 dpi；
|
||
- 图题、坐标轴、单位和图例为中文论文风格；
|
||
- `summary.json` 与图中柱顶数值一致；
|
||
- 同一输入重复运行时，除耗时外的结果一致。
|
||
|
||
## 14. 验收标准
|
||
|
||
满足以下条件才视为完成：
|
||
|
||
1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始；
|
||
2. 当前优化 QP 复用现有实现和右臂参数；
|
||
3. 三种方法使用同一输出安全口径，任何失败均安全保持；
|
||
4. DLS 固定阻尼选择过程和最终值可追溯；
|
||
5. 生成三张与报告公式和图号一致的正式对比图；
|
||
6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字；
|
||
7. 所有实际执行的测试和构建结果如实记录；
|
||
8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。
|