自主移動(dòng)機(jī)器人即時(shí)定位與地圖構(gòu)建方法研究

曾品善，妙全興，徐 磊

(1.武警工程大學(xué) 信息工程系，陜西 西安710086;2.武警警官學(xué)院 信息工程系，四川 成都610213)

在Leonard、Durrant［1］等人提出的關(guān)于自主移動(dòng)機(jī)器人導(dǎo)航3個(gè)基本問題“Where am I?Where am I going?How should I get there?”中，第一個(gè)問題就是關(guān)于自主移動(dòng)機(jī)器人的定位問題。自主移動(dòng)機(jī)器人的即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)，也稱為并發(fā)建圖與定位(Concurrent Mapping and Localization，CML)?？梢悦枋鰹?機(jī)器人在未知環(huán)境中從一個(gè)未知的位置開始移動(dòng)，在移動(dòng)過程中根據(jù)自身攜帶的傳感器觀測(cè)數(shù)據(jù)逐步構(gòu)建周圍環(huán)境地圖，同時(shí)在所構(gòu)建地圖基礎(chǔ)上對(duì)機(jī)器人的位姿進(jìn)行估計(jì)和更新，把機(jī)器人的即時(shí)定位與增量式地圖構(gòu)建融為一體，從而實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航。

即時(shí)定位與地圖構(gòu)建(SLAM)是自主移動(dòng)機(jī)器人實(shí)現(xiàn)真正自主運(yùn)動(dòng)的關(guān)鍵，因此SLAM被視為自主移動(dòng)機(jī)器人領(lǐng)域的“圣杯”，是當(dāng)今機(jī)器人研究中的一個(gè)熱點(diǎn)問題。Smith Self和Cheesman［2］最早提出了SLAM問題，并將定位與地圖構(gòu)建問題整合在一起。在SLAM問題中，為了實(shí)現(xiàn)精確定位，機(jī)器人需要通過位置相對(duì)確定的環(huán)境特征來不斷校正自身位置;而另一方面，為了確定環(huán)境特征的位置，機(jī)器人需要知道自身所在的精確位置。這類似于一個(gè)“雞生蛋，蛋生雞”的問題，二者互為前提。

1 SLAM研究的主要問題

1.1 不確定信息的描述和處理方法

自主移動(dòng)機(jī)器人是通過對(duì)機(jī)器人位姿和環(huán)境中路標(biāo)的遞歸估計(jì)來進(jìn)行即時(shí)定位與地圖構(gòu)建的。根據(jù)系統(tǒng)的初始狀態(tài)和從0～t時(shí)刻的觀測(cè)信息與控制信息，自主移動(dòng)機(jī)器人需要對(duì)各個(gè)時(shí)刻的不確定信息進(jìn)行處理。不確定信息的描述和處理包括以下幾個(gè)問題:(1)在地圖和位置的表示中，如何描述運(yùn)動(dòng)和感知信息的不確定性。(2)在迭代過程中，如何處理舊信息與新信息的關(guān)系，連續(xù)更新地圖和位置。(3)如何依據(jù)不確定的信息進(jìn)行決策。

對(duì)于在自主移動(dòng)機(jī)器人SLAM中遇到的不確定信息，研究人員已經(jīng)提出了多種度量方法來進(jìn)行處理，如概率度量、模糊度量、信任度量、可能性度量和證據(jù)理論度量等［3］。由于概率度量和模糊度量相較于其他的方法，能夠更好地適應(yīng)不確定模型，在實(shí)際應(yīng)用中魯棒性也較好，因此目前在SLAM中使用較多的主要就是這兩種方法。但是這兩種方法仍然有一些缺點(diǎn)，如概率度量的計(jì)算量較大，且算法需建立在一定假設(shè)條件之上，模糊度量在參數(shù)設(shè)定上需要通過大量的實(shí)驗(yàn)經(jīng)驗(yàn)等。

1.2 數(shù)據(jù)關(guān)聯(lián)

數(shù)據(jù)關(guān)聯(lián)技術(shù)是指把來自一個(gè)或多個(gè)傳感器的測(cè)量數(shù)據(jù)與已有的測(cè)量數(shù)據(jù)集合進(jìn)行關(guān)聯(lián)分析，確定它們是否來自同一數(shù)據(jù)源。對(duì)于分析后未關(guān)聯(lián)的測(cè)量，它們可能來自新的目標(biāo)，也可能是由于噪聲或雜波產(chǎn)生的干擾。數(shù)據(jù)關(guān)聯(lián)在SLAM中主要完成3個(gè)任務(wù):新環(huán)境特征的檢測(cè)、環(huán)境特征的匹配和地圖的匹配。自主移動(dòng)機(jī)器人周圍環(huán)境的復(fù)雜程度直接影響著數(shù)據(jù)關(guān)聯(lián)的計(jì)算量，一般的數(shù)據(jù)關(guān)聯(lián)技術(shù)難以滿足SLAM的實(shí)時(shí)性要求。在SLAM領(lǐng)域，最常用的數(shù)據(jù)關(guān)聯(lián)方法是最近鄰法(Nearest Neighbor，NN)，該方法計(jì)算簡單，但對(duì)于距離較近的2個(gè)環(huán)境特征可能會(huì)出現(xiàn)關(guān)聯(lián)錯(cuò)誤。數(shù)據(jù)關(guān)聯(lián)正確與否直接決定了機(jī)器人定位與地圖構(gòu)建的準(zhǔn)確性，錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)結(jié)果會(huì)導(dǎo)致此后對(duì)特征的預(yù)測(cè)發(fā)生錯(cuò)誤，甚至?xí)?dǎo)致定位與制圖誤差的發(fā)散。

2 SLAM的實(shí)現(xiàn)方法

2.1 擴(kuò)展卡爾曼濾波

自主移動(dòng)機(jī)器人在進(jìn)行即時(shí)定位與地圖構(gòu)建時(shí)涉及到對(duì)未知且不確定的環(huán)境信息和傳感器噪聲的處理，一般使用以概率論為基礎(chǔ)的卡爾曼濾波(Kalman Filter，KF)來描述此類問題。KF是一種最優(yōu)線性遞歸估計(jì)算法，它能從一系列的不完全包含噪聲的測(cè)量中估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)?；镜腒F必須應(yīng)用在符合高斯分布的離散時(shí)間線性動(dòng)態(tài)系統(tǒng)中，然而在許多實(shí)際情況中，系統(tǒng)的狀態(tài)方程和觀測(cè)方程卻是非線性的，這就需要對(duì)KF進(jìn)行改進(jìn)以應(yīng)用于時(shí)間非線性的動(dòng)態(tài)系統(tǒng)中。

擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)是一種較常用的非線性濾波方法，它是利用最常使用的線性化技術(shù)泰勒級(jí)數(shù)展開，將非線性濾波問題轉(zhuǎn)化為近似的線性濾波問題基本KF，然后利用線性濾波理論求解，得到非線性濾波問題的次優(yōu)濾波算法。EKF由預(yù)測(cè)和更新兩步組成，是一種經(jīng)典的濾波方式，也是自主移動(dòng)機(jī)器人SLAM中常見的一種濾波算法。但是由于在濾波過程中不可避免地引入了線性化誤差，只有當(dāng)系統(tǒng)的狀態(tài)和觀測(cè)模型都接近線性時(shí)，濾波結(jié)果才可能接近真實(shí)值，否則線性化誤差會(huì)嚴(yán)重影響濾波精度，最終導(dǎo)致濾波器發(fā)散，從而使地圖構(gòu)建發(fā)生錯(cuò)誤。

2.2 不敏卡爾曼濾波

不敏卡爾曼濾波(Unscented Kalman Filter，UKF)與EKF類似，也是應(yīng)用卡爾曼濾波框架處理非線性高斯系統(tǒng)的另一種濾波算法。UKF的理論基礎(chǔ)是UT變換(Unscented Transformation)，通過對(duì)確定性Sigma點(diǎn)的采樣來近似非線性函數(shù)的概率密度分布，而不需要對(duì)非線性狀態(tài)和觀測(cè)模型進(jìn)行線性化處理。這些Sigma采樣點(diǎn)能夠完全體現(xiàn)高斯概率密度函數(shù)的真實(shí)均值和協(xié)方差［4］。與EKF相比，在計(jì)算復(fù)雜度方面幾乎相同，但UKF不需要計(jì)算雅克比矩陣。更重要的是當(dāng)系統(tǒng)非線性程度高時(shí)UKF可以取得更好的結(jié)果。

2.3 粒子濾波

粒子濾波(Particle Filter，PF)是基于順序重要采樣(Sequential Importance Sampling，SIS)的一種濾波方法，通過一組從概率密度函數(shù)上隨機(jī)抽取的加權(quán)樣本(粒子)來逼近后驗(yàn)概率，因此粒子濾波適用于任何狀態(tài)空間模型，不受非高斯和非線性模型的影響，理論上足夠多的粒子可以取得良好的逼近效果。

粒子濾波的思想是通過尋找粒子集和粒子對(duì)應(yīng)權(quán)值組成的隨機(jī)采樣數(shù)據(jù)集合s(k)來對(duì)概率密度函數(shù)進(jìn)行近似，以有限樣本點(diǎn)的求和運(yùn)算代替積分運(yùn)算，從而獲得狀態(tài)最小方差估計(jì)。用數(shù)學(xué)語言描述如下:對(duì)平穩(wěn)隨機(jī)過程，假定k－1時(shí)刻系統(tǒng)的后驗(yàn)概率密度為，依據(jù)一定原則選取n個(gè)隨機(jī)樣本點(diǎn)，k時(shí)刻獲得測(cè)量信息后，經(jīng)過狀態(tài)和時(shí)間更新過程，n個(gè)粒子的后驗(yàn)概率密度可近似為。隨著粒子數(shù)目的增加，粒子的概率密度函數(shù)逐漸逼近狀態(tài)的概率密度函數(shù)，粒子濾波估計(jì)即達(dá)到最優(yōu)貝葉斯估計(jì)的效果。粒子濾波算法使非線性濾波問題擺脫了隨機(jī)量必須滿足高斯分布的制約條件，因此近年來該算法在SLAM問題上得到成功應(yīng)用［5］。

對(duì)于粒子濾波算法來說，如何選擇較好的粒子分布預(yù)測(cè)函數(shù)是關(guān)鍵問題。一個(gè)好的粒子分布預(yù)測(cè)函數(shù)，不僅能夠減少重采樣，降低退化效應(yīng)，還能提高預(yù)測(cè)精度，使粒子的分布更接近狀態(tài)的真實(shí)后驗(yàn)概率分布。

2.4 基于視覺的SLAM算法

目前SLAM中使用的傳感器大部分是聲納和激光雷達(dá)，但聲納傳感器成本低且精度不高，激光雷達(dá)精度高卻成本較高。另一類應(yīng)用較多的傳感器是視覺傳感器，視覺傳感器的視覺圖像中包含了豐富的環(huán)境信息，運(yùn)用一定的先驗(yàn)信息或目標(biāo)識(shí)別算法，可以得到意義更明確的地圖［6］。常用的視覺傳感器的配置方式有單目攝像頭和雙目攝像頭兩種。雙目攝像頭利用立體視覺的原理能夠得到圖像中物體的精確深度信息，但它的缺點(diǎn)是成本較高，并且由于要對(duì)兩幅圖像中的同一物體進(jìn)行精確匹配，造成過程處理的時(shí)間過長。單目攝像頭成本低廉，通過結(jié)合EKF等濾波算法，在加入一定約束條件下可以快速獲取環(huán)境信息，因此得到了廣泛應(yīng)用。

3 結(jié)束語

近年來研究者們對(duì)自主移動(dòng)機(jī)器人SLAM問題進(jìn)行了大量的研究，一方面對(duì)現(xiàn)有的SLAM算法進(jìn)行改進(jìn)，以提高算法的性能和可行性，另一方面積極結(jié)合不斷更新的機(jī)器人傳感器設(shè)備，提出新的SLAM算法。但是在以下幾個(gè)方面還需要進(jìn)行更進(jìn)一步的研究:(1)實(shí)時(shí)性。通過提高算法的效率和進(jìn)一步降低算法的時(shí)間和空間復(fù)雜度，從軟件上提升SLAM問題的實(shí)時(shí)性。同時(shí)開發(fā)專用高速高精度傳感器和構(gòu)圖芯片或板卡，通過硬件設(shè)備來提高處理速度。(2)三維SLAM。目前SLAM問題還大量局限于二維靜態(tài)環(huán)境的研究與應(yīng)用，而現(xiàn)實(shí)中的環(huán)境通常是三維動(dòng)態(tài)的。將SLAM的研究與應(yīng)用從二維靜態(tài)環(huán)境擴(kuò)展到三維動(dòng)態(tài)環(huán)境具有重要的意義。(3)多機(jī)器人協(xié)作SLAM?；诙鄼C(jī)器人協(xié)作的SLAM通過機(jī)器人之間的信息互補(bǔ)與共享，能夠彌補(bǔ)單機(jī)器人定位不準(zhǔn)確和構(gòu)圖精度不高等問題，同時(shí)提高了自主移動(dòng)機(jī)器人在復(fù)雜地形下完成繁重任務(wù)的能力。

［1］LEONARD J，DURRANT W H.Mobile robot localization by tracking geometric beacons［J］.IEEE Transactions on Robotics and Automation，1991，7(3):376－382.

［2］SMITH R，SELF M，CHEESEMAN P.Estimating uncertain spatial relationships in robotics［J］.Autonomous Robot Vehicles，1990，8(5):167－193.

［3］ 王衛(wèi)華，陳衛(wèi)東，席裕庚.基于不確定信息的移動(dòng)機(jī)器人地圖創(chuàng)建研究進(jìn)展［J］.機(jī)器人，2001，23(6):563－568.

［4］ 潘泉，楊峰，葉亮，等.一類非線性濾波器——UKF綜述［J］.控制與決策，2005，20(5):481－489.

［5］ 胡士強(qiáng)，敬忠良.粒子濾波算法綜述［J］.控制與決策，2005，20(4):361－365.

［6］ 朱磊，樊繼壯，趙杰，等.未知環(huán)境下的移動(dòng)機(jī)器人SLAM方法［J］.華中科技大學(xué)學(xué)報(bào):自然科學(xué)版，2011，39(7):9－13.