100 lines
4.7 KiB
Markdown
100 lines
4.7 KiB
Markdown
# 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`；
|
||
- 新增本中文设计文档；
|
||
- 后续新增一份中文实施计划。
|