193 lines
7.0 KiB
Markdown
193 lines
7.0 KiB
Markdown
# RM75 QP 收敛与低跟随稳定性优化设计
|
||
|
||
## 背景
|
||
|
||
右臂真机以90 Hz、`follow: false`运行时，用户观察到：
|
||
|
||
- 手柄移动约10 cm后，`target_pose`很快稳定；
|
||
- `current_pose`仍需约3秒缓慢追赶；
|
||
- 运动过程中机械臂存在肉眼可见的轻微晃动。
|
||
|
||
现场 timing 日志同时表明：
|
||
|
||
- 控制周期约11.111 ms；
|
||
- 控制回调平均约2.6 ms，最大约6.0 ms；
|
||
- QP平均约0.39 ms；
|
||
- CANFD发送平均约0.21 ms；
|
||
- UDP实际关节反馈平均约25 ms一帧，即约40 Hz。
|
||
|
||
因此，控制线程、QP单次计算和CANFD调用本身没有耗尽90 Hz周期；慢速发生在
|
||
`target_pose`生成之后。
|
||
|
||
## 根因
|
||
|
||
当前 `PlacoIkSolver.solve()` 每次只调用一次：
|
||
|
||
```python
|
||
self._solver.solve(True)
|
||
```
|
||
|
||
该调用把一次QP增量应用为 `q + Δq`。与此同时，90 Hz控制循环每次都会先用
|
||
最新实际关节反馈重置Placo模型。由于实际反馈约40 Hz，同一帧反馈通常会被重复
|
||
使用两到三次。
|
||
|
||
结果是每次下发的关节目标只位于实际关节角前方一小步，而不是当前TCP目标对应的
|
||
收敛关节解。低跟随控制器持续追逐这个短距离移动点，表现为：
|
||
|
||
- 对稳定TCP目标呈缓慢的渐近追赶；
|
||
- 实际反馈每约25 ms更新一次时，关节目标随反馈发生台阶式修正；
|
||
- 低跟随内部平滑与台阶式关节目标叠加，形成轻微晃动。
|
||
|
||
本地RM75模型对照结果支持该判断：从右臂初始姿态求解7 cm平移目标时，单次QP
|
||
只产生约7.6 mm TCP位移；在同一次逆解中连续迭代30次后，目标误差可降至接近
|
||
零，计算耗时约3.56 ms。
|
||
|
||
## 目标
|
||
|
||
保持现有安全基线并实现：
|
||
|
||
- 手柄移动10 cm后，机械臂约1秒内稳定到位；
|
||
- 运动和到位后无持续肉眼可见晃动；
|
||
- 控制频率保持90 Hz；
|
||
- `rm_movej_canfd()`保持低跟随；
|
||
- 运行时仍以UDP `joint_position`作为实际关节反馈；
|
||
- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度与关节加速度限制；
|
||
- 保留反馈超时、CANFD错误恢复、Grip重新使能和安全停止逻辑。
|
||
|
||
## 不在本次范围
|
||
|
||
- 不启用高跟随；
|
||
- 不提高TCP或关节安全上限；
|
||
- 不修改XR手柄滤波和坐标映射；
|
||
- 不修改UDP反馈周期或增加反馈预测器；
|
||
- 不新增线程、RealMan连接、依赖或状态机；
|
||
- 不处理双臂碰撞检测。
|
||
|
||
## 方案比较
|
||
|
||
### 方案一：有限次数迭代QP
|
||
|
||
每个控制周期仍从实际关节角开始，但在一次 `solve()` 调用内部迭代QP，直到TCP
|
||
目标收敛或达到固定迭代上限。得到的完整关节目标继续经过现有关节速度与加速度
|
||
限幅后才发送。
|
||
|
||
优点：
|
||
|
||
- 直接修复单步QP只生成近距离移动点的根因；
|
||
- 不需要预测状态，不会在反馈中断时继续外推；
|
||
- 复用现有限速、错误回退和CANFD发送路径；
|
||
- 本地测量表明计算量可放入90 Hz周期。
|
||
|
||
缺点：
|
||
|
||
- 单周期QP耗时会高于当前单步求解；
|
||
- 不可达目标需要明确的未收敛处理。
|
||
|
||
### 方案二：反馈帧之间维护预测关节状态
|
||
|
||
仅在新UDP反馈到达时校正模型，其余90 Hz周期从上一条关节命令继续积分QP。
|
||
|
||
优点：
|
||
|
||
- 每周期仍只求解一次QP；
|
||
- 可避免同一反馈帧反复重置模型。
|
||
|
||
缺点：
|
||
|
||
- 引入预测状态、反馈校正和漂移处理；
|
||
- 反馈与预测偏差可能在校正时产生新的关节跳动；
|
||
- 超时与恢复逻辑需要同时管理实际状态和预测状态。
|
||
|
||
### 方案三：只调整滤波、速度或高跟随参数
|
||
|
||
`target_pose`已经快速稳定，继续提高 `max_linear_speed` 或减小目标滤波不能解决
|
||
下游渐近追赶。启用高跟随则违反本次低跟随约束。
|
||
|
||
## 决策
|
||
|
||
采用方案一。它在不引入预测状态的情况下直接修复根因，改动范围只涉及Placo
|
||
求解器及其测试。
|
||
|
||
## 控制数据流
|
||
|
||
正常运行时的数据流调整为：
|
||
|
||
```text
|
||
UDP实际关节反馈
|
||
→ 更新Placo实际关节状态和current_pose
|
||
→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
|
||
→ 有限次数迭代QP，得到收敛关节目标
|
||
→ 现有关节速度与加速度限幅
|
||
→ rm_movej_canfd(..., follow=false)
|
||
```
|
||
|
||
反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续
|
||
超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。
|
||
|
||
## QP迭代规则
|
||
|
||
`PlacoIkSolver.solve()`按以下规则执行：
|
||
|
||
1. 校验目标变换。
|
||
2. 记录本次内部迭代前的关节状态。
|
||
3. 调用一次 `self._solver.solve(True)`。
|
||
4. 更新Placo运动学。
|
||
5. 校验本次候选关节状态：
|
||
- 7个有限数值；
|
||
- 不违反RM75关节位置限制；
|
||
- 本次数值迭代步长不超过Placo按 `dt=1/90` 应用的URDF关节速度限制。
|
||
6. 使用位置任务与姿态任务的 `error_norm()`检查收敛：
|
||
- 位置误差不超过1 mm；
|
||
- 姿态误差不超过0.005 rad。
|
||
7. 未收敛则继续迭代，最多30次。
|
||
|
||
30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
|
||
`_solve_joint_target()`错误路径，在终端限频打印QP失败原因，并保持上一组安全
|
||
关节目标。
|
||
|
||
内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
|
||
`_limit_joint_command_step()`，因此每个90 Hz真实命令继续满足现有
|
||
`joint_max_speed` 和 `joint_max_acc`。
|
||
|
||
## 晃动抑制
|
||
|
||
本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：
|
||
|
||
1. QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
|
||
2. 最终关节目标由现有关节速度与加速度限幅器生成连续90 Hz命令。
|
||
|
||
若真机仍存在晃动，再根据“目标关节角与实际关节角误差”追加诊断；本次不预先
|
||
引入预测器或额外滤波。
|
||
|
||
## 性能与安全验收
|
||
|
||
自动验证：
|
||
|
||
- 7 cm可达TCP平移目标在一次 `solve()` 后位置误差不超过1 mm；
|
||
- 姿态误差满足0.005 rad阈值；
|
||
- 非法结果和未收敛目标继续触发现有安全回退；
|
||
- 关节命令速度与加速度限幅测试继续通过；
|
||
- `xr_rm_teleop`全部pytest通过；
|
||
- `colcon build --symlink-install`通过；
|
||
- `arm_debug.launch.py arm:=right use_mock:=true`正常启动。
|
||
|
||
真机由用户验证：
|
||
|
||
- 手柄快速移动10 cm并保持不动，机械臂约1秒内稳定；
|
||
- 无持续肉眼可见晃动；
|
||
- 连续四个5秒 timing 窗口中 `total` 最大值低于11.111 ms；
|
||
- 无QP失败、反馈超时、CANFD错误或故障锁存日志；
|
||
- 松开Grip后仍立即退出遥操作并执行安全停止。
|
||
|
||
若单周期 `total` 达到或超过11.111 ms，停止真机运动并降低最大QP迭代次数，
|
||
不得通过提高控制频率或关闭安全检查规避计算超时。
|
||
|
||
## 文件范围
|
||
|
||
- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`。
|
||
- 修改 `xr_rm_teleop/test/test_placo_transforms.py`。
|
||
- 如现有QP失败测试需要补充未收敛原因断言，只精确修改
|
||
`xr_rm_teleop/test/test_joint_control.py`。
|
||
|
||
不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
|