大学生科创竞赛获奖作品：机械机电控制上位机软件设计

1.算法仿真环境

算法仿真环境主要需要实现以下功能：

1）前期运控算法编写。

2）运控算法结果校验。

3）可视化仿真。

4）实验评估。

针对以上功能需要，结合机械臂运动学研究中对矢量、矩阵运算的大量需求，决定上位机系统以MATLAB为核心进行开发，使用功能强大的MATLAB机器人学工具箱（Robotics Toolbox for MATLAB）辅助完成工作。MATLAB机器人学工具箱是由澳大利亚Pinjarra Hills联邦科学与工业研究组织的Peter I.Corke编写的开源工具箱。该工具箱提供了大量在机器人学研究中起到重要作用的函数，如运动学、动力学和轨迹规划等方面的函数。该工具箱可以较好地满足研究人员对机器人实体的仿真与实验结果分析的需求。

该工具箱从方法上对串联结构机械臂的运动学与动力学研究进行了一般性的定义。通过刚体杆件的DH参数模型建立，即可完成从运动学、动力学计算到模型可视化的全部机器人研究过程。该工具箱内还包含了大量典型的工业机器人模型实例，如Puma560、Stanford Arm、Fanuc10L等。除此之外，该工具箱还提供了向量、齐次变换阵与单位四元数等用于描述机械臂在三维空间中位姿的操作与变换函数，这为机器人学中运动学与动力学中的具体运算提供了数学工具层面的便利性。

同时利用MATLAB交互性好和易于处理数据生成图表的特点，通过MATLAB在上位机中建立了仿真环境，具体包括可视化界面和评估系统。

可视化界面通过调用Robotics Toolbox中重定向的plot函数实现，以杆件模型的形式可视化地表示出机械臂的关节和杆件信息。具体效果如图6-25所示。该函数既可以用于静态显示某一特定正运动学位形，也可以通过矩阵的形式输入二维的关节空间控制矢量q，以动画形式反映机械臂的运动过程。

图6-25 可视化界面实际效果

评估系统通过将各种控制参数信息以图形的形式直观显示，反映控制效果，图6-26所示为关节角位置曲线评估系统效果。图6-27所示为校徽刀路实际运行仿真。

图6-26 关节角位置曲线评估系统效果

图6-27 校徽刀路实际运行仿真

2.交互式仿真实验系统及评估系统

科学可视化、计算机仿真和虚拟现实是近年来计算机仿真领域的三大热门技术，而这三大热门技术的核心均是三维真实感图形的显示与交互。其中机器人三维运动仿真技术在机器人的研究与应用中发挥着重要的作用。机械臂三维运动仿真是机器人仿真研究中一个很重要的组成部分。对于本项目研究的新型焊接工业机械臂而言，利用三维运动仿真技术对机械臂的运控算法进行检验，甚至对机械臂的加工轨迹进行规划和对加工过程进行干涉检验，可以把对机械臂的示教过程转移到虚拟测试平台中，从而科学有效地模拟机器人的动态特性，验证机器人的工作原理和工作空间，还可以避免因现实中的错误操作而给机械臂带来无法挽回的损失。因此，充分利用机器人三维运动仿真技术在是实用化离线编程技术的重要部分。

机械臂仿真有各种方式，比如可以用MATLAB、ADAMS等这些平台进行运动仿真，都能达到一定的效果。如今在机器人运动学方面，尤其是对多关节结构的机械臂的运动学的研究已经比较深入成熟，已经有从事相关研究的研究人员开发了在MATLAB平台下，能够通过函数实现简单的运动学仿真的机器人运动学工具箱。如果基于MATLAB开发平台的，想在机械臂仿真平台上扩展些新功能不如在VC平台上方便，而且显示效果并不是很好。目前，OpenGL是较实用的开发机器人图形仿真系统工具。OpenGL（即开放性图形库Open Graphics Library）是一种与硬件、窗口系统和操作系统相独立的一系列应用程序接口（API）运用OpenGL开发人员可方便绘制出客观世界逼真的三维景象。OpenGL已广泛应用于可视化技术、实体造型、CAD/CAM、模拟仿真等诸多领域。综合考虑，本项目采用在VC++6.0开发平台上，基于MFC框架类和OpenGL的函数库来开发仿真软件。

（1）仿真方案的确定 进行机器人仿真的三维实体建模工作方案一般有：①使用VRML和Java3D在一般的微机上构造轻量级的仿真平台，可应用于网络功能要求较高的机器人运动仿真。VRML和Java3D的跨平台性、网络化和强大的可编程能力，对于实现网络化机器人仿真不失为一种简单、廉价而有效的手段。②使用虚拟样机技术。通过在Pro/E或其他三维环境下建立的机器人三维模型和在ADAMS环境下建立的力学模型对机器人进行仿真研究。主要应用于检验机器人各部件的设计性能及部件之间的兼容性，并检查整机的综合设计性能，实现高质量、快速、低成本的设计。③在Windows环境下，配合某些三维建模工具如AutodeskInventor或3DMax等，使用VisualC++工具调用OpenGL图形库中的函数，实现三维运动仿真。目前，Microsoft、SGI、IBM等大公司都采用了OpenGL作为三维图形标准。特别是随着PC性能的不断提升和微软的加入，使得在微机上实现三维真实感图形的显示与交互成为可能，也为广大用户提供了在微机上使用以前只能在高性能图形工作站上运行各种软件的机会。另外，由于系统中涉及较多的机械臂正、逆运动学方程求解问题，因而采用VC作为编程语言，一方面可以方便地调用OpenGL图形库函数；另一方面有利于算法的实现。

（2）仿真实体的绘制在本系统中，三维实体的绘制采用了以下方法来实现：

1）对于结构比较复杂而控制要求简单的工作对象或者其他附件，使用OpenGL直接绘制是一件十分烦琐的工作。而3DMax是一个相当好的流行建模工具，通过对简单几何形体进行并、交、切等布尔运算和曲面编辑等功能就能构造出复杂的几何形体。在完成复杂的建模后，输出3DS格式文件，再通过一些相关工具软件（如VIEW3DS）可以生成^*.gl与^*.h格式的数据文件，直接导入到VC工程中，稍加修改就可完成复杂模型的绘制工作。图6-28所示为单件模型仿真效果。

图6-28 单件模型仿真效果

2）对于结构简单而控制要求较复杂的机械臂各轴，可直接使用OpenGL提供的三维建模函数完成绘制。在此过程中，对各轴的缩放、位置、角度的调整主要使用函数glScalef（）、glTranslatef（）和glRotatef（）来完成，并且在程序中采用了嵌套结构，使得机械臂的每个部件都是在前一个部件的相对坐标下完成绘制，这样可以保证组成机械臂的某一部件在空间姿态有所改变时，在其后顺序联接的每个部分相对该部件的姿态不变，而相对于空间坐标系发生与该部件同样的姿态改变。这就保证了机械臂部件在OpenGL中保持相对独立性的同时依然在部件与部件之间保持相互联系。如以下例程所示：

为使绘制出来的各轴形象逼真，可对各轴进行相应的材质、光照设置；在轴之间进行装配时，需要使用矩阵堆栈来保证预留对各轴控制接口的正确性，其函数原型是glPushMatrix（）和glPopMatrix（），另外，这里所介绍的机械臂属于双端协调工作方式，所以装配还受到工作模式的动态控制;为使仿真显示的图形变化连续，一般使用OpenGL提供的缓存交换函数auxSwapBuffers（）来实现双缓存绘制。

VC6.0人机交互界面与MATLAB运控程序通信问题：

首先，人机界面与MATLAB运控程序之间的通信方式采用串口通信方式。借助MATLAB强大的矩阵运算功能，通过CAM软件把某一图形的轮廓生成为NC刀路图，结合定距插补算法生成一段G代码运控程序，以G代码中机械臂末端执行器轨迹点坐标为逆运动学已知数据，在MATLAB环境下进行逆运动学求解运算。求解所得机械臂各关节旋转角度以二进制方式通过串口（目前调试使用虚拟串口）从MATLAB求解器发送到人机界面。OpenGL在接收到串口发来的指令时，触发OnThetaTransmit（）函数，对指令进行译码，从指令中提取关节旋转角值并将它们相应地赋予OpenGL中控制每个关节旋转角的6个变量（theta1，theta2，theta3，theta4，theta5，theta6）。每接受一条指令就翻译一条指令，并进行相应的关节旋转角值的传递，这样就能实现MATLAB求解与OpenGL环境中机械臂运动的同步。串口发来的指令以2个各占一个字节的FF开头，接下来的12个字节用来描述机械臂6个关节的关节旋转角度，总共14个字节。除了指令头，余下的每2个字节表示一个关节的旋转角，分别以高8位（前一个字节）与低8位（后一个字节）表示。

一般来说，译码的过程要求高效可靠。高效性主要由程序的运算速度决定。而对于任何一种语言的编程来说，移位与逻辑运算速度远高于乘除法运算，所以译码的高效性在本程序中主要由移位逻辑运算保证。而可靠性主要由准确捕捉指令头“FFFF”决定。

译码代码如以下所示：

交互式仿真系统实际仿真效果如图6-29所示。

图6-29 交互式仿真系统实际仿真效果

3.接口软件设计

在执行具体的指令时，指令层级接口如图6-30所示。

接口软件设计部分主要包含：NC代码译码接口、笛卡尔空间指令接口、轴驱动译码接口，以及用于底层伺服控制的RS485数据接口。

图6-30 指令层级接口

（1）NC代码译码接口 作为与现有工业标准兼容的数控加工设备，“新型机械臂”首先需要兼容现有的NC代码控制指令。NC代码就是数字信息控制机械控制器能识别的代码，例如数控切割设备上就有Ｇ代码、ESSI码、EIA码等。本控制系统主要使用G代码，G代码是一种在计算机数控机床（CNC）中应用最为广泛的自动编程语言，是计算机辅助工程（CAE）的重要组成部分。作为一种ISO标准广泛应用于各种数控机床中的G代码，它将图纸中工件的点、线、面的信息提取为加工工具的运动路径。这就大大简化了将其转换为机器人加工指令的难度，并且几乎所有的大型CAD/CAM制图软件，如UG、Pro/E、MasterCAM及AutoCAD都可以直接生成G代码文件。本项目控制系统软件指令流如图6-31所示。

图6-31 控制系统软件指令流图

对于执行NC代码来说，最关键的软件接口为NC代码的译码接口。NC代码的保存形式为标准ASCII码文本形式。所以常用字符栈扫描方式进行译码，译码接口的算法流程图如图6-32所示。(www.xing528.com)

通过本算法，以ASCII码形式存储的NC代码，经过译码保存在二维数组NC_data中，NC_data的奇数行用于以ASCII码的形式存储具体的NC代码指令类型，偶数行双精度浮点数的形式存储上一行对应位置的数据值。例如表6-2中所示的G代码与其输出结果。

表6-2 G代码与其输出结果

该接口在上位机与嵌入式系统均有涉及，所以使用C语言编写，在MATLAB中使用m脚本内嵌C的方式实现，具体为先将C语言源程序添加MATLAB工作空间接口后编译为mexw32动态链接库，然后在m脚本中直接调用动态库实现算法功能。通过这种方式，在保证各部分功能均可实现的情况下，提高了算法的可移植性。

图6-32 NC代码译码接口的算法流程图

（2）上下位机角位置指令接口 上下位机角位置指令接口的功能为将上位机的角位置指令与图6-30中所示的轴驱动译码接口进行匹配的软件接口。上位机算法仿真系统中的角位置指令，通过本软件接口，将关节空间矢量对应的各轴角位置值发送给嵌入式控制系统的轴驱动模块。接口的链路层协议使用USART串行协议，物理层使用RS-232串行通信接口。因此在指令帧设计上参考了Robotics Dynamixel数据协议。

上下位机通信角位置指令帧格式见表6-3。

表6-3 上下位机通信角位置指令帧格式

其中，帧头与校验和的定义与Robotics Dynamixel数据协议相同，不再赘述。有效的数据字节包括针对“新型机械臂”7个自由度的控制指令。因为RX24F舵机的角位置反馈为10位A/D精度采样的精密电位器，每个角位置指令长为两字节，在PC端发送前，将角位置经过量纲变换取整后拆分为高、低字节。以θ₁为例的MATLAB代码实现：

嵌入式系统接收后将高、低字节个合并为一个16位整形变量（unsigned int类型）。最终指令队列的有效长度为14字节。

（3）RS485数据接口 “新型机械臂”设计使用的舵机RX-24F使用Robotics Dynamixel专有格式的指令帧协议。

1）RS485指令帧格式见表6-4。

表6-4 RS485指令帧格式

其中各字节表示含义如下：

①帧头：连续收到两个0×FF，表示有新数据包到达。

②ID：每个舵机都有一个ID号。ID号编码范围0～253，转换为十六进制0×00～0×FD。ID号254为广播ID，若控制器发出的ID号为254（0×FE），所有的舵机均会接收到此指令指令，但都不返回应答信息。

③数据长度：等于待发送的参数长度N加上2，即“N+2”。

④参数：除指令外需要补充的控制信息。

⑤指令：共有7种有效控制指令，分别为：

a.查询（0x01）：查询工作状态。

b.读（0x02）：查询控制表里的数据。

c.写（0x03）：向控制表内写入数据。

d.异步写（0x04）：类似于“写”指令，但是控制数据写入后并不立即执行，而是直到“执行”指令到达。

e.执行（0x05）：与“异步写”配合使用，触发“异步写”写入的动作。

f.复位（0x06）：把控制表复位为出厂值默认。

g.同写（0x83）：对多个舵机的相同寄存器进行连续写入。

⑥校验和：校验和Check Sum，计算方法如下。

Check Sum=～(ID+N+2+I+参数1+…参数N）

数据类型为unsigned char，不考虑溢，“～”表示C语言按位取反。

2）应答帧的帧格式与指令帧基本相同，具体格式见表6-5。

表6-5 RS485应答帧格式

由于应答帧的帧格式其他参数与指令帧相同，在此不做赘述，与指令帧“指令类型”字节位置对应的字节，在应答帧中表示当前工作状态，该字节各个数据位定义如下：

①BIT7：保留。

②BIT6：指令错误，如果收到一个未定义的指令或收到“执行”前未收到“异步写”指令置1。

③BIT5：过载，位置模式运行时输出扭矩小于负载置1。

④BIT4：校验和错校验和错误置1。

⑤BIT3：指令超范围，指令超过指定范围置1。

⑥BIT2：过热，温度超过指定范围置1。

⑦BIT1：角度超范围，角度超过设定范围置1。

⑧BIT0：过压欠压，电压超过指定范围置1。