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

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

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

1 引言

在月球表面廣泛部署定位基礎設施是非常困難的。因此需研究以母機器人為中心的多機器人通信與定位系統，計算子機器人到母機器人的相對位置，最終轉換為所有機器人的絕對位置。目前，采用無線傳感器網絡實現機器人通信與定位的研究已經成為熱點問題。

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

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

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

2 系統框架和技術指標

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

圖1 系統總體架構圖Fig.1 System architecture

3 方案設計

3.1 多機器人通信設計

多機器人的通信系統中，母機器人擁有對外通信的信道，所有子機器人通過母機器人間接對外通信。母機器人對外通信，如與基站的通信，需根據基站采用的通信方式決定，在此不加討論。多機器人通信需要在月球表面實現以母機器人為中心的多機器人全雙工實時無線通信信道，有多機器人組成的本地通信網絡，能滿足所有機器人間的通信需求。

根據多機器人遠距離通信和定位技術需求，組網通信以母機器人為中心，形成星型網絡，如圖2所示；在實現定位時，母機器人及子機器人均可互通信，形成網狀網，如圖3所示。

圖2 母機器人為中心的星型網絡Fig.2 Mother robot-centered star network

圖3 定位時形成網狀網Fig.3 Mesh network during positioning

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

3.2 多機器人定位設計

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

3.2.1 移動機器人自定位

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

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

3.2.2 多機器人相對定位

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

3.2.3 多機器人通信與定位硬件設計

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

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

4 實驗結果

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

4.1 多機器人通信實驗

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

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

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

4.2 多機器人定位實驗

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

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

4.3 多機器人通信與定位聯調實驗

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

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

表3 平均測量距離統計表Table 3 Average measurement distance statistics/m

5 結論

針對月面環境多機器人通信與定位，進行了系統方案設計、算法研究及硬件實現。設計了基于混合協同定位的多機器人通信方法，定位模塊采用自定位和相對定位結合、近定位和遠定位相結合的方法。采用基于MODBUS的網絡通信方法進行信息互通，能夠實現多機器人之間的精準通信。在基于STM32平臺上開發了相應的通信與定位測試平臺，實驗結果表明，在400 m范圍內機器人組通信正常，定位準確，驗證了系統的可行性，達到了技術指標要求。