一種混合協(xié)同定位的多機(jī)器人通信方法研究

艾浩軍，韓亮亮，黃 樂，金子龍，王壹豐

(1.武漢大學(xué)國家網(wǎng)絡(luò)安全學(xué)院，武漢430073；2.上海宇航系統(tǒng)工程研究所，上海201109；3.武漢工程大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院，武漢430205；4.中國航天科技集團(tuán)有限公司空間結(jié)構(gòu)與機(jī)構(gòu)技術(shù)實(shí)驗(yàn)室，上海201108)

1 引言

在月球表面廣泛部署定位基礎(chǔ)設(shè)施是非常困難的。因此需研究以母機(jī)器人為中心的多機(jī)器人通信與定位系統(tǒng)，計(jì)算子機(jī)器人到母機(jī)器人的相對位置，最終轉(zhuǎn)換為所有機(jī)器人的絕對位置。目前，采用無線傳感器網(wǎng)絡(luò)實(shí)現(xiàn)機(jī)器人通信與定位的研究已經(jīng)成為熱點(diǎn)問題。

通信方面，在美國國防高級研究計(jì)劃局的大力支持下，卡內(nèi)基梅隆大學(xué)于1978年開展分布式傳感器網(wǎng)絡(luò)課題研究。上世紀(jì)90年代之后，很多美國大學(xué)也爭相進(jìn)入了這個(gè)新興研究領(lǐng)域，如加州大學(xué)、南加州大學(xué)、麻省理工學(xué)院、康奈爾大學(xué)、佐治亞理工學(xué)院等。這些知名大學(xué)對無線傳感器網(wǎng)絡(luò)的關(guān)鍵技術(shù)和系統(tǒng)理論進(jìn)行了深入研究[1-2]。隨后，國內(nèi)外不同研究機(jī)構(gòu)分別從物理層、數(shù)據(jù)鏈路層、網(wǎng)絡(luò)層、傳輸層、應(yīng)用層等方面著手，并取得了一系列可喜的研究進(jìn)展[3-5]。如張清國等[6]提出一個(gè)基于蜂窩結(jié)構(gòu)的覆蓋優(yōu)化算法，算法每次選取一個(gè)移動傳感器節(jié)點(diǎn)，尋找傳感器網(wǎng)絡(luò)中離它最近的覆蓋漏洞位置，基于蜂窩結(jié)構(gòu)計(jì)算移動節(jié)點(diǎn)的候選目標(biāo)位置，修補(bǔ)漏洞，提高網(wǎng)絡(luò)的覆蓋率。此外，美國加州大學(xué)伯克利分校開展的大鴨島生態(tài)環(huán)境監(jiān)測項(xiàng)目、英特爾公司演示的有關(guān)家庭醫(yī)療護(hù)理的無線傳感器網(wǎng)絡(luò)系統(tǒng)等[7-9]都是在開發(fā)出的Mote節(jié)點(diǎn)及相應(yīng)無線傳感器網(wǎng)絡(luò)系統(tǒng)基礎(chǔ)上，開展的廣泛應(yīng)用研究。

定位方面，月面環(huán)境下的定位技術(shù)在月球車上應(yīng)用逐漸成熟。近年來多位學(xué)者進(jìn)行了月面巡視器的定位研究。徐辛超等[10-12]應(yīng)用單攝像機(jī)完成近距離的定位；劉少創(chuàng)等[13]完成了月面巡視器立體相機(jī)共線方程的建立；SQUYRES等[14]研究了火星探測器的定位方法。但上述月球車導(dǎo)航研究中都是假設(shè)機(jī)器人已經(jīng)知道自己的全局位置，只探討如何進(jìn)行機(jī)器人的路徑規(guī)劃。隨著月球車自主化要求越來越高，其定位問題將受到越來越廣泛的重視。

針對月面環(huán)境下機(jī)器人組無法獲取全局位置的問題，借鑒地面類似應(yīng)用場景下的前沿技術(shù)，提出多機(jī)器人通信與定位的系統(tǒng)設(shè)計(jì)方案，研究多機(jī)器人的無線數(shù)字通信技術(shù)，在無法依賴外界設(shè)備環(huán)境下(如無GPS環(huán)境、無場景建模條件)，實(shí)現(xiàn)多機(jī)器人混合協(xié)同定位。研制母子機(jī)器人板載通信與定位模塊，及與機(jī)器人其它模塊的集成，形成多機(jī)器人通信、定位與系統(tǒng)設(shè)計(jì)的完整解決方案。并通過基于STM32平臺開發(fā)相應(yīng)的通信與定位測試平臺，驗(yàn)證系統(tǒng)的可行性及是否達(dá)到實(shí)際應(yīng)用要求。

2 系統(tǒng)框架和技術(shù)指標(biāo)

本項(xiàng)目由多機(jī)器人通信、定位兩大模塊以及整體系統(tǒng)設(shè)計(jì)來解決月球表面的通信和定位的問題。方案包括：多機(jī)器人通信方案、多機(jī)器人定位方案、多機(jī)器人系統(tǒng)設(shè)計(jì)方案，如圖1所示。其技術(shù)指標(biāo)如下：近作業(yè)區(qū)(200 m范圍)，視距條件下定位精度分米級，優(yōu)于1 m；遠(yuǎn)作業(yè)區(qū)(2 km范圍)，視距條件下，定位精度米級，優(yōu)于10 m。近作業(yè)區(qū)，一般工作條件下，以母機(jī)器人為中心，可實(shí)現(xiàn)200 m半徑的通信，相對定位精度達(dá)到分米級；遠(yuǎn)作業(yè)區(qū)，極限工作條件，子母機(jī)器人通信距離大于2 km，相對定位精度達(dá)米級。

圖1 系統(tǒng)總體架構(gòu)圖Fig.1 System architecture

3 方案設(shè)計(jì)

3.1 多機(jī)器人通信設(shè)計(jì)

多機(jī)器人的通信系統(tǒng)中，母機(jī)器人擁有對外通信的信道，所有子機(jī)器人通過母機(jī)器人間接對外通信。母機(jī)器人對外通信，如與基站的通信，需根據(jù)基站采用的通信方式?jīng)Q定，在此不加討論。多機(jī)器人通信需要在月球表面實(shí)現(xiàn)以母機(jī)器人為中心的多機(jī)器人全雙工實(shí)時(shí)無線通信信道，有多機(jī)器人組成的本地通信網(wǎng)絡(luò)，能滿足所有機(jī)器人間的通信需求。

根據(jù)多機(jī)器人遠(yuǎn)距離通信和定位技術(shù)需求，組網(wǎng)通信以母機(jī)器人為中心，形成星型網(wǎng)絡(luò)，如圖2所示；在實(shí)現(xiàn)定位時(shí)，母機(jī)器人及子機(jī)器人均可互通信，形成網(wǎng)狀網(wǎng)，如圖3所示。

圖2 母機(jī)器人為中心的星型網(wǎng)絡(luò)Fig.2 Mother robot-centered star network

圖3 定位時(shí)形成網(wǎng)狀網(wǎng)Fig.3 Mesh network during positioning

網(wǎng)絡(luò)協(xié)議采用MODBUS協(xié)議。MODBUS是OSI模型第7層上的應(yīng)用層報(bào)文傳輸協(xié)議，其作用是在為連接至不同類型總線或網(wǎng)絡(luò)的設(shè)備提供客戶機(jī)/服務(wù)器通信。MODBUS是一種請求/應(yīng)答協(xié)議，并且可以根據(jù)需要設(shè)置功能碼。MODBUS協(xié)議定義了一個(gè)與基礎(chǔ)通信層無關(guān)的簡單協(xié)議數(shù)據(jù)單元 PDU(Protocol Data Unit)，MODBUS功能碼是MODBUS請求/應(yīng)答PDU的元素。特定總線或網(wǎng)絡(luò)上的 MODBUS協(xié)議映射能夠在應(yīng)用數(shù)據(jù)單元ADU(Asynchronous Data Unit)上引入附加域。

3.2 多機(jī)器人定位設(shè)計(jì)

多機(jī)器人定位設(shè)計(jì)主要是為確保位置的精準(zhǔn)性，從而達(dá)到實(shí)時(shí)準(zhǔn)確的信息配對傳遞。本文采用自定位和相對定位結(jié)合，近定位和遠(yuǎn)定位相結(jié)合的方案。

3.2.1 移動機(jī)器人自定位

移動機(jī)器人的自定位需要測量出機(jī)器人所移動的距離以及在此時(shí)間內(nèi)機(jī)器人方位的變化。移動機(jī)器人的自定位采用M6050慣性導(dǎo)航模塊實(shí)現(xiàn)，通過M6050自帶陀螺儀和加速度計(jì)分別測量出旋轉(zhuǎn)率和加速度，然后對測量結(jié)果進(jìn)行積分運(yùn)算，以此計(jì)算出機(jī)器人移動的距離以及機(jī)器人方位的變化，再根據(jù)軌跡推算算法，求得移動機(jī)器人的位置以及姿態(tài)，這種方法的優(yōu)點(diǎn)之一是無需外部參考。但會隨時(shí)間有漂移，任何小的常數(shù)誤差在積分之后都會無限增長。因此，系統(tǒng)將自定位和相對定位結(jié)合，進(jìn)行精確定位，并將新的位置記錄作為新的起點(diǎn)，以減小累積誤差。移動機(jī)器人自主定位原理如圖4所示，圖中顯示了移動機(jī)器人的加速度積分過程，其中，(X(i)、Y(i))(i=0，1，2)表示機(jī)器人的位置信息，φ(i)表示位置偏移量。

圖4 移動機(jī)器人自主定位原理圖Fig.4 Schematic diagram of mobile robot autonomous position

3.2.2 多機(jī)器人相對定位

多機(jī)器人相對定位是將被定位的子機(jī)器人當(dāng)作是標(biāo)簽節(jié)點(diǎn)，母機(jī)器人及其它子機(jī)器人當(dāng)作是基站，采用了雙向測距的方法來獲取標(biāo)簽與基站之間的距離。由于標(biāo)簽與基站之間時(shí)間不同步，因此利用標(biāo)簽發(fā)出信號與收到反饋信號之間的時(shí)間差，加之發(fā)送的速度為電磁波的傳播速度，可獲得基站與標(biāo)簽之間的距離。采用TDOA雙曲線定位算法，其原理如圖5所示。圖中，N(x0，y0)表示被定位的機(jī)器人坐標(biāo)，Ci(xi，yi)(i=1，2，3)表示其他子機(jī)器人或母機(jī)器人坐標(biāo)，R表示其之間的距離。

圖5 雙曲線定位原理圖Fig.5 Schematic diagram of hyperbolic position

利用TDOA雙曲定位算法進(jìn)行標(biāo)簽坐標(biāo)解算，求得3個(gè)以上基站到標(biāo)簽的距離，利用雙曲線方程就可解算出標(biāo)簽坐標(biāo)。即只要有1個(gè)母機(jī)器人，例如C1坐標(biāo)為(x1，y1)，2個(gè)或2個(gè)以上的子機(jī)器人在視距范圍內(nèi)，例如 C2(x2，y2)和 C3(x3，y3)，就可以計(jì)算出被測子機(jī)器人的位置N(x0，y0)。TOF測距技術(shù)可以認(rèn)為是飛行時(shí)差測距(Time of Flight Measurement)方法，屬于雙向測距技術(shù)，其主要原理是測量信號在2個(gè)異步收發(fā)機(jī)之間往返的飛行時(shí)間，結(jié)合電波飛行的速度測量節(jié)點(diǎn)間的距離。在視距范圍內(nèi)，基于TOF測距方法測得的結(jié)果比較理想，隨距離呈線性變化。TOF測距方法的關(guān)鍵約束為發(fā)送設(shè)備和接收設(shè)備必須始終同步，但是由于發(fā)送端和接收端的時(shí)鐘不一致，從而導(dǎo)致很難達(dá)到絕對時(shí)間同步。在此采取改進(jìn)的TOF測距方法，通過時(shí)鐘偏移量來解決時(shí)鐘同步問題，且考慮兩端節(jié)點(diǎn)中時(shí)鐘偏移量的影響。即A發(fā)送數(shù)據(jù)包，B收并回應(yīng)確認(rèn)，B回應(yīng)確認(rèn)不僅帶B的時(shí)間，同時(shí)也是完整的數(shù)據(jù)包，待A收并確認(rèn)，也計(jì)算回應(yīng)確認(rèn)的時(shí)間，與B的相比較，均衡處理。實(shí)際測量過程中，采用多次測量取平均值的方法計(jì)算距離，可達(dá)到更好的精度。對于不同的模塊，在投入使用前，標(biāo)定時(shí)間作為實(shí)際使用過程中對實(shí)測值的校正。

3.2.3 多機(jī)器人通信與定位硬件設(shè)計(jì)

多機(jī)器人通信與定位硬件實(shí)現(xiàn)框圖如圖6所示。多機(jī)器人通信與定位系統(tǒng)主控制器選擇STC12LE5A60S2單片機(jī)，組網(wǎng)選擇CC1310，433M技術(shù)組網(wǎng)，與主控制器之間采用串口通信；近距離定位采用 UWB(Ultra Wideband)技術(shù)，選擇DWM1000模塊，與主控制器采用SPI(Serial Peripheral Interface)接口連接；遠(yuǎn)距離定位采用433M擴(kuò)頻技術(shù)遠(yuǎn)距離組網(wǎng)技術(shù)，自定位采用陀螺儀和加速度計(jì)獲得方向和位移參數(shù)進(jìn)行積分計(jì)算獲得，選擇MPU6050，采用I2C接口與主控制器相連；如有需要，可設(shè)置鍵盤控制電路、顯示輸出模塊、指示燈或報(bào)警輸出等。

圖6 硬件組成框圖Fig.6 Block diagram of hardware composition

4 實(shí)驗(yàn)結(jié)果

在基于STM32平臺上開發(fā)通信與定位測試平臺，系統(tǒng)測試在空曠無干擾的環(huán)境下進(jìn)行，主機(jī)和從機(jī)分別用串口線連接電腦，電腦端打開串口調(diào)試助手，并調(diào)整波特率為115.2 kbps，停止位1位，數(shù)據(jù)位8位，無奇偶校驗(yàn)。主機(jī)連接的電腦可充當(dāng)上位機(jī)功能，從機(jī)連接的電腦充當(dāng)下位機(jī)。實(shí)驗(yàn)由3部分組成：多機(jī)器人通信實(shí)驗(yàn)、多機(jī)器人定位實(shí)驗(yàn)和多機(jī)器人通信與定位聯(lián)調(diào)實(shí)驗(yàn)。室外通信測試范圍為武漢工程大學(xué)圖書館樓頂?shù)酱蠡?。測試工具為Win10環(huán)境的筆記本電腦，采用XCOM串口調(diào)試助手。

4.1 多機(jī)器人通信實(shí)驗(yàn)

組網(wǎng)通信以母機(jī)器人為中心形成星型網(wǎng)絡(luò)，采用MODBUS協(xié)議。母機(jī)器人作為主節(jié)點(diǎn)，子機(jī)器人作為子節(jié)點(diǎn)。通信總是由主節(jié)點(diǎn)發(fā)起，母機(jī)器人作為主控節(jié)點(diǎn)。在實(shí)驗(yàn)過程中，采用電源供電，為保證母機(jī)器人信息不丟失，在母機(jī)器人的硬件上加有輔助存儲器?？紤]母機(jī)器人負(fù)責(zé)對外通信，采用485串口與上位機(jī)通信模擬母機(jī)器人對外通信。實(shí)驗(yàn)中子機(jī)器人作為子節(jié)點(diǎn)，從降低功耗的角度考慮，其通信模塊可以采用電池供電，亦可采用電源供電。子機(jī)器人有唯一的地址，母機(jī)器人也有固定地址，保證了多機(jī)器人的信號傳輸?shù)臏?zhǔn)確性。

母機(jī)器人可發(fā)送廣播命令及單播命令單獨(dú)給各子機(jī)器人。在2 km范圍內(nèi)，每隔100 m進(jìn)行不同命令測試、極限帶寬測試以及誤碼率測試。表1給出了主機(jī)與從機(jī)2的通訊測試結(jié)果，其測出的各項(xiàng)指標(biāo)均達(dá)到設(shè)計(jì)要求。

表1 主機(jī)與從機(jī)2通信測試結(jié)果Table 1 Test result of communication between host and slave 2

4.2 多機(jī)器人定位實(shí)驗(yàn)

多機(jī)器人定位實(shí)驗(yàn)包括200 m內(nèi)UWB定位，遠(yuǎn)距離采用LORA(Long Range Radio)遠(yuǎn)距離定位，先分別進(jìn)行定位實(shí)驗(yàn)再進(jìn)行聯(lián)合測試。在正式定位前，借助UNI-T手持式激光測量儀測量各節(jié)點(diǎn)間距離完成標(biāo)定，然后利用標(biāo)定結(jié)果，進(jìn)行具體定位測試。經(jīng)過反復(fù)測試，將定標(biāo)程序及測試算法程序標(biāo)準(zhǔn)化，集成在系統(tǒng)中，以方便不同環(huán)境下的標(biāo)定。表2給出了UWB近距離定位測試和LORA遠(yuǎn)距離定位測試結(jié)果。

表2 多機(jī)器人定位測試結(jié)果Table 2 Test result of multi-robot position

4.3 多機(jī)器人通信與定位聯(lián)調(diào)實(shí)驗(yàn)

多機(jī)器人通信中母機(jī)器人作為主節(jié)點(diǎn)，子機(jī)器人作為子節(jié)點(diǎn)。母機(jī)器人發(fā)定位指令給子機(jī)器人，子機(jī)器人便進(jìn)入定位程序，利用UWB近距離定位、LORA遠(yuǎn)距離定位，同時(shí)計(jì)算自定位值估算與母機(jī)器人的距離，并將所得定位數(shù)據(jù)通過擴(kuò)張的卡爾曼濾波算法得到期望數(shù)據(jù)。此時(shí)母機(jī)器人作為主控節(jié)點(diǎn)，以廣播形式發(fā)送定位信息，獲取周圍其他機(jī)器人的響應(yīng)，以測得相應(yīng)的距離信息來定位，并將定位結(jié)果給母機(jī)器人。

聯(lián)調(diào)實(shí)驗(yàn)中，實(shí)驗(yàn)主機(jī)位于武漢工程大學(xué)圖書館8樓樓頂，從機(jī)位于大化工C613室，利用Google地圖測量距離顯示距離為399.99 m。每次記錄的測試數(shù)據(jù)為10次的平均值，分別記錄3次，如表3所示。平均值為3次測試數(shù)據(jù)的平均值，誤差值為平均值與Google地圖測量距離的差值，可看出誤差值小于3 m。

表3 平均測量距離統(tǒng)計(jì)表Table 3 Average measurement distance statistics/m

5 結(jié)論

針對月面環(huán)境多機(jī)器人通信與定位，進(jìn)行了系統(tǒng)方案設(shè)計(jì)、算法研究及硬件實(shí)現(xiàn)。設(shè)計(jì)了基于混合協(xié)同定位的多機(jī)器人通信方法，定位模塊采用自定位和相對定位結(jié)合、近定位和遠(yuǎn)定位相結(jié)合的方法。采用基于MODBUS的網(wǎng)絡(luò)通信方法進(jìn)行信息互通，能夠?qū)崿F(xiàn)多機(jī)器人之間的精準(zhǔn)通信。在基于STM32平臺上開發(fā)了相應(yīng)的通信與定位測試平臺，實(shí)驗(yàn)結(jié)果表明，在400 m范圍內(nèi)機(jī)器人組通信正常，定位準(zhǔn)確，驗(yàn)證了系統(tǒng)的可行性，達(dá)到了技術(shù)指標(biāo)要求。