246 lines
7.9 KiB
Markdown
246 lines
7.9 KiB
Markdown
# 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 单条完整轨迹
|
||
|
||
每条轨迹由以下连续段组成：
|
||
|
||
1. 初始 TCP 位姿；
|
||
2. 采摘点前方 5 cm 的预接近点；
|
||
3. 沿公共 `+Y` 直线进入采摘点；
|
||
4. 沿原路径退回预接近点；
|
||
5. 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
|
||
6. 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
|
||
7. 垂直抬升 10 cm；
|
||
8. 返回初始 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 硬门槛
|
||
|
||
候选角度首先按以下顺序筛选：
|
||
|
||
1. `N_fail` 最少；
|
||
2. `N_complete` 最多；
|
||
3. 最大位置误差不超过 2 mm；
|
||
4. 最大姿态误差不超过 0.005 rad；
|
||
5. 不违反第四关节、URDF 关节位置和速度限制；
|
||
6. 不出现超过配置阈值的单周期关节跳变。
|
||
|
||
只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的
|
||
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. 实施边界与后续流程
|
||
|
||
标定完成后的顺序为：
|
||
|
||
1. 根据仿真结果形成正式 QP 修改规格；
|
||
2. 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
|
||
3. 在 `use_mock:=true` 下运行完整遥操作控制链路；
|
||
4. 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
|
||
5. 经过安全评审后，在真机上以低速、小范围方式复验；
|
||
6. 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。
|