面向托卡馬克第一壁檢測的機械臂實時運動控制系統設計與實現

屈云飛 陳衛東

(上海交通大學自動化系系統控制與信息處理教育部重點實驗室,上海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高效的數據傳輸機制和分布式時鐘高精度的同步策略，設計并實現了一種實時運動控制系統。該系統具備傳輸數據量大、實時性高、連線簡單的特點。通過實驗，證明了該系統具備微秒級的實時特性，且能實現快速精確的控制。