# 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_i`
- `Iterations` / `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 限制，直接生成可下发的稠密点位序列”，通常有两条路径：

1. **保留 ICSP retiming**：继续用 `ICspPlanner` 求时间轴，再在采样层按固定周期生成中间点位（当前架构就是这条路）。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。
2. **做真正的自适应插点/细分**：把“插点策略”升级为基于约束的细分（而不只是插中点），这更自然的落点是 `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`

