月面仿人機(jī)器人實(shí)時(shí)控制系統(tǒng)設(shè)計(jì)

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

(1.浙江大學(xué)智能系統(tǒng)與控制研究所，杭州310027；2.杭州云深處科技有限公司，杭州310027；3.浙江大學(xué)工業(yè)控制技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，杭州310027)

1 引言

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

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

仿人機(jī)器人基于腿足交替運(yùn)動(dòng)而移動(dòng)，雖然具有優(yōu)良的地形適應(yīng)能力，但其運(yùn)動(dòng)過程是一個(gè)復(fù)雜非穩(wěn)態(tài)過程，微小的控制偏差可能導(dǎo)致整個(gè)機(jī)器人運(yùn)動(dòng)失衡而摔倒。針對外太空復(fù)雜崎嶇地面，仿人機(jī)器人必須具備快速檢測自身和環(huán)境狀態(tài)，以及快速做出反應(yīng)的能力。因此，高速率、高帶寬和高穩(wěn)定的實(shí)時(shí)性是月面仿人機(jī)器人必備的條件。

本文將開展月面仿人機(jī)器人控制系統(tǒng)的實(shí)時(shí)性能分析和實(shí)時(shí)系統(tǒng)設(shè)計(jì)，構(gòu)建QNX實(shí)時(shí)操作系統(tǒng)和以太網(wǎng)控制自動(dòng)化技術(shù)(Ethernet for Control Automation Technology，EtherCAT)的實(shí)時(shí)仿人機(jī)器人控制系統(tǒng)。通過對線程運(yùn)算耗時(shí)進(jìn)行試驗(yàn)，建立實(shí)時(shí)調(diào)度策略，優(yōu)化線程調(diào)度，從底層到應(yīng)用層實(shí)現(xiàn)整體實(shí)時(shí)性。

2 仿人機(jī)器人控制系統(tǒng)

WUKONG-II仿人機(jī)器人是針對復(fù)雜環(huán)境下的快速運(yùn)動(dòng)而研發(fā)的雙足機(jī)器人，見圖1[12]。機(jī)器人身高1.5 m，重42 kg，安裝有足底力傳感器和IMU傳感器，腿部總共10個(gè)自由度，包括左右髖俯仰、髖滾動(dòng)和髖偏轉(zhuǎn)3個(gè)自由度、膝俯仰關(guān)節(jié)1自由度、腳踝俯仰關(guān)節(jié)1自由度。

仿人機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的響應(yīng)速度具體表現(xiàn)為仿人機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的控制周期?？刂浦芷谠介L，運(yùn)動(dòng)控制指令時(shí)間間隔越長，系統(tǒng)純滯后越大，對于外界擾動(dòng)的反應(yīng)也就越慢；控制周期越短，運(yùn)動(dòng)控制指令時(shí)間間隔越短，系統(tǒng)純滯后越小，機(jī)器人能夠?qū)ν饨鐢_動(dòng)更快做出響應(yīng)[13]。

針對仿人機(jī)器人WUKONG-II驅(qū)動(dòng)器數(shù)量多、對通信速率和準(zhǔn)確性要求高的特點(diǎn)，為機(jī)器人的運(yùn)動(dòng)控制主控機(jī)選擇運(yùn)行QNX操作系統(tǒng)，通信接口選擇EtherCAT工業(yè)以太網(wǎng)協(xié)議棧?；赒NX操作系統(tǒng)和EtherCAT實(shí)時(shí)以太網(wǎng)，主控機(jī)網(wǎng)口與支持EtherCAT協(xié)議的電機(jī)驅(qū)動(dòng)器相連，通過運(yùn)行過程中的數(shù)據(jù)交互實(shí)現(xiàn)對關(guān)節(jié)電機(jī)的實(shí)時(shí)控制。WUKONG-II的控制系統(tǒng)硬件結(jié)構(gòu)框圖見圖2。

機(jī)器人上還配置了無線網(wǎng)卡，可以通過WiFi信號(hào)與上位機(jī)進(jìn)行無線通訊，獲取上位機(jī)的控制指令。機(jī)器人電源模塊為控制器及驅(qū)動(dòng)器供電，并配置有急停開關(guān)控制驅(qū)動(dòng)器的急停信號(hào)。

為了評(píng)估機(jī)器人控制系統(tǒng)的性能，接下來將從操作系統(tǒng)、通信總線、控制框架3個(gè)方面進(jìn)行試驗(yàn)測試與分析。

3 QNX操作系統(tǒng)與實(shí)時(shí)性

實(shí)時(shí)是指信號(hào)的輸入、運(yùn)算和輸出都要在極短的時(shí)間內(nèi)完成，并根據(jù)過程的變化及時(shí)地進(jìn)行處理[14]。實(shí)時(shí)性和快速性并不等價(jià)，不論處理器的運(yùn)算處理和網(wǎng)絡(luò)數(shù)據(jù)傳輸速度有多快，只要在規(guī)定的響應(yīng)時(shí)間內(nèi)發(fā)生響應(yīng)動(dòng)作并進(jìn)行處理，則稱系統(tǒng)具有實(shí)時(shí)性。

圖2 WUKONG-II控制硬件結(jié)構(gòu)框圖Fig.2 Block diagram of control hardware structure in WUKONG-II

QNX是一個(gè)多任務(wù)、多用戶、分布、可嵌入、符合POSIX標(biāo)準(zhǔn)的嵌入式實(shí)時(shí)操作系統(tǒng)，由微內(nèi)核和若干負(fù)責(zé)系統(tǒng)管理的共操作進(jìn)程組成，各個(gè)管理器與內(nèi)核之間互相隔離，具有出色的容錯(cuò)性[15]。為驗(yàn)證QNX操作系統(tǒng)是否滿足仿人機(jī)器人對實(shí)時(shí)性能的要求，將根據(jù)以下指標(biāo)對QNX系統(tǒng)進(jìn)行測試評(píng)估：

1)上下文切換時(shí)間。上下文就是進(jìn)程執(zhí)行時(shí)的外部變量、環(huán)境和數(shù)據(jù)，包括所有的寄存器變量、進(jìn)程打開的文件、內(nèi)存信息等。上下文切換就是保存一個(gè)進(jìn)程任務(wù)的上下文狀態(tài)并運(yùn)行或恢復(fù)另一個(gè)進(jìn)程的上下文，以便該程序執(zhí)行。

2)中斷延遲時(shí)間。通常中斷的優(yōu)先級(jí)高于普通任務(wù)，所以當(dāng)有中斷產(chǎn)生時(shí)，正在執(zhí)行的任務(wù)上下文會(huì)被保存，轉(zhuǎn)而由中斷函數(shù)執(zhí)行，中斷延遲時(shí)間就是指中斷產(chǎn)生到中斷函數(shù)第一條指令開始運(yùn)行的時(shí)間差。如果中斷事件不夠及時(shí)，可能產(chǎn)生嚴(yán)重后果，所以中斷延遲時(shí)間是操作系統(tǒng)實(shí)時(shí)性的一個(gè)重要指標(biāo)。

3)時(shí)鐘精度。實(shí)時(shí)時(shí)鐘為操作系統(tǒng)提供了計(jì)時(shí)標(biāo)準(zhǔn)，它的作用為提供系統(tǒng)時(shí)間、為任務(wù)時(shí)間片提供基準(zhǔn)、更新資源消耗和處理器時(shí)間統(tǒng)計(jì)值等，時(shí)鐘的誤差會(huì)被操作系統(tǒng)累積而放大。

試驗(yàn)測試使用的主控機(jī)型號(hào)為Kontron KEEX-5106工控機(jī)，CPU為Intel Broadwell Core i7-5650U，內(nèi)存為8G DDR3L。對機(jī)器人控制主控機(jī)安裝不同的操作系統(tǒng)，分別對上下文切換時(shí)間、中斷延遲時(shí)間等指標(biāo)進(jìn)行多次測試，并獲取平均值。

3.1 上下文切換時(shí)間試驗(yàn)

試驗(yàn)?zāi)康氖菧y試上下文切換這一過程的最長時(shí)間開銷。試驗(yàn)方法是創(chuàng)建高、低不同優(yōu)先級(jí)的2個(gè)實(shí)時(shí)進(jìn)程TaskHigh和TaskLow，并創(chuàng)建一個(gè)定時(shí)器。先讓高優(yōu)先級(jí)進(jìn)程TaskHigh試圖獲取低優(yōu)先級(jí)進(jìn)程TaskLow的消息隊(duì)列而掛起等待；然后 TaskLow開始運(yùn)行，并通過消息隊(duì)列向TaskHigh發(fā)送消息，讓系統(tǒng)計(jì)時(shí)器開始計(jì)時(shí)并得到喚醒高優(yōu)先級(jí)任務(wù)的起始時(shí)間點(diǎn)t1，然后后延時(shí)等待；TaskHigh會(huì)收到消息，并由等待狀態(tài)進(jìn)入就緒狀態(tài)，由CPU調(diào)度到內(nèi)核進(jìn)行運(yùn)行，在等到消息的代碼之后執(zhí)行定時(shí)器的另一次計(jì)時(shí)輸出，記錄此時(shí)的時(shí)間點(diǎn)t2。兩次計(jì)時(shí)時(shí)間之差即為上下文切換時(shí)間，t(switch)=t2-t1。重復(fù)10次，取中位數(shù)。測試結(jié)果見表1。

表1 操作系統(tǒng)上下文切換時(shí)間試驗(yàn)結(jié)果Table 1 Test results of context switching time

結(jié)果表明，QNX的上下文切換時(shí)間開銷非常小，可以滿足仿人機(jī)器人系統(tǒng)的實(shí)時(shí)切換要求。這一優(yōu)點(diǎn)得益于其微內(nèi)核、高優(yōu)先級(jí)任務(wù)搶占式的特性。相比之下，Linux Ubuntu 16.04切換時(shí)間較長，而Windows 10上下文切換時(shí)間更是QNX的十幾倍。

3.2 中斷響應(yīng)時(shí)間試驗(yàn)

試驗(yàn)?zāi)康氖菧y試系統(tǒng)調(diào)度延遲。操作系統(tǒng)對一個(gè)中斷的響應(yīng)，其實(shí)包括了產(chǎn)生中斷、調(diào)度器進(jìn)行調(diào)度、實(shí)現(xiàn)上下文切換3個(gè)過程，所以中斷響應(yīng)時(shí)間=中斷延遲+調(diào)度器調(diào)度延遲+上下文切換延遲。具體試驗(yàn)方法和上下文切換時(shí)間測試類似，即先讓一個(gè)任務(wù)執(zhí)行，并創(chuàng)建一個(gè)定時(shí)器，初值設(shè)為t1，當(dāng)定時(shí)器減為0時(shí)，進(jìn)入中斷服務(wù)程序，同時(shí)定時(shí)器會(huì)自動(dòng)變?yōu)槌踔祎1；當(dāng)切換到中斷函數(shù)執(zhí)行時(shí)，讓中斷函數(shù)立即調(diào)用時(shí)間函數(shù)記錄定時(shí)器時(shí)間t2，則t(interrupt)=t1-t2。重復(fù)10次，取中位數(shù)。測試結(jié)果見表2。

表2 操作系統(tǒng)中斷響應(yīng)時(shí)間試驗(yàn)結(jié)果Table 2 Test results of interrupt response time

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

3.3 時(shí)鐘精度

實(shí)時(shí)時(shí)鐘為周期控制提供了基本依據(jù)，準(zhǔn)確的時(shí)間周期對腿足機(jī)器人控制實(shí)時(shí)性非常重要。Windows、Linux和QNX均提供高精度系統(tǒng)時(shí)鐘，所以在相同的Core i7 CPU上，時(shí)鐘精度最高均能到達(dá)相同的納秒級(jí)，但系統(tǒng)獲取時(shí)間的接口會(huì)帶來定時(shí)器誤差。試驗(yàn)?zāi)康氖菧y試系統(tǒng)時(shí)鐘誤差，試驗(yàn)原理是每隔1 ms，通過IO操作程序向主板的GPIO輸出一次方波，用示波器觀察波形的實(shí)際時(shí)間間隔，和1 ms的設(shè)定定時(shí)周期進(jìn)行做差比較，重復(fù)10次，取中位數(shù)。測試結(jié)果見表3。

表3 操作系統(tǒng)時(shí)鐘精度試驗(yàn)結(jié)果Table 3 Test results of clock time accuracy

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

4 EtherCAT以太網(wǎng)總線與實(shí)時(shí)性

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

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

連接好11個(gè)電機(jī)驅(qū)動(dòng)器，通過EtherCAT以太網(wǎng)周期地獲取API讀取電機(jī)控制通信周期，每隔5 ms讀取一次并記錄數(shù)據(jù)。測試EtherCAT總線通信周期時(shí)間和每個(gè)周期的抖動(dòng)。每個(gè)周期讀取和寫入的數(shù)據(jù)內(nèi)容如表4所示。

表4 輸入/輸出數(shù)據(jù)參數(shù)和長度Table 4 Input/output data parameters and length

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

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

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

根據(jù)試驗(yàn)結(jié)果可知，平均通信周期一直非常穩(wěn)定地保持在249μs，這是機(jī)器人系統(tǒng)通信實(shí)時(shí)性和穩(wěn)定性的保證。但同時(shí)每20個(gè)周期內(nèi)，最小和最大通信周期都和平均值發(fā)生了一定的偏離。這在通信網(wǎng)絡(luò)中無法避免，-25～+35μs的抖動(dòng)在控制系統(tǒng)可接受范圍之內(nèi)。

5 控制系統(tǒng)程序框架

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

5.1 線程功能模塊

線程1是主進(jìn)程本身所在的線程。負(fù)責(zé)創(chuàng)建并啟動(dòng)其他功能線程，并在程序結(jié)束時(shí)注銷其他線程，把記錄數(shù)據(jù)存入硬盤，釋放內(nèi)存，結(jié)束進(jìn)程。

線程2是Motor(電機(jī))模塊。通過EtherCAT的數(shù)據(jù)鏡像，將電機(jī)驅(qū)動(dòng)器這一周期的目標(biāo)控制參數(shù)寫入，并將當(dāng)前周期的實(shí)際狀態(tài)參數(shù)讀取，通過姿態(tài)控制算法計(jì)算得到下一周期的目標(biāo)數(shù)據(jù)，等待下一周期再次寫入數(shù)據(jù)。

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

線程4是Listener(監(jiān)聽)模塊。循環(huán)查詢鍵盤或手柄的按鍵指令中斷，當(dāng)收到外部設(shè)備的遠(yuǎn)程按鍵指令就修改機(jī)器人狀態(tài)，使機(jī)器人從一個(gè)狀態(tài)切換到另一個(gè)狀態(tài)，實(shí)現(xiàn)不同的運(yùn)動(dòng)模式。

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

線程6是Arm(手臂控制)模塊。根據(jù)雙足姿態(tài)調(diào)整手臂運(yùn)動(dòng)狀態(tài)，并將生成的手臂運(yùn)動(dòng)姿態(tài)位置值通過Ethernet網(wǎng)口發(fā)送到舵機(jī)，實(shí)現(xiàn)手和腳的協(xié)同動(dòng)作。

線程7是 Head(感知與通信)模塊。通過Ethernet網(wǎng)口和 UDP/IP協(xié)議，獲取“WUKONGII”感知系統(tǒng)的反饋信息，針對不同的感知、路徑規(guī)劃結(jié)果，做出不同的運(yùn)動(dòng)策略。

5.2 傳感器數(shù)據(jù)采集模塊

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

圖4 數(shù)據(jù)采集周期-運(yùn)行時(shí)間圖Fig.4 Diagram of data collection cycle vs.runtime

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

5.3 控制算法模塊

仿人機(jī)器人的雙足步行模式使得機(jī)器人易受到各類模型誤差與環(huán)境擾動(dòng)的影響，導(dǎo)致機(jī)器人失衡而摔倒，較短的控制周期使仿人機(jī)器人能夠根據(jù)傳感器信息針對各類擾動(dòng)及時(shí)進(jìn)行反饋補(bǔ)償與矯正，保持機(jī)器人穩(wěn)定性[13]。

系統(tǒng)控制周期是指整個(gè)程序完成算法計(jì)算、電機(jī)控制、數(shù)據(jù)采集、數(shù)據(jù)記錄等所有任務(wù)所需的時(shí)間，控制算法周期是指其中控制算法的運(yùn)行時(shí)間?？刂扑惴ㄖ芷诘牟▌?dòng)很大且沒有規(guī)律，這是由于機(jī)器人在不同的狀態(tài)下，選擇的算法模式不同，計(jì)算量不同，因此算法運(yùn)行時(shí)間一直在改變。而且機(jī)器人控制算法要時(shí)刻根據(jù)傳感數(shù)據(jù)反饋進(jìn)入不同的模式，因此難以預(yù)測下一時(shí)刻的系統(tǒng)狀態(tài)。為了給算法的運(yùn)行留出足夠的CPU計(jì)算時(shí)間，系統(tǒng)周期必須大于其中最大的控制算法周期。測試時(shí)間內(nèi)，算法運(yùn)行時(shí)間集中在250μs以內(nèi)，所以給系統(tǒng)周期設(shè)定為1000μs可以滿足算法的時(shí)間要求。分別使用系統(tǒng)定時(shí)器函數(shù)記錄控制系統(tǒng)和控制算法計(jì)算的始、終時(shí)間，并做差，得到算法周期和系統(tǒng)周期，見圖5。

圖5 系統(tǒng)周期和算法周期圖Fig.5 Diagram of system cycle and algorithm cycle

根據(jù)3.2可知，當(dāng)計(jì)時(shí)器計(jì)時(shí)滿1000μs產(chǎn)生系統(tǒng)中斷后，中斷響應(yīng)仍然需要一定時(shí)間。因此，實(shí)際周期會(huì)略大于設(shè)定周期值。由圖5可知，設(shè)定系統(tǒng)周期為1000μs，大多數(shù)實(shí)際周期誤差都在3μs以內(nèi)。但會(huì)出現(xiàn)1250μs的異常值，這是由于獲取系統(tǒng)控制周期時(shí)間的函數(shù)精度為250μs、細(xì)粒度不夠?qū)е隆?/p>

最終，在WUKONG-II上開展了雙足跑步試驗(yàn)，并實(shí)現(xiàn)了雙足機(jī)器人室外操場和平整路面的快速小跑，最快速度達(dá)到3.24 km/h，可以適用于月面不平整路面的較快速移動(dòng)。試驗(yàn)證明，本文所開發(fā)的控制系統(tǒng)在仿人機(jī)器人WUKONG-II上具有良好的實(shí)時(shí)性，確保了機(jī)器人控制與運(yùn)行的穩(wěn)定性，也為以后的航天員-仿人機(jī)器人人機(jī)協(xié)同、航天員遠(yuǎn)程運(yùn)動(dòng)操控等更多智能任務(wù)的開發(fā)提供實(shí)時(shí)控制基礎(chǔ)。

6 結(jié)論

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

2)在0.62 Mbps的有效數(shù)據(jù)帶寬下，Ether-CAT的通信周期可以穩(wěn)定在250μs附近，并且抖動(dòng)較小，具有高帶寬、低時(shí)延、小抖動(dòng)的特點(diǎn)；

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