yunxiao.zhu
d120ef2a39
* 将 speedRatio 前移到规划/准备阶段，运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
461 lines
17 KiB
Markdown
461 lines
17 KiB
Markdown
# 飞拍任务 `speedRatio` 执行侧调速设计
|
||
|
||
## 1. 背景
|
||
|
||
当前系统同时存在两个速度相关参数：
|
||
|
||
- `planning_speed_scale`
|
||
- 规划阶段参数。
|
||
- 作用是让规划结果在时长与整体动力学上更接近现场旧系统或当前配置要求。
|
||
- 启动后固定，不希望在运行中再修改语义。
|
||
- `speedRatio`
|
||
- 运行阶段参数。
|
||
- 期望成为统一在线调速入口。
|
||
- 本次范围内只要求对“下一次开始执行的飞拍任务”生效，不要求对正在执行中的任务中途热切换。
|
||
|
||
本次任务已经明确收敛：
|
||
|
||
- 只修改飞拍任务链路。
|
||
- 不修改 `move_joint`。
|
||
- 不通过放宽阈值、关闭校验或只看平均速度来规避问题。
|
||
|
||
## 2. 现状问题
|
||
|
||
当前飞拍执行链路的核心语义是：
|
||
|
||
1. 规划层先生成 `DenseJointTrajectory`。
|
||
2. 运行时保持 `8ms` 物理发送周期不变。
|
||
3. 对第 `k` 个发送点，按 `trajectoryTime = sendTime * speedRatio` 回映射到规划轨迹时间。
|
||
4. 再在原始稠密轨迹上做线性插值，得到实际要发的关节目标。
|
||
5. 最后再用离散差分检查 `vel/acc/jerk`。
|
||
|
||
该结构在 `speedRatio < 1` 时有天然风险：
|
||
|
||
- 物理发送周期固定为 `8ms`，但轨迹时间推进被压缩。
|
||
- 线性插值会把规划轨迹中原本“看起来连续”的局部变化映射成新的离散点列。
|
||
- 这些新点列在三阶后向差分上容易形成尖峰。
|
||
- 该风险在 `speedRatio = 0.5` 时已经暴露为明显的 Jerk 超限告警。
|
||
|
||
问题本质不是“平均速度是否合理”，而是：
|
||
|
||
- 最终 `8ms` 发送点列本身没有先被当作一等公民建模。
|
||
- `speedRatio` 的执行语义落在“发送前临场回采样”阶段。
|
||
- 离散约束校验发生得太晚，只能报错或事后分析。
|
||
|
||
## 3. 目标
|
||
|
||
本设计目标如下：
|
||
|
||
- 保持 `planning_speed_scale` 只属于规划层。
|
||
- `speedRatio` 是运行时统一执行倍率，可影响下一次启动的飞拍、普通轨迹和 `MoveJoint`。
|
||
- 飞拍链路在任意有效 `speedRatio` 下，最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
|
||
- 不依赖“插值后自然会平滑”的经验假设。
|
||
- 当请求 `speedRatio` 下的最终发送队列不满足约束时，拒绝执行并输出超限诊断；不在执行侧偷偷改写本次请求倍率。
|
||
- 不破坏现有 `shot timeline / sample offset` 语义。
|
||
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
|
||
|
||
## 4. 非目标
|
||
|
||
本次设计明确不包含以下内容：
|
||
|
||
- 不恢复旧的发送阶段临场回采样机制；普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段折算。
|
||
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
|
||
- 不修改 `planning_speed_scale` 的含义。
|
||
- 不在任务执行中途支持 `speedRatio` 热切换。
|
||
- 不通过调大容差、降低限值检查强度或直接跳过 Jerk 检查来“解决问题”。
|
||
|
||
## 5. 设计总览
|
||
|
||
飞拍执行链路调整为“两段式”：
|
||
|
||
1. 规划段
|
||
- 维持当前实现。
|
||
- 仍由 `planning_speed_scale` 参与规划时长和规划层轨迹生成。
|
||
- 输出 `TrajectoryResult.DenseJointTrajectory`、`TriggerTimeline`、`ShotEvents` 等规划结果。
|
||
2. 执行准备段
|
||
- 新增“飞拍执行侧最终发送序列生成器”。
|
||
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
|
||
- 生成最终 `8ms` 发送队列。
|
||
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
|
||
- 如果不通过，则拒绝执行，提示降低 `planning_speed_scale` 重新规划或显式设置更低的 `speedRatio`。
|
||
3. 发送段
|
||
- 运行时只消费已经准备好的最终 `8ms` 队列。
|
||
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
|
||
|
||
核心变化是：
|
||
|
||
- `speedRatio` 不再直接驱动“发送前临场插值”。
|
||
- `speedRatio` 改为驱动“执行侧最终队列构建”。
|
||
- 最终队列一旦生成并校验通过，运行时只负责按 J519 状态包节奏出队发送。
|
||
|
||
## 6. 统一语义
|
||
|
||
本次以飞拍任务为主线，但 `speedRatio` 允许作为运行时统一执行倍率影响普通轨迹和 `MoveJoint`，需要把三类入口的边界说清楚，避免再次混淆规划倍率和执行倍率。
|
||
|
||
### 6.1 `planning_speed_scale`
|
||
|
||
- 只影响规划结果。
|
||
- 影响 `DenseJointTrajectory` 的时间轴与规划层速度/加速度/Jerk 使用方式。
|
||
- 飞拍开始执行前就已经固定。
|
||
- 在线设置 `speedRatio` 时不应反向修改它。
|
||
|
||
### 6.2 `speedRatio`
|
||
|
||
- 影响下一次启动的运动任务，不在任务执行中途热切换。
|
||
- 对飞拍任务，表示用户期望的执行层时间推进速度，可前移到规划/执行准备阶段生成最终发送队列。
|
||
- 对普通轨迹和 `MoveJoint`，允许在规划或临时 PTP 轨迹生成阶段折算进有效关节限位，使运行时只消费已经生成好的 `8ms` 稠密点列。
|
||
- `speedRatio = 0.8` 表示执行节奏接近把同几何轨迹时间轴整体拉长 `1 / 0.8`。
|
||
- 例如 `planning_speed_scale = 0.9` 且 `speedRatio = 0.8` 时，最终执行节奏应接近同几何轨迹在 `planning_speed_scale ≈ 0.72` 下的结果。
|
||
|
||
也就是说：
|
||
|
||
- `speedRatio` 是确定的执行倍率，不是校验失败后的自动搜索变量。
|
||
- `finalSpeedRatio` 正常必须等于请求值。
|
||
- 若该倍率下最终队列超限，问题暴露给调用方，而不是由执行侧自动改成另一个倍率。
|
||
|
||
## 7. 最终发送序列生成
|
||
|
||
### 7.1 新增执行侧生成器
|
||
|
||
建议新增一个飞拍专用生成器，命名可类似：
|
||
|
||
- `FlyshotExecutionSendSequenceBuilder`
|
||
- 或 `FlyshotExecutionQueueBuilder`
|
||
|
||
职责：
|
||
|
||
- 输入规划层连续 `PlannedTrajectory`，优先使用经过执行侧定时处理后的 `ExecutionTrajectory`。
|
||
- 输入 `servoPeriodSeconds`、`speedRatio`。
|
||
- 输出最终真实发送的 `8ms` 点列。
|
||
- 输出与该点列一致的触发绑定结果。
|
||
- 输出请求倍率、最终倍率和历史倍率改写次数等诊断信息；当前语义下最终倍率等于请求倍率、改写次数为 0。
|
||
|
||
### 7.2 最终请求倍率队列
|
||
|
||
最终队列按固定时间轴语义生成：
|
||
|
||
- `sendTime = sampleIndex * 0.008`
|
||
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
|
||
- 关节目标从连续样条直接取点，并从 `rad` 转为 J519 需要的 `deg`
|
||
|
||
该队列就是本次请求倍率下的最终发送队列，后续只做校验和触发绑定，不再进行二次滤波或倍率搜索。
|
||
|
||
### 7.3 校验失败策略
|
||
|
||
当最终队列的离散 `vel/acc/jerk` 校验失败时：
|
||
|
||
- 不修改规划层轨迹。
|
||
- 不修改 `planning_speed_scale`。
|
||
- 不放宽校验阈值。
|
||
- 不降低本次 `speedRatio`。
|
||
- 直接拒绝执行并保留首个超限诊断。
|
||
|
||
这样可以避免把 `speedRatio` 误实现成安全优化器。现场若希望从 `planning_speed_scale = 0.9` 的 10s 轨迹降到接近 `planning_speed_scale = 0.7` 的执行效果，应显式设置类似 `speedRatio = 0.8`，而不是让系统在失败后自动猜一个更低倍率。
|
||
|
||
## 8. 逐周期约束校验
|
||
|
||
最终发送队列必须在执行前通过离散检查。
|
||
|
||
### 8.1 校验对象
|
||
|
||
校验对象必须是最终真实发送的 `8ms` 点列，而不是：
|
||
|
||
- 规划层原始稠密轨迹，
|
||
- 中间候选插值轨迹，
|
||
- 或平均时长推导。
|
||
|
||
### 8.2 校验指标
|
||
|
||
保留现有逐周期离散差分口径：
|
||
|
||
- 速度
|
||
- 加速度
|
||
- Jerk
|
||
|
||
并继续使用当前机器人模型和运行时限值：
|
||
|
||
- `velocity_eff = velocity_base`
|
||
- `acceleration_eff = acceleration_base * acc_limit`
|
||
- `jerk_eff = jerk_base * jerk_limit`
|
||
|
||
### 8.3 校验失败的处理
|
||
|
||
飞拍链路按当前修正语义采用“确定倍率 + 失败拒绝”：
|
||
|
||
- 若请求倍率下失败，立即拒绝执行。
|
||
- 不进行执行侧自动倍率搜索。
|
||
- 调用方可以选择降低 `planning_speed_scale` 重新规划，或显式设置更低的 `speedRatio` 后再次执行。
|
||
|
||
拒绝执行时必须输出：
|
||
|
||
- 首次失败的轴
|
||
- 时间窗
|
||
- 指标类型
|
||
- `actual / limit / ratio`
|
||
- 请求 `speedRatio`
|
||
- 当前 `finalSpeedRatio`，正常等于请求值
|
||
|
||
## 9. 触发时序与绑定
|
||
|
||
### 9.1 保持现有语义
|
||
|
||
以下语义不变：
|
||
|
||
- `TriggerTimeline`
|
||
- `ShotEvents`
|
||
- `TriggerSampleIndexOffsetCycles`
|
||
- `io_keep_cycles`
|
||
|
||
### 9.2 绑定对象改为最终发送队列
|
||
|
||
当前实现中，触发通常绑定到由回采样生成的 `samples`。
|
||
|
||
本次改为：
|
||
|
||
- 先生成最终发送队列。
|
||
- 再用该最终队列做 `TriggerSampleBinder.Bind(...)`。
|
||
|
||
这样可以保证：
|
||
|
||
- 触发绑定和真实发送完全一致。
|
||
- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。
|
||
- 触发点可被精确追溯到本次请求倍率下的最终发送点索引。
|
||
|
||
### 9.3 关于触发数量与顺序
|
||
|
||
请求倍率改变最终发送时长时：
|
||
|
||
- 触发数量不能变。
|
||
- 触发顺序不能变。
|
||
- 触发在最终发送队列中的绑定索引应随时间轴拉长后移或保持可解释等价。
|
||
|
||
## 10. 运行时职责调整
|
||
|
||
本次需要显式收紧 `FanucControllerRuntime` 的职责。
|
||
|
||
### 10.1 改造前
|
||
|
||
`FanucControllerRuntime.SendDenseTrajectory(...)` 当前同时负责：
|
||
|
||
- 读取 `DenseJointTrajectory`
|
||
- 根据 `_speedRatio` 回采样
|
||
- 绑定触发
|
||
- 生成命令队列
|
||
- 写出 `ActualSend*` 工件
|
||
|
||
### 10.2 改造后
|
||
|
||
`FanucControllerRuntime` 只负责：
|
||
|
||
- 接收已经准备好的最终发送队列
|
||
- 接收与该队列一致的触发绑定结果
|
||
- 预构建命令队列并交给 J519 客户端
|
||
- 记录真实发送过程与导出工件
|
||
|
||
不再负责：
|
||
|
||
- 按 `_speedRatio` 从 `DenseJointTrajectory` 临场生成发送点
|
||
|
||
这样做的目的是让：
|
||
|
||
- 运行时负责发送。
|
||
- 执行准备层负责离散稳定性。
|
||
|
||
两层边界更清晰，也更利于测试。
|
||
|
||
## 11. 导出工件与日志
|
||
|
||
### 11.1 旧规划导出和实发诊断必须分开
|
||
|
||
`saveTrajectory` 目录中存在两类不同用途的文件，不能再混成同一条时间轴：
|
||
|
||
第一类是旧格式规划导出，用于和旧 RVBUST 导出的规划轨迹、离线分析脚本做对比。它们只受 `planning_speed_scale` 和规划/整形参数影响，不受运行时 `speedRatio` 影响：
|
||
|
||
- `JointTraj.txt`
|
||
- `JointDetialTraj.txt`
|
||
- `CartTraj.txt`
|
||
- `CartDetialTraj.txt`
|
||
- `JointDetialTraj.analysis.txt`
|
||
|
||
第二类是实发诊断，用于观察当前 `speedRatio` 下真实准备发送的 8ms 队列和触发绑定。它们必须全部从最终发送队列生成：
|
||
|
||
- `ActualSendJointTraj.txt`
|
||
- `ActualSendTiming.txt`
|
||
- `ActualSendJerkStats.txt`
|
||
- `ShotEvents.json`
|
||
|
||
不允许再出现：
|
||
|
||
- 旧规划导出被运行时 `speedRatio` 改写，
|
||
- `ActualSend*` 基于一套样本，
|
||
- 真实发送队列又基于另一套样本。
|
||
|
||
### 11.2 推荐新增日志字段
|
||
|
||
每次飞拍执行建议至少记录：
|
||
|
||
- 请求 `speedRatio`
|
||
- 规划层轨迹时长
|
||
- 最终发送队列点数
|
||
- 最终发送队列是否通过校验
|
||
- 若失败，首个失败窗口的：
|
||
- 关节轴
|
||
- 时间区间
|
||
- 指标类型
|
||
- `actual`
|
||
- `limit`
|
||
- `ratio`
|
||
- 倍率改写次数，当前正常为 0
|
||
- 最终采用的执行时长
|
||
- 最终发送队列点数
|
||
- 最终触发绑定数量
|
||
- 最终校验通过结论
|
||
|
||
### 11.3 日志级别建议
|
||
|
||
- `Information`
|
||
- 记录执行请求、最终采用结果、最终通过结论
|
||
- `Warning`
|
||
- 记录请求倍率下最终发送队列校验失败
|
||
- `Debug`
|
||
- 记录最终发送队列的时间映射与详细差分统计
|
||
|
||
## 12. 验收口径
|
||
|
||
### 12.1 功能验收
|
||
|
||
- `speedRatio` 可在线设置。
|
||
- 该值对下一次启动的飞拍、普通轨迹或 `MoveJoint` 生效。
|
||
- 不需要修改 `planning_speed_scale`。
|
||
|
||
### 12.2 约束验收
|
||
|
||
在 `speedRatio = {1.0, 0.8, 0.5}` 下：
|
||
|
||
- 最终发送队列逐周期 `vel/acc/jerk` 不超限。
|
||
- 不出现固定区间正负交替的大幅 Jerk 尖峰。
|
||
|
||
### 12.3 工件与日志验收
|
||
|
||
- `JointTraj.txt / JointDetialTraj.txt / CartTraj.txt / CartDetialTraj.txt` 不随运行时 `speedRatio` 改变。
|
||
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
|
||
- 日志能定位请求倍率、最终发送队列和校验失败窗口。
|
||
|
||
### 12.4 回归验收
|
||
|
||
- `speedRatio = 1.0` 时不退化。
|
||
- 触发事件数量不异常漂移。
|
||
- 触发绑定顺序不异常漂移。
|
||
|
||
## 13. 测试方案
|
||
|
||
### 13.1 单元测试
|
||
|
||
建议新增：
|
||
|
||
- 飞拍执行侧最终发送序列生成器测试
|
||
- `speedRatio` 非法边界值测试
|
||
- 请求倍率下队列超限时拒绝执行测试
|
||
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
|
||
- `planning_speed_scale = 0.9` 加 `speedRatio = 0.8` 的执行时间轴等价于约 `0.72` 规划倍率的回归测试
|
||
- 普通轨迹和 `MoveJoint` 在倍率乘积相同时生成等价或可解释等价的稠密执行轨迹
|
||
- 触发绑定始终与最终发送队列一致的测试
|
||
|
||
### 13.2 编排测试
|
||
|
||
建议补充：
|
||
|
||
- `ExecuteFlyShotTraj` 进入运行时前，已经拿到最终发送队列
|
||
- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样
|
||
- `ExecuteTrajectory` 和 `MoveJoint` 的调速在规划/生成阶段完成，运行时不再做临场倍率重采样
|
||
- `SaveTrajectoryInfo` 的旧格式规划导出不随运行时 `speedRatio` 改变，而 `ActualSend*` 仍随运行时 `speedRatio` 改变
|
||
|
||
### 13.3 集成与黄金样本
|
||
|
||
建议至少覆盖：
|
||
|
||
- `UTTC_MS11`
|
||
- `speedRatio = 1.0`
|
||
- `speedRatio = 0.8`
|
||
- `speedRatio = 0.5`
|
||
|
||
检查项：
|
||
|
||
- 最终发送点数
|
||
- 最终发送时长
|
||
- `ActualSendTiming`
|
||
- `ActualSendJointTraj`
|
||
- `ActualSendJerkStats`
|
||
- `ShotEvents` 绑定结果
|
||
- 是否消除当前已知 Jerk 尖峰
|
||
|
||
## 14. 代码落点建议
|
||
|
||
建议实现边界如下：
|
||
|
||
### 14.1 `src/Flyshot.Core.Planning/Sampling/`
|
||
|
||
新增飞拍执行侧最终发送序列生成器及相关结果类型，负责：
|
||
|
||
- 候选队列构建
|
||
- 离散校验
|
||
- 触发绑定输入准备
|
||
|
||
### 14.2 `src/Flyshot.ControllerClientCompat/`
|
||
|
||
在飞拍执行准备阶段调用该生成器，把最终发送队列放入执行结果或新的执行上下文对象。
|
||
普通轨迹规划入口和 `MoveJoint` 临时轨迹生成入口可以读取当前 `speedRatio`，并在进入运行时前折算成已调速的稠密轨迹。
|
||
|
||
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||
|
||
调整 `SendDenseTrajectory(...)`：
|
||
|
||
- 不再自行根据 `_speedRatio` 从 `DenseJointTrajectory` 临场重采样
|
||
- 直接消费已经构建好的最终发送队列
|
||
|
||
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
|
||
|
||
`FlyshotTrajectoryArtifactWriter` 接收两份视图：
|
||
|
||
- 旧格式规划导出视图：写 `JointTraj.txt`、`JointDetialTraj.txt`、`CartTraj.txt`、`CartDetialTraj.txt`
|
||
- 实发诊断视图：写 `ActualSendJointTraj.txt`、`ActualSendTiming.txt`、`ActualSendJerkStats.txt`、`ShotEvents.json`
|
||
|
||
### 14.5 `tests/Flyshot.Core.Tests/`
|
||
|
||
补飞拍专项单测、UTTC 回归和工件一致性测试。
|
||
|
||
## 15. 风险与注意事项
|
||
|
||
### 15.1 不能把 speedRatio 做成安全优化器
|
||
|
||
对外语义必须保持：
|
||
|
||
- 用户请求的是某个 `speedRatio`
|
||
- 系统按这个 `speedRatio` 构建最终发送队列
|
||
- 若队列超限，则拒绝执行并要求调用方显式选择新的规划倍率或执行倍率
|
||
|
||
不能因为校验失败就在执行侧自动把 `0.8` 改成 `0.2`；这会让 `speedRatio` 和 `planning_speed_scale` 的关系失去可解释性。
|
||
|
||
### 15.2 不能让触发绑定脱离最终队列
|
||
|
||
只要最终发送点数变化，触发绑定就必须重新基于最终队列计算，否则工件与执行会再次分叉。
|
||
|
||
### 15.3 `speedRatio = 1.0` 必须优先做回归保护
|
||
|
||
这是最容易被“执行侧重构”误伤的路径，必须作为首要回归项。
|
||
|
||
## 16. 结论
|
||
|
||
本次飞拍调速设计的核心结论是：
|
||
|
||
- `planning_speed_scale` 继续只属于规划层。
|
||
- `speedRatio` 是运行时统一执行倍率，并且只对下一次启动的运动任务生效。
|
||
- 普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段，运行时只发送已生成好的 `8ms` 点列。
|
||
- 飞拍执行必须先生成最终 `8ms` 发送队列，再对该队列做逐周期 `vel/acc/jerk` 校验。
|
||
- `speedRatio` 按 `trajectoryTime = sendTime * speedRatio` 确定性拉长执行时间轴，`finalSpeedRatio` 不应被自动改写。
|
||
- 若请求倍率下最终队列不满足约束，则拒绝执行并提示降低 `planning_speed_scale` 重新规划或显式设置更低 `speedRatio`。
|
||
- 运行时不再对飞拍轨迹做发送前临场回采样。
|
||
- 旧格式规划导出必须保持不受运行时 `speedRatio` 影响；`ActualSend*` 和 `ShotEvents.json` 必须统一到真实发送队列上。
|
||
|
||
该方案可以在不放宽校验阈值、不中断现有触发语义的前提下，把 `speedRatio` 调速从“发送阶段经验插值”收敛为“规划/准备阶段完成、运行时确定发送”的执行机制。
|