基于EtherCAT總線的人形機器人控制系統設計

余秋蕾，張崇峰，陳 萌，韓亮亮(.華中科技大學機械科學與工程學院，武漢430074；.上海宇航系統工程研究所，上海008)

?

基于EtherCAT總線的人形機器人控制系統設計

余秋蕾1，張崇峰2，陳 萌2，韓亮亮2
(1.華中科技大學機械科學與工程學院，武漢430074；2.上海宇航系統工程研究所，上海201108)

摘要:針對傳統人形機器人控制系統存在的軟硬件模塊化程度低、通訊非實時、控制功能有限的問題，在分析空間站對人形機器人控制系統功能需求的基礎上，以四核嵌入式控制器為操作平臺，搭建了基于EtherCAT總線的人形機器人分布式實驗控制系統，實現了機器人任務規劃與決策、數據管理和系統通信等功能，具有靈活高效的網絡拓撲結構、模塊化的軟件架構。實驗表明，該系統保證了信號采集的實時性和可靠性，控制的穩定性和高精度。

關鍵詞:人形機器人；EtherCAT總線；分布式控制系統；四核控制器

1　引言

我國載人航天工程任務向更高階段發展，對空間操控能力的提高也提出進一步需求。利用空間機器人協助航天員完成載荷及設備更換、平臺巡檢與養護以及出艙協同作業等艙內外任務將成為未來機器人應用的重要方向。人形機器人作為具備人類的外形特征和行動能力的智能機器人[1]，在與航天員進行交互過程中，它類人的外形能夠使航天員在交互環境中更舒適。人形機器人系統屬于非線性、高耦合系統，在協助航天員完成復雜的艙內外任務時，動力學效應多變，機器人關節脆弱、不穩定，運動過程中存在協調控制等問題[2]，進行機器人控制系統設計時，不僅要滿足控制系統的普適性能，提供靈活、方便的操作方式，以減輕航天員的操作負擔，對控制系統響應的快速性、實時性和操作的精度、安全可靠性也提出更高要求。

人形機器人的研究起步于1960年代后期[3]，美國是世界上研究機器人航天員最早的國家，目前已經積累了豐富的技術能力和應用經驗，2011年其研發的機器人航天員R2已進入太空站，主要進行一系列微重力條件下的功能測試工作[3]。日本研發的首個能夠進行人機交流的機器人航天員Kirobo于2013年11月開始在太空進行人機對話試驗[3]。俄羅斯研發的人形機器人航天員SAR-401能夠執行比R2更復雜的精細操作，它不是完全自動的，而是由工作人員在地面遠程遙控執行任務[4-6]。它們均采用分布式控制系統，結合多種交互設備進行控制。

我國在仿人機器人方面也做了大量研究，其中部分是進行雙足仿人機器人的研究，但實現的動作和功能十分有限，自主性不足，軟硬件模塊化程度低，或者系統各層次之間采用非實時通訊總線、串口方式進行通信，系統達不到較高的實時性和可靠性要求[7-8]，且應用于空間操作的人形機器人系統研究較少。

為此，本文以四核嵌入式控制器為運行平臺，搭建基于EtherCAT總線的空間人形機器人分布式實驗控制系統，以降低底層硬件驅動程序的開發難度，為軟件總體框架的設計提供良好的通信、控制思路，控制算法采用模塊化設計以簡化重用代碼的編寫，節約系統資源。通過TwinCAT/ C ++編程結合Windows非實時操作系統以實現系統的實時控制，縮短系統的控制周期。

2　控制系統總體設計

2. 1 系統功能設計

考慮空間工作環境的特殊性，控制系統的具體功能分析如下:

1)靈巧、高效的作業任務能力。人形機器人系統應具備開展通用化、高精度和高靈活性任務的能力，其工作空間或可達范圍與航天員相當，能夠輔助航天員完成或者自主完成艙內周期性的活動。

2)多樣化人機交互方式。航天員可通過多種人機交互方式與人形機器人進行自然、直接的交互。

3)實時穩定的通信方案。系統能夠提供高效實時的通信方式，且易于進行網絡結構拓撲。

4)安全可靠的控制器。為應對惡劣的空間環境，機器人的控制器應該保證具有較高的可靠性和安全性，提供多種控制模式接口。

根據上述功能要求進行設計，人形機器人控制系統結構如圖1所示。系統采用基于Ether-CAT總線的分布式控制方式，以四核嵌入式PC為控制器操作平臺，實時性高，易于拓展應用，各模塊之間通過接口進行連接實現主從關系或平行關系。冗余自由度的雙臂保證機器人有足夠的操作空間，左臂末端安裝的可變工具集完成規則對象的操作，右臂末端的五指靈巧手完成螺釘旋擰、電連接器拆裝等任務，腰頸關節進一步擴大機器人的工作范圍。

圖1　控制系統框圖Fig. 1 Block diagram of the control system

從系統框圖可以看出，中央控制器是人形機器人控制系統的核心模塊，本文將主要針對中央控制器模塊的軟件設計進行分析與驗證。

2. 2 系統組成

基于EtherCAT總線的人形機器人系統是由遙操作子系統、中央控制子系統和操作環境等組成的天地半自主服務體系。其中，中央控制子系統又包括中央控制器和驅動執行機構。系統結構如圖2所示。

遙操作子系統的基本構成為上位機和多種人機交互設備，其中，上位機包括生成指令的遙操作計算機與視頻圖像處理的視覺計算機；中央控制子系統控制器采用倍福公司提供的四核控制器，該控制器基于Intel CoreTMi7處理器，主頻2. 1 GHZ。作為中央控制子系統的核心組成，它主要完成指令解析與高速率的數據處理，并將處理后的指令數據定時發送至下位機；驅動執行機構是由兩個7自由度手臂、2自由度腰部、3自由度頸部以及裝有雙目視覺相機和深度相機的機器人構成。

圖2　控制系統組成Fig. 2 Composition of the control system

3　軟件設計

中央控制器作為控制系統的核心，對系統的實施起決定性作用，也是系統優化的主要功能模塊，需要考慮與上位機、下位機的通信，機器人各模塊的任務規劃及分配、協調控制以及模塊間數據交換的安全策略等問題。

在前人研究的基礎上，本文采用一種新的機器人控制器平臺-倍福四核嵌入式計算機，結合一種新的軟件編寫平臺-TwinCAT3實現多個模塊的協調任務操作，結合高速實時現場總線降低底層驅動軟件開發難度，提高網絡拓撲靈活性。

中央控制器軟件總體框架如圖3所示。由控制器中單獨一個核作為指令接收與解析單位，便于集中控制；為保證雙臂能夠進行協同作業，分別在其各自核中進行任務規劃與設計。頭腰協同要求較低、控制算法簡單，共用一個核進行控制。在TwinCAT3平臺下，各核分別執行三個任務，每個任務固定一個時間片，即控制周期運行，核之間通過共享內存進行數據交換。

圖3　軟件總體設計框架Fig. 3 General design frame of the software

從指令發送-機器人任務執行-關節數據反饋全過程進行分析，對應軟件設計框圖中的順序，中央控制器軟件模塊主要實現六個功能:

1)通過與上位機進行通訊接收控制指令，并進行數據幀的解析；

2)根據解析的數據進行任務分解和模式控制，即確定采用哪種模式進行控制以及采用哪種路徑規劃算法；

3)為實現雙臂、雙足、頭頸部、腰部單個模塊的控制和多個模塊協調控制任務，系統需要進行單腿/單臂路徑規劃算法、雙腿/雙臂運動協調算法、力柔順控制算法、頭腰協調控制算法等的算法設計；

4)通過現場總線將步驟三中解算出的控制指令分別發送到對應的關節控制器中；

5)獲取雙臂、雙足、頭頸部、腰部各關節的傳感器信息，包括位置、電流、模擬量等信息，并進行信息的管理，返回給中央控制器；

6)中央控制器通過與上位機通訊，將組幀后的反饋信息發送至遙操作端。

從以上分析可以看出，上位機與中央控制器、中央控制器與下位機的通訊是指令數據接收、解析、發送以及反饋的前提條件，任務經過分解及模式識別后，各模塊的控制算法是下發控制指令的核心。因此，通訊模塊及控制算法的設計即為軟件設計的重點。

3. 1 通信拓撲結構

人形機器人控制系統的通信拓撲結構如圖4所示。

圖4　控制系統通信拓撲圖Fig. 4 Communication topology of the control system

本文主要采用ADS協議和EtherCAT總線進行通信。上位機和中央控制器之間采用基于TCP/ IP的ADS協議進行非實時通訊，該協議對Windows Socket類進行了再封裝，支持定時、通知、異步等多種傳輸模式；下位機與中央控制器端、中央控制器與視覺控制器之間采用EtherCAT總線進行實時通訊。EtherCAT具有高速和高效率的傳輸特性，支持多種設備連接拓撲結構，正因如此，本文的控制系統采用分布式設計的方法，形成星形拓撲結構。該總線從站節點使用專門的控制芯片，主站使用標準的以太網控制器。Ether-CAT刷新周期短，可以達到小于100 μs的數據刷新周期，并且各從站設備可以達到小于1 μs的時鐘同步精度[9-11]。

在TwinCAT3軟件平臺下，支持EtherCAT總線的設備可直接通過控制器進行掃描，完成設備的添加、刪除操作，進行網絡拓撲結構的更改；通過修改EtherCAT總線協議的對象字典進行與底層設備的網絡連接和對底層設備驅動控制。

3. 2 控制算法設計

為了保證精度和穩定性，在進行任務操作過程中，本文結合路徑規劃、柔順控制、視覺伺服等多種算法實現機器人多臂化操作，在TwinCAT平臺下，對控制算法進行封裝操作，生成可模塊化調用的靜態庫。控制算法的框圖如圖5所示。

對于兩自由度腰部、三自由度頸部，若想到達笛卡爾空間內某一位置，僅存在唯一一組解，對于七自由度冗余左右臂，則存在無數組解使得機械臂到達笛卡爾空間內指定位置。在執行任務中，主要執行機構為左右臂，輔助機構為腰部和頸部，手臂七自由度反解是本文控制算法設計的基礎。

機器人在執行任務的過程中，會與操作環境發生接觸，左右手臂之間可能會產生接觸，較小的位置偏差可能產生較大的接觸力，若不加以控制，會對機器人和末端接觸物體造成損壞，甚至可能會導致系統崩潰[12]，力控制在人形機器人控制系統中也是一項非常重要的任務。

因此，本文主要進行七自由度反解和力控制算法的研究。

圖5　控制算法框圖Fig. 5 Block diagram of the control algorithm

3. 2. 1 七自由度反解

目前，進行七自由度逆運動學求解的方法主要有解析法和迭代法兩種，當各關節無限位時，兩種方法求解效果差別不大，但解析法能夠更快的排除無效解，縮短計算時間，減小空間復雜度[13]。

本文采用基于臂形角的參數化方法進行各關節角的求解，依次求出肘部、腕部和肩部關節的角度。軟件控制流程如圖6所示。

臂形角可以確定機械臂的基本構型，實際求解過程中，臂形角是一定范圍內的變量，既保證了左右臂在關節限位條件下有更多可行較優解，又避免了由于限位導致的逆解突變，保證了逆解的連續性。

3. 2. 2 力控制算法

關節力矩控制模式復雜，須各關節扭矩傳感器采集數據作為檢測力/力矩是否超限的判據，保證機械臂處于安全工作狀態，主要采用末端的六維力傳感器進行阻抗控制。阻抗控制的特點是不直接控制機器人與環境的作用力，而是通過控制力和位置之間的動態關系來實現柔順功能[14]。首先進行重力補償，補償后若末端力/力矩超過設定的閾值時，則進行力/力矩控制，然后將力/力矩轉換為末端位姿修正值，進行位置閉環控制。阻抗控制的動態關系方程[14]表示為式(1):

式中: Md為期望的慣性質量，Bd為期望的阻尼系數，Kd為期望的剛度系數，T為末端各方向力/力矩，xr為實際的末端各方向位置/姿態，xd為期望的末端各方向位置/姿態。

基于阻抗控制的人形機器人控制器主要有兩種工作模式:手把手示教工作模式與過程中柔順控制模式。兩種工作模式的軟件控制流程圖如圖7(a)、(b)所示。

圖6　七自由度逆解流程圖Fig. 6 Flow chart of the seven DOF inverse

圖7　力控制流程圖Fig. 7 Flow chart of the force control

手把手示教模式即指機器人在一個給定位置進入力控制模式，操作人員直接引領機器人手臂末端，中央控制器采集末端六維力傳感器信息，將操作者所施加力/力矩轉化為相應的位置/姿態大小發送給底層驅動器，從而驅動各關節電機運動，整個手臂沿操作人員操作方向運動。該種模式能夠實現手動對準任務，不需要借助三維模型的測量即可得出理想操作位置的笛卡爾空間表示，經過一定訓練后，并結合濾波算法，能夠完成路徑復現功能。

過程中柔順控制模式即指在機器人任務操作過程中，控制器實時地進行六維力信息的檢測，同時進行位置修正，自動調整操作過程中力/力矩大小，防止其超過閾值對機器人甚至系統造成損傷，當檢測到的力/力矩數據超過一定值時，機器人自動執行緊急停止操作。在這種控制模式下，機器人能夠自動適應環境，避開任務過程中出現的障礙物，若此時障礙物對其產生的作用力過大，機器人啟動自動報警功能，提醒操作人員關閉電源，并完成自身緊急停止任務。

4　實驗平臺搭建與結果分析

4. 1 實驗平臺搭建

基于本文前三節的設計思想，進行實驗平臺的搭建以及軟件實現，如圖8所示。圖8(a)所示為地面驗證實驗平臺，由人機交互設備、上位機、控制計算機、下位機、電源模塊及模擬操作面板等組成。圖8(b)為人機交互界面，左中右依次為指令發送、數據反饋、虛擬仿真界面。圖8(c)為中央控制器設置各核任務執行周期及優先級的界面。

4. 2 結果分析

底層設備掃描周期為T =1 ms，在圖8(c)中的TwinCAT3平臺下設置控制周期C為T的整數倍，當C =2 ms時，機器人運動穩定，當C＜2 ms時，數據反饋鏈路阻塞，控制出現中斷，設置控制系統的周期為2 ms。在控制穩定的前提下，驗證運動學及動力學算法的有效性，以插孔操作為例，結合路徑規劃和阻抗控制算法完成圓孔的柔順插拔，其中，操作圓孔固定在圖8(a)所示機器人正前方黑色操作面板上。圓孔的直徑為4 cm，為保證效果明顯，在實驗過程中給定末端y方向1 cm的位置偏差，即目標點與圓孔中心在水平方向有1 cm的偏差，圖9所示為末端六維力傳感器在柔順插拔過程中采集的各方向的力、力矩大小和末端y方向的位置，圖9(d)為規劃的末端x、z方向位置與運動過程中采集的末端x、z實際位置之間的差值，即末端跟蹤誤差。由圖9(a)～(c)可知，當末端力/力矩超過閾值時，阻抗控制可以有效修正偏差，使末端所受力/力矩保持在有效范圍內，并且使末端到達規劃目標位置，由圖9(d)可知，末端位置跟蹤誤差控制在1. 5 mm以內，一定周期后誤差逐漸減少至接近為零，控制器實現了對機械臂的實時控制。

圖8　實驗平臺及軟件界面Fig. 8 Experimental platform and software interface

5　結論

在分析空間站對人形機器人控制系統功能需求的基礎上，本文完成了基于EtherCAT總線的人形機器人控制系統設計，進行了實驗分析與驗證，得出以下結論:

1)EtherCAT總線通信的方式將系統總的控制周期縮短至2 ms，保證信號采集的實時性和控制的穩定性。

2)在柔順控制模式下，基于位置的阻抗控制算法能夠將機械臂與操作對象的接觸力修正到一定范圍內，保證控制的高精度，且避免了運動過程中較大接觸力對機械臂產生的損壞。同時，通過調整阻抗控制參數，實現了機器人手把手示教功能，節省了模型測量時間。

圖9　實驗采集數據信息Fig. 9 The experimental data information

參考文獻(References)

[ 1 ] 虞漢中，馮雪梅.人形機器人技術的發展與現狀[J].機械工程師，2010(7): 3-6. Yu H Z，Feng M X. Development and actuality of humanoid robot technology[J]. Mechanical Engineer，2010(7): 3-6. (in Chinese)

[ 2 ] 陳光偉.仿人機器人控制系統設計與穩定性研究[D].成都:西華大學，2010. Chen G Y. Study on Control System Design and Stability of Humanoid Robot[D]. Chengdu: Northwest university，2010. (in Chinese)

[ 3 ] 劉宏，蔣再男，劉業超.空間機械臂技術發展綜述[J].載人航天，2015，21(5): 435-443. Liu H，Jiang Z N，Liu Y C. Review of space manipulator technology[J]. Manned Spaceflight，2015，21(5): 435-443 (in Chinese).

[ 4 ] Tsay T I J，Lai C H. Design and control of a humanoid robot [C] / / Intelligent Robots and Systems，2006 IEEE/ RSJ International Conference on. IEEE，2006: 2002-2007.

[ 5 ] Taira T，Kamata N，Yamasaki N. Design and implementation of reconfigurable modular humanoid robot architecture[C]/ / Intelligent Robots and Systems，2005. (IROS 2005). 2005 IEEE/ RSJ International Conference on. IEEE，2005: 3566-3571.

[ 6 ] Qi J，Wang L，Jia H，et al. Design and performance evaluation of networked data acquisition systems based on EtherCAT [C] / / Information Management and Engineering (ICIME)，2010 The 2nd IEEE International Conference on. IEEE，2010: 467-469.

[ 7 ] 鄭嫦娥，錢樺.仿人機器人國內外研究動態[J].機床與液壓，2006(3): 1-4. Zheng C E，Qian H. The domestic and international research situation of humanoid robot[J]. Machine tool&Hydraulics，2006(3):1-4. (in Chinese)

[ 8 ] 劉莉，汪勁松，陳懇，等. THBIP-1擬人機器人研究進展[J].機器人，2002，24(3): 262-267. Liu L，Wang J S，Chen K，et al. The research on the biped humanoid robot THBIP-1 [J]. Robot，2002，24 (3):262-267. (in Chinese)

[ 9 ] 郇極，劉艷強.工業以太網現場總線EtherCAT驅動程序設計及應用[M].北京:北京航空航天大學出版社，2012: 3-4. Xun J. Liu Y Q. EtherCAT-Industrial Ethernet[M]. Beijing: Beijing University of Aeronautics and Astronautics Press，2012:2-4. (in Chinese)

[10] 蔡林海，蔣曉春，趙國亮，等.基于EtherCAT總線技術的鏈式STATCOM控制系統[J].電力建設，2012，33(8): 27-30. Cai L H，Jiang X C，Zhao G L，et al. Chain STATCOM control system based on EtherCAT bus technology[J]. Electric Power Construction，2012，33(8):27-30. (in Chinese)

[11] 張楨，趙君，劉衛華，等. EtherCAT總線分布式多電機控制研究[J].電子科技，2014，27(6):106-112. Zhang Z，Zhao J，Liu W H，et al. Research on distributed control system of multi-motor based on EtherCAT bus[J]. E-lectronic Technology，2014，27(6):106-112. (in Chinese)

[12] 徐文福，周瑞興，孟得山.空間機器人在軌更換ORU的力/位混合控制方法[J].宇航學報，2013，34 (10): 1353-1361. Xu W F，Zhou R X，Meng D S. A hybrid Force/ Position Control Method of Space Robot Performing On-Orbit ORU Replacement[ J]. Acta Astronautica，2013，34 (10): 1353-1361. (in Chinese)

[13] Zhang Z，Xu D，Yu J. Research and latest development of ping-pong robot player[C] / / Intelligent Control and Automation，2008. WCICA 2008. 7th World Congress on. IEEE，2008: 4881-4886.

[14] JUNG S，HSIA T C，BONITZ R G. Force tracking impedance control of robot manipulators under unknown environment[J]. IEEE Transaction Control Systems Technology，2004，12 (3): 474-483.

Control System Design for Humanoid Robot Based on EtherCAT Bus Technology

YU Qiulei1，ZHANG Chongfeng2，CHEN Meng2，HAN Liangliang2
(1. School of MechanicalScience& Engineering of HUST，Wuhan 430074，China；2. Shanghai Aerospace System Engineering Research Institute，Shanghai 201108，China)

Abstract:The control system of the traditional humanoid robot has many problems such as low degree of modularity in software and hardware，non-real-time communication and limited control functions. On the basis of analyzing the function requirements for humanoid robot system in the space station，a distributed control system based on EtherCAT bus for humanoid robot was put forward. It adopted the quad-core embedded controller as its operating platform and achieved the following functions: robot mission planning and decision-making，data management and system communication. The experiment results showed that the system could not only guarantee the real-time and reliability of the signal acquisition process，but also ensure the stability and high precision of the control system.

Key words:humanoid robot；EtherCAT bus；distributed control system；quad-core controller

作者簡介:余秋蕾(1991 - )，女，碩士，助理研究員，研究方向為機器人控制。E-mail:yuqiulei@ hust. edu. cn

基金項目:載人航天預先研究項目(050101)

收稿日期:2015-09-26；修回日期:2015-12-16

中圖分類號:TP39

文獻標識碼:A

文章編號:1674-5825(2016)01-0016-07