工業(yè)機(jī)器人三維示教模型的設(shè)計(jì)與實(shí)現(xiàn)

楊勝安，汪明，*，趙永國(guó)

（1.山東建筑大學(xué) 信息與電氣工程學(xué)院，山東 濟(jì)南250101；2.山東省科學(xué)院自動(dòng)化研究所 山東省機(jī)器人與制造自動(dòng)化技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東濟(jì)南250014）

0 引言

工業(yè)機(jī)器人三維示教的原理是利用計(jì)算機(jī)圖形學(xué)仿真機(jī)器人三維模型，通過(guò)運(yùn)動(dòng)學(xué)計(jì)算公式與三維模型相結(jié)合，將工業(yè)機(jī)器人運(yùn)動(dòng)狀態(tài)直觀地顯示出來(lái)，還可以對(duì)工業(yè)機(jī)器人的運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)動(dòng)態(tài)模擬顯示，這對(duì)某些特殊空間作業(yè)的工業(yè)機(jī)器人顯得尤為重要［1］。目前，國(guó)外的機(jī)器人研究機(jī)構(gòu)已開(kāi)始三維示教器的研發(fā)，同時(shí)有工業(yè)機(jī)器人示教器研發(fā)機(jī)構(gòu)提出了基于虛擬現(xiàn)實(shí)VR（Virtual Reality）技術(shù)的虛擬示教方式；國(guó)內(nèi)對(duì)三維示教器的研發(fā)力度逐步加大，而三維仿真示教在工業(yè)機(jī)器人發(fā)展過(guò)程中發(fā)揮著日益重要的作用［2］。在工業(yè)機(jī)器人三維仿真示教研發(fā)中，有研發(fā)機(jī)構(gòu)直接使用OpenGL繪制三維模型，這種建模仿真方式簡(jiǎn)單，但畫(huà)面粗糙、畫(huà)質(zhì)較差，很難達(dá)到逼真的視覺(jué)效果，還有研發(fā)機(jī)構(gòu)基于機(jī)器操作系統(tǒng)ROS（Robot Operating System）系統(tǒng)開(kāi)發(fā)出較為逼真的效果，但其無(wú)法基于Windows系統(tǒng)開(kāi)發(fā)運(yùn)行［3-6］。而Qt與OpenGL均具備優(yōu)秀的跨平臺(tái)特性，在多平臺(tái)運(yùn)行編譯時(shí)，幾乎不需要修改程序，只需在不同系統(tǒng)平臺(tái)下編譯即可運(yùn)行［7］。

文章面向6關(guān)節(jié)工業(yè)機(jī)器人，為尋求更為逼真的示教效果和更為優(yōu)秀的仿真系統(tǒng)通用性與跨平臺(tái)特性，采用SolidWorks繪制三維模型，使用3DS渲染，基于Qt和OpenGL相結(jié)合對(duì)三維模型仿真顯示系統(tǒng)進(jìn)行開(kāi)發(fā)研究；采用D-H（Denavit-Hartenberg）參數(shù)法找出相鄰桿件的參數(shù)，根據(jù)參數(shù)對(duì)機(jī)器人進(jìn)行正、逆運(yùn)動(dòng)學(xué)解析，并通過(guò)Matlab程序?qū)ζ溥\(yùn)動(dòng)進(jìn)行仿真驗(yàn)證。最終將運(yùn)動(dòng)學(xué)解析程序與三維模型程序融合，使用四元數(shù)法表示其姿態(tài)，工業(yè)機(jī)器人的三維模型在Qt中以逼真的視覺(jué)效果顯示。

1 工業(yè)機(jī)器人三維示教模型運(yùn)動(dòng)學(xué)設(shè)計(jì)

仿真模型采用工業(yè)機(jī)器人領(lǐng)域典型的6關(guān)節(jié)機(jī)器人，機(jī)器人本體外形模仿人的手臂共有6個(gè)旋轉(zhuǎn)關(guān)節(jié)，包括基座（關(guān)節(jié)J1）、肩部（關(guān)節(jié)J2）、肘部（關(guān)節(jié) J3）、腕部1（關(guān)節(jié) J4）、腕部 2（關(guān)節(jié) J5）、腕部 3（關(guān)節(jié)J6）。每一個(gè)關(guān)節(jié)代表一個(gè)自由度，其中3個(gè)自由度控制位置，另3個(gè)自由度控制姿勢(shì)。

1.1 模型參數(shù)確定

工業(yè)機(jī)器人末端坐標(biāo)系的標(biāo)定采用相鄰桿件的位置關(guān)系進(jìn)行描述，相鄰桿件的關(guān)系通常采用一種通用矩陣表達(dá)的D-H參數(shù)法，對(duì)工業(yè)機(jī)器人每個(gè)連桿均采用4×4的齊次變換矩陣描述相鄰兩連桿的空間關(guān)系，通過(guò)各關(guān)節(jié)的變換矩陣推導(dǎo)末端執(zhí)行器與基坐標(biāo)的相對(duì)位置姿態(tài)［8］。D-H參數(shù)法由4個(gè)參數(shù)構(gòu)成，公共法線ɑi與垂直于ɑi所在平面的兩軸線間夾角αi描述一個(gè)桿件，兩桿件的相對(duì)位置di和相鄰桿件法線的夾角θi描述相鄰兩桿件的關(guān)系。0x0y0z0為固定參考坐標(biāo)系又稱(chēng)為基坐標(biāo)系，0xi yi zi為工業(yè)機(jī)器人桿件i在桿件i+1軸上的坐標(biāo)系。仿真研究的6關(guān)節(jié)機(jī)器人三維模型和連桿參數(shù)坐標(biāo)系分別如圖1、2所示，6關(guān)節(jié)機(jī)器人的連桿參數(shù)見(jiàn)表1。

圖1 6關(guān)節(jié)機(jī)器人三維模型圖

圖2 6關(guān)節(jié)機(jī)器人連桿坐標(biāo)系圖

表1 6關(guān)節(jié)機(jī)器人的連桿參數(shù)表

1.2 工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)模型分析

（1）工業(yè)機(jī)器人正運(yùn)動(dòng)學(xué)模型

機(jī)器人正運(yùn)動(dòng)學(xué)解析方法采用D-H參數(shù)法，根據(jù)表1給定的6關(guān)節(jié)機(jī)器人的模型連桿參數(shù)，結(jié)合各關(guān)節(jié)的相對(duì)旋轉(zhuǎn)、平移變化得到關(guān)節(jié)i相對(duì)于關(guān)節(jié)i-1的變換矩陣i-1Ti。已知各關(guān)節(jié)角度θi，求解末端執(zhí)行器變化矩陣0T6，描述了末端連桿坐標(biāo)系0x6y6z6相對(duì)于基坐標(biāo)系0x0y0z0的位姿。末端位姿矩陣T包含法向矢量n、姿態(tài)矢量o、接近矢量ɑ和位置矢量p［9］。末端姿態(tài)矩陣由式（1）表示為

式中：nx＝-c6［s5c1s23-c5（s1s4+c4c1c23）］-s6（c4s1-s4c1c23）；ny＝s6（c1c4+s1s4c23）-c6（s5s1s23+c5（s4c1-s1c4c23））；nz＝-c6（s5c23+c4c5s23）-s4s6s23；ox＝s6（s5c1s23-c5（s1s4+c4c1c23））-c6（c4s1-s4c1c23）；

oy＝c6（c1c4+s1s4c23）+s6［s5s1s23+c5（s4c1-s1c4c23）］；oz＝s6（s5c23+c4c5s23）-s4c6s23；ɑx＝c5c1s23+s5（s1s4+c4c1c23）；ɑy＝c5s1s23-s5（c1s4-c4s1c23）；ɑz＝c5c23-c4c5s23；px＝ɑ1c1+ɑ3c1c23-d4c1s23+ɑ2c1c2；py＝ɑ1s1+ɑ3s1c23-d4s1s23+ɑ2c2s1；pz＝-ɑ2s2-ɑ3s23-d4c23。 其中，c1為 cosθi；si為 sinθi，i＝1，2，……，6；c23為 cos（θ2+θ3）＝c2c3-s2s3；s23為 sin（θ2+θ3）＝c2s3+s2c3。

（2）工業(yè)機(jī)器人逆運(yùn)動(dòng)學(xué)模型

已知機(jī)器人末端執(zhí)行器的位姿T，求解機(jī)器人各關(guān)節(jié)旋轉(zhuǎn)角度θi，即為機(jī)器人逆運(yùn)動(dòng)學(xué)求解的過(guò)程。在求解關(guān)節(jié)角θi時(shí)，可用逆矩陣i-1Ti依次左乘矩陣0T6，逐步分離各關(guān)節(jié)變量，從而對(duì)其求解。比如求解 θ1時(shí)，讓逆變換左乘得到并令上式左、右兩端乘積矩陣的元素（3，4）對(duì)應(yīng)相等，即-px s1+py c1＝0，經(jīng)計(jì)算得出θ1＝αtan2（px，py）。類(lèi)似于 θ1的求解方法，可求得θ2、θ3、θ4、θ5、θ6，分別由式（2）～（7）表示為

圖3 6關(guān)節(jié)機(jī)器人姿態(tài)圖

式中：

根據(jù)θi的角度公式和θi的變化范圍基本可排除多解情況。若出現(xiàn)多值情況無(wú)法根據(jù)角度范圍排除時(shí)，則方差最小的一組為最優(yōu)解。

1.3 Matlab運(yùn)動(dòng)學(xué)模型仿真

通過(guò) Matlab強(qiáng)大的計(jì)算平臺(tái)，利用 Robotic Toolbox對(duì)6關(guān)節(jié)機(jī)器人進(jìn)行建模［10］。根據(jù)各連桿之間的D-H參數(shù)在Matlab中建立robot.m文件進(jìn)行仿真演示，在空間中選取2個(gè)關(guān)節(jié)空間點(diǎn)矢量qA＝［0，0，0，0，0，0］、qB＝［2，0，4，1，3，1］，對(duì)空間點(diǎn)qA到qB進(jìn)行關(guān)節(jié)空間規(guī)劃軌跡，給定的時(shí)間向量長(zhǎng)度為10 s，采樣時(shí)間為100 ms。并且對(duì)各關(guān)節(jié)的位移曲線、角位移、角速度曲線、角加速度曲線、末端軌跡圖像進(jìn)行仿真驗(yàn)證，6關(guān)節(jié)機(jī)器人最終姿態(tài)和末端軌跡分別如圖3、4所示。

圖4 機(jī)器人末端軌跡圖

2 工業(yè)機(jī)器人三維示教實(shí)現(xiàn)

2.1 三維仿真模型繪制

通常繪制復(fù)雜三維模型的一般方式為使用專(zhuān)門(mén)的建模軟件（如3dsMax、Maya）創(chuàng)建逼真度較高的模型，以特定格式導(dǎo)出模型，將模型導(dǎo)入三維場(chǎng)景中實(shí)現(xiàn)繪制與控制，營(yíng)造出逼真的三維場(chǎng)景。采用模型導(dǎo)入的方式進(jìn)行三維模型繪制，由于SolidWorks在三維圖形繪制上具有組件多、易操作、功能強(qiáng)大等優(yōu)點(diǎn)，所以基本三維圖形采用SolidWorks軟件進(jìn)行繪制；三維模型以STL格式文件導(dǎo)出，再將其導(dǎo)入3DSMax中進(jìn)行紋理坐標(biāo)確立；然后輸出3DS格式文件，得到比較逼真的效果，再將3DS文件導(dǎo)入Deep Exploration進(jìn)行三維模型材質(zhì)和紋理的二次處理后導(dǎo)出.cpp文件；通過(guò)Qt導(dǎo)入以O(shè)penGL為標(biāo)準(zhǔn)的工作場(chǎng)景中渲染，最后得到畫(huà)面逼真的三維模型虛擬仿真平臺(tái)［11］。三維仿真模型設(shè)計(jì)流程如圖5所示。

圖5 三維仿真模型設(shè)計(jì)流程圖

2.2 三維仿真示教界面實(shí)現(xiàn)

3DS模型文件主要包括物體、材質(zhì)、燈光、關(guān)鍵幀等部分，其基本構(gòu)成單位是塊。每個(gè)塊由塊信息和塊數(shù)據(jù)組成，塊信息又包含塊ID和塊長(zhǎng)度，塊與塊之間是相互嵌套的，在讀取的時(shí)候必須以遞歸的方式進(jìn)行［12］。經(jīng)渲染后的3DS模型載入方式有多種，設(shè)計(jì)時(shí)采用比較簡(jiǎn)單的一種方式。在 Deep Exploration中打開(kāi)*.3ds模型，轉(zhuǎn)換為“OpenGL CPPCode（*.cpp）”格式，設(shè)置“Settings”按鈕選擇Display list，在“Geometry Transform”中設(shè)置模型大小范圍與原點(diǎn)。設(shè)置后將所需要的類(lèi)文件和函數(shù)文件轉(zhuǎn)換成.h和.cpp文件，在后者中 structsample_MATERIAL材料信息由結(jié)構(gòu)體來(lái)表述；static GLfloat textures［10393］［2］存儲(chǔ)3D模型所有紋理坐標(biāo)；static GLfloat vertices［9153］［3］存儲(chǔ) 3D模型中所有頂點(diǎn)坐標(biāo)；GLint Gen3DObjectList（）為對(duì)外接口函數(shù)。在Qt中建立顯示列表glGenList（），調(diào)用相關(guān)的.h和.cpp文件即可顯示視覺(jué)效果十分逼真的三維仿真模型。

在Qt中加載三維模型.cpp文件時(shí)，常會(huì)出現(xiàn)顏色的丟失，整個(gè)模型呈現(xiàn)白色，仿真模型將嚴(yán)重失真，解決此問(wèn)題通常有2種方法：（1）使用紋理貼圖函數(shù)載入紋理 bindTexture（QPixmap（m_FileName））。（2）使用 sun_light_position［］設(shè)置三維空間的光源位置，使用glLightfv（GL_LIGHTn，GL_POSITION，sun_light_position）指定三維空間中第n號(hào)光源，產(chǎn)生使能 glEnable（GL_LIGHTn）打開(kāi)光源［13］。三維模型視圖窗口如圖6所示。

2.3 運(yùn)動(dòng)學(xué)與三維模型融合界面實(shí)現(xiàn)

6關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)學(xué)模型與三維模型設(shè)計(jì)完成后，三維模型經(jīng)OpenGL函數(shù)調(diào)用，導(dǎo)入到Qt界面中顯示，并以單獨(dú)的界面顯示三維模型視圖窗口，如圖5所示。顯示的三維模型僅可以實(shí)現(xiàn)單軸旋轉(zhuǎn)控制，在實(shí)際的工業(yè)現(xiàn)場(chǎng)作業(yè)環(huán)境中，工業(yè)機(jī)器人需要執(zhí)行的控制指令如直線運(yùn)動(dòng)、弧線運(yùn)動(dòng)等指令，單軸運(yùn)動(dòng)已經(jīng)無(wú)法滿(mǎn)足作業(yè)要求，需要機(jī)器人各關(guān)節(jié)同時(shí)聯(lián)動(dòng)。在三維模型空間內(nèi)定義用戶(hù)坐標(biāo)系，將各種運(yùn)動(dòng)軌跡轉(zhuǎn)化為工業(yè)機(jī)器人TCP點(diǎn)在三維空間坐標(biāo)系下的空間坐標(biāo)點(diǎn)的集合，將空間坐標(biāo)點(diǎn)（x，y，z）的坐標(biāo)帶入在運(yùn)動(dòng)學(xué)解析方程中進(jìn)行運(yùn)算。在程序中定義結(jié)構(gòu)體 typedef struct XYZPos｛float fXPos；float fYPos；float fZPos；｝POS；／／3個(gè)方向的位移。floatg_fInAngle［6］；／／輸入 6個(gè)關(guān)節(jié)的角度值。float g_fOutXYZ［3］；／／輸出坐標(biāo)系。控制機(jī)器人運(yùn)動(dòng)方向?qū)嶋H是控制坐標(biāo)點(diǎn)的位置，根據(jù)正、逆運(yùn)動(dòng)學(xué)解析公式進(jìn)行計(jì)算坐標(biāo)點(diǎn)坐標(biāo)，經(jīng)過(guò)逆運(yùn)動(dòng)學(xué)解析將TCP點(diǎn)的空間坐標(biāo)點(diǎn)解析得到6個(gè)關(guān)節(jié)實(shí)時(shí)的角度。將解析結(jié)果的角度使用C++語(yǔ)言程序編寫(xiě)，控制量為機(jī)器人各關(guān)節(jié)模塊三維模型模塊的旋轉(zhuǎn)角度。為取得三維模型具備良好控制性，精確顯示工業(yè)機(jī)器人的位置與姿態(tài)信息、速度信息等，將運(yùn)動(dòng)學(xué)的正、逆分析求解的結(jié)果程序化并與三維模型相結(jié)合。將運(yùn)動(dòng)學(xué)解析結(jié)果程序化時(shí)角度量轉(zhuǎn)化為弧度量，用位置矢量p表達(dá)末端執(zhí)行器相對(duì)于基坐標(biāo)的空間（x，y，z）位置。在控制角度旋轉(zhuǎn)時(shí)，各關(guān)節(jié)在角度范圍內(nèi)運(yùn)動(dòng)，需要用float judge（floatf）函數(shù)對(duì)個(gè)角度量進(jìn)行角度范圍判斷。姿態(tài)表達(dá)則需要n、o、ɑ等3個(gè)矢量表示，末端姿態(tài)采用四元數(shù)表示法［14］。

式中：q為四元數(shù)；ɑ、b、c、d為實(shí)數(shù)；u為矢量＜b，c，d＞。

工業(yè)機(jī)器人的三維仿真模型顯示以獨(dú)立窗口顯示，工程中需要將該顯示窗口融入到示教器的機(jī)器人示教界面中，通過(guò)機(jī)器人示教界面的按鈕對(duì)三維機(jī)器人模型進(jìn)行控制。將 OpenGL顯示窗口QOpenGLWidget進(jìn)行提升，QOpenGLWidget在Qt的OpenGL界面內(nèi)部使用，創(chuàng)建QGLWidget類(lèi)設(shè)置繼承自QOpenGLWidget類(lèi)，在Qt的顯示界面中放置OpenGLWidget控件并且將控件提升即可將三維顯示窗口融入到示教界面中，提升的類(lèi)名為QGLWidget。機(jī)器人運(yùn)動(dòng)學(xué)模型與三維模型融合之后三維仿真示教界面如圖7所示。

圖7 三維仿真示教界面圖

工業(yè)機(jī)器人三維仿真模型的每個(gè)關(guān)節(jié)分別有每組兩個(gè)PushButton表示，其中+表示關(guān)節(jié)度數(shù)增加，-表示關(guān)節(jié)度數(shù)遞減，仿真視圖中X+、X-、Y+、Y-、Z+、Z-分別表示機(jī)器人在用戶(hù)基坐標(biāo)系中TCP點(diǎn)的運(yùn)動(dòng)方向。三維示教仿真界面中MousePressEvent接口獲取鼠標(biāo)按鍵信息、MouseMoveEvent接口獲取鼠標(biāo)前后移動(dòng)信息、WheelEvent鼠標(biāo)滾輪信息來(lái)控制三維仿真圖形的平移、旋轉(zhuǎn)、縮放，設(shè)計(jì)為了方便與某些特殊工業(yè)平板電腦的操作增加觸屏功能［15］。

3 結(jié)語(yǔ)

使用D-H參數(shù)法對(duì)6關(guān)節(jié)工業(yè)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)正、逆分析，并利用Matlab的Robotic Toolbox對(duì)模型進(jìn)行了驗(yàn)證，采用SolidWorks繪制三維模型，在3DS中渲染出逼真的三維效果，三維仿真系統(tǒng)操作界面基于Qt與OpenGL開(kāi)發(fā)具備良好視覺(jué)效果，將運(yùn)動(dòng)學(xué)解析結(jié)果程序與三維模型程序融合并取得良好控制，使用四元數(shù)法對(duì)機(jī)器人姿態(tài)表示，四元數(shù)提供了平滑差值，可以方便下位機(jī)做插補(bǔ)運(yùn)算。對(duì)三維模型的顯示研究中遇到的問(wèn)題，提供了解決方法，三維仿真系統(tǒng)具有優(yōu)秀的跨平臺(tái)特性，可在多平臺(tái)向直接編譯運(yùn)行。在工業(yè)機(jī)器人工業(yè)現(xiàn)場(chǎng)控制中起到良好的示教效果，未來(lái)會(huì)對(duì)示教器的通信方式進(jìn)行優(yōu)化，采用無(wú)線通信增加示教靈活性與安全性。

參考文獻(xiàn)：

［1］張智.六自由度機(jī)械手三維可視化仿真研究［J］.計(jì)算機(jī)仿真，2015（2）：374-377.

［2］馮清秀，阿占文.多平臺(tái)快速成型軟件設(shè)計(jì)［J］.制造技術(shù)與機(jī)床，2014（4）：33-38.

［3］付兵.基于OpenGL的多機(jī)器人仿真實(shí)驗(yàn)平臺(tái)研究及實(shí)現(xiàn)［J］.組合機(jī)床與自動(dòng)化加工技術(shù)，2016（1）：20-23.

［4］李想，錢(qián)歡，付莊，等.基于QT和OpenGL的機(jī)器人離線編程和仿真系統(tǒng)設(shè)計(jì)［J］.機(jī)電一體化，2013（4）：56-59.

［5］Huang H C，Chen C P，Wang PR.Particle Swarm Optimization for Solving the Inverse Kinematicsof7-DOFRoboticmanipulators［C］.Budapest：IEEE International Conference on Systems，Man，and Cybernetics，2012.

［6］梁春艷，李志雄，蘇清茂.六自由度機(jī)器人3D建模仿真研究［J］.西南科技大學(xué)學(xué)報(bào)，2016（2）：81-86.

［7］呂?？?，周小平.實(shí)戰(zhàn)OpenGL三維可視化系統(tǒng)開(kāi)發(fā)與源碼精解［M］.北京：電子工業(yè)出版社，2009.

［8］蔡自興.機(jī)器人學(xué)基礎(chǔ)［M］.北京：機(jī)械工業(yè)出版社，2009.

［9］劉金國(guó)，高宏偉，駱海濤.智能機(jī)器人系統(tǒng)與建模仿真［M］.北京：科學(xué)出版社，2014.

［10］羅家佳，胡國(guó)清.基于MATLAB的機(jī)器人運(yùn)動(dòng)學(xué)仿真研究［J］.廈門(mén)大學(xué)學(xué)報(bào)（自然科學(xué)版），2005（9）：640-644.

［11］李聲弘，周楚涵，董遠(yuǎn).基于OpenGL的3DS圖形文件讀取和顯示［J］.北京測(cè)繪，2015（4）：71-75.

［12］沈雅瓊.基于WinCE的工業(yè)機(jī)器人示教器軟件設(shè)計(jì)［D］.武漢：華中科技大學(xué)，2014.

［13］Richard S，Wright JR.Nicholas Haemel，OenGL SuperBible Fifth Edition［M］.張琪，付飛，譯.北京：人民郵電出版社，2012.

［14］王偉，劉立冬，王剛.基于四元數(shù)表示法的機(jī)器人基坐標(biāo)系標(biāo)定方法［J］.北京航空航天大學(xué)學(xué)報(bào)，2015（3）：411-416.

［15］Jasmin B，Mark S.C++GUIQt4編程（第二版）［M］.閆鋒欣，曾泉人，譯.北京：電子工業(yè)出版社，2013.