前言

今天我們來聊聊軌跡插值，在機器人的運動規劃和控制領域，參考軌跡的生成是一個歷史悠久的問題，已經發展出了一系列的方法。今天我們就來聊一聊軌跡插值領域中最常見的軌跡插值方法：多項式插值。

說明：本文約5000字，全部看懂需要一定的時間，建議先收藏。本文所涉及的代碼全部開源，提供Matlab和Python兩個版本，倉庫地址詳見文末。

首先，我們定義一下問題：

如下圖所示，給定一些離散的數據點，我們需要通過插值的方式生成一條能夠通過所有給定數據點的參考曲線。

并且，根據實際應用對于“平滑”的要求，通常會有以下不同的約束：

要求生成的參考曲線是連續的；

在1的基礎上，要求參考曲線的速度是連續的；

在1和2的基礎上，要參考曲線的加速度是連續的；

注意：速度是位置對時間的導數，加速度是速度對時間的導數，加速度對時間的導數我們稱之為jerk。

通常情況下，在機器人高速運動的時候，想要得到非常連續、平滑、噪音低的運動控制，第3個約束條件是必不可少的，有的甚至還要求加速度的導數jerk都是連續的。

在多項式插值里面，給定多項式的階次越高，能擬合的函數曲線就越復雜，但越高階次的多項式對于計算資源的要求越多。因此對于這3個要求，我們可以分別用不同階次的多項式函數來擬合，實際應用時根據需求選擇合適的方法。

1. 線性插值（一階，恒定速度）

線性插值，顧名思義，就是使用線性的方法來進行插值。即將給定的數據點依次用線段連起來，點與點之間運動的速度是恒定值。假設我們用

來表示插值以后的曲線，則用數學的方式來表示線性插值就是：

其中，

是待確定的常量參數。 表示初始時刻， 表示初始時刻的位置， 表示斜率，也就是速度，這里為常量。因此，給定下一個時刻 處的位置 ，我們就有：

可以計算得到兩個常量參數：

曲線的速度為：

線性插值的實驗結果為：

線性插值

紅圈內為給定的中間點值，黑色實線為插值的結果。

從圖中可以明顯地看到，線性插值帶來的最大問題就是在各個數據點交接處會出現一個急劇的“拐彎”，在這個拐彎處其速度不連續，因此對于運動控制來說，在這里會有一個速度的階躍。例如，對于電機來說，這里會導致控制電流急劇變化，使得電機的輸出不穩定，從而引發“抖動”問題，嚴重者還會損壞機構本身。因此，線性插值本身的問題導致其在控制領域應用范圍受限。

2. 拋物線插值（二階，恒定加速度）

拋物線差值（Parabolic Spline）是二階多項式插值方法。與線性插值法將各個數據點用線段連起來不同，拋物線插值方法是用二次曲線將各個數據點連接起來，在連接處使用平滑的曲線來過渡，而避免速度不連續導致的“急劇拐彎”。拋物線差值的特征是具有恒定的加速度/減速度，一般是由兩個多項式的組合來得到。為什么是兩個多項式呢？因為一個用于“加速階段”，一個用于“減速階段”?！凹铀匐A段”和“減速階段”的分割點叫flex point。

考慮2個數據點之間插值的情況。假設初始時刻是

, 在flex point處對應的時刻是 ，最終時刻為 。

先來看一個特殊情況，flex point的位置是起點和終點的中間位置（也可以任意設定，后面我們會討論，這里先討論在中間位置的情況），即

， 。我們作幾個符號定義： 。

2.1 加速階段

對于加速階段，其數學表達式為：

其中，

是待確定的常量參數，當我們給定 和 以及初始時刻的速度 以后，有如下關系：

因此，常量參數可以計算為：

這樣，我們可以計算得到當

時，插值曲線為：

在flex point的速度可以這樣計算：

大家可能注意到了，與線性插值不同的是，除了指定初始時刻的位置，我們還需要給定初始時刻的速度

。

在flex point處，由加速階段進入減速階段，因此此時加速度的符號是會反轉的，會導致加速度不連續。

2.2 減速階段

對于減速階段，其數學表達式為：

其中，$a_3, a_4, a_5$為待定的常量參數。如果給定最終時刻的速度$v_1$，則有如下關系：

因此，我們可以計算得到：

這樣，當

時，插值曲線為：

這里值得注意的是，如果

，那么在 處（flex point），速度曲線是不連續的。

如果在

處， 不處于起點和終點的中間位置，即不滿足 ，那么，為了保證速度曲線的連續，即 ，我們有以下關系：

其中，

，則聯立多項式我們可以得到：

從圖中我們可以看到，插值的結果中，加速度并不恒定，在

時刻，加速度存在一個階躍，但加速度和減速度的絕對值是一樣的，因此，這種情況屬于加速度對稱的情況，加速時間和減速時間也一致，都為 。

本文附帶的代碼里面實現了加速度對稱的情況，但是留了設置不對稱加速度的接口，非常容易實現。

下面我們來看一下加速度不對稱的情況。

2.3 加速度不對稱的情況

直觀上，加速度不對稱也就是表示加速時間和減速時間不一致，也就是說

，但是不局限于 ， 可以是區間內的任意值。此時，我們可以結合前面的兩個多項式來構造插值的曲線：

給定初始時刻

和最終時刻 處的位置和速度信息，且給定在 處要保證位置和速度曲線的連續性，則我們可以計算得到插值曲線：

假定加速時間為

，減速時間為 ，則由上述多項式可以計算得到：

因此，在

階段，曲線的速度和加速度可以計算為：

在

階段，曲線的速度和加速度可以計算為：

值得注意的是，如果

，那么就與前面我們討論的加速度對稱的情況結果一致了。實驗結果如下：

二次多項式插值

從圖中可以看到，位置曲線是“平滑”的，速度曲線是連續的，加速度也是恒定的（在加速和減速階段內保持恒定），但是加速度曲線在

時刻存在一個階躍。

3. 三次多項式插值（三階，加速度可變）

三次多項式插值方法（Cubic Spline）是一種常用的插值方法，其位置和速度曲線是連續的，加速度是可變的，但加速度不一定連續?？紤]2個數據點之間插值的情況，其數學表達式為：

其中，

為待確定的參數。

3.1 給定每一個點的位置和速度信息

考察給定2個數據點進行插值的情況，如果給定了在初始時刻

和最終時刻 處的位置與速度信息（ ），設 ，則這些參數可以使用以下公式計算：

對于給定

個一系列數據點進行插值的情況，只需要對所有相鄰的兩個數據點使用上述公式即可依次計算得到整條插值曲線。

3.2 給定每一個點的位置信息，但中間點的速度未給定

如果我們只是通過給定一系列的位置信息（

），而中間點的速度信息并未給定，整條曲線最開始的起點和最終的終點速度需要直接給定，一般為零， 。中間各個數據點的速度我們可以通過啟發式方法得到，即通過求解位置對時間的導數得到，那么對于第$k$個中間點，我們有：

其中，

，表示曲線的導數或者“斜率”， 為符號函數，返回值為 或者 。直觀上的理解也就是說，考察第 個數據點，如果其導數在該點進行了符號反轉，則該點速度為 ，否則，該點速度為其導數。

三次多項式插值能夠保證位置曲線和速度曲線是連續的，但加速度曲線不一定連續。雖然已經可以滿足許多應用上對于“平滑”的要求了，但是在高速控制領域，一般要求加速度也要是連續的。因此，我們需要引入更高階次的多項式插值方法。

實驗結果如下：

三次多項式插值

從圖中可以看到，位置曲線是“平滑”的，速度曲線是連續的，加速度曲線是可變的，但是不連續。這樣，對于高速控制的場合來說，控制器的輸入仍然會存在階躍，導致不連續的情況。

4. 五次多項式插值（五階，加速度連續）

考慮2個數據點之間插值的情況，與其他階次的多項式形式類似，五次多項式插值方法的數學表達式為：

其中，

為待確定的參數。這里我們一共需要6個約束條件，即起點 和終點 的位置、速度和加速度信息。即給定如下條件：

設

，則我們可以計算得到：

對于具有

個數據點的情況，可以對所有相鄰的2個點應用上述公式，最終得到最終的插值曲線。實驗結果如下：

五次多項式插值

從圖中可以看出，位置、速度、加速度三條曲線都是連續的，并且位置和速度還是“平滑”的。到這里，我們已經滿足了本文最開始所提到的三個要求。因此，五階的多項式插值已經能夠覆蓋大多數應用場景。如果我們對加速度曲線也要求是平滑的，那么就需要更高階次的多項式插值方法了，例如七階多項式插值。

5. 七次及更高階次的多項式插值

理論上，多項式的階次越高，我們可以獲得越“平滑”的曲線，但是同時帶來的是對運算資源要求的急劇上升，所以一般情況下，七次及更高階次的多項式方法只用于某些特殊的場合，因此這里我們不再做深入的分析，有興趣的讀者請參考經典書籍《Trajectory Planning for Automatic Machines and Robots》，可在微信公眾號【博士的沙漏】底部點擊菜單欄或后臺回復【TPA】獲取下載鏈接。網上類似的書籍有：

6. 實驗結果對比

在實際的實驗中，我們除了實現給定位置點，還給定了速度點和加速度點。這里我們放一張所有方法插值結果的對比圖，從中可以直觀地看到使用各個階次多項式進行插值的結果差異。

本文所涉及的代碼全部開源，提供Matlab和Python兩個版本：

Github倉庫?github.com

參考

《Trajectory Planning for Automatic Machines and Robots》

與50位技術專家面對面20年技術見證，附贈技術全景圖

總結

以上是生活随笔為你收集整理的matlab 平滑曲线连接_平滑轨迹插值方法之多项式插值（附代码）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。