一種六軸機(jī)器人的三維可視化仿真程序設(shè)計(jì)

蔡嘉暉，王紅偉，王漪夢(mèng)

(1. 北方工業(yè)大學(xué)機(jī)械與材料工程學(xué)院，北京 100041；2. 北京航天測(cè)控技術(shù)有限公司，北京 100041)

1 引言

近年來，我國(guó)工業(yè)機(jī)器人產(chǎn)業(yè)呈現(xiàn)“快速成長(zhǎng)”和“國(guó)產(chǎn)替代”雙重特征，對(duì)于機(jī)器人的研究越來越受到重視，國(guó)家提出了工業(yè)2025計(jì)劃，確定了工業(yè)機(jī)器人的發(fā)展和應(yīng)用是我國(guó)制造業(yè)走向高端化和智能化的重中之重。在機(jī)器人研究過程中，機(jī)器人仿真技術(shù)可以幫助研究人員了解機(jī)器人的運(yùn)動(dòng)特性，設(shè)計(jì)合理的運(yùn)動(dòng)控制算法并進(jìn)行分析驗(yàn)證，提前解決機(jī)器人在設(shè)計(jì)制造和運(yùn)行方面的問題，能大幅降低研究成本，對(duì)機(jī)器人的研究提供便利性。

目前，國(guó)外有著如robotics機(jī)器人工具箱、DMWorks和Robotmaster等成熟的機(jī)器人仿真軟件，許多研究人員也通過其進(jìn)行機(jī)器人仿真技術(shù)的研究，如余樂、穆煜、石磊等通過matlab的robotics機(jī)器人工具箱進(jìn)行機(jī)器人的仿真技術(shù)研究[1-3]；高藝、呂明珠等借助DMWorks和Robotmaster專用機(jī)器人仿真軟件進(jìn)行工業(yè)機(jī)器人的模擬仿真研究[4，5]。然而上述幾種機(jī)器人仿真軟件都都是封裝好的機(jī)器人仿真集成庫，存在一定的局限性，無法根據(jù)自身需求進(jìn)行功能刪減，其應(yīng)用擴(kuò)展有較大的限制，對(duì)于某些國(guó)產(chǎn)或自研機(jī)器人無法提供支持。同時(shí)，在國(guó)內(nèi)也缺少相關(guān)成熟的機(jī)器人仿真軟件，無法滿足國(guó)內(nèi)對(duì)于機(jī)器人研究的大量需求。

故本文以AUBO-I5型協(xié)作機(jī)器人為研究對(duì)象，提出了一種基于OpenGL圖形庫和C++編程軟件的三維可視化仿真程序設(shè)計(jì)方法，其結(jié)構(gòu)簡(jiǎn)單，操作簡(jiǎn)便，具有良好的擴(kuò)展性。首先利用Solidworks創(chuàng)建機(jī)器人零件模型，建立機(jī)器人D-H模型求解機(jī)器人的運(yùn)動(dòng)學(xué)正逆解，根據(jù)“行程最短”法來選取機(jī)器人運(yùn)動(dòng)最優(yōu)解。最后搭建的機(jī)器人三維可視化仿真程序，可對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)仿真，并通過TCP連接真實(shí)機(jī)器人實(shí)現(xiàn)了仿真運(yùn)動(dòng)示教實(shí)驗(yàn)，驗(yàn)證了所提出機(jī)器人運(yùn)動(dòng)學(xué)算法的正確性。

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

2.1 機(jī)器人的正運(yùn)動(dòng)學(xué)求解

工業(yè)機(jī)器人由各連桿連接而成，根據(jù)D-H坐標(biāo)系建立準(zhǔn)則可以確定相鄰兩連桿之間的轉(zhuǎn)換關(guān)系，通過圖1所示的機(jī)器人D-H坐標(biāo)系，可以確定機(jī)器人的D-H參數(shù)表如下所示。假設(shè)第i-1坐標(biāo)系到第i坐標(biāo)系轉(zhuǎn)換的四個(gè)參數(shù)為[6]：θ為關(guān)節(jié)轉(zhuǎn)角，α為兩關(guān)節(jié)扭轉(zhuǎn)角，d連桿長(zhǎng)度，a兩連桿間偏置距離，其中只有關(guān)節(jié)轉(zhuǎn)角θ為變量，其余均為常量。

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

表1 AUBO-I5機(jī)器人D-H參數(shù)

根據(jù)機(jī)器人D-H參數(shù)確定相鄰兩連桿i-1和i之間的相對(duì)關(guān)系可知

Trans(0，0，di)·Rot(zi，θi)

(1)

機(jī)器人末端的位姿可描述為

(2)

其中：

nx=-s6c1sθ1-3-4-c6(c5c1cθ4-3+2+s1s5)

ox=s6(c5c1cθ4-3+2+s1s5)+c6c1sθ4-3+2

ax=s1c5-s5(c4(c1s2s3+c1c2c3)+s4(c1c2s3-c1s2c3))

px=d6(c5s1-s5c1cθ3-2-4)-a3c1cθ2-3+d2s1-d5c1sθ3-2-4-a2c1s2

ny=c6(c1s5-c5s1cθ3-2-4)-a3c1cθ2-3+d2s1-d5c1sθ3-2-4-a2c1s2

oy=-s6(c1s5-c5s1cθ3-2-4)-c6s1sθ3-2-4

ay=-c1c5-s5(c4(s1s2s3+s1c2c3)+s4(s1c2s3-s1c2c3))

Py=-a3s1cθ3-2-d2c1-d6(c1c5+s5s1cθ3-2-4)-d5s1sθ3-2-4-a2s1sθ3-2

nz=s5c6cθ3-2-4-s6cθ3-2-4

oz=-c6cθ3-2-4-c5s6sθ3-2-4

az=s5sθ3-2-4

Pz=d1-d5cθ3-2-4+a3sθ3-2+a2c2+d6s5sθ3-2-4

其中s1=sin(θ1)、θ1-3-4=θ1-θ3-θ4，其它相同。

2.2 機(jī)器人的逆運(yùn)動(dòng)學(xué)求解

(3)

根據(jù)方程左右兩邊矩陣對(duì)應(yīng)元素相等原則，即L(2，3)=R(2，3)、L(2，4)=R(2，4)可得

θ1=atan2(-py+d6ay，-px+d6ax)-atan2(d2，±A)

由等號(hào)兩邊矩陣對(duì)應(yīng)元素相等可得

θ5=atan 2(±B，axsθ1-nycθ1)

同理可得

(4)

同理

θ4=atan2(Hc(θ2-θ3)-Is(θ2-θ3)，Ic(θ2-θ3)+Hs(θ2-θ3))

其中

C=d5E+d6(axcθ1+aysθ1)-pxcθ1-pysθ1

D=pz-d1-azd6-d5(nxcθ6+nzsθ6)

E=cθ6(oxcθ1+oysθ1)+sθ6(nxcθ1+nysθ1)

H=-cθ6(oxcθ1+oysθ1)-sθ6(nxcθ1+nysθ1)

I=ozcθ6+nzsθ6

機(jī)器人θ1、θ5、θ3有兩個(gè)解，其余為一個(gè)解，因此得到在機(jī)器人非奇異位置，末端位姿對(duì)應(yīng)8組理論解。

2.3 機(jī)器人最優(yōu)解的選取

通過逆運(yùn)動(dòng)學(xué)求解得到的機(jī)器人關(guān)節(jié)角度有8組解，但在實(shí)際的機(jī)器人控制中只需要選取一組最適合的解作為最終的可行解。最優(yōu)解的篩選原則有“行程最短”準(zhǔn)則，能量消耗最小準(zhǔn)則、運(yùn)動(dòng)最平穩(wěn)準(zhǔn)則等，根據(jù)AUBO-i5機(jī)器人的結(jié)構(gòu)特點(diǎn)和控制原理，本文將基于“行程最短”準(zhǔn)則選取機(jī)器人的最優(yōu)可行解[8]，同時(shí)遵循大臂少動(dòng)，小臂多動(dòng)的原則，引入大臂加權(quán)系數(shù)ω1，小臂加權(quán)系數(shù)ω2。最短行程準(zhǔn)則最優(yōu)解的選取流程如圖2所示。

圖2 最短行程準(zhǔn)則算法流程圖

3 三維可視化方程程序的設(shè)計(jì)

機(jī)器人的三維可視化仿真程序通過C++編程軟件進(jìn)行搭建，包含機(jī)器人的三維模型和數(shù)據(jù)通訊兩部分，它能模仿機(jī)器人的真實(shí)三維模型同時(shí)具有真實(shí)機(jī)器人的運(yùn)動(dòng)學(xué)關(guān)系，可在人機(jī)分離環(huán)境中有效實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)可視化并對(duì)機(jī)器人進(jìn)行動(dòng)作示教控制。

1)創(chuàng)建機(jī)器人零件模型

機(jī)器人分為底座和機(jī)械臂1到機(jī)械臂6一共7個(gè)零件。在solidworks中根據(jù)AUBO機(jī)器人的真實(shí)結(jié)構(gòu)參數(shù)，對(duì)機(jī)器人各零件進(jìn)行建模，并確定各零件的零件坐標(biāo)系，各零件模型如圖3所示。

圖3 機(jī)器人各零件模型

把機(jī)器人模型文件轉(zhuǎn)換為obj格式的文本文件，其原理是把三維模型表面分解紛若干個(gè)三角面，把各三角面的三點(diǎn)坐標(biāo)進(jìn)行保存。

3)C++重建機(jī)器人模型

根據(jù)機(jī)器人的結(jié)構(gòu)，確定各零件的初始位置和姿態(tài)，確定個(gè)零件坐標(biāo)系在機(jī)器人坐標(biāo)系下的相對(duì)位置，通過C++讀取obj文件，在確定的坐標(biāo)位置下繪制零件，構(gòu)造整體的機(jī)器人仿真模型。

4)代入機(jī)器人運(yùn)動(dòng)學(xué)

確定機(jī)器人各關(guān)節(jié)的轉(zhuǎn)角范圍，設(shè)計(jì)關(guān)節(jié)類型和關(guān)節(jié)驅(qū)動(dòng)，根據(jù)搭建的機(jī)器人仿真模型，代入上述的機(jī)器人正逆運(yùn)動(dòng)學(xué)方程，使機(jī)器人仿真模型具有運(yùn)動(dòng)學(xué)關(guān)系。

5)數(shù)據(jù)通訊

機(jī)器人的三維可視化仿真程序可與真實(shí)機(jī)器人進(jìn)行連接，可實(shí)時(shí)反饋機(jī)器人的運(yùn)動(dòng)信息并對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)控制。機(jī)器人與仿真軟件的控制流程如圖4所示，仿真軟件通過按鍵和示教輸入得到機(jī)器人位姿增量，計(jì)算出機(jī)器人運(yùn)動(dòng)目標(biāo)位姿，然后根據(jù)逆運(yùn)動(dòng)學(xué)算法計(jì)算出機(jī)器人的各組逆解，結(jié)合機(jī)器人當(dāng)前關(guān)節(jié)角度得到機(jī)器人的運(yùn)動(dòng)目標(biāo)最優(yōu)解，最后更新系統(tǒng)數(shù)據(jù)，從而控制AUBO機(jī)器人的運(yùn)動(dòng)以及更新軟件的三維仿真圖像顯示。

圖4 控制流程

機(jī)器人的三維仿真軟件界面如圖5所示，可實(shí)時(shí)顯示機(jī)器人的關(guān)節(jié)角度和末端位姿信息，同時(shí)可通過點(diǎn)擊程序按鍵和編程兩種方式對(duì)連接的真實(shí)機(jī)器人進(jìn)行動(dòng)作示教控制。

圖5 三維可視化仿真軟件界面

4 三維可視化程序的運(yùn)動(dòng)學(xué)仿真

4.1 正運(yùn)動(dòng)學(xué)算法驗(yàn)證

由于三維可視化仿真程序與D-H法構(gòu)建的運(yùn)動(dòng)學(xué)模型的零位不同，因此需要在機(jī)器人仿真程序中代入初始關(guān)節(jié)角度θ，使兩者零位統(tǒng)一。在AUBO機(jī)器人示教器上選取5組關(guān)節(jié)角度作為機(jī)器人正解算法的輸入，通過計(jì)算可得機(jī)器人在對(duì)應(yīng)關(guān)節(jié)角度下末端坐標(biāo)系在基坐標(biāo)中的位置矢量，其結(jié)果如表2所示。

“周長(zhǎng)”與“面積”是從不同維度對(duì)空間的度量。周長(zhǎng)限于一維空間，面積發(fā)展至二維空間。小學(xué)生對(duì)面積理解的基本趨勢(shì)是：從面積表征與長(zhǎng)度表征或周長(zhǎng)表征混淆，到最后分化為清楚的面積表征。因此，讓學(xué)生建立面積概念的初始階段，應(yīng)注重多種對(duì)比的方式，幫助學(xué)生區(qū)別。前期對(duì)于兩者的區(qū)分主要停留在“描周長(zhǎng)、涂面積”的階段，實(shí)踐下來學(xué)生對(duì)兩者的區(qū)分還是不夠明晰，后期還可從以下方面繼續(xù)落實(shí)。

表2 正運(yùn)動(dòng)學(xué)算法驗(yàn)證數(shù)據(jù)

對(duì)比AUBO機(jī)器人與正解算法結(jié)果，可知兩者在相同關(guān)節(jié)角度下對(duì)應(yīng)的工具坐標(biāo)系位置矢量完全一致，同時(shí)仿真程序與真實(shí)機(jī)器人的姿態(tài)相同，從而驗(yàn)證了所提出運(yùn)動(dòng)學(xué)正解算法的正確性。

4.2 逆運(yùn)動(dòng)學(xué)算法驗(yàn)證

隨機(jī)選取一組機(jī)器人的關(guān)節(jié)角度(40°，20°，-30°，70°，120°，-50°)，通過正運(yùn)動(dòng)算法得到機(jī)器人的末端位姿為(-378.836，-415.134，742.833，68.829，-40.776，-7.946)，把改組位姿輸入機(jī)器人逆運(yùn)動(dòng)學(xué)算法中，可以得到機(jī)器人對(duì)應(yīng)關(guān)節(jié)的8組解，其數(shù)值如表3所示。

表3 逆運(yùn)動(dòng)學(xué)驗(yàn)證數(shù)據(jù)

圖6為機(jī)器人運(yùn)動(dòng)學(xué)逆解對(duì)應(yīng)關(guān)節(jié)角度的三維仿真位形結(jié)果顯示，可以看到8組逆解對(duì)應(yīng)的機(jī)器人末端位姿是一致的，且等于選取的初始角正解計(jì)算結(jié)果，從而證明了正逆運(yùn)動(dòng)學(xué)算法的可逆性和正確性。通過比較運(yùn)動(dòng)學(xué)逆解關(guān)節(jié)角度和三維仿真結(jié)果圖，考慮關(guān)節(jié)運(yùn)動(dòng)范圍和桿件干涉等因素，可知第8組逆解超過了機(jī)器人4軸的關(guān)節(jié)運(yùn)動(dòng)范圍，排除不合理的逆解，其余七組逆解均符合運(yùn)動(dòng)要求，然后通過“行程最短”法進(jìn)行處理，進(jìn)而得到機(jī)器人運(yùn)動(dòng)的一組最優(yōu)逆解。

圖6 機(jī)器人逆解對(duì)應(yīng)位形

5 示教實(shí)驗(yàn)驗(yàn)證

本文構(gòu)建的機(jī)器人三維可視化仿真系統(tǒng)可通過TCP連接機(jī)器人，得到機(jī)器人的關(guān)節(jié)角度及末端位姿，對(duì)機(jī)器人進(jìn)行實(shí)時(shí)的三維顯示，同時(shí)能通過仿真程序?qū)C(jī)器人進(jìn)行示教控制，分為按鍵和編程兩種示教方式。圖7為本次機(jī)器人仿真示教控制的實(shí)驗(yàn)平臺(tái)。

圖7 仿真軟件實(shí)驗(yàn)平臺(tái)

如圖8所示，圖8(a)是通過虛擬按鍵對(duì)機(jī)器人進(jìn)行關(guān)節(jié)控制和末端位姿控制，圖8(b)是通過輸入代碼進(jìn)行編程示教。在仿真軟件的機(jī)器人控制實(shí)驗(yàn)中，其仿真三維模型與真實(shí)機(jī)器人位形相同，同時(shí)其關(guān)節(jié)角度和末端位姿參數(shù)相同，證明仿真軟件的機(jī)器人運(yùn)動(dòng)學(xué)算法與AUBO機(jī)器人一致，通過仿真軟件對(duì)機(jī)器人的動(dòng)作示教控制是可行的。

圖8 仿真軟件機(jī)器人控制實(shí)驗(yàn)

6 結(jié)論

本文以AUBO-I5型機(jī)器人為研究對(duì)象，基于C++和OpenGL提出了一種機(jī)器人的三位可視化仿真程序設(shè)計(jì)方法，通過Solidworks建立了機(jī)器人的各零件模型，分析了機(jī)器人的正、逆運(yùn)動(dòng)學(xué)求解方法和最優(yōu)解的選取問題，并對(duì)比真實(shí)機(jī)器人驗(yàn)證了算法的正確性。同時(shí)，仿真程序能連接真實(shí)機(jī)器人，能對(duì)其進(jìn)行控制示教，對(duì)于機(jī)器人的控制研究提供了一定的便利性。本文設(shè)計(jì)的機(jī)器人三維可視化仿真程序具有模型逼真，動(dòng)畫刷新率高等優(yōu)點(diǎn)，同時(shí)基于C++的編程軟件擴(kuò)展性強(qiáng)，能針對(duì)各種控制條件和末端執(zhí)行器進(jìn)行功能擴(kuò)展，對(duì)于后續(xù)機(jī)器人的設(shè)計(jì)與控制研究奠定了基礎(chǔ)。