7.0 KiB
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() 每次只调用一次：
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 求解器及其测试。
控制数据流
正常运行时的数据流调整为：
UDP实际关节反馈
→ 更新Placo实际关节状态和current_pose
→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
→ 有限次数迭代QP，得到收敛关节目标
→ 现有关节速度与加速度限幅
→ rm_movej_canfd(..., follow=false)
反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续 超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。
QP迭代规则
PlacoIkSolver.solve()按以下规则执行：
- 校验目标变换。
- 记录本次内部迭代前的关节状态。
- 调用一次
self._solver.solve(True)。 - 更新Placo运动学。
- 校验本次候选关节状态：
- 7个有限数值；
- 不违反RM75关节位置限制；
- 本次数值迭代步长不超过Placo按
dt=1/90应用的URDF关节速度限制。
- 使用位置任务与姿态任务的
error_norm()检查收敛：- 位置误差不超过1 mm；
- 姿态误差不超过0.005 rad。
- 未收敛则继续迭代，最多30次。
30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
_solve_joint_target()错误路径，在终端限频打印QP失败原因，并保持上一组安全
关节目标。
内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
_limit_joint_command_step()，因此每个90 Hz真实命令继续满足现有
joint_max_speed 和 joint_max_acc。
晃动抑制
本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：
- QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
- 最终关节目标由现有关节速度与加速度限幅器生成连续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、依赖或公开入口。