7.5 KiB
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 失败恢复与笛卡尔参考状态提交
这是除目标函数外最关键的修复。当前风险流程为：
QP 失败
→ 关节指令保持不动
→ 笛卡尔目标历史仍向前更新
→ 下一周期误差进一步增大
→ 连续失败或恢复时突跳
修改后，QP 求解结果、关节目标和笛卡尔参考状态按同一周期提交。
QP 成功时：
QP 成功
→ 发送新关节目标
→ 关节目标发送成功
→ 提交新的笛卡尔参考状态
QP 失败或关节目标发送失败时：
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. 验证顺序与通过标准
实施按以下顺序进行：
- 先实现 QP 失败保持，以及关节目标与笛卡尔参考状态的成功后统一提交；
- 加入 J4 的 10°硬下限与 25°软缓冲区；
- 加入左右臂 J3 初始姿态软参考；
- 加入按六维最小奇异值激活的
both可操作度任务； - 在
use_mock:=true下运行初始位姿、前方 30～50 cm 采摘、本侧下方 40 cm 收集 筐和返回初始位姿的完整严格六维轨迹。
至少记录并比较修改前后的：QP 成功/失败周期数、连续失败长度、失败周期参考状态是否 保持不变、恢复时的关节跳变量、完整轨迹成功数、 六维最小奇异值、最大位置/姿态误差、J4 最小余量、最大关节速度以及目标历史与实际 TCP 的偏差。只有失败周期下降、完整轨迹成功率不降低、严格六维误差和全部安全约束 仍满足时，辅助项才保留；否则首先回退可操作度或 J4 软项，不回退失败状态修复。