工业机器人三维示教模型的设计与实现

杨胜安，汪明，*，赵永国

（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.