基于二分法的EKF-SLAM算法

閆旭亮，楊功流，王 璐

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京 100083)

0 引言

隨著機(jī)器人技術(shù)的不斷發(fā)展和相關(guān)技術(shù)的突破，機(jī)器人逐漸被應(yīng)用在軍事偵察、海底探索、核污染測定、掃雷排險(xiǎn)等環(huán)境惡劣且極其危險(xiǎn)的情況下[1-2]。機(jī)器人技術(shù)被認(rèn)為是21世紀(jì)的核心武器，將成為新的工業(yè)技術(shù)革新的核心，由于機(jī)器人技術(shù)的前景充滿了無數(shù)種未知的可能性，世界各國對它的研究越來越重視[3]。

在眾多考量機(jī)器人的指標(biāo)中，機(jī)器人的移動能力是決定其任務(wù)成敗的關(guān)鍵，機(jī)器人自由移動的能力是建立在其對周圍環(huán)境特征的了解層次基礎(chǔ)上的[4-5]。只有當(dāng)移動載體機(jī)器人深入了解周圍的環(huán)境特征時(shí)，才可以更快更準(zhǔn)確地完成任務(wù)，因此迫切需要一種能夠精確定位并且創(chuàng)建環(huán)境地圖的方法，同步定位與地圖創(chuàng)建(Simultaneous Localiza-tion and Mapping，SLAM)技術(shù)應(yīng)運(yùn)而生[6-7]。SLAM，是指機(jī)器人移動平臺在行進(jìn)時(shí)創(chuàng)建環(huán)境地圖，同時(shí)根據(jù)創(chuàng)建的地圖進(jìn)行定位，利用2個(gè)信息的相互關(guān)系不斷地修正來改善推算得到的數(shù)據(jù)結(jié)果。同雞和蛋的問題類似，定位和地圖創(chuàng)建之間相互聯(lián)系，精度互為相關(guān)。

在SLAM中，常用的表示地圖的典型方法有三種，分別是特征地圖、柵格地圖和拓?fù)涞貓D。其中，特征地圖不需要對地圖的全局進(jìn)行描述，而是只需要對有環(huán)境特征存在的地方進(jìn)行描述即可，這樣做可大大減少地圖的計(jì)算量，提高定位的精度[8-9]。對于占用空間較小的物體可以通過計(jì)算物理中心，用點(diǎn)的坐標(biāo)進(jìn)行表述；對于由線段或由線段組合組成的特征，可以利用每個(gè)線段的描述來對特征進(jìn)行表述[10-13]。本文從線特征的角度出發(fā)，提出了一種基于二分法的線特征模擬觀測器，并通過MATLAB軟件對其正確性和在擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF) SLAM中的適用性進(jìn)行了仿真驗(yàn)證。

1 基于EKF的SLAM算法

SLAM的同時(shí)定位和構(gòu)建地圖的過程可以用估計(jì)—觀測—更新的簡化模型來描述。SLAM的實(shí)現(xiàn)流程圖如圖1所示，機(jī)器人移動平臺從某一位置出發(fā)，按照一定的路徑在未知環(huán)境中行進(jìn)。每一時(shí)刻，機(jī)器人移動平臺利用分析傳感器采集到的信息得到的結(jié)果，同步計(jì)算出機(jī)器人移動平臺的位置和周圍環(huán)境地圖的特征信息，完成定位和對地圖更新的任務(wù)。理論上，SLAM的狀態(tài)信息誤差和初始誤差及觀測誤差相關(guān)。圖1中，xk為機(jī)器人移動平臺在k時(shí)刻的狀態(tài)，Zk為k時(shí)刻機(jī)器人移動平臺的觀測值，mj為環(huán)境地圖中的特征信息，uk為機(jī)器人移動平臺在k時(shí)刻的控制輸入信號。

圖1 SLAM實(shí)現(xiàn)流程圖Fig.1 Flow chart of SLAM implementation

基于EKF的SLAM算法狀態(tài)矩陣包含機(jī)器人移動平臺的位姿以及環(huán)境中的路標(biāo)特征

(1)

其中，機(jī)器人移動平臺的位姿為

(2)

環(huán)境路標(biāo)特征為

(3)

若為點(diǎn)特征，則用其在世界坐標(biāo)系的坐標(biāo)表示；若為線特征，則用機(jī)器人到線段的直線距離以及觀測線段的起始角和終止角來表示。本文在討論時(shí)著重于線特征的描述。

基于EKF的SLAM算法框架如圖2所示。

圖2 SLAM算法框架圖Fig.2 SLAM algorithm framework diagram

狀態(tài)協(xié)方差矩陣表述了狀態(tài)信息矩陣中各個(gè)變量之間的聯(lián)系[14]。在協(xié)方差矩陣中，主對角元素描述了對應(yīng)狀態(tài)的協(xié)方差，體現(xiàn)了對狀態(tài)變量估計(jì)結(jié)果可能的變化動態(tài)范圍；其余元素描述了不同狀態(tài)變量之間的協(xié)方差，體現(xiàn)了兩者之間的聯(lián)系。正是由于各個(gè)變量之間存在的這種聯(lián)系，才可以通過對重復(fù)信息的不斷觀測對之前得到的結(jié)果進(jìn)行修正，減小誤差。

1.1 機(jī)器人運(yùn)動模型

在SLAM問題中，機(jī)器人載體的運(yùn)動模型被認(rèn)為是一個(gè)遞歸的過程，下一時(shí)刻的狀態(tài)信息可以由上一時(shí)刻的狀態(tài)信息來預(yù)測[15]。機(jī)器人的運(yùn)動模型描述的是機(jī)器人的狀態(tài)信息的動態(tài)變化規(guī)律，是指存在干擾噪聲w和控制信號uk的情況下隨時(shí)間的變化規(guī)律。常用離散化后的模型為

xk=f(xk-1,uk)+w

(4)

其中，函數(shù)f(·)為系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)，大部分情況下是線性的；xk表示的是k時(shí)刻系統(tǒng)的狀態(tài)信息；uk表示的是控制信號輸入；w是干擾噪聲，包括輪子打滑和參數(shù)漂移等。

對機(jī)器人移動平臺進(jìn)行建模，簡化后的模型只考慮橫軸、縱軸坐標(biāo)以及機(jī)器人移動平臺的航向角這3個(gè)狀態(tài)量，同時(shí)將各個(gè)通道的噪聲假設(shè)為高斯白噪聲。機(jī)器人移動平臺簡化后的運(yùn)動模型為

(5)

其中，Vk是機(jī)器人移動平臺的行進(jìn)速度；ak是機(jī)器人移動平臺的前輪轉(zhuǎn)角；T是機(jī)器人移動平臺傳感器的采樣處理周期；L是機(jī)器人移動平臺的2個(gè)輪軸之間的距離；vx、vy、vΦ是各個(gè)通道的噪聲,用于描述環(huán)境噪聲等對模型精確度的影響。

其中，狀態(tài)轉(zhuǎn)移函數(shù)對狀態(tài)信息矩陣和控制信號的雅可比矩陣為

(6)

(7)

1.2 觀測模型

利用航位推算的機(jī)器人定位算法往往會隨著時(shí)間的變化而不斷積累單調(diào)上升，利用傳感器對周圍環(huán)境進(jìn)行感知來對估計(jì)算法進(jìn)行校正可提高定位的精度[16]。由于激光傳感器探測精度高、抗干擾能力強(qiáng)以及測量范圍廣泛，本文采用激光傳感器作為觀測手段。

激光傳感器的輸出值是障礙物和激光儀之間的距離以及和激光儀始發(fā)處的角度，即極徑和極角。

Z=[rθ]T

(8)

其中，Z是觀測器的輸出信號。

觀測器的量測模型表述了機(jī)器人位姿和環(huán)境中障礙物兩者之間的相對關(guān)系。量測方程如下

Zk=h(Xk)+wk

(9)

其中，Zk是k時(shí)刻激光儀的輸出信號；h(·)是量測函數(shù)；wk是量測噪聲，表述環(huán)境噪聲和模型的精確度的影響。

在本文的SLAM問題中，量測模型可寫為

(10)

其中，(xi,yi)描述的是在探測到的特征中的第i個(gè)特征在世界坐標(biāo)系中的位置。

量測模型相對于狀態(tài)矩陣的雅可比矩陣為

(11)

其中

(12)

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

路標(biāo)在世界地圖中的位置可用坐標(biāo)原點(diǎn)在直線上的投影點(diǎn)的坐標(biāo)來表示，稱為路標(biāo)特征點(diǎn)。在k時(shí)刻，機(jī)器人觀測到路標(biāo)時(shí)首先需要判斷觀測到的路標(biāo)是否已經(jīng)記錄在狀態(tài)矩陣中。常用的判斷方法是計(jì)算觀測路標(biāo)的特征點(diǎn)和狀態(tài)矩陣中的特征點(diǎn)之間的歐氏距離，當(dāng)小于設(shè)定的閾值時(shí)，認(rèn)為兩者是同一路標(biāo)，否則是新的路標(biāo)。

但是，歐氏距離在數(shù)據(jù)分析中有2個(gè)缺陷。第一，當(dāng)兩點(diǎn)固定時(shí)，歐式距離并不是固定不變的，當(dāng)選用不同量綱時(shí)得到的結(jié)果也會不同；第二，當(dāng)2個(gè)變量之間存在某種聯(lián)系時(shí)，歐氏距離并不能體現(xiàn)出這種聯(lián)系。因此，本文引入數(shù)據(jù)之間的協(xié)方差陣進(jìn)行計(jì)算，公式如下

D2(x,y)=(x-y)TV-1(x-y)

(13)

其中，x和y表示狀態(tài)矩陣中的特征點(diǎn)坐標(biāo)和觀測路標(biāo)的特征點(diǎn)坐標(biāo)，V表示觀測引起的方差。

當(dāng)計(jì)算距離大于閾值時(shí)，判斷為新的路標(biāo)，將觀測路標(biāo)的特征點(diǎn)加入狀態(tài)矩陣中擴(kuò)充狀態(tài)矩陣；當(dāng)距離小于閾值時(shí)，判斷為已經(jīng)存在的路標(biāo)。此時(shí)，觀測到的路標(biāo)特征點(diǎn)為觀測信息，因?yàn)槁窐?biāo)為靜態(tài)，所以特征點(diǎn)保持不變，即轉(zhuǎn)移矩陣為單位陣，利用EKF方程進(jìn)行狀態(tài)矩陣更新。

2 線特征模擬觀測器構(gòu)建

在基于點(diǎn)特征的SLAM仿真時(shí)，可以通過判斷點(diǎn)到機(jī)器人的距離和夾角是否滿足設(shè)定的要求來判斷點(diǎn)特征是否被觀測到，然后通過人為地加上隨機(jī)噪聲來模擬實(shí)際的觀測過程。在線段特征的模擬觀測時(shí)，可以把線段特征拆分為點(diǎn)特征，按照點(diǎn)特征模擬觀測的方法進(jìn)行模擬觀測。但是，線段特征是由無數(shù)個(gè)點(diǎn)的特征組合到一起的。若把一個(gè)線段拆分為幾十甚至上百個(gè)點(diǎn)，那么一條線段特征的計(jì)算量將變的很大，更不用說幾個(gè)、十幾個(gè)線段特征組成的模擬環(huán)境。

二分法是一種通過不斷利用排除法，將不可能的解集排除在外，通過不斷排除減小函數(shù)的解區(qū)間來得到滿足誤差條件的解的近似值的算法，而對于線段特征的模擬觀測過程可以理解為函數(shù)求零點(diǎn)的過程。設(shè)路標(biāo)上的點(diǎn)到機(jī)器人的直線距離為d，觀測器的模擬觀測最大距離為Dmax，假設(shè)存在以下函數(shù)F=d-Dmax。那么，函數(shù)的零點(diǎn)的含義是模擬觀測器可觀測點(diǎn)所到的范圍的臨界值，當(dāng)d繼續(xù)增加時(shí)，路標(biāo)上的點(diǎn)到機(jī)器人位置之間的距離將大于模擬觀測器的最大觀測距離，線段特征上的點(diǎn)將不會再被觀測到。因此，對于滿足函數(shù)條件F<0的線段特征上的點(diǎn)將被觀測到。SLAM中判斷線段特征能被觀測到的范圍的問題，轉(zhuǎn)化為了求解函數(shù)F>0的解集的問題。線特征模擬觀測器構(gòu)建過程可根據(jù)垂足的位置分別進(jìn)行構(gòu)建。其中，G表示載體機(jī)器人，AB表示線特征，O表示垂足。

如圖3所示，垂足在線段以外。AB上的點(diǎn)到G點(diǎn)的距離從點(diǎn)A到點(diǎn)B單調(diào)遞增。若點(diǎn)A到G點(diǎn)的距離大于Dmax，那么線段將不能被觀測到；若點(diǎn)A到G點(diǎn)的距離小于Dmax，且點(diǎn)B到G點(diǎn)的距離也小于Dmax，則線段特征將能被全部觀測到；若點(diǎn)A到G點(diǎn)的距離小于Dmax，點(diǎn)B到G點(diǎn)的距離大于Dmax，那么將會滿足二分法中對函數(shù)f(a)·f(b)<0的要求，可以應(yīng)用二分法求解臨界點(diǎn)，點(diǎn)A到臨界點(diǎn)的線段特征將能被觀測到。此處以垂足在左側(cè)為例，垂足在右側(cè)同理。

圖3 模擬觀測器(一)Fig.3 Analog observer (I)

如圖4所示，垂足在線段以內(nèi)。此時(shí)，OB段長于OA段，則只需判斷OB段即可。若OG距離大于Dmax，那么該線段特征將不能被觀測到；若OG距離小于Dmax，且BG距離也小于Dmax，那么線段特征將能被全部觀測到；若OG距離小于Dmax，BG距離大于Dmax，那么可以利用二分法得到OB段上的臨界點(diǎn)，并求得關(guān)于O點(diǎn)的對稱點(diǎn)，若對稱點(diǎn)在A點(diǎn)左側(cè)，則A點(diǎn)到臨界點(diǎn)的線段特征能被觀測到，否則對稱點(diǎn)到臨界點(diǎn)的線段特征能被觀測到。此處以垂足在左側(cè)為例，垂足在右側(cè)同理。

圖4 模擬觀測器(二)Fig.4 Analog observer (II)

在SLAM仿真時(shí)，機(jī)器人運(yùn)動過程中傳感器采集到的速度信息通過機(jī)器人的運(yùn)動模型和設(shè)置好的運(yùn)動軌跡加上白噪聲進(jìn)行模擬。進(jìn)行地圖構(gòu)建時(shí)，每一時(shí)刻利用模擬采集到的速度信息和機(jī)器人運(yùn)動模型由上一時(shí)刻信息推測出機(jī)器人當(dāng)前時(shí)刻的位置，利用本文構(gòu)建的模擬觀測器可以得到觀測信息，通過數(shù)據(jù)關(guān)聯(lián)過程完成地圖的更新和擴(kuò)充，最后達(dá)到地圖構(gòu)建的目的。

3 仿真驗(yàn)證

仿真算法流程如圖5所示。基于二分法的仿真實(shí)驗(yàn)需要設(shè)置的內(nèi)容包括機(jī)器人參數(shù)、路標(biāo)參數(shù)和軌跡設(shè)定參數(shù)。機(jī)器人參數(shù)設(shè)置為，速度V=3m/s，速度誤差0.3m/s，車輪最大轉(zhuǎn)速(π/9)rad/s,最大轉(zhuǎn)向角30°，轉(zhuǎn)向角誤差3°，采樣周期0.025s，激光雷達(dá)探測半徑30m，探測距離誤差0.1m，角度誤差1°。路標(biāo)參數(shù)設(shè)置分別為線段的左端橫坐標(biāo)、右端橫坐標(biāo)、斜率和截距。圖6中的紅色線段為路標(biāo)，編號為1～4。參數(shù)為1(-30，0，2/3，30)，2(0,20，-1，20)，3(0,60，7/9，-35)，4(-60,0，-0.7，-35)。機(jī)器人的軌跡設(shè)定為起點(diǎn)(0,35)，然后分別向(25,0)、(0，-20)、(-35，0)、(-35,10)和(0,35)5個(gè)點(diǎn)運(yùn)動，轉(zhuǎn)向參數(shù)見機(jī)器人參數(shù)設(shè)置。仿真后的結(jié)果如圖6所示，中間構(gòu)成回路的紅色線段為機(jī)器人真實(shí)軌跡，藍(lán)色線段表示SLAM算法得到的軌跡和線特征。圖7所示為軌跡估計(jì)的誤差圖。

圖5 仿真算法流程圖Fig.5 Flow chart of simulation algorithm

圖6 模擬觀測器仿真結(jié)果(一)Fig.6 Simulation results of analog observer (I)

圖7 模擬觀測器仿真誤差(一)Fig.7 Simulation error of analog observer (I)

當(dāng)線特征與機(jī)器人之間的距離大于雷達(dá)探測范圍時(shí)被稱為超限。加大3號線特征的長度，使線段左右側(cè)端點(diǎn)的橫坐標(biāo)由45增加到60，如圖8中紅色虛線圈所示。超限仿真結(jié)果如圖8所示，圖9所示為超限仿真的誤差分析圖。

圖8 模擬觀測器仿真結(jié)果(二)Fig.8 Simulation results of analog observer (II)

圖9 模擬觀測器仿真誤差(二)Fig.9 Simulation error of analog observer (II)

未出現(xiàn)超限時(shí)，如圖6所示，模擬觀測器可以觀測出整條線段完成SLAM仿真；出現(xiàn)超限時(shí)，由仿真結(jié)果圖8可以看出，觀測器觀測結(jié)果(藍(lán)線)較圖6的結(jié)果最右側(cè)坐標(biāo)延長至54.4412，但并未達(dá)到60，即未完全觀測到延長后的線段。由于圖6對應(yīng)的線段較短，線段整體都在觀測器可觀測范圍內(nèi)，所以在圖8中當(dāng)線段延長后，觀測器觀測到的線段右端點(diǎn)橫坐標(biāo)延長至54.4412；但是由于線段較長，存在不可被觀測到的范圍，所以觀測器觀測到的線段右端點(diǎn)橫坐標(biāo)未達(dá)到60，這符合預(yù)期的實(shí)驗(yàn)結(jié)果。

上述兩種情況下的仿真結(jié)果表明，基于線特征模擬觀測器的SLAM仿真實(shí)驗(yàn)，即使出現(xiàn)超限情況，仍然可以得到較為準(zhǔn)確的結(jié)果，除去未在觀測范圍內(nèi)的部分線特征外，其余環(huán)境路標(biāo)特征估計(jì)和機(jī)器人定位仍然可行。實(shí)驗(yàn)結(jié)果驗(yàn)證了基于二分法構(gòu)建的線特征模擬觀測器的正確性。

基于二分法和逐點(diǎn)法的仿真運(yùn)行時(shí)間如表1所示，前三行是超限時(shí)的運(yùn)行時(shí)間統(tǒng)計(jì)，后三行是正常時(shí)的運(yùn)行時(shí)間統(tǒng)計(jì)。從表1中可以看出，基于二分法的仿真算法縮短了計(jì)算時(shí)間，提高了運(yùn)行速度。

表1 算法運(yùn)行時(shí)間對比

4 結(jié)論

本文針對線特征模擬觀測時(shí)數(shù)據(jù)量過大的問題，提出了一種模擬觀測器的構(gòu)建方法。算法分析和仿真實(shí)驗(yàn)結(jié)果表明：

1)將觀測器對路標(biāo)的觀測問題轉(zhuǎn)換為函數(shù)零點(diǎn)求解問題。本文提出的線特征觀測器利用二分法近似求解得到線特征的被觀測范圍，為SLAM問題中對觀測問題的研究提供了新的思路。

2)相比于點(diǎn)特征觀測器的判斷過程，基于二分法的線特征模擬觀測器可大量減小運(yùn)算量。在滿足誤差條件的前提下，具有較好的實(shí)時(shí)性。

3)本文提出的線特征模擬觀測器的運(yùn)算量隨著誤差的減小而逐漸增加，在高精度的場合會影響算法的實(shí)時(shí)性，因此需要進(jìn)一步改進(jìn)。針對高精度的要求，需要其他方法的輔助來進(jìn)一步降低運(yùn)算復(fù)雜度以達(dá)到實(shí)時(shí)性的目的。