孫颙琰 郭文勇 孫云嶺 王晶航

（海軍工程大學(xué)動(dòng)力工程學(xué)院 武漢 430033）

1 引言

水下機(jī)械臂已經(jīng)越來(lái)越多的應(yīng)用于艦船水下檢測(cè)與維修［1］，為了水下機(jī)械臂能夠穩(wěn)定、快速地完成水下維修任務(wù)，滿足艦船水下應(yīng)急維修的需求，基于實(shí)驗(yàn)室的六自由度水下機(jī)械臂如圖1所示，對(duì)水下機(jī)械臂進(jìn)行軌跡規(guī)劃研究。

圖1 水下機(jī)械臂實(shí)物圖

機(jī)械臂通常主要是在關(guān)節(jié)空間或笛卡爾空間進(jìn)行軌跡規(guī)劃［2］。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃主要是針對(duì)機(jī)械臂各個(gè)關(guān)節(jié)變量，將關(guān)節(jié)的位移、速度、加速度等參數(shù)表示成時(shí)間的函數(shù)，由于不需要考慮末端執(zhí)行機(jī)構(gòu)的運(yùn)動(dòng)狀態(tài)，該方法主要應(yīng)用在點(diǎn)位控制（PTP）中。在笛卡爾空間中的進(jìn)行軌跡規(guī)劃則主要針對(duì)末端執(zhí)行機(jī)構(gòu)得運(yùn)動(dòng)狀態(tài)，將末端執(zhí)行機(jī)構(gòu)的位姿、速度、加速度等參數(shù)表示成時(shí)間的函數(shù)，該方法適用于連續(xù)軌跡控制（CP）中，常運(yùn)用于對(duì)末端執(zhí)行機(jī)構(gòu)的運(yùn)動(dòng)狀態(tài)有特殊要求的軌跡規(guī)劃［3］。

笛卡爾空間軌跡規(guī)劃對(duì)于機(jī)械臂末端雖然能準(zhǔn)確控制位姿、速度等參數(shù)，但是在規(guī)劃過(guò)程中需要不斷地進(jìn)行運(yùn)動(dòng)學(xué)逆解來(lái)得到其他關(guān)節(jié)角，將位姿序列映射成關(guān)節(jié)角度，而運(yùn)動(dòng)學(xué)逆解的運(yùn)算量比正解的運(yùn)算量大了好幾倍，使得求解時(shí)間增加，而在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃則省去了這一步，使得計(jì)算量大量減少，不用考慮奇異問(wèn)題［4］，還可以直接反映出各關(guān)節(jié)的參數(shù)變換，機(jī)械臂各關(guān)節(jié)的速度、加速度和加加速度等參數(shù)在關(guān)節(jié)空間中實(shí)時(shí)性更好，且控制和描述更方便［5］，因此本文采用在關(guān)節(jié)空間下進(jìn)行軌跡規(guī)劃的方法。

2 關(guān)節(jié)空間軌跡規(guī)劃方法

在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃最常用的方法有三次多項(xiàng)式曲線插值、五次次多項(xiàng)式曲線插值、B樣條曲線插值和NURBS曲線插值等。

2.1 NURBS曲線插值算法

B 樣條曲線是貝塞爾曲線的一般化，就是通過(guò)控制點(diǎn)進(jìn)行局部控制形狀的曲線，廣泛應(yīng)用于曲線擬合中。B 樣條曲線在保留了貝塞爾曲線優(yōu)點(diǎn)的基礎(chǔ)之上，同時(shí)也克服了它的缺點(diǎn)，能夠?qū)η€進(jìn)行更加方便的控制，在曲線次數(shù)較低時(shí)仍有較多控制點(diǎn)去控制曲線；可以通過(guò)某個(gè)控制點(diǎn)去修改局部曲線而不會(huì)導(dǎo)致整個(gè)曲線形狀的發(fā)生變化；同時(shí)有著更好的凸包性質(zhì)，對(duì)曲線的形狀能夠進(jìn)行更加細(xì)致的控制。常用于需要經(jīng)過(guò)多個(gè)關(guān)鍵節(jié)點(diǎn)的軌跡規(guī)劃中。司艷偉等［6］對(duì)通過(guò)分析三次B 樣條曲線和七次B 樣條曲線，證明了三次B 樣條曲線比七次B 樣條曲線運(yùn)算效率高，且能夠有效的避免龍格現(xiàn)象。Guan Y.S［7］結(jié)合三次B 樣條四次B 樣條曲線的優(yōu)點(diǎn)，提出了4-3-4 復(fù)合軌跡規(guī)劃方法，該方法在保持較高運(yùn)算效率的同時(shí)，也能夠保證加速度曲線的連續(xù)性與平滑性，但無(wú)法保證加加速度的連續(xù)。

NURBS（Non-Uniform Rational B-Spli-nes）即非均勻有理B 樣條曲線，最早由美國(guó)西拉丘茲大學(xué)的Versprille 在其博士論文中提出。波音公司的羅溫首先將有理參數(shù)曲線曲面引入到了形狀設(shè)計(jì)中，隨后，1991年在國(guó)際標(biāo)準(zhǔn)化組織頒布的工業(yè)產(chǎn)品數(shù)據(jù)交換標(biāo)準(zhǔn)STEP 中，決定將NURBS 確定為定義工業(yè)產(chǎn)品幾何形狀的唯一數(shù)學(xué)方法［8］。為解決插值點(diǎn)較遠(yuǎn)時(shí)曲線產(chǎn)生尖點(diǎn)和突變問(wèn)題，Kubota 等［9］采用三次NURBS 曲線插值進(jìn)行軌跡規(guī)劃，發(fā)現(xiàn)三次NURBS 曲線能夠較好地控制端點(diǎn)，有效地避免了曲線突變，得到的位移、速度曲線平滑無(wú)突變。

無(wú)論是多項(xiàng)式插值擬合方法還是B 樣條方法，都具有很大的局限性［10］，比如對(duì)小范圍復(fù)雜曲線的擬合能力差，范圍過(guò)大又需要進(jìn)行分段處理等。將NURBS 曲線應(yīng)用在機(jī)械臂的軌跡規(guī)劃方法中可以解決大部分問(wèn)題，NURBS 曲線優(yōu)秀的局部性能由其定義決定，通過(guò)改變?nèi)我鈫蝹€(gè)參數(shù)對(duì)曲線形狀的修改，均只影響相應(yīng)的局部位置，整體性能不變，并且k次NURBS曲線的k-1階導(dǎo)數(shù)連續(xù)［11］。為降低軌跡規(guī)劃所得曲線關(guān)節(jié)位置的跟蹤誤差，從而減少機(jī)械臂的系統(tǒng)損耗，最終達(dá)到延長(zhǎng)其使用壽命的目的，所提出的軌跡規(guī)劃方法須保證結(jié)果的位移、速度、加速度平滑并且加加速度連續(xù)。故本文采用四階導(dǎo)數(shù)連續(xù)的五次NURBS 曲線進(jìn)行插值軌跡規(guī)劃。

B樣條曲線方程可以表示為

其中di(i=0，1···，n)是控制頂點(diǎn)。Ni，k(u)(i=0，1···n)稱為基函數(shù)，u為節(jié)點(diǎn)矢量，是由非遞減的參數(shù)組成的序列：u0≤u1≤···un+k+1。

在本文的軌跡規(guī)劃問(wèn)題中，用機(jī)械臂運(yùn)行的時(shí)間來(lái)表示節(jié)點(diǎn)矢量。

B 樣條有著許多種等價(jià)定義，其中最為常用的是得布爾-考克斯遞推公式：

由式（2），要確定第i個(gè)k次B樣條基函數(shù)需要k+2 個(gè)節(jié)點(diǎn)，節(jié)點(diǎn)矢量如下：

ui，ui+1，···，ui+k+1

由于求解曲線方程中相應(yīng)n+1 個(gè)控制頂點(diǎn)需要n+1 個(gè)k次B 樣條基函數(shù)，而確定這些基函數(shù)所需的節(jié)點(diǎn)構(gòu)成整條曲線的節(jié)點(diǎn)矢量U，參數(shù)的數(shù)量為n+k+2。并且在參數(shù)u軸上任意一點(diǎn)u∈[ui，ui+1]處，至多有k+1 個(gè)k次B 樣條基函數(shù)是非零的，即：

Nx，k(u)(x=i-k，i-k+1，···i)≠0

因?yàn)檫m用于自由型曲線的B 樣條曲線無(wú)法滿足初等曲線的要求，于是把B 樣條方法改造成為有理B 樣條方法來(lái)滿足使用要求。而在實(shí)際應(yīng)用中，B 樣條曲線往往出現(xiàn)非均勻的特性，所以研究者們決定將這種曲線稱為非均勻有理B 樣條曲線，即NURBS 曲線。之所以在軌跡規(guī)劃中使用NURBS曲線，是因?yàn)镹URBS 曲線在進(jìn)行有理的過(guò)程中引入了權(quán)因子ω，這就讓曲線有了較好的幾何解釋，并且ω能夠影響曲線的局部變化，這一點(diǎn)在軌跡規(guī)劃中有著重要意義［12］。一條k次NURBS 曲線可表示如下：

由NURBS 曲線的定義可知，確定一條NURBS曲線需要知道節(jié)點(diǎn)矢量U 和控制頂點(diǎn)di(i=0，1···，n)，由于本文中的節(jié)點(diǎn)矢量代表時(shí)間分配，是待優(yōu)化項(xiàng)，可以先采用平均分配或隨機(jī)分配的方式給定初始值，確定NURBS 曲線后再對(duì)時(shí)間進(jìn)行優(yōu)化。所以，再通過(guò)已給定曲線需要經(jīng)過(guò)的路徑點(diǎn)求出出NURBS 曲線的控制頂點(diǎn)，就能夠畫(huà)出NURBS曲線。

給定m+1 個(gè)需要經(jīng)過(guò)路徑點(diǎn)qi，其中i=1，2，···，m-1。為方便計(jì)算取ωi=1。

則k次B樣條曲線方程就可以簡(jiǎn)寫(xiě)為

由于控制頂點(diǎn)d0和dn分別與路徑起點(diǎn)q0與終點(diǎn)qm重合，故d0和dn兩個(gè)控制點(diǎn)已知，還需求出n-1 個(gè)未知控制頂點(diǎn)。將節(jié)點(diǎn)值帶入式（4），即：

由式（5）可得n-k=m+1 個(gè)方程，分別對(duì)應(yīng)m+1 個(gè)路徑點(diǎn)。另外，軌跡始末速度和加速度的方程如下：

其中、為關(guān)節(jié)初始速度與加速度，、為其關(guān)節(jié)停止時(shí)速度與加速度，一般均設(shè)置為0。

于是現(xiàn)有n-1 個(gè)未知參數(shù)，n-k+4 個(gè)方程，當(dāng)k=5 時(shí)，位置參數(shù)與方程式數(shù)相等，故當(dāng)使用5次NURBS 曲線對(duì)關(guān)節(jié)位置進(jìn)行插值所得軌跡曲線時(shí)，可以將初始速度與加速度、停止速度與加速度作為邊界條件。由曲線特性可知，五次NURBS 曲線是C4連續(xù)的，可以滿足加速度與加加速度曲線連續(xù)的要求。根據(jù)給定軌跡點(diǎn)可以求出NURBS 曲線的全部n+1 個(gè)控制頂點(diǎn)。再根據(jù)式（3）得到五次NURBS曲線方程。

2.2 基于五次多項(xiàng)式速度控制的NURBS 曲線插值算法

通過(guò)NURBS 曲線插值算法得到曲線，會(huì)產(chǎn)生曲率突變，曲線峰值過(guò)高現(xiàn)象，導(dǎo)致機(jī)械臂速度突變，抖動(dòng)嚴(yán)重，甚至超過(guò)機(jī)械臂的加減速度的最大范圍，從而無(wú)法完成規(guī)定軌跡。為改進(jìn)一般NURBS 曲線插值算法的缺點(diǎn)，提出一種基于五次多項(xiàng)式速度控制的NURBS 曲線插值算法，簡(jiǎn)稱QV-NURBS曲線插值算法。

該改進(jìn)算法首先通過(guò)NURBS 曲線插值得到軌跡曲線，然后通過(guò)分析曲率的變化情況找到特征點(diǎn)，將曲線分成曲率平緩段和曲率突變段，通過(guò)對(duì)曲率突變段的曲線進(jìn)行分析，獲該突變段的速度、加速度以及加加速度等參數(shù)，采用五次多項(xiàng)式對(duì)該段進(jìn)行速度規(guī)劃控制，利用式（7）的C4連續(xù)性，通過(guò)控制速度使得曲率突變段更加平滑，實(shí)現(xiàn)在突變段能夠穩(wěn)定運(yùn)行，以保持機(jī)械臂全程的穩(wěn)定性。

該算法分為5步：

1）根據(jù)式（2）和式（3），由給定的時(shí)間節(jié)點(diǎn)矢量和軌跡點(diǎn)求出NURBS曲線。

2）掃描特征點(diǎn)。特征點(diǎn)包括曲線間斷點(diǎn)和曲率突變點(diǎn)。曲線間斷點(diǎn)一般位于節(jié)點(diǎn)處，能夠通過(guò)節(jié)點(diǎn)矢量得到，不在節(jié)點(diǎn)處的大曲率點(diǎn)可以利用式（7）進(jìn)行判定得到。

其中，ρi為第i各插值點(diǎn)的曲率半徑。

3）判斷相鄰特征點(diǎn)的速度規(guī)劃交叉類型。當(dāng)兩相鄰特征點(diǎn)之間的曲線弧長(zhǎng)出現(xiàn)較小情況時(shí)，一般由于加速度限制，速度就無(wú)法加速至最大。兩相鄰特征點(diǎn)的速度軌跡規(guī)劃就出現(xiàn)了交叉區(qū)域。具體分為以下七種類型［13］。

類型1：速度軌跡規(guī)劃無(wú)交叉區(qū)域。其中為第i-1 個(gè)特征點(diǎn)加速度階段的位移，為第i個(gè)特征點(diǎn)減速階段位移，Si為第和第i個(gè)特征點(diǎn)間的曲線弧長(zhǎng)。

4）針對(duì)上訴7種類型進(jìn)行速度規(guī)劃。

（1）若判定為類型1，則加速至最大速度Vmax后，然后保持勻速運(yùn)，最后到減速點(diǎn)后減速。

（2）若判定為類型2或類型5，速度加速到Vi后減速。Vi可以使用二分法［14］計(jì)算得到。

（3）若判定為類型3或類型4，以起始特征點(diǎn)開(kāi)始，按照五次多項(xiàng)式軌跡曲線運(yùn)動(dòng)至終止特征點(diǎn)，其中五次多項(xiàng)式曲線的起點(diǎn)、終點(diǎn)速度與起始、終止特征點(diǎn)的速度相同。

（4）若判定為類型6或類型7，則把此類特征點(diǎn)從特征點(diǎn)序列中除去。

5）得到經(jīng)過(guò)五次多項(xiàng)式速度控制的NURBS 曲線，即QV-NURBS曲線。

3 軌跡規(guī)劃算法仿真分析

通過(guò)表1中的D-H 參數(shù)，利用Robotic Toolbox建立機(jī)械臂仿真模型，如圖2。機(jī)械臂執(zhí)行一段曲線軌跡，起點(diǎn)A 坐標(biāo)為（0.557，-0.134，-0.289），終點(diǎn)坐標(biāo)為（-0.458，-0.028，-0.395），通過(guò)逆運(yùn)動(dòng)學(xué)方程求解出各關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)角度如表2。

表1 水下機(jī)械臂D-H參數(shù)

表2 起點(diǎn)與終點(diǎn)的各關(guān)節(jié)角

圖2 水下機(jī)械臂仿真模型

要求機(jī)械臂末端從A 點(diǎn)運(yùn)動(dòng)到B 點(diǎn)，運(yùn)動(dòng)時(shí)間為5s，初始速度和加速度、終止速度和加速度均為0，然后運(yùn)用五次多項(xiàng)式插值和QV-NURBS 曲線插值進(jìn)行軌跡規(guī)劃。其中通過(guò)五次多項(xiàng)式插值得到機(jī)械臂各個(gè)關(guān)節(jié)角度位移、速度、加速度以及加加速度曲線如圖3所示。

圖3 五次多項(xiàng)式插值各個(gè)關(guān)節(jié)角度位移、速度和加速度曲線

通過(guò)圖3可知，五次多項(xiàng)式插值的位移、速度、加速度曲線光滑連續(xù)，無(wú)突變，但是加加速度曲線初始有突變，但后續(xù)也能保持光滑連續(xù)且無(wú)突變，能夠保證機(jī)械臂較為平穩(wěn)的運(yùn)行，但在初始階段機(jī)械臂所受沖擊較大。

通過(guò)QV-NURBS 曲線插值對(duì)路徑點(diǎn)進(jìn)行軌跡規(guī)劃，得到機(jī)械臂各關(guān)節(jié)角度位移、速度、加速度和加加速度曲線如圖4所示。

圖4 QV-NURBS曲線插值各個(gè)關(guān)節(jié)角度位移、速度和加速度曲線

通過(guò)圖4可知，QV-NURBS 曲線插值算法可以使得機(jī)械臂各關(guān)節(jié)的位移、速度、加速度以及加加速度曲線光滑連續(xù)且無(wú)突變，使得機(jī)械臂能夠在運(yùn)行軌跡的整個(gè)過(guò)程中保持運(yùn)動(dòng)的平穩(wěn)性。

通過(guò)對(duì)兩種種插值算法的仿真對(duì)比可知：兩種插值算法均能保持位移、速度曲線的連續(xù)平滑；對(duì)于加加速度曲線，五次多項(xiàng)式插值在啟動(dòng)階段有突變，但在運(yùn)行階段可以較好保持平滑，QV-NUBRS曲線插值則可以很好的保持曲線全程平滑。綜上所述，QV-NUBRS 曲線插值可以解決起始位置的震動(dòng)和運(yùn)行過(guò)程的不平穩(wěn)問(wèn)題，從而完成對(duì)各關(guān)節(jié)運(yùn)行角度、速度以及加速度的連續(xù)有效控制。

4 結(jié)語(yǔ)

對(duì)關(guān)節(jié)空間中NURBS 曲線插值算法進(jìn)行公式推導(dǎo)，并針對(duì)NURBS 曲線插值算法的缺點(diǎn)進(jìn)行改進(jìn)，提出了QV-NURBS 曲線插值算法，并建立該插值算法在六軸水下機(jī)械臂軌跡規(guī)劃中的數(shù)學(xué)模型。

對(duì)機(jī)械臂軌跡規(guī)劃的兩種插值算法進(jìn)行Matlab 仿真分析，對(duì)比優(yōu)劣性，結(jié)果表明基于QV-NURBS 曲線插值的軌跡規(guī)劃，使得機(jī)械臂末端運(yùn)行穩(wěn)定，軌跡平滑連續(xù)，任意一條路徑都可以采用QV-NURBS 曲線插值的方法進(jìn)行規(guī)劃實(shí)現(xiàn)，為水下機(jī)械臂試驗(yàn)驗(yàn)證以及機(jī)械臂控制系統(tǒng)的設(shè)計(jì)奠定了基礎(chǔ)。