基于5次NURBS的六足機(jī)器人足端軌跡規(guī)劃

周敬東，汪 宇，高偉周，楊 磊

(湖北工業(yè)大學(xué)湖北省農(nóng)業(yè)機(jī)械工程研究設(shè)計(jì)院，湖北 武漢 430068)

軌跡規(guī)劃是六足機(jī)器人實(shí)現(xiàn)行走功能的重要環(huán)節(jié)。軌跡規(guī)劃方法有兩種：一是在操作空間內(nèi)，機(jī)器人足端行走過的路徑比較直觀，容易觀察，但是該方法未考慮運(yùn)動(dòng)的奇異性，運(yùn)動(dòng)需要經(jīng)常計(jì)算逆解；二是在關(guān)節(jié)空間中，通過運(yùn)動(dòng)學(xué)逆解方程，將笛卡爾空間中腿部所處的位置時(shí)間序列與其關(guān)節(jié)空間中的位置時(shí)間序列相對應(yīng)，該方法對關(guān)節(jié)的角速度和角加速度有一定程度上的約束，還避免了運(yùn)動(dòng)奇異性[1]。

在實(shí)際研究中，軌跡規(guī)劃有多種方法，例如梯形法、正弦法[2]、多項(xiàng)式以及B樣條[3-4]等。其中，NURBS(非均勻有理B樣條)曲線插值法能保證軌跡規(guī)劃時(shí)角度、角速度和角加速度不發(fā)生突變且穩(wěn)定性良好，軌跡曲線在控制頂點(diǎn)形成的凸?fàn)畎畠?nèi)，便于軌跡的控制與關(guān)節(jié)的約束[5]。

綜上所述，已知六足機(jī)器人腿部足端首末兩端位置，通過機(jī)器人逆運(yùn)動(dòng)學(xué)得到腿部各關(guān)節(jié)的角度和角速度，再基于5次NURBS曲線插值法進(jìn)行擬合，得到各關(guān)節(jié)的角度、角速度以及角加速度曲線，從而實(shí)現(xiàn)對六足機(jī)器人的足端軌跡規(guī)劃[6-7]。

1 機(jī)器人腿部位姿描述

1.1 機(jī)器人腿部正運(yùn)動(dòng)學(xué)分析

機(jī)器人腿部由基節(jié)、股節(jié)和脛節(jié)三部分組成，機(jī)器人機(jī)體和這三部分肢節(jié)分別通過基關(guān)節(jié)、股關(guān)節(jié)和脛關(guān)節(jié)進(jìn)行連接[8]。對機(jī)器人腿部進(jìn)行正運(yùn)動(dòng)學(xué)分析，即根據(jù)機(jī)器人機(jī)體的位置和三個(gè)關(guān)節(jié)的角度，計(jì)算出腿部足端末點(diǎn)的位置坐標(biāo)的過程[9]。使用D-H方法在機(jī)器人的某條腿上建立坐標(biāo)系(圖1)。機(jī)器人腿部的D-H的相關(guān)參數(shù)如表1所示。

圖 1 機(jī)器人腿部坐標(biāo)系

表1 D-H參數(shù)

通過代入D-H參數(shù)，機(jī)器人腿部各關(guān)節(jié)之間的變換矩

(1)

(2)

式中：cθ1=cosθ1，sθ1=sinθ1，sθ23=sin(θ2+θ3)，cθ23=cos(θ2+θ3)。

機(jī)器人腿部足端點(diǎn)各坐標(biāo)為：

(3)

1.2 機(jī)器人腿部逆運(yùn)動(dòng)學(xué)分析

對機(jī)器人腿部進(jìn)行逆運(yùn)動(dòng)學(xué)分析，根據(jù)機(jī)器人某條腿足端末點(diǎn)的位置，利用左乘逆變換矩陣，計(jì)算出這條腿上各個(gè)關(guān)節(jié)角度的大?。?/p>

(4)

(5)

六足機(jī)器人的腿部雅克比矩陣和各關(guān)節(jié)角度的關(guān)系為：

(6)

在求出機(jī)器人腿部的雅克比矩陣后，可以通過足端點(diǎn)的線速度來得到腿部各關(guān)節(jié)的角速度，兩者之間的關(guān)系為：

(7)

式中，vx，vy，vz分別為腿部足端點(diǎn)在x軸、y軸、z軸的線速度。

2 基于5次NURBS的軌跡規(guī)劃

六足機(jī)器人的足端軌跡規(guī)劃包含的內(nèi)容有：根據(jù)機(jī)器人的運(yùn)動(dòng)過程與軌跡，確定相應(yīng)的軌跡參數(shù)，利用MATLAB軟件進(jìn)行模擬和描述；進(jìn)行相應(yīng)的計(jì)算，求解出機(jī)器人足端對應(yīng)的各關(guān)節(jié)的角度、角速度和角加速度[10]。本文研究的六足機(jī)器人選擇在笛卡爾空間中進(jìn)行軌跡規(guī)劃，通過機(jī)器人逆運(yùn)動(dòng)學(xué)求解關(guān)節(jié)空間中的各關(guān)節(jié)角度，再結(jié)合5次NURBS曲線插值法得到關(guān)節(jié)運(yùn)動(dòng)軌跡曲線。

NURBS又稱為非均勻有理B樣條，它能對曲線進(jìn)行精確的解析，可快速穩(wěn)定地計(jì)算出相應(yīng)的結(jié)果，所以本文將NURBS曲線設(shè)置成機(jī)器人腿部關(guān)節(jié)空間軌跡曲線的插補(bǔ)方法。NURBS曲線可以設(shè)置起終點(diǎn)的角速度和角加速度,除此之外，還能進(jìn)行特定點(diǎn)的優(yōu)化配置，使其保持連續(xù)。表達(dá)式為[11]：

(8)

式中：di(i=0,1,...,n)是對應(yīng)曲線的控制頂點(diǎn)；Ni,k(u)(i=1,2,…,n)是k次規(guī)范B樣條基函數(shù)，基函數(shù)可定義為：

(9)

式中：k為NURBS的次數(shù)；i為NURBS的序號。u=[u0,u1,…,un+2k]，是k次NURBS軌跡曲線定義域的節(jié)點(diǎn)矢量。須對時(shí)間節(jié)點(diǎn)進(jìn)行歸一化處理：

(10)

式中，Δti=ti+1-ti。將[uk,un+k]區(qū)域內(nèi)的節(jié)點(diǎn)應(yīng)用于插值的n+1個(gè)數(shù)據(jù)點(diǎn)pi的k次B樣條曲線方程，得到符合插值條件的相應(yīng)方程[11]：

(11)

k次B樣條需要增加k-1個(gè)邊界條件。設(shè)兩端節(jié)點(diǎn)重復(fù)度為k，B樣條曲線的首端和末端的控制點(diǎn)等同于首端和末端的數(shù)據(jù)點(diǎn)，相應(yīng)的邊界條件為：

(12)

對式(12)的n+k個(gè)方程進(jìn)行聯(lián)立，就可以解出控制頂點(diǎn)di(i=0,1,…,n)[11],再利用前面的公式進(jìn)行推演，就可以解出k次B樣條基函Ni,k(u)(i=1,2,…,n)，進(jìn)而解出機(jī)器人腿部運(yùn)動(dòng)軌跡上每一個(gè)點(diǎn)對應(yīng)的關(guān)節(jié)位置p(u)，再對得到的NURBS曲線進(jìn)行求導(dǎo)，最終求解出機(jī)器人腿部各關(guān)節(jié)在運(yùn)動(dòng)過程中的角速度和角加速度。

3 仿真及分析

本文所研究的機(jī)器人腿部基節(jié)、股節(jié)和脛節(jié)的長度分別為a1=50 mm，a2=70 mm，a3=140 mm。對機(jī)器人腿部在關(guān)節(jié)空間中以點(diǎn)到點(diǎn)的方式來控制軌跡，所經(jīng)過的機(jī)器人腿部足端位置坐標(biāo)依次為(0，130，-30)、(4，140，-15)、(8，150，10)、(12，160，20)、(16，170，10)、(20，180，-15)、(24，190，-30)。代入六足機(jī)器人的運(yùn)動(dòng)學(xué)模型進(jìn)行逆解，得到各關(guān)節(jié)在不同位置時(shí)的角度θ1、θ2、θ3。

將整個(gè)運(yùn)動(dòng)行程機(jī)器人腿部足端初始端點(diǎn)、末端點(diǎn)以及求解出來的相應(yīng)位置的各關(guān)節(jié)角度通過5次NURBS曲線插值法進(jìn)行軌跡規(guī)劃，規(guī)定機(jī)器人腿部運(yùn)動(dòng)的時(shí)間為2 s，各關(guān)節(jié)初始和終止角速度為0。通過MATLAB對機(jī)器人腿部進(jìn)行足端軌跡規(guī)劃仿真，得到機(jī)器人腿部足端在關(guān)節(jié)空間里的運(yùn)動(dòng)軌跡曲線(圖2)。腿部各關(guān)節(jié)的角度、角速度和角加速度隨足端在關(guān)節(jié)空間里位置的變化而發(fā)生改變。圖3—5為各關(guān)節(jié)在關(guān)節(jié)空間內(nèi)的角度、角速度和角加速度變化曲線[12]。

圖 2 腿部足端關(guān)節(jié)空間軌跡

從圖2可以看出，機(jī)器人足端經(jīng)過的點(diǎn)的位置與規(guī)劃的點(diǎn)的位置一致，且軌跡曲線光滑連續(xù)，表明5次NURBS曲線插值法能成功應(yīng)用于足端軌跡規(guī)劃。由圖3—5可知，機(jī)器人腿部各關(guān)節(jié)角度、角速度以及角加速度的軌跡曲線平滑而且連續(xù)，在加減速過程中沒有產(chǎn)生突變的情況，能使機(jī)器人足端穩(wěn)定著陸，減少了角度變化對腿部產(chǎn)生的沖擊，從而有利于實(shí)現(xiàn)機(jī)器人腿部的平穩(wěn)以及快速運(yùn)動(dòng)。

圖 3 腿部各關(guān)節(jié)角度軌跡

圖 4 腿部各關(guān)節(jié)角速度軌跡

圖 5 腿部各關(guān)節(jié)角加速度軌跡

4 試驗(yàn)

針對基于5次NURBS插值法的軌跡規(guī)劃，以六足機(jī)器人為研究對象，機(jī)器人和試驗(yàn)環(huán)境如圖6所示。設(shè)置機(jī)器人腿部足端起點(diǎn)為(0，130，-30)，終點(diǎn)為(24，190，-30)，運(yùn)行時(shí)間為2 s.在腿部運(yùn)動(dòng)過程中，將規(guī)劃形成的腿部足端位置信息和時(shí)間點(diǎn)輸入到機(jī)器人控制系統(tǒng)當(dāng)中，通過上位機(jī)導(dǎo)入相應(yīng)程序代碼以控制機(jī)器人腿部足端進(jìn)行軌跡規(guī)劃，得到機(jī)器人腿部在x、y、z方向上的位移曲線。圖6為未引入5次NURBS插值法的機(jī)器人足端坐標(biāo)軸方向位移曲線圖，圖7為引入5次NURBS插值法后的機(jī)器人足端坐標(biāo)軸方向位移曲線圖。

圖 6 六足機(jī)器人

(a)x方向位移 (b)y方向位移

(a)x方向位移 (b)y方向位移

試驗(yàn)結(jié)果表明：未引入5次NURBS插值法時(shí)機(jī)器人腿部運(yùn)動(dòng)加減速過程不明顯，易產(chǎn)生沖擊；引入5次NURBS插值法后，機(jī)器人腿部足端在實(shí)際運(yùn)動(dòng)過程中運(yùn)動(dòng)軌跡光滑連續(xù)，加速和減速過程明顯，使得機(jī)器人腿部運(yùn)動(dòng)更加平穩(wěn)，試驗(yàn)結(jié)果與仿真結(jié)果一致，證明采用5次NURBS插值法進(jìn)行軌跡規(guī)劃的可行性。

5 結(jié)論

在對六足機(jī)器人運(yùn)動(dòng)軌跡優(yōu)化的過程中，為提高機(jī)器人腿部運(yùn)動(dòng)的穩(wěn)定性和準(zhǔn)確性，本文基于機(jī)器人正逆運(yùn)動(dòng)學(xué)，采用5次NURBS曲線插值法對六足機(jī)器人的足端軌跡進(jìn)行規(guī)劃。根據(jù)機(jī)器人腿部足端所處的位置，通過機(jī)器人正逆運(yùn)動(dòng)學(xué)計(jì)算，得出機(jī)器人抬腿過程中所經(jīng)過位置的各關(guān)節(jié)角度以及角速度，再用5次NURBS曲線插值法進(jìn)行仿真擬合，得到腿部各關(guān)節(jié)角度、角速度和角加速度變化，再通過試驗(yàn)得到機(jī)器人腿部在x、y、z方向上的位移曲線。仿真和試驗(yàn)結(jié)果表明：基于5次NURBS的足端軌跡規(guī)劃的關(guān)節(jié)角度、角速度和角加速度的軌跡較為光滑，沒有出現(xiàn)突變的情況，避免了機(jī)器人運(yùn)動(dòng)過程中腿部產(chǎn)生振動(dòng)沖擊的情況，試驗(yàn)結(jié)果驗(yàn)證了仿真結(jié)果的合理性與可行性，達(dá)到預(yù)期的效果。采用5次NURBS曲線插值法構(gòu)造的足端軌跡能夠使六足機(jī)器人腿部運(yùn)動(dòng)平穩(wěn)、動(dòng)作快速。