前言
今天我們來聊聊軌迹插值，在機器人的運動規劃和控制領域，參考軌迹的生成是一個曆史悠久的問題，已經發展出了一系列的方法。今天我們就來聊一聊軌迹插值領域中最常見的軌迹插值方法：多項式插值。
說明：本文約5000字，全部看懂需要一定的時間，建議先收藏。本文所涉及的代碼全部開源，提供Matlab和Python兩個版本，倉庫位址詳見文末。
首先，我們定義一下問題：
如下圖所示，給定一些離散的資料點，我們需要通過插值的方式生成一條能夠通過所有給定資料點的參考曲線。
并且，根據實際應用對于“平滑”的要求，通常會有以下不同的限制：
- 要求生成的參考曲線是連續的；
- 在1的基礎上，要求參考曲線的速度是連續的；
- 在1和2的基礎上，要參考曲線的加速度是連續的；
注意：速度是位置對時間的導數，加速度是速度對時間的導數，加速度對時間的導數我們稱之為jerk。
通常情況下，在機器人高速運動的時候，想要得到非常連續、平滑、噪音低的運動控制，第3個限制條件是必不可少的，有的甚至還要求加速度的導數jerk都是連續的。
在多項式插值裡面，給定多項式的階次越高，能拟合的函數曲線就越複雜，但越高階次的多項式對于計算資源的要求越多。是以對于這3個要求，我們可以分别用不同階次的多項式函數來拟合，實際應用時根據需求選擇合适的方法。
1. 線性插值（一階，恒定速度）
線性插值，顧名思義，就是使用線性的方法來進行插值。即将給定的資料點依次用線段連起來，點與點之間運動的速度是恒定值。假設我們用來表示插值以後的曲線，則用數學的方式來表示線性插值就是：
其中，是待确定的常量參數。表示初始時刻，表示初始時刻的位置，表示斜率，也就是速度，這裡為常量。是以，給定下一個時刻處的位置，我們就有：
可以計算得到兩個常量參數：
曲線的速度為：
線性插值的實驗結果為：
紅圈内為給定的中間點值，黑色實線為插值的結果。
從圖中可以明顯地看到，線性插值帶來的最大問題就是在各個資料點交接處會出現一個急劇的“拐彎”，在這個拐彎處其速度不連續，是以對于運動控制來說，在這裡會有一個速度的階躍。例如，對于電機來說，這裡會導緻控制電流急劇變化，使得電機的輸出不穩定，進而引發“抖動”問題，嚴重者還會損壞機構本身。是以，線性插值本身的問題導緻其在控制領域應用範圍受限。
2. 抛物線插值（二階，恒定加速度）
抛物線內插補點（Parabolic Spline）是二階多項式插值方法。與線性插值法将各個資料點用線段連起來不同，抛物線插值方法是用二次曲線将各個資料點連接配接起來，在連接配接處使用平滑的曲線來過渡，而避免速度不連續導緻的“急劇拐彎”。抛物線內插補點的特征是具有恒定的加速度/減速度，一般是由兩個多項式的組合來得到。為什麼是兩個多項式呢？因為一個用于“加速階段”，一個用于“減速階段”。“加速階段”和“減速階段”的分割點叫flex point。
考慮2個資料點之間插值的情況。假設初始時刻是, 在flex point處對應的時刻是，最終時刻為 。
先來看一個特殊情況，flex point的位置是起點和終點的中間位置（也可以任意設定，後面我們會讨論，這裡先讨論在中間位置的情況），即 ，。我們做幾個符号定義：。
2.1 加速階段
對于加速階段，其數學表達式為：
其中，是待确定的常量參數，當我們給定和以及初始時刻的速度以後，有如下關系：
是以，常量參數可以計算為：
這樣，我們可以計算得到當 時，插值曲線為：
在flex point的速度可以這樣計算：
大家可能注意到了，與線性插值不同的是，除了指定初始時刻的位置，我們還需要給定初始時刻的速度。
在flex point處，由加速階段進入減速階段，是以此時加速度的符号是會反轉的，會導緻加速度不連續。
2.2 減速階段
對于減速階段，其數學表達式為：
其中，為待定的常量參數。如果給定最終時刻的速度，則有如下關系：
是以，我們可以計算得到：
這樣，當時，插值曲線為：
這裡值得注意的是，如果 ，那麼在處（flex point），速度曲線是不連續的。
如果在處，不處于起點和終點的中間位置，即不滿足，那麼，為了保證速度曲線的連續，即，我們有以下關系：
其中，，則聯立多項式我們可以得到：
從圖中我們可以看到，插值的結果中，加速度并不恒定，在時刻，加速度存在一個階躍，但加速度和減速度的絕對值是一樣的，是以，這種情況屬于加速度對稱的情況，加速時間和減速時間也一緻，都為。
本文附帶的代碼裡面實作了加速度對稱的情況，但是留了設定不對稱加速度的接口，非常容易實作。
下面我們來看一下加速度不對稱的情況。
2.3 加速度不對稱的情況
直覺上，加速度不對稱也就是表示加速時間和減速時間不一緻，也就是說 ，但是不局限于，可以是區間内的任意值。此時，我們可以結合前面的兩個多項式來構造插值的曲線：
$$\begin{array}{ll} q_{a}(t)=a_{0}+a_{1}\left(t-t_{0}\right)+a_{2}\left(t-t_{0}\right)^{2}, & t_{0} \leq t<t_{f} \\="" q_{b}(t)="a_{3}+a_{4}\left(t-t_{f}\right)+a_{5}\left(t-t_{f}\right)^{2}," &="" t_{f}="" \leq="" t<t_{1}="" \end{array}="" $$=""和最終時刻處的位置和速度資訊，且在給定在處要保證位置和速度曲線的連續性，則我們可以計算得到插值曲線：
假定加速時間為，減速時間為，則由上述多項式可以計算得到：
是以，在階段，曲線的速度和加速度可以計算為：
在階段，曲線的速度和加速度可以計算為：
值得注意的是，如果，那麼就與前面我們讨論的加速度對稱的情況結果一緻了。
實驗結果如下：
從圖中可以看到，位置曲線是“平滑”的，速度曲線是連續的，加速度也是恒定的（在加速和減速階段内保持恒定），但是加速度曲線在時刻存在一個階躍。
3. 三次多項式插值（三階，加速度可變）
三次多項式插值方法（Cubic Spline）是一種常用的插值方法，其位置和速度曲線是連續的，加速度是可變的，但加速度不一定連續。考慮2個資料點之間插值的情況，其數學表達式為：
其中，為待确定的參數。
3.1 給定每一個點的位置和速度資訊
考察給定2個資料點進行插值的情況，如果給定了在初始時刻和最終時刻處的位置與速度資訊（），設，則這些參數可以使用以下公式計算：
對于給定個一系列資料點進行插值的情況，隻需要對所有相鄰的兩個資料點使用上述公式即可依次計算得到整條插值曲線。
3.2 給定每一個點的位置資訊，但中間點的速度未給定
如果我們隻是通過給定一系列的位置資訊（），而中間點的速度資訊并未給定，整條曲線最開始的起點和最終的終點速度需要直接給定，一般為零，。中間各個資料點的速度我們可以通過啟發式方法得到，即通過求解位置對時間的導數得到，那麼對于第個中間點，我們有：
其中，，表示曲線的導數或者“斜率”，為符号函數，傳回值為或者。直覺上的了解也就是說，考察第個資料點，如果其導數在該點進行了符号反轉，則該點速度為0，否則，該點速度為其導數。
三次多項式插值能夠保證位置曲線和速度曲線是連續的，但加速度曲線不一定連續。雖然已經可以滿足許多應用上對于“平滑”的要求了，但是在高速控制領域，一般要求加速度也要是連續的。是以，我們需要引入更高階次的多項式插值方法。
實驗結果如下：
從圖中可以看到，位置曲線是“平滑”的，速度曲線是連續的，加速度曲線是可變的，但是不連續。這樣，對于高速控制的場合來說，控制器的輸入仍然會存在階躍，導緻不連續的情況。
4. 五次多項式插值（五階，加速度連續）
考慮2個資料點之間插值的情況，與其他階次的多項式形式類似，五次多項式插值方法的數學表達式為：
其中，為待确定的參數。這裡我們一共需要6個限制條件，即起點和終點的位置、速度和加速度資訊。即給定如下條件：
設，則我們可以計算得到：
對于具有個資料點的情況，可以對所有相鄰的2個點應用上述公式，最終得到最終的插值曲線。
實驗結果如下：
從圖中可以看出，位置、速度、加速度三條曲線都是連續的，并且位置和速度還是“平滑”的。到這裡，我們已經滿足了本文最開始所提到的三個要求。是以，五階的多項式插值已經能夠覆寫大多數應用場景。如果我們對加速度曲線也要求是平滑的，那麼就需要更高階次的多項式插值方法了，例如七階多項式插值。
5. 七次及更高階次的多項式插值
理論上，多項式的階次越高，我們可以獲得越“平滑”的曲線，但是同時帶來的是對運算資源要求的急劇上升，是以一般情況下，七次及更高階次的多項式方法隻用于某些特殊的場合，是以這裡我們不再做深入的分析，有興趣的讀者請參考經典書籍《Trajectory Planning for Automatic Machines and Robots》
6. 實驗結果對比
在實際的實驗中，我們除了實作給定位置點，還給定了速度點和加速度點。這裡我們放一張所有方法插值結果的對比圖，從中可以直覺地看到使用各個階次多項式進行插值的結果差異。
本文所涉及的代碼全部開源，提供Matlab和Python兩個版本，Github倉庫位址：https://github.com/chauby/PolynomialInterpolation
參考
《Trajectory Planning for Automatic Machines and Robots》