黃 偉,閔柏成,張世超

(中國(guó)船舶集團(tuán)有限公司第七二三研究所,江蘇 揚(yáng)州 225101)

0 引 言

使用機(jī)器人替代一些傳統(tǒng)的制造產(chǎn)業(yè)是實(shí)現(xiàn)制造強(qiáng)國(guó)的必要途徑[1-3],機(jī)器人建模與仿真技術(shù)已成為研究機(jī)器人的一個(gè)不可或缺的途徑。目前機(jī)器人仿真軟件常見(jiàn)的有RobotStudio、RoboGuide等,但這些軟件需要昂貴的價(jià)格和后續(xù)使用價(jià)格。三維軟件如UG、PROE等雖然可以進(jìn)行仿真,但是其擴(kuò)展性差[4]。OpenGL圖形庫(kù)功能強(qiáng)大,但需大量軟件開(kāi)發(fā)工作才能用于機(jī)器人建模和仿真。Matlab機(jī)器人工具箱的仿真顯示效果比較差,難以進(jìn)行逼真的仿真。CoppeliaSim仿真軟件可以將三維軟件里面的精確模型導(dǎo)入其中進(jìn)行仿真,還可以聯(lián)合Python、Matlab等軟件進(jìn)行聯(lián)合仿真[5]。利用Matlab的矩陣計(jì)算優(yōu)勢(shì),結(jié)合CoppeliaSim仿真軟件進(jìn)行聯(lián)合仿真,可以建立多種機(jī)器人工作環(huán)境,并可以快速進(jìn)行運(yùn)動(dòng)學(xué)解算[6]。

1 機(jī)器人模型的建立

在CoppeliaSim仿真平臺(tái)中進(jìn)行機(jī)器人的模型建立,先通過(guò)三維軟件(例如UG、PROE等)畫(huà)出機(jī)器人各個(gè)部件的三維信息,然后將其轉(zhuǎn)化成stl格式的文件;其次在CoppeliaSim仿真平臺(tái)中導(dǎo)入這些文件,即可以看到各個(gè)部件在CoppeliaSim軟件界面上;最后在CoppeliaSim軟件中添加相關(guān)關(guān)節(jié)的位置姿態(tài)信息,將各個(gè)導(dǎo)入的機(jī)器人部件附屬在各個(gè)關(guān)節(jié)上,建立好機(jī)器人各系統(tǒng)的樹(shù)形結(jié)構(gòu),附屬好關(guān)節(jié)的部件就會(huì)隨著機(jī)器人關(guān)節(jié)的旋轉(zhuǎn)進(jìn)行相應(yīng)的跟隨運(yùn)動(dòng)。圖1(a)為在CoppeliaSim中建立的UR5模型,圖1(b)為在CoppeliaSim中建立的樹(shù)形結(jié)構(gòu)圖。整個(gè)流程如圖2所示。

圖1 CoppeliaSim中建立的UR5模型及其結(jié)構(gòu)樹(shù)

圖2 CoppeliaSim中建立機(jī)器人模型的流程

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

UR5機(jī)器人為6自由度串聯(lián)機(jī)器人模型,可以用標(biāo)準(zhǔn)D-H建模法對(duì)UR5進(jìn)行建模。圖3、圖4分別為UR5結(jié)構(gòu)圖和其標(biāo)準(zhǔn)D-H參數(shù)坐標(biāo)系圖。以UR5機(jī)器人為例,由表1的參數(shù)可以進(jìn)行運(yùn)動(dòng)學(xué)方程的推導(dǎo)。坐標(biāo)系從{i-1}到坐標(biāo)系{i}的變換矩陣可以寫(xiě)成:

表1 UR5型機(jī)器人的連桿參數(shù)表

圖3 UR5機(jī)器人結(jié)構(gòu)圖

圖4 UR5機(jī)器人D-H坐標(biāo)系

(1)

代入表1中參數(shù),最終變換矩陣為:

(2)

就完成了正運(yùn)動(dòng)學(xué)的求解。

逆運(yùn)動(dòng)學(xué)求解是已知變換矩陣T,求6個(gè)關(guān)節(jié)角度。逆運(yùn)動(dòng)學(xué)求解有解析法、迭代法,這里采用解析法求解。逆運(yùn)動(dòng)學(xué)算法求解的角度范圍為θ∈[-π,π],因?yàn)闃?biāo)準(zhǔn)的反正切arctan的值域是[-π/2,π/2],所以不能使用。使用一個(gè)改進(jìn)的反正切的函數(shù)Atan2(y,x)(Matlab里有這個(gè)函數(shù)),這個(gè)反正切的求法值域也滿(mǎn)足要求。已知最終變換矩陣:

(3)

左右變換逆乘得到:

(4)

式(4)左右側(cè)第3行第4列對(duì)應(yīng)相等,求得關(guān)節(jié)角θ1為:

(5)

式(4)左右2側(cè)第3行第3列對(duì)應(yīng)相等,求得關(guān)節(jié)角θ5為:

θ5=±arccos(axs1-ayc1)

(6)

式中:axs1-ayc1≤1。

式(4)左右側(cè)第3行第1列對(duì)應(yīng)相等,求得關(guān)節(jié)角θ6為:

θ6=Atan2(m,n)-Atan2(s5,0)=

Atan2(m/s5,n/s5),s5≠0

(7)

左右變換逆乘得到右側(cè)等式:

(8)

式(8)左右第1行第4列對(duì)應(yīng)相等,第2行第4列對(duì)應(yīng)相等,求得關(guān)節(jié)角θ3為:

(9)

式中:m2+n2≤(a2+a3)2,并有:

(10)

為了簡(jiǎn)化縮寫(xiě),約定c23=cos(θ2+θ3),s23=sin(θ2+θ3)。

將式(10)展開(kāi),將θ3代入式(10),求得關(guān)節(jié)角θ2為:

θ2=Atan2(s2,c2)

(11)

約定s6=sinθ6,c6=cosθ6,式(8)左右第1行第2列對(duì)應(yīng)相等,第2行第2列對(duì)應(yīng)相等,求得關(guān)節(jié)角:

θ4=Atan2(-s6(nxc1+nys1)-c6(oxc1+oys1),

ozc6+nzs6)-θ2-θ3

(12)

假設(shè)6個(gè)關(guān)節(jié)角都取1°,經(jīng)Matlab編程驗(yàn)證得出末端位置姿態(tài)矩陣T。將得出的末端位置姿態(tài)矩陣在Matlab中代入逆運(yùn)動(dòng)學(xué)求解,得出8組逆解,其中一組為6個(gè)關(guān)節(jié)角,都是1°,從而驗(yàn)證了運(yùn)動(dòng)學(xué)公式的正確性。

3 機(jī)器人碼垛環(huán)境坐標(biāo)系之間的關(guān)系

機(jī)器人完成碼垛作業(yè)需要知道搬運(yùn)起始點(diǎn)和最終點(diǎn)的機(jī)器人末端位置和姿態(tài)。以UR5機(jī)器人為例,搬運(yùn)長(zhǎng)方體如圖5所示,UR5機(jī)器人搬運(yùn)過(guò)程中,用末端手爪工具從傳送帶抓取物件,然后放置在指定的位置。

圖5 UR機(jī)器人碼垛及其坐標(biāo)系之間的關(guān)系

圖5中{Ba}代表UR5基座坐標(biāo)系;{6}代表UR5機(jī)器人末端坐標(biāo)系;{M}代表UR5手爪工具坐標(biāo)系;{0}代表起始抓取點(diǎn)坐標(biāo)系;{1}代表搬運(yùn)結(jié)束點(diǎn)的坐標(biāo)系;{N}代表搬運(yùn)過(guò)程中點(diǎn)的坐標(biāo)系。各個(gè)坐標(biāo)系關(guān)系組成一個(gè)閉環(huán)鏈如圖5(b),各個(gè)坐標(biāo)系關(guān)系如下:

(13)

等式兩邊同乘以逆矩陣得:

(14)

式(14)給出了UR5機(jī)器人末端夾持工具沿給定軌跡運(yùn)動(dòng)時(shí),機(jī)器人的運(yùn)動(dòng)學(xué)方程。除了{(lán)6}系相對(duì)于{Ba}系的位置和姿態(tài)在變動(dòng)中不確定之外,其余的位姿都是預(yù)先規(guī)定好的,求出UR5碼垛運(yùn)動(dòng)軌跡上各個(gè)目標(biāo)軌跡點(diǎn)處{6}系相對(duì)于{B}系的位姿,就完成了工件坐標(biāo)系到機(jī)器人坐標(biāo)系的轉(zhuǎn)換。

完成工件坐標(biāo)系到機(jī)器人坐標(biāo)系的轉(zhuǎn)換后,根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)解算就可以求出機(jī)器人碼垛運(yùn)動(dòng)到各個(gè)軌跡點(diǎn)處所對(duì)應(yīng)的機(jī)器人的各個(gè)關(guān)節(jié)角。

首先每組解對(duì)應(yīng)關(guān)節(jié)角度值應(yīng)該符合機(jī)器人手冊(cè)里規(guī)定的關(guān)節(jié)角取值范圍。其次從滿(mǎn)足關(guān)節(jié)角運(yùn)動(dòng)范圍的解集中選擇關(guān)節(jié)角絕對(duì)值之和最小的解,最小的解即是波動(dòng)范圍最低的解,也是最合適的解。

求得的運(yùn)動(dòng)學(xué)反解往往不唯一,須根據(jù)機(jī)器人的各項(xiàng)參數(shù)和工作環(huán)境,從中選擇最合適的解[7]。

此關(guān)節(jié)角數(shù)據(jù)是在Matlab中計(jì)算生成的,并以弧度值的形式保存成文本格式,此文本可以用來(lái)提供給CoppeliaSim進(jìn)行仿真。

4 基于CoppeliaSim的UR5機(jī)器人仿真運(yùn)動(dòng)

CoppeliaSim軟件的運(yùn)動(dòng)仿真可以在建立好模型和場(chǎng)景的基礎(chǔ)上,通過(guò)對(duì)模型添加相應(yīng)的腳本語(yǔ)言進(jìn)行運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制,但是其不能進(jìn)行位置姿態(tài)計(jì)算,也不能進(jìn)行復(fù)雜的矩陣計(jì)算。機(jī)器人運(yùn)動(dòng)學(xué)動(dòng)力學(xué)涉及到很多矩陣方面的知識(shí),所以需要結(jié)合C、MATLAB、Python等語(yǔ)言進(jìn)行矩陣運(yùn)算,然后將運(yùn)算結(jié)果提供給CoppeliaSim軟件進(jìn)行仿真。本文的UR5機(jī)器人需要將4個(gè)長(zhǎng)方體運(yùn)送到指定位置,初始和最終長(zhǎng)方體碼垛結(jié)束后的位置如圖6所示。

圖6 初始和最終長(zhǎng)方體碼垛結(jié)束后的位置

本文用Matlab解算逆運(yùn)動(dòng)學(xué)生成UR5機(jī)器人進(jìn)行碼垛運(yùn)動(dòng)所需的關(guān)節(jié)角,然后將關(guān)節(jié)角參數(shù)傳給CoppeliaSim軟件控制機(jī)器人進(jìn)行運(yùn)動(dòng)。在CoppeliaSim軟件中添加腳本程序調(diào)用相應(yīng)函數(shù)來(lái)控制機(jī)器人關(guān)節(jié)、傳送帶、手爪,具體參考CoppeliaSim中菜單欄的使用說(shuō)明。當(dāng)傳送帶將長(zhǎng)方體傳送到指定位置時(shí)自動(dòng)停止傳動(dòng),相同的長(zhǎng)方體都傳送到相同的位置停止。長(zhǎng)方體被送到傳送帶上指定的位置后,機(jī)器人通過(guò)控制各個(gè)關(guān)節(jié)的轉(zhuǎn)角讓手爪到指定位置進(jìn)行抓取,靠近長(zhǎng)方體時(shí)通過(guò)腳本程序設(shè)定夾住物塊,然后將長(zhǎng)方體搬移至指定的位置上。重復(fù)上述操作,將所有長(zhǎng)方體放置在相應(yīng)的位置上,就完成了碼垛作業(yè)。

整個(gè)過(guò)程如圖7所示:(1)傳送帶送到指定位置;(2)UR5機(jī)器人移動(dòng)到手爪夾取位置;(3)機(jī)器人進(jìn)行裝夾;(4)機(jī)器人搬運(yùn)到指定位置;(5)機(jī)器人夾具松開(kāi),機(jī)器人移動(dòng)回原點(diǎn);(6)重復(fù)上述5個(gè)步驟,將不同的長(zhǎng)方體物塊放置在指定位置。

圖7 機(jī)器人碼垛過(guò)程

5 結(jié)束語(yǔ)

采用本文方法對(duì)UR5機(jī)器人碼垛過(guò)程進(jìn)行了仿真。在CoppeliaSim中建立了機(jī)器人及碼垛作業(yè)環(huán)境的三維模型,對(duì)UR5機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析并在Matlab中計(jì)算獲取了碼垛過(guò)程中的機(jī)器人各個(gè)關(guān)節(jié)的轉(zhuǎn)角軌跡,將Matlab生成的轉(zhuǎn)角信息傳遞給CoppeliaSim,在CoppeliaSim軟件仿真平臺(tái)中對(duì)碼垛作業(yè)過(guò)程進(jìn)行了運(yùn)動(dòng)仿真。仿真結(jié)果表明,所提方法能夠生成無(wú)干涉碼垛運(yùn)動(dòng)軌跡和正確的機(jī)器人在運(yùn)動(dòng)中的各個(gè)轉(zhuǎn)角參數(shù)。本文方法可以運(yùn)用到其它機(jī)器人仿真中。