基于多傳感器融合的移動機器人定位?

何壯壯 丁德銳 王永雄

（上海理工大學光電信息與計算機工程學院 上海 200093）

1 引言

定位是機器人實現自主運動所必需具備的功能［1］。具體地，對機器人位置和姿態的正確估計是對機器人控制操作的前提，是全局或局部路徑規劃［2］的先決條件。在實現動態跟蹤的機器人定位問題中，對機器人的初始位置和方向是已知的局部定位情形，其唯一目標是基于里程計等傳感器信息確定特定時刻的位置。值得一提的是，定位過程中需要補償傳感器隨時間累積的誤差。而在機器人初始位姿未知的全局定位中，定位過程必須首先推導出初始機器人狀態，然后在運動過程中確定正確的位置和方向。所以又被叫做絕對定位，依賴于外部傳感器信息獲得機器人在全局坐標系下的位置。傳統的定位方法有ZigBee定位［4］、UWB定位［5］和RFID定位［6］和Wifi定位［7］等。

值得一提的是，依靠單一傳感器定位方法都有弊端，受信號強度影響較大，甚至在有信號遮擋情況下會出現定位失敗。考慮測量噪聲以及硬件精度限制，應用不同類型的傳感器可以互補彼此的利弊，通過數據融合最大限度地發揮每個傳感器的有效性，從而克服單個傳感器信息的不準確和不完整性，提高定位精度［8］。另一方面，鑒于傳感器所收集的信息皆存在一定程度的誤差，使用概率方法可以有效地降低這些誤差對定位精度的影響［9］。常用的概率定位方法有卡爾曼濾波方法［10～11］，馬爾可夫定位方法［12］和粒子濾波方法［13］。它們的主要區別在于概率分布函數的表示方式。

綜上所述，借助于粒子濾波定位技術，本文致力于多傳感器數據融合的室內輪式機器人全局定位算法研究。首先，利用編碼器、慣性傳感器（加速度計）和激光雷達實施機器人實驗平臺的數據采集。然后，建立基于信息融合的機器人運動模型，并以此描述相鄰兩個時刻的機器人運動狀態的改變。進而，充分利用了激光雷達的測量信息，更新粒子的權值，并基于這些權重，獲得了機器人的位置和方向。通過采取基于權值的融合，消除了連續的迭代步驟中“跳躍”行為的影響，通過粒子剔除機制，優化了粒子種群，提高算法的運行效率［14］。

2 基于多傳感器融合的定位方法研究

為了有效地消除單個傳感器所帶來的信息不完整性及硬件精度的限制，考慮的移動機器人平臺安裝了以下傳感器：

1）HOKUYO URG-04LX二維激光雷達：用于獲取周圍環境信息，用于同地圖進行實時掃描匹配；

2）MPU6050傳感器模塊：內嵌了加速度計和陀螺儀，用于測量機器人行進方向上的加速度值和旋轉角；

3）歐姆龍編碼器（里程計）：用于實時獲得機器人行進的速度和距離值。

為了抑制測量噪聲，本文采用基于粒子濾波的概率方法，即蒙特卡洛定位方法。蒙特卡洛定位的主要特點是：

1）能夠使用來自多個獨立的傳感器數據，容錯率較高；

2）粒子數量選取的靈活性，且計算的復雜度與粒子數量成正比，計算成本集中在機器人位置發生概率最大的地方；

3）估計機器人當前狀態由概率密度函數表示，該函數可以實現全局定位。

不難看出，擬提出的算法的核心在于信息融合，以及基于融合數據的蒙特卡洛算法的預測、權重更新和重采樣四個部分［15］，具體的算法流程如圖1所示。在該算法中，在已有環境地圖的前提條件下，加速度計的測量結果對于有車輪打滑的機器人運動情況具有校正功能；激光雷達通過將選定激光束的實際讀數與每個顆粒的虛擬測量結果進行比較，可有效地確定粒子權重。

圖1 算法流程圖

在粒子濾波的初始化過程中，它在地圖上隨機生成M個粒子，并為每個粒子i分配相同的歸一化權重；在預測階段，使用運動模型預測來自集合S的每個粒子i的狀態；在隨后的權重更新階段，計算S集中第i個粒子權重ωi，并進行歸一化處理。進而，執行S集合的重采樣工作。對來自S集的粒子按照權重值成比例替換（因為具有較大權重的粒子會被更頻繁地選擇），從而構成新的S′集合［16］。顯而易見，機器人位置存在于該集合的概率相較于先前的S集來得更大。在這個階段，可以額外引入基于有效樣本大小(ESS)系數減少粒子數量的算法［17］。當幾乎整個粒子總體將接近未知機器人狀態x時，該系數可用于逐漸從地圖的其余無關區域去除粒子。采樣完成后，S'組成為下一個計算循環的S組。接下來，我們將給出算法的細節。

2.1 信息融合與預測

為了提出多傳感器數據融合的室內輪式機器人全局定位算法，我們首先給出兩輪機器人的運動模型，如圖2所示。在該圖中，O代表全局坐標系；R代表機器人坐標系；θl和θr表示由編碼器獲得的左右驅動輪的旋轉角速度；Raax表示由加速度計獲得的x方向速度；Rvx表示機器人前進方向上的速度。

在理想的機器人運動過程中，為了預測機器人運動速度，需要借助于編碼器和加速度計。假設不存在輪子打滑等狀況且橫向速度Rvey=0，在坐標系R下，由編碼器獲得的驅動輪的旋轉角速度θl和θr可得機器人運動過程中的縱向速度Rvex和轉彎的角速度Rvw滿足：

圖2 兩輪機器人運動模型

其中，r是車輪的幾何半徑，W是左右側車輪的幾何中心之間的距離。

然而，在室內環境中，由于地面的光滑程度較高或者雜物較多情況下，機器人在縱向運動和旋轉時會產生嚴重的輪子打滑問題，從而影響著運動控制和定位的精準度。傳統的運動控制和速度反饋是通過安裝在底層驅動電機上的編碼器來實現，由于存在輪子打滑現象，機器人輪子的短暫空轉會帶來里程計信息的不準，從而導致位置誤差的不斷累積，對機器人的定位產生致命的錯誤［18］。

眾所周知，無論輪子處于打滑的過程中還是處于正常行駛的過程中，加速度計的測量始終是準確的。為此，我們引入加速度計的測量信息，彌補編碼器因輪子打滑而產生的機器人行進速度的錯誤反饋。假定機器人移動的地面是水平的，我們忽略重力加速度對加速度計的影響?；诩铀俣扔嫷淖x取數據Raax和Raay，機器人的運動速度滿足：

其中，t是預測的時間，Δt是時間間隔。

此外，不考慮機器人轉彎時發生的向心加速度和與地球轉動相關的科里奧利加速度。通過里程計和加速度計運動模型的融合，可得機器人縱向速度為

進而，在全局參考系統中，機器人運動速度有如下關系：

下面，利用速度與距離的關系得到距離信息。首先，機器人定位的本質就是利用傳感器測量獲得其位置Ox=[OxROyROφ]Τ的估計Ox?。在粒子濾波框架下用xi表示每一個粒子的狀態向量。根據機器人運動模型，第i個粒子狀態向量的預測值Ox?i=[Ox?RiOy?RiOφ?i]Τ由以下關系確定：

其中ni為運動的噪聲（不確定性），由機器人運動的縱向和橫向的標準偏差σtrans和σdrift確定。該運動模型適用于S集合中的每個粒子i。

2.2 權重更新

對于來自S集的每個粒子i，必須給予適當的權重wi（反映預測測量值與實際值的接近程度）。如果預測的測量值越接近于實際的測量值，則應給定較大的權重，相反權重越小。因為激光雷達從n個等間隔的激光束獲得距離值，所以每個粒子都有n個激光束的測量值［19］。為此，使用基于高斯分布的傳感器模型，可得如下權重計算公式：

其中Olaser,k表示第k束激光掃描所獲得的距離值，Op,k,i表示投放的粒子模擬的激光束中第k束激光的預測測量數值，σ02表示傳感器的噪聲方差。就σ02而言，較小的值導致窄而高的高斯函數。這意味著只有非常接近真實測量的粒子才能獲得更大的權重。對于大的σ02值，高斯分布扁而寬，權重之間的差異較小。

接下來，對于所得的粒子權重，需進行權重歸一化處理，從而得到更新后的權值：

2.3 重采樣

重新采樣的任務是從S集合中按比例選擇粒子及其權重，并創建新的集合S'，為迭代地獲得機器人的位置提供保障（即創建的S'集與S集交換，實現連續迭代）。在這一過程中，因權重較小而未被選中粒子將被權重較大的粒子所取代。對我們研究的定位問題，這意味著在迭代程序中只保留那些最有可能的位置。

傳統方法是直接選用權重值最大的粒子所在的位置作為機器人的位置，但是該方法的缺點也很明顯，即在連續的迭代步驟中存在“跳躍”行為。為了消除這一不利因素，本文采取基于權值的融合來確定機器人的位置，即利用每個粒子的權重和位置作為理想位置的估計。由于所有粒子的權重之和等于1，因而將粒子的所有坐標和方向按權重比例疊加可得：

最后，對粒子數量進行了優化，減少算法在連續迭代中的計算時間。由于缺乏關于機器人的初始狀態的信息，蒙特卡洛定位需要在過程的最開始時使用大量的粒子來執行初始定位。對于沒有“機器人綁架問題”的任務來說，隨著時間的推移，保持大量的粒子就不那么重要了，因為定位問題順利地過渡到位置跟蹤問題。為此，引入了減少粒子數的機制，當算法發現跟蹤問題比定位問題更突出時，引入了跟蹤問題。這里主要介紹了ESS（有效樣本量）的計算方法，它給出了粒子之間各權值的接近程度，并基于以下關系進行了計算：

這意味著當大多數粒子集中在一個地方時，會適當減少粒子數量，即當有效樣本數滿足以下條件時來減少粒子數量：

其中：τ∈(0,1)是實驗經驗參數。在滿足上式后以合適的比例選取粒子，從而降低了粒子的規模，提高算法的運行效率。

3 實驗和結果分析

3.1 實驗平臺搭建

圖3 兩輪機器人平臺

用于研究開發定位方法的移動機器人如圖3所示。該機器人可被設計用于機場、火車站、大型商場等場所的智能共享搬運機器人。機器人運動系統是由兩個帶有減速器和編碼器的直流電機獨立驅動獨立輪子。通過安裝附加層來安裝必要的設備可測量驅動輪的旋轉角速度，同時安裝有慣性測量單元和二維激光雷達。機器人的移動平臺半徑約為0.6m，其質量約為30kg，載重高達100kg。且機器人配備了兩個非轉向車輪，因此在轉彎過程中會出現一定的車輪滑動漂移。另外，從工程實際的角度出發，室內機器人在行進的過程當中有時車輪會出現嚴重的打滑和空轉現象。所以在一般情況下，僅基于編碼器數據來確定機器人運動參數是非常不準確的，因此機器人依靠單一傳感器來進行定位要困難得多。

3.2 環境獲得與表示

機器人環境必須是全局可觀測的，即在每個時刻地圖和機器人都可以唯一的表示。且環境的表示必須是確定性的，所以由機器人執行的動作的結果是確定可觀測的。另外，在本研究中，機器人的環境必須是靜態的，即環境地圖在機器人操作期間不改變，若存在移動障礙物，需采取適當的方法來檢測其對定位的影響。綜上所述，在目前的工作中，環境以二維靜態確定性和完全可觀測的地圖表達形式。

環境地圖的建立我們使用安裝在機器人上的二維激光雷達傳感器，環境信息來自于激光雷達的環境“掃描”。激光雷達在180°的范圍內掃描障礙距離數據，采用極坐標表示這些距離數據，進而得到激光雷達掃描得到視圖。在機器人行駛過程中結合編碼器距離信息進行自定位，從而生成增量式地圖，進而生成機器人視圖，通過多次地圖更新創建出全局地圖。建立環境地圖時主要使用Rao-Blackwellzed粒子濾波器［21］，這種方法的核心思想是先利用一個粒子濾波器估計機器人的軌跡，然后再用這一軌跡估計來計算一個環境地圖的后驗估計。我們建立與仿真圖相同的實物地圖環境（如圖4所示），并利用已有機器人平臺，利用二維激光SLAM技術建立環境的二維柵格地圖如圖5所示。

圖4 模擬的實驗環境

圖5 二維柵格地圖

3.3 實驗與結果分析

在實驗室光滑地板環境上，基于搭建好的實物環境地圖，運行我們的機器人平臺。在實物環境中，利用上節建立的數據融合方法，將機器人加速度計信息和編碼器信息相融合，獲得機器人的速度信息。同時通過激光雷達的掃描數據初步確定機器人的位置信息和初始的粒子分布，并通過式（8）和式（9）給每一個粒子分配權值。經由這些操作和運算，可得機器人的初始位姿和粒子分布，如圖6所示。接著，讓機器人向前行駛并偏轉一定角度。在行駛的過程當中，根據式（5）和式（6）融合得到的速度信息，通過積分的方法計算式（7）得到位置信息，并循環利用式（8）和式（9）不斷更新粒子集，進而根據式（12）和式（13）方法剔除無效粒子，效果如圖7所示。從該圖中不難發現粒子數明顯減少并逐步收斂至最終機器人位置，當剩余有效粒子最終收斂并聚集在一起即可得到機器人的確切位姿，如圖8所示。精確的定位為后續機器人路徑規劃與導航提供準確信息。

圖6 機器人初始位姿和粒子分布

圖7 機器人行進中位姿和粒子分布

上述實驗結果表明了所提出方法的有效性。接下來，將通過與單純使用里程計的定位方法進行對比，進一步揭示本文方法在定位精度方面的優越性。具體地，在上述實驗的環境下，通過給機器人設定目標點，然后讓機器人分別利用上述方法行進到目標位置，總共進行了19次獨立對比實驗，效果如圖9所示。從圖中可以看出本文剔除的方法誤差在2cm左右，傳統方法誤差在12cm左右，實驗結果進一步驗證了該方法的有效性。

4 結語

本文所提出了基于激光雷達、加速度計和編碼器的機器人定位算法，并在真實的移動機器人平臺上加以驗證。相比于單一傳感器的定位算法，本文提出的方法展具有較高的定位精度。為了進一步研究所提出的方法并提高機器人運動和定位的準確性，計劃進行以下工作：1）增強來自傳感器的數據與來自陀螺儀的數據的融合，以提高機器人航向的預測精度；2）增加環境的第三個空間維度，這將使得能夠在表面傾斜或不平整的環境中進行定位。