基于超寬帶通訊技術的多傳感器融合定位方法研究

杜鑫 朱文亮＊ 文西芹 朱加豪

（1、江蘇海洋大學機械工程學院，江蘇 連云港 222005 2、鹽城工學院機械工程學院優集學院，江蘇 鹽城 224007）

1 概述

同步定位建圖（Simultaneous Localization and Mapping,SLAM）作為機器人自主移動的關鍵技術，已成為眾多移動機器人研究者的關注焦點，該技術可以讓機器人感知周圍環境，并獲取其自身位置。定位精度的高低直接關乎到移動機器人能否準確行進到目的地，因此如何提高定位精度是移動機器人領域的重要研究內容。

國內外研究者提出了許多定位與建圖方法，包括早期的聲納、超聲波定位和后來的射頻識別定位技術，以及目前主流的基于視覺傳感器與激光雷達的定位與建圖方法[1]。基于視覺傳感器的方法在較為復雜的場景表現較好，傳感器從起初的單目視覺、雙目視覺，到RGB-D（深度圖像），有較大進步，但視覺傳感器存在場景光線依賴程度高和計算資源占用大等缺點[2-3]。激光雷達傳感器有2D 和3D 之分，3D 激光雷達成本高，不適用于室內移動機器人的開發與推廣。2D 激光雷達通過二維平面點的方式來采集機器人周圍環境信息，其精度較高，但因為有效測量距離有限和采集信息量過少，在重定位和閉環檢測方面表現欠佳[4]。

超寬帶[5](Utral Wideband, UWB)是近年來新興的一種無載波通信技術，以占空比很低（幾十分之一）的超短電磁能量窄脈沖作為信息載體，因此又被稱為脈沖無線電,其具有高速數據傳輸、低功耗、保密性強、高處理增益、抗多徑能力強和定位精準等特點，單個標簽定位精度可達10cm[6]。但超寬帶定位具有漂移誤差，在標簽與基站之間有遮擋時尤其明顯。

針對以上情況，本文提出一種四類傳感器融合的室內移動機器人定位方法，該方法是在麥克納姆輪式移動機器人上，使用超寬帶定位為機器人提供初始位姿和絕對坐標，使用加速度計來消除超寬帶的漂移誤差，使用慣性測量單元(Inertial Measurement Unit, IMU)為機器人提供偏轉角度，并使用激光雷達和里程計組合在提前建好的地圖中進行自適應蒙特卡洛定位（Adaptive Monte Carlo Localization,AMCL），以此提高移動機器人室內定位精度。

2 定位算法原理

2.1 激光雷達定位算法原理

本文的激光雷達定位算法，采用自適應蒙特卡洛定位算法,該算法基于蒙特卡洛統計思想，可理解為將粒子均勻地撒在已知地圖中，機器人在移動過程中統計堆積粒子,并用通過多次迭代，估計出機器人自身所處的位置，同時在該過程中融入（Kullback-LeiblerDivergence, KLD）自適應方法，解決了機器人綁架等問題。

2.2 超寬帶定位算法原理

本文的超寬帶定位方法[7]，采用到達時間定位算法，又稱TOA（Time of Arrival）定位算法，該算法依據信號傳播時間來直接計算出被測點（標簽）與參考點（基站）之間的距離。此算法中，標簽發送信號到至少3 個以上的基站上，系統通過測量信號到達各個基站的時間，從而獲取標簽與基站之間的距離，然后再以各個基站為圓心所得距離為半徑作圓，各圓的交點便是目的節點的參考位置。TOA 定位算法原理如圖1所示。

圖1 TOA 模式

已知三個基站坐標為A0(x0,y0)、A1(x1,y1)和A2(x2,y2)，激光雷達的紅外線信號傳播速度V 為光速，t0、t1和t2為信號從標簽TAG 到各個基站的飛行時間。標簽的坐標設為T0（X,Y），可由以下式（1）求得。

2.3 里程計定位算法原理

里程計定位算法選用慣性測量單元與輪式里程計組合定位，慣性測量單元采用三軸慣性測量單元來獲取機器人的姿態角和位移量，輪式里程計數據由編碼器從直流電機上采集。由麥克納姆輪運動方程與輪子半徑，可估計當前位置與偏轉角大小：

式（2）中，V1、V2、V3和V4是麥克納姆輪的線速度，Vx和Vy代表移動機器人在自身X 和Y 方向上的分速度，ω 代表移動機器人繞豎直方向的旋轉角速度，L 和W 代表麥克納姆輪與地面的四個觸點構成的矩形長和寬，由此可求得Vx、Vy和ω，繼而求出一定時間內移動機器人的位姿變化量。

3 激光雷達、里程計和超寬帶定位融合

該方法首先對超寬帶定位數據進行擴展卡爾曼濾波處理，將濾波后的超寬帶定位數據用作移動機器人的初始絕對坐標，此舉可以提高移動機器人初始定位速度。然后在機器人移動過程中，每隔0.5s 將AMCL 定位輸出坐標與超寬帶定位坐標進行對比，在邏輯判斷中，如果二者間出現5%以上的差值，則用超寬帶定位坐標對AMCL 定位進行修正，以此來降低里程計組合定位造成的累積誤差，該方法通過程序在移動機器人上實現。融合算法流程圖如圖2 所示。

圖2 融合定位流程圖

4 實驗

本實驗采用對比方式，令移動機器人在室內邊長3 米的正方形路線進行順時針閉環移動。隨機截取端點實驗數據如表1 所示。

表1 三種定位方法的定位實驗數據

超寬帶定位效果如圖3 所示，其定位軌跡存在明顯的漂移誤差。

圖3 單一超寬帶定位軌跡圖

單一里程計的定位軌跡如圖4 所示，其定位軌跡存在較大的累積誤差。

圖4 單一里程計定位軌跡圖

激光雷達、里程計與UWB 融合的定位軌跡如圖5 所示，累積誤差較里程計定位有所減少，且能基本實現軌跡閉環。

圖5 激光雷達、里程計與超寬帶融合軌跡圖

5 結論

5.1 傳統AMCL 定位方法是在移動機器人行進過程中，不斷迭代處理粒子來實現定位，而利用機器人自主初始化位姿的方法，時間更是長達90s 左右。本研究中引入了超寬帶定位方法，在10s 內即可完成初始定位，顯著提高了室內移動機器人初始定位速度。

5.2 在融合定位方法中，超寬帶定位為移動機器人提供了絕對坐標，消除了里程計與慣性測量單元在定位上的累積誤差，融合后的定位軌跡能基本實現閉合，較里程計與慣性測量單元的組合定位，閉環效果更好。

通過實驗數據分析，該定位算法較AMCL 定位方法加快了位姿初始化的速度，時間節省了80s 左右；其全局定位精度較單一超寬帶定位方法或激光雷達、里程計組合定位方法，都有不同程度的提高，尤其相于AMCL 定位方法，定位精度可提高8%。該方法對室內移動機器人定位精度的提高具有一定的參考價值。