工業機器人三維示教模型的設計與實現

楊勝安，汪明，*，趙永國

（1.山東建筑大學 信息與電氣工程學院，山東 濟南250101；2.山東省科學院自動化研究所 山東省機器人與制造自動化技術重點實驗室，山東濟南250014）

0 引言

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

文章面向6關節工業機器人，為尋求更為逼真的示教效果和更為優秀的仿真系統通用性與跨平臺特性，采用SolidWorks繪制三維模型，使用3DS渲染，基于Qt和OpenGL相結合對三維模型仿真顯示系統進行開發研究；采用D-H（Denavit-Hartenberg）參數法找出相鄰桿件的參數，根據參數對機器人進行正、逆運動學解析，并通過Matlab程序對其運動進行仿真驗證。最終將運動學解析程序與三維模型程序融合，使用四元數法表示其姿態，工業機器人的三維模型在Qt中以逼真的視覺效果顯示。

1 工業機器人三維示教模型運動學設計

仿真模型采用工業機器人領域典型的6關節機器人，機器人本體外形模仿人的手臂共有6個旋轉關節，包括基座（關節J1）、肩部（關節J2）、肘部（關節 J3）、腕部1（關節 J4）、腕部 2（關節 J5）、腕部 3（關節J6）。每一個關節代表一個自由度，其中3個自由度控制位置，另3個自由度控制姿勢。

1.1 模型參數確定

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

圖1 6關節機器人三維模型圖

圖2 6關節機器人連桿坐標系圖

表1 6關節機器人的連桿參數表

1.2 工業機器人運動學模型分析

（1）工業機器人正運動學模型

機器人正運動學解析方法采用D-H參數法，根據表1給定的6關節機器人的模型連桿參數，結合各關節的相對旋轉、平移變化得到關節i相對于關節i-1的變換矩陣i-1Ti。已知各關節角度θi，求解末端執行器變化矩陣0T6，描述了末端連桿坐標系0x6y6z6相對于基坐標系0x0y0z0的位姿。末端位姿矩陣T包含法向矢量n、姿態矢量o、接近矢量ɑ和位置矢量p［9］。末端姿態矩陣由式（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）工業機器人逆運動學模型

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

圖3 6關節機器人姿態圖

式中：

根據θi的角度公式和θi的變化范圍基本可排除多解情況。若出現多值情況無法根據角度范圍排除時，則方差最小的一組為最優解。

1.3 Matlab運動學模型仿真

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

圖4 機器人末端軌跡圖

2 工業機器人三維示教實現

2.1 三維仿真模型繪制

通常繪制復雜三維模型的一般方式為使用專門的建模軟件（如3dsMax、Maya）創建逼真度較高的模型，以特定格式導出模型，將模型導入三維場景中實現繪制與控制，營造出逼真的三維場景。采用模型導入的方式進行三維模型繪制，由于SolidWorks在三維圖形繪制上具有組件多、易操作、功能強大等優點，所以基本三維圖形采用SolidWorks軟件進行繪制；三維模型以STL格式文件導出，再將其導入3DSMax中進行紋理坐標確立；然后輸出3DS格式文件，得到比較逼真的效果，再將3DS文件導入Deep Exploration進行三維模型材質和紋理的二次處理后導出.cpp文件；通過Qt導入以OpenGL為標準的工作場景中渲染，最后得到畫面逼真的三維模型虛擬仿真平臺［11］。三維仿真模型設計流程如圖5所示。

圖5 三維仿真模型設計流程圖

2.2 三維仿真示教界面實現

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

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

2.3 運動學與三維模型融合界面實現

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

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

工業機器人的三維仿真模型顯示以獨立窗口顯示，工程中需要將該顯示窗口融入到示教器的機器人示教界面中，通過機器人示教界面的按鈕對三維機器人模型進行控制。將 OpenGL顯示窗口QOpenGLWidget進行提升，QOpenGLWidget在Qt的OpenGL界面內部使用，創建QGLWidget類設置繼承自QOpenGLWidget類，在Qt的顯示界面中放置OpenGLWidget控件并且將控件提升即可將三維顯示窗口融入到示教界面中，提升的類名為QGLWidget。機器人運動學模型與三維模型融合之后三維仿真示教界面如圖7所示。

圖7 三維仿真示教界面圖

工業機器人三維仿真模型的每個關節分別有每組兩個PushButton表示，其中+表示關節度數增加，-表示關節度數遞減，仿真視圖中X+、X-、Y+、Y-、Z+、Z-分別表示機器人在用戶基坐標系中TCP點的運動方向。三維示教仿真界面中MousePressEvent接口獲取鼠標按鍵信息、MouseMoveEvent接口獲取鼠標前后移動信息、WheelEvent鼠標滾輪信息來控制三維仿真圖形的平移、旋轉、縮放，設計為了方便與某些特殊工業平板電腦的操作增加觸屏功能［15］。

3 結語

使用D-H參數法對6關節工業機器人進行運動學正、逆分析，并利用Matlab的Robotic Toolbox對模型進行了驗證，采用SolidWorks繪制三維模型，在3DS中渲染出逼真的三維效果，三維仿真系統操作界面基于Qt與OpenGL開發具備良好視覺效果，將運動學解析結果程序與三維模型程序融合并取得良好控制，使用四元數法對機器人姿態表示，四元數提供了平滑差值，可以方便下位機做插補運算。對三維模型的顯示研究中遇到的問題，提供了解決方法，三維仿真系統具有優秀的跨平臺特性，可在多平臺向直接編譯運行。在工業機器人工業現場控制中起到良好的示教效果，未來會對示教器的通信方式進行優化，采用無線通信增加示教靈活性與安全性。

參考文獻：

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

［2］馮清秀，阿占文.多平臺快速成型軟件設計［J］.制造技術與機床，2014（4）：33-38.

［3］付兵.基于OpenGL的多機器人仿真實驗平臺研究及實現［J］.組合機床與自動化加工技術，2016（1）：20-23.

［4］李想，錢歡，付莊，等.基于QT和OpenGL的機器人離線編程和仿真系統設計［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］梁春艷，李志雄，蘇清茂.六自由度機器人3D建模仿真研究［J］.西南科技大學學報，2016（2）：81-86.

［7］呂希奎，周小平.實戰OpenGL三維可視化系統開發與源碼精解［M］.北京：電子工業出版社，2009.

［8］蔡自興.機器人學基礎［M］.北京：機械工業出版社，2009.

［9］劉金國，高宏偉，駱海濤.智能機器人系統與建模仿真［M］.北京：科學出版社，2014.

［10］羅家佳，胡國清.基于MATLAB的機器人運動學仿真研究［J］.廈門大學學報（自然科學版），2005（9）：640-644.

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

［12］沈雅瓊.基于WinCE的工業機器人示教器軟件設計［D］.武漢：華中科技大學，2014.

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

［14］王偉，劉立冬，王剛.基于四元數表示法的機器人基坐標系標定方法［J］.北京航空航天大學學報，2015（3）：411-416.

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