機器人實時遠(yuǎn)程控制系統(tǒng)及其開發(fā)庫的設(shè)計與實現(xiàn)

近年來,隨著無線網(wǎng)絡(luò)技術(shù)的發(fā)展和日益成熟,利用wlan和lan的混和組網(wǎng),使基于網(wǎng)絡(luò)的機器人遠(yuǎn)程控制成為一個熱門的研究領(lǐng)域。

本文設(shè)計了一套移動機器人遠(yuǎn)程控制系統(tǒng)。該系統(tǒng)可用于如時延網(wǎng)絡(luò)環(huán)境下控制系統(tǒng)的穩(wěn)定性理論、實時控制系統(tǒng)的網(wǎng)絡(luò)通信協(xié)議、基于事件驅(qū)動方式的網(wǎng)絡(luò)系統(tǒng)控制策略、目標(biāo)識別技術(shù)等方面的研究。

1 硬件環(huán)境

被控的機器人采用了美國activmedia robotics公司的p3-dx多功能智能移動機器人。它配備了車載計算機(裝有無線網(wǎng)卡、視頻采集卡)、帶抓手的5自由度手臂、水平轉(zhuǎn)動/傾斜攝像頭及4組共16個聲納。

在實驗室內(nèi)搭建了無線網(wǎng)絡(luò)環(huán)境, 無線路由器通過其廣域連接端口連接到校園網(wǎng),并能通過校園網(wǎng)訪問internet,從而構(gòu)成了一個廣域網(wǎng)系統(tǒng)。機器人通過其車載計算機上的無線網(wǎng)卡與實驗室內(nèi)的無線局域網(wǎng)互聯(lián),從網(wǎng)絡(luò)的任何節(jié)點都可以遠(yuǎn)程控制機器人。

2 機器人實時遠(yuǎn)程控制系統(tǒng)的設(shè)計

本機器人遠(yuǎn)程控制系統(tǒng)以實時性及可靠性為前提進(jìn)行設(shè)計。

2.1 基本結(jié)構(gòu)

如圖1所示,整個遠(yuǎn)程控制系統(tǒng)由遠(yuǎn)程控制平臺、本地控制系統(tǒng)兩部分組成。整套軟件系統(tǒng)建立在windows2000上。

如圖2左半部分所示,整個遠(yuǎn)程控制平臺分為控制區(qū)和狀態(tài)反饋區(qū)。在控制區(qū),用戶通過圖形化控制接口觸發(fā)指令發(fā)送模塊中的相應(yīng)指令,并按照命令包的協(xié)議進(jìn)行封包(根據(jù)所設(shè)計的通信協(xié)議,可以選擇是否進(jìn)行加密和壓縮),通過網(wǎng)絡(luò)發(fā)送給本地控制系統(tǒng)。

狀態(tài)反饋區(qū)負(fù)責(zé)接收從本地控制系統(tǒng)實時發(fā)送過來的機器人狀態(tài)反饋信息和視頻數(shù)據(jù)并顯示。

2.3 本地控制系統(tǒng)

如圖2右半部分所示,整個本地控制系統(tǒng)分為控制執(zhí)行區(qū)和反饋發(fā)送區(qū)。

在控制執(zhí)行區(qū),當(dāng)收到命令包并解包后,可以選擇進(jìn)行命令包的實時性檢查,然后通過指令轉(zhuǎn)發(fā)模塊將指令轉(zhuǎn)發(fā):如果是基本的控制指令則直接通過aria接口發(fā)送給機器人;如果是高級任務(wù)指令則轉(zhuǎn)發(fā)給高級任務(wù)模塊,調(diào)用高級任務(wù)模塊中對應(yīng)的子模塊進(jìn)行控制,由它發(fā)送一系列的命令給機器人。

在反饋發(fā)送區(qū),系統(tǒng)定時接收從串口發(fā)來的機器人狀態(tài)數(shù)據(jù)并打包發(fā)送給遠(yuǎn)程控制平臺,同時采集現(xiàn)場的視頻數(shù)據(jù),并進(jìn)行壓縮編碼,然后根據(jù)視頻數(shù)據(jù)包的協(xié)議打包發(fā)送到遠(yuǎn)程控制平臺。此外狀態(tài)和視頻數(shù)據(jù)將作為高級任務(wù)模塊的輸入數(shù)據(jù)來源。圖2中的心跳包用于處理通信異常斷開的情況。

3 關(guān)鍵技術(shù)

3.1 采用udp協(xié)議

tcp協(xié)議提供了可靠的數(shù)據(jù)傳輸,但是其擁塞控制、數(shù)據(jù)校驗、重傳機制的網(wǎng)絡(luò)開銷很大,不適合實時通信,所以選擇開銷很小的udp協(xié)議來傳輸數(shù)據(jù)。

udp協(xié)議是無連接的數(shù)據(jù)傳輸協(xié)議并且無重傳機制,會發(fā)生丟包、收到重復(fù)包、亂序等情況。而對于數(shù)據(jù)精確性要求不高的狀態(tài)數(shù)據(jù)以及視頻數(shù)據(jù),丟包的影響不大。因為會不斷收到新的包,丟失的個別包會有新的包來覆蓋,所以只需在遠(yuǎn)程控制系統(tǒng)的通信部分自行處理亂序及重復(fù)包的問題,而對于丟包的問題一般不作處理。

但對于命令包這種需要精確收發(fā)的數(shù)據(jù), 可在程序的開發(fā)中加入丟包重發(fā)和超時丟棄的處理。

當(dāng)然,如果開發(fā)的是對于實時性要求不高的事件型控制命令的傳輸,不希望發(fā)生指令的丟失也可以直接采用tcp協(xié)議。tcp的重傳機制正好適合這種情況。

3.2 使用非阻塞套接字結(jié)合多線程進(jìn)行通信

整個系統(tǒng)的通信部分采用winsock api進(jìn)行開發(fā),摒棄了其他遠(yuǎn)程控制系統(tǒng)中多采用的阻塞字加多線程的編程模式,而使用了非阻塞套接字并采用了異步事件選擇i/o模型。這是因為非阻塞套接字能更好地在非占先的windows環(huán)境下工作,而阻塞套接字在性能上要差一些。基于整個系統(tǒng)的實時性要求,采用了非阻塞的套接字。

由于狀態(tài)數(shù)據(jù)和視頻數(shù)據(jù)的收發(fā)量大而且頻繁,所以仍需采用多線程技術(shù)將數(shù)據(jù)的收發(fā)放到子線程中,否則將影響主線程的運行。

3.3 本地控制系統(tǒng)中的高級任務(wù)模塊的作用

位于本地控制系統(tǒng)中的高級任務(wù)模塊使得本地控制系統(tǒng)具有自動避障、目標(biāo)識別、自主漫游等高級任務(wù)的決策能力。這一設(shè)計的實質(zhì)是將屬于遠(yuǎn)程控制的復(fù)雜控制功能下放到本地控制系統(tǒng)實現(xiàn)。如圖2所示,只需從遠(yuǎn)程控制平臺發(fā)出啟動某高級任務(wù)指令給本地控制系統(tǒng),本地控制系統(tǒng)的高級任務(wù)模塊調(diào)用相應(yīng)的子模塊并以本地實時更新的狀態(tài)數(shù)據(jù)和視頻數(shù)據(jù)作為輸入數(shù)據(jù)進(jìn)行處理,實時發(fā)出一系列的指令給機器人,便可完成特定的任務(wù)。

該設(shè)計雖然增加了車載計算機的運算負(fù)荷,但是由于可以直接在本地實時收到狀態(tài)數(shù)據(jù)以及視頻數(shù)據(jù),既避免了將這些數(shù)據(jù)發(fā)送到遠(yuǎn)程控制平臺以及控制指令發(fā)回到本地控制系統(tǒng)所需的網(wǎng)絡(luò)時延,又降低了網(wǎng)絡(luò)的不可預(yù)測性造成的系統(tǒng)不可靠性,從而加快了系統(tǒng)的控制響應(yīng)速度,保證了控制的實時性。同時也提高了遠(yuǎn)程控制平臺操作的集成度、機器人的自主性和系統(tǒng)的可靠性。

3.4 心跳包的使用

圖2中的心跳包用于處理通信異常斷開的情況,例如突然斷電、網(wǎng)絡(luò)中斷等。心跳包是一個非常小的數(shù)據(jù)包,包里只包含時間戳、包的序號等信息,并使用udp協(xié)議傳輸。遠(yuǎn)程控制平臺每隔一定的時間(如1分鐘)就向本地控制系統(tǒng)發(fā)送一個心跳包。當(dāng)本地控制系統(tǒng)收到該包,只需簡單地將該包發(fā)回來以表明它收到了這個包。

如果遠(yuǎn)程控制平臺在超過一定的時間(如3分鐘)沒有收到已發(fā)送的心跳包的回包,就可以認(rèn)為本地控制系統(tǒng)和遠(yuǎn)程控制平臺已經(jīng)異常斷開,從而停止發(fā)送心跳包,并立即啟動緊急處理模塊,執(zhí)行如操作人員發(fā)出警報、重新建立網(wǎng)絡(luò)連接或者重新啟動遠(yuǎn)程控制平臺等應(yīng)急功能。

如果本地控制系統(tǒng)在一定的時間(如3分鐘)內(nèi)都沒有收到從遠(yuǎn)程控制平臺發(fā)來的心跳包,就可以認(rèn)為本地控制系統(tǒng)已經(jīng)與遠(yuǎn)程控制平臺異常斷開,從而立即啟動本地控制系統(tǒng)中的緊急處理模塊,由該模塊發(fā)出如停止機器人或啟動自主控制程序的指令給機器人。同時如果網(wǎng)絡(luò)未斷,將恢復(fù)監(jiān)聽網(wǎng)絡(luò)上的連接請求,以等待建立新的連接。

通過使用心跳包使系統(tǒng)的應(yīng)急處理能力得到提高,從而提升了整個系統(tǒng)的可靠性。

3.5 應(yīng)用層協(xié)議的設(shè)計

根據(jù)實時性要求,包的大小應(yīng)盡可能小,以減少帶寬的占用,所以整套應(yīng)用層傳輸協(xié)議的設(shè)計應(yīng)該力求簡潔。以下的協(xié)議都是建立在udp傳輸協(xié)議之上。
(1)命令包協(xié)議

｜包頭｜時間戳｜序列號｜命令｜參數(shù)類型｜參數(shù)｜校驗和｜包尾｜

(2)狀態(tài)包協(xié)議

｜包頭｜時間戳｜序列號｜狀態(tài)數(shù)據(jù)塊｜校驗和｜包尾｜

(3)心跳包協(xié)議

｜包頭｜｜時間戳｜序列號｜校驗和｜包尾｜

(4)視頻數(shù)據(jù)協(xié)議

｜包頭｜時間戳｜序列號｜同一時間戳數(shù)據(jù)塊數(shù)量｜數(shù)據(jù)塊序號｜數(shù)據(jù)塊｜校驗和｜包尾｜

在以上協(xié)議中:時間戳用于實時性檢查;序列號用于丟包的檢測和統(tǒng)計;校驗和用于檢查包的完整性。

以上所設(shè)計的協(xié)議具有可擴展性,可根據(jù)設(shè)計要求增減字段。例如當(dāng)需要考慮信息傳輸?shù)陌踩?可加入加密字段;當(dāng)不考慮延時,可刪除時間戳字段等。

3.6 自適應(yīng)實時視頻傳輸方案

現(xiàn)有的遠(yuǎn)程視頻監(jiān)控系統(tǒng)多采用rtp/rtcp協(xié)議(實時傳輸協(xié)議),它是因特網(wǎng)上針對多媒體數(shù)據(jù)流的一種傳輸協(xié)議。rtp是一種封裝協(xié)議,用于對多媒體數(shù)據(jù)塊進(jìn)行封裝,然后用下層傳輸協(xié)議(如udp)進(jìn)行傳輸。rtcp封裝的是發(fā)送端或者接收端的統(tǒng)計報表。發(fā)送多媒體數(shù)據(jù)的端將根據(jù)接收到的rtcp反饋信息調(diào)整視頻數(shù)據(jù)傳輸質(zhì)量。

但是針對本機器人遠(yuǎn)程控制系統(tǒng),該協(xié)議并不完全合適。由于該遠(yuǎn)程控制系統(tǒng)是一對一的傳輸模式,只傳輸視頻數(shù)據(jù)而無其他多媒體數(shù)據(jù)流,所以具有特殊性。而rtp協(xié)議具有一般性,它的很多字段并不適合該系統(tǒng)。例如關(guān)于數(shù)據(jù)源的同步以及關(guān)于組播的部分,由于不斷發(fā)送rtcp包將占用有限的網(wǎng)絡(luò)帶寬,不如直接在遠(yuǎn)程控制平臺進(jìn)行丟包統(tǒng)計并在視頻質(zhì)量明顯變化時發(fā)送調(diào)節(jié)視頻質(zhì)量的命令給本地控制系統(tǒng)。根據(jù)這一思路,本系統(tǒng)以rtp協(xié)議的基本思想為基礎(chǔ),針對系統(tǒng)的特殊性開發(fā)了一套自適應(yīng)實時視頻傳輸方案。

如圖3所示,本地控制系統(tǒng)將采集的一幀視頻數(shù)據(jù)使用mpeg4或m-jpeg協(xié)議進(jìn)行壓縮編碼;然后按照一定的大小(一般比mtu稍微小點,因為ip和udp包頭要占用一定的字節(jié)數(shù))對編碼后的視頻數(shù)據(jù)進(jìn)行分塊,并使用視頻數(shù)據(jù)協(xié)議進(jìn)行數(shù)據(jù)塊的封裝;最后使用udp協(xié)議發(fā)送到遠(yuǎn)程控制平臺。其中時間戳用于表明這些數(shù)據(jù)塊屬于同一視頻幀,數(shù)據(jù)塊序號用于表明該數(shù)據(jù)塊在此幀中的位置。

由于udp協(xié)議的不可靠性會引發(fā)包的亂序、重復(fù)包以及丟包等情況,所以遠(yuǎn)程控制平臺采用多級緩存池接收視頻數(shù)據(jù)。該緩存池是一個可以容納0.2s～0.4s視頻數(shù)據(jù)的多級緩存,每一級是一個緩存鏈表,用于存儲具有相同時間戳的所有數(shù)據(jù)塊。

該多級緩存池的基本算法是:

(1)當(dāng)接收到的數(shù)據(jù)塊的時間戳在多級緩存中不存在時:如果該時間戳比緩存中最舊的時間戳要新,則在多級緩存池中插入屬于該時間戳的緩存鏈表,在該鏈表中加入該數(shù)據(jù)塊,并刪除時間戳最舊的數(shù)據(jù)鏈表;如果該時間戳比緩存中最舊的時間戳還要舊,就丟棄該數(shù)據(jù)塊。

(2)當(dāng)接收到的視頻數(shù)據(jù)塊的時間戳在多級緩存中存在時:檢查該時間戳對應(yīng)緩存鏈表,通過該數(shù)據(jù)塊序號判斷該數(shù)據(jù)塊是否存在該時間戳的緩存鏈表中。若不存在就加入該數(shù)據(jù)塊,否則丟棄。

(3)當(dāng)接收到的數(shù)據(jù)塊使得同一時間戳的視頻數(shù)據(jù)完全收到,就將其解碼并顯示,然后釋放包括該時間戳在內(nèi)的、所有小于該時間戳的緩存鏈表。

視頻的壓縮標(biāo)準(zhǔn)采用mjpeg或者mpeg4。mjpeg屬于幀內(nèi)編碼,傳輸中發(fā)生丟幀不會影響其他的幀;而mpeg4屬于幀間編碼,傳輸中發(fā)生丟幀,特別是關(guān)鍵幀的丟失將嚴(yán)重影響其他幀的完整。所以在實際的系統(tǒng)設(shè)計中,如果網(wǎng)絡(luò)的通信狀況很好,則選擇mpeg4,因為它的壓縮比高;如果網(wǎng)絡(luò)的通信狀況很差,則選擇mjpeg,雖然壓縮比相對較小,但是發(fā)生丟幀時不會影響其他幀。

在接收視頻數(shù)據(jù)的同時,應(yīng)根據(jù)時間戳和序列等進(jìn)行視頻傳輸狀況統(tǒng)計,主要是丟包及延時統(tǒng)計。根據(jù)統(tǒng)計信息,當(dāng)視頻傳輸狀況發(fā)生了明顯變化時,就發(fā)送調(diào)節(jié)視頻質(zhì)量的命令給本地控制系統(tǒng):

(1)當(dāng)傳輸?shù)臓顩r變差(如丟包、延時嚴(yán)重等)時就給本地控制系統(tǒng)發(fā)送降低視頻質(zhì)量的命令,使其降低視頻采集的頻率或者提高壓縮比率。至于是降低視頻采集的頻率還是提高壓縮比率,將根據(jù)當(dāng)前機器人的移動速度來決定:當(dāng)機器人移動較快時,并不需要非常高的畫面質(zhì)量,所以優(yōu)先采用提高圖像壓縮比率的方法。當(dāng)只提高壓縮比率效果不明顯時,再同時采用降低視頻采集頻率的方法;當(dāng)機器人移動速度很慢時如抓取東西時,需要更高的視頻清晰度,此時優(yōu)先采用降低視頻采集頻率的方法。當(dāng)只降低視頻采集頻率效果不明顯時,再同時采用提高壓縮比率的方法。

(2)同理,如果傳輸?shù)臓顩r較好,則可以發(fā)送提高視頻質(zhì)量的命令給本地控制系統(tǒng),根據(jù)移動速度決定采取優(yōu)先提高視頻采集的頻率或者優(yōu)先降低壓縮比率的方法來提高視頻的質(zhì)量。

4 機器人遠(yuǎn)程控制系統(tǒng)開發(fā)庫

4.1 目的與用處

提供一套使開發(fā)者擺脫編程細(xì)節(jié)的、快速開發(fā)的、模塊化的、可擴展的機器人遠(yuǎn)程控制開發(fā)庫,可以方便研究人員進(jìn)行軟件的開發(fā),降低其在軟件編程方面的重復(fù)勞動。

4.2 設(shè)計原則

(1)采用面向?qū)ο蟮乃枷?以c++類進(jìn)行設(shè)計,并將類封裝到動態(tài)鏈接庫中。
(2)開發(fā)庫具有多選擇性,使開發(fā)庫的使用者有多種技術(shù)可選,如提供多種視頻編碼庫。
(3)在類中對多線程進(jìn)行封裝 ,并加入線程的同步,使類具有線程安全性。
(4)函數(shù)中的算法要力求效率高、運算速度快,以減少因為占用過多的計算時間造成的網(wǎng)絡(luò)時延。
(5)類中定義的接口應(yīng)設(shè)計完整,不可隨便改動,以保證今后升級和擴展時接口的一致性。
(6)類的代碼要多采用基本c/c++標(biāo)準(zhǔn)庫函數(shù)來開發(fā),而盡量不要使用依靠平臺的如mfc庫,以方便移植到linux平臺。

4.3 開發(fā)庫cylib介紹

圖4是依照以上設(shè)計原則設(shè)計的機器人遠(yuǎn)程控制系統(tǒng)開發(fā)庫cylib的類層次圖。該庫由4個動態(tài)鏈接庫文件組成:cycore.dll、cytools.dll、cyrobot.dll、cygui.dll。其中基類加陰影顯示,其他的為派生類。

cycore.dll封裝了應(yīng)用層數(shù)據(jù)包協(xié)議、遠(yuǎn)程控制命令、數(shù)據(jù)傳輸接口、延時及丟包模擬等。

cyrobot.dll包含了對機器人編程接口aria的封裝、控制指令的執(zhí)行、狀態(tài)數(shù)據(jù)的輪詢和發(fā)送、視頻的采集壓縮發(fā)送以及色彩識別、漫游等智能控制功能等。

cytools.dll提供了一系列的工具類用于輔助研究人員的程序開發(fā)。其中包含線程進(jìn)程同步類、時間類、算術(shù)類、日志功能、壓縮解壓縮、加密解密、視頻采集等。

cygui.dll提供了圖形化顯示類,如多功能按鈕、地圖生成、聲納圖形顯示等。

5 開發(fā)實例

根據(jù)以上的系統(tǒng)結(jié)構(gòu)設(shè)計并利用開發(fā)庫cylib,開發(fā)了一套p3-dx機器人遠(yuǎn)程控制系統(tǒng)。圖5展示了機器人遠(yuǎn)程控制平臺的控制現(xiàn)場。該平臺實現(xiàn)了機器人本體、機械臂和攝像頭的控制、狀態(tài)數(shù)據(jù)的顯示、聲納羅盤數(shù)據(jù)圖形化顯示、視頻監(jiān)控以及自動漫游、色彩識別等智能控制功能。機器人本地控制系統(tǒng)的軟件開發(fā)為車載計算機的一個windows2000常駐服務(wù)程序。它實現(xiàn)了命令的解析和中轉(zhuǎn)、智能任務(wù)控制的執(zhí)行、狀態(tài)及視頻數(shù)據(jù)的采集、打包、發(fā)送等功能。

本機器人遠(yuǎn)程控制系統(tǒng)開發(fā)庫目前還不夠完善,需要繼續(xù)改進(jìn)和擴展。最后介紹的開發(fā)實例基本達(dá)到了設(shè)計的要求,但是部分高級任務(wù)如機械臂智能控制、自主控制等尚未完成,需要進(jìn)一步加以研究和開發(fā)。