基于三次參數樣條的機器人路徑平滑算法研究

喬亮 李澤文 胡杰 李照祥

摘要：本文針對機器人運動規劃中必須解決的路徑平滑問題，對常用的分段插值算法進行對比分析，緊密聯系實際，說明其局限性，提出了基于三階參數樣條插值的機器人路徑平滑的研究。解決了機器人在實際運動規劃中出現的拐點及曲率不連續的問題，以機器人本身的硬件能力做參數調整。仿真實驗表明調參后的路徑平滑效果很好。

關鍵詞：三次參數樣條;路徑平滑;平滑參數;移動機器人

中國分類號：TP301? ?文獻標識碼：A

文章編號：1009-3044（2019）22-0191-02

開放科學（資源服務）標識碼（OSID）：

1 背景及研究意義

為了提高工作精度，在實際機器人開發中，運動過程不允許速度、加速度、航向角和曲率發生突變，在實現例如避障、抓取等功能的同時，保證機器人平穩運動，這對機器人在運動過程中的路徑規劃提出嚴格要求。三次樣條插值，構造的曲線二次連續，且構造時只需事先給出很少的導數信息，如端點條件（這里我們選取自由邊界，各區間首尾兩端二次導為0）。但求解中不能出現軌跡平行或垂直于坐標軸[1]。B樣條插值允許局部控制曲線的形狀，又能保證曲率的連續性，但由于其原理導致不能經過所有的樣點[2]。然而機器人在運動中，所在空間可能由于障礙物出現往返運動，導致軌跡圖計算會出現垂直切線問題。為了滿足機器人實際運動的要求，本文提出了基于三次參數樣條的機器人路徑平滑算法，并討論如何根據硬件條件選擇適合的參數。

2 三階樣條插值的算法改進

2.1 三階樣條插值

機器人在二維平面地圖信息中包括地圖中的坐標位置，以此將地圖信息離散化，通過地圖坐標信息機器人使用路徑算法來獲取可行的路徑，在此過程中通過給定一系列的路徑點，為了使軌跡更加平滑通過將帶有坐標信息的路徑點來進行插值[3]。樣條插值法是根據可變樣條連接樣點的光滑曲線數學方法[4]。滿足路徑規劃的條件需要：三次樣條曲線在銜接點處是連續光滑的;選用自由邊界的三次樣條（Nature Cubic Spline），且邊界二階導數也是連續的。在任意兩個路徑點[SiSi+1]之間可以通過三次多項式生成一段平滑的曲線。該插值函數可以通過以下方程形式表示：

[Six=a i+b i（x-xi）+c i（x-xi）2+d i（x-xi）3]? ? ?（1）

各段擬合曲線系數不同，通過給定的n-1個公式和兩個端點的限制，得到n+1個未知量的值。

2.2 三階參數樣條插值

對于三次樣條算法不能解決具有垂直切線的問題以及[xi+1

[ti=sqrtxi+1-xi2+yi+1-yi2+ti-1]? （[t0]=0）? ? ? ? ? ? ?（2）

構建一系列樣點在每兩點之間可以[得到]參數三次多項式插值的形式：

[x=a i+b i（t-ti）+c i（t-ti）2+di（t-ti）3]? ? ? ? ? ?（3）

[y=a i+b i（t-ti）+c i（t-ti）2+d i（t-ti）3]? ? ? ? ? ?（4）

每一段曲線中，通過上述求解三階樣條的方法[6]，即通過公式（3）和（4）的求解過程可分別得到x、y兩個關于t的三階樣條插值方程。每段自變量t的坐標系是不同的，但量化標準是一致的，與每段起始點到終止點的連線長度和方向有關。這樣就很好地解決了原始坐標系的機器人垂直、水平及往返運動的情況。這里選取的邊界條件為自由邊界，因此可以得到每一段曲線的參數方程，其流程如下：

2.3 參數的選取

曲率就是針對曲線上某個點的切線方向角對弧長的轉動率，數學上表明曲線在某一點的彎曲程度的數值。曲率的倒數就是曲率半徑。參數t表示三次參數樣條中要求單調遞增的自變量的值，參數的變化決定了整個曲線的變化趨勢，參數的選取是算法優化的重要一欄。其檢驗標準以曲率為主，包括連續性、變化趨勢和最值。

在常規坐標系中，計算x、y的一階二階導數，得到的數據在物理屬性上表示速度和加速度分量，本參數方程選取的自變量是基于分段轉換后的坐標系，故本文中的參數方程得到的軌跡速度和加速度不能代表實際的情況。但計算的曲率是軌跡屬性，與本文坐標系選取關系無關，得到的結果符合實際情況。

3 實驗結果

3.1 三次參數樣條插值實驗結果

生成的平滑軌跡如圖2所示：主要選取邊界情況與異常點情況，進行測試主要考慮，水平點和垂直的點，以及點過少，步長的大小，以及曲線的抖動情況并對遇到的問題進行程序修改，保證程序穩定。這里輸入為一串點包括三個連續垂直和水平的點。這里選取112個單位長的一段曲線，從中選取20個插值點，以0.5個單位的步長進行采樣，共采得226個采樣點。圖2中的虛線表示之前通過路徑規劃算法直接連接的路徑，紅色的實線為平滑后的路徑對比。

左側為插值算法的測試結果;右側為插值軌跡與樣點連線對比圖

調用構造和采樣函數100次，計算所消耗的時間，并與三次B樣條插值的時間進行對比，結果如下：

從圖3可以看出在調用三階參數樣條算法計算100次所消耗的時間為89ms左右，而三階B樣條算法計算100次所消耗的時間為181ms左右，因此每次采用三階參數樣條算法處理更高效。

3.2 參數討論

對于曲線軌跡平滑，參數調整主要體現在自變量選取。令[（xi-xi-1）2-（yi-yi-1）2]為[h]，下圖4中，展示t為[h13]、[h12]（對應歐式距離）、[h23]、[h1]、[h2]時的三階樣條插值的對比圖，樣點滿足自變量自由變換，二維平面取樣點。這里以（-4，1.2）、（-2，0.6）、（1.5，1.3）、（-0.1，1.5）、（4，3.8）、（6，5）、（10，3）先后插值平滑。

圖4表示t的指數選取2、1、2/3、1/2、1/3時的路徑平滑圖，單位為1。以參數t=[h23]為標準，由圖可以看出，t的指數值越大則路徑長度越長，但對于t指數為2時，路徑過長且存在路徑交疊，不可取。t的指數值越小，在路徑長度方面更優。

圖5中表示t的指數取1、2/3、1/2、1/3時的路徑平滑軌跡以及以0.05個單位為間隔的曲率點，單位為1。由圖中可見，在某些軌跡變化幅度較大的地方，各個t的指數值的曲率變化突顯，最值之差較大。軌跡平滑的區域，曲率差別不大且變化平穩。參數t的指數值越小，對應軌跡的最大曲率值越大，且局部曲率變化增長幅度也越大。

減小軌跡長度，可以降低機器人運動的能源損耗;曲率半徑（曲率倒數）參數值對機器人的轉向能力提出了要求，降低曲率可以降低機器人在轉向能力的要求，繼而降低機器人硬件成本。因而，需要根據實際機器人的硬件的實際能力和硬件成本，考慮路徑長度優化及轉向能力選取最優t值。

4 總結

本文通過說明常用的三階樣條插值及B樣條插值的實際應用優缺點，尤其針對機器人路徑規劃領域中的局限性進行說明。對于機器人領域的應用提出了三階參數樣條插值，實現了包含一階二階導數連續，曲率連續，不受單調性限制，擬合軌跡包含樣點。并對選取的自變量參數t進行充分討論分析，結合實際機器人性能選取最優的參數。

參考文獻：

[1] 張小江，高秀華.三次樣條插值在機器人軌跡規劃應用中的改進研究[J].機械設計與制造，2008（9）：170-171.

[2] 任重，楊燦軍，陳鷹.軌跡規劃中的B樣條插值算法[J].機電工程，2001（5）：38-39.

[3] Elbanhawi M ， Simic M ， Jazar R N . Continuous Path Smoothing for Car-Like Robots Using B-Spline Curves[J]. Journal of Intelligent and Robotic Systems， 2015， 80.

[4] Durrleman S， Simon R. Flexible regression models with cubic [J].Statistics in Medicine， 1989， 8（5）：551-561.

[5] 張彩明.高精度三次參數樣條曲線的構造[J].計算機學報，2002，25（3）：262-268.

[6] 許小勇，鐘太勇.三次樣條插值函數的構造與Matlab實現[J].兵工自動化，2006，25（11）：76-78.

【通聯編輯：梁書】