夏 磊，郝 鋼

(黑龍江大學(xué) 電子工程學(xué)院,哈爾濱 150080)

0 引 言

移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建(Simultaneous Localization and Mapping，SLAM)可以描述為：移動(dòng)機(jī)器人在未知環(huán)境下依靠傳感器設(shè)備獲取環(huán)境信息，并利用這些環(huán)境信息增量式地構(gòu)建環(huán)境地圖和自主定位[1]。SLAM最先由Smith R C和Cheeseman P提出，并結(jié)合擴(kuò)展卡爾曼濾波(EKF)來解決移動(dòng)機(jī)器人SLAM(EKF-SLAM)問題[2]。EKF-SLAM算法的實(shí)質(zhì)是對(duì)非線性的系統(tǒng)模型和觀測(cè)模型一階泰勒級(jí)數(shù)展開，將模型線性化后再通過標(biāo)準(zhǔn)的卡爾曼濾波處理，這種方法實(shí)現(xiàn)簡(jiǎn)單，但是會(huì)引入較大的截?cái)嗾`差，并且計(jì)算量較大。EKF算法在執(zhí)行時(shí)需要假設(shè)系統(tǒng)服從高斯分布，且存在線性化問題[3]，因此，文獻(xiàn)[4]將無跡卡爾曼濾波(unscented Kalman filter，UKF)引入SLAM領(lǐng)域，在保證復(fù)雜度同階的情況下，通過無跡變換計(jì)算估計(jì)SLAM問題中的后驗(yàn)密度。但利用UKF處理高維數(shù)(維數(shù)n> 4)系統(tǒng)時(shí)，濾波過程中協(xié)方差可能出現(xiàn)非正定情況，導(dǎo)致濾波數(shù)值不穩(wěn)定甚至發(fā)散，需要“調(diào)整”UKF中的參數(shù)。為克服UKF在高維系統(tǒng)中出現(xiàn)的數(shù)值不穩(wěn)定及精度降低問題，英國(guó)萊斯特大學(xué)的研究人員提出了基于容積卡爾曼濾波(CKF)的SLAM算法[5]，降低了估計(jì)誤差，但是在遞推過程中，存在計(jì)算量大、數(shù)值不穩(wěn)定等情況。

隨著粒子濾波(Particle Filter,PF)的出現(xiàn)及其在非線性系統(tǒng)中表現(xiàn)出的高估計(jì)精度性能，粒子濾波很快被用于SLAM系統(tǒng)(PF-SLAM)。PF-SLAM是將聯(lián)合后驗(yàn)概率分布因式分解為機(jī)器人路徑部分及以機(jī)器人路徑為條件的地圖部分[6]。在實(shí)際應(yīng)用中，PF-SLAM算法由于環(huán)境中路標(biāo)的增加會(huì)產(chǎn)生PF-SLAM算法狀態(tài)向“級(jí)數(shù)災(zāi)難”問題，導(dǎo)致求解計(jì)算過程十分復(fù)雜。文獻(xiàn)[7]將Rao-Blackwellised粒子濾波器應(yīng)用于SLAM算法中，提出一種RBPF-SLAM算法。該算法將機(jī)器人定位和建圖進(jìn)行分解[8-9]，用粒子濾波器對(duì)機(jī)器人實(shí)現(xiàn)定位，卡爾曼濾波器進(jìn)行建圖。RBPF進(jìn)行定位和建圖需要較多的粒子，計(jì)算量較大，并且頻繁執(zhí)行重采樣造成粒子退化。文獻(xiàn)[10]基于RBPF-SLAM提出一種激光SLAM算法Gmapping，該算法已成功用于基于激光雷達(dá)的SLAM算法之中。由于里程計(jì)易受外部環(huán)境的干擾，該算法將高精度、魯棒性較強(qiáng)的激光雷達(dá)測(cè)量數(shù)據(jù)加入到建議分布求取中，使建議分布更接近實(shí)際后驗(yàn)分布，Gmapping在重采樣的時(shí)通過閾值判斷是否進(jìn)行重采樣，從而減少了粒子數(shù)，提高了算法的準(zhǔn)確度和效率。但隨著場(chǎng)景增大所需的粒子增加，每個(gè)粒子攜帶一幅地圖，在構(gòu)建大地圖時(shí)所需內(nèi)存和計(jì)算量增加，Gmapping算法不適合構(gòu)建大場(chǎng)景地圖。

現(xiàn)有的基于激光雷達(dá)的經(jīng)典的SLAM算法多數(shù)采用兩段方式進(jìn)行同時(shí)定位與地圖創(chuàng)建，即定位-建圖。定位階段基于里程計(jì)或者慣性傳感器(IMU)，結(jié)合激光雷達(dá)修正累積誤差。建圖階段依賴定位數(shù)據(jù)，基于激光雷達(dá)進(jìn)行地圖構(gòu)建。顯然兩個(gè)階段存在耦合，不能很好地解決定位階段產(chǎn)生的累積誤差，以及定位誤差帶來的地圖形變?；谝陨显?，提出了一種基于激光雷達(dá)和超寬帶(UWB)定位的SLAM算法，該算法通過采用UWB技術(shù)[11-12]和CKF濾波來獲取目標(biāo)的位姿。通過車載激光雷達(dá)獲取路標(biāo)點(diǎn)與目標(biāo)的距離和角度，用泰勒展開更新觀測(cè)方程，再結(jié)合容積Kalman濾波器(CKF)得到路標(biāo)點(diǎn)的位置。該算法流程見圖1。

圖1 算法流程Fig.1 Algorithm flow chart

提出的系統(tǒng)采用UWB定位網(wǎng)絡(luò)，避免了定位階段的累積誤差，為高精度建圖提供了良好支撐。定位算法以及建圖算法采用了CKF，降低了系統(tǒng)的計(jì)算負(fù)擔(dān)?；赟LAM算法原理見圖2，其中UWB基站為基站A、B和C，負(fù)責(zé)目標(biāo)的定位；移動(dòng)目標(biāo)帶有UWB標(biāo)簽和2D激光雷達(dá)，激光雷達(dá)可以根據(jù)檢測(cè)的障礙物的距離R以及角度α構(gòu)建地圖信息。

圖2 基于激光雷達(dá)和UWB的SLAM算法原理Fig.2 Schematic of SLAM algorithm based on Lidar and UWB

1 基于UWB和濾波算法的定位算法

1.1 CKF濾波算法

系統(tǒng)的狀態(tài)方程與觀測(cè)方程可描述為

(1)

其中，xk-1為在k-1時(shí)刻系統(tǒng)的狀態(tài)；fk-1(·)為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)；hk(·)和zk分別為k時(shí)刻系統(tǒng)傳感器的觀測(cè)函數(shù)和觀測(cè)值，且假設(shè)wk和vk為均值為零的互不相關(guān)的白噪聲，Qk為wk的方差，Rk為vk的方差。

2009年，Arasaratnam提出了一種新的非線性濾波算法容積卡爾曼濾波(cubature Kalman filter，CKF)[13]。CKF根據(jù)容積準(zhǔn)則，通過一組具有相同權(quán)重的點(diǎn)經(jīng)過非線性系統(tǒng)方程轉(zhuǎn)換后產(chǎn)生新的點(diǎn)來給出下一時(shí)刻系統(tǒng)狀態(tài)的預(yù)測(cè)，避免了對(duì)非線性模型的線性化處理，其精度達(dá)Taylor級(jí)數(shù)展開3階[14]，因此CKF較EKF的精度更高。CKF主要是由時(shí)間更新和觀測(cè)更新兩個(gè)部分組成：

時(shí)間更新：在k-1時(shí)刻的后驗(yàn)概率密度為

(2)

將Pk-1|k-1進(jìn)行Cholesky分解可得

(3)

k-1時(shí)刻的容積點(diǎn)：

(4)

其中，點(diǎn)集ξi為

(5)

計(jì)算經(jīng)系統(tǒng)方程傳遞后的容積點(diǎn)：

(6)

估計(jì)k時(shí)刻的狀態(tài)預(yù)測(cè)值：

(7)

估計(jì)k時(shí)刻時(shí)的誤差協(xié)方差陣：

(8)

觀測(cè)更新：對(duì)Pk|k-1進(jìn)行Cholesky分解：

(9)

計(jì)算新的容積點(diǎn)：

(10)

k時(shí)刻的觀測(cè)預(yù)測(cè)值為

Zi,k|k-1=hk(Xi,k|k-1,0)

(11)

k時(shí)刻的觀測(cè)值為

(12)

k時(shí)刻的自相關(guān)協(xié)方差陣為

(13)

k時(shí)刻的互協(xié)方差陣為

(14)

系統(tǒng)在k時(shí)刻的增益估計(jì)為

(15)

k時(shí)刻的狀態(tài)估計(jì)值為

(16)

k時(shí)刻的誤差協(xié)方差為

(17)

1.2 結(jié)合UWB和CKF的濾波定位算法

UWB技術(shù)是利用頻譜極寬的超寬基帶脈沖進(jìn)行通信，具有低功耗、強(qiáng)穿透力、高時(shí)間分辨率和高傳輸數(shù)據(jù)速率等優(yōu)勢(shì)，而且可覆蓋較大的范圍。UWB技術(shù)主要應(yīng)用在室內(nèi)定位和測(cè)距領(lǐng)域，誤差為10～30 cm[15-16]。

目前無線定位方法主要是通過已知坐標(biāo)的定位基站和未知坐標(biāo)的標(biāo)簽進(jìn)行通信，再將得到的測(cè)距信息進(jìn)行相應(yīng)的解算從而得到標(biāo)簽的坐標(biāo)[17-22]。本文UWB測(cè)距原理采用飛行時(shí)間測(cè)距法(Time of Flight,TOF)[23-26]中的雙邊雙向測(cè)距(Double-side Two-way Ranging)方式[27]。基于測(cè)距的定位方法可分為三角解算與濾波解算兩種。其中三角解算法是在獲取測(cè)距信息后，按照數(shù)學(xué)公式對(duì)位置坐標(biāo)進(jìn)行解算。三角解算法優(yōu)點(diǎn)是計(jì)算簡(jiǎn)單，實(shí)時(shí)性好，但由于傳感器誤差等因素，通常情況下3個(gè)基站的測(cè)距半徑無法交于一點(diǎn)，需要估算。在沒有更多先驗(yàn)概率的情況下，估計(jì)點(diǎn)經(jīng)常位于幾何圖形的中心，顯然是不合理的。而濾波方法可根據(jù)更多的先驗(yàn)概率(各個(gè)傳感器誤差方差等)合理地估計(jì)被測(cè)目標(biāo)的位置信息，因此濾波方法得到的位置估計(jì)更加準(zhǔn)確可靠。

將選用CKF濾波解算法對(duì)目標(biāo)進(jìn)行位置解算，考慮如下具有M個(gè)傳感器的離散非線性隨機(jī)系統(tǒng)：

xC(k+1)=f(xC(k))+ΓCwC(k)

(18)

(19)

(20)

其中，Ε為數(shù)學(xué)期望，δkt和δil為克羅內(nèi)克函數(shù)，即δkk=1，δkt=0(k≠t)。

設(shè)平面定位系統(tǒng)有M≥3個(gè)基站，目標(biāo)在k時(shí)刻接收到M個(gè)基站的測(cè)距信息，因此系統(tǒng)的測(cè)量方程為

(21)

式中，(x(i),y(i))為已知的第i個(gè)基站的坐標(biāo)(i=1,…,M)；(xC(k),yC(k))為k時(shí)刻目標(biāo)的坐標(biāo)。采用集中式融合方式將各個(gè)傳感器測(cè)量方程增廣到集中式觀測(cè)融合方程：

(22)

(23)

h(0)(xC(k))=[(h(1)(xC(k)))Τ,…,(h(3)(xC(k)))Τ]Τ

(24)

(25)

(26)

其中，diag(·)為對(duì)角矩陣。將式(18)和式(22)代入CKF濾波算法，可解算出目標(biāo)的位姿。

2 激光雷達(dá)估計(jì)路標(biāo)點(diǎn)位置

激光雷達(dá)具有極高的距離分辨率、角分辨率和速度分辨率，以及極強(qiáng)的抗干擾能力。2D激光雷達(dá)可測(cè)量周圍障礙物的距離信息以及角度信息。UWB定位方法獲取目標(biāo)的位姿后，激光雷達(dá)可獲取當(dāng)前時(shí)刻目標(biāo)在激光雷達(dá)檢測(cè)范圍內(nèi)與路標(biāo)點(diǎn)之間的距離和角度，并利用這兩個(gè)信息定位路標(biāo)點(diǎn)在世界坐標(biāo)系下的位置。

設(shè)在世界坐標(biāo)系下環(huán)境路標(biāo)是靜止的，因此，路標(biāo)點(diǎn)定位模型為

xL(k+1)=xL(k)

(27)

(28)

(29)

(30)

(31)

由于vL為白噪聲，且與wC相互獨(dú)立，則vd與vL相互獨(dú)立，式(31)可寫為

(32)

路標(biāo)點(diǎn)觀測(cè)模型更新為

(33)

且vs(k)∈N(0,Rs)。將式(27)和式(33)代入CKF濾波算法，可解算出路標(biāo)點(diǎn)的坐標(biāo)。

3 仿真與分析

設(shè)目標(biāo)在22 m×22 m的平面做勻轉(zhuǎn)彎率運(yùn)動(dòng)(CT模型)。UWB定位系統(tǒng)共有3個(gè)基站，并設(shè)置了6個(gè) 路標(biāo)點(diǎn)。目標(biāo)的系統(tǒng)模型為

xC(k+1)=ΦCxC(k)+ΓCwC(k)

(34)

(35)

(36)

估計(jì)性能為k時(shí)刻路標(biāo)點(diǎn)位置的累積均方誤差(AMSE)：

(37)

圖3 目標(biāo)定位跟蹤Fig.3 Target location tracking

圖4 路標(biāo)點(diǎn)兩次估計(jì)AMSE對(duì)比Fig.4 Comparison of twice estimated AMSE for landmarks

4 結(jié) 論

提出了一種基于激光雷達(dá)和UWB的SLAM算法，該算法將整個(gè)SLAM過程分解為定位和建圖兩部分，采用兩段容積卡爾曼濾波對(duì)目標(biāo)和路標(biāo)點(diǎn)進(jìn)行估計(jì)，用UWB定位系統(tǒng)，結(jié)合CKF和集中式融合估計(jì)目標(biāo)位姿；估計(jì)路標(biāo)點(diǎn)時(shí)分為兩種方法：①利用CKF估計(jì)路標(biāo)點(diǎn)位置，②在CKF濾波估計(jì)的基礎(chǔ)上對(duì)第一段CKF估計(jì)目標(biāo)位姿產(chǎn)生的誤差利用泰勒展開進(jìn)行誤差補(bǔ)償。在相同的實(shí)驗(yàn)條件下，對(duì)這兩種方法進(jìn)行了仿真實(shí)驗(yàn)對(duì)比。仿真結(jié)果表明，引入誤差補(bǔ)償提高了估計(jì)路標(biāo)點(diǎn)的精確度。