✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制
* 新增 FlyshotExecutionSendSequenceBuilder，负责在运行时前构建最终 8ms 发送队列，并进行离散限幅校验。 * 引入 FlyshotPreparedExecution 类，封装最终发送结果及相关诊断信息。 * 调整 ControllerClientCompatService 和 FanucControllerRuntime，确保运行时直接使用预生成的发送队列，避免临场重采样。 * 更新 TrajectoryResult 和 PlannedExecutionBundle，支持准备好的执行队列。 * 增加单元测试，验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
This commit is contained in:
457
docs/flyshot-speedratio-execution-design-20260509.md
Normal file
457
docs/flyshot-speedratio-execution-design-20260509.md
Normal file
@@ -0,0 +1,457 @@
|
||||
# 飞拍任务 `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` 只作用于飞拍执行层，且仅对下一次启动的飞拍任务生效。
|
||||
- 任意有效 `speedRatio` 下，最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
|
||||
- 不依赖“插值后自然会平滑”的经验假设。
|
||||
- 当 `speedRatio = 0.5` 等较慢倍率导致候选点列不满足约束时，系统自动拉长执行时长，直到点列通过校验。
|
||||
- 不破坏现有 `shot timeline / sample offset` 语义。
|
||||
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
|
||||
|
||||
## 4. 非目标
|
||||
|
||||
本次设计明确不包含以下内容：
|
||||
|
||||
- 不修改 `move_joint` 的调速和生成机制。
|
||||
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
|
||||
- 不修改 `planning_speed_scale` 的含义。
|
||||
- 不在任务执行中途支持 `speedRatio` 热切换。
|
||||
- 不通过调大容差、降低限值检查强度或直接跳过 Jerk 检查来“解决问题”。
|
||||
|
||||
## 5. 设计总览
|
||||
|
||||
飞拍执行链路调整为“两段式”：
|
||||
|
||||
1. 规划段
|
||||
- 维持当前实现。
|
||||
- 仍由 `planning_speed_scale` 参与规划时长和规划层轨迹生成。
|
||||
- 输出 `TrajectoryResult.DenseJointTrajectory`、`TriggerTimeline`、`ShotEvents` 等规划结果。
|
||||
2. 执行准备段
|
||||
- 新增“飞拍执行侧最终发送序列生成器”。
|
||||
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
|
||||
- 生成最终 `8ms` 发送队列。
|
||||
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
|
||||
- 如果不通过，则自动拉长执行时长后重建队列，直到通过。
|
||||
3. 发送段
|
||||
- 运行时只消费已经准备好的最终 `8ms` 队列。
|
||||
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
|
||||
|
||||
核心变化是：
|
||||
|
||||
- `speedRatio` 不再直接驱动“发送前临场插值”。
|
||||
- `speedRatio` 改为驱动“执行侧最终队列构建”。
|
||||
- 最终队列一旦生成并校验通过，运行时只负责按 J519 状态包节奏出队发送。
|
||||
|
||||
## 6. 统一语义
|
||||
|
||||
本次只改飞拍任务，但仍需把飞拍内部语义说清楚，避免再次混淆规划倍率和执行倍率。
|
||||
|
||||
### 6.1 `planning_speed_scale`
|
||||
|
||||
- 只影响规划结果。
|
||||
- 影响 `DenseJointTrajectory` 的时间轴与规划层速度/加速度/Jerk 使用方式。
|
||||
- 飞拍开始执行前就已经固定。
|
||||
- 在线设置 `speedRatio` 时不应反向修改它。
|
||||
|
||||
### 6.2 `speedRatio`
|
||||
|
||||
- 只影响飞拍执行准备阶段。
|
||||
- 只对下一次启动的飞拍任务生效。
|
||||
- 表示“用户期望的执行层时间推进速度”。
|
||||
- 该值先用于构建候选发送队列。
|
||||
- 如果候选发送队列在离散 `8ms` 点列上不满足约束，则系统自动进一步拉长执行时长，直到满足约束。
|
||||
|
||||
也就是说：
|
||||
|
||||
- `speedRatio` 是执行目标倍率。
|
||||
- 但最终真实执行时长允许比该目标更保守。
|
||||
- 保守的原因不是放宽标准，而是严格保证离散动力学约束。
|
||||
|
||||
## 7. 最终发送序列生成
|
||||
|
||||
### 7.1 新增执行侧生成器
|
||||
|
||||
建议新增一个飞拍专用生成器，命名可类似：
|
||||
|
||||
- `FlyshotExecutionSendSequenceBuilder`
|
||||
- 或 `FlyshotExecutionQueueBuilder`
|
||||
|
||||
职责：
|
||||
|
||||
- 输入规划层 `DenseJointTrajectory`。
|
||||
- 输入 `durationSeconds`、`servoPeriodSeconds`、`speedRatio`。
|
||||
- 输出最终真实发送的 `8ms` 点列。
|
||||
- 输出与该点列一致的触发绑定结果。
|
||||
- 输出校验与自动拉长过程中的诊断信息。
|
||||
|
||||
### 7.2 第一版候选队列
|
||||
|
||||
候选队列仍可以沿用当前时间轴语义作为起点：
|
||||
|
||||
- `sendTime = sampleIndex * 0.008`
|
||||
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
|
||||
|
||||
但该候选队列只作为“第一轮尝试”，不能直接视为最终执行结果。
|
||||
|
||||
### 7.3 自动拉长策略
|
||||
|
||||
当候选队列的离散 `vel/acc/jerk` 校验失败时：
|
||||
|
||||
- 不修改规划层轨迹。
|
||||
- 不修改 `planning_speed_scale`。
|
||||
- 不放宽校验阈值。
|
||||
- 只在执行侧拉长最终发送时长，然后重新构建候选队列。
|
||||
|
||||
可实现为等价的两种方式之一：
|
||||
|
||||
1. 延长物理发送总点数，使发送总时长变长。
|
||||
2. 在保持 `8ms` 周期不变的前提下，降低执行侧有效轨迹时间推进速度。
|
||||
|
||||
本质上二者等价，建议统一落成第二种表达：
|
||||
|
||||
- 对外仍说“自动拉长执行时长”。
|
||||
- 对内通过更保守的 `trajectoryTime(sendTime)` 映射来实现。
|
||||
|
||||
### 7.4 自动拉长的迭代规则
|
||||
|
||||
建议采用单调保守策略：
|
||||
|
||||
1. 先按请求 `speedRatio` 构建第 1 版候选队列。
|
||||
2. 对候选队列做离散校验。
|
||||
3. 若失败，则按固定步长或倍率逐轮拉长，再次构建候选队列。
|
||||
4. 一旦通过，立即停止迭代，产出最终队列。
|
||||
5. 若超过安全迭代上限仍未通过，则拒绝执行并输出首个超限诊断。
|
||||
|
||||
这样可以保证：
|
||||
|
||||
- 自动拉长过程是可解释、可记录的。
|
||||
- 不会因为局部修补而引入新的不可预测尖峰。
|
||||
|
||||
## 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 校验失败的处理
|
||||
|
||||
飞拍链路按本次确认采用 A：
|
||||
|
||||
- 若失败，优先自动拉长执行时长重试。
|
||||
- 只有在超过拉长上限后仍失败时才拒绝执行。
|
||||
|
||||
拒绝执行时必须输出：
|
||||
|
||||
- 首次失败的轴
|
||||
- 时间窗
|
||||
- 指标类型
|
||||
- `actual / limit / ratio`
|
||||
- 失败发生在哪一轮自动拉长尝试中
|
||||
|
||||
## 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 工件统一基于最终发送队列
|
||||
|
||||
以下文件必须全部从最终发送队列生成：
|
||||
|
||||
- `ActualSendJointTraj.txt`
|
||||
- `ActualSendTiming.txt`
|
||||
- `ActualSendJerkStats.txt`
|
||||
- `ShotEvents.json`
|
||||
|
||||
不允许再出现：
|
||||
|
||||
- 导出文件基于一套样本，
|
||||
- 真实发送队列又基于另一套样本。
|
||||
|
||||
### 11.2 推荐新增日志字段
|
||||
|
||||
每次飞拍执行建议至少记录：
|
||||
|
||||
- 请求 `speedRatio`
|
||||
- 规划层轨迹时长
|
||||
- 第一轮候选队列点数
|
||||
- 第一轮候选队列是否通过校验
|
||||
- 若失败，首个失败窗口的：
|
||||
- 关节轴
|
||||
- 时间区间
|
||||
- 指标类型
|
||||
- `actual`
|
||||
- `limit`
|
||||
- `ratio`
|
||||
- 自动拉长轮数
|
||||
- 最终采用的执行时长
|
||||
- 最终发送队列点数
|
||||
- 最终触发绑定数量
|
||||
- 最终校验通过结论
|
||||
|
||||
### 11.3 日志级别建议
|
||||
|
||||
- `Information`
|
||||
- 记录执行请求、最终采用结果、最终通过结论
|
||||
- `Warning`
|
||||
- 记录第一轮候选失败与自动拉长启动
|
||||
- `Debug`
|
||||
- 记录每一轮拉长的中间参数与详细差分统计
|
||||
|
||||
## 12. 验收口径
|
||||
|
||||
### 12.1 功能验收
|
||||
|
||||
- 飞拍 `speedRatio` 可在线设置。
|
||||
- 该值对下一次启动的飞拍任务生效。
|
||||
- 不需要修改 `planning_speed_scale`。
|
||||
|
||||
### 12.2 约束验收
|
||||
|
||||
在 `speedRatio = {1.0, 0.8, 0.5}` 下：
|
||||
|
||||
- 最终发送队列逐周期 `vel/acc/jerk` 不超限。
|
||||
- 不出现固定区间正负交替的大幅 Jerk 尖峰。
|
||||
|
||||
### 12.3 工件与日志验收
|
||||
|
||||
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
|
||||
- 日志能定位自动拉长前后的关键参数和校验结果。
|
||||
|
||||
### 12.4 回归验收
|
||||
|
||||
- `speedRatio = 1.0` 时不退化。
|
||||
- 触发事件数量不异常漂移。
|
||||
- 触发绑定顺序不异常漂移。
|
||||
|
||||
## 13. 测试方案
|
||||
|
||||
### 13.1 单元测试
|
||||
|
||||
建议新增：
|
||||
|
||||
- 飞拍执行侧最终发送序列生成器测试
|
||||
- `speedRatio` 非法边界值测试
|
||||
- 第一轮候选失败后自动拉长成功测试
|
||||
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
|
||||
- 触发绑定始终与最终发送队列一致的测试
|
||||
|
||||
### 13.2 编排测试
|
||||
|
||||
建议补充：
|
||||
|
||||
- `ExecuteFlyShotTraj` 进入运行时前，已经拿到最终发送队列
|
||||
- `FanucControllerRuntime` 不再自行按 `_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/`
|
||||
|
||||
在飞拍执行准备阶段调用该生成器，把最终发送队列放入执行结果或新的执行上下文对象。
|
||||
|
||||
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
|
||||
调整 `SendDenseTrajectory(...)`：
|
||||
|
||||
- 不再自行根据 `_speedRatio` 从 `DenseJointTrajectory` 临场重采样
|
||||
- 直接消费已经构建好的最终发送队列
|
||||
|
||||
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
|
||||
|
||||
`ActualSend*` 导出改为复用最终发送队列，确保导出工件与运行时一致。
|
||||
|
||||
### 14.5 `tests/Flyshot.Core.Tests/`
|
||||
|
||||
补飞拍专项单测、UTTC 回归和工件一致性测试。
|
||||
|
||||
## 15. 风险与注意事项
|
||||
|
||||
### 15.1 不能把自动拉长误解为“偷偷改 speedRatio”
|
||||
|
||||
对外语义仍应保留：
|
||||
|
||||
- 用户请求的是某个 `speedRatio`
|
||||
- 系统为了满足离散动力学约束，自动采用了更保守的最终执行时长
|
||||
|
||||
日志里必须把这件事说清楚，不能让现场误以为参数未生效。
|
||||
|
||||
### 15.2 不能让触发绑定脱离最终队列
|
||||
|
||||
只要最终发送点数变化，触发绑定就必须重新基于最终队列计算，否则工件与执行会再次分叉。
|
||||
|
||||
### 15.3 `speedRatio = 1.0` 必须优先做回归保护
|
||||
|
||||
这是最容易被“执行侧重构”误伤的路径，必须作为首要回归项。
|
||||
|
||||
## 16. 结论
|
||||
|
||||
本次飞拍调速设计的核心结论是：
|
||||
|
||||
- `planning_speed_scale` 继续只属于规划层。
|
||||
- `speedRatio` 只属于飞拍执行层，并且只对下一次启动的飞拍任务生效。
|
||||
- 飞拍执行必须先生成最终 `8ms` 发送队列，再对该队列做逐周期 `vel/acc/jerk` 校验。
|
||||
- 若 `speedRatio < 1` 导致第一轮候选队列不满足约束，则自动拉长执行时长后重建，直到通过。
|
||||
- 运行时不再对飞拍轨迹做发送前临场回采样。
|
||||
- 触发绑定、导出工件与真实发送必须全部统一到同一份最终发送队列上。
|
||||
|
||||
该方案可以在不修改 `move_joint`、不放宽校验阈值、不中断现有触发语义的前提下，把飞拍 `speedRatio` 调速从“经验插值”收敛为“可校验、可追踪、可自动保守化”的执行机制。
|
||||
Reference in New Issue
Block a user