面向托卡马克第一壁检测的机械臂实时运动控制系统设计与实现

屈云飞 陈卫东

(上海交通大学自动化系系统控制与信息处理教育部重点实验室,上海200240)

热核聚变实验装置(Experimental Advanced Superconducting Tokamak，EAST)的研发是国家“九五”重大科学工程，该装置是同时具有全超导磁体和灵活冷却结构的超导托卡马克装置[1]。托卡马克第一壁因长期处于高温、高真空、高辐射的环境中，且受高能粒子撞击而较易损坏。为保证托卡马克设备高效运行，需快速完成对第一壁的检测和维护任务。

机器人遥操作技术是托卡马克设备远程维护中广泛采用的有效手段之一。为完成大范围高精度的检测任务，维护机器人往往选用具有超冗余结构的链式机械臂[2,3]。针对冗余结构机械臂这一复杂的控制对象，运动控制系统需具备较高的实时性与安全性[4]。这是因为：首先，机械臂作业环境即托卡马克腔，结构复杂，为保证作业过程中机械臂不与核心设备第一壁发生碰撞，运动控制系统需对异常事件具备快速的反应处理能力；其次，机械臂构型复杂，关节数目多，伺服单元与运动控制系统通信数据量大，为实现高精度控制，需采用高速实时的数据传输总线以保证系统的实时性；最后，为保证能对遥操作系统控制指令做出快速响应，特别是紧急停车等重要控制指令，运动控制系统需在确定的时间内完成对控制指令的接收和处理。

针对上述问题，笔者设计了超冗余10-DOF机械臂及其实时运动控制系统。该系统采用EtherCAT通信协议及其分布式时钟技术，能满足多伺服单元与运动控制系统间高速实时的数据交换。借助QNX RTOS的强实时软件环境和APIC模式下的HPET高精度定时器，能保证对外部事件做出快速、确定的响应。

1 系统硬件①

笔者设计的超冗余10-DOF机械臂运动控制系统硬件结构如图1所示，该系统主要由AIMB-769工控机、可支持EtherCAT通信协议的Elmo伺服驱动器、Maxon 370356-1391707伺服电机和多摩川17位绝对式编码器组成。

整个硬件系统的设计需遵循轻量化、紧凑化的原则。这是因为当机械臂完全展开时，其长度在7m左右，为降低机械臂柔性效应，需尽可能减少机械臂负重。同时，由于系统复杂、信号连线多，为提高系统的安全性与稳定性，需降低系统连线数目。

为减少连线数目，设计的运动控制系统与遥操作系统和各伺服单元均仅通过一根CAT 6网线连接，各伺服单元之间以EtherCAT总线串行相连，所有控制指令和状态数据仅通过一根网线反馈至运动控制系统。为减轻机械臂负重，选用了尺寸为73.4mm×46.5mm×35.8mm、重量为106g、最大可提供1.6kW持续驱动能力的Elmo G-SOLWHIL-E系列驱动器。该驱动器提供了6个可编程数字输入、两个可编程数字输出和一个可编程模拟输入。为进一步减少连线数目，本系统利用可编程模拟输入实现了对机械臂和腔体的温度测量。

图1 运动控制系统硬件结构

因为相对编码器在掉电之后会丢失位置信息，基于限位开关的寻找零位方式并不适用于该机械臂。又因为机械臂所处环境是非自由空间，利用限位开关寻找零位的过程可能导致机械臂与环境发生碰撞，因此笔者选用多摩川17位绝对式编码器，保证在系统掉电之后机械臂的位置信息依然能被保存。

2 系统软件

为提高运动控制系统程序的可维护性，降低系统各软件模块间的耦合度，笔者基于模块化的设计原则设计了如图2所示的运动控制软件系统。该软件系统主要由遥操作通信模块、故障检测与应急处理模块、系统信息检测模块、EtherCAT Master模块和运动控制器模块组成。遥操作通信模块获取控制指令信息并发送至运动控制器模块，同时将当前系统状态信息和异常信息反馈至遥操作系统；系统信息监测模块获得包括关节位置及速度等在内的状态信息并发送至遥操作通信模块；故障检测与应急处理模块获取系统异常信息并分别送至遥操作通信模块和运动控制器模块；运动控制器模块负责计算出各关节运动指令，并通过EtherCAT Master模块下发至各伺服关节；EtherCAT Master模块主要负责收、发EtherCAT报文并对报文数据进行基本的解析。各个模块功能相对独立，耦合度低。

图2 运动控制系统软件结构

运动控制系统的实时性很大程度上取决于EtherCAT Master模块和系统所依赖的QNX RTOS平台，为提高运动控制系统的实时性，必须对它们进行一系列特殊的配置，以下将分别从两个方面进行介绍。

2.1 EtherCAT Master模块

EtherCAT是由德国倍福自动化有限公司研发的一种开放的实时工业以太网协议。相对于其他常用的现场总线，如HART、FF总线、Profibus、Modbus、Controlnet及Devicenet等[5]，EtherCAT具备更高的实时性和传输效率[6]。另外，EtherCAT具备拓扑结构灵活、构建成本低、同步精度高的特点，因此笔者选用EtherCAT现场总线作为系统伺服总线。

一个完整的EtherCAT伺服系统由一个EtherCAT Master(以下称主站)和一个或多个EtherCAT Slave(以下称从站)组成。主站对硬件没有特殊要求，只需一个100Mb/s网卡和通用的硬件环境(如x86工控机)即可搭建。目前市场上支持EtherCAT协议的从站较多，而主站相对较少。主站可分为开源主站和商业主站两种，其中开源主站主要有Igh和SOEM两个，商业主站主要有德国倍福公司的TwinCAT及Koenig公司的KPA Master EtherCAT等。从站需借助特殊硬件以降低物理层和数据链路层传输延时的不确定性，本系统选用Elmo伺服控制器作为从站。由于所设计的运动控制系统是基于PC的开放分布式控制系统，此类系统控制的精度依赖于各伺服单元间的同步性。为提高各伺服单元的同步精度，笔者借助EtherCAT分布式时钟技术，将各伺服单元之间的同步误差限制在纳秒级。

2.1.1EtherCAT网络配置

EtherCAT实时工业以太网主要支持两类通信协议：PDO和SDO，其中SDO又包括COE、SOE、VOE及EOE等协议。PDO数目不易过多，否则会加重总线传输负担，降低系统实时性。在选择待映射的PDO时，为了降低总线负担，应尽量避免映射不必要的PDO。对于需要高速实时交互的数据对象，如期望位置及实际位置等可通过PDO映射将其映射为PDO，对于那些对实时性要求不高的数据对象，如最大速度及最大加速度等可通过SDO设置。

利用主站所提供的网络配置工具可以对当前的EtherCAT网络进行配置。笔者配置的用于周期性通信的PDO见表1。Elmo系列伺服驱动器均支持DS402协议，所有映射的对象在DS402中均有详细的定义，此处不再赘述。通过表1可以发现，输入PDO共有4个对象，输出PDO共有6个对象，所有对象的数据长度合计为29Byte。考虑到检测机械臂共有10个自由度，即10个伺服单元，所以一次周期性通信的数据吞吐量共计290Byte。

表1 用于周期性通信的PDO Byte

2.1.2分布式时钟设置

机械臂末端位置的控制精度严重依赖于各伺服单元间的同步性，EtherCAT为从站间的同步提供了一种分布式时钟(DC)机制，该机制可将伺服单元间的同步误差限制在纳秒级。伺服单元的同步误差主要来源于传输延时、各单元与参考时钟间的偏差、本地时钟因时钟信号源的不稳定带来的偏移这3个方面。DC假设伺服单元间的传输延时相同，通过补偿本地时钟与参考时钟的偏差并在线调整本地时钟偏移，能够获得纳秒级的同步精度。需要说明的是，一般默认选择第一个与主站连接的从站的时钟作为参考时钟。

一般地，EtherCAT网络配置工具会提供选择参考时钟和设置同步周期的选项。某些情况下，仅需要修改同步周期或者参考时钟源而不修改其他网络配置参数，且考虑到配置工具一般运行于Windows系统上，EtherCAT主站运行于其他实时操作系统上，网络配置步骤繁琐，此时仅仅因修改上述参数而重新配置网络没有必要。针对此类情况，用户可直接对生成的网络配置文件ENI.xml进行修改。

2.2 QNX RTOS环境设置

运动控制系统对实时性的要求决定了其与实时操作系统存在密不可分的关系。QNX RTOS作为一款真正意义的具有微内核结构的实时操作系统，在网络通信、中断延时、线程切换时间及最高中断率等关键指标上均优于VxWorks、WinCE和各类实时Linux操作系统[1,7]。因此笔者选择QNX 6.5.0作为运动控制系统的实时操作系统平台。

由于EtherCAT主站也运行于QNX RTOS之上，且主站对定时周期的稳定性有更严格的要求，传统的可编程计数器(8254 PIT)难以提供高于4kHz的稳定定时周期，因此需采用更先进的高精度定时器HPET。但是HPET运行在APIC模式下，而QNX默认运行于PIC模式下，因此为了使能HPET，需修改系统所使用的工控机板级支持包(BSP)，并在运动控制系统程序中修改对应的定时器中断索引号。经测试，8254 PIT可提供最小步长为29.333μs的定时周期，而HPET可提供最小步长为10μs的定时周期。通过上述设定能改善运动控制系统运行环境的实时性。

3 实验

为验证所设计运动控制系统的有效性，采用如图3所示的实验系统。该实验系统由遥操作系统(三维仿真系统)、运动控制系统和10轴Elmo测试箱组成。该实验系统利用10轴Elmo测试箱来模拟10-DOF检测机械臂，以验证运动控制系统的有效性。

图3 运动控制实验系统

实验流程为：操作人员通过三维仿真系统下达期望的正弦波位置运动指令，运动控制系统接收并解析该指令，将关节位置分别下发至各个Elmo驱动器。三维仿真系统与运动控制系统的通信周期为2ms(基于libpcap实现)，运动控制系统与伺服驱动器以EtherCAT实时工业以太网相连接，设置PDO同步周期为250μs，单次PDO通信数据吞吐量为290Byte。通过记录两次PDO数据的发送时间间隔，可测试在高速实时数据交换的情况下，运动控制系统伺服周期的时间抖动情况，如图4所示。记录下的Elmo测试箱第9、10两个关节的位置跟踪曲线和跟踪误差曲线如图5、6所示。

图4 PDO数据交换循环周期时间

图5 关节9轨迹跟踪和跟踪误差曲线

图6 关节10轨迹跟踪和跟踪误差曲线

通过图4可以得出，当将PDO数据同步周期设置为250μs时，运动控制系统伺服周期的波动主要在±2μs之间，且最大值不超过9μs。通过图5、6可以看出，各关节可快速准确地跟踪期望位置曲线，关节9的最大跟踪误差为0.000 2rad，是期望位置的0.48‰；关节10的最大跟踪误差为0.000 1rad，是期望位置的0.98‰，两个关节的动态跟踪误差均较小。通过该实验，验证了所设计的运动控制系统的有效性。

4 结束语

针对托卡马克第一壁高稳定、高安全的检测需要，借助于QNX RTOS优异的实时特性、EtherCAT高效的数据传输机制和分布式时钟高精度的同步策略，设计并实现了一种实时运动控制系统。该系统具备传输数据量大、实时性高、连线简单的特点。通过实验，证明了该系统具备微秒级的实时特性，且能实现快速精确的控制。