基于三次參數(shù)樣條的機(jī)器人路徑平滑算法研究

喬亮 李澤文 胡杰 李照祥

摘要：本文針對(duì)機(jī)器人運(yùn)動(dòng)規(guī)劃中必須解決的路徑平滑問(wèn)題，對(duì)常用的分段插值算法進(jìn)行對(duì)比分析，緊密聯(lián)系實(shí)際，說(shuō)明其局限性，提出了基于三階參數(shù)樣條插值的機(jī)器人路徑平滑的研究。解決了機(jī)器人在實(shí)際運(yùn)動(dòng)規(guī)劃中出現(xiàn)的拐點(diǎn)及曲率不連續(xù)的問(wèn)題，以機(jī)器人本身的硬件能力做參數(shù)調(diào)整。仿真實(shí)驗(yàn)表明調(diào)參后的路徑平滑效果很好。

關(guān)鍵詞：三次參數(shù)樣條;路徑平滑;平滑參數(shù);移動(dòng)機(jī)器人

中國(guó)分類號(hào)：TP301? ?文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2019）22-0191-02

開(kāi)放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

1 背景及研究意義

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

2 三階樣條插值的算法改進(jìn)

2.1 三階樣條插值

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

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

各段擬合曲線系數(shù)不同，通過(guò)給定的n-1個(gè)公式和兩個(gè)端點(diǎn)的限制，得到n+1個(gè)未知量的值。

2.2 三階參數(shù)樣條插值

對(duì)于三次樣條算法不能解決具有垂直切線的問(wèn)題以及[xi+1

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

構(gòu)建一系列樣點(diǎn)在每?jī)牲c(diǎn)之間可以[得到]參數(shù)三次多項(xiàng)式插值的形式：

[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）

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

2.3 參數(shù)的選取

曲率就是針對(duì)曲線上某個(gè)點(diǎn)的切線方向角對(duì)弧長(zhǎng)的轉(zhuǎn)動(dòng)率，數(shù)學(xué)上表明曲線在某一點(diǎn)的彎曲程度的數(shù)值。曲率的倒數(shù)就是曲率半徑。參數(shù)t表示三次參數(shù)樣條中要求單調(diào)遞增的自變量的值，參數(shù)的變化決定了整個(gè)曲線的變化趨勢(shì)，參數(shù)的選取是算法優(yōu)化的重要一欄。其檢驗(yàn)標(biāo)準(zhǔn)以曲率為主，包括連續(xù)性、變化趨勢(shì)和最值。

在常規(guī)坐標(biāo)系中，計(jì)算x、y的一階二階導(dǎo)數(shù)，得到的數(shù)據(jù)在物理屬性上表示速度和加速度分量，本參數(shù)方程選取的自變量是基于分段轉(zhuǎn)換后的坐標(biāo)系，故本文中的參數(shù)方程得到的軌跡速度和加速度不能代表實(shí)際的情況。但計(jì)算的曲率是軌跡屬性，與本文坐標(biāo)系選取關(guān)系無(wú)關(guān)，得到的結(jié)果符合實(shí)際情況。

3 實(shí)驗(yàn)結(jié)果

3.1 三次參數(shù)樣條插值實(shí)驗(yàn)結(jié)果

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

左側(cè)為插值算法的測(cè)試結(jié)果;右側(cè)為插值軌跡與樣點(diǎn)連線對(duì)比圖

調(diào)用構(gòu)造和采樣函數(shù)100次，計(jì)算所消耗的時(shí)間，并與三次B樣條插值的時(shí)間進(jìn)行對(duì)比，結(jié)果如下：

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

3.2 參數(shù)討論

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

圖4表示t的指數(shù)選取2、1、2/3、1/2、1/3時(shí)的路徑平滑圖，單位為1。以參數(shù)t=[h23]為標(biāo)準(zhǔn)，由圖可以看出，t的指數(shù)值越大則路徑長(zhǎng)度越長(zhǎng)，但對(duì)于t指數(shù)為2時(shí)，路徑過(guò)長(zhǎng)且存在路徑交疊，不可取。t的指數(shù)值越小，在路徑長(zhǎng)度方面更優(yōu)。

圖5中表示t的指數(shù)取1、2/3、1/2、1/3時(shí)的路徑平滑軌跡以及以0.05個(gè)單位為間隔的曲率點(diǎn)，單位為1。由圖中可見(jiàn)，在某些軌跡變化幅度較大的地方，各個(gè)t的指數(shù)值的曲率變化突顯，最值之差較大。軌跡平滑的區(qū)域，曲率差別不大且變化平穩(wěn)。參數(shù)t的指數(shù)值越小，對(duì)應(yīng)軌跡的最大曲率值越大，且局部曲率變化增長(zhǎng)幅度也越大。

減小軌跡長(zhǎng)度，可以降低機(jī)器人運(yùn)動(dòng)的能源損耗;曲率半徑（曲率倒數(shù)）參數(shù)值對(duì)機(jī)器人的轉(zhuǎn)向能力提出了要求，降低曲率可以降低機(jī)器人在轉(zhuǎn)向能力的要求，繼而降低機(jī)器人硬件成本。因而，需要根據(jù)實(shí)際機(jī)器人的硬件的實(shí)際能力和硬件成本，考慮路徑長(zhǎng)度優(yōu)化及轉(zhuǎn)向能力選取最優(yōu)t值。

4 總結(jié)

本文通過(guò)說(shuō)明常用的三階樣條插值及B樣條插值的實(shí)際應(yīng)用優(yōu)缺點(diǎn)，尤其針對(duì)機(jī)器人路徑規(guī)劃領(lǐng)域中的局限性進(jìn)行說(shuō)明。對(duì)于機(jī)器人領(lǐng)域的應(yīng)用提出了三階參數(shù)樣條插值，實(shí)現(xiàn)了包含一階二階導(dǎo)數(shù)連續(xù)，曲率連續(xù)，不受單調(diào)性限制，擬合軌跡包含樣點(diǎn)。并對(duì)選取的自變量參數(shù)t進(jìn)行充分討論分析，結(jié)合實(shí)際機(jī)器人性能選取最優(yōu)的參數(shù)。

參考文獻(xiàn)：

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

[2] 任重，楊燦軍，陳鷹.軌跡規(guī)劃中的B樣條插值算法[J].機(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] 張彩明.高精度三次參數(shù)樣條曲線的構(gòu)造[J].計(jì)算機(jī)學(xué)報(bào)，2002，25（3）：262-268.

[6] 許小勇，鐘太勇.三次樣條插值函數(shù)的構(gòu)造與Matlab實(shí)現(xiàn)[J].兵工自動(dòng)化，2006，25（11）：76-78.

【通聯(lián)編輯：梁書(shū)】