176 lines
7.5 KiB
Markdown
176 lines
7.5 KiB
Markdown
# RM75 双臂采摘 QP 稳健性优化方案概述
|
||
|
||
## 1. 目标与边界
|
||
|
||
本方案面向当前双臂机器人从初始位姿向机器人公共坐标系 `+Y` 前方采摘，再移动到
|
||
本侧机械臂初始 TCP 正下方约 40 cm、位于底盘车上的收集筐这一流程。首要目标是：
|
||
|
||
- 保持手柄给出的 TCP 位置和姿态严格参与六维逆解；
|
||
- 减少奇异点附近的构型恶化、QP 不收敛和连续失败；
|
||
- QP 失败时保持上一安全关节解，并且不提交本周期笛卡尔参考状态；
|
||
- 保留现有工作空间、速度、加速度、指令超时和安全停止限制。
|
||
|
||
本轮不加入自动采摘状态机、自动放松姿态或真机自动运动，不取消现有安全限制。
|
||
|
||
此前离线扫描中所有候选均未通过完整轨迹硬门槛，因此不能把扫描得到的左臂 34°、
|
||
右臂 0°写成“最优 J3”。仿真只能说明：两臂 `q4 >= 10°` 均保持正余量，J4 的 10°
|
||
硬下限不是当次 QP 失败的直接原因。
|
||
|
||
## 2. 更新后的 QP 目标
|
||
|
||
主任务和辅助任务写为：
|
||
|
||
\[
|
||
\begin{aligned}
|
||
\min_{\Delta q}\quad
|
||
&\left\|J_p\Delta q-e_p\right\|_{W_p}^2
|
||
+\left\|J_R\Delta q-e_R\right\|_{W_R}^2 \\
|
||
&+\lambda\left\|\Delta q\right\|^2
|
||
-w_m\alpha_m(\sigma)\nabla m_6(q)^T\Delta q \\
|
||
&+w_3\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2 \\
|
||
&+w_4\alpha_4(q_4)
|
||
\left[q_{4,\mathrm{warn}}-(q_4+\Delta q_4)\right]_+^2，
|
||
\end{aligned}
|
||
\]
|
||
|
||
其中：
|
||
|
||
- `e = 目标位姿 - 当前位姿`，因此主任务使用 `JΔq - e`。如果误差定义相反，公式
|
||
才写成加号；当前 Placo 代码不翻转误差符号。
|
||
- 前两项是严格六维 TCP 位置和姿态任务，始终保持最高权重。
|
||
- `λ||Δq||²` 是现有动能正则，用于抑制过大的关节增量和数值抖动。
|
||
- `m6` 使用 Placo 支持的 `both` 类型六维可操作度；`αm` 只在完整六维雅可比的
|
||
最小奇异值进入预警区时逐渐激活，正常区域为零。
|
||
- J3 是低权重软引导，不属于可行性硬门槛。
|
||
- `[x]+ = max(0, x)`；J4 软项只在进入预警区后产生作用，提前远离 10° 硬下限。
|
||
|
||
辅助项不能通过提高权重来抵消六维 TCP 跟踪。第一版复用 Placo 现有任务接口，不
|
||
引入新的分层 QP 框架或外部依赖。
|
||
|
||
## 3. 四处修改
|
||
|
||
### 3.1 六维主任务、可操作度与数值迭代
|
||
|
||
严格六维 TCP 主任务保持不变，可操作度由“全程恒定启用的位置任务”改为“接近奇异
|
||
区才启用的六维任务”：
|
||
|
||
- `sigma_min >= sigma_warn`：`αm = 0`，不干扰正常遥操作；
|
||
- `sigma_stop < sigma_min < sigma_warn`：`αm` 从 0 平滑增加到 1；
|
||
- `sigma_min <= sigma_stop`：保持最大辅助权重，但仍不降低六维 TCP 权重。
|
||
|
||
`sigma_warn`、`sigma_stop` 和最大辅助权重先保留为仿真可调参数，根据现有完整轨迹
|
||
日志确定；不直接沿用此前效果不明显的恒定位置可操作度权重。
|
||
|
||
当前 30 次求解是同一目标的数值迭代，不是 30 个真实控制周期。生产控制链路仍由
|
||
现有关节速度和加速度限制器约束实际运动，因此不把“单个物理周期到达完整目标”作为
|
||
收敛要求。求解器逐次检查六维误差；只有达到当前位置和姿态阈值的结果才允许发送。
|
||
30 次内未收敛则恢复到本周期实际关节反馈，不发送未收敛的中间结果。
|
||
|
||
### 3.2 J3 初始姿态软参考
|
||
|
||
取消继续扫描 J3 最优角，使用当前 YAML 初始姿态作为第一版参考：
|
||
|
||
\[
|
||
q_{3,\mathrm{ref}}^L=67.96^\circ，\qquad
|
||
q_{3,\mathrm{ref}}^R=-89.57^\circ。
|
||
\]
|
||
|
||
J3 只使用低权重软任务，不设置 J3 硬限位，不因追踪参考角而放松 TCP 位姿任务。
|
||
代表性严格六维 mock 路径显示：左臂使用 `1e-5` 可完成路径，提高到 `1e-4` 会提前
|
||
触及关节限位；右臂使用 `1e-5` 时 J6 到达 URDF 下限，提高到 `1e-4` 后保留约
|
||
23° J6 余量并完成前伸段。因此第一版分别取：
|
||
|
||
\[
|
||
w_3^L=10^{-5}，\qquad w_3^R=10^{-4}。
|
||
\]
|
||
|
||
左右臂参数分别配置，后续只在完整 mock 轨迹明显改善时再调整，不把参考角本身当作
|
||
成功保证。
|
||
|
||
### 3.3 J4 硬下限与软缓冲区
|
||
|
||
左右臂暂时保持相同硬约束：
|
||
|
||
\[
|
||
q_4\geq q_{4,\min}=10^\circ。
|
||
\]
|
||
|
||
在硬下限上方增加预警区，第一版取 `q4_warn = 25°`：
|
||
|
||
- `q4 >= 25°`：J4 软项关闭；
|
||
- `10° < q4 < 25°`：软项随接近 10°逐渐增强；
|
||
- `q4 <= 10°`：由硬约束禁止继续向下。
|
||
|
||
这样保留收集筐下降阶段所需的可达空间，同时避免 QP 到达 10°附近才突然遇到约束
|
||
边界。`25°` 是待仿真验证的缓冲起点，不是新的硬下限；左右臂允许分别调整预警角，
|
||
但除非轨迹数据证明有必要，不增加更多参数。
|
||
|
||
### 3.4 QP 失败恢复与笛卡尔参考状态提交
|
||
|
||
这是除目标函数外最关键的修复。当前风险流程为：
|
||
|
||
```text
|
||
QP 失败
|
||
→ 关节指令保持不动
|
||
→ 笛卡尔目标历史仍向前更新
|
||
→ 下一周期误差进一步增大
|
||
→ 连续失败或恢复时突跳
|
||
```
|
||
|
||
修改后，QP 求解结果、关节目标和笛卡尔参考状态按同一周期提交。
|
||
|
||
QP 成功时：
|
||
|
||
```text
|
||
QP 成功
|
||
→ 发送新关节目标
|
||
→ 关节目标发送成功
|
||
→ 提交新的笛卡尔参考状态
|
||
```
|
||
|
||
QP 失败或关节目标发送失败时：
|
||
|
||
```text
|
||
QP 失败
|
||
→ 丢弃失败后的 Placo 内部迭代结果
|
||
→ 保持上一有效关节目标
|
||
→ 不提交本周期笛卡尔参考状态
|
||
→ 操作者把手柄移回可行区域后继续求解
|
||
```
|
||
|
||
“不提交笛卡尔参考状态”包括不更新本周期候选的滤波状态、
|
||
`_last_sent_target`、`_last_sent_orientation` 和命令时间。下一周期仍从上一已提交的
|
||
笛卡尔参考状态以及实际关节反馈出发计算，防止 QP 误差在机械臂不动时继续累积。
|
||
|
||
手柄原始输入仍正常接收，不会被程序改写，也不会自动改变操作者给出的末端姿态。
|
||
失败时机械臂不会为了恢复而自行移动；操作者主动将手柄移回可行区域后，QP 使用新的
|
||
手柄输入重新求解。收集筐到达和松开夹爪仍以实际 TCP 反馈及位置、姿态容差为判据。
|
||
|
||
## 4. 保留约束
|
||
|
||
QP 和下游控制继续保留：
|
||
|
||
- URDF 关节位置限制和 J4 的 10°额外硬下限；
|
||
- 现有关节速度、关节加速度、TCP 线速度和角速度限制；
|
||
- 工作空间/圆柱限位、指令超时和安全停止；
|
||
- `configure_safety_limits` 默认启用；
|
||
- `move_to_initial_pose_on_connect` 默认关闭；
|
||
- mock 模式不依赖睿尔曼真机 SDK。
|
||
|
||
## 5. 验证顺序与通过标准
|
||
|
||
实施按以下顺序进行：
|
||
|
||
1. 先实现 QP 失败保持，以及关节目标与笛卡尔参考状态的成功后统一提交；
|
||
2. 加入 J4 的 10°硬下限与 25°软缓冲区；
|
||
3. 加入左右臂 J3 初始姿态软参考；
|
||
4. 加入按六维最小奇异值激活的 `both` 可操作度任务；
|
||
5. 在 `use_mock:=true` 下运行初始位姿、前方 30～50 cm 采摘、本侧下方 40 cm 收集
|
||
筐和返回初始位姿的完整严格六维轨迹。
|
||
|
||
至少记录并比较修改前后的：QP 成功/失败周期数、连续失败长度、失败周期参考状态是否
|
||
保持不变、恢复时的关节跳变量、完整轨迹成功数、
|
||
六维最小奇异值、最大位置/姿态误差、J4 最小余量、最大关节速度以及目标历史与实际
|
||
TCP 的偏差。只有失败周期下降、完整轨迹成功率不降低、严格六维误差和全部安全约束
|
||
仍满足时，辅助项才保留；否则首先回退可操作度或 J4 软项，不回退失败状态修复。
|