1. 为什么需要实时五次插值

1.1 从三次插值说起

做过机器人轨迹规划的人，大概率都接触过三次多项式插值。它的思路很朴素：给定起点和终点的位置、速度，构造一条光滑曲线把两端连起来。四个约束条件对应四个系数，数学上干净利落，代码写起来也就十来行。

但三次插值有个绕不开的硬伤——加速度不连续。位置和速度确实平滑了，可加速度在起点和终点是跳变的。这意味着什么？如果你驱动的是一个真实的关节电机，力矩正比于加速度，加速度跳变就等于力矩跳变。电机在那一瞬间会“咯噔”一下，机械结构承受冲击，长期下来齿轮磨损、皮带松弛、定位精度下降，全是这么来的。

我第一次在机械臂上跑三次插值时，末端执行器在起步和停止的瞬间肉眼可见地抖了一下。当时以为是电机参数没调好，折腾了半天PID，最后才发现问题出在轨迹本身。

1.2 五次插值解决了什么

五次多项式有六个系数，可以同时约束起点和终点的位置、速度、加速度。加速度连续了，力矩就连续了，机械系统运行起来才真正“丝滑”。

代价是什么？计算量比三次插值大一些，但说实话，在现在的处理器上，这点计算量根本不算什么。一个六次多项式求值，无非就是几次乘加运算，哪怕在几十块钱的单片机上跑也毫无压力。

真正需要关注的是“实时”两个字。所谓实时五次插值，不是说算得快就行，而是要求在每个控制周期内，根据当前状态动态生成或更新轨迹，并且保证轨迹的连续性和可执行性。这跟离线规划好一整条轨迹再播放，是完全不同的思路。

1.3 适用场景与读者定位

这套方法最典型的应用场景包括：工业机械臂的关节空间轨迹规划、协作机器人的柔顺控制、数控机床的进给轴控制、甚至四足机器人的步态过渡。只要你的系统对运动平滑性有要求，五次插值就是一个性价比很高的选择。

这篇文章适合有一定机器人学基础、写过轨迹规划代码、但可能对实时实现细节还不太有把握的开发者。如果你完全没接触过多项式插值，建议先补一下三次插值的基本推导，再来看这篇会顺畅很多。我会尽量把每一步的来龙去脉讲清楚，但不会从泰勒展开开始讲起。

2. 五次多项式的数学推导

2.1 基本形式与约束条件

五次多项式的一般形式是：

q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5

对时间求导，得到速度和加速度：

q'(t) = a1 + 2*a2*t + 3*a3*t^2 + 4*a4*t^3 + 5*a5*t^4 q''(t) = 2*a2 + 6*a3*t + 12*a4*t^2 + 20*a5*t^3

假设我们规划一段从 t=0 到 t=T 的轨迹，已知边界条件：

六个已知量，六个未知系数，方程组有唯一解。

2.2 系数求解的推导过程

把 t=0 代入三个方程，直接得到：

a0 = q0 a1 = v0 a2 = acc0 / 2

这三个系数是白送的，不需要解方程。剩下的 a3、a4、a5 需要联立 t=T 的三个方程。

为了推导方便，先把已知量做归一化处理。令 h = q1 - q0 - v0T - acc0T^2/2，这个 h 代表了扣掉已知部分后剩余的位置增量。

经过整理（中间代数过程略去，网上到处都能找到），最终得到：

a3 = (20*h - (8*v1 + 12*v0)*T - (3*acc0 - acc1)*T^2) / (2*T^3) a4 = (30*h + (14*v1 + 16*v0)*T + (3*acc0 - 2*acc1)*T^2) / (2*T^4) a5 = (12*h - 6*(v1 + v0)*T - (acc0 - acc1)*T^2) / (2*T^5)

这个公式看起来有点吓人，但实际编码时就是照着抄的事。我建议你把这几个表达式封装成一个函数，输入 q0、v0、acc0、q1、v1、acc1、T，输出六个系数。

注意：当 T 非常小的时候，T^3、T^4、T^5 会趋近于零，除法可能导致数值不稳定。实际工程中要加一个最小时间保护，比如 T 不小于 1ms。

2.3 归一化时间变量的技巧

上面用的是绝对时间 t，每次求值都要算 t^2、t^3、t^4、t^5。如果控制周期是固定的，比如 1ms，那 t 的取值就是 0.001、0.002、0.003……这些数的五次方算起来虽然不快但也不慢。

更优雅的做法是引入归一化时间 s = t / T，s 的取值范围是 [0, 1]。把多项式改写成 s 的形式：

q(s) = b0 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5

这样做的好处有两个：第一，s 的幂次计算可以用 Horner 方法快速求值；第二，系数 b 只跟边界条件有关，跟 T 无关，可以预先算好。

归一化后的系数关系是：

b0 = q0 b1 = v0 * T b2 = acc0 * T^2 / 2 b3 = 10*(q1-q0) - 6*v0*T - 4*v1*T + (3*acc0 - acc1)*T^2/2 b4 = -15*(q1-q0) + 8*v0*T + 7*v1*T + (-3*acc0 + 2*acc1)*T^2/2 b5 = 6*(q1-q0) - 3*v0*T - 3*v1*T + (acc0 - acc1)*T^2/2

这组公式是我个人比较推荐的实现方式，代码更简洁，数值稳定性也更好。

3. 实时插值的工程实现

3.1 实时性的真正含义

很多人一听到“实时”，第一反应是“算得快”。这个理解只对了一半。在机器人控制里，实时至少包含三层含义：

第一层是时间确定性。每个控制周期必须在规定时间内完成计算，不能有时候快有时候慢。这就要求算法的时间复杂度是固定的，不能有循环次数不确定的迭代。

第二层是状态连续性。每个周期生成的轨迹点，必须跟上一个周期平滑衔接。不能出现位置跳变，速度跳变也不行。

第三层是动态可更新。如果中途收到新的目标点或者需要调整轨迹，系统要能在下一个周期就切换到新轨迹，并且保证切换点的位置、速度、加速度连续。

五次插值天然满足前两层，第三层需要额外设计。

3.2 单段轨迹的实时生成

最简单的实时实现是这样的：收到一段新轨迹请求时，计算六个系数，然后每个控制周期把 t 代入求值，直到 t 超过 T，标记该段完成。

伪代码大概长这样：

class QuinticSegment: def __init__(self, q0, v0, a0, q1, v1, a1, T): self.T = T self.coeffs = compute_coeffs(q0, v0, a0, q1, v1, a1, T) self.t = 0.0 def step(self, dt): self.t += dt if self.t >= self.T: self.t = self.T done = True else: done = False s = self.t / self.T q = horner_eval(self.coeffs, s) return q, done

这里用 Horner 方法求值，五次多项式只需要五次乘法和五次加法：

def horner_eval(c, s): # c = [b0, b1, b2, b3, b4, b5] result = c[5] result = result * s + c[4] result = result * s + c[3] result = result * s + c[2] result = result * s + c[1] result = result * s + c[0] return result

如果你还需要速度，可以对归一化多项式求导，得到速度的表达式，同样用 Horner 求值。加速度同理。

3.3 多段轨迹的拼接与过渡

实际应用中，一条完整的运动轨迹往往由多段组成。比如机械臂从 A 点经过 B 点到达 C 点，中间可能还有若干路径点。

最直接的做法是每段单独用五次插值，段与段之间在路径点处速度为零、加速度为零。这样确实能保证连续，但运动效率很低——每到一个路径点都要停下来，动作一顿一顿的。

更好的做法是让路径点处的速度不为零，由前后两段的边界条件共同决定。这就涉及到速度规划的问题，通常需要根据路径几何和动力学约束来分配每个路径点的速度。

一个实用的简化策略是：对于不需要精确停靠的中间点，用前后两段的方向向量来估计一个合理的过点速度，大小可以根据路径长度和曲率来调整。这样既保证了平滑，又不会让运动变得太慢。

实操心得：过点速度的选取非常关键。选大了，轨迹可能超出关节限位或者速度限幅；选小了，运动效率低。我一般会先给一个保守值，跑一遍看实际的速度和加速度曲线，再逐步调整。

3.4 时间同步与周期对齐

在多轴联动的情况下，每个关节的轨迹段可能长度不同。如果各轴独立规划，就会出现有的轴已经到位、有的轴还在动的情况，末端轨迹就会偏离预期。

解决办法是让所有轴共享同一个时间参数。具体来说，先根据各轴的运动需求确定一个统一的段时长 T，然后每个轴都用这个 T 来规划自己的五次多项式。这样各轴同时开始、同时结束，末端轨迹才是可控的。

如果某个轴在 T 时间内达不到目标位置（比如速度限幅卡住了），就需要拉长 T，重新规划所有轴。这是一个迭代过程，通常一到两次就能收敛。

4. 代码实现与参数调试

4.1 完整的 Python 实现

下面是一个可以直接跑的 Python 实现，包含了系数计算、求值和简单的多段拼接：

import numpy as np def quintic_coeffs(q0, v0, a0, q1, v1, a1, T): """计算归一化五次多项式系数""" b0 = q0 b1 = v0 * T b2 = a0 * T * T / 2.0 b3 = 10*(q1-q0) - 6*v0*T - 4*v1*T + (3*a0 - a1)*T*T/2.0 b4 = -15*(q1-q0) + 8*v0*T + 7*v1*T + (-3*a0 + 2*a1)*T*T/2.0 b5 = 6*(q1-q0) - 3*v0*T - 3*v1*T + (a0 - a1)*T*T/2.0 return np.array([b0, b1, b2, b3, b4, b5]) def quintic_eval(coeffs, s): """用 Horner 方法求位置""" c = coeffs return ((((c[5]*s + c[4])*s + c[3])*s + c[2])*s + c[1])*s + c[0] def quintic_eval_deriv(coeffs, s, T): """求速度（对实际时间 t 的导数）""" c = coeffs # 对 s 求导 d = np.array([c[1], 2*c[2], 3*c[3], 4*c[4], 5*c[5]]) ds = ((((d[4]*s + d[3])*s + d[2])*s + d[1])*s + d[0]) return ds / T def quintic_eval_deriv2(coeffs, s, T): """求加速度""" c = coeffs d2 = np.array([2*c[2], 6*c[3], 12*c[4], 20*c[5]]) d2s = (((d2[3]*s + d2[2])*s + d2[1])*s + d2[0]) return d2s / (T*T)

这段代码可以直接嵌入到你的控制循环里。注意quintic_eval_deriv和quintic_eval_deriv2返回的是对实际时间的导数和二阶导数，因为归一化变量 s 和实际时间 t 之间差了一个 T 的因子。

4.2 参数选择与边界条件设定

边界条件的设定直接决定了轨迹的形状。起点和终点的位置通常是给定的，速度和加速度则需要根据实际情况来定。

对于点到点运动，最常用的设定是起止速度为零、起止加速度为零。这样轨迹最平滑，但运动时间会偏长。

对于连续路径跟踪，中间点的速度不能为零，需要根据路径切线方向来估计。一个简单的估计方法是：

v_mid = (q_next - q_prev) / (T_prev/2 + T_next/2)

这个公式用前后两段的位置差除以总时间的一半，得到平均速度作为过点速度。实际使用中效果还不错，但如果路径拐弯比较急，可能需要根据角度做衰减。

加速度的设定更微妙一些。理论上可以设为零，但如果前后两段的加速度不连续，力矩还是会有跳变。更好的做法是让前一段的终点加速度等于后一段的起点加速度，形成一个全局连续的加速度曲线。

4.3 数值稳定性与边界保护

前面提到过，T 很小时除法会出问题。除了加最小时间保护，还有几个地方需要注意：

第一，当 q1 和 q0 非常接近时，b3、b4、b5 的计算会出现大数相减，有效位数损失。如果位置差小于某个阈值（比如 1e-6），可以直接输出恒定值，跳过插值。

第二，如果计算出的速度或加速度超过了系统限幅，需要重新规划。最简单的处理是把 T 拉长，重新算系数。更精细的做法是引入速度约束下的时间最优规划，但那就复杂了。

第三，浮点数累积误差。如果一段轨迹持续很长时间，t 不断累加可能会有微小误差。建议用周期计数乘以周期时长来得到 t，而不是累加 dt。

踩过的坑：我曾经在一个项目里用累加 dt 的方式计时，跑了几个小时之后发现轨迹末端位置偏了几毫米。后来改成用周期数乘以固定周期，问题就消失了。这个坑不大，但排查起来挺费时间的。

5. 常见问题与排查技巧

5.1 轨迹抖动与加速度跳变

现象：机械臂在运动过程中出现高频抖动，或者在某些位置有明显的“咯噔”感。

排查思路：首先检查加速度曲线是否连续。如果加速度有跳变，说明边界条件没设对，或者段与段之间的过渡没有处理好。其次检查控制周期是否稳定，如果周期抖动大，即使轨迹本身平滑，输出也会抖动。

解决办法：确保每段轨迹的起止加速度与相邻段匹配。如果是单段运动，起止加速度设为零。如果控制周期不稳定，考虑用硬件定时器来触发控制循环。

5.2 计算溢出与精度丢失

现象：轨迹在某些点出现异常大的值，或者末端位置明显偏离预期。

排查思路：检查 T 是否过小，导致系数计算时出现除以接近零的数。检查位置差是否过小，导致大数相减损失精度。

解决办法：加最小 T 保护，加位置差阈值判断。如果使用定点数运算，要特别注意系数的缩放因子，避免溢出。

5.3 多轴不同步问题

现象：多轴联动时末端轨迹偏离直线或圆弧，走出来的路径歪歪扭扭。

排查思路：检查各轴的段时长 T 是否一致。如果各轴独立规划且 T 不同，末端轨迹必然偏离。

解决办法：统一各轴的 T，取所有轴所需时间的最大值。如果某个轴的速度或加速度超限，拉长 T 重新规划所有轴。

5.4 实时性不足与周期超时

现象：控制周期偶尔超时，导致轨迹输出不连续。

排查思路：用示波器或者 GPIO 翻转来测量控制循环的实际执行时间。如果五次插值的计算占了很大比例，考虑优化求值方式。

解决办法：用 Horner 方法求值，避免重复计算幂次。如果还是不够快，可以考虑查表法：预先算好一批轨迹点，运行时直接查表加插值。但查表法会牺牲一些灵活性，适合轨迹固定的场景。

6. 进阶话题与扩展方向

6.1 带约束的五次插值

实际系统中，关节有位置限位、速度限幅、加速度限幅，甚至加加速度（jerk）也可能有限制。标准的五次插值只约束了起止点的状态，中间过程是否超限需要额外检查。

一个实用的做法是：先按标准五次插值算一条轨迹，然后采样检查中间点的速度和加速度。如果超限，就拉长 T 重新算。这个过程可以迭代几次，直到满足所有约束。

更系统的方法是引入优化框架，把时间 T 作为优化变量，约束条件是速度和加速度限幅，目标函数是时间最短。这就变成了一个带约束的优化问题，可以用数值求解器来处理。

6.2 与样条插值的对比

五次插值本质上是两点的多项式插值。如果路径点很多，逐段五次插值会导致多项式次数很高，计算量大且数值不稳定。这时候样条插值就更合适。

B 样条和三次样条是机器人轨迹规划中常用的方法。它们用低阶多项式拼接出全局光滑的曲线，计算量可控，而且天然支持多路径点。五次插值更适合点对点的快速规划，样条更适合复杂路径的离线规划。

选择哪种方法，取决于你的具体需求。如果只是简单的点到点运动，五次插值足够好。如果需要跟踪复杂曲线，样条是更好的选择。

6.3 在 ROS 中的集成思路

如果你在用 ROS 做开发，五次插值可以集成到joint_trajectory_controller的轨迹生成环节。具体来说，可以写一个自定义的轨迹生成器，接收FollowJointTrajectory动作请求，用五次插值生成每个控制周期的位置指令，然后发给控制器。

关键点是要跟 ROS 的控制周期对齐。通常ros_control的周期是 1ms 或 10ms，你的插值器需要在这个周期内完成计算并输出。如果计算量太大，可以考虑把插值放在单独的线程里，用实时缓冲区来传递数据。

个人体会：ROS 的实时性一直是个痛点。如果对实时性要求很高，建议把插值逻辑放到更底层的控制器里，ROS 只负责发送目标点和参数。这样能避开 ROS 通信的不确定性。

6.4 从五次到更高阶的思考

五次插值保证了加速度连续，但加加速度（jerk）仍然是不连续的。对于超高精度的应用，比如光学镜片打磨或者精密装配，jerk 不连续也可能引起振动。

理论上可以继续往上推到七次多项式，约束起止的 jerk。但多项式次数越高，数值稳定性越差，计算量也越大。实际工程中，五次插值已经能满足绝大多数场景的需求。如果确实需要 jerk 连续，可以考虑用滤波方法对五次插值的输出做平滑，或者直接上七次插值。

我个人的经验是：先把五次插值用熟，遇到具体问题再考虑升级。不要一上来就追求最高阶，那样容易陷入过度设计的陷阱。

7. 写在最后

五次插值这个事，说复杂也复杂，说简单也简单。核心就是六个系数、六个约束条件，推导一遍就明白了。真正花时间的是工程实现中的各种细节：时间同步、边界保护、数值稳定性、实时性优化。

我见过不少项目，算法本身没问题，但工程实现上偷了懒，结果跑起来各种问题。比如没做最小时间保护，偶尔出现除零；比如没统一多轴时间，末端轨迹走偏；比如用累加计时，跑久了精度漂移。这些问题都不难解决，但需要你在写代码的时候就想到。

如果你正在做机器人轨迹规划相关的工作，建议把五次插值作为一个基础工具掌握好。它不一定是最优解，但在大多数场景下都够用，而且实现简单、调试方便。等你把五次插值吃透了，再去看样条、看优化方法，会顺畅很多。

最后分享一个小技巧：调试轨迹的时候，把位置、速度、加速度三条曲线都画出来。很多时候问题一眼就能看出来——速度曲线有尖角，说明加速度不连续；加速度曲线有跳变，说明边界条件没设对。图形化调试比看数字快得多。