面向托卡馬克第一壁檢測的機(jī)械臂實(shí)時運(yùn)動控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

屈云飛 陳衛(wèi)東

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

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

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

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

1 系統(tǒng)硬件①

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

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

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

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

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

2 系統(tǒng)軟件

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

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

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

2.1 EtherCAT Master模塊

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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