实现PVT插补的三次多项式（Hermite cubic）

在 PVT（Position–Velocity–Time） 模式里，驱动在收到一段数据

$$(p_0,v_0)\underset{T}{\overset{\text{段时长}}{\to }}(p_1,v_1)$$

后，“加速度曲线不是直接给的”，而是驱动根据这四个边界条件，在段内自行插补算出来的。最常见实现是用一条 三次多项式（Hermite cubic），也就是：

1) 段内轨迹模型（常见默认）

令段内时间 $t\in [0,T]$，驱动构造

$$p(t)=p_0+v_{0}t+c{t}^2+d{t}^3$$

满足：

$$p(T)=p_1,\dot{p}(0)=v_0,\dot{p}(T)=v_1$$

解得（写法给便于工程实现）：

$$\begin{array}{rl}A& =p_1-p_0-v_{0}T,B=v_1-v_0\\ c& =\frac{3A-BT}{T^2},d=\frac{BT-2A}{T^3}\end{array}$$

于是**速度、加速度、加加速度（跃度/jerk）**分别是：

KaTeX parse error: Undefined control sequence: \dddot at position 64: …c+6d\,t,\qquad \̲d̲d̲d̲o̲t̲ ̲p(t)=6d\;(\text…

结论：PVT 段内加速度是线性函数，跃度为常数。也就是说，这段的加速度从段首到段末线性变化。

2) 峰值加速度（快速检查是否越限）

因为 $\ddot{p}(t)$ 随 $t$ 线性变化，所以段内最大/最小加速度出现在两端：

$$a_{\text{start}}=\ddot{p}(0)=2c,a_{\text{end}}=\ddot{p}(T)=2c+6dT$$

工程上可以用这两个值与驱动的 $a_{\max }$ 做比较，预估是否会触发越限。

3) 如果越限，驱动一般会怎么做？

不同厂商策略略有差异，常见有三类（手册会说明）：

1. 时间拉伸（time-scaling）：自动增大 $T$（或施加速度覆盖系数）直到满足 $a_{\max }$ / $j_{\max }$。
2. 拒收/报错：本段不执行，置故障（常见于严格遵循设定极限的驱动）。
3. 内部重规划：启用内置 S 曲线/限幅器，实际运行与给定端点等价但段内曲线被“柔化”。

最常见、也最“稳”的方式是 时间拉伸： $p_0,v_0,p_1,v_1$ 不变，只把 $T$ 加长一点，三次多项式自然降低所需的峰值加速度/跃度。

4) 与 PT 的差异

• PT（Position–Time）只有 $(p_0,p_1,T)$（没有 $v_1$ 约束），简化实现通常用分段线性：段内速度常数、段边界速度跳变 ⇒ 加速度呈脉冲（不平滑）。一些驱动会内置滤波/限加速度让它更温和，但严格说 PT 不保证速度连续。
• PVT多一个 $v_1$，驱动能构造速度连续的三次多项式 ⇒ 加速度线性、跃度常数，轮廓明显更平滑。

5) 小示例（便于在上位机预估）

给定一段 $T=0.02\text{s}$（20 ms），$p_0=1000$，$v_0=50$，$p_1=1200$，$v_1=60$（单位随系统而定）：

$$\begin{array}{rl}A& =1200-1000-50\cdot 0.02=200-1=199\\ B& =60-50=10\\ c& =\frac{3\cdot 199-10\cdot 0.02}{0.02^2}\approx \frac{597-0.2}{4\times 10^{-4}}\approx 1.492\times 10^6\\ d& =\frac{10\cdot 0.02-2\cdot 199}{0.02^3}\approx \frac{0.2-398}{8\times 10^{-6}}\approx -4.97\times 10^7\end{array}$$

于是

$$a_{\text{start}}=2c\approx 2.98\times 10^6,a_{\text{end}}=2c+6dT\approx 2.98\times 10^6+6(-4.97\times 10^7)(0.02)\approx -2.0\times 10^6$$

（数值只是演示量级：说明段越短、位移/速度变化越大 ⇒ 需要的加速度越大，因此合理选 $T$ 很关键。）

实战建议：在上位机按上述公式先行估算峰值加速度/跃度；若超限，增大段长 $T$ 或减小端点速度差 $|v_1-v_0|$、位移量 $|p_1-p_0|$。

6) 有些驱动提供更高级的插补器

部分品牌在 PVT 内部直接使用 五次多项式（quintic）/S 曲线（可同时约束 $a_{\max }$、$j_{\max }$），甚至把 $a_0,a_1$ 也当边界条件（但这已超出标准 PVT 的最小约束集）。遇到这类驱动，你会在手册里看到：“PVT 插补为 jerk-limited / S-curve”，加速度会是分段线性或连续非线性，比三次更“柔”。

---

一句话总结

• 标准 PVT：驱动用三次 Hermite插补，加速度在段内线性变化、跃度常数；是否越限取决于 $(p_0,p_1,v_0,v_1,T)$。
• 工程做法：上位机先按公式估峰值 → 若超限就拉长 $T$ 或减小速度/位移跨度；或选用支持 S 曲线/时间缩放 的驱动。

例子：100ms内移动10mm

错误思路：10 mm / 100 ms → 每 1 ms 递增 0.1 mm
这种做法本质上是恒速线性插值，等价于 PT（Position-Time），不是严格的 PVT。

• 这样做的速度是常数 100 mm/s，加速度为 0（理想数学上起止瞬间需要无穷大加速度跳变）。
• 真实伺服会自己“圆滑”起止，导致线性位置和电机实际轨迹不一致、冲击也更大。

如果要实现“驱动内部 PVT 插补”，标准做法是用一段三次多项式（Hermite cubic），同时满足段首/段末的位置和速度（$p_0,v_0$ 到 $p_1,v_1$ ，用时 $T$）。这样速度连续、加速度线性变化（jerk 常数）。

---

一段 PVT 的通用公式（落地可用）

给定：$p_0,v_0$ → $p_1,v_1$ ，段时长 $T$。

令段内时间 $t\in [0,T]$，取

$$p(t)=p_0+v_{0}t+c{t}^2+d{t}^3$$

满足边界条件可解得（记 $A=p_1-p_0-v_{0}T,B=v_1-v_0$）：

$$c=\frac{3A-BT}{T^2},d=\frac{BT-2A}{T^3}.$$

由此：

KaTeX parse error: Undefined control sequence: \dddot at position 84: …c+6dt,\\ j(t)&=\̲d̲d̲d̲o̲t̲ ̲p(t)=6d\;\;(\te…

性质：速度连续；加速度在段内线性变化；jerk 为常数。

---

代入例子（做对比）

目标：从 $p_0=0$ 到 $p_1=10\text{ mm}$，$T=0.1\text{ s}$。

• 若做 PT/恒速：每 1 ms 加 0.1 mm ⇒ $v=100\text{ mm/s}$，但起止瞬间加速度跳变（不建议作为驱动内部的标准插补）。

• 若做 PVT（更合理），常见两种边界速度设法：

  1. 起停都为 0：$v_0=v_1=0$。

     • 有 $A=10,B=0\Rightarrow c=\frac{30}{0.01}=3000,d=\frac{-20}{0.001}=-20000$（单位按 mm、s）。
     • 峰值加速度出现在端点：$|a_{\max }|=|2c|=6000{\text{ mm/s}}^2$（另一端为 $-6000$）。
     • 速度在中点达峰值，$v_{\max }\approx 150\text{ mm/s}$（平均速仍是 100）。

  2. 保持段内基本恒速：选 $v_0=v_1=100\text{ mm/s}$（更像匀速段，但两端仍会有有限的加速度过渡，而非脉冲）。

---

约束与整定（很重要）

• 加速度/jerk 限制（驱动或机械允许的上限）

  • v0=v1=0 时，端点加速度幅值 $|a{|}_{\text{max}}=\frac{6\Delta p}{T^2}$。

    • 反推 最小 T：$T\ge \sqrt{\frac{6\Delta p}{a_{\max }}}$。

  • jerk 幅值 $|j|=|6d|=\left|\frac{6(BT-2A)}{T^3}\right|$。

• 超限怎么办：做 time-scaling（把本段 $T$ 按比例放大），或减小 $|p_1-p_0|$、$|v_1-v_0|$；必要时改用 S 曲线/五次多项式（可同时约束 $a$、$j$）。

---

固件实现步骤（驱动侧）

1. 接收段：得到 $(p_1,v_1,T)$，内部保存上一段末端 $(p_0,v_0)$。

2. 计算系数：按上式求 $c,d$。

3. 限幅检查：用端点加速度 $a(0)=2c,a(T)=2c+6dT$ 与 $|j|=|6d|$ 校验上限；如超限 → 拉长 T 或告警。

4. 按内部高速周期（如 1 kHz）采样：

   $$t_k=k\Delta t(\Delta t=1\text{ ms}),p_k=p(t_k).$$

   将 $p_k$ 送入位置环（或把 $v(t_k)$ 送入速度环）。

5. 段切换：t 到 $T$ 时切入下一段，保证上一段末端与下一段起点（含 $v$）一致。

如果你一定要“1 ms 一点”，那是 上位 PVT 规划 + 驱动内部再细分 的冗余；驱动侧仍然以多项式生成 1 ms 目标，不是简单 0.1 mm 线性累加。

---

何时用线性（PT）也能接受？

• 轨迹要求不高、速度较低；或由上位机已经做了充分的 S 曲线限加速度，驱动只需“按时到点”。
• 否则，更推荐 PVT 的三次插补（速度连续、冲击更小）。

---

小结

• “每 1 ms 加 0.1 mm”＝PT/恒速，不是标准的 PVT。
• PVT 正确做法：用三次多项式满足 $(p_0,v_0)\to (p_1,v_1)$ 与 $T$，速度连续、加速度线性；配合加速度/jerk 限制与时间缩放，机械更安全。

C/C++ 内联函数

20 kHz 位置环（50 µs 周期）非常适合在驱动内部做 PVT（三次 Hermite）插补。思路是：段内跃度 $j$ 恒定，所以可以用一个常加加速度（jerk 常数）的离散递推在 50 µs 中断里高效“滚动”出 $p,v,a$。

• 段开始一次性算好三次系数 $c,d$、端点加速度 $a(0),a(T)$ 和 jerk $j=6d$。

• 中断里不用幂函数，仅做加减乘：

  $$\begin{array}{rl}a& \leftarrow a+j\Delta t\\ v& \leftarrow v+a\Delta t+\frac{1}{2}j\Delta t^2\\ p& \leftarrow p+v\Delta t+\frac{1}{2}a\Delta t^2+\frac{1}{6}j\Delta t^3\end{array}$$

• 最后一步“对齐段末”用闭式公式消除累计误差。

• STM32F405 有单精度 FPU，float 足够；段初始化时也可以用 double 再转 float。

---

代码（可直接用在 50 µs 定时中断）

// pvt_hermite_fast.h
#pragma once
#include <math.h>
#include <stdbool.h>
#include <stdint.h>

typedef float  pvt_real;     // F405 有单精度 FPU，float 最快
typedef double pvt_real64;   // 段初始化可用 double 提升一次性精度

typedef struct {
  // 边界
  pvt_real p0, v0, p1, v1;
  pvt_real T;          // 段时长(可能因限幅被拉长)

  // 三次系数与派生量
  pvt_real c, d;       // p(t) = p0 + v0 t + c t^2 + d t^3
  pvt_real a0;         // 段首加速度 = 2c
  pvt_real j;          // 常数 jerk = 6d

  // 运行状态
  pvt_real t;          // 已运行时间
  pvt_real p, v, a;    // 当前设定 p/v/a
  bool active;

  // 采样常量（减少乘法次数）
  pvt_real dt, dt2, dt3;

  // 限幅
  pvt_real a_max;      // 若<=0 则不限制
  pvt_real j_max;      // 若<=0 则不限制
} PVT_Seg;

/*—— 一次性计算 c,d ——*/
static inline void pvt_compute_cd64(pvt_real64 p0,pvt_real64 v0,
                                    pvt_real64 p1,pvt_real64 v1,
                                    pvt_real64 T,
                                    pvt_real64* c, pvt_real64* d)
{
  pvt_real64 A = (p1 - p0) - v0*T;   // p1 - p0 - v0 T
  pvt_real64 B = (v1 - v0);
  *c = (3.0*A - B*T) / (T*T);
  *d = (B*T - 2.0*A) / (T*T*T);
}

/*—— 按 a_max / j_max 需要拉长时间 ——*/
static inline pvt_real pvt_timescale_for_limits(pvt_real a0, pvt_real aT, pvt_real j,
                                                pvt_real a_max, pvt_real j_max)
{
  pvt_real s = 1.0f;
  if (a_max > 0.0f) {
    pvt_real a_need = fmaxf(fabsf(a0), fabsf(aT));
    if (a_need > a_max) s = fmaxf(s, sqrtf(a_need / a_max));   // a ~ 1/T^2
  }
  if (j_max > 0.0f) {
    pvt_real j_need = fabsf(j);
    if (j_need > j_max) s = fmaxf(s, cbrtf(j_need / j_max));   // j ~ 1/T^3
  }
  return s;
}

/*—— 启动一段（设定 50us 周期等）——*/
static inline void pvt_start(PVT_Seg* s,
                             float p0, float v0, float p1, float v1,
                             float T,      // 例如 0.02f = 20ms
                             float a_max,  // mm/s^2 或 脉冲/s^2；<=0 表示不限制
                             float j_max,  // mm/s^3；<=0 表示不限制
                             float dt)     // 例如 50e-6f
{
  if (T <= 0.0f) T = 1e-3f;
  if (dt <= 0.0f) dt = 50e-6f;

  s->p0=p0; s->v0=v0; s->p1=p1; s->v1=v1; s->T=T;
  // 用 double 求一次 c,d
  pvt_real64 c64,d64;
  pvt_compute_cd64(p0, v0, p1, v1, T, &c64, &d64);
  float c = (float)c64, d = (float)d64;

  float a0 = 2.0f*c;
  float aT = 2.0f*c + 6.0f*d*T;
  float j  = 6.0f*d;

  // 可选：自动拉长时间
  float s_scale = pvt_timescale_for_limits(a0,aT,j, a_max,j_max);
  if (s_scale > 1.0f) {
    s->T = T * s_scale;
    pvt_compute_cd64(p0, v0, p1, v1, s->T, &c64, &d64);
    c = (float)c64; d = (float)d64;
    a0 = 2.0f*c;
    aT = 2.0f*c + 6.0f*d*s->T;
    j  = 6.0f*d;
  }

  s->c=c; s->d=d; s->a0=a0; s->j=j;

  // 初始化运行状态（t=0 用精确端点）
  s->t = 0.0f;
  s->p = p0;
  s->v = v0;
  s->a = a0;

  s->dt  = dt;
  s->dt2 = dt*dt * 0.5f;        // 0.5 dt^2
  s->dt3 = dt*dt*dt / 6.0f;     // (1/6) dt^3

  s->a_max = a_max;
  s->j_max = j_max;
  s->active = true;
}

/*—— 中断里每 50us 调一次；返回 false 表示段已结束 ——*/
static inline bool pvt_step_50us(PVT_Seg* s, float* p_ref, float* v_ref, float* a_ref)
{
  if (!s->active) return false;

  // 若剩余时间不足一个 dt，直接跳到段末避免累积误差
  float t_left = s->T - s->t;
  if (t_left <= s->dt) {
    s->p = s->p1;
    s->v = s->v1;
    s->a = 2.0f*s->c + 6.0f*s->d*s->T;  // 端点加速度
    s->t = s->T;
    s->active = false;
  } else {
    // 常数 jerk 递推：先更新加速度，再更新速度与位置
    s->a += s->j * s->dt;
    s->v += s->a * s->dt + s->j * s->dt2;
    s->p += s->v * s->dt + s->a * s->dt2 + s->j * s->dt3;
    s->t += s->dt;
  }

  if (p_ref) *p_ref = s->p;
  if (v_ref) *v_ref = s->v;
  if (a_ref) *a_ref = s->a;
  return s->active;
}

用法示例（20 ms 一段，ISR 里 50 µs 推算设定值）：

// 全局/任务里准备一段
PVT_Seg seg;
void start_one_segment(void) {
  // 例：从 0mm,0 到 10mm,0，用时 20ms；限制 a_max/j_max（按机械设）
  pvt_start(&seg, 0.0f, 0.0f, 10.0f, 0.0f,
            0.020f,    // T = 20ms
            6000.0f,   // a_max
            1.0e6f,    // j_max
            50e-6f);   // dt = 50us (20kHz)
}

// 20kHz 定时器中断（位置环）
void TIMx_UP_IRQHandler(void)
{
  float p, v, a;
  if (!pvt_step_50us(&seg, &p, &v, &a)) {
    // 段结束：这里切下一段（把上一段 p1,v1 当下一段 p0,v0）
    // pvt_start(&seg, ... 下一段 ...);
  }

  // 将 p 作为位置给定送入位置环（或 v 给速度环）
  // controller_set_position_ref(p);
  // 其余：前馈、限幅、抗饱和等…
}

---

关键参数与工程建议

• 段长 vs. 内环周期：20 ms/段 × 20 kHz = 400 步/段，非常平滑。高速段可 10 ms/段。
• 时间对齐：保证段长 $T$ 是 1 ms 的整数倍（多数驱动/规划器假设 1 ms 内插）。内部 50 µs 只是控制环采样，更细。
• 限幅：上面 pvt_start() 自带 time-scaling，避免超过 $a_{\max }$ / $j_{\max }$。
• FPU/编译：用 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -Ofast；中断里避免 powf() 这类重函数。
• 数值收尾：最后一步直接用端点值“对齐”，避免累计误差渗漏到下一段。
• 多轴同步：多轴一起调用 pvt_start()，共用同一时钟；段边界统一触发即可天然同步。
• 前瞻缓冲：上位机侧仍建议提前喂 ≥150–200 ms 数据，以便容错和流畅衔接。