鄭天江，李俊杰，陳慶盈，楊桂林，張 馳

（中科院寧波材料技術(shù)與工程研究所，寧波 315201）

基于三次B樣條的移動(dòng)機(jī)器人實(shí)時(shí)軌跡規(guī)劃研究

鄭天江，李俊杰，陳慶盈，楊桂林，張 馳

（中科院寧波材料技術(shù)與工程研究所，寧波 315201）

基于B樣條曲線生成的軌跡曲線具有幾何不變性和連續(xù)性等優(yōu)點(diǎn)，被廣泛應(yīng)用于數(shù)控機(jī)床設(shè)備或機(jī)器人的軌跡規(guī)劃算法中，然而傳統(tǒng)的軌跡規(guī)劃基本上是在離線的情況下進(jìn)行的，即所有的型值點(diǎn)已知，或者控制點(diǎn)已知，然而這種方法很難運(yùn)用到移動(dòng)機(jī)器人的軌跡規(guī)劃中，此外如果簡單地將B樣條軌跡規(guī)劃方法應(yīng)用于移動(dòng)機(jī)器人的軌跡規(guī)劃，即通過給定控制點(diǎn)，進(jìn)行插補(bǔ)生成一條新的軌跡曲線，但是這種方法通常不會(huì)經(jīng)過所有的控制點(diǎn)，這樣會(huì)導(dǎo)致實(shí)際運(yùn)行路徑與規(guī)劃的路徑有偏差，提出了一種基于三次B樣條的移動(dòng)機(jī)器人實(shí)時(shí)軌跡規(guī)劃算法，在移動(dòng)機(jī)器人的運(yùn)動(dòng)過程中能夠?qū)崟r(shí)生成插補(bǔ)點(diǎn)，不需要預(yù)先輸入所有軌跡點(diǎn)（型值點(diǎn)），并利用反求控制點(diǎn)算法保證機(jī)器人能夠經(jīng)過所有的（型值點(diǎn)），從而保證機(jī)器人既能經(jīng)過所有預(yù)先規(guī)定的點(diǎn)又能保證機(jī)器人運(yùn)行的連續(xù)性和平穩(wěn)性，通過仿真驗(yàn)證了其有效性。

B樣條；實(shí)時(shí)軌跡規(guī)劃；移動(dòng)機(jī)器人；插補(bǔ)算法；樣條函數(shù)

0 引言

軌跡規(guī)劃問題是運(yùn)動(dòng)控制的核心問題，從數(shù)控機(jī)床設(shè)備到機(jī)器人設(shè)備，軌跡規(guī)劃都是其中必不可少的關(guān)系著系統(tǒng)性能的關(guān)鍵因素。一般的數(shù)控機(jī)床或者機(jī)器人搭載的運(yùn)動(dòng)控制器均具有簡單的軌跡規(guī)劃和插補(bǔ)功能，其中梯形曲線、S曲線[1]、三次多項(xiàng)式[2]以及高次多項(xiàng)式算法[3]等最為常見，樣條函數(shù)與軌跡規(guī)劃的原理最早出現(xiàn)在凸輪輪廓設(shè)計(jì)上，隨著自動(dòng)化技術(shù)的發(fā)展，其在數(shù)控機(jī)床和機(jī)器人領(lǐng)域有了廣泛的應(yīng)用。其中B-Spline[4]以及PH（速端螺旋線）[5]等樣條函數(shù)被廣泛采納與應(yīng)用。其中采用B樣條曲線的軌跡規(guī)劃方法可以實(shí)現(xiàn)對(duì)時(shí)間的一階二階導(dǎo)數(shù)連續(xù)，并且形成軌跡的局部支撐性，被較多地應(yīng)用到了數(shù)控機(jī)床上。近年來，隨著工業(yè)機(jī)器人的性能不斷提高，除了簡單的PTP（點(diǎn)對(duì)點(diǎn)）運(yùn)動(dòng)以外，機(jī)器人還需要高性能、復(fù)雜軌跡的運(yùn)動(dòng)模式。B樣條等軌跡規(guī)劃技術(shù)也越來越多地被應(yīng)用于機(jī)器人的軌跡規(guī)劃[6]中，現(xiàn)有的用于機(jī)器人的B樣條大多是集中在離線的軌跡規(guī)劃研究，也就是說，其全局的運(yùn)動(dòng)軌跡點(diǎn)（型值點(diǎn)）已經(jīng)給定，軌跡規(guī)劃的任務(wù)就是通過B樣條算法以軌跡點(diǎn)為參考生成一條優(yōu)化的曲線，然后將優(yōu)化的曲線坐標(biāo)和速度等參數(shù)輸入機(jī)器人控制器。

Koch[7]等人最早將B樣條曲線應(yīng)用于機(jī)器人的軌跡規(guī)劃中，主要是針對(duì)關(guān)節(jié)類型的機(jī)器人，實(shí)現(xiàn)了離線的軌跡規(guī)劃；Ling Chen等人利用Bezier曲線進(jìn)行了輪式機(jī)器人的軌跡規(guī)劃[8]，而Bezier曲線進(jìn)行軌跡規(guī)劃時(shí)需要知道全部控制點(diǎn)的坐標(biāo)，并且控制點(diǎn)的修改會(huì)影響全局的曲線軌跡。上述的方法對(duì)于移動(dòng)機(jī)器人來說很難適用，原因是移動(dòng)機(jī)器人的工作環(huán)境往往是變化的，機(jī)器人很難制造工作環(huán)境中的所有障礙物及即將通過的路徑的環(huán)境動(dòng)態(tài)障礙物，這些信息一般需要利用機(jī)器人自身的傳感器檢測得來，也就是說這些信息是實(shí)時(shí)改變的，因此對(duì)于移動(dòng)機(jī)器人來說，其軌跡規(guī)劃必須是實(shí)時(shí)計(jì)算的。此外雖然有學(xué)者[9]也采用了B樣條的軌跡規(guī)劃應(yīng)用于移動(dòng)機(jī)器人，但是他們的研究中尚沒有考慮到讓軌跡曲線經(jīng)過路徑點(diǎn)，并且也沒有考慮到移動(dòng)機(jī)器人實(shí)時(shí)性的要求。

本文針對(duì)B樣條生成原理進(jìn)行了分析，并且提出了一種基于三次B樣條的實(shí)時(shí)快速算法，用來進(jìn)行移動(dòng)機(jī)器人的軌跡生成，該方法基于分段B樣條的思想，在每一個(gè)時(shí)鐘步長時(shí)取歷史的路徑點(diǎn)和下一個(gè)即將路過的路徑點(diǎn)作為樣條的一段，采用滑動(dòng)的方法在不同時(shí)刻均采用均勻的三次B樣條函數(shù)進(jìn)行軌跡規(guī)劃，并且應(yīng)用反求控制點(diǎn)的算法來保證機(jī)器人的軌跡路過給定的路徑點(diǎn)。

1 傳統(tǒng)B樣條曲線介紹及分析

B樣條基函數(shù)公式由下式給出：

其中，n可以看作為B樣條的次數(shù)，那么三次均勻B樣條基函數(shù)可以寫成：

三次B樣條曲線段P0,3(t)為：

其中Pi為控制點(diǎn)，當(dāng)給定控制點(diǎn)Pi的時(shí)候，利用式(3)就可以求出一系列的滿足三次B樣條的曲線點(diǎn)。

圖1 通過控制點(diǎn)求曲線點(diǎn)

從圖1可以看出，如果把期望的軌跡點(diǎn)作為控制點(diǎn)來求得軌跡曲線，很明顯，軌跡曲線將不通過控制點(diǎn)，因此，本文需要將軌跡點(diǎn)看成是型值點(diǎn)，通過型值點(diǎn)反求控制點(diǎn)來保證生成的曲線經(jīng)過給定的軌跡點(diǎn)。

2 B樣條實(shí)時(shí)曲線生成方法

1）單線段反求控制點(diǎn)算法

從而控制該線段的四個(gè)控制點(diǎn)可寫為Pi-1，Pi-1，Pi，Pi，顯然，對(duì)于單段線段而言，再通過控制點(diǎn)求出來的曲線點(diǎn)，僅僅是過軌跡點(diǎn)的直線插值而已。對(duì)于移動(dòng)機(jī)器人而言，通常我們會(huì)已知移動(dòng)機(jī)器人走過的歷史路徑，并且通過實(shí)時(shí)的障礙物傳感器能夠得當(dāng)局部范圍內(nèi)的一個(gè)或者幾個(gè)障礙物信息，因此，移動(dòng)機(jī)器人即將路過的一個(gè)或數(shù)個(gè)路徑點(diǎn)是可以實(shí)時(shí)得到的，因而由歷史軌跡點(diǎn)和下一時(shí)刻的軌跡點(diǎn)可以生成一組B樣條曲線。

2）多線段反求控制點(diǎn)算法

由多段線段反求控制點(diǎn)的方程可以通過下式給出，對(duì)于n個(gè)點(diǎn)組成的n-1個(gè)線段，可以列出n-2個(gè)獨(dú)立的端點(diǎn)條件方程，即：

在這里需要假設(shè)第i條線段與第i-1條線段的控制點(diǎn)有三個(gè)是重合的，從而保證第i條線段與第i-1條線段的二階連續(xù)性。為了保證方程能夠求解，需要在最左端線段的左端點(diǎn)和最右端線段的右端點(diǎn)設(shè)置重節(jié)點(diǎn)條件，即增加兩個(gè)控制點(diǎn)，P0=P1，Pn+1=Pn，從而可以得出由矩陣形式組成的方程組：

上式可解出n個(gè)控制點(diǎn)，再加上P0和Pn+1共有n+2個(gè)控制點(diǎn)，將上述n+2個(gè)控制點(diǎn)分別作為這n-1個(gè)線段的控制點(diǎn)，每段曲線由四個(gè)控制點(diǎn)控制，分配如圖2所示。

圖2 控制點(diǎn)分配

利用上述控制點(diǎn)及公式(3)便可生成一系列平滑的曲線，由于第i個(gè)線段與第i-1個(gè)線段有三個(gè)控制點(diǎn)是重疊的，因此這些曲線是二階連續(xù)的，即速度和加速度是平滑的。

3）滑動(dòng)實(shí)時(shí)軌跡生成方法

上述方法可以在已知機(jī)器人所需要經(jīng)過的軌跡點(diǎn)情況下生成一條連續(xù)的軌跡曲線，但是此種方法需要一次讀入多個(gè)軌跡點(diǎn)，導(dǎo)致系統(tǒng)在生成一條軌跡曲線之后需要等待多個(gè)時(shí)間間隔，例如，當(dāng)n=5時(shí)，每次運(yùn)算完成后需要等待5個(gè)時(shí)間間隔，當(dāng)選擇的n增大時(shí)，這個(gè)時(shí)間間隔需要更大，從而不能保證機(jī)器人軌跡規(guī)劃的實(shí)時(shí)性。

本文利用滑動(dòng)的方法，先將一定數(shù)量的軌跡點(diǎn)存儲(chǔ)起來，然后在機(jī)器人運(yùn)行的每一時(shí)刻丟棄上一時(shí)刻存儲(chǔ)區(qū)域中的最早一個(gè)軌跡點(diǎn)，然后加入最新讀入的機(jī)器人即將經(jīng)過的軌跡點(diǎn)，例如對(duì)于第si時(shí)刻，假設(shè)存儲(chǔ)區(qū)域中的軌跡點(diǎn)為Ti，Ti+1，…，Ti+n-1，那么可利用這些點(diǎn)求得對(duì)應(yīng)的控制點(diǎn)，生成n-1條軌跡曲線，然后通過優(yōu)化方法選擇其中的某一條軌跡曲線作為機(jī)器人第si時(shí)刻規(guī)劃出來的路線，而在si+1時(shí)刻，我們則可以取軌跡點(diǎn)為Ti+1，Ti+2，…，Ti+n-1，Ti+1，其中Ti+n為最后一次機(jī)器人讀入的軌跡點(diǎn)。在si+1時(shí)刻，我們同樣可以規(guī)劃出n-1條軌跡曲線，而取其中的某一條曲線作為第si+1時(shí)刻規(guī)劃出來的路線。利用這種方法，便可以實(shí)現(xiàn)在機(jī)器人每讀到一個(gè)新的軌跡點(diǎn)時(shí)便馬上生成一條新的軌跡曲線點(diǎn)作為機(jī)器人的軌跡插值點(diǎn)，從而保證了機(jī)器人運(yùn)行的實(shí)時(shí)性。

然而，通過研究發(fā)現(xiàn)，由于在si和si+1時(shí)刻用于規(guī)劃B樣條曲線的軌跡點(diǎn)（型值點(diǎn)）發(fā)生了變化，因此在si和si+1時(shí)刻生成的軌跡曲線在交接觸不能保證二階連續(xù)性，需要后續(xù)的算法探索來改進(jìn)。

3 仿真結(jié)果分析

本文針對(duì)給定的軌跡點(diǎn)利用B樣條函數(shù)進(jìn)行了仿真，給定平面上軌跡點(diǎn)x,y如表1所示。

表1 給定平面上軌跡點(diǎn)x,y值

在軌跡點(diǎn)已知的情況下，首先利用反求控制點(diǎn)方法求得相應(yīng)的控制點(diǎn)，然后將控制點(diǎn)進(jìn)行分段，求出每段曲線的曲線點(diǎn)，如圖3所示。速度和加速度分別如圖4和圖5所示，可以看出，如果是軌跡點(diǎn)已知的離線規(guī)劃，那么其求出的多段軌跡曲線的速度和加速度也是連續(xù)的。

此外，本文采用滑動(dòng)軌跡生成的方法，假設(shè)總共的軌跡點(diǎn)仍然是所設(shè)定表1中的內(nèi)容，但是每次只讀入5個(gè)軌跡點(diǎn)，每次都選擇所生成的5端曲線中的第3段，按照不同時(shí)刻拼接成一個(gè)曲線，由于每個(gè)時(shí)刻都需要5個(gè)數(shù)據(jù)點(diǎn)，因此這里總共規(guī)劃出來的曲線為6段。仿真結(jié)果如圖6、圖7和圖8所示。

圖3 控制點(diǎn)及B樣條曲線

圖4 x,y方向的速度曲線

圖5 x,y方向的加速度曲線

圖6 滑動(dòng)法求得控制點(diǎn)及B樣條曲線

圖7 滑動(dòng)法求得x,y方向的速度曲線

圖8 滑動(dòng)法求得x,y方向的加速度曲線

采用滑動(dòng)方法可以在機(jī)器人每掃描到一個(gè)軌跡點(diǎn)時(shí)便生成一段B樣條曲線，能夠保證機(jī)器人路徑的實(shí)時(shí)規(guī)劃，并且在每一段規(guī)劃出來的曲線中間具有二階連續(xù)性，但是其軌跡曲線的速度和加速度在兩段曲線的拼接處存在一定的波動(dòng)，可將生成的曲線數(shù)據(jù)點(diǎn)進(jìn)行濾波處理來降低波動(dòng)。除此之外在研究過程中還發(fā)現(xiàn)，如果采用的軌跡點(diǎn)數(shù)越多采用此方法拼接出來的實(shí)時(shí)曲線越平滑，并且曲線是否平滑也跟拼接時(shí)選擇曲線中的第幾段有關(guān)，為了進(jìn)一步高其實(shí)用性，下一步可以考慮采用優(yōu)化算法來確定最優(yōu)的選擇方法和軌跡點(diǎn)選擇方法。

4 結(jié)束語

本文提出了一種基于三次均勻B樣條函數(shù)的適用于移動(dòng)機(jī)器人的實(shí)時(shí)軌跡規(guī)劃方法，仿真研究表明，該方法能夠依據(jù)機(jī)器人的歷史路徑點(diǎn)和利用傳感器掃描到的路徑點(diǎn)生成基于B樣條曲線的機(jī)器人實(shí)時(shí)路徑，在曲線的中間具有二階的連續(xù)性，但是在曲線的拼接處有少量的抖動(dòng)情況，因此該方法基本上能夠應(yīng)用于移動(dòng)機(jī)器人的實(shí)時(shí)軌跡規(guī)劃，通過濾波處理或者利用優(yōu)化算法選擇最優(yōu)方案，可以降低曲線之間的跳動(dòng)，實(shí)現(xiàn)移動(dòng)機(jī)器人的平滑、平穩(wěn)運(yùn)行。

[1] 潘海鴻,楊微,陳琳,等.全程S曲線加減速控制的自適應(yīng)分段NURBS曲線插補(bǔ)算法[J].中國機(jī)械工程,2010,21(2)：190-195.

[2] Kee-Whan K, Hyun-Sik K, Young-Kiu C, et al. Optimization of cubic polynomial joint trajectories and sliding mode controllers for robots using evolution strategy： Industrial Electronics, Control and Instrumentation[A].IECON 97.23rd International Conference [C].1997.

[3] 高巖.工業(yè)機(jī)器人軌跡規(guī)劃算法的研究與實(shí)現(xiàn)[D].2014.

[4] Arney T.Dynamic path planning and execution using B-Splines：Information and Automation for Sustainability[A].Third International Conference on,Melbourne,VIC, 2007[C].20074-6 Dec.2007.

[5] Liu IH, Tsai Mc,Cheng MY,et al.Planning and implementation of motion trajectory based on C2 PH spline[A],word[C].2012.

[6] 錢東海,譚偉,趙錫芳.基于B樣條路徑的機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃[J].上海交通大學(xué)學(xué)報(bào),1998(12)：31-35.

[7] Koch P E, Wang K.Introduction of b-splines to trajectory planning for robot manipulators[J].Modeling,Identification and Control：A Norwegian Research Bulletin,1988,9(2)：69-80.

[8] Ling C, Sen W, Huosheng H, et al.Bezier curve based trajectory planning for an intelligent wheelchair to pass a doorway：Control (CONTROL)[A].2012 UKACC International Conference [C].2012.

[9] Sprunk C.Planning Motion Trajectories for Mobile Robots Using Splines[D].Albert Ludwigs University,2008.

A study of robot motion controller based on pure network

ZHENG Tian-jiang, LI Jun-jie, CHEN Qing-ying, YANG Gui-lin, ZHANG Chi

TP242

：A

：1009-0134(2017)05-0004-04

2017-01-20

浙江省“錢江人才”計(jì)劃（QJD1602033）；寧波市數(shù)控一代機(jī)械產(chǎn)品創(chuàng)新應(yīng)用示范工程計(jì)劃（2014BAF10B00）；湖北省省院合作項(xiàng)目：工業(yè)自動(dòng)化開放式智能控制系統(tǒng)研發(fā)及產(chǎn)業(yè)化

鄭天江（1984 -），男，湖南永州人，博士，研究方向?yàn)槎噍S運(yùn)動(dòng)控制系統(tǒng)和軟體機(jī)器人建模仿真與系統(tǒng)控制等。