輪式機器人的室內多融合精準定位技術

中國礦業大學（北京） 黃忠睿 蘭曉玉 周 健 李潤峰 李 傲 胡世博

本文針對室內移動機器人要求精準定位的應用需求，在實驗分析激光雷達定位優勢的基礎上，研究其與慣性傳感器融合的定位方法。對基于粒子濾波的SLAM定位算法進行研究，分析其實現原理、推導其實現過程。利用樹莓派和STM32構建的實驗平臺，對改進算法的構圖和定位效果進行驗證。具體實驗結果表明多設備融合定位更加精確，構建的地圖邊界平滑。

1 研究背景、現狀及實驗測試環境

1.1 研究背景

隨著經濟的發展和生活水平的提高，人們對服務機器人的需求日益增加。據統計，我國各類殘疾人總數已達到8500萬，約占中國總人口的比例的6.21%，居世界前列。此外，截止2018年年底，我國60歲以上的人口已達2.49億人，占全國總人口比重的17.9%，65周歲以上的人口達到1.7億人，占全國總人口比重的11.9%。因此未來殘疾人和老年人的護理工作將是一大社會問題。若服務類機器人能提供基本的護理工作，將大大減輕社會負擔，提高老年人、殘疾人等特殊群體的生活質量。

服務類機器人的應用通常發生在室內環境，對機器人室內定位技術的研究已成為熱點。不同于室外定位，室內定位不能使用GPS、北斗等衛星服務，只能依靠其他的室內無線設施，比如WIFI、藍牙、ZIGBEE、超聲波、激光、UWB、射頻識別標簽、LED、磁場等。室內定位精度要求高，但室內環境卻復雜多變。因此要把服務機器人引入室內，首先要解決的就是室內定位問題。

1.2 國內外研究現狀

目前室內定位的主流技術主要有WIFI、超聲波、超寬帶、射頻識別、藍牙、ZIGBEE、計算機視覺、激光雷達等。其中，WIFI定位、藍牙定位等技術成本低但精度不高，在室內面積有限的環境里無法找到機器人的準確位置；超聲波定位成本低但容易受環境干擾；激光雷達定位精度高、響應時間短，成本適中。

室內定位種類雖然比較多，但是室內定位算法一般都是通用的?？偨Y起來室內定位有4種常見的定位算法：鄰近法、三角測量定位算法、指紋定位算法、TDOA定位算法。4種定位算法相比較而言，近鄰法定位精度不高，一般在5-8米，勝在部署簡便；三角測量定位算法理論上精度較高，但在實際的室內定位中，時間、角度這些參數的獲取比較困難。指紋采集定位算法前期地圖繪制工作量大，而且場地布置臨時有變化各位置上的信號就會變化，從而導致定位不準。也受到非視距和多徑環境的影響，精度誤差大，一般在2-3米左右。TDOA定位算法定位精度高，微能信息的UWB定位精度一般根據定位場景，誤差在10-30cm，抗干擾能力和穿透能力較強，但是UWB基站和標簽部署成本比較高。

最早的室內定位技術是由AT&T公司開發的Active Badges，該系統由Roy Want等人設計研發，但單一的硬件設備的定位精確度無法保證。所以在成本有限的情況下，更高數量級的精度要求就需要一些算法和方法的配合才能夠實現，即多傳感器的融合。根據這一構想，我們提出了室內移動機器人多融合定位方法。

1.3 本文的實驗測試環境

本課題組四輪小車定位機器人的實驗范圍為中國礦業大學（北京）學院路校區逸夫科研實驗樓七樓的環形走廊。通過這一有代表性的區域的實驗，驗證想法，完善裝置。在完成基本任務的基礎上，會考慮在更復雜多變的室內環境中實現定位。

2 本文所使用的設備及定位方法

2.1 本文選用的實驗平臺

根據現有技術設備的優點和不足，我們選用了基于ROS、STM32和樹莓派平臺構建的輪式機器人為本文的實驗設備（圖1所示）。其擁有的里程計、陀螺儀等慣性導航設備和激光雷達測距裝置可以提供定位原始數據，室內地圖的構建，還可以通過Wi-Fi與上位機進行通訊，完成數據回傳。

圖1 本文使用的實驗設備

2.1.1 慣性導航設備

慣性導航設備是基于慣性傳感器（Inertial Measurement Units,IMU），對狀態進行預測的定位設備，其利用加速度計、磁力計和陀螺儀等傳感器對前一時刻的數據信息進行處理，由已知的位置和姿態開始，測量車體在慣性坐標系的加速度和角度，將它們在時間上連續積分，并且變換到導航坐標系中，推算出車體在導航坐標系中的速度、航向和位置等信息，得到當前時刻的相對位置。

IMU具有比較強的自主性，短時間內的定位精度和連續性較高，但慣性傳感器不可避免地會隨著時間的變化而產生累積誤差，需要依靠其他定位信息源對位置推算結果進行校驗。移動機器人的IMU控制結構如圖2所示。

圖2 移動機器人的IMU控制結構

2.1.2 激光雷達（RPLIDAR）

激光傳感器定位方法利用激光的反射到達時間（Time of Arrival,TOA）、信號到達角度（Angle of Arrival，AOA）和接收信號強度(Received Signal Strength,RSS)等來測量到障礙物的距離，實現相對定位。

雖然其硬件成本較高，但激光具有的高精度、高分辨率、便攜安裝、不受可見光影響等優點，對精確的室內定位至關重要。通過分析處理雷達數據，可以對里程計的位置姿態信息進行修正，較為準確地估計移動機器人的位置姿態，極大彌補慣性傳感器的短板，從而大幅提高定位精確度。

2.2 本文所使用的定位方法

自主移動機器人具備的自主導航功能是其完成其他任務的前提，自主導航功能包括環境的識別和路徑的規劃。環境識別是路徑規劃的前提，通過機器人攜帶的傳感器探測周圍環境，得到環境中的信息，并且確定其在環境中的相對位置關系，故環境識別即為Simultaneous Localization And Mapping——同時定位與地圖建設技術（SLAM）。

本文使用的激光雷達利用的是三角測距原理，在每次激光掃描測距過程中，發射光束信號，遇到目標物體之后產生的反射信號被其采集系統接收，然后通過內部處理器的實時計算，得到激光雷達到目標物體之間的距離。

為了克服航跡推算誤差隨時間累積的固有缺陷，本文使用卡爾曼濾波算法來進行里程計和激光雷達的融合定位。卡爾曼濾波是基于狀態空間的一套遞推濾波算法，在狀態空間中引入狀態變量的概念。卡爾曼濾波模型包括狀態向量和觀測向量，狀態向量始終在變化，這種變化可用一個方程表示，而觀測向量也可用一個方程表示，這兩個方程組成了卡爾曼濾波模型?？柭鼮V波首先通過狀態轉移方程預測下一個狀態，再根據觀測變量來得到系統的最優估計。但實驗環境的邊界清晰，地圖描繪的較為準確，卡爾曼濾波圖解如圖3所示。

圖3 卡爾曼濾波圖解

3 研究所需的環境地圖構建

3.1 基本方法

環境地圖的構建方法較多，常用的有拓撲地圖、柵格地圖和特征地圖等等，其中最適用于導航的是柵格地圖。

柵格地圖將觀測量轉換為柵格，再將地圖變為緊密排列的方格，柵格地圖的構建過程稱為柵格化。將觀測量根據機器人的坐標位置投影到坐標系的柵格當中，這樣每一個柵格都會被多次監測，提高了精度。小車的精確位姿（坐標和朝向）可通過其在參考坐標下的朝向和相對障礙物的距離計算。

采用bresenham直線段掃面算法，障礙物所處的柵格標注為occupy，在機器人所處的柵格與障礙物所處的柵格之間畫直線，直線所到之處都為free，柵格的占據用概率表示而不只是0、1兩種狀態。若某一個柵格在激光束a掃描下標識為occupy，在激光束b掃描下也標識為occupy，那該柵格的占據概率就變大，反之，則變小。這樣，機器人所處的環境就通過二維柵格地圖得以表征。

3.2 實驗過程

（1）在現實環境中選取一個合適的點，將機器人放置在選取的點上，開啟行機器人的啟動節點。

（2）在ssh連接的窗口內編寫launch1程序。打開機器人鍵盤控制節點，方便在建圖時移動機器人。在新的終端內ssh連接到樹莓派，編寫運行launch2程序。按鍵盤控制機器人時，確保鼠標光標在keyboard_teleop.launch運行的窗口內。

（3）運行Gmapping建圖包。在新的終端內將ssh連接到樹莓派，然后編寫運行launch3程序。

（4）以上三個launch程序運行正常后，在主機上打開新的終端，運行rviz，在數據窗口內查看地圖和機器人姿態數據。

地圖、激光點、小車位姿數據如圖4所示。

圖4 地圖、激光點、小車位姿數據

3.3 存在的誤差

從圖5所示中可以看出實驗環境的邊界清晰，地圖描繪的較為準確，但其中有一些散射形狀的白色噪點，這主要是因為小車在運行過程中觸碰到墻壁造成的。通過進一步優化卡爾曼濾波算法可以解決這一問題，本組將在后續的實驗中繼續完善算法，改進實驗。

圖5 實驗環境的地圖構建

4 機器人的室內精準定位

4.1 基本方法

迭代最近點法（ICP）是小車室內定位中最常用的方法之一，由Besl和Mc Kay等人在20世紀90年代提出，主要是用于兩個點集之間的對位匹配，ICP最初用于三維點云的配準，但其也適用于移動機器人所使用的二維激光，其基本原理是對于相互關系未知的情況下的兩組點集，構造最小二乘問題來求解出點集之間的相互配準。

I C P 存在迭代不收斂的情況，故比較經典的蒙特卡洛定位法和迭代最近點法（ICP）等定位方法，本文最后選用了AMCL（Adaptive Monte Carlo Localization）定位法，A M C L 法是一種升級版的蒙特卡洛定位法，蒙特卡洛定位法使用的是粒子濾波的方法來進行定位，AMCL使用的是自適應KLD方法來更新粒子。卡爾曼濾波再定位過程中的應用是一開始在地圖空間均勻地布下大量粒子，然后通過獲取機器人的動作信息來移動粒子，例如機器人向前移動了一米，所有的粒子也向前移動一米。利用每個粒子所處的位置建立一個虛擬傳感器，其傳遞的信息跟實際觀測到的傳感器的信息作對比，賦給每一個粒子一個概率。再根據產生的概率來重新生成粒子，概率越高的生成粒子的幾率越大。這樣迭代之后，所有粒子會收斂到一起，這樣即可推算出機器人的確切位置。

AMCL的算法步驟如下：

4.2 實驗過程

實驗的核心是在ROS中運行AMCL算法。

（1）通過激光雷達發布一個新的話題，再通過各個坐標系的tf轉換，使初始位置給定，并用map_server加載地圖到ROS空間。

（2）將機器人放到地圖的起點，連接樹莓派開放的WIFI。

（3）打開新的終端ssh連接到樹莓派，運行HiBot啟動節點和AMCL節點，在主機上打開新的終端運行rviz，可以看到機器人初始定位狀態下的粒子分布。

起初粒子（紅色）的分布比較散亂。隨著機器人的運動，粒子逐漸收斂，通過計算機可以看到在此過程中我們成功實現了機器人的室內精準定位。如圖6所示，圖6中的黑色定位點為小車的定位位置。

圖6 小車的定位結果

4.3 實驗結果與分析

本課題組在實驗環境中選取了幾個特定點進行測試。經過實驗，機器人在地圖上的定位點比較精準，誤差不超過10cm。本實驗結果說明了多個傳感器進行融合可以實現多種傳感器之間的互補，增強定位算法的效率和穩定性，這也是本文研究多傳感器融合定位算法的意義所在。但是由于不可避免的因為小車形狀產生的物理誤差，定位時的環境探測有一些問題，后期的實驗中我們會嘗試進一步修改算法，使小車的定位精度進一步提高。

5 總結與展望

5.1 課題總結

針對目前室內定位方案中存在的定位精度差的問題，我們提出了一種基于激光與航跡推算的融合定位系統。本文的研究工作主要分為三部分，第一部分是對本課題所選用算法的研究，提出了多融合傳感器的優化算法；第二部分是對實驗測試環境地圖的構建研究；第三部分是對小車精確定位的研究，實現了課題計劃的目標，證明了多融合算法的優越性。課題使用的里程計、陀螺儀等慣性導航設備和激光雷達測距裝置為小車提供主要定位原始數據。通過對數據的分析處理，比對同時建立的環境地圖，將處理后的位置信號作用于相應的車輪驅動電路，實現小車的即時定位，并通過WIFI與上位機通信，完成位置信息的回傳。通過實驗我們證明了多傳感器融合定位的精確性和必要性，提高了小車的定位精度。

5.2 實際應用價值及展望

網絡互聯的時代，獲取信息的便捷極大地促進了人們對位置的需求，只有在位置精確的基礎上，機器人的移動才有意義，才能為室內機器人實現更多的功能提供了最基礎的前提，如物品的運送、人員的引導、環境的檢測等實用實際性的工作，人們可以發揮想象利用此最基礎的功能來設計更為復雜的功能來應用于緊急救援、生活社交、安全監控等領域，讓機器人更好地服務于我們，我們能更好地體會到機器人對于生活的幫助和自身生活質量的提升，更大程度的讓普通老百姓感受到前端科技帶給我們的影響，從而能實現機器人作為服務工具的使用價值。

未來，我們會進一步優化算法，并引入人工智能技術，讓機器人可以自主實現地圖構建和路徑規劃，更加方便地實現它的功能。