每周期单步QP改为有界迭代QP

2026-07-30 09:49:39 +08:00
parent 6d22d5600a
commit 2c128c1f54
5 changed files with 932 additions and 7 deletions
@@ -0,0 +1,99 @@
+# RM75 关节命令提前制动设计
+
+## 背景
+
+右臂真机保持 90 Hz 和 `follow: false`。有界迭代 QP 提高跟随速度后，手柄上下
+移动约 10 mm 时出现整臂剧烈晃动，并且到达目标后仍持续振荡。
+
+现场日志表明控制计算未超时，但存在少量 QP 未收敛警告。离线检查确认：
+
+- 手柄上下移动按现有映射对应机器人 X 方向；
+- 当前初始姿态的关节雅可比条件数约为 116，该方向的逆解对关节运动较敏感；
+- 机器人 X 方向 10 mm 的收敛逆解可能包含最大约 17° 的关节变化；
+- 现有关节命令限幅器只限制速度和加速度，没有根据剩余距离提前制动。
+
+固定 10° 关节目标的离线复现中，现有限幅器运行 3 秒后仍处于约 10.56°、
+-20°/s，证明稳定目标本身也会被反复越过。这与真机“到位后继续晃动”的现象
+一致。
+
+## 目标
+
+- 关节目标稳定后，90 Hz 关节命令提前减速并停止在目标上；
+- 不再因命令限幅器反复越过目标而持续振荡；
+- 保留当前较快的有界迭代 QP；
+- 保留现有关节最大速度和最大加速度限制；
+- 保持 `rm_movej_canfd(..., follow=false)`；
+- 不改变 UDP 反馈、超时保持、CANFD 错误恢复和安全停止行为。
+
+## 不在本次范围
+
+- 不修改 QP 迭代次数、收敛阈值或失败回退；
+- 不修改初始姿态、XR 坐标映射或姿态控制；
+- 不增加奇异点阻尼、预测器、新线程、新依赖或新 ROS 参数；
+- 不修改三份机械臂 YAML、RealMan 适配器、launch 或 UI；
+- 不连接真机，不由 Codex 发送运动命令。
+
+若修复制动后机械臂运动已经平稳，但上下运动仍伴随不可接受的整臂大幅构型变化，
+再单独设计奇异点处理；本次不把两个问题混在同一改动中。
+
+## 方案
+
+只修改 `SingleArmVelocityTeleop._limit_joint_command_step()`。
+
+现有逻辑在目标仍位于运动方向前方时持续加速，只有到达或越过目标后才开始反向
+减速。新逻辑对每个关节使用同一组现有状态：
+
+- 上一次已发送的关节目标；
+- 上一次关节命令速度；
+- 当前 QP 关节目标；
+- 现有关节最大速度、最大加速度和控制周期。
+
+每周期按以下规则生成命令：
+
+1. 计算关节剩余距离和单周期最大速度变化
+   `velocity_step = max_acceleration * dt`。
+2. 根据 90 Hz 离散积分规则，计算“本周期再加速一次、随后以最大允许减速度制动”
+   所需的总距离。
+3. 若关节正在朝目标运动，并且剩余距离已经不大于该制动距离，则本周期开始减速；
+   否则继续朝目标加速，但不超过现有最大速度。
+4. 使用 `velocity_step` 限制本周期速度变化，保持现有加速度上限。
+5. 使用新速度积分得到本周期关节目标。
+6. 当关节已经停下且剩余距离不超过一个最大加速度位移
+   `max_acceleration * dt²` 时，在不违反单周期加速度限制的前提下精确落到目标，
+   避免离散步长形成极小往复振荡。
+
+该逻辑只负责命令轨迹制动，不改变 QP 输出。对于持续移动的目标，若目标突然越过
+当前命令位置，控制器仍优先遵守加速度限制，以最大允许减速度反向；不会为禁止
+瞬时越界而跳变速度。
+
+## 安全行为
+
+- 每周期命令速度绝对值不超过 `joint_max_speed`；
+- 相邻周期间速度变化不超过 `joint_max_acc * dt`；
+- 输出继续校验 NaN 和 Inf；
+- QP 异常仍保持上一组安全目标并在终端限频打印警告；
+- UDP 短暂超时仍以 90 Hz 重发最后一次已限速目标，不运行 QP；
+- UDP 持续超时、CANFD 错误、Grip 重新使能和安全停止逻辑保持不变。
+
+## 测试
+
+先增加失败测试，再修改实现：
+
+1. 对固定 10° 关节目标连续运行限幅器，验证命令不越过目标并最终停止；
+2. 在整个序列中验证速度不超过现有上限；
+3. 验证相邻命令速度变化不超过现有加速度上限；
+4. 保留现有“从静止开始限制首周期加速度”测试；
+5. 运行 `xr_rm_teleop` 全部测试；
+6. 按项目规则运行 `colcon build --symlink-install`；
+7. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证 90 Hz、低跟随和启动路径。
+
+真机只由用户分阶段验证：先小位移、低风险姿态，确认不再到位后持续振荡，再逐步
+增加位移。若仍有明显整臂构型变化但不再振荡，应停止扩大位移并转入奇异点处理，
+不得通过提高速度、加速度或启用高跟随规避。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
+- 修改 `xr_rm_teleop/test/test_joint_control.py`；
+- 新增本中文设计文档；
+- 后续新增一份中文实施计划。