月面仿人機器人實時控制系統設計

姜朝峰，莫小波，朱秋國，3，吳 俊，3，熊 蓉，3

(1.浙江大學智能系統與控制研究所，杭州310027；2.杭州云深處科技有限公司，杭州310027；3.浙江大學工業控制技術國家重點實驗室，杭州310027)

1 引言

月球的表面覆蓋著環山、月海、月陸和山脈等[1]，所以月面移動機器人必須有較強的月面地形適應能力和避障越障能力。月面移動機器人主要有輪式機器人[2-4]、腿足式機器人[5-7]、模塊化可重構機器人[8]，其中適應地形能力最強的就是腿足式機器人。仿人機器人為腿足式機器人的最典型代表，在復雜環境中的通行和作業具有較大優勢，可以在月面復雜地形穩定移動并執行科研任務。仿人機器人可以代替航天員執行有危險性的載人登月任務，如太空行走和出艙維修；可以替代人類在月球基地進行大規模適應性測試，檢驗人造環境是否適合人類生存與活動。各種為人類準備的設備和工具可以由仿人機器人直接操作使用，不必重復設計。仿人機器人在月球運動的關節受力、姿態數據可以為低重力環境對人類健康可能造成的傷害評估提供支撐，并做出預防措施。這些都是其他智能設備或機器人無法取代的。

目前，月面仿人機器人的研究與開發較少。Baker等[9]研制的R2機器人基于機器人操作系統ROS研發，為航天員提供修理輔助，雙足主要起固定作用；Radford等[10]研制的R5仿人機器人也基于ROS，使用了4塊高性能主控板保證并發控制計算的實時性，成本和空間要求較高；Kuehn等[11]研制的黑猩猩月球探測機器人的通信和控制頻率最高為 100 Hz，通信總帶寬為50 kbps，無法滿足實時控制的性能要求。

仿人機器人基于腿足交替運動而移動，雖然具有優良的地形適應能力，但其運動過程是一個復雜非穩態過程，微小的控制偏差可能導致整個機器人運動失衡而摔倒。針對外太空復雜崎嶇地面，仿人機器人必須具備快速檢測自身和環境狀態，以及快速做出反應的能力。因此，高速率、高帶寬和高穩定的實時性是月面仿人機器人必備的條件。

本文將開展月面仿人機器人控制系統的實時性能分析和實時系統設計，構建QNX實時操作系統和以太網控制自動化技術(Ethernet for Control Automation Technology，EtherCAT)的實時仿人機器人控制系統。通過對線程運算耗時進行試驗，建立實時調度策略，優化線程調度，從底層到應用層實現整體實時性。

2 仿人機器人控制系統

WUKONG-II仿人機器人是針對復雜環境下的快速運動而研發的雙足機器人，見圖1[12]。機器人身高1.5 m，重42 kg，安裝有足底力傳感器和IMU傳感器，腿部總共10個自由度，包括左右髖俯仰、髖滾動和髖偏轉3個自由度、膝俯仰關節1自由度、腳踝俯仰關節1自由度。

仿人機器人運動控制系統的響應速度具體表現為仿人機器人運動控制系統的控制周期。控制周期越長，運動控制指令時間間隔越長，系統純滯后越大，對于外界擾動的反應也就越慢；控制周期越短，運動控制指令時間間隔越短，系統純滯后越小，機器人能夠對外界擾動更快做出響應[13]。

針對仿人機器人WUKONG-II驅動器數量多、對通信速率和準確性要求高的特點，為機器人的運動控制主控機選擇運行QNX操作系統，通信接口選擇EtherCAT工業以太網協議棧。基于QNX操作系統和EtherCAT實時以太網，主控機網口與支持EtherCAT協議的電機驅動器相連，通過運行過程中的數據交互實現對關節電機的實時控制。WUKONG-II的控制系統硬件結構框圖見圖2。

機器人上還配置了無線網卡，可以通過WiFi信號與上位機進行無線通訊，獲取上位機的控制指令。機器人電源模塊為控制器及驅動器供電，并配置有急停開關控制驅動器的急停信號。

為了評估機器人控制系統的性能，接下來將從操作系統、通信總線、控制框架3個方面進行試驗測試與分析。

3 QNX操作系統與實時性

實時是指信號的輸入、運算和輸出都要在極短的時間內完成，并根據過程的變化及時地進行處理[14]。實時性和快速性并不等價，不論處理器的運算處理和網絡數據傳輸速度有多快，只要在規定的響應時間內發生響應動作并進行處理，則稱系統具有實時性。

圖2 WUKONG-II控制硬件結構框圖Fig.2 Block diagram of control hardware structure in WUKONG-II

QNX是一個多任務、多用戶、分布、可嵌入、符合POSIX標準的嵌入式實時操作系統，由微內核和若干負責系統管理的共操作進程組成，各個管理器與內核之間互相隔離，具有出色的容錯性[15]。為驗證QNX操作系統是否滿足仿人機器人對實時性能的要求，將根據以下指標對QNX系統進行測試評估：

1)上下文切換時間。上下文就是進程執行時的外部變量、環境和數據，包括所有的寄存器變量、進程打開的文件、內存信息等。上下文切換就是保存一個進程任務的上下文狀態并運行或恢復另一個進程的上下文，以便該程序執行。

2)中斷延遲時間。通常中斷的優先級高于普通任務，所以當有中斷產生時，正在執行的任務上下文會被保存，轉而由中斷函數執行，中斷延遲時間就是指中斷產生到中斷函數第一條指令開始運行的時間差。如果中斷事件不夠及時，可能產生嚴重后果，所以中斷延遲時間是操作系統實時性的一個重要指標。

3)時鐘精度。實時時鐘為操作系統提供了計時標準，它的作用為提供系統時間、為任務時間片提供基準、更新資源消耗和處理器時間統計值等，時鐘的誤差會被操作系統累積而放大。

試驗測試使用的主控機型號為Kontron KEEX-5106工控機，CPU為Intel Broadwell Core i7-5650U，內存為8G DDR3L。對機器人控制主控機安裝不同的操作系統，分別對上下文切換時間、中斷延遲時間等指標進行多次測試，并獲取平均值。

3.1 上下文切換時間試驗

試驗目的是測試上下文切換這一過程的最長時間開銷。試驗方法是創建高、低不同優先級的2個實時進程TaskHigh和TaskLow，并創建一個定時器。先讓高優先級進程TaskHigh試圖獲取低優先級進程TaskLow的消息隊列而掛起等待；然后 TaskLow開始運行，并通過消息隊列向TaskHigh發送消息，讓系統計時器開始計時并得到喚醒高優先級任務的起始時間點t1，然后后延時等待；TaskHigh會收到消息，并由等待狀態進入就緒狀態，由CPU調度到內核進行運行，在等到消息的代碼之后執行定時器的另一次計時輸出，記錄此時的時間點t2。兩次計時時間之差即為上下文切換時間，t(switch)=t2-t1。重復10次，取中位數。測試結果見表1。

表1 操作系統上下文切換時間試驗結果Table 1 Test results of context switching time

結果表明，QNX的上下文切換時間開銷非常小，可以滿足仿人機器人系統的實時切換要求。這一優點得益于其微內核、高優先級任務搶占式的特性。相比之下，Linux Ubuntu 16.04切換時間較長，而Windows 10上下文切換時間更是QNX的十幾倍。

3.2 中斷響應時間試驗

試驗目的是測試系統調度延遲。操作系統對一個中斷的響應，其實包括了產生中斷、調度器進行調度、實現上下文切換3個過程，所以中斷響應時間=中斷延遲+調度器調度延遲+上下文切換延遲。具體試驗方法和上下文切換時間測試類似，即先讓一個任務執行，并創建一個定時器，初值設為t1，當定時器減為0時，進入中斷服務程序，同時定時器會自動變為初值t1；當切換到中斷函數執行時，讓中斷函數立即調用時間函數記錄定時器時間t2，則t(interrupt)=t1-t2。重復10次，取中位數。測試結果見表2。

表2 操作系統中斷響應時間試驗結果Table 2 Test results of interrupt response time

試驗結果表明，相比起 Linux和 Windows，QNX的中斷響應時間明顯小很多，僅有11.30μs，幾乎只是 Windows 10 的 1/3，Linux Ubuntu 16.04的2/3。這足以證明QNX操作系統的小巧和快速。

3.3 時鐘精度

實時時鐘為周期控制提供了基本依據，準確的時間周期對腿足機器人控制實時性非常重要。Windows、Linux和QNX均提供高精度系統時鐘，所以在相同的Core i7 CPU上，時鐘精度最高均能到達相同的納秒級，但系統獲取時間的接口會帶來定時器誤差。試驗目的是測試系統時鐘誤差，試驗原理是每隔1 ms，通過IO操作程序向主板的GPIO輸出一次方波，用示波器觀察波形的實際時間間隔，和1 ms的設定定時周期進行做差比較，重復10次，取中位數。測試結果見表3。

表3 操作系統時鐘精度試驗結果Table 3 Test results of clock time accuracy

上述對上下文切換時間、中斷響應時間和時鐘精度的測試結果表明，作為最廣泛使用的桌面用戶操作系統和服務計算系統，Windows和Linux為了更廣泛的用戶交互友好和兼容性、提高CPU有效利用率，其內核任務調度算法更傾向于公平的循環調度和非實時搶占，而犧牲了實時與快速響應性能。QNX操作系統則具有良好的實時性能，在不犧牲CPU密集型任務的情況下，保證了IO密集型任務的反應能力，能良好地保證仿人機器人控制系統的底層快速響應，可以作為月面仿人機器人控制的實時性操作系統。

4 EtherCAT以太網總線與實時性

EtherCAT是德國Beckhoff公司最早提出的開放式實時以太網，其高速性能確定了實時以太網性能的新標準[16]。EtherCAT的主站為標準的以太網卡，從站為專用芯片，采用主從介質訪問模式，通信過程中由主站控制所有從站的收發，它可在30μs內通過雙絞線或光纖電纜處理1000個數字量 I/O，全網可連接設備多達 65 535臺。EtherCAT技術使得總線系統不再是控制理念的瓶頸，通訊技術開始可以和高性能的工業計算機相匹配[17]。

由于其他以太網總線在每個連接點接收以太網數據包，然后解碼并復制到過程數據存儲區，而EtherCAT報文在設備的持續傳送過程中，就由每個I/O端子中的現場總線存儲管理單元FMMU讀出報文數據。同樣，輸入數據可以在報文通過時插入報文中，報文僅有幾納秒延遲。通過專門的芯片硬件完成收發。因此，EtherCAT突破了其它以太網解決方案的系統收發數據高延遲問題，實現了其它的以太網解決方案不能達到的實時能力。因此，可以讓仿人機器人各電機驅動器、傳感器和控制器之間的控制信號傳輸速度產生巨大的提升。

連接好11個電機驅動器，通過EtherCAT以太網周期地獲取API讀取電機控制通信周期，每隔5 ms讀取一次并記錄數據。測試EtherCAT總線通信周期時間和每個周期的抖動。每個周期讀取和寫入的數據內容如表4所示。

表4 輸入/輸出數據參數和長度Table 4 Input/output data parameters and length

每個電機需要0xE0 bit的數據傳輸量，11個電機在250μs的周期內共傳輸0xE0×11=154 bit，則通信帶寬為154 bit÷0.000 25 s=616 000 bit/s≈0.62 Mbps。對于EtherCAT標稱的百兆甚至千兆帶寬，即使再加上幀頭、幀尾、標志符、控制幀等其他幀封裝數據，也完全滿足通信需求。

設定EtherCAT通信周期為250μs，每隔5 ms(即20個通信周期)利用EtherCAT協議棧提供的API接口讀取一次數據，獲取平均通信周期、最大通信周期、最小通信周期和平均抖動并寫入記錄，數據見圖3。

圖3 Ether CAT通信周期圖Fig.3 Diagram of communication cycle of Ether CAT

根據試驗結果可知，平均通信周期一直非常穩定地保持在249μs，這是機器人系統通信實時性和穩定性的保證。但同時每20個周期內，最小和最大通信周期都和平均值發生了一定的偏離。這在通信網絡中無法避免，-25～+35μs的抖動在控制系統可接受范圍之內。

5 控制系統程序框架

基于QNX實時操作系統和EtherCAT工業以太網，為仿人機器人WUKONG-II搭建了控制系統程序框架。為了提高數據分發速度，減少進程間通信帶來的數據傳輸延遲，該程序框架在一個進程中實現，并在進程中創建了其他6個線程，并行實現電機控制、外設監聽、IMU和足底力傳感器數據讀取、過程數據記錄、手臂舵機控制和外部通信等仿人機器人需要的功能。

5.1 線程功能模塊

線程1是主進程本身所在的線程。負責創建并啟動其他功能線程，并在程序結束時注銷其他線程，把記錄數據存入硬盤，釋放內存，結束進程。

線程2是Motor(電機)模塊。通過EtherCAT的數據鏡像，將電機驅動器這一周期的目標控制參數寫入，并將當前周期的實際狀態參數讀取，通過姿態控制算法計算得到下一周期的目標數據，等待下一周期再次寫入數據。

線程3是Sensor(傳感器)模塊。通過2個串口分別讀取力傳感器和IMU的上傳數據。力傳感器和IMU都采用主動上傳協議模式，即腳底力傳感器模塊和IMU模塊每采集一個完整的新數據包，就主動上傳到主控板。

線程4是Listener(監聽)模塊。循環查詢鍵盤或手柄的按鍵指令中斷，當收到外部設備的遠程按鍵指令就修改機器人狀態，使機器人從一個狀態切換到另一個狀態，實現不同的運動模式。

線程5是Recorder(記錄)模塊。每個控制周期都將電機實時位置、速度、力矩、控制參數、狀態值等重要數據放到內存空間，等待主線程結束后把內存中的記錄寫入硬盤，為測試后的分析保留數據。

線程6是Arm(手臂控制)模塊。根據雙足姿態調整手臂運動狀態，并將生成的手臂運動姿態位置值通過Ethernet網口發送到舵機，實現手和腳的協同動作。

線程7是 Head(感知與通信)模塊。通過Ethernet網口和 UDP/IP協議，獲取“WUKONGII”感知系統的反饋信息，針對不同的感知、路徑規劃結果，做出不同的運動策略。

5.2 傳感器數據采集模塊

為了實現仿人機器人的可靠穩定行走，需要IMU傳感器和足底力傳感器提供姿態和狀態信息。WUKONG-II在腹部位置安裝了IMU模塊，設定數據采集周期為5 ms，在腳底安裝了4個足底力傳感器模塊，設定數據采集周期為5 ms。在各自的數據采集線程中一直輪詢式讀取數據，每完成一次數據包讀取，記錄時間間隔，數據記錄見圖4。

圖4 數據采集周期-運行時間圖Fig.4 Diagram of data collection cycle vs.runtime

由圖4可知，數據采集模塊實際周期時間的方差較大，伴隨著隨機誤差。IMU模塊和腳底力傳感器模塊的實際讀取周期都在5 ms左右，符合設定值。其中，IMU的讀取周期方差小，腳底力傳感器的讀取周期方差更大。

5.3 控制算法模塊

仿人機器人的雙足步行模式使得機器人易受到各類模型誤差與環境擾動的影響，導致機器人失衡而摔倒，較短的控制周期使仿人機器人能夠根據傳感器信息針對各類擾動及時進行反饋補償與矯正，保持機器人穩定性[13]。

系統控制周期是指整個程序完成算法計算、電機控制、數據采集、數據記錄等所有任務所需的時間，控制算法周期是指其中控制算法的運行時間。控制算法周期的波動很大且沒有規律，這是由于機器人在不同的狀態下，選擇的算法模式不同，計算量不同，因此算法運行時間一直在改變。而且機器人控制算法要時刻根據傳感數據反饋進入不同的模式，因此難以預測下一時刻的系統狀態。為了給算法的運行留出足夠的CPU計算時間，系統周期必須大于其中最大的控制算法周期。測試時間內，算法運行時間集中在250μs以內，所以給系統周期設定為1000μs可以滿足算法的時間要求。分別使用系統定時器函數記錄控制系統和控制算法計算的始、終時間，并做差，得到算法周期和系統周期，見圖5。

圖5 系統周期和算法周期圖Fig.5 Diagram of system cycle and algorithm cycle

根據3.2可知，當計時器計時滿1000μs產生系統中斷后，中斷響應仍然需要一定時間。因此，實際周期會略大于設定周期值。由圖5可知，設定系統周期為1000μs，大多數實際周期誤差都在3μs以內。但會出現1250μs的異常值，這是由于獲取系統控制周期時間的函數精度為250μs、細粒度不夠導致。

最終，在WUKONG-II上開展了雙足跑步試驗，并實現了雙足機器人室外操場和平整路面的快速小跑，最快速度達到3.24 km/h，可以適用于月面不平整路面的較快速移動。試驗證明，本文所開發的控制系統在仿人機器人WUKONG-II上具有良好的實時性，確保了機器人控制與運行的穩定性，也為以后的航天員-仿人機器人人機協同、航天員遠程運動操控等更多智能任務的開發提供實時控制基礎。

6 結論

1)在相同的硬件條件下，QNX6.6操作系統的上下文切換時間、中斷響應時間明顯短于Windows 10和Linux Ubuntu 16.04，時鐘精度高于這兩者，具有良好的硬實時性能；

2)在0.62 Mbps的有效數據帶寬下，Ether-CAT的通信周期可以穩定在250μs附近，并且抖動較小，具有高帶寬、低時延、小抖動的特點；

3)在IMU數據采集周期和足底力傳感器數據采集周期為5 ms的基礎上，設計的多線程并發程序框架可以保證控制周期穩定在1000μs、時間抖動在可接受范圍內，并在仿人機器人WUKONG-II上實現了3.24 km/h的平地快速移動，證明了該實現電機控制、多傳感器數據采集、手臂運動、數據記錄、外部通信功能的程序框架的實時有效性。