中國(guó)礦業(yè)大學(xué)（北京） 黃忠睿 蘭曉玉 周 健 李潤(rùn)峰 李 傲 胡世博

本文針對(duì)室內(nèi)移動(dòng)機(jī)器人要求精準(zhǔn)定位的應(yīng)用需求，在實(shí)驗(yàn)分析激光雷達(dá)定位優(yōu)勢(shì)的基礎(chǔ)上，研究其與慣性傳感器融合的定位方法。對(duì)基于粒子濾波的SLAM定位算法進(jìn)行研究，分析其實(shí)現(xiàn)原理、推導(dǎo)其實(shí)現(xiàn)過(guò)程。利用樹莓派和STM32構(gòu)建的實(shí)驗(yàn)平臺(tái)，對(duì)改進(jìn)算法的構(gòu)圖和定位效果進(jìn)行驗(yàn)證。具體實(shí)驗(yàn)結(jié)果表明多設(shè)備融合定位更加精確，構(gòu)建的地圖邊界平滑。

1 研究背景、現(xiàn)狀及實(shí)驗(yàn)測(cè)試環(huán)境

1.1 研究背景

隨著經(jīng)濟(jì)的發(fā)展和生活水平的提高，人們對(duì)服務(wù)機(jī)器人的需求日益增加。據(jù)統(tǒng)計(jì)，我國(guó)各類殘疾人總數(shù)已達(dá)到8500萬(wàn)，約占中國(guó)總?cè)丝诘谋壤?.21%，居世界前列。此外，截止2018年年底，我國(guó)60歲以上的人口已達(dá)2.49億人，占全國(guó)總?cè)丝诒戎氐?7.9%，65周歲以上的人口達(dá)到1.7億人，占全國(guó)總?cè)丝诒戎氐?1.9%。因此未來(lái)殘疾人和老年人的護(hù)理工作將是一大社會(huì)問(wèn)題。若服務(wù)類機(jī)器人能提供基本的護(hù)理工作，將大大減輕社會(huì)負(fù)擔(dān)，提高老年人、殘疾人等特殊群體的生活質(zhì)量。

服務(wù)類機(jī)器人的應(yīng)用通常發(fā)生在室內(nèi)環(huán)境，對(duì)機(jī)器人室內(nèi)定位技術(shù)的研究已成為熱點(diǎn)。不同于室外定位，室內(nèi)定位不能使用GPS、北斗等衛(wèi)星服務(wù)，只能依靠其他的室內(nèi)無(wú)線設(shè)施，比如WIFI、藍(lán)牙、ZIGBEE、超聲波、激光、UWB、射頻識(shí)別標(biāo)簽、LED、磁場(chǎng)等。室內(nèi)定位精度要求高，但室內(nèi)環(huán)境卻復(fù)雜多變。因此要把服務(wù)機(jī)器人引入室內(nèi)，首先要解決的就是室內(nèi)定位問(wèn)題。

1.2 國(guó)內(nèi)外研究現(xiàn)狀

目前室內(nèi)定位的主流技術(shù)主要有WIFI、超聲波、超寬帶、射頻識(shí)別、藍(lán)牙、ZIGBEE、計(jì)算機(jī)視覺(jué)、激光雷達(dá)等。其中，WIFI定位、藍(lán)牙定位等技術(shù)成本低但精度不高，在室內(nèi)面積有限的環(huán)境里無(wú)法找到機(jī)器人的準(zhǔn)確位置；超聲波定位成本低但容易受環(huán)境干擾；激光雷達(dá)定位精度高、響應(yīng)時(shí)間短，成本適中。

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

最早的室內(nèi)定位技術(shù)是由AT&T公司開發(fā)的Active Badges，該系統(tǒng)由Roy Want等人設(shè)計(jì)研發(fā)，但單一的硬件設(shè)備的定位精確度無(wú)法保證。所以在成本有限的情況下，更高數(shù)量級(jí)的精度要求就需要一些算法和方法的配合才能夠?qū)崿F(xiàn)，即多傳感器的融合。根據(jù)這一構(gòu)想，我們提出了室內(nèi)移動(dòng)機(jī)器人多融合定位方法。

1.3 本文的實(shí)驗(yàn)測(cè)試環(huán)境

本課題組四輪小車定位機(jī)器人的實(shí)驗(yàn)范圍為中國(guó)礦業(yè)大學(xué)（北京）學(xué)院路校區(qū)逸夫科研實(shí)驗(yàn)樓七樓的環(huán)形走廊。通過(guò)這一有代表性的區(qū)域的實(shí)驗(yàn)，驗(yàn)證想法，完善裝置。在完成基本任務(wù)的基礎(chǔ)上，會(huì)考慮在更復(fù)雜多變的室內(nèi)環(huán)境中實(shí)現(xiàn)定位。

2 本文所使用的設(shè)備及定位方法

2.1 本文選用的實(shí)驗(yàn)平臺(tái)

根據(jù)現(xiàn)有技術(shù)設(shè)備的優(yōu)點(diǎn)和不足，我們選用了基于ROS、STM32和樹莓派平臺(tái)構(gòu)建的輪式機(jī)器人為本文的實(shí)驗(yàn)設(shè)備（圖1所示）。其擁有的里程計(jì)、陀螺儀等慣性導(dǎo)航設(shè)備和激光雷達(dá)測(cè)距裝置可以提供定位原始數(shù)據(jù)，室內(nèi)地圖的構(gòu)建，還可以通過(guò)Wi-Fi與上位機(jī)進(jìn)行通訊，完成數(shù)據(jù)回傳。

圖1 本文使用的實(shí)驗(yàn)設(shè)備

2.1.1 慣性導(dǎo)航設(shè)備

慣性導(dǎo)航設(shè)備是基于慣性傳感器（Inertial Measurement Units,IMU），對(duì)狀態(tài)進(jìn)行預(yù)測(cè)的定位設(shè)備，其利用加速度計(jì)、磁力計(jì)和陀螺儀等傳感器對(duì)前一時(shí)刻的數(shù)據(jù)信息進(jìn)行處理，由已知的位置和姿態(tài)開始，測(cè)量車體在慣性坐標(biāo)系的加速度和角度，將它們?cè)跁r(shí)間上連續(xù)積分，并且變換到導(dǎo)航坐標(biāo)系中，推算出車體在導(dǎo)航坐標(biāo)系中的速度、航向和位置等信息，得到當(dāng)前時(shí)刻的相對(duì)位置。

IMU具有比較強(qiáng)的自主性，短時(shí)間內(nèi)的定位精度和連續(xù)性較高，但慣性傳感器不可避免地會(huì)隨著時(shí)間的變化而產(chǎn)生累積誤差，需要依靠其他定位信息源對(duì)位置推算結(jié)果進(jìn)行校驗(yàn)。移動(dòng)機(jī)器人的IMU控制結(jié)構(gòu)如圖2所示。

圖2 移動(dòng)機(jī)器人的IMU控制結(jié)構(gòu)

2.1.2 激光雷達(dá)（RPLIDAR）

激光傳感器定位方法利用激光的反射到達(dá)時(shí)間（Time of Arrival,TOA）、信號(hào)到達(dá)角度（Angle of Arrival，AOA）和接收信號(hào)強(qiáng)度(Received Signal Strength,RSS)等來(lái)測(cè)量到障礙物的距離，實(shí)現(xiàn)相對(duì)定位。

雖然其硬件成本較高，但激光具有的高精度、高分辨率、便攜安裝、不受可見光影響等優(yōu)點(diǎn)，對(duì)精確的室內(nèi)定位至關(guān)重要。通過(guò)分析處理雷達(dá)數(shù)據(jù)，可以對(duì)里程計(jì)的位置姿態(tài)信息進(jìn)行修正，較為準(zhǔn)確地估計(jì)移動(dòng)機(jī)器人的位置姿態(tài)，極大彌補(bǔ)慣性傳感器的短板，從而大幅提高定位精確度。

2.2 本文所使用的定位方法

自主移動(dòng)機(jī)器人具備的自主導(dǎo)航功能是其完成其他任務(wù)的前提，自主導(dǎo)航功能包括環(huán)境的識(shí)別和路徑的規(guī)劃。環(huán)境識(shí)別是路徑規(guī)劃的前提，通過(guò)機(jī)器人攜帶的傳感器探測(cè)周圍環(huán)境，得到環(huán)境中的信息，并且確定其在環(huán)境中的相對(duì)位置關(guān)系，故環(huán)境識(shí)別即為Simultaneous Localization And Mapping——同時(shí)定位與地圖建設(shè)技術(shù)（SLAM）。

本文使用的激光雷達(dá)利用的是三角測(cè)距原理，在每次激光掃描測(cè)距過(guò)程中，發(fā)射光束信號(hào)，遇到目標(biāo)物體之后產(chǎn)生的反射信號(hào)被其采集系統(tǒng)接收，然后通過(guò)內(nèi)部處理器的實(shí)時(shí)計(jì)算，得到激光雷達(dá)到目標(biāo)物體之間的距離。

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

圖3 卡爾曼濾波圖解

3 研究所需的環(huán)境地圖構(gòu)建

3.1 基本方法

環(huán)境地圖的構(gòu)建方法較多，常用的有拓?fù)涞貓D、柵格地圖和特征地圖等等，其中最適用于導(dǎo)航的是柵格地圖。

柵格地圖將觀測(cè)量轉(zhuǎn)換為柵格，再將地圖變?yōu)榫o密排列的方格，柵格地圖的構(gòu)建過(guò)程稱為柵格化。將觀測(cè)量根據(jù)機(jī)器人的坐標(biāo)位置投影到坐標(biāo)系的柵格當(dāng)中，這樣每一個(gè)柵格都會(huì)被多次監(jiān)測(cè)，提高了精度。小車的精確位姿（坐標(biāo)和朝向）可通過(guò)其在參考坐標(biāo)下的朝向和相對(duì)障礙物的距離計(jì)算。

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

3.2 實(shí)驗(yàn)過(guò)程

（1）在現(xiàn)實(shí)環(huán)境中選取一個(gè)合適的點(diǎn)，將機(jī)器人放置在選取的點(diǎn)上，開啟行機(jī)器人的啟動(dòng)節(jié)點(diǎn)。

（2）在ssh連接的窗口內(nèi)編寫launch1程序。打開機(jī)器人鍵盤控制節(jié)點(diǎn)，方便在建圖時(shí)移動(dòng)機(jī)器人。在新的終端內(nèi)ssh連接到樹莓派，編寫運(yùn)行l(wèi)aunch2程序。按鍵盤控制機(jī)器人時(shí)，確保鼠標(biāo)光標(biāo)在keyboard_teleop.launch運(yùn)行的窗口內(nèi)。

（3）運(yùn)行Gmapping建圖包。在新的終端內(nèi)將ssh連接到樹莓派，然后編寫運(yùn)行l(wèi)aunch3程序。

（4）以上三個(gè)launch程序運(yùn)行正常后，在主機(jī)上打開新的終端，運(yùn)行rviz，在數(shù)據(jù)窗口內(nèi)查看地圖和機(jī)器人姿態(tài)數(shù)據(jù)。

地圖、激光點(diǎn)、小車位姿數(shù)據(jù)如圖4所示。

圖4 地圖、激光點(diǎn)、小車位姿數(shù)據(jù)

3.3 存在的誤差

從圖5所示中可以看出實(shí)驗(yàn)環(huán)境的邊界清晰，地圖描繪的較為準(zhǔn)確，但其中有一些散射形狀的白色噪點(diǎn)，這主要是因?yàn)樾≤囋谶\(yùn)行過(guò)程中觸碰到墻壁造成的。通過(guò)進(jìn)一步優(yōu)化卡爾曼濾波算法可以解決這一問(wèn)題，本組將在后續(xù)的實(shí)驗(yàn)中繼續(xù)完善算法，改進(jìn)實(shí)驗(yàn)。

圖5 實(shí)驗(yàn)環(huán)境的地圖構(gòu)建

4 機(jī)器人的室內(nèi)精準(zhǔn)定位

4.1 基本方法

迭代最近點(diǎn)法（ICP）是小車室內(nèi)定位中最常用的方法之一，由Besl和Mc Kay等人在20世紀(jì)90年代提出，主要是用于兩個(gè)點(diǎn)集之間的對(duì)位匹配，ICP最初用于三維點(diǎn)云的配準(zhǔn)，但其也適用于移動(dòng)機(jī)器人所使用的二維激光，其基本原理是對(duì)于相互關(guān)系未知的情況下的兩組點(diǎn)集，構(gòu)造最小二乘問(wèn)題來(lái)求解出點(diǎn)集之間的相互配準(zhǔn)。

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

AMCL的算法步驟如下：

4.2 實(shí)驗(yàn)過(guò)程

實(shí)驗(yàn)的核心是在ROS中運(yùn)行AMCL算法。

（1）通過(guò)激光雷達(dá)發(fā)布一個(gè)新的話題，再通過(guò)各個(gè)坐標(biāo)系的tf轉(zhuǎn)換，使初始位置給定，并用map_server加載地圖到ROS空間。

（2）將機(jī)器人放到地圖的起點(diǎn)，連接樹莓派開放的WIFI。

（3）打開新的終端ssh連接到樹莓派，運(yùn)行HiBot啟動(dòng)節(jié)點(diǎn)和AMCL節(jié)點(diǎn)，在主機(jī)上打開新的終端運(yùn)行rviz，可以看到機(jī)器人初始定位狀態(tài)下的粒子分布。

起初粒子（紅色）的分布比較散亂。隨著機(jī)器人的運(yùn)動(dòng)，粒子逐漸收斂，通過(guò)計(jì)算機(jī)可以看到在此過(guò)程中我們成功實(shí)現(xiàn)了機(jī)器人的室內(nèi)精準(zhǔn)定位。如圖6所示，圖6中的黑色定位點(diǎn)為小車的定位位置。

圖6 小車的定位結(jié)果

4.3 實(shí)驗(yàn)結(jié)果與分析

本課題組在實(shí)驗(yàn)環(huán)境中選取了幾個(gè)特定點(diǎn)進(jìn)行測(cè)試。經(jīng)過(guò)實(shí)驗(yàn)，機(jī)器人在地圖上的定位點(diǎn)比較精準(zhǔn)，誤差不超過(guò)10cm。本實(shí)驗(yàn)結(jié)果說(shuō)明了多個(gè)傳感器進(jìn)行融合可以實(shí)現(xiàn)多種傳感器之間的互補(bǔ)，增強(qiáng)定位算法的效率和穩(wěn)定性，這也是本文研究多傳感器融合定位算法的意義所在。但是由于不可避免的因?yàn)樾≤囆螤町a(chǎn)生的物理誤差，定位時(shí)的環(huán)境探測(cè)有一些問(wèn)題，后期的實(shí)驗(yàn)中我們會(huì)嘗試進(jìn)一步修改算法，使小車的定位精度進(jìn)一步提高。

5 總結(jié)與展望

5.1 課題總結(jié)

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

5.2 實(shí)際應(yīng)用價(jià)值及展望

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

未來(lái)，我們會(huì)進(jìn)一步優(yōu)化算法，并引入人工智能技術(shù)，讓機(jī)器人可以自主實(shí)現(xiàn)地圖構(gòu)建和路徑規(guī)劃，更加方便地實(shí)現(xiàn)它的功能。