三次样条插补的实现

     机械臂的平滑运动需要确定各个轨迹点的位置、速度、各点间的运行时间，甚至还需要加速度。应对这种需求，一般驱动器设计会做三次样条插值（Cubic Spline Interpolation），控制端需要发送PVT信息，没有加速度信息。

相对于插值曲线函数规划，驱动器的问题在于跟踪，因为规划的路径是理想的，但实际上电机运行受各种扰动的影响，如何很好的动态跟踪，达到设定目的才是关键。

设i为当前位置点坐标，j为下一位置点坐标，Δt为间隔时间，三次样条插值的计算公式如下：

插值参数

d = P[t₀] = P[i]

c = P[t₀] = V[i]

b = -Δt ^-2 * ( 3 * ( P[i] - P[j] ) + Δt * ( 2 * V[i] + V[j] ) )

a = Δt ^-3 * ( 2 * ( P[i] - P[j] ) + Δt * ( V[i] + V[j] ) )

插值函数：

P(t) = a * t³ + b * t² + c * t + d 

V(t) = 3a * t² + 2b * t + c

A(t) = 6a * t + 2b

考虑实际运行中的各种扰动，必须实时更新轨迹函数，才能确保准时、准速到达目标点。