基于擴(kuò)展卡爾曼濾波的同步定位與地圖構(gòu)建（SL A M）算法研究進(jìn)展

劉 暢

（天津職業(yè)技術(shù)師范大學(xué)機(jī)械工程學(xué)院，天津300222）

同步定位與地圖構(gòu)建（Simultaneous location and mapping，SLAM），指身處未知環(huán)境的機(jī)器人通過自身傳感器信息實(shí)現(xiàn)定位并生成環(huán)境增量式地圖的過程[1]。該問題是概率機(jī)器人學(xué)領(lǐng)域的重要問題，是利用基于模型的方法與基于行為的方法相結(jié)合的形式來處理機(jī)器人在未知環(huán)境自主移動(dòng)問題的一門學(xué)科，是移動(dòng)機(jī)器人實(shí)現(xiàn)智能的根本[2]。大疆科技工程師表示：“關(guān)于無人機(jī)的所有美好愿景都基于SLAM技術(shù)，SLAM技術(shù)是區(qū)分無人機(jī)與玩具的標(biāo)準(zhǔn)，是無人機(jī)進(jìn)入能飛時(shí)代后最核心的技術(shù)”[3]。SLAM問題的難點(diǎn)在于其系統(tǒng)誤差的高度耦合性，定位需要精準(zhǔn)的地圖，同時(shí)構(gòu)建精準(zhǔn)的地圖需要精確的機(jī)器人位姿。因此，早期對(duì)于SLAM問題的研究的主要內(nèi)容在于累積噪聲的消除。

1 SL A M問題的數(shù)學(xué)模型

同步定位與建圖是可以歸結(jié)為狀態(tài)估計(jì)問題，其核心在于已知含有噪聲的機(jī)器人控制量、環(huán)境觀測(cè)信息來估算環(huán)境特征和機(jī)器人路徑并使其誤差達(dá)到最小，其數(shù)學(xué)描述如圖1所示[4]。

其中，xk代表k時(shí)刻表示機(jī)器人位姿的狀態(tài)向量；mi表示機(jī)器人觀測(cè)到的路標(biāo)點(diǎn)，即環(huán)境特征；uk為機(jī)器人的控制向量；zk，*為機(jī)器人在k時(shí)刻觀測(cè)向量的集合，其中*指代路標(biāo)點(diǎn)的序號(hào)，如在k-1時(shí)刻觀測(cè)向量為：zk-1，i，zk-1，i.采用集合的方式表述上述變量：

所有環(huán)境特征的集合為 m1∶l＝ {m1，……，ml}；k 時(shí)刻之前所有觀測(cè)向量的集合為 z1∶k＝ {z1，……，zk}；k時(shí)刻之前所有控制向量的集合為u1∶k＝{u1，……，uk}.

假設(shè)SLAM問題中的運(yùn)動(dòng)與觀測(cè)模型遵循馬爾可夫假設(shè)，即未來與過去數(shù)據(jù)相互獨(dú)立，所以可將其描述為關(guān)于路徑與環(huán)境地圖的估計(jì)，其概率模型為：P（xk，m1∶l｜z1∶k，u1∶k）.

2 SL A M問題的研究方法

早期的SLAM算法為基于擴(kuò)展卡爾曼濾波的EKFSLAM算法。卡爾曼濾波器是遞推算法的一種，通過實(shí)時(shí)獲得的受噪聲污染的離散觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行線性、無偏及最小誤差方差的最優(yōu)估計(jì)[5]。

首先，通過運(yùn)動(dòng)方程及觀測(cè)方程對(duì)SLAM過程進(jìn)行描述，有

其中，wk、vk，j為噪聲項(xiàng)，yj為路標(biāo)點(diǎn)坐標(biāo)值。假設(shè)整個(gè)SLAM系統(tǒng)為線性高斯系統(tǒng)，有：

其中x為包含機(jī)器人位姿信息及地圖信息的聯(lián)合狀態(tài)矢量，Ak為狀態(tài)轉(zhuǎn)移矩陣，Ck為系統(tǒng)觀測(cè)矩陣。根據(jù)高斯分布性質(zhì)有：

此外，根據(jù)觀測(cè)方程有;

根據(jù)SLAM系統(tǒng)概率模型得出，k時(shí)刻狀態(tài)量的估計(jì)滿足高斯分布，設(shè)其后驗(yàn)概率分布的均值與協(xié)方差分別為 ，則有：

由此可得：

設(shè)

則有：

同樣可得：

由此完成了k時(shí)刻狀態(tài)量的后驗(yàn)概率分布估計(jì)，該過程被稱為“更新”。卡爾曼濾波器通過控制量“預(yù)測(cè)”再利用觀測(cè)值“更新”，其結(jié)構(gòu)圖如圖2所示。

圖2 卡爾曼濾波算法

然而在實(shí)際應(yīng)用中所面臨的大部分觀測(cè)方程與運(yùn)動(dòng)方程為非線性函數(shù)。為此，研究人員將卡爾曼濾波器進(jìn)行推廣，得到適用于非線性系統(tǒng)的擴(kuò)展卡爾曼濾波器，其基本思想在于將觀測(cè)方程運(yùn)動(dòng)方程在特定點(diǎn)附近進(jìn)行泰勒展開，并只保留一階項(xiàng)，從而近似為線性系統(tǒng)，即

則有預(yù)測(cè)過程：

計(jì)算卡爾曼增益：

更新過程為：

在非線性優(yōu)化方法中，模型將軌跡中離散的所有位姿設(shè)為變量，將運(yùn)動(dòng)方程與觀測(cè)方程作為變量間約束，構(gòu)造代價(jià)函數(shù)：

其中，z指代觀測(cè)數(shù)據(jù)，在以視覺傳感器為數(shù)據(jù)來源的SLAM中指代像素坐標(biāo)（us，vs），ξ即相機(jī)外參R、t對(duì)應(yīng)的李代數(shù)形式，也即前面數(shù)學(xué)模型中的x，p為三維空間點(diǎn)坐標(biāo)，即數(shù)學(xué)模型中的的路標(biāo)m.

3 與機(jī)器學(xué)習(xí)相結(jié)合的SL A M研究

現(xiàn)有的SLAM經(jīng)典算法仍存在局限，基于EKF的SLAM算法存在引入線性化誤差、難以表達(dá)回環(huán)、對(duì)數(shù)據(jù)關(guān)聯(lián)失誤的魯棒性差，而基于非線性優(yōu)化方法的SLAM算法仍存在容易陷入局部最小，在巨大場(chǎng)景下計(jì)算效率低導(dǎo)致無法實(shí)時(shí)化。隨著人工神經(jīng)網(wǎng)絡(luò)學(xué)科的發(fā)展，研究人員開始尋找新的SLAM問題解決途徑：針對(duì)觀測(cè)運(yùn)動(dòng)方程運(yùn)動(dòng)方程高度非線性、實(shí)際物理模型不能完全獲取或系統(tǒng)參數(shù)變化等情況，文獻(xiàn)[6]中論述了一種利用神經(jīng)網(wǎng)絡(luò)構(gòu)建逼近函數(shù)e（X（k），u（k），β（k））的方法擬合實(shí)際模型與理論模型之間的誤差，并將該誤差加入到時(shí)間更新模型中，以提高系統(tǒng)狀態(tài)估計(jì)的精確度，其結(jié)構(gòu)如圖3所示。

圖3 神經(jīng)網(wǎng)絡(luò)復(fù)合卡爾曼濾波算法

在經(jīng)典的基于擴(kuò)展卡爾曼濾波的SLAM系統(tǒng)中，系統(tǒng)狀態(tài)狀態(tài)向量由機(jī)器人位姿向量Xv=[xv，yv，φv]T和環(huán)境特征位置向量 Xm=[xT1，xT2……xTm]組成：X=[xTv，xTm]T，系統(tǒng)協(xié)方差矩陣由機(jī)器人位姿協(xié)方差矩陣Pvv、環(huán)境特征協(xié)方差矩陣Pmm和機(jī)器人位姿與環(huán)境特征協(xié)方差矩陣Pvm組成神經(jīng)網(wǎng)絡(luò)復(fù)合卡爾曼濾波算法將前向神經(jīng)網(wǎng)絡(luò)的權(quán)值Xβ融入到SLAM模型中，將SLAM問題的維數(shù)擴(kuò)展，則系統(tǒng)狀態(tài)向量為：X=[Xv，Xβ，Xm]T，系統(tǒng)協(xié)方差矩陣中增加了機(jī)器人位姿、環(huán)境特征與神經(jīng)網(wǎng)絡(luò)權(quán)值的協(xié)方差 Pvβ及 Pβm，為

早期應(yīng)用的神經(jīng)網(wǎng)絡(luò)模型隱含層層數(shù)普遍較少，這限制了模型對(duì)系統(tǒng)分類、擬合的表現(xiàn)。而近年來，人工神經(jīng)網(wǎng)絡(luò)的規(guī)模大約每2.4年翻一番[7]，其歸根結(jié)底來源于計(jì)算機(jī)硬件更新的推動(dòng)，GPU（Graphics Processing Unit，圖形處理器）以其出色的浮點(diǎn)計(jì)算性能、高效的并行結(jié)構(gòu)為多層神經(jīng)網(wǎng)絡(luò)模型開發(fā)部署提供了理想平臺(tái)，推動(dòng)了深度學(xué)習(xí)的發(fā)展。與傳統(tǒng)人工神經(jīng)網(wǎng)絡(luò)相比，深度學(xué)習(xí)模型非線性操作層級(jí)數(shù)更多[8]，能夠構(gòu)建更為復(fù)雜的模型處理更高數(shù)量級(jí)的數(shù)據(jù)，從應(yīng)用上講，深度學(xué)習(xí)能夠處理更高層次、更抽象的特征，也因此部分學(xué)者致力于采用卷積神經(jīng)網(wǎng)絡(luò)等深度學(xué)習(xí)模型來代替?zhèn)鹘y(tǒng)的SLAM框架實(shí)現(xiàn)對(duì)于SLAM領(lǐng)域的顛覆，而其表現(xiàn)仍需等待時(shí)間來考證[9-12]。

[1]高 翔，張 濤，劉 毅，等.視覺SLAM十四講從理論到實(shí)踐[M].北京：電子工業(yè)出版社，2017.

[2]Sebastian Trun，Wolfram Burgard，Dieter Fox.概率機(jī)器人[M].北京：機(jī)械工業(yè)出版社，2017.

[3]常 建.無所不能的SLAM無人車、無人機(jī)、AR都離不開它[J].計(jì)算機(jī)與網(wǎng)絡(luò)，2017，43（5）：18-19.

[4]趙一路.移動(dòng)機(jī)器人SLAM問題研究[D].上海：復(fù)旦大學(xué)，2010.

[5]Charles K.Chui，Guanrong Chen.卡爾曼濾波及其實(shí)時(shí)應(yīng)用[M].北京：電子工業(yè)出版社清華大學(xué)出版社，2013.

[6]王忠善.移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2008.

[7]伊恩·古德費(fèi)洛，約書亞·本吉奧，亞倫·庫維爾.深度學(xué)習(xí)[M].北京：人民郵電出版社，2017.8.

[8]尹寶才，王文通，王立春.深度學(xué)習(xí)研究綜述[J].北京工業(yè)大學(xué)學(xué)報(bào)，2015，41（1）：48—59.

[9]Wang Sen，Ronald Clark ，Wen Hongkai et al.DeepVO：To wards End-to-End Visual Odometry with Deep Recurrent Convolutional Neural Networks[C]//2017 IEEE International Conference on Robotics and Automation（ICRA）.Singapore，2017：2043－2050.

[10]Antonio Loquercio，Marcin Dymczyk，Bernhard Zeisl et al.Efficient Descriptor Learning for Large Scale Localization[C]//2017 IEEE International Conference on Robotics and Au tomation（ICRA）.Singapore，2017：3170-3177.

[11]Gao Xiang，Zhang Tao.Loop Closure Detection for Visual SLAM Systems Using Deep Neural Networks[C]//Processing of the 34th Chinese Control Conference.Hangzhou，China，2017：5851－5856.

[12]John Mccormac，Ankur Handa，Andrew Davison et al.Se manticFusion：Dense 3D Semantic Mapping with Convolu tional Neural Networks[C]//2017 IEEE International Confer ence on Robotics and Automation（ICRA）.Singapore，2017：4628-4635.