蔡嘉暉，王紅偉，王漪夢

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

1 引言

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

目前，國外有著如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ò)展有較大的限制，對于某些國產(chǎn)或自研機(jī)器人無法提供支持。同時，在國內(nèi)也缺少相關(guān)成熟的機(jī)器人仿真軟件，無法滿足國內(nèi)對于機(jī)器人研究的大量需求。

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

2 機(jī)器人運動學(xué)分析

2.1 機(jī)器人的正運動學(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)換的四個參數(shù)為[6]：θ為關(guān)節(jié)轉(zhuǎn)角，α為兩關(guān)節(jié)扭轉(zhuǎn)角，d連桿長度，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之間的相對關(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ī)器人的逆運動學(xué)求解

(3)

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

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

由等號兩邊矩陣對應(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有兩個解，其余為一個解，因此得到在機(jī)器人非奇異位置，末端位姿對應(yīng)8組理論解。

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

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

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

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

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

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

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

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

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

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

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

4)代入機(jī)器人運動學(xué)

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

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

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

圖4 控制流程

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

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

4 三維可視化程序的運動學(xué)仿真

4.1 正運動學(xué)算法驗證

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

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

表2 正運動學(xué)算法驗證數(shù)據(jù)

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

4.2 逆運動學(xué)算法驗證

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

表3 逆運動學(xué)驗證數(shù)據(jù)

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

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

5 示教實驗驗證

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

圖7 仿真軟件實驗平臺

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

圖8 仿真軟件機(jī)器人控制實驗

6 結(jié)論

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