五自由度機(jī)械臂三維建模與仿真實驗平臺的構(gòu)建

鄧曉燕, 林燦光, 施翔宇, 吳澤榮, 陳浩彬

(華南理工大學(xué) 自動化科學(xué)與工程學(xué)院, 廣東 廣州 510641)

隨著機(jī)器人技術(shù)的飛速發(fā)展,多自由度機(jī)械臂已成為工業(yè)自動化的重要設(shè)備,在部件組裝、生產(chǎn)搬運、精細(xì)加工等生產(chǎn)中發(fā)揮著越來越大的作用。目前,有關(guān)機(jī)械臂的運動學(xué)、控制理論和動力學(xué)的研究正在不斷深入。建立機(jī)械臂仿真系統(tǒng),對機(jī)械臂的運動控制進(jìn)行仿真分析,并執(zhí)行目標(biāo)抓取任務(wù),可以對機(jī)器人運動學(xué)有更深入的理解。

機(jī)械臂運動學(xué)仿真系統(tǒng)可以通過Matlab、ADAMS等軟件平臺實現(xiàn),目前較多的是借助Matlab的Robotics Toolbox工具箱,利用D-H參數(shù)表建立機(jī)器人對象。任崇軒、馬如奇等人對機(jī)器人實驗教學(xué)平臺進(jìn)行了研究,在一定程度上促進(jìn)了機(jī)器人實驗教學(xué)的改革與發(fā)展[1-7]。然而,由Matlab平臺開發(fā)的仿真界面不能最大限度地反映機(jī)械臂的真實狀態(tài),其三維模型較為粗糙且難以表達(dá)機(jī)械臂末端姿態(tài)。此外,ADAMS及Matlab的開放性和功能擴(kuò)展性不如Qt或Visual Studio,并且難以滿足實時仿真的要求。本文采用在Qt Widgets框架上開發(fā)上位機(jī)程序,利用OpenGL圖形程序接口建立了五自由度機(jī)械臂三維仿真實驗平臺,并進(jìn)行正逆運動學(xué)的分析。該仿真實驗平臺能很好地反映機(jī)械臂的真實運動,實時執(zhí)行目標(biāo)抓取任務(wù),有效地驗證了機(jī)械臂數(shù)學(xué)模型以及正逆運動學(xué)求解方法的正確性。

1 實驗平臺總體設(shè)計

本實驗平臺運用D-H參數(shù)表建立了五自由度機(jī)械臂的運動學(xué)模型,利用Z-Y-Z歐拉旋轉(zhuǎn)方法推導(dǎo)出正運動學(xué)方程,并采用代數(shù)方法進(jìn)行逆運動學(xué)求解并控制機(jī)械臂的運動,最后利用OpenGL圖形語言開發(fā)離線仿真功能。在OpenGL仿真界面,通過定時器不斷更新各關(guān)節(jié)的旋轉(zhuǎn)量,能在多種模式下對機(jī)械臂進(jìn)行運動控制,使機(jī)械臂達(dá)到期望的目標(biāo)點。

如圖1所示,本機(jī)械臂仿真實驗平臺主要由物理建模、Qt界面設(shè)計、運動學(xué)分析和OpenGL離線仿真4個模塊組成。

(1) 物理建模。該模塊主要是根據(jù)D-H參數(shù)表建立起機(jī)械臂的運動學(xué)模型,并確定機(jī)械臂的各種約束條件,例如機(jī)械臂各個舵機(jī)的PWM限制范圍、機(jī)械臂各個關(guān)節(jié)的有效旋轉(zhuǎn)角度以及舵機(jī)PWM與旋轉(zhuǎn)角度的準(zhǔn)確轉(zhuǎn)換關(guān)系。

(2) Qt上位機(jī)設(shè)計。本機(jī)械臂實驗平臺的上位機(jī)具有良好的交互界面,能與機(jī)械臂進(jìn)行串口通信,并能在多種模式下完成對機(jī)械臂的運動控制,例如對單個或同時對多個機(jī)械臂關(guān)節(jié)的運動進(jìn)行控制,單指令控制或者連續(xù)控制,在笛卡爾空間或關(guān)節(jié)空間對機(jī)械臂進(jìn)行運動控制,實現(xiàn)離線仿真等。

(3) 運動學(xué)解析。正運動學(xué)要求給出各個關(guān)節(jié)的角度后,能求解并控制機(jī)械臂的末端位置和姿態(tài)。本平臺運用Z-Y-Z歐拉旋轉(zhuǎn)方法進(jìn)行正運動學(xué)方程的推導(dǎo)。而逆運動學(xué)是在已知末端位置的基礎(chǔ)上,求解各個關(guān)節(jié)變量的角度。

(4) OpenGL仿真。該模塊要求搭建機(jī)械臂的三維模型,將組成機(jī)械臂的各個基本構(gòu)件封裝成模塊并精確組裝成3D模型,還要能對各個模式下的控制指令做出反應(yīng),實時地模擬機(jī)械臂運動的狀況。

2 運動學(xué)建模與求解

2.1 機(jī)械臂運動學(xué)模型

5-DOF機(jī)器臂是具有5個關(guān)節(jié)的空間機(jī)構(gòu),自下而上分別是5個旋轉(zhuǎn)關(guān)節(jié)和一個末端的機(jī)械夾具。為描述末端執(zhí)行器在空間的位置和姿態(tài),可以在每個關(guān)節(jié)上建立一個坐標(biāo)系(見圖2),利用坐標(biāo)系之間的關(guān)系來描述末端執(zhí)行器的位置。

圖2 機(jī)械臂坐標(biāo)系

圖2中所建立的坐標(biāo)系均滿足右手定則,故省略垂直于圖像平面的坐標(biāo)軸。其中坐標(biāo)系0為固定的世界坐標(biāo)系,歐拉坐標(biāo)系1—5分別依附于5個機(jī)械臂的旋轉(zhuǎn)關(guān)節(jié),并隨關(guān)節(jié)的轉(zhuǎn)動而變化。當(dāng)機(jī)械臂處于直立狀態(tài)且坐標(biāo)系1與世界坐標(biāo)系重合時,各個關(guān)節(jié)的轉(zhuǎn)動角度為0,定義此時的狀態(tài)為機(jī)械臂的初始狀態(tài)。此外,X2、X3、X4分別與所在關(guān)節(jié)連接的下一級連桿相平行。

為能夠?qū)C(jī)械臂建立較為精確的運動學(xué)模型,首先要建立機(jī)械臂的D-H參數(shù)表。由機(jī)械臂的初始狀態(tài)測得D-H參數(shù)表如表1所示。此外,由于機(jī)械臂結(jié)構(gòu)的限制,機(jī)械臂的各個關(guān)節(jié)并不能隨意旋轉(zhuǎn),所以需要測出機(jī)械臂各個關(guān)節(jié)的PWM極限值,以及機(jī)械臂各個關(guān)節(jié)的有效旋轉(zhuǎn)角度,并求出PWM與關(guān)節(jié)角度的關(guān)系。經(jīng)測量,PWM與關(guān)節(jié)旋轉(zhuǎn)角度存在線性關(guān)系,滿足公式(1)。機(jī)械臂各個關(guān)節(jié)的極限角度和極限PWM如表2所示。

θi=(PWMi-1 500)/7.6

(1)

表1 D-H參數(shù)表

相關(guān)的參數(shù)定義如下:

αi-1:繞Xi-1軸從Zi-1旋轉(zhuǎn)到Zi的角度；

ai-1:沿Xi-1軸從Zi-1移動到Zi的距離；

di:沿Zi從Xi-1移動到Xi的距離；

θi:繞Zi從Xi-1旋轉(zhuǎn)到Xi的角度。

表2 機(jī)械臂極限PWM及角度

2.2 正運動學(xué)分析

圖3 相鄰坐標(biāo)系位置關(guān)系圖

(2)

點P在坐標(biāo)系[i]中的坐標(biāo)表示iP與在坐標(biāo)系[i-1]中的坐標(biāo)表示i-1P之間的關(guān)系，如公式(3)所示。

(3)

定義了旋轉(zhuǎn)矩陣之后,利用Z-Y-Z歐拉旋轉(zhuǎn)法得到機(jī)械臂各個坐標(biāo)系之間的旋轉(zhuǎn)矩陣。公式(4)為Z-Y-Z歐拉旋轉(zhuǎn)公式[9]。由于篇幅所限，本文將不進(jìn)行公式的推導(dǎo)。在機(jī)械臂上兩個坐標(biāo)系間的旋轉(zhuǎn)矩陣可以看成是先將原始坐標(biāo)系O繞Z軸旋轉(zhuǎn)α角度得到坐標(biāo)系O ′,再將坐標(biāo)系O ′繞Y′軸旋轉(zhuǎn)β角度得到坐標(biāo)系O ″,最后再將坐標(biāo)系O ″繞Z″軸旋轉(zhuǎn)γ角度得到。

(4)

此外,由于相鄰坐標(biāo)系之間一般同時存在旋轉(zhuǎn)和位移,則相鄰兩坐標(biāo)系的齊次變換矩陣可表示為公式(5)。其中每個變換矩陣的i-1P(i)ORG可由D-H參數(shù)表得到。假設(shè)機(jī)械臂的末端位置在坐標(biāo)系5下表示為5P=(0,0,PZ),則根據(jù)正運動學(xué)求解即公式(6),可以得到機(jī)械臂末端位置在基坐標(biāo)系下的坐標(biāo)。

(6)

2.3 逆運動學(xué)分析

逆運動學(xué)的求解是根據(jù)已知的末端執(zhí)行器相對于參考坐標(biāo)系的位置,求解機(jī)械臂各個關(guān)節(jié)變量的過程,在相同的末端位置下可能存在多種關(guān)節(jié)轉(zhuǎn)角組合。

首先根據(jù)幾何關(guān)系和機(jī)械結(jié)構(gòu)的約束,機(jī)械臂末端目標(biāo)位置與基坐標(biāo)原點的距離應(yīng)滿足公式(7)。若給定的末端執(zhí)行器位置不滿足公式(7),上位機(jī)將彈出終點不可達(dá)的提示信息。

(7)

由于在機(jī)械臂上只有關(guān)節(jié)1能夠改變機(jī)械臂所在的平面,因此可以先利用幾何法確定關(guān)節(jié)1的轉(zhuǎn)動變量,先將目標(biāo)點投影到世界坐標(biāo)系的X0O0Y0平面上得到(Px,Py),并由公式(8)求解關(guān)節(jié)1的轉(zhuǎn)動量。

θ1=Atan 2(Py,Px)

(8)

由于機(jī)械臂末端的坐標(biāo)位置跟姿態(tài)無關(guān),所以機(jī)械臂的逆運動學(xué)只需要考慮關(guān)節(jié)1到關(guān)節(jié)4的4個關(guān)節(jié)變量,且關(guān)節(jié)1的轉(zhuǎn)動量可由公式(8)確定。本實驗平臺采用代數(shù)方法進(jìn)行逆運動學(xué)的求解,即假設(shè)已知1P=(1Px,1Py,1Pz)和4P=(4Px,4Py,4Pz),求解公式(9)中的θ2,θ3,θ4。

(9)

由于代數(shù)求解過程比較復(fù)雜且存在多解,故本文省略求解過程,最終選取與原來狀態(tài)相比有最小變化角度的結(jié)果作為最優(yōu)解。依據(jù)以上方法,機(jī)械臂可以在100 ms以內(nèi)完成逆運動學(xué)求解,符合實時控制的要求。配合機(jī)械臂的逆運動學(xué)方法,可以在目標(biāo)點位置已知的前提下完成對目標(biāo)物體的準(zhǔn)確抓取和搬運。

3 上位機(jī)界面設(shè)計

本實驗平臺上位機(jī)是基于QtWidget框架開發(fā)的,集成了串口通信模塊、關(guān)節(jié)空間控制模塊、笛卡爾空間控制模塊、離線仿真模塊以及機(jī)械臂末端實時坐標(biāo)(見圖4)。該平臺可在多種模式下對機(jī)械臂進(jìn)行運動控制以及離線仿真,具有簡潔的界面和良好的交互性。

離線仿真功能是運用圖形開發(fā)語言O(shè)penGL實現(xiàn)的。OpenGL是以SGI的GL三維圖形庫為基礎(chǔ)制定的一個通用、共享的開放式三維圖形標(biāo)準(zhǔn)，它是與硬件、窗口系統(tǒng)和操作系統(tǒng)相獨立的一系列API[10]。OpenGL具有編程建模、容易實現(xiàn)高度清晰感的實時三維仿真等優(yōu)點,逐漸被廣泛應(yīng)用于機(jī)器臂的設(shè)計和運動仿真中。OpenGL采用模塊化的方法進(jìn)行三維模型的創(chuàng)建,具有靈活性好、易于擴(kuò)展的優(yōu)點。

圖4 機(jī)械臂Qt上位機(jī)界面

(1) 串口通信模塊。利用Qt的SerialPort模塊,在上位機(jī)界面獲取串口的配置參數(shù)后開啟串口,從而與機(jī)械臂建立串口通信,并用對話框提示相關(guān)信息。

(2) 關(guān)節(jié)空間控制模塊。該模塊既可通過text控件控制機(jī)械臂的關(guān)節(jié)角度,也可以通過拉動滑動條控制機(jī)械臂,此外還能選擇單擊“發(fā)送”(進(jìn)行單指令控制)或“連續(xù)發(fā)送”兩種模式。模塊具有復(fù)位功能,并且可以設(shè)置機(jī)械臂完成動作的時間間隔。

(3) 笛卡爾空間控制模塊。該模塊是利用逆運動學(xué),將給定的機(jī)械臂末端位置轉(zhuǎn)換為各關(guān)節(jié)變量的角度。

(4) 機(jī)械臂末端實時坐標(biāo)。在上述不同的控制模式下,利用正運動學(xué)求解機(jī)械臂末端的位置。

(5) 離線仿真模塊。利用OpenGL圖形開發(fā)語言創(chuàng)建機(jī)械臂的各個基本構(gòu)件并將它們組裝起來,搭建起機(jī)械臂的3D模型(見圖5)。該模型也可以接收其他模塊的控制信號,并通過定時器不斷刷新視圖以達(dá)到動態(tài)仿真的效果。該模型可以表達(dá)機(jī)械臂末端執(zhí)行器的姿態(tài),模擬機(jī)械臂執(zhí)行目標(biāo)抓取任務(wù)等。

圖5 機(jī)械臂仿真界面

4 離線仿真實驗

在組裝機(jī)械臂3D模型之前,先創(chuàng)建組成機(jī)械臂的各個基本構(gòu)件的形狀并封裝成模塊,預(yù)留構(gòu)件的位置、旋轉(zhuǎn)軸以及旋轉(zhuǎn)量作為接口；然后再根據(jù)機(jī)械臂實物搭建3D模型。5-DOF機(jī)械臂的每個關(guān)節(jié)由多個形狀組成,每個形狀由多個面組成。繪制模型時,先繪制面,再由面組成形狀,然后由多個形狀組成關(guān)節(jié),最后由6個關(guān)節(jié)組成機(jī)械臂。

機(jī)械臂每個關(guān)節(jié)的空間位置隨著前一級關(guān)節(jié)的轉(zhuǎn)動而變化。為了達(dá)到關(guān)節(jié)聯(lián)動的效果,后一個關(guān)節(jié)的局部坐標(biāo)系必須與前一個關(guān)節(jié)保持一致。在調(diào)用關(guān)節(jié)的Draw()接口時調(diào)用OpenGL的庫函數(shù)glPushMatrix(),使上一個關(guān)節(jié)的局部坐標(biāo)系作為當(dāng)前坐標(biāo)系。如果該關(guān)節(jié)是最后一個關(guān)節(jié),繪圖完成后要調(diào)用glPopMatrix()來釋放棧上的當(dāng)前坐標(biāo)系[11]。

最終實驗結(jié)果表明,機(jī)械臂OpenGL仿真界面可以高度地對機(jī)械臂實體進(jìn)行在線或離線的仿真。仿真效果如圖6所示。

圖6 機(jī)械臂3D模型仿真效果對比圖

5 結(jié)語

在五自由度機(jī)械臂的三維物理模型的基礎(chǔ)上進(jìn)行正逆運動學(xué)的分析,運用OpenGL對機(jī)械臂的各個構(gòu)件進(jìn)行模塊化,最終實現(xiàn)了實驗平臺的離線仿真功能。該仿真實驗平臺能很好地反映機(jī)械臂的真實運動,實時地執(zhí)行目標(biāo)抓取任務(wù),有效地驗證了機(jī)械臂數(shù)學(xué)模型以及正逆運動學(xué)求解方法的正確性。

References)

[1] 任崇軒.五自由度機(jī)械臂運動和控制仿真分析[D].廣州:華南理工大學(xué),2012.

[2] 馬如奇,郝雙暉,鄭偉峰,等.基于MATLAB與ADAMS的機(jī)械臂聯(lián)合仿真研究[J].機(jī)械設(shè)計與制造,2010(4):93-95.

[3] 楊偉超,許德章.基于ADAMS的雙臂作業(yè)機(jī)器人的運動學(xué)分析及仿真[J].井岡山大學(xué)學(xué)報(自然科學(xué)版),2016,37(1):80-84.

[4] 孫亮,馬江,阮曉鋼.基于OpenGL的六自由度機(jī)械臂三維仿真工具的設(shè)計[J].計算機(jī)測量與控制,2009(5):982-985.

[5] 陳素麗,任福深.基于OpenGL的弧焊機(jī)器人仿真設(shè)計[J].科學(xué)技術(shù)與工程,2012,12(3):544-550.

[6] 吳越文,郝正航,陳卓,等.基于實時仿真器的開放式機(jī)器人實驗平臺[J].實驗技術(shù)與管理,2017,34(1):114-118.

[7] 劉延飛,李琪,畢經(jīng)存,等.一種全自主移動機(jī)器人實驗平臺的設(shè)計與研究[J].實驗技術(shù)與管理,2009,26(2):67-70.

[8] Craig J J. Introduction to robotics - mechanics and control[J].Automatica,2005,23(2):263-264.

[9] Niku S B. 機(jī)器人學(xué)導(dǎo)論:分析、系統(tǒng)及應(yīng)用[M].孫富春,譯.北京:電子工業(yè)出版社,2004.

[10] 嚴(yán)勇杰,朱齊丹,蔣丞.基于OpenGL的機(jī)械臂控制系統(tǒng)仿真平臺研究[J].計算機(jī)仿真,2006,23(8):252-257.

[11] 張華文,梁艷陽,賴永林.基于OpenGL的六自由度機(jī)械臂三維建模與仿真[J].自動化技術(shù)與應(yīng)用,2014(3):21-24,33.