机器人三次多项式插值和带抛物线过渡的线性插值法详解

在机器人轨迹规划中，三次多项式插值和**带抛物线过渡的线性插值（LSPB）**是两种最基础、最常用的方法。它们分别对应"平滑但不可控速度"和"可控速度但加速度有突变"的典型场景。

下面详细拆解这两种方法。

一、三次多项式插值

这种方法为每一段关节空间轨迹生成一个三次多项式函数 θ(t)\theta(t)θ(t)，保证位置和速度在起点和终点连续。

1. 数学模型

设单关节从起点 θ0\theta_0θ0 运动到终点 θf\theta_fθf，总时间 tft_ftf，起点速度为 v0v_0v0，终点速度为 vfv_fvf。

三次多项式形式：

θ(t)=a0+a1t+a2t2+a3t3 \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 θ(t)=a0+a1t+a2t2+a3t3

速度：

θ˙(t)=a1+2a2t+3a3t2 \dot{\theta}(t) = a_1 + 2a_2 t + 3a_3 t^2 θ˙(t)=a1+2a2t+3a3t2

代入边界条件：

θ(0)=θ0=a0θ˙(0)=v0=a1θ(tf)=θf=a0+a1tf+a2tf2+a3tf3θ˙(tf)=vf=a1+2a2tf+3a3tf2 \begin{aligned} \theta(0) &= \theta_0 = a_0 \\ \dot{\theta}(0) &= v_0 = a_1 \\ \theta(t_f) &= \theta_f = a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 \\ \dot{\theta}(t_f) &= v_f = a_1 + 2a_2 t_f + 3a_3 t_f^2 \end{aligned} θ(0)θ˙(0)θ(tf)θ˙(tf)=θ0=a0=v0=a1=θf=a0+a1tf+a2tf2+a3tf3=vf=a1+2a2tf+3a3tf2

解得：

a0=θ0,a1=v0,a2=3(θf−θ0)−(2v0+vf)tftf2,a3=−2(θf−θ0)+(v0+vf)tftf3 a_0 = \theta_0,\quad a_1 = v_0,\quad a_2 = \frac{3(\theta_f - \theta_0) - (2v_0 + v_f)t_f}{t_f^2},\quad a_3 = \frac{-2(\theta_f - \theta_0) + (v_0 + v_f)t_f}{t_f^3} a0=θ0,a1=v0,a2=tf23(θf−θ0)−(2v0+vf)tf,a3=tf3−2(θf−θ0)+(v0+vf)tf

2. 典型情况：起点与终点速度为零

若 v0=vf=0v_0 = v_f = 0v0=vf=0：

θ(t)=θ0+3(θf−θ0)tf2t2−2(θf−θ0)tf3t3 \theta(t) = \theta_0 + \frac{3(\theta_f - \theta_0)}{t_f^2} t^2 - \frac{2(\theta_f - \theta_0)}{t_f^3} t^3 θ(t)=θ0+tf23(θf−θ0)t2−tf32(θf−θ0)t3

此时加速度为线性变化（因为三次多项式求导二次为一次函数），起点和终点加速度不为零（分别为 6(θf−θ0)tf2\frac{6(\theta_f - \theta_0)}{t_f^2}tf26(θf−θ0) 和 −6(θf−θ0)tf2-\frac{6(\theta_f - \theta_0)}{t_f^2}−tf26(θf−θ0)）。

3. 特点

优点：位置、速度、加速度均连续（二阶连续），无突变，对电机冲击小。
缺点：
- 中间段速度不可控，可能超过关节极限。
- 加速度在起点和终点不为零（若要求起止速度为零），意味着需要电机瞬间提供较大加速度。
- 只能保证经过端点，不能精确经过中间点（除非拼接多段多项式）。

4. 适用场景

单段点到点运动。
对冲击敏感，且对速度曲线形状无特殊要求的场合。

二、带抛物线过渡的线性插值（LSPB）

这种方法为了克服纯线性插值（梯形速度曲线）在速度转折点加速度无穷大的问题，在两端用抛物线（匀加速/减）过渡，中间段保持匀速。

1. 基本思想

加速段 ：恒定加速度 aaa（抛物线形状的位置曲线）。
匀速段 ：速度恒定 vmaxv_{\text{max}}vmax。
减速段 ：恒定减速度 −a-a−a（对称设计）。

2. 数学推导（以对称 LSPB 为例，起点终点速度为零）

设总行程 θf−θ0=D\theta_f - \theta_0 = Dθf−θ0=D，总时间 tft_ftf，加速度 aaa（正值）。

加速段时间 tat_ata，加速度段位移 12ata2\frac{1}{2} a t_a^221ata2。

由于对称，加速和减速位移相等，匀速段位移 D−ata2D - a t_a^2D−ata2，匀速段时间 tm=D−ata2atat_m = \frac{D - a t_a^2}{a t_a}tm=ataD−ata2。

总时间：

tf=2ta+tm=2ta+D−ata2ata=Data+ta t_f = 2t_a + t_m = 2t_a + \frac{D - a t_a^2}{a t_a} = \frac{D}{a t_a} + t_a tf=2ta+tm=2ta+ataD−ata2=ataD+ta

给定 tft_ftf 和 DDD，需选择 aaa 或 tat_ata 满足 ata≤D/2a t_a \le D/2ata≤D/2（保证有匀速段）且 ata2≤Da t_a^2 \le Data2≤D。

3. 位置分段表达式

设 tb=tat_b = t_atb=ta（加速结束时间），tc=tf−tat_c = t_f - t_atc=tf−ta（减速开始时间），vmax=atav_{\text{max}} = a t_avmax=ata。

加速段 (0≤t≤ta0 \le t \le t_a0≤t≤ta)：

θ(t)=θ0+12at2 \theta(t) = \theta_0 + \frac{1}{2} a t^2 θ(t)=θ0+21at2

匀速段 (ta≤t≤tf−tat_a \le t \le t_f - t_ata≤t≤tf−ta)：

θ(t)=θ0+ata(t−ta/2) \theta(t) = \theta_0 + a t_a (t - t_a/2) θ(t)=θ0+ata(t−ta/2)

减速段 (tf−ta≤t≤tft_f - t_a \le t \le t_ftf−ta≤t≤tf)：

θ(t)=θf−12a(tf−t)2 \theta(t) = \theta_f - \frac{1}{2} a (t_f - t)^2 θ(t)=θf−21a(tf−t)2

4. 特点

优点：
- 加速度有限（仅在起点、终点、加速‑匀速交接点有阶跃，但不为无穷大）。
- 速度可严格控制在 vmaxv_{\text{max}}vmax 以内，适合受限于关节速度的机器人。
- 计算简单，实时性好。
缺点：
- 加速度不连续（在 tat_ata 和 tf−tat_f - t_atf−ta 处有突变），理论上会激起机械振动。
- 需要提前选定 aaa 和 tft_ftf 或 aaa 和 vmaxv_{\text{max}}vmax，若行程太短可能无匀速段，自动退化为三角形速度曲线。

5. 适用场景

工业机器人快速搬运、喷涂等对路径精度要求不高但需严格控制最大速度的场合。
多用于关节空间 或笛卡尔空间直线插补（与直线插补结合，变成"加速‑匀速‑减速"直线运动）。

三、两者对比表

特性	三次多项式插值	抛物线过渡线性插值 (LSPB)
加速度连续性	连续（线性变化）	不连续（在过渡点阶跃）
速度控制	中间速度不可预测，可能超限	能严格限制最大速度
计算量	简单，仅需解 4 个系数	需要判断是否有匀速段，计算过渡时间
对电机冲击	较小（加速度连续）	较大（加速度突变）
适用场景	精密装配、高平滑轨迹	高速搬运、点到点快速运动
过中间点方式	多段三次多项式拼接（需额外约束）	常与 PVT 模式结合，或使用高阶多项式过渡

四、实际应用建议

若机器人负载较轻、刚性较好 （如 SCARA、小型六轴），且希望运动极致平滑，优先选择五次多项式 （加速度也连续）而不是三次------三次的加速度在端点不为零，仍有一定冲击。

但若只能用三次多项式，注意检查中间速度是否超过关节限位。
在通用工业机器人轨迹规划中 ，需要明确的"加‑匀‑减"阶段以便于时间最优控制，LSPB 是一个实用的选择 。实际代码中常将 LSPB 与速度前瞻 、S 形曲线（加速度也连续，但计算更复杂）结合使用。
一个常见陷阱 ：LSPB 中加速度突变会导致理论上的无穷大加加速度（jerk）。对于大负载或高精度机器人，这会引起残余振动。此时应改用 S 形速度曲线 （七段式，加加速度有限）或五次多项式。