三次樣條插補的實現

     機械臂的平滑運動需要確定各個軌跡點的位置、速度、各點間的運行時間，甚至還需要加速度。應對這種需求，一般驅動器設計會做三次樣條插值（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

考慮實際運行中的各種擾動，必須實時更新軌跡函數，才能確保准時、准速到達目標點。