基于二分法的EKF-SLAM算法

閆旭亮，楊功流，王 璐

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

0 引言

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

在眾多考量機(jī)器人的指標(biāo)中，機(jī)器人的移動(dòng)能力是決定其任務(wù)成敗的關(guān)鍵，機(jī)器人自由移動(dòng)的能力是建立在其對(duì)周圍環(huán)境特征的了解層次基礎(chǔ)上的[4-5]。只有當(dāng)移動(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ī)器人移動(dòng)平臺(tái)在行進(jìn)時(shí)創(chuàng)建環(huán)境地圖，同時(shí)根據(jù)創(chuàng)建的地圖進(jìn)行定位，利用2個(gè)信息的相互關(guān)系不斷地修正來(lái)改善推算得到的數(shù)據(jù)結(jié)果。同雞和蛋的問(wèn)題類似，定位和地圖創(chuàng)建之間相互聯(lián)系，精度互為相關(guān)。

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

1 基于EKF的SLAM算法

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

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

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

(1)

其中，機(jī)器人移動(dòng)平臺(tái)的位姿為

(2)

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

(3)

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

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

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

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

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

在SLAM問(wèn)題中，機(jī)器人載體的運(yùn)動(dòng)模型被認(rèn)為是一個(gè)遞歸的過(guò)程，下一時(shí)刻的狀態(tài)信息可以由上一時(shí)刻的狀態(tài)信息來(lái)預(yù)測(cè)[15]。機(jī)器人的運(yùn)動(dòng)模型描述的是機(jī)器人的狀態(tài)信息的動(dòng)態(tài)變化規(guī)律，是指存在干擾噪聲w和控制信號(hào)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表示的是控制信號(hào)輸入；w是干擾噪聲，包括輪子打滑和參數(shù)漂移等。

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

(5)

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

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

(6)

(7)

1.2 觀測(cè)模型

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

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

Z=[rθ]T

(8)

其中，Z是觀測(cè)器的輸出信號(hào)。

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

Zk=h(Xk)+wk

(9)

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

在本文的SLAM問(wèn)題中，量測(cè)模型可寫(xiě)為

(10)

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

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

(11)

其中

(12)

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

路標(biāo)在世界地圖中的位置可用坐標(biāo)原點(diǎn)在直線上的投影點(diǎn)的坐標(biāo)來(lái)表示，稱為路標(biāo)特征點(diǎn)。在k時(shí)刻，機(jī)器人觀測(cè)到路標(biāo)時(shí)首先需要判斷觀測(cè)到的路標(biāo)是否已經(jīng)記錄在狀態(tài)矩陣中。常用的判斷方法是計(jì)算觀測(cè)路標(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é)果也會(huì)不同；第二，當(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)和觀測(cè)路標(biāo)的特征點(diǎn)坐標(biāo)，V表示觀測(cè)引起的方差。

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

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

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

二分法是一種通過(guò)不斷利用排除法，將不可能的解集排除在外，通過(guò)不斷排除減小函數(shù)的解區(qū)間來(lái)得到滿足誤差條件的解的近似值的算法，而對(duì)于線段特征的模擬觀測(cè)過(guò)程可以理解為函數(shù)求零點(diǎn)的過(guò)程。設(shè)路標(biāo)上的點(diǎn)到機(jī)器人的直線距離為d，觀測(cè)器的模擬觀測(cè)最大距離為Dmax，假設(shè)存在以下函數(shù)F=d-Dmax。那么，函數(shù)的零點(diǎn)的含義是模擬觀測(cè)器可觀測(cè)點(diǎn)所到的范圍的臨界值，當(dāng)d繼續(xù)增加時(shí)，路標(biāo)上的點(diǎn)到機(jī)器人位置之間的距離將大于模擬觀測(cè)器的最大觀測(cè)距離，線段特征上的點(diǎn)將不會(huì)再被觀測(cè)到。因此，對(duì)于滿足函數(shù)條件F<0的線段特征上的點(diǎn)將被觀測(cè)到。SLAM中判斷線段特征能被觀測(cè)到的范圍的問(wèn)題，轉(zhuǎn)化為了求解函數(shù)F>0的解集的問(wèn)題。線特征模擬觀測(cè)器構(gòu)建過(guò)程可根據(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，那么線段將不能被觀測(cè)到；若點(diǎn)A到G點(diǎn)的距離小于Dmax，且點(diǎn)B到G點(diǎn)的距離也小于Dmax，則線段特征將能被全部觀測(cè)到；若點(diǎn)A到G點(diǎn)的距離小于Dmax，點(diǎn)B到G點(diǎn)的距離大于Dmax，那么將會(huì)滿足二分法中對(duì)函數(shù)f(a)·f(b)<0的要求，可以應(yīng)用二分法求解臨界點(diǎn)，點(diǎn)A到臨界點(diǎn)的線段特征將能被觀測(cè)到。此處以垂足在左側(cè)為例，垂足在右側(cè)同理。

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

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

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

在SLAM仿真時(shí)，機(jī)器人運(yùn)動(dòng)過(guò)程中傳感器采集到的速度信息通過(guò)機(jī)器人的運(yùn)動(dòng)模型和設(shè)置好的運(yùn)動(dòng)軌跡加上白噪聲進(jìn)行模擬。進(jìn)行地圖構(gòu)建時(shí)，每一時(shí)刻利用模擬采集到的速度信息和機(jī)器人運(yùn)動(dòng)模型由上一時(shí)刻信息推測(cè)出機(jī)器人當(dāng)前時(shí)刻的位置，利用本文構(gòu)建的模擬觀測(cè)器可以得到觀測(cè)信息，通過(guò)數(shù)據(jù)關(guān)聯(lián)過(guò)程完成地圖的更新和擴(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á)探測(cè)半徑30m，探測(cè)距離誤差0.1m，角度誤差1°。路標(biāo)參數(shù)設(shè)置分別為線段的左端橫坐標(biāo)、右端橫坐標(biāo)、斜率和截距。圖6中的紅色線段為路標(biāo)，編號(hà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)動(dòng)，轉(zhuǎn)向參數(shù)見(jiàn)機(jī)器人參數(shù)設(shè)置。仿真后的結(jié)果如圖6所示，中間構(gòu)成回路的紅色線段為機(jī)器人真實(shí)軌跡，藍(lán)色線段表示SLAM算法得到的軌跡和線特征。圖7所示為軌跡估計(jì)的誤差圖。

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

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

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

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

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

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

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

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

基于二分法和逐點(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í)間對(duì)比

4 結(jié)論

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

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

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

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