屈云飛 陳衛(wèi)東

(上海交通大學(xué)自動(dòng)化系系統(tǒng)控制與信息處理教育部重點(diǎn)實(shí)驗(yàn)室,上海200240)

熱核聚變實(shí)驗(yàn)裝置(Experimental Advanced Superconducting Tokamak，EAST)的研發(fā)是國(guó)家“九五”重大科學(xué)工程，該裝置是同時(shí)具有全超導(dǎo)磁體和靈活冷卻結(jié)構(gòu)的超導(dǎo)托卡馬克裝置[1]。托卡馬克第一壁因長(zhǎng)期處于高溫、高真空、高輻射的環(huán)境中，且受高能粒子撞擊而較易損壞。為保證托卡馬克設(shè)備高效運(yùn)行，需快速完成對(duì)第一壁的檢測(cè)和維護(hù)任務(wù)。

機(jī)器人遙操作技術(shù)是托卡馬克設(shè)備遠(yuǎn)程維護(hù)中廣泛采用的有效手段之一。為完成大范圍高精度的檢測(cè)任務(wù)，維護(hù)機(jī)器人往往選用具有超冗余結(jié)構(gòu)的鏈?zhǔn)綑C(jī)械臂[2,3]。針對(duì)冗余結(jié)構(gòu)機(jī)械臂這一復(fù)雜的控制對(duì)象，運(yùn)動(dòng)控制系統(tǒng)需具備較高的實(shí)時(shí)性與安全性[4]。這是因?yàn)椋菏紫龋瑱C(jī)械臂作業(yè)環(huán)境即托卡馬克腔，結(jié)構(gòu)復(fù)雜，為保證作業(yè)過(guò)程中機(jī)械臂不與核心設(shè)備第一壁發(fā)生碰撞，運(yùn)動(dòng)控制系統(tǒng)需對(duì)異常事件具備快速的反應(yīng)處理能力；其次，機(jī)械臂構(gòu)型復(fù)雜，關(guān)節(jié)數(shù)目多，伺服單元與運(yùn)動(dòng)控制系統(tǒng)通信數(shù)據(jù)量大，為實(shí)現(xiàn)高精度控制，需采用高速實(shí)時(shí)的數(shù)據(jù)傳輸總線(xiàn)以保證系統(tǒng)的實(shí)時(shí)性；最后，為保證能對(duì)遙操作系統(tǒng)控制指令做出快速響應(yīng)，特別是緊急停車(chē)等重要控制指令，運(yùn)動(dòng)控制系統(tǒng)需在確定的時(shí)間內(nèi)完成對(duì)控制指令的接收和處理。

針對(duì)上述問(wèn)題，筆者設(shè)計(jì)了超冗余10-DOF機(jī)械臂及其實(shí)時(shí)運(yùn)動(dòng)控制系統(tǒng)。該系統(tǒng)采用EtherCAT通信協(xié)議及其分布式時(shí)鐘技術(shù)，能滿(mǎn)足多伺服單元與運(yùn)動(dòng)控制系統(tǒng)間高速實(shí)時(shí)的數(shù)據(jù)交換。借助QNX RTOS的強(qiáng)實(shí)時(shí)軟件環(huán)境和APIC模式下的HPET高精度定時(shí)器，能保證對(duì)外部事件做出快速、確定的響應(yīng)。

1 系統(tǒng)硬件①

筆者設(shè)計(jì)的超冗余10-DOF機(jī)械臂運(yùn)動(dòng)控制系統(tǒng)硬件結(jié)構(gòu)如圖1所示，該系統(tǒng)主要由AIMB-769工控機(jī)、可支持EtherCAT通信協(xié)議的Elmo伺服驅(qū)動(dòng)器、Maxon 370356-1391707伺服電機(jī)和多摩川17位絕對(duì)式編碼器組成。

整個(gè)硬件系統(tǒng)的設(shè)計(jì)需遵循輕量化、緊湊化的原則。這是因?yàn)楫?dāng)機(jī)械臂完全展開(kāi)時(shí)，其長(zhǎng)度在7m左右，為降低機(jī)械臂柔性效應(yīng)，需盡可能減少機(jī)械臂負(fù)重。同時(shí)，由于系統(tǒng)復(fù)雜、信號(hào)連線(xiàn)多，為提高系統(tǒng)的安全性與穩(wěn)定性，需降低系統(tǒng)連線(xiàn)數(shù)目。

為減少連線(xiàn)數(shù)目，設(shè)計(jì)的運(yùn)動(dòng)控制系統(tǒng)與遙操作系統(tǒng)和各伺服單元均僅通過(guò)一根CAT 6網(wǎng)線(xiàn)連接，各伺服單元之間以EtherCAT總線(xiàn)串行相連，所有控制指令和狀態(tài)數(shù)據(jù)僅通過(guò)一根網(wǎng)線(xiàn)反饋至運(yùn)動(dòng)控制系統(tǒng)。為減輕機(jī)械臂負(fù)重，選用了尺寸為73.4mm×46.5mm×35.8mm、重量為106g、最大可提供1.6kW持續(xù)驅(qū)動(dòng)能力的Elmo G-SOLWHIL-E系列驅(qū)動(dòng)器。該驅(qū)動(dòng)器提供了6個(gè)可編程數(shù)字輸入、兩個(gè)可編程數(shù)字輸出和一個(gè)可編程模擬輸入。為進(jìn)一步減少連線(xiàn)數(shù)目，本系統(tǒng)利用可編程模擬輸入實(shí)現(xiàn)了對(duì)機(jī)械臂和腔體的溫度測(cè)量。

圖1 運(yùn)動(dòng)控制系統(tǒng)硬件結(jié)構(gòu)

因?yàn)橄鄬?duì)編碼器在掉電之后會(huì)丟失位置信息，基于限位開(kāi)關(guān)的尋找零位方式并不適用于該機(jī)械臂。又因?yàn)闄C(jī)械臂所處環(huán)境是非自由空間，利用限位開(kāi)關(guān)尋找零位的過(guò)程可能導(dǎo)致機(jī)械臂與環(huán)境發(fā)生碰撞，因此筆者選用多摩川17位絕對(duì)式編碼器，保證在系統(tǒng)掉電之后機(jī)械臂的位置信息依然能被保存。

2 系統(tǒng)軟件

為提高運(yùn)動(dòng)控制系統(tǒng)程序的可維護(hù)性，降低系統(tǒng)各軟件模塊間的耦合度，筆者基于模塊化的設(shè)計(jì)原則設(shè)計(jì)了如圖2所示的運(yùn)動(dòng)控制軟件系統(tǒng)。該軟件系統(tǒng)主要由遙操作通信模塊、故障檢測(cè)與應(yīng)急處理模塊、系統(tǒng)信息檢測(cè)模塊、EtherCAT Master模塊和運(yùn)動(dòng)控制器模塊組成。遙操作通信模塊獲取控制指令信息并發(fā)送至運(yùn)動(dòng)控制器模塊，同時(shí)將當(dāng)前系統(tǒng)狀態(tài)信息和異常信息反饋至遙操作系統(tǒng)；系統(tǒng)信息監(jiān)測(cè)模塊獲得包括關(guān)節(jié)位置及速度等在內(nèi)的狀態(tài)信息并發(fā)送至遙操作通信模塊；故障檢測(cè)與應(yīng)急處理模塊獲取系統(tǒng)異常信息并分別送至遙操作通信模塊和運(yùn)動(dòng)控制器模塊；運(yùn)動(dòng)控制器模塊負(fù)責(zé)計(jì)算出各關(guān)節(jié)運(yùn)動(dòng)指令，并通過(guò)EtherCAT Master模塊下發(fā)至各伺服關(guān)節(jié)；EtherCAT Master模塊主要負(fù)責(zé)收、發(fā)EtherCAT報(bào)文并對(duì)報(bào)文數(shù)據(jù)進(jìn)行基本的解析。各個(gè)模塊功能相對(duì)獨(dú)立，耦合度低。

圖2 運(yùn)動(dòng)控制系統(tǒng)軟件結(jié)構(gòu)

運(yùn)動(dòng)控制系統(tǒng)的實(shí)時(shí)性很大程度上取決于EtherCAT Master模塊和系統(tǒng)所依賴(lài)的QNX RTOS平臺(tái)，為提高運(yùn)動(dòng)控制系統(tǒng)的實(shí)時(shí)性，必須對(duì)它們進(jìn)行一系列特殊的配置，以下將分別從兩個(gè)方面進(jìn)行介紹。

2.1 EtherCAT Master模塊

EtherCAT是由德國(guó)倍福自動(dòng)化有限公司研發(fā)的一種開(kāi)放的實(shí)時(shí)工業(yè)以太網(wǎng)協(xié)議。相對(duì)于其他常用的現(xiàn)場(chǎng)總線(xiàn)，如HART、FF總線(xiàn)、Profibus、Modbus、Controlnet及Devicenet等[5]，EtherCAT具備更高的實(shí)時(shí)性和傳輸效率[6]。另外，EtherCAT具備拓?fù)浣Y(jié)構(gòu)靈活、構(gòu)建成本低、同步精度高的特點(diǎn)，因此筆者選用EtherCAT現(xiàn)場(chǎng)總線(xiàn)作為系統(tǒng)伺服總線(xiàn)。

一個(gè)完整的EtherCAT伺服系統(tǒng)由一個(gè)EtherCAT Master(以下稱(chēng)主站)和一個(gè)或多個(gè)EtherCAT Slave(以下稱(chēng)從站)組成。主站對(duì)硬件沒(méi)有特殊要求，只需一個(gè)100Mb/s網(wǎng)卡和通用的硬件環(huán)境(如x86工控機(jī))即可搭建。目前市場(chǎng)上支持EtherCAT協(xié)議的從站較多，而主站相對(duì)較少。主站可分為開(kāi)源主站和商業(yè)主站兩種，其中開(kāi)源主站主要有Igh和SOEM兩個(gè)，商業(yè)主站主要有德國(guó)倍福公司的TwinCAT及Koenig公司的KPA Master EtherCAT等。從站需借助特殊硬件以降低物理層和數(shù)據(jù)鏈路層傳輸延時(shí)的不確定性，本系統(tǒng)選用Elmo伺服控制器作為從站。由于所設(shè)計(jì)的運(yùn)動(dòng)控制系統(tǒng)是基于PC的開(kāi)放分布式控制系統(tǒng)，此類(lèi)系統(tǒng)控制的精度依賴(lài)于各伺服單元間的同步性。為提高各伺服單元的同步精度，筆者借助EtherCAT分布式時(shí)鐘技術(shù)，將各伺服單元之間的同步誤差限制在納秒級(jí)。

2.1.1EtherCAT網(wǎng)絡(luò)配置

EtherCAT實(shí)時(shí)工業(yè)以太網(wǎng)主要支持兩類(lèi)通信協(xié)議：PDO和SDO，其中SDO又包括COE、SOE、VOE及EOE等協(xié)議。PDO數(shù)目不易過(guò)多，否則會(huì)加重總線(xiàn)傳輸負(fù)擔(dān)，降低系統(tǒng)實(shí)時(shí)性。在選擇待映射的PDO時(shí)，為了降低總線(xiàn)負(fù)擔(dān)，應(yīng)盡量避免映射不必要的PDO。對(duì)于需要高速實(shí)時(shí)交互的數(shù)據(jù)對(duì)象，如期望位置及實(shí)際位置等可通過(guò)PDO映射將其映射為PDO，對(duì)于那些對(duì)實(shí)時(shí)性要求不高的數(shù)據(jù)對(duì)象，如最大速度及最大加速度等可通過(guò)SDO設(shè)置。

利用主站所提供的網(wǎng)絡(luò)配置工具可以對(duì)當(dāng)前的EtherCAT網(wǎng)絡(luò)進(jìn)行配置。筆者配置的用于周期性通信的PDO見(jiàn)表1。Elmo系列伺服驅(qū)動(dòng)器均支持DS402協(xié)議，所有映射的對(duì)象在DS402中均有詳細(xì)的定義，此處不再贅述。通過(guò)表1可以發(fā)現(xiàn)，輸入PDO共有4個(gè)對(duì)象，輸出PDO共有6個(gè)對(duì)象，所有對(duì)象的數(shù)據(jù)長(zhǎng)度合計(jì)為29Byte?？紤]到檢測(cè)機(jī)械臂共有10個(gè)自由度，即10個(gè)伺服單元，所以一次周期性通信的數(shù)據(jù)吞吐量共計(jì)290Byte。

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

2.1.2分布式時(shí)鐘設(shè)置

機(jī)械臂末端位置的控制精度嚴(yán)重依賴(lài)于各伺服單元間的同步性，EtherCAT為從站間的同步提供了一種分布式時(shí)鐘(DC)機(jī)制，該機(jī)制可將伺服單元間的同步誤差限制在納秒級(jí)。伺服單元的同步誤差主要來(lái)源于傳輸延時(shí)、各單元與參考時(shí)鐘間的偏差、本地時(shí)鐘因時(shí)鐘信號(hào)源的不穩(wěn)定帶來(lái)的偏移這3個(gè)方面。DC假設(shè)伺服單元間的傳輸延時(shí)相同，通過(guò)補(bǔ)償本地時(shí)鐘與參考時(shí)鐘的偏差并在線(xiàn)調(diào)整本地時(shí)鐘偏移，能夠獲得納秒級(jí)的同步精度。需要說(shuō)明的是，一般默認(rèn)選擇第一個(gè)與主站連接的從站的時(shí)鐘作為參考時(shí)鐘。

一般地，EtherCAT網(wǎng)絡(luò)配置工具會(huì)提供選擇參考時(shí)鐘和設(shè)置同步周期的選項(xiàng)。某些情況下，僅需要修改同步周期或者參考時(shí)鐘源而不修改其他網(wǎng)絡(luò)配置參數(shù)，且考慮到配置工具一般運(yùn)行于Windows系統(tǒng)上，EtherCAT主站運(yùn)行于其他實(shí)時(shí)操作系統(tǒng)上，網(wǎng)絡(luò)配置步驟繁瑣，此時(shí)僅僅因修改上述參數(shù)而重新配置網(wǎng)絡(luò)沒(méi)有必要。針對(duì)此類(lèi)情況，用戶(hù)可直接對(duì)生成的網(wǎng)絡(luò)配置文件ENI.xml進(jìn)行修改。

2.2 QNX RTOS環(huán)境設(shè)置

運(yùn)動(dòng)控制系統(tǒng)對(duì)實(shí)時(shí)性的要求決定了其與實(shí)時(shí)操作系統(tǒng)存在密不可分的關(guān)系。QNX RTOS作為一款真正意義的具有微內(nèi)核結(jié)構(gòu)的實(shí)時(shí)操作系統(tǒng)，在網(wǎng)絡(luò)通信、中斷延時(shí)、線(xiàn)程切換時(shí)間及最高中斷率等關(guān)鍵指標(biāo)上均優(yōu)于VxWorks、WinCE和各類(lèi)實(shí)時(shí)Linux操作系統(tǒng)[1,7]。因此筆者選擇QNX 6.5.0作為運(yùn)動(dòng)控制系統(tǒng)的實(shí)時(shí)操作系統(tǒng)平臺(tái)。

由于EtherCAT主站也運(yùn)行于QNX RTOS之上，且主站對(duì)定時(shí)周期的穩(wěn)定性有更嚴(yán)格的要求，傳統(tǒng)的可編程計(jì)數(shù)器(8254 PIT)難以提供高于4kHz的穩(wěn)定定時(shí)周期，因此需采用更先進(jìn)的高精度定時(shí)器HPET。但是HPET運(yùn)行在A(yíng)PIC模式下，而QNX默認(rèn)運(yùn)行于PIC模式下，因此為了使能HPET，需修改系統(tǒng)所使用的工控機(jī)板級(jí)支持包(BSP)，并在運(yùn)動(dòng)控制系統(tǒng)程序中修改對(duì)應(yīng)的定時(shí)器中斷索引號(hào)。經(jīng)測(cè)試，8254 PIT可提供最小步長(zhǎng)為29.333μs的定時(shí)周期，而HPET可提供最小步長(zhǎng)為10μs的定時(shí)周期。通過(guò)上述設(shè)定能改善運(yùn)動(dòng)控制系統(tǒng)運(yùn)行環(huán)境的實(shí)時(shí)性。

3 實(shí)驗(yàn)

為驗(yàn)證所設(shè)計(jì)運(yùn)動(dòng)控制系統(tǒng)的有效性，采用如圖3所示的實(shí)驗(yàn)系統(tǒng)。該實(shí)驗(yàn)系統(tǒng)由遙操作系統(tǒng)(三維仿真系統(tǒng))、運(yùn)動(dòng)控制系統(tǒng)和10軸Elmo測(cè)試箱組成。該實(shí)驗(yàn)系統(tǒng)利用10軸Elmo測(cè)試箱來(lái)模擬10-DOF檢測(cè)機(jī)械臂，以驗(yàn)證運(yùn)動(dòng)控制系統(tǒng)的有效性。

圖3 運(yùn)動(dòng)控制實(shí)驗(yàn)系統(tǒng)

實(shí)驗(yàn)流程為：操作人員通過(guò)三維仿真系統(tǒng)下達(dá)期望的正弦波位置運(yùn)動(dòng)指令，運(yùn)動(dòng)控制系統(tǒng)接收并解析該指令，將關(guān)節(jié)位置分別下發(fā)至各個(gè)Elmo驅(qū)動(dòng)器。三維仿真系統(tǒng)與運(yùn)動(dòng)控制系統(tǒng)的通信周期為2ms(基于libpcap實(shí)現(xiàn))，運(yùn)動(dòng)控制系統(tǒng)與伺服驅(qū)動(dòng)器以EtherCAT實(shí)時(shí)工業(yè)以太網(wǎng)相連接，設(shè)置PDO同步周期為250μs，單次PDO通信數(shù)據(jù)吞吐量為290Byte。通過(guò)記錄兩次PDO數(shù)據(jù)的發(fā)送時(shí)間間隔，可測(cè)試在高速實(shí)時(shí)數(shù)據(jù)交換的情況下，運(yùn)動(dòng)控制系統(tǒng)伺服周期的時(shí)間抖動(dòng)情況，如圖4所示。記錄下的Elmo測(cè)試箱第9、10兩個(gè)關(guān)節(jié)的位置跟蹤曲線(xiàn)和跟蹤誤差曲線(xiàn)如圖5、6所示。

圖4 PDO數(shù)據(jù)交換循環(huán)周期時(shí)間

圖5 關(guān)節(jié)9軌跡跟蹤和跟蹤誤差曲線(xiàn)

圖6 關(guān)節(jié)10軌跡跟蹤和跟蹤誤差曲線(xiàn)

通過(guò)圖4可以得出，當(dāng)將PDO數(shù)據(jù)同步周期設(shè)置為250μs時(shí)，運(yùn)動(dòng)控制系統(tǒng)伺服周期的波動(dòng)主要在±2μs之間，且最大值不超過(guò)9μs。通過(guò)圖5、6可以看出，各關(guān)節(jié)可快速準(zhǔn)確地跟蹤期望位置曲線(xiàn)，關(guān)節(jié)9的最大跟蹤誤差為0.000 2rad，是期望位置的0.48‰；關(guān)節(jié)10的最大跟蹤誤差為0.000 1rad，是期望位置的0.98‰，兩個(gè)關(guān)節(jié)的動(dòng)態(tài)跟蹤誤差均較小。通過(guò)該實(shí)驗(yàn)，驗(yàn)證了所設(shè)計(jì)的運(yùn)動(dòng)控制系統(tǒng)的有效性。

4 結(jié)束語(yǔ)

針對(duì)托卡馬克第一壁高穩(wěn)定、高安全的檢測(cè)需要，借助于QNX RTOS優(yōu)異的實(shí)時(shí)特性、EtherCAT高效的數(shù)據(jù)傳輸機(jī)制和分布式時(shí)鐘高精度的同步策略，設(shè)計(jì)并實(shí)現(xiàn)了一種實(shí)時(shí)運(yùn)動(dòng)控制系統(tǒng)。該系統(tǒng)具備傳輸數(shù)據(jù)量大、實(shí)時(shí)性高、連線(xiàn)簡(jiǎn)單的特點(diǎn)。通過(guò)實(shí)驗(yàn)，證明了該系統(tǒng)具備微秒級(jí)的實(shí)時(shí)特性，且能實(shí)現(xiàn)快速精確的控制。