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

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

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

1 引言

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

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

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

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

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

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

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

3 方案設(shè)計

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

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

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

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

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

網(wǎng)絡(luò)協(xié)議采用MODBUS協(xié)議。MODBUS是OSI模型第7層上的應(yīng)用層報文傳輸協(xié)議，其作用是在為連接至不同類型總線或網(wǎng)絡(luò)的設(shè)備提供客戶機/服務(wù)器通信。MODBUS是一種請求/應(yīng)答協(xié)議，并且可以根據(jù)需要設(shè)置功能碼。MODBUS協(xié)議定義了一個與基礎(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 多機器人定位設(shè)計

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

3.2.1 移動機器人自定位

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

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

3.2.2 多機器人相對定位

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

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

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

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

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

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

4 實驗結(jié)果

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

4.1 多機器人通信實驗

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

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

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

4.2 多機器人定位實驗

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

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

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

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

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

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

5 結(jié)論

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