基于TwinCAT3的一種開源機器人控制系統設計

黃睿, 張新*, 朱華炳, 張濤, 曾億山

(1.安徽理工大學機械工程學院, 淮南 232001; 2.合肥工業大學機械工程學院, 合肥 230031)

當前,機器人已成為數字化經濟時代最標志性的智能工具,推動了自動化技術、集成化技術的快速發展[1]。如工業機器人、健康醫療機器人等不同類型的機器人被廣泛應用于各行業中,機器人技術已然是新時代科學技術發展的重要方向。

控制系統是機器人的核心,其運行狀況的好壞與系統密不可分。目前市場上主流的工業機器人所配套的控制系統是封閉式結構[2],外部人員只能在其固有的軟件平臺上進行二次開發。即使不乏有學者基于ROS(robot operating system)[3]、嵌入式[4]等系統開發機器人,但ROS開發周期長、嵌入式系統開發難度大,借助其他系統平臺[5-6]也難以在短期內開發出高性能與高實時通信的機器人控制系統。

不少研究人員開始采用EtherCAT(ethernet for control automation technology)技術[7-8],旨在利用實時以太網通信開發機器人控制系統。Ahn等[9]通過搭建雙通道的EtherCAT主站,成功研發出具有高實時性和高穩定性的33自由度仿人機器人控制系統。Zhao等[10]基于EtherCAT通信網絡,設計的雙足輪機器人可以實現高頻力控制和良好的反向驅動能力。周惠興等[11]將Beckhoff嵌入式PC計算機(personal computer, PC)作為上位機,以EtherCAT為通信基礎,研究出的機器人控制系統可實時、準確完成瓷磚鋪貼。周楷文等[12]基于PC控制,以 ARM嵌入式處理器(advanced RISC machine, ARM)為系統核心搭建了EtherCAT主站,結合MATLAB軟件平臺設計出的協作機器人控制系統可實現高實時性的數據反饋,但產品的升級換代會因設備的不兼容而影響二次開發。鄧劍楠等[13]基于ROS和EtherCAT研制了一種智能抹平機器人的多軸控制系統,其機器人控制的實時性可取代人工作業,但ROS開發難度較大,延長了開發周期。這些基于EtherCAT開發的控制系統使得機器人具有不錯地穩定性和實時性,但目前并沒有見到一種易開發的,同時兼顧移植性、拓展性和實時性的開放式機器人控制系統。

為促進、完善開放式機器人控制系統的發展[14],設計一種跨平臺的開源機器人控制系統架構。以倍福TwinCAT3為控制系統的軟件開發平臺基礎,綜合PyCharm、MATLAB、CoppeliaSim等軟件平臺,以EtherCAT與ADS (automation device specification)通信為核心,采用一主多從的工作模式。同時結合視覺和運動控制,借助數字孿生技術,建立虛擬模型與機器人本體之間的聯動[15],并通過離線仿真和在線控制實驗分析,以期驗證該系統架構設計的可行性。該跨平臺開源機器人控制系統架構的研究推動了開源機器人控制系統的進步,為未來機器人技術的發展和應用具有深刻的指導和參考價值。

1 機器人控制系統架構設計

以實驗室自研的五自由度輕型工業機器人為例(圖1),設計了一種基于TwinCAT3 (the windows control and automation technology)的開源機器人控制系統架構,包括軟件和硬件。

圖1 五自由度機器人Fig.1 Five-degree-of-freedom robot

在軟件方面,選擇開源算法和軟件平臺開發機器人的視覺檢測與運動控制,如PyCharm、MATLAB、TwinCAT3等,可以縮短開發周期,降低成本。其中,以嵌入TwinCAT3的Visual Studio 2017為核心開發環境,使用Python、C/C++、ST等語言編寫程序,同時基于EtherCAT實時工業以太網總線技術和ADS通信協議進行數據傳輸,滿足系統實時控制的需求。在硬件方面,工業相機選擇大恒圖像的水星二代系列,該系列相機具有可二次開發的軟件開發包SDK,圖像數據傳輸穩定、快速;運動控制器選擇德國Beckhoff公司的CX系列,該系列控制器基于PC控制,憑借PC強大的計算能力,在最大限度控制255個軸時仍能高效作業;關節模組選擇泰科的RJSIIZ系列,該系列模組集成化程度高,一體化結構極大地簡化了機器人本體,而且支持EtherCAT通信協議;伺服驅動器亦選擇支持EtherCAT通信協議的松下A6SE系列,保證了整體系統通信的協同性與兼容性。

如圖2 所示,系統采用模塊化設計的思想,主要劃分為視覺模塊、運動控制模塊、算法集成與仿真控制模塊,分別負責機器人的視覺檢測、運動控制、相關算法集成與人機交互界面的設計。其中,算法集成與仿真控制模塊于PC端開發,包括各開源軟件平臺,具有集成視覺與運動控制算法、仿真機器人運動和設計上位機人機交互界面的功能;視覺模塊包括工業相機和PC,具有圖像采集和處理的功能。運動控制模塊關鍵是EtherCAT主站和從站(節點或設備)的搭建,PC與倍福控制器為主站,從站設備包括RJSIIZ14/17關節模組和松下伺服驅動器,該模塊具有發送運動控制指令和接收視覺模塊與各從站設備傳感器數據信息的功能。當五自由度機器人運動時,機器人CoppeliaSim數字孿生模型與實物虛實交互,同時人機交互界面實時顯示機器人各關節位姿信息。以這種系統架構的設計方案實現機器人控制,可以隨時對機器人運動狀態進行監控并調整。

2 相關模塊設計

2.1 算法集成與仿真控制模塊

算法集成與仿真控制模塊包括上位機軟件和下位機軟件,如圖3所示。

TwinCAT3為倍福自動化控制軟件;Visual Studio 2017為核心開發環境;MATLAB為計算機軟件;CoppeliaSim為機器人仿真軟件;Qt/Pyqt5為人機交互界面開發軟件;PyCharm為Python語言開發的集成開發環境圖3 算法集成與仿真控制模塊Fig.3 Algorithm integration and simulation control module

TwinCAT3是五自由度機器人算法程序設計和開發的核心編程環境,上位機軟件集成的視覺和運動控制模塊算法在該下位機軟件系統中交互數據。視覺方面,PyCharm集成視覺目標檢測算法,通過Pyqt5封裝并設計視覺檢測人機交互界面,如圖4所示;運動控制方面,MATLAB集成機器人的運動學控制和軌跡規劃算法(如多項式插值軌跡規劃等),CoppeliaSim完成對機器人數字孿生模型仿真運動,并聯合Qt設計機器人人機交互界面,如圖5所示。

圖4 目標檢測人機交互界面Fig.4 Object detection human-computer interface

2.2 視覺模塊

視覺模塊以TwinCAT3軟件系統為數據傳輸媒介與運動控制模塊進行數據交互,借助該軟件系統中的ADS模塊將視覺圖像數據輸送至EtherCAT主站。

2.2.1 ADS通信

ADS是TwinCAT3軟件系統中各模塊之間通信的消息路由,其采用的工作模式為服務器/客戶端(server/client);是一種訪問ADS設備的接口控制通信協議[16]。協議規定ADS設備的唯一標識為用于確定設備硬件的ADS網絡ID和用于確定軟件服務的端口,任何ADS通信的請求必須指定ID和端口,才能找到對應的服務器。TwinCAT ADS模塊數據訪問的方法有兩種,一種是訪問TwinCAT PLC變量的地址,變量地址由分組索引和偏移地址組成,這種方法先訪問變量地址,再訪問變量;另一種是直接訪問PLC變量名,TwinCAT PLC程序中的任意變量都有唯一的句柄,該方法需要先獲取變量句柄,變量讀寫完成后,釋放句柄。

2.2.2 基于ADS的視覺模塊設計

為提高機器人運動精度,減少如抓取、焊接任務時的定位誤差,視覺模塊采用開源深度學習框架Pytorch搭建視覺檢測算法的網絡模型,利用強大的GPU硬件快速、精準識別出目標2D坐標,通過立體校正與匹配獲取目標3D位姿,最后封裝至Pyqt5中。該模塊基于ADS通信,調用倍福提供的TcAdsDll庫構建通信功能,以直接訪問變量名的方式,建立上位機軟件(Pyqt5)控制接口的變量與下位機軟件(TwinCAT3)PLC變量之間一一對應的關系,完成圖像數據的傳輸。具體涉及的變量接口及通信流程如圖6所示。

首先,PC端完成Pyqt5與TwinCAT3 PLC通信地址和端口的配對;其次,打開ADS通信,將Pyqt5封裝的算法輸出接口變量名與PLC變量名完成對應變量鏈接;最后,同步傳輸視覺圖像數據。視覺圖像數據傳輸主要分為兩部分,第一部分是將圖像數據輸送至Pyqt5人機交互界面中視覺檢測算法輸入變量的控制接口,算法處理后將數據結果存儲在輸出變量的控制接口,隨后由客戶端(Pyqt5)向服務器(TwinCAT3 PLC)發送請求;第二部分是服務器接受請求,打開通信端口,獲取對應PLC變量句柄,從客戶端中同步讀取圖像數據結果并寫數據到PLC變量中,以便后續運動控制模塊接收該圖像數據信息,最后釋放句柄關閉端口,等待下一次請求。

2.3 運動控制模塊

運動控制模塊是五自由度機器人控制系統的關鍵部分,為實現對機器人的快速、實時控制,該模塊所選的控制器、關節模組、伺服驅動器均支持EtherCAT通信協議。

2.3.1 EtherCAT通信

在倍福基于PC和EtherCAT的控制技術中, EtherCAT通信是核心技術[17]。是一種用于過程數據的優化協議技術,解決了傳統專用交換機分配以太網包存在嚴重時間延遲的問題。該技術無需接受以太網數據包,可以直接解碼數據包并復制數據到每個從站設備,而后從站設備接收并處理數據。系統數據傳輸時間延遲只有幾納秒,加快和便捷了主站控制器與其他外部從站設備之間的通信,足以滿足控制系統高實時性的需求而不受時間延遲的限制。

2.3.2 基于EtherCAT的運動控制模塊設計

在機器人運動時,各從站設備的運動控制指令來自倍福控制器,機器人到達指定位置并完成特定任務所需的位姿通過各關節模組、伺服驅動電機輸出的電壓電流信號轉換而來,而這些數據信號的傳輸由EtherCAT技術支撐。

如圖7所示,機器人運動控制模塊以TwinCAT3和倍福控制器為EtherCAT主站控制器,PC端集成有MATLAB在Robotics Toolbox中開發的運動控制算法、Qt設計的機器人人機交互界面,以及CoppeliaSim數字孿生技術實時顯示機器人運動軌跡(位移、速度和加速度等數據變化曲線)的仿真控制。

TCP/IP為網絡通信協議;RemoteAPI為CoppeliaSim軟件API框架的遠程通信接口函數;Robotics Toolbox為機器人工具箱;C++ module為C++模塊圖7 運動控制模塊通信流程Fig.7 Motion control module communication process

首先,基于TCP/IP協議,借助Remote API函數分別建立CoppeliaSim與Qt、MATLAB之間的通信,通過TwinCAT3 C++ Module調用MATLAB封裝的運動控制算法;其次,完成TwinCAT3與倍福控制器之間的Ethernet連接;最后,基于EtherCAT協議建立倍福控制器與機器人本體中各從站設備之間的通信。當機器人進行如關節空間、笛卡爾空間的軌跡規劃時,軌跡規劃算法生成各關節所有插值點數據,由EtherCAT主站一邊發送該控制指令(各關節值)至CoppeliaSim中控制機器人仿真模型運動,同時運動仿真模型的位姿信息實時反饋至Qt人機交互界面;另一邊控制指令經過數據處理后由EtherCAT發送給各從站設備,控制機器人本體按規劃的軌跡進行運動。此外,各關節模組中的編碼器等傳感器接收、處理機器人與外界的數據信息并反饋給EtherCAT主站,等待主站控制器的下一次指令。

3 離線仿真與在線控制

五自由度機器人控制系統按功能模式分為離線仿真和在線控制。視覺在系統模式切換中,始終為機器人提供目標3D位姿,輔助其精準定位。如圖8所示,綠色虛線框為離線仿真模式,視覺僅為機器人仿真提供目標位姿,完成機器人軌跡規劃;紫色虛線框為在線控制模式,視覺提供的目標位姿需在TwinCAT3設計的視覺伺服控制器中進行處理,獲取機器人末端與目標之間的誤差位姿,完成誤差位姿變量與調用MATLAB運動控制算法的C++ Module、TwinCAT PLC之間的變量鏈接,等待機器人運動的控制指令。

在機器人系統控制中,離線仿真主要包括軌跡規劃和仿真,視覺提供目標位姿、MATLAB進行軌跡規劃和CoppeliaSim完成機器人模型仿真,篩選出安全、高效的運動軌跡策略;在線控制主要包括仿真和通信,選擇合適的離線運動策略,由主站向伺服驅動器、各關節模組發出控制指令,驅動機器人本體沿期望的軌跡路線運動,同時接收各從站設備反饋的位移(d為直線滑臺的水平位移)、角度(θ1、θ2、θ3、θ4為4個旋轉關節的旋轉角度)。調用CoppeliaSim仿真,實時顯示機器人的運動狀態,通過視覺伺服控制器不斷補償機器人末端位姿與目標之間的位姿誤差,最終協助機器人到達指定位置。

4 實驗驗證

為了驗證本開源機器人控制系統設計的可行性和有效性,在圖1所示的五自由度機械臂上進行實驗并討論。

圖9為實現的機器人離線仿真。通過視覺給定的目標位姿,采用笛卡爾空間直線軌跡規劃生成各關節所有插值點的數據,借助CoppeliaSim仿真控制機器人模型各關節運動到目標位置。分別記錄了機器人數字孿生模型在不同時刻末端的位移和速度曲線變化,可以明顯地觀察到機器人末端的運動軌跡為一條直線,同時位移和速度曲線沒有發生突變和離散,驗證了在離線仿真模式下,機器人軌跡規劃的正確性。

圖10為實現的機器人在線控制。根據離線仿真的軌跡規劃策略,在線控制機器人樣機按規劃的軌跡做直線運動實驗,根據視覺與從站設備反饋的數據信息實時調整機器人各關節角度θ(或d)。分別記錄機器人實物在不同時刻的運動軌跡曲線,觀察到機器人實物與CoppeliaSim仿真中機器人數字孿生模型(圖9)各關節的位姿一致,驗證了機器人在線控制的可行性,同時驗證了機器人控制系統的高實時性能。

T為機器人運動的不同時刻圖10 在線控制Fig.10 Online control

5 結論

針對主流工業機器人為封閉式控制結構的問題,設計一種基于TwinCAT3的開源機器人控制系統架構,既能拓展控制系統的功能,又能支持二次開發。通過上述系統架構的設計,得出以下結論。

(1)系統采用開源可擴展架構,包含視覺、運動控制和算法集成與仿真控制模塊,以ADS、EtherCAT通信技術傳輸數據。

(2)視覺模塊借助開源深度學習算法視覺算法實現精準定位;運動控制模塊建立了一主多從工作模式;算法集成與仿真控制模塊集成視覺、運動控制等智能算法,設計了目標檢測和機器人人機交互界面。

(3)系統結合數字孿生技術,通過離線與在線控制完成了仿真模型與實物之間虛實交互的聯動,優化運動策略,提高控制精度。驗證了該系統架構的可拓展性好且實時性強。