基于OpenGL的六自由度机械臂三维仿真工具的设计

1、 文章编号：1671-4598(200905-0982-04中图分类号：TP274.2 文献标识码：A基于OpenGL 的六自由度机械臂三维仿真工具的设计孙 亮 ,马 江 ,阮晓钢（北京工业大学 电子信息与控制工程学院 人工智能与机器人研究所，北京 100124）针对六自由度链式机械臂，为了解决运动学分析结果不易验证以及在实际本体上试验成本较高的问题，开发了一套六自由度机械臂三摘要：维仿真软件；该仿真软件是在VC+6.0开发平台上，基于MFC 框架类和OpenGL 的图形库进行开发的；仿真软件有效地验证了机械臂数学模型以及正、逆运动学求解工程的正确性，并且对4种轨迹规划方式的效果做了直观的比较

2、；结果表明开发出来的仿真软件对机械臂的研究与教学起了相应的作用。机械臂；MFC ；OpenGI ；运动仿真；运动学分析关键词：Three-dimensional Simulation Tool Design Aiming at 6-DOFManipulator Based on OpenGLSun Liang, Ma Jiang, Ruan Xiaogangcial Intelligence and Robotics, College of Electronic Information and Control Engineering, （Institute of ArtiBeijing Uni

3、versity of Technology，Beijing 100124，China ）Abstract: Aiming at 6-DOF manipulator, in order to solve the problem that the result of kinematics analysis is dif cult to verify and the cost of the trial is high on the machine entity, a 3D motion simulation software system was developed based on MFC and

4、 OpenGL graphic library on VC+6.0 IDE . The simulation software system of 6-DOF manipulator shows the validity of the kinematics model, and intuitively compared the four types of trajectory planning methods. The result shows that the software system plays an important role in the research and teachi

5、ng for manipulator.Key words: manipulator；MFC ；OpenGl ；motion simulation；modeling kinematics analysis0 引言机器人的仿真研究已经成为机器人学中一个引人瞩目的领域, 而机械臂三维运动仿真是机器人仿真研究中一个很重要的组成部分。机械臂仿真有各种方式，比如可以用MATLAB ，ADAMS 等这些平台进行运动仿真，都能达到一定的效果。文献1 在MATLAB 平台下开发了ROBOTICS 工具箱，能够通过函数实现简单的运动学仿真。文献2基于ADAMS 软件对机械臂进行运动学仿真。文献3基于OpenGL

6、图形库开发了一套机械臂仿真系统，实现了机械臂的正、逆运动学仿真。文献4利用文献3的方法，且通过定时器，不断刷新视图，达到了动画的效果。文献1和文献2都是基于MATLAB 开发平台的，想在机械臂仿真平台上扩展些新功能不如在VC 平台上方便，而且显示效果也不是很好，文献3 和文献4只是针对正逆运动学设计，并没有阐述具体轨迹规划算法的实现。综合上文文献工作中的优缺点，本文采用在VC+6.0开发平台上，基于MFC 框架类和OpenGL 的函数库来开发仿真软件。三维图形标准。它是一种与硬件、窗口系统和操作系统相独立的一系列API 。当前，Microsoft 公司开发的面向对象的可视化软件开发工具Visu

7、alC+6.0中已经集成OPenGL 图形标准，在加上OpenGL 具有编程建模、容易实现高度清晰感的实时三维仿真等优点, 逐渐被广泛应用于机器臂的设计和运动仿真中。Windows 是用GDI 绘图的，在Windows 下GDI 二维绘图是在设备描述表(Device Contexts简写DC 中进行的。在所有的Windows 图形程序中都需要申请一个DC ，并在DC 中完成各种绘图操作。而OpenGL 是用绘制描述表（Rendering Contexts 简写RC ）绘图的，而且使用特殊的像素格式。Windows 下的窗口和设备描述表支持“位图格式”属性，和RC 有着位图结构上的一致。只要在创

8、建RC 时与一个DC 建立联系，OpenGL 的函数就可以通过RC 对应的DC 画到相应的显示设备上。为了有效地完成基于Windows 的应用程序开发，可以利用微软提供的基础类库MFC ，它是一套面向对象的函数库，以类的方式提供给用户使用。1 Windows对OpenGL 的支持OpenGL 5是在SGI 等多家著名的计算机公司的倡导下, 以SGI 的GL 三维图形库为基础制定的一个通用共享的开放式2 仿真软件的设计62.1 仿真软件框架的设计为了能够实时地观察到六自由度链式（6R ）机械臂的运动过程，把软件分为控制面板和图像显示两部分。上面边用于控制和输出，下面用于显示图像。实现步骤包括：（

9、1）利用MFC AppWizrd建立一个单文档应用程序。（2）用CspliterWnd 类把窗口分成两部分。在资源管理器中插入控制面板对话框，建立一个基于CFormView 类的CFommandView 类，在CMainFrame 类的定义中声明一2009-01-15； 2009-02-26。收稿日期：修回日期：国家自然科学基金项目：国家“863计划”项目(2007AA04Z226；基金项目(60774077；北京市教委重点项目(KZ200810005002；北京市人才强教计划项目教育部博士点基金项目；教育部博士点基金项目。孙 亮(1951 ，男，北京人，副教授，硕导, 主要从事作者简介：学习

10、控制、智能系统方向的研究。中华测控网第 5 期孙 亮，等：基于OpenGL的六自由度机械臂三维仿真工具的设计· 983 ·个CSpltterWnd 类的对象m_wndSplitter，完成CMainFrame 类的OnCreateClient 事件处理程序，从而生成控制面板类CFormCommandView 和视窗类CArmView 。（3）在控制面板类CFormCommandView 里的每个控件编写事件处理函数，包括按钮、静态编辑框和滚动条等，完成与视窗类的数据交换。下面是分窗口的核心代码：m_wndSplitter.CreateStatic(this,2,1,WS_C

11、HILD|WS_VISIBLE/把窗口分成上下两部分m_wndSplitter.CreateView(0, 0,RUNTIME_CLASS(CFormCommandView, CSize(200,140, pContext /创建200×140像素的控制面板m_wndSplitter.CreateView(1, 0,RUNTIME_CLASS(CArmView,CSize(200,200, pContext/创建200×200像素的视窗面板PFD_DOUBLEBUFFER ,PFD_TYPE_RGBA,24,0,0,0,0,0,0,0,0,0,0,0,0,0,32,0,0,

12、PFD_MAIN_PLANE,0,0,0,0;CClientDC clientdc(this;int pf=ChoosePixelFormat(clientdc.m_hDC,&pfd; BOOL rt=SetPixelFormat(clientdc.m_hDC,pf,&pfd; hglrc=wglCreateContext(clientdc.m_hDC; return 0; 下面是机械臂建模的核心代码：void CArmView:OnDraw(CDC* pDC/机械臂建模 CArmDoc* pDoc = GetDocument(;ASSERT_VALID(pDoc;wglMak

13、eCurrent(pDC->m_hDC ,hglrc;/设置一个线程的当前绘图描述表display(;/机械臂的建模、材质、光源位置等wglMakeCurrent(pDC->m_hDC ,NULL;/释放绘图描述表所使用的设备描述表，并忽略设备描述表SwapBuffers(pDC->m_hDC; 交换前后的缓冲区2.2 仿真实体的绘制对于结构简单而控制要求较复杂的机械臂，可直接使用OpenGL 提供的三维建模函数完成绘制。对6R 机械臂各轴的位置、角度、比例的调整，可以使用函数glTranslatef(、 glRotatef(、glScalef(来完成, 为使绘制出来的各轴形

14、象逼真, 可对各轴进行相应的材质、光照设置。旋转关节绘制时将旋转值设为一个变量，改变其值，可以改变机械臂的位姿。为使仿真显示的图形变化连续, 使用OpenGL 提供的缓存交换函数auxSwapBuffers(来实现双缓存绘制。6R 机械臂的建模效果如图1所示。2.3 运动学算法的实现6R 机器臂是具有6个关节的空间机构，为描述末端执行器在空间的位置和姿态，可以在每个关节上建立一个坐标系，利用坐标系之间的关系来描述末端执行器的位置。其结构如图2所示。图1 机械臂建模效果实现步骤包括：(1 在CArmView 类中添加WM_CREATE消息，然后编写OnCreate 事件处理程序来设置像素格式并创

15、建OpenGL 绘制描述表。（2）在类CArmView 的成员函数OnDraw 中，添加6R 机械臂绘制程序。包括机械臂的建模、材质、光源位置、背景色、视角程序等。（3）在CArmView 类中添加键盘WM_KEYDOWN消息，然后编写OnKeyDown 事件处理程序，通过键盘来控制视角、比例等设置。（4）在CArmView 类中添加定时器WM_TIMER消息，然后编写OnTimer 事件处理程序。使用SetTimer （）函数设置定时器的序号和记时周期。一个周期执行OnTimer （）函数一次，各关节变量的变化实时反馈到视窗中，视窗通过函数Invalidate(FALSE不断重绘，为了避免显

16、示动画时闪动，采用双缓存技术，通过SwapBuffers （）函数来实现。下面是设置像素格式的核心代码：Int CArmView:OnCreate(LPCREATESTRUCT lpCreateStruct PIXELFORMATDESCRIPTOR pfd= sizeof(PIXELFORMATDESCRIPTOR,1, PFD_DRAW_TO_WINDOW|PFD_SUPPORT_OPENGL|图2 机械臂坐标系运动学分析包括正运动学、逆运动学以及运动规划三大部分。（1）正运动学的求解过程是根据已知关节变量1，2，3，4，5，6求末端抓持器相对于参考坐标系的位姿的过程。将参考坐标系设在6R

17、 机械臂的基座上，从基座开始变换到第一关节，然后到第二关节, 最后到末端抓持器。那么6R 机器臂的基座和末端抓持器之间的总变换为7个齐次变换矩阵相乘，软件中通过成员函数CArmView:GetArmPos(来实现。下面是正运动学的核心代码：void CArmView:GetArmPos( MatrixMultiply(A0,A1,AidedMatrix1;MatrixMultiply(AidedMatrix1,A2,AidedMatrix2;MatrixMultiply(AidedMatrix2,A3,AidedMatrix3;MatrixMultiply(AidedMatrix3,A4,Ai

18、dedMatrix4;MatrixMultiply(AidedMatrix4,A5,AidedMatrix5;MatrixMultiply(AidedMatrix5,A6,T;/齐次矩阵相乘CFormCommandView *pView1=(CFormCommandView *GetCFormCommandView(;/获得指向类CFormCommandView 的指针中华测控网· 984 ·char ch120,ch220,ch330;sprintf(ch1,"%f",T03;计算机测量与控制第 17 卷p_theta5_t0,temp_theta6_

19、t0,c1_0,c1_1,c1_2,c1_3,c2_0,c2_1,c2_2,c2_3,c3_0,c3_1,c3_2,c3_3,c4_0,c4_1,c4_2,c4_3,c5_0,c5_1,c5_2,c5_3,c6_0,c6_1,c6_2,c6_3;extern float temp_theta1_tf,temp_theta2_tf,temp_theta3_tf,temp_theta4_tf,temp_theta5_tf,temp_theta6_tf;int t0=0,tf=150;/ /定义插补次数时间if (t<=150 temp_theta1_t0=theta1;temp_theta2

20、_t0=theta2;temp_theta3_t0=theta3;temp_theta4_t0=theta4;temp_theta5_t0=theta5;temp_theta6_t0=theta6;c1_0=temp_theta1_t0; c1_1=0;c1_2=3*(temp_theta1_tf-temp_theta1_t0/(tf*tf;c1_3=-2*(temp_theta1_tf-temp_theta1_t0/(tf*tf*tf;theta1=c1_0+c1_2*t*t+c1_3*t*t*t; /计算theta1同理计算theta2、theta3、theta4、theta15、thet

21、a6在这略去。pView1->m_theta1=theta1;pView1->m_theta2=theta2;pView1->m_theta3=theta3;pView1->m_theta4=theta4;pView1->m_theta5=theta5;pView1->m_theta6=theta6;pView1->m_ScrollBar1.SetScrollPos(theta1;pView1->m_ScrollBar2.SetScrollPos(theta2;pView1->m_ScrollBar3.SetScrollPos(theta3

22、;pView1->m_ScrollBar4.SetScrollPos(theta4;pView1->m_ScrollBar5.SetScrollPos(theta5;pView1->m_ScrollBar6.SetScrollPos(theta6;pView1->UpdateData(FALSE;Invalidate(FALSE;t+;else KillTimer(1;t=0; pView1->SetDlgItemText(IDC_EDIT7,ch1;/显示抓持器末端的X 坐标sprintf(ch2,"%f",T13;pView1->Se

23、tDlgItemText(IDC_EDIT8,ch2;/显示抓持器末端的Y 坐标sprintf(ch3,"%f",T23;pView1->SetDlgItemText(IDC_EDIT9,ch3;/显示抓持器末端的Z 坐标（2）逆运动学的求解过程是根据已知的末端抓持器相对于参考坐标系的位姿，求关节变量1，2，3，4，5，6的过程，在相同的位姿下可能存在23种关节转角组合，这里选择功率最省的标准来确定唯一组合。软件中通过“go ”按钮的事件处理函数CFormCommandView:OnButton2(来实现算法。下面是逆运动学的核心代码：void CFormComman

24、dView:OnButton2( extern float l1,l2,l3,l4,l5,temp_theta1_tf,temp_theta2_tf,temp_theta3_tf,temp_theta4_tf,temp_theta5_t f,temp_theta6_tf;CArmView *pView=(CArmView *GetArmView(;UpdateData(;Float px,py,pz,nx,ny,nz,ox,oy,oz,ax,ay,az,c3,s3,theta234,temp_1,temp_2,temp_3,temp_4,temp_5,temp_6,temp_7,temp_8;

25、然后是求逆解算法，由于篇幅限制在这省略。（3）运动规划包括路径规划和轨迹规划。路径与时间无关，表示机械臂空间位姿的连续序列。轨迹是机械臂位姿关于时间的函数。路径规划目标是在两个作业点之间生成一条无碰撞路径。轨迹规划是给出一条无冲突路径，求出沿这条路径(使指定的性能指标达到最小 机械臂位置和姿势的时间经历。轨迹规划既可以在关节空间也可以在直角空间中进行。在CArmView 类中添加4个定时器WM_TIMER消息，然后编写各个OnTimer 事件处理程序，程序中包括在关节空间用三次多项式和五次多项式时间函数规划轨迹算法，以及在直角坐标空间用直线插补和圆弧插补路径点算法。可以通过选择控制面板中的复选

26、框，来实现不同的轨迹规划方法。图3表示在直角坐标空间中用直线插补，在关节空间用五次多项式时间函数来规划轨迹。图3 规划轨迹由于篇幅限制只给出三次多项式时间函数算法的核心代码：void CArmView:OnTimer(UINT nIDEvent nIDEvent=1;/选择定时器1CFormCommandView *pView1=(CFormCommandView*GetCFormCommandView(;/UpdateData(;Float temp_theta1_t0,temp_theta2_t0,temp_theta3_t0,temp_theta4_t0,tem图4 6R机械臂运动仿真软

27、件中华测控网 第 5 期孙 亮，等：基于OpenGL的六自由度机械臂三维仿真工具的设计· 985 ·3 仿真软件的功能六自由度机械臂三维仿真界面如图4所示。上面为控制面板部分，下面为图像显示部分。控制面板从左到右分别为正运动学仿真、逆运动学仿真、抓小球仿真模块。3.1 正运动学仿真模块如图5所示，当在关节角对应的静态编辑框中输入角度值或者单击滚动条时，末端抓持器的位姿所对应的静态编辑框里面的数值随之变化，三维6R 机械臂视图也随之运动。末端抓持器的姿态是用通过RPY （滚动角、俯仰角和偏航角）旋转 给出的。插补的方式，在每个路径点采用五次多项式插值时间函数抓取 小球。图7

28、小球位置坐标4 结论本文首先对机械臂仿真的几种方法进行了比较，对比之后阐述了OpenGL 在三维仿真中的优势，以及Windows 对OpenGL 的支持。然后从软件框架的设计、仿真实体的绘制以及运动学算法的实现3个部分较为详细地介绍了6R 机械臂三维运动仿真软件的设计。最后从正运动学仿真、逆运动学仿真以及抓小球仿真3个模块介绍了软件的实现。仿真软件有效地验证了机械臂数学模型以及正、逆运动学解分析的正确性，并且对4种轨迹规划方式的效果做了直观的比较。结果表明开发出来的仿真软件对机械臂研究与教学起了很大的作用。参考文献:1 Corke P I. A robotics toolbox for MAT

29、LAB. J.IEEE Robotics and Automation Magazine,1996,3(1:24-32.2 曹春芳，孔庆忠. 基于ADAMS 的五自由度机械手运动学：71-73. 仿真J.机械，2007，12（34）3 Kim S H, Kim Y H.Realization of a Virtual Simulator System on Window 98/NT J.Industrial Electronic,2001,1:216-220.4 严勇杰，朱齐丹，蒋 丞. 基于OpenGL 的机械臂控制系统仿真平台研究J.计算机仿真，2006，08（23）：252-257.5 邓启文，韦 庆，杨乐平. 机械臂绝对定位精度测量J.计算机测量与控制, 2007,15.(3:305-307.6 乔 林，费广正. OpenGL程序设计M.北京：清华大学出版社，2000. 7 Niku S B.机器人学导论分析、系统及应用M. 孙福春, 朱纪洪, 刘国栋, 等，译