關(guān)節(jié)型機(jī)器人曲線軌跡規(guī)劃方法研究

樊 豪,徐立佳,鞠健陽(yáng)

(長(zhǎng)安大學(xué) 工程機(jī)械學(xué)院,陜西 西安 710064)

0 引言

工業(yè)機(jī)器人在某個(gè)特定的工作空間內(nèi)進(jìn)行作業(yè)時(shí),一般都會(huì)遵循著某些特定的規(guī)則,比如以設(shè)定好的速度沿著特定的路徑運(yùn)動(dòng)[1]。路徑規(guī)劃與軌跡規(guī)劃組成了機(jī)器人的運(yùn)動(dòng)規(guī)劃,路徑是空間中的一條確定的曲線,不與時(shí)間相關(guān)聯(lián),路徑上每一點(diǎn)對(duì)應(yīng)著機(jī)器人工作空間的一個(gè)位姿以及關(guān)節(jié)空間中的一組向量;而軌跡則是機(jī)器人末端以規(guī)定好的速度、加速度在特定的曲線上運(yùn)行[2]。軌跡與時(shí)間相關(guān)聯(lián),每一個(gè)時(shí)刻對(duì)應(yīng)著機(jī)器人末端在工作空間內(nèi)的一個(gè)位姿以及關(guān)節(jié)空間中的一組向量。

在體積相近的情況下,關(guān)節(jié)型機(jī)器人的工作空間要比其他類型的機(jī)器人更大,幾乎在任何軌跡和角度都可以進(jìn)行工作。因此,相較于其他類型的機(jī)器人,關(guān)節(jié)型機(jī)器人的應(yīng)用范圍要更廣[3]。一般而言,關(guān)節(jié)型機(jī)器人是多自由度、多連桿組成的空間強(qiáng)耦合機(jī)構(gòu),在其作業(yè)過(guò)程中需要實(shí)時(shí)控制各關(guān)節(jié)的運(yùn)動(dòng),使其沿著預(yù)定的軌跡運(yùn)動(dòng)。

關(guān)節(jié)機(jī)器人的軌跡規(guī)劃分為關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃,關(guān)節(jié)空間的軌跡規(guī)劃將關(guān)節(jié)變量直接與時(shí)間關(guān)聯(lián)起來(lái),一般也需要同時(shí)規(guī)劃關(guān)節(jié)變量對(duì)時(shí)間的1階導(dǎo)數(shù)和2階導(dǎo)數(shù)。關(guān)節(jié)空間的軌跡規(guī)劃因?yàn)椴豢紤]機(jī)器人末端必須經(jīng)過(guò)連續(xù)的空間位置,因此通常較為簡(jiǎn)單。常用的關(guān)節(jié)空間下的軌跡規(guī)劃方法有3次多項(xiàng)式、5次多項(xiàng)式、拋物線多項(xiàng)式等[4]。

笛卡爾空間內(nèi)的軌跡規(guī)劃相較而言要復(fù)雜一些,一般要求機(jī)器人末端在2點(diǎn)之間按照一定形狀的軌跡運(yùn)動(dòng)。現(xiàn)有的笛卡爾空間軌跡規(guī)劃算法主要有直線插補(bǔ)算法和圓弧插補(bǔ)算法。直線插補(bǔ)算法以當(dāng)前點(diǎn)到起點(diǎn)的直線距離與直線總長(zhǎng)度的比值為依據(jù)規(guī)劃軌跡;圓弧插補(bǔ)算法以當(dāng)前點(diǎn)到起點(diǎn)之間的扇形的角度與圓弧扇形總角度的比值為依據(jù)規(guī)劃軌跡[5]。而對(duì)于任意的已知表達(dá)式的空間曲線,這兩種方法將無(wú)法使用。

為實(shí)現(xiàn)任意已知表達(dá)式的空間曲線的規(guī)劃軌跡,本文首先規(guī)劃?rùn)C(jī)器人末端在空間曲線上的運(yùn)動(dòng)速度;而后根據(jù)速度的積分求取當(dāng)前位置機(jī)器人末端與起點(diǎn)之間曲線的長(zhǎng)度;最后根據(jù)當(dāng)前時(shí)刻、下一時(shí)刻機(jī)器人末端所處位置與起點(diǎn)之間曲線的長(zhǎng)度之差以及當(dāng)前時(shí)刻機(jī)器人末端所處位置,選擇下一時(shí)刻機(jī)器人末端將要到達(dá)的位置,將機(jī)器人末端所處位置與時(shí)間相關(guān)聯(lián),完成軌跡規(guī)劃。

1 系統(tǒng)構(gòu)成

機(jī)器人系統(tǒng)的構(gòu)成如圖1所示,主要由6自由度關(guān)節(jié)型機(jī)器人、運(yùn)動(dòng)控制器、伺服驅(qū)動(dòng)器、手操器等組成。

圖1 機(jī)器人系統(tǒng)

運(yùn)動(dòng)控制器負(fù)責(zé)實(shí)現(xiàn)運(yùn)動(dòng)規(guī)劃,并將運(yùn)動(dòng)指令實(shí)時(shí)發(fā)送給伺服驅(qū)動(dòng)器。伺服驅(qū)動(dòng)器負(fù)責(zé)接收運(yùn)動(dòng)控制器的指令,并驅(qū)動(dòng)機(jī)器人各關(guān)節(jié)按照指令運(yùn)動(dòng)[6]。

在笛卡爾空間軌跡規(guī)劃任務(wù)中,伺服驅(qū)動(dòng)器一般工作在位置模式,這意味著,運(yùn)動(dòng)控制器在每個(gè)周期要向伺服驅(qū)動(dòng)器發(fā)送下一個(gè)運(yùn)行周期機(jī)器人各關(guān)節(jié)應(yīng)到達(dá)的位置。

機(jī)器人系統(tǒng)運(yùn)行的過(guò)程如圖2所示。首先,將一條已知空間曲線的表達(dá)式輸入到運(yùn)動(dòng)控制器中;其次,運(yùn)動(dòng)控制器進(jìn)行軌跡規(guī)劃,將末端執(zhí)行器的位姿與機(jī)器人的運(yùn)行時(shí)間相關(guān)聯(lián);而后,將末端執(zhí)行器的位姿代入到逆運(yùn)動(dòng)學(xué)中,求出下一時(shí)刻各關(guān)節(jié)的角度值,并傳送給伺服驅(qū)動(dòng)器;最后,由伺服驅(qū)動(dòng)器完成關(guān)節(jié)空間運(yùn)動(dòng)插補(bǔ)并驅(qū)動(dòng)機(jī)器人各關(guān)節(jié)在下一運(yùn)行周期內(nèi)到達(dá)目標(biāo)位置。

圖2 軌跡規(guī)劃過(guò)程

2 關(guān)節(jié)型機(jī)器人正逆運(yùn)動(dòng)學(xué)

2.1 關(guān)節(jié)型機(jī)器人正運(yùn)動(dòng)學(xué)

機(jī)器人正運(yùn)動(dòng)學(xué)描述的是各關(guān)節(jié)角度與末端執(zhí)行器位姿之間的正向關(guān)系,即給定一組關(guān)節(jié)空間內(nèi)的各關(guān)節(jié)角度值,求出末端執(zhí)行器的位姿。

圖3 機(jī)械臂關(guān)節(jié)坐標(biāo)系

(1)

圖1中機(jī)械臂的正運(yùn)動(dòng)學(xué)模型為式(2)。

(2)

表1 機(jī)械臂改進(jìn)D-H參數(shù)表

2.2 關(guān)節(jié)型機(jī)器人逆運(yùn)動(dòng)學(xué)

機(jī)械臂逆運(yùn)動(dòng)學(xué)用于表示關(guān)節(jié)轉(zhuǎn)角與末端執(zhí)行器位姿之間的逆向關(guān)系,即給定笛卡爾空間中末端執(zhí)行器的一組位姿,求解各關(guān)節(jié)對(duì)應(yīng)的角度。

給定一組末端執(zhí)行器的位姿后,可將該位姿用齊次變換矩陣Te表示,則有式(3)和式(4)。

(3)

(4)

同樣有式(5)和式(6)。

(5)

(6)

θ2可由θ3求得;θ4可由θ1、θ2、θ3求得;θ5可由θ1、θ2、θ3、θ4求得。

(7)

根據(jù)式(7)等式兩邊對(duì)應(yīng)矩陣元素相等,可求得θ6。

在求解關(guān)節(jié)空間內(nèi)各關(guān)節(jié)的角度值時(shí),θ1以及θ3各有2 個(gè)解,則一共存在4 組解,需要根據(jù)機(jī)械臂的運(yùn)動(dòng)情況選取一組合適的解。而由于機(jī)器人不可能在一個(gè)運(yùn)行周期內(nèi)發(fā)生過(guò)大的跳變,因此可選取與當(dāng)前各關(guān)節(jié)角度值最接近的那組解作為逆解。

3 笛卡爾空間曲線軌跡規(guī)劃

3.1 笛卡爾空間軌跡規(guī)劃原理

當(dāng)要求機(jī)器人末端執(zhí)行器在不同點(diǎn)之間按照不同的形狀軌跡運(yùn)動(dòng)時(shí),關(guān)節(jié)空間內(nèi)的軌跡規(guī)劃無(wú)法滿足要求,此時(shí)一般需要在笛卡爾空間進(jìn)行軌跡規(guī)劃,這個(gè)過(guò)程可按照以下流程完成[7]:

1) 機(jī)器人運(yùn)行在當(dāng)前時(shí)刻t時(shí),給t加上機(jī)器人的運(yùn)行周期得到th;

2) 根據(jù)時(shí)間th,利用軌跡函數(shù)求解出末端執(zhí)行器的位姿;

3) 將末端的位姿帶入到機(jī)器人逆運(yùn)動(dòng)學(xué)中,求出下一時(shí)刻機(jī)器人各關(guān)節(jié)的角度值;

4) 伺服驅(qū)動(dòng)器控制機(jī)器人各關(guān)節(jié)在下一運(yùn)行周期內(nèi)運(yùn)動(dòng)到目標(biāo)位置;

5)重復(fù)步驟1)到4),直到當(dāng)前運(yùn)行時(shí)間等于軌跡運(yùn)行總時(shí)間。

3.2 機(jī)器人末端速度規(guī)劃

為了使機(jī)器人運(yùn)行平穩(wěn),機(jī)器人末端的速度和加速度都不應(yīng)有突變,這要求機(jī)器人在開(kāi)始運(yùn)動(dòng)、進(jìn)入勻速段、離開(kāi)勻速段、停止時(shí)加速度均為0。速度規(guī)劃采用如圖4 所示的形式。圖4 中,加速段與減速段均采用3 次多項(xiàng)式,為了計(jì)算方便,加速段與減速段的時(shí)間均為2 s。

圖4 笛卡爾坐標(biāo)系下機(jī)器人末端速度規(guī)劃形式

如圖4所示,加速段時(shí)間ta為2 s,減速段時(shí)間td也為2 s,加速段與減速段均為3次多項(xiàng)式。設(shè)機(jī)器人末端勻速度為Vu,勻速運(yùn)行時(shí)間為tu,L為曲線總長(zhǎng)度。空間曲線參數(shù)方程的表達(dá)式如式(8)所示。

(8)

式中,a表示曲線參數(shù)方程的參數(shù)。

曲線總長(zhǎng)度L的計(jì)算方式如式(9)所示。

(9)

設(shè)機(jī)器人末端運(yùn)行的最低速度為Vmin,如果L/Vmin小于4.5 s,則無(wú)法用本文的方法進(jìn)行軌跡規(guī)劃,可采用直線插補(bǔ)的方法完成軌跡規(guī)劃;如果L/Vu小于5 s并大于4.5 s,則將Vu設(shè)置為L(zhǎng)/5,再規(guī)劃軌跡。

設(shè)加速段速度V的計(jì)算公式如式(10)所示。

V=ax3+bx2+cx+d

(10)

式(10)滿足式(11)。

(11)

根據(jù)式(11),可以得到式(12)。

(12)

勻速運(yùn)行時(shí)間tu的計(jì)算方式如式(13)所示。

(13)

則機(jī)器人末端在笛卡爾空間運(yùn)行時(shí),速度遵循式(14)。

(14)

3.3 笛卡爾坐標(biāo)系下軌跡規(guī)劃

得到速度V的表達(dá)式以后,可以根據(jù)表達(dá)式來(lái)近似求解任一時(shí)刻機(jī)器人末端所在位置與曲線起點(diǎn)之間曲線的距離。一般來(lái)說(shuō),機(jī)器人的運(yùn)行周期非常小,通常不超過(guò)10 ms,因此,使用式(15)計(jì)算曲線長(zhǎng)度是可行的。

(15)

圖5為笛卡爾空間下機(jī)器人末端軌跡規(guī)劃原理圖,為說(shuō)明方便,采用平面曲線。設(shè)機(jī)器人的運(yùn)行周期為T,機(jī)器人當(dāng)前運(yùn)行時(shí)刻為t,可利用式(15)求出此時(shí)刻機(jī)器人末端所處位置與起點(diǎn)之間曲線的長(zhǎng)度lnow。給當(dāng)前運(yùn)行時(shí)刻加上T,再次利用式(15)求出下一時(shí)刻機(jī)器人末端距離起點(diǎn)之間曲線的長(zhǎng)度lnext,下一時(shí)刻機(jī)器人末端應(yīng)該到達(dá)的位置便是曲線上與起點(diǎn)間曲線長(zhǎng)度為lnext的點(diǎn)。已知lnow、lnext、xnow,便可比較方便的計(jì)算出下一時(shí)刻機(jī)器人末端的位置(xnext,ynext)。

圖5 軌跡規(guī)劃原理圖

對(duì)于空間內(nèi)的任意表達(dá)式已知的曲線,可利用式(16)計(jì)算得到下一時(shí)刻機(jī)器人末端應(yīng)在的位置。

lnow-lnext

(16)

式中,anow表示當(dāng)前時(shí)刻曲線參數(shù)的值;anext表示下一時(shí)刻曲線參數(shù)的取值。利用式(16),便可根據(jù)當(dāng)前運(yùn)行時(shí)間t,獲得下一時(shí)刻機(jī)器人末端應(yīng)到達(dá)的空間坐標(biāo)。將機(jī)器人末端位姿帶入到逆運(yùn)動(dòng)學(xué)中,便可以獲得下一時(shí)刻,機(jī)器人各關(guān)節(jié)要達(dá)到的角度值。

實(shí)際應(yīng)用中,因?yàn)楹茈y求得任意函數(shù)的不定積分,所以很難利用式(9)求曲線總長(zhǎng)度或利用式(16)求下一時(shí)刻機(jī)器人末端應(yīng)到達(dá)的空間位置。在工程應(yīng)用中,曲線可劃分成多段,每段距離足夠小,然后將每?jī)牲c(diǎn)之間的距離相加,求出曲線長(zhǎng)度。

4 軌跡規(guī)劃實(shí)驗(yàn)

為不失一般性,本文采用表達(dá)式為式(17)的平面曲線,單位是mm,如圖6所示。空間曲線的規(guī)劃也可采用同樣的方法。

圖6 空間曲線

(17)

設(shè)機(jī)器人末端在笛卡爾空間內(nèi)的勻速度Vu為2 mm/s,則曲線總長(zhǎng)L為7.6401 mm,勻速運(yùn)行時(shí)間tu為1.8201 s,總運(yùn)行時(shí)間為5.8201 s。為了使得軌跡規(guī)劃的效果更為明顯,將機(jī)器人的運(yùn)行周期設(shè)置為80 ms。由2.2節(jié)的方法可得到每個(gè)運(yùn)行周期機(jī)器人應(yīng)到達(dá)的位置,將每個(gè)時(shí)刻對(duì)應(yīng)的坐標(biāo)繪制在圖上,可得到圖7 所示的軌跡規(guī)劃示意圖。從圖7 中可以看到,在運(yùn)動(dòng)起始段,2點(diǎn)間的距離逐漸增大;當(dāng)運(yùn)行時(shí)間達(dá)到2 s時(shí),機(jī)器人末端進(jìn)入勻速運(yùn)行階段,在此階段,每2點(diǎn)之間的距離不變,機(jī)器人平穩(wěn)運(yùn)行;當(dāng)運(yùn)行時(shí)間到達(dá)3.8201 s后,機(jī)器人末端進(jìn)入減速運(yùn)行階段,每2點(diǎn)之間的距離逐漸減小,直至機(jī)器人停止運(yùn)行。

圖7 軌跡規(guī)劃示意圖

5 結(jié)語(yǔ)

1)設(shè)計(jì)了一種在笛卡爾空間規(guī)劃?rùn)C(jī)器人末端運(yùn)動(dòng)軌跡的方法。求出曲線的總長(zhǎng)度,并根據(jù)設(shè)定好的機(jī)器人末端勻速度求出總運(yùn)行時(shí)間。

2)規(guī)劃?rùn)C(jī)器人末端的運(yùn)動(dòng)速度,得到各個(gè)時(shí)刻機(jī)器人末端的速度值;在機(jī)器人運(yùn)行周期較短的前提下,通過(guò)對(duì)速度積分,得到各個(gè)時(shí)刻機(jī)器人末端所在位置與起點(diǎn)間曲線的長(zhǎng)度。

3)利用當(dāng)前時(shí)刻與下一時(shí)刻機(jī)器人末端和起點(diǎn)間曲線長(zhǎng)度之差以及當(dāng)前時(shí)刻機(jī)器人末端所在位置,求出下一時(shí)刻機(jī)器人末端的位置。通過(guò)實(shí)驗(yàn)證明,該方法能準(zhǔn)確的在笛卡爾空間規(guī)劃出機(jī)器人末端的運(yùn)行軌跡。