【《某特种机械臂控制系统的软件设计及调试案例》8400字】

某特种机械臂控制系统的软件设计及调试案例目录TOC\o"1-3"\h\u7626某特种机械臂控制系统的软件设计及调试案例 1327661运动控制 2140681.1单电机机构控制 2213921.2双电机机构控制 4273141.3本章小结 525842控制系统软件设计 5149652.1软件设计概述 517562.2软件平台 6116772.2.1TwinCAT平台 6266682.2.2Labwindows/CVI 7296912.3EtherCAT总线通讯 816102.3.1EtherCAT通讯原理 8137232.3.2寻址方式 1078362.3.3同步方式 12168122.3.4应用层数据交换 13136812.4ADS通讯 1476062.5下位机软件设计 19191622.2.1软件的配置 19162632.2.2运动模块设计 2090652.6上位机软件设计 23203442.7小结 25112233实验测试 26322573.1测试准备 26304293.2单轴控制测试 2637583.3多轴控制测试 311运动控制1.1单电机机构控制图1.1电机控制方法给定特种机械臂的执行机构是伺服电机，在控制其运动时，伺服系统将控制器的指令脉冲放大，并转换为机械运动，使机械臂平稳运行。伺服电机的控制由三闭环负反馈控制系统来完成，主要通过转速环、电流环来调节。其工作原理是给定目标位置指令，由位置环控制器将其转换为对应的速度信号，该信号再作为转速环的给定量，速度调节器通过给定速度和实际速度输出电流信号，电流调节器速度环给出的电流和目标电流转换为输出电流，达到驱动电机的目的。各个控制环通过反馈校正，完成给定量的跟随，其结构流程图如图1.1所示。图1.1电机控制方法由上图可知，整个控制系统分为三个控制环，由内到外分别为电流环、速度环、位置环。三环结构中，电流环可以对控制对象的传递函数进行改造，提高系统的响应速度，并抑制电流环内部的干扰，保证系统稳定运行；速度环的作用是增强系统抗负载扰动能力，抑制速度波动；位置环的作用是保证系统静态精度和动态跟踪性能，使整个伺服系统能稳定、高精度运行，三环控制结构可以使伺服系统获得较好的动态跟随性能和抗干扰性能[13]。这三个控制环，都采用PID控制方法来进行控制。PID控制的控制原理如图1.2所示。PID控制的表达式如公式（1.1）v(t)=Kp[e(t)+1KI式中，e(t)为偏差信号，Kp为比例系数，KI为积分时间常数，图1.2PID控制原理图1.2PID控制原理使系统的响应速度变慢，升值可能造成系统震荡；微分环节会提前校正信号，使系统的响应速度加快，但会减弱系统的抗干扰能力，从而造成系统震荡。由此可见，这三个环节各有优缺点给，但又会在一定程度上形成互补，所以在应用此方法时，要根据执行机构的特征，选择合适的控制方法。本文中电流环和速度环通过比例环节和积分环节进行调节，位置环采取比例、积分和微分调节控制。1.2双电机机构控制偏航机构设计的是两个电动缸，通过一伸一缩，完成偏航机构的偏转，由于实际工程中机械结构无法设计到同理论一样，还是存在极小的误差，而伺服电机在运动过程中，对运行的位置要求非常精确，当两个电动缸运动达不到一致时，两个电动缸就会产生相互作用，由此导致两个电动缸力矩都增大，累积到一定程度，极其容易使伺服系统报错，导致系统停止运行，此时需要用到运动控制中的插补方式，将实际轴运行的两个位置点做成一个运动关系曲线，即当某个侧滑角为确定值时，两个电动缸对应的位移量也都确定，通过理论计算和实际测量，完成整个曲线的绘制，并导入到控制器内。1.3本章小结本章主要分析了系统的运动控制环节。单电机机构的运动控制环节由外到内分为位置环、速度环、电流环这三个环节，而这三个环节又都由PID控制方法来实现控制双电机机构运动需用到控制中的插补方式，这样就能控制整个机器人运动。5控制系统软件设计2.1软件设计概述系统的软件设计是系统的重要组成部分，在此系统中，上位机运行在工控机上，上位机和控制器通过ADS协议进行连接控制，控制器将信号传递给其内部的运动控制部分，运动控制部分再对应TwinCATNC部分，可将控制信号进行解算，换算成NC指令，此指令通过总线传递给驱动器，驱动器将NC指令转换为相应的控制信号实现对各关节电机的控制。文中采用PC+嵌入式的控制方案，需要在PC端编写上位机程序，上位机程序采用Labwindows/CVI编写。下位机程序编写基于TwinCAT软件平台，包括运动模块，I/O控制模块，关节轴状态读取等。其中，运动模块又包括单轴运动、末端位姿控制、停止运动等部分。控制器是倍福控制器，其中Y向控制器程序已经编写好。软件系统总体框架如图2.1所示。图图5.1系统软件功能模块设计图2.2软件平台2.2.1TwinCAT平台控制器编程的平台是基于德国倍福的TwinCAT平台，TwinCAT软件开发平台基于国际标准IEC61131-3标准。该平台具有多种优势，首先是平台支持多种PLC编程语言，如结构化文本等，可以完美与众多厂家产品兼容。其次就是实时性强，TwinCAT的实时性是通过它的实时核来保证的，实时核将每个任务，从输入到处理，输出保证在某个确定的时间片内，最小的时间片可达到50μs，当控制器连接到工控机时，会调用工控机的CPU资源，将工控机变成一台具有多PLC和NC的实时操作系统，利用任务和优先级，TwinCAT优先级较高，所以可以分时运行多个PLC程序，保证多PLC的实时性，完成实时的逻辑运算和运动控制。并且相对于传统PLC，TwinCAT的浮点运算更佳快捷，NC轴除了拥有和传统运动控制卡一样的功能（比如单轴点位运动，多轴联动，插补等）外，其控制的轴的数量更多，最多达255轴[16]。完全满足了项目的需要。而且该平台所有的功能都可以通过ADS通讯来实现数据交换。最后平台可在项目中扩展使用C++以及MATLAB图5.2TwinCAT软件平台图等软件语言，使其用途更广泛。平台组成如图2.2所示。图5.2TwinCAT软件平台图2.2.2Labwindows/CVILabWindows/CVI是NI公司较早发布的一款软件。它的开发环境是ANSIC，对于有C语言基础的很容易就能上手。该软件内可以直接对程序进行编辑、编译、链接，调试等。软件通过事件驱动与回调函数方式，完成面板的某些功能。通过面板上的控件，可设计处美观的界面，并可实现一定的操作和功能。由于其丰富的控件以及易操作性，常被用于仪器控制，数据采集处理和显示等测控领域。2.3EtherCAT总线通讯文中控制器通过EtherCAT协议与驱动器进行通讯，EtherCAT是一种高速传输，效率高，开放性、实时性强的以太网协议。而且EtherCAT基于ISO/OSI模型，不过是将原有的七层转化为三层。观察下表2.1，可以发现，它们的物理层都是相互兼容的，因此设备可以直接通过标准的以太网卡交换信息。表2.1通讯协议模型对比ISO/OSI模型TCP/IP参考模型EtherCAT应用层表示层应用层应用层会话层传输层传输层网络层网络层数据链路层数据链路层数据链路层物理层物理层物理层2.3.1EtherCAT通讯原理为避免通信混乱，EtherCAT通信只能由主站发起，从站不能发起通信。发起通信后，主站控制器创建以太网帧，以太网帧内含有EtherCAT帧，EtherCAT帧内的子报文内包含了对应从站的地址和数据，该EtherCAT帧会根据连接顺序依次传送到各从站，个从站的处理器会对接收到的帧进行识别，判断子报文的地址是否属于该从站，若属于则读入所需数据，将要输出的数据放入该帧并对计数进行修改，若不属于则直接略过，并不会改变计数。当依次传送完所有从站后再回到主站，主站会对传回来的数据进行分析处理，并依据计数判断此次通讯是否成功。主站每次只会发送一两帧的数据，这样占据了绝大部分的带宽，使得通讯速度提升了不少，也使得通讯效率更佳。具体的传输过程如下图2.3所示[17]。图5.3EtherCAT数据传输过程图图5.3EtherCAT数据传输过程图以太网数据帧的结构如下图图2.4所示。图5.4以太网数据帧图5.4以太网数据帧以太网数据帧的帧类型的值为0x88A4，表示传输数据为EtherCAT数据帧，FCS为帧校验序列，EtherCAT数据帧内有一个帧头和多个子报文。在anno机械臂中，有6个驱动器，EtherCAT数据长度表示这6个驱动器总体数据长度，其中每个驱动器的输入数据长度为25字节，输出长度为9字节。。中间一位为保留位，类型用来确定是否为从站通信，子报文包含子报文头和数据以及WKC，WKC表示计数器，在传输完成后，可通过对比计数器判断此次传输是否有效；子报文头内命令表示寻址方式和读写方式，索引表示帧编码，长度是该报文的长度，R表示保留位，M表示后续报文，C表示帧循环标志，中断位表示中断申请[18]。2.3.2寻址方式用EtherCAT通讯进行交换数据的时，是将数据帧内的数据写入到从站存储区域内或者将从站区域内的数据读取到数据帧内。其寻址的方式有网段寻址和段内寻址；前者有直连和开放模式，后者分为设备寻址和逻辑寻址。直连模式是主站网线连接第一个从站，该从站与下一个从站相连，直到最后一个；且无须经过交换机，所有的从站相当于一个以太网设备。如下图2.5所示。图5.5直连模式下的EtherCAT网段图5.5直连模式下的EtherCAT网段开放模式就是多主站设备多从站设备连接到一个交换机，不同网段的从站，就是不同的以太网设备，如图2.6所示。系统是通过交换机连接主站，主站再连接其他从站，伺服控制器相当于主站，伺服驱动器相当于从站。图5.6开放模式下的EtherCAT网段图5.6开放模式下的EtherCAT网段网段寻址是寻找不同网段的以太网设备。设备寻址，是在网段内寻找对应的某一个从站；EtherCAT子报文内有32位地址，其中16位是某一个从站地址，另外16位是该从站内的物理存储地址。逻辑寻址，同计算机内逻辑寻址方式一样，总线管理单元将物理地址和段内逻辑地址形成映射关系，通过缓冲区完成数据的交换。当从站收到EtherCAT子报文时，检查是否与总线管理单元内的地址匹配；如有匹配，则读取数据至对应的存储区；同时从站输出数据到对应变量区内。通过逻辑寻址，能更好统一管理系统结构，在传递周期性的数据时候，十分便捷。2.3.3同步方式EtherCAT的同步性十分重要，在控制多个轴运动时，如果不能按照预定的时间轨迹同步走，有时候会造成很严重的影响。EtherCAT内通过分布时钟(DC,DistributedClock)实现设备的实时同步性，第一个从站会有一个参考时钟，其他每个从站都是通过这个参考时钟同步，并都会有一个从时钟，在上电的时候，会首先测试传输的延迟以及时间偏移，将这些参数写进从时钟，完成时钟的初始化。分布式时钟是实现系统实时性的重要保证。2.3.4应用层数据交换应用层是整个通讯的最高层，可兼容多种协议，比如CoE,SoE等等，CoE协议是使用范围较广泛的，是EtherCAT在CANopen协议上扩展的协议。在EtherCAT通讯开始时，会通过邮箱通信，访问对象字典，完成网络通讯的初始化，通过应急对象以及供选择的时间驱动PDO消息，对网络进行管理；通过过程数据对象完成对周期型数据的交换和控制。伺服驱动器和主控制器之间就是采用PDO映射完成数据的交换，其中包括输入和输出PDO。其结构如下图2.7所示[19]，而在实际系统中控制器和驱动器的PDO映射关系如图2.8所示，可通过PDO映射完成控制器和驱动器数据的交换。图5.8控制器和驱动器的PDO映射图5.7CoE通讯结构图图5.8控制器和驱动器的PDO映射图5.7CoE通讯结构图2.4ADS通讯ADS(AutomationDeviceSpecification)是应用层之间的程序进行通信的一种接口。其不仅可以让TwinCAT平台支持的设备之间通讯，还可在由其他上位机程序，通过第三方的函数接口可以实现通讯。上面EtherCAT协议是完成主从站的通讯，在实际项目中，需要读取控制器内不同模块的信息，这些模块是相互独立的，通过ADS通讯就可以实现从上位机程序读写控制器内的变量。ADS基于TCP/IP的四层协议,位于四层协议中的应用层。其可为不同设备通讯提供路由。倍福控制器基于TwinCAT平台，在该平台，将PLC、运动控制、系统管理、HMI、还有观测器等等模块都配有其独特的地址和端口号，称为AdsAmsNetId以及AdsPort。AdsAmsNetId用于确定硬件设备，AdsPort用于确定软件服务。这些设备通过ADS通讯协议通讯。如下图2.9所示。由于ADS通讯传输数据在TCP/IP基础上，所以AdsAmsNetId地址是基于TCP/IP扩展的。两个都装有TwinCAT的平台的设备要实现通讯，必须通过路由管理器先广播搜索到该设备，并且将其添加到路由中，方可进行通讯。图5.9ADS设备层次图图5.9ADS设备层次图ADS的通讯方式有同步方式以及异步方式。同步方式适用于应答次数少并且不需要太多操作的系统，原理是当客户端需要访问一个数据，服务器只能返回该数据，客户端此时停止运行，直达结束后才可进行其他操作，能即时返回结果，但效率较低；异步方式包括异步读写以及通知方式。异步读写是客户端申请服务器的数据时，服务器返回数据，此时客户端会继续它的工作，这极大提高了效率。系统软件设计部分采取的通讯方式也就是异步读写；通知方式是客户端访问服务器数据时，服务器不响应，当客户端没有取消访问时，服务器不断的返回响应的数据。建立通讯后，上位机可以通过ADS协议访问控制器内的数据，ADS访问数据的方式有两种，分别是按照地址访问和按照变量名访问。按照地址访问的原理是，在控制器内有些数据已经分配了地址，根据输入输出和类型，分配的地址以及偏移量都不一样，上位机可以通过变量对应的区域和偏移量进行访问。按照变量名方式读写变量原理是，在给控制器内的变量名创建在句柄后，通过访问变量名即可访问该变量的数据。对比两个访问方式，按照地址访问需要知道其地址和偏移量，当访问变量多，很容易造成访问数据的混乱，在编写以及调试的时间上花费的时间也会更多，而变量名只需要建立句柄，在通过读写操作即可读写变量的数据，不仅方便，在软件调试的时候也容易找出错误点。故软件设计中采用的是按照变量名方式访问变量数据的方式，比如在上位机需要读取下位机中的压力值，下位机中压力传感器采集的数据通过程序转换成实际值后，存储到变量名为“get_presure_pc”的变量中，通过ADS协议，上位机可建立句柄，通过匹配对应变量名，访问变量中的数据,并将数据存储到上位机定义的变量getpressure中。程序例子如下：strcpy(szVar,"MAIN.get_presure_pc");G_1_Err2=AdsSyncReadWriteReq(G_pst_pAddr2,ADSIGRP_SYM_HNDBYNAME,0,sizeof(lHdlVar),&lHdlVar,sizeof(szVar),szVar);G_l_Err2=AdsSyncReadReq(G_pst_pAddr2,ADSIGRP_SYM_VALBYHND,lHdlVar,sizeof(getpressure),&getpressure);本系统中，上位机通过ADS协议和下位机进行通讯，而通过ADS需要交换的数据如下图2.10所示。图5.10ADS通讯交换的数据图5.10ADS通讯交换的数据2.5下位机软件设计下位机的软件就是在控制器内的程序，下位机软件主要包括下位机软件的配置和程序模块的设计。2.2.1软件的配置在完成硬件的装配后，需要完成基本底层参数配置，首先是从新建项目中扫描出I/O模块等从站设备。由于汇川驱动器是第三方的厂家，控制器内的TwinCAT平台无法直接扫描到汇川驱动器，需要将伺服驱动器的XML配置文件拷贝到“C:\TwinCAT\3.1\Config\Io\EtherCAT“，之后便可识别。当模块都扫描出来后，需要完成轴和变量的配置。TwinCAT中的轴，可分为PLC轴，NC轴，以及物理轴。关系如下图2.11所示。图5.11TwinCAT中运动轴的关系图5.11TwinCAT中运动轴的关系物理轴对应实际的伺服电机，NC轴是软件平台运动控制部分的虚拟轴，以及PLC轴是运行在程序里轴；在控制器内可以进行高速的信息交换。每个NC轴会绑定一个物理轴和PLC轴，根据情况可自己设定实际轴的顺序，每个NC轴都需要进行参数配置。其中有些变量会自动绑定，有些需要人工配置，比如速度限制，位移限制，以及编码器与脉冲关系等等。绑定完对应轴后，根据电路设计图绑定I/O模块中的变量，比如开关量和模拟输入量。轴的绑定图如下图2.12所示。其他需要绑定的变量原理相同。图5.12变量的绑定图5.12变量的绑定2.2.2运动模块设计运动模块主要包括单轴点动模块，单轴定量运动模块，末端位姿模块等。单轴点动模块单轴点动指的是对机器人的单个关节轴进行微调。在控制机器人时，有时需要对其某个关节进行调整，此时通过鼠标点击来使此关节轴正向点动或者反向点动，松开鼠标则关节轴停止运动。是机器人控制中常用功能之一，功能框图如图2.13所示。图5.13单轴点动流程图图5.13单轴点动流程图单轴定量运动当机器人的某个轴需要进行较大的角度移动的时候，可以直接对该轴进行运动控制。在确定要控制某个轴运动时，通过直接输入要运动的角度或者位移量，控制器会将输入的量换算成伺服电机对应的运动量并控制器运动。单轴定量运动流程图如图2.14所示。图5.14单轴定量运动流程图图5.14单轴定量运动流程图末端位姿模块在机器人控制中，经常需要用到末端位姿控制，末端位姿控制，即输入空间点和姿态，由前面章节逆向运动学所推出公式求解，得到单个机构的运动量，再转换成轴的运动量。由于现场对速度要求较低，通过设置默认的速度，得到每个轴的速度，最终达到指定运动地点。流程图如下图2.15所示。图5.15末端位姿控制流程图图5.15末端位姿控制流程图2.6上位机软件设计上位机软件是操作者直接操作的界面。操作者可以通过工控机中的上位机软件与下位机建立连接，读取机器人的状态并对机器人进行控制。上位机软件分为系统连接启动，操作运行和上位机安全性设计三个部分，其流程图如图2.16所示。图5.16上位机软件系统流程图图5.16上位机软件系统流程图连接启动模块主要完成上位机和下位机的连接及启动。在控制柜将远程/本地旋钮打到远程，在启动控制器程序后，上位机可以通过ADS通讯连接到控制器，在进行连接前，需要把第三方提供的ADS函数接口文件放进上位机指定路径内，连接需要提供控制器的AdsAmsNetId以及AdsPort。该模块内还有驱动开关和使能开关，使能开关是对机器人的伺服系统完成电机的松刹车和电机的使能。操作运行模块是在系统都准备就绪后，根据实际需求，选择输入运动量，确定输入数据，再点运行，即可完成该机构运行。操作运行主要是通过ADS通讯协议中异步读写方式，将运动量和速度读写到控制器内对应的模块，控制器处理完成后，得到对应机构需要运动量和速度，输入到对应每个轴的运动函数里。完成相应的运动。当其需要停止，按下对应停止按钮即可。显示存储模块，作用是不断读取系统内状态，并且显示在软件界面上，让操作者直接看到系统关键的状态信息；以及能够切换界面，显示配置系统预设的参数和显示简单功能菜单。上位机可以实时采集电机的运动数据以及机构的姿态数据，方便以后分析和存储备份。上位机安全性设计模块主要是限制输入框的输入范围，将运动量限制在可靠范围内，确保系统的安全。2.7小结本章主要是系统软件设计，软件分为上位机、下位机和通讯协议。介绍了软件开发平台，分析了系统采用的通讯协议EtherCAT协议和ADS协议，设计并描述了上位机和下位机的软件模块。3实验测试3.1测试准备首先对硬件进行连接，整个硬件的连接是从机械臂到中转箱，中转箱到控制柜，控制柜有专门的走线，控制柜和Y轴控制柜以及工控机通过交换机相连，电源都是从供电箱引出。设置控制器、工控机地址，检查是否能联通。之后对控制器的参数进行配置。控制器是基于倍福的TwinCAT平台，参数配置也在该平台，首先通过平台扫描，添加路由，连上工控机，打开工程文件，连接控制器，在配置模式下，扫描设备，将所有设备添加至项目，设备包括驱动器和电机以及总线端子模块。对这些设备某些进行配置，其中配置包括，驱动器内参数添加至控制器，电机参数的配置，总线端子的配置，将程序内的变量和设备的参数绑定在一起。(b)图6.1中转箱和控制柜的连线（(b)图6.1中转箱和控制柜的连线（a）3.2单轴控制测试完成测试的准备工作后，要对特种机械臂的控制性能进行测试特种机械臂的控制性能由单机构的控制性能决定。单机构的控制性能主要包括运动精度和重复定位精度，每次运动固定值，读取编码器值和红外激光测量仪的测试进行对比即可得出运动精度。机械臂实际运动图片如图3.2所示。图6图6.2机械臂实际运动图X向机构精度测试将X机构移到支架附近适当位置，在机构支架和机构上安装红外测量点；将此时的X轴编码器值记为零点，设置固定运动值，当机构不动并且编码器数值一定时，记录数据。采集一定量数据，其中X轴的编码器值。经过实际转换，表示为实际长度。X机构运动初始值为350.25mm，实际数据如下表3.1所示。表3.1X向机构运动精度测试数据序号位置给定值测量值偏差序号位置给定值测量值偏差1350.2551.950.057370.252521.990.012352.25109.970.038362.252019.970.033360.251511.950.059360.251511.990.014362.252019.980.0210352.25109.970.035370.252522.01-0.0111350.2551.980.026372.253030.05-0.0512342.2500.01-0.01根据表格中数据，绘制误差分析图,如图3.3所示。图6.3X机构图6.3X机构控制误差由图显示看出，X机构总体运行精确，其误差范围在0.1mm以内，平均误差为0.015mm。（2）偏航机构精度测试偏航机构包括前偏航和后偏航，由于两个机构测量的方法一致，这里仅展示其中一个的测量结果，每个偏航机构是由两个作动筒组成，测量时，根据角度传感器和Optotrak光学跟踪测量仪测量实际的空间角度对比计算。初始值为0.31°。在初始位置末端放置一个平木板，偏航角度编码器处和木板上各安放一个红外点，以及在末端安放一个红外点，在开始前对角度进行标定，等待标定完成后，开始运动偏航机构，等待运动停止后，读取角度编码器值和通过测量仪采集记录当前数值。表3.2偏航机构运动精度测试序号位置给定值测量值偏差序号位置给定值测量值偏差13.3132.9820.018712.311511.9690.03123.3163.033-0.033812.311211.9410.05939.3199.014-0.01499.3198.970.03412.311211.9780.022103.3162.9730.027512.311511.9590.041113.3133.013-0.013618.311817.9790.0211200-0.0240.024画出误差分析图如图3.4所示。图6.4偏航图6.4偏航机构控制误差由表中数据计算出，实际运动中的误差维持在0.8°以内，平均误差为0.0178°。（3）滚转机构精度测试滚转机构分为前滚转和后滚转，测试方法与偏航机构一样，由于滚转机构安装角度编码器不便，这里采用电机自带的编码器和测量仪比较；同样这里展示其中一个的滚转角的数据，初始值为3.15°。表3.3滚转机构运动精度测试序号位置给定值测量值偏差序号位置给定值测量值偏差13.1533.041-0.041