7.9 KiB
RM75 双臂 J3 参考角仿真标定设计
1. 目标
在不连接真机、不修改现有生产控制参数的前提下，基于当前双臂 URDF、Placo QP 求解器和 MuJoCo 运动学模型，分别标定左臂与右臂的第三关节软引导参考角：
[ q_{3,\mathrm{ref}}^{L,},\qquad q_{3,\mathrm{ref}}^{R,}。 ]
标定结果只作为当前机器人初始姿态、采摘区域、工具安装和本侧收集筐布局下的 仿真初值。后续必须通过 mock 完整控制链路和真机低速试验复验，允许根据实测结果 更新参数。
2. 范围
本轮只做离线参数标定：
- 左右臂分别从当前 YAML 初始关节姿态出发；
- 左臂放入左臂初始 TCP 下方约 40 cm 的本侧收集筐；
- 右臂放入右臂初始 TCP 下方约 40 cm 的本侧收集筐；
- 每个轨迹点同时指定 TCP 位置与姿态，保持严格六维跟踪；
- 第四关节下限暂定左右臂均为 10°；
- 使用现有 Placo 任务接口临时加入 J3 软任务和位置可操作度任务；
- 将每个有效关节结果同步写入现有 MuJoCo 双臂模型，检查关节映射和状态有效性；
- 输出候选角度的逐轨迹指标、汇总排名和推荐平台区间。
本轮不修改 placo_ik_solver.py、遥操作节点或 YAML，不测试真机，不加入任务阶段
状态机、自动姿态放松、碰撞规划或新依赖。
3. 坐标与姿态约定
- 双臂机器人公共坐标系
+Y为正前方； - 公共坐标系
+Z为机器人垂直向上； - 左右方向使用公共坐标系
X； - 采摘点保持对应机械臂初始 TCP 的横向
X位置； - 采摘与退出阶段保持初始 TCP 姿态；
- 从退出点移动到收集筐上方时，TCP 姿态采用四元数球面插值，平滑旋转为工具工作
轴沿公共坐标系
-Z； - 收集筐上方至筐内的垂直下降段保持工具朝下姿态；
- 返回初始位姿时平滑恢复初始 TCP 姿态。
“严格六维”表示每个时刻的位置和姿态目标均参与同一 QP，仿真不会因接近奇异点 而自动降低姿态权重。
4. 轨迹族
4.1 采摘点
每条机械臂使用 3 个前向距离和 3 个高度：
[ y_h\in{0.30,0.40,0.50}\ \mathrm{m}, ]
[ z_h\in{-0.30,-0.20,-0.10}\ \mathrm{m}。 ]
这些点位于用户给定的前方 30～50 cm、相对机械臂基座高度 ±50 cm 范围内，并且 是当前固定初始工具姿态下离线预扫描得到的主要可解高度区间。左右臂各 9 条轨迹， 共 18 条完整轨迹。
4.2 单条完整轨迹
每条轨迹由以下连续段组成：
- 初始 TCP 位姿；
- 采摘点前方 5 cm 的预接近点；
- 沿公共
+Y直线进入采摘点； - 沿原路径退回预接近点；
- 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
- 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
- 垂直抬升 10 cm；
- 返回初始 TCP 位姿。
轨迹按当前控制频率 90 Hz 离散，平移速度不超过 0.15 m/s，角速度不超过 0.5 rad/s。每条轨迹均从相同初始关节状态重新开始，避免上一候选角或上一轨迹的 状态污染下一次评估。
4.3 边界轨迹
工作区边界、不可达目标和 QP 失败恢复轨迹不参与第一轮 J3 参数排名。J3 参数确定后， 再使用这些轨迹验证失败保持和恢复逻辑，避免不可达点数量掩盖参考角本身的差异。
5. QP 试验配置
主任务保持现有严格六维相对位姿任务，内部等价于：
[ \left|J_p\Delta q-e_p\right|^2 +\left|J_R\Delta q-e_R\right|^2， ]
其中误差定义为目标减当前。仿真脚本不改变 Placo 的误差符号。
在主任务之外临时加入：
[ w_e\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2, \qquad w_e=10^{-5}， ]
以及 TCP 位置可操作度任务：
[ -w_m\nabla m_p(q)^T\Delta q, \qquad w_m=10^{-4}。 ]
保留当前动能正则、URDF 关节位置限制和关节速度限制。第四关节临时增加：
[ q_4\ge10^\circ。 ]
6. 参数扫描
6.1 粗扫
左臂：
[ q_{3,\mathrm{ref}}^L\in{0^\circ,10^\circ,\ldots,100^\circ}。 ]
右臂：
[ q_{3,\mathrm{ref}}^R\in{0^\circ,-10^\circ,\ldots,-120^\circ}。 ]
6.2 细扫
在粗扫最优候选附近 ±10° 内以 2° 为步长再次扫描。若多个相邻候选没有明显差异， 选择稳定平台区的中心，而不是选择孤立的单点峰值。
6.3 基线
同时运行两组基线：
original：当前原始 QP，不含 J3 软任务、位置可操作度任务和 J4 额外下限；manip_j4：不含 J3 软任务，但加入位置可操作度任务和 J4 额外下限。
所有 J3 候选均在 manip_j4 基础上只改变 J3 参考角。最终结果必须同时报告：
- 相对当前原始 QP 的改善；
- 相对“只加位置可操作度”的改善；
- J3 软任务是否降低六维跟踪成功率。
7. 记录指标
对每个候选角度、每条轨迹记录：
- QP 求解失败周期数
N_fail； - 完成全部轨迹的数量
N_complete； - 整条轨迹六维雅可比的最小奇异值
sigma_min； - 第四关节最小安全余量
m_q4 = min(q4 - 10°)； - 肘部最小外展量
d_elbow； - 最大 TCP 位置误差和姿态误差；
- 最大关节速度；
- 累计关节运动代价
E_q； - 是否出现超过阈值的单周期关节构型跳变。
肘部外展量使用公共坐标系中第四连杆位置计算：
[ d_{\mathrm{elbow}}^L=-x_{\mathrm{elbow}}^L, \qquad d_{\mathrm{elbow}}^R=x_{\mathrm{elbow}}^R。 ]
当前 URDF 碰撞网格对相邻连杆存在已知自碰撞警告，因此本轮不把 MuJoCo/Placo 碰撞距离加入评分，避免错误碰撞几何影响 J3 选择。
8. 选择规则与评分函数
8.1 硬门槛
候选角度首先按以下顺序筛选：
N_fail最少；N_complete最多；- 最大位置误差不超过 2 mm；
- 最大姿态误差不超过 0.005 rad；
- 不违反第四关节、URDF 关节位置和速度限制；
- 不出现超过配置阈值的单周期关节跳变。
只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的 QP 失败或更差的 TCP 跟踪。
8.2 并列候选评分函数
对通过硬门槛的候选，将各项指标在同一机械臂的候选集合内转换为 [0,1] 的百分位
排名。数值越大越好的指标直接排名，数值越小越好的指标反向排名：
r_sigma：全轨迹最小奇异值排名；r_q4：第四关节最小安全余量排名；r_elbow：肘部最小外展量排名；r_smooth：累计关节运动代价与最大关节速度的联合反向排名；r_track：最大六维 TCP 跟踪误差的反向排名。
并列候选的综合评分为：
[ S(q_{3,\mathrm{ref}}) =0.45r_{\sigma} +0.20r_{q4} +0.20r_{\mathrm{elbow}} +0.10r_{\mathrm{smooth}} +0.05r_{\mathrm{track}}。 ]
选择：
[ q_{3,\mathrm{ref}}^*=\arg\max S(q_{3,\mathrm{ref}})。 ]
最小奇异值权重最高，因为本轮首要目标是降低奇异点和 QP 失败风险；第四关节余量 和肘部外展各占 0.20；平滑性和跟踪误差用于区分性能接近的候选。若评分最高点与 相邻角度差异小于 2%，取相邻稳定平台的中心角度。
8.3 结果报告
左右臂分别输出：
- 推荐参考角；
- 推荐稳定区间；
- 粗扫与细扫排名表；
- 与两组基线的指标对比；
- 最差轨迹及其失败位置；
- 是否建议保留左右臂统一的第四关节 10° 下限。
9. 实施边界与后续流程
标定完成后的顺序为：
- 根据仿真结果形成正式 QP 修改规格；
- 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
- 在
use_mock:=true下运行完整遥操作控制链路； - 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
- 经过安全评审后，在真机上以低速、小范围方式复验；
- 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。