yunxiao.zhu
1779067b5c
* 增加 J519 稠密发送采样校验与保姿回发逻辑 * 调整 saveTrajectory 导出与 sequence buffer 行为 * 补充 10010 解析脚本、ICSP 说明和回归测试
7.0 KiB
ICSP 算法说明（ICspPlanner）
本文档用于解释 Flyshot.Core.Planning.ICspPlanner 当前实现的 ICSP 规划算法在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。
适用范围：本文描述的是当前 C# 实现的 “CubicSpline + 逐段时间缩放迭代（retiming）” 版本。
重要澄清：ICspPlanner的主要输出是 时间轴（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。
1. 名词与数据形态
1.1 输入
- 示教点（路点）：
request.Program.Waypoints
每个路点是关节空间向量 (q_i \in \mathbb{R}^{dof})。 - 关节约束：
request.Robot.JointLimits[d]提供每轴上限：- 速度上限 (v_{lim}[d])
- 加速度上限 (a_{lim}[d])
- 跃度（jerk）上限 (j_{lim}[d])
1.2 输出（PlannedTrajectory）
ICspPlanner 的输出 不是稠密轨迹点序列，而是：
PlannedWaypoints：规划后路点（对于普通icsp，与输入示教点相同；补点发生在SelfAdaptIcspPlanner）WaypointTimes：每个路点的绝对时间 (t_i)（秒）SegmentDurations：每段时长 (T_i = t_{i+1}-t_i)（秒）SegmentScales：每段缩放因子scale_iIterations/Threshold：收敛信息
后续模块会基于 PlannedWaypoints + WaypointTimes 重建样条并采样，生成稠密点：
- 规划层稠密采样：
TrajectorySampler.SampleJointTrajectory(...) - 运行时 J519 重采样（速度倍率映射 + rad->deg）：
J519SendTrajectorySampler.SampleDenseJointTrajectory(...)
2. 算法总体目标（retiming）
给定一组关节示教点 ({q_i}{i=0}^{N-1})，在不改变路点位置的前提下，为每段分配时长 ({T_i}{i=0}^{N-2})，使得用 clamped-zero 三次样条连接后的轨迹在每段上满足：
- (\max|\dot q_d(t)| \le v_{lim}[d])
- (\max|\ddot q_d(t)| \le a_{lim}[d])
- (\max|\dddot q_d(t)| \le j_{lim}[d])
实现策略是“逐段缩放时长”的迭代法：每轮用当前 ({T_i}) 构造样条并解析求导峰值，再根据超限程度把相应段时长乘以缩放因子，使峰值回落。
3. 计算步骤（与代码一致）
3.1 前置条件
- 路点数 (N \ge 4)（否则抛异常）
3.2 初始段时长
段数 (nseg = N-1)。
初始段时长取相邻路点关节空间欧氏距离：
[ T_i^{(0)} = |q_{i+1}-q_i|_2 ]
3.3 由段时长构造绝对时间轴
[ t_0 = 0,\quad t_{i+1} = t_i + T_i ]
3.4 用 clamped-zero 边界构造三次样条
以 ((t_i, q_i)) 为节点构造分段三次多项式：
[ S_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i,\quad t \in [t_i, t_{i+1}] ]
边界条件为 clamped-zero（起点/终点一阶导为 0），用于与逆向锁定的参考行为对齐。
3.5 解析计算每段导数峰值
对每段、每轴，解析求最大绝对值：
- 一阶导（速度）是二次函数：端点与顶点候选取最大
- 二阶导（加速度）是一次函数：端点取最大
- 三阶导（跃度）是常数：直接取绝对值
得到三张矩阵：
maxDq[seg,d] = max_t |dq/dt|maxDdq[seg,d] = max_t |d²q/dt²|maxDddq[seg,d] = max_t |d³q/dt³|
3.6 计算每段缩放因子（核心公式）
对段 seg，对每个关节 d 计算三类“超限比”：
[ s_v = \left|\frac{maxDq[seg,d]}{v_{lim}[d]}\right| ]
[ s_a = \sqrt{\left|\frac{maxDdq[seg,d]}{a_{lim}[d]}\right|} ]
[ s_j = \sqrt[3]{\left|\frac{maxDddq[seg,d]}{j_{lim}[d]}\right|} ]
段缩放因子取所有轴、三类约束的最大值：
[ scale_{seg}=\max_d \max(s_v, s_a, s_j) ]
指数来源：时间拉长 (k) 倍时，速度按 (1/k) 缩小、加速度按 (1/k^2) 缩小、跃度按 (1/k^3) 缩小，因此超限比需要分别取一次方/平方根/立方根来求“应当拉长多少倍”。
3.7 收敛指标与最优解保存
每轮用如下指标衡量“离约束满足还差多少”：
[ threshold = \sum_{seg} |scale_{seg} - 1| ]
若本轮 threshold 小于历史最佳，则保存当前解作为 best（包含 bestDurations / bestScales / bestWaypointTimes 等）。
3.8 收敛判定与段时长更新
- 若
threshold < _threshold（默认1e-3），认为收敛并提前结束迭代 - 否则更新每段时长：
[ T_{seg} \leftarrow T_{seg} \cdot scale_{seg} ]
并进入下一轮。
4. 最终判定（global_scale）
迭代结束后取历史最优缩放因子的最大值：
[ globalScale=\max_{seg}(scale_{seg}) ]
若启用强制判定（enforceFinalScale=true）且：
[ globalScale > 1 + \text{finalScaleTolerance} ]
则判定“未收敛/不可执行”并抛异常。默认容差 finalScaleTolerance=1e-2，用于容忍 C# spline 与参考实现间的小量数值差异。
5. 与“补点/中间点位”的关系（常见误解澄清）
5.1 ICspPlanner 不负责生成固定周期的中间点位
ICspPlanner 的核心工作是 时间轴规划（retiming）：在不改变示教点位置的情况下，通过缩放每段时长让样条导数峰值满足约束。
固定周期（例如 8ms/16ms）的“中间点位序列”属于 采样层：
TrajectorySampler：按samplePeriod在样条上取样，得到[time, j1..jN]（关节单位仍为 rad）J519SendTrajectorySampler：按servoPeriod生成真实发送序列，用speedRatio把sendTime映射到trajectoryTime并线性插值，再做rad -> deg
5.2 SelfAdaptIcspPlanner 才包含“补点”逻辑，但它很粗
self-adapt-icsp 的补点策略在 SelfAdaptIcspPlanner 中：当某些段 scale > 1 + tolerance 时，对这些段插入关节空间中点再重规划。该策略的目的主要是“救收敛”，不是生成最终稠密序列。
6. 后续改造建议（定位落点）
如果需求是“根据示教点 + v/a/j 限制，直接生成可下发的稠密点位序列”，通常有两条路径：
- 保留 ICSP retiming：继续用
ICspPlanner求时间轴，再在采样层按固定周期生成中间点位（当前架构就是这条路）。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。 - 做真正的自适应插点/细分：把“插点策略”升级为基于约束的细分（而不只是插中点），这更自然的落点是
SelfAdaptIcspPlanner或新增一个“约束驱动细分器”，而不是把稠密点生成塞进ICspPlanner。
7. 关联实现位置（便于跳转）
- 算法入口：
src/Flyshot.Core.Planning/ICspPlanner.cs - 自适应补点：
src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs - 三次样条实现（clamped-zero + 解析导峰值）：
src/Flyshot.Core.Planning/CubicSplineInterpolator.cs - 规划层稠密采样：
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs - J519 实发重采样：
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs