docs: 添加逆运动学对比实验方案

2026-08-24 17:40:56 +08:00
parent 7a5c27d6b9
commit 5785e73edb
2 changed files with 2008 additions and 0 deletions
@@ -0,0 +1,415 @@
+# 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 实现。