基于擴展卡爾曼濾波的同步定位與地圖構建（SL A M）算法研究進展

劉 暢

（天津職業技術師范大學機械工程學院，天津300222）

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

1 SL A M問題的數學模型

同步定位與建圖是可以歸結為狀態估計問題，其核心在于已知含有噪聲的機器人控制量、環境觀測信息來估算環境特征和機器人路徑并使其誤差達到最小，其數學描述如圖1所示[4]。

其中，xk代表k時刻表示機器人位姿的狀態向量；mi表示機器人觀測到的路標點，即環境特征；uk為機器人的控制向量；zk，*為機器人在k時刻觀測向量的集合，其中*指代路標點的序號，如在k-1時刻觀測向量為：zk-1，i，zk-1，i.采用集合的方式表述上述變量：

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

假設SLAM問題中的運動與觀測模型遵循馬爾可夫假設，即未來與過去數據相互獨立，所以可將其描述為關于路徑與環境地圖的估計，其概率模型為：P（xk，m1∶l｜z1∶k，u1∶k）.

2 SL A M問題的研究方法

早期的SLAM算法為基于擴展卡爾曼濾波的EKFSLAM算法。卡爾曼濾波器是遞推算法的一種，通過實時獲得的受噪聲污染的離散觀測數據，對系統狀態進行線性、無偏及最小誤差方差的最優估計[5]。

首先，通過運動方程及觀測方程對SLAM過程進行描述，有

其中，wk、vk，j為噪聲項，yj為路標點坐標值。假設整個SLAM系統為線性高斯系統，有：

其中x為包含機器人位姿信息及地圖信息的聯合狀態矢量，Ak為狀態轉移矩陣，Ck為系統觀測矩陣。根據高斯分布性質有：

此外，根據觀測方程有;

根據SLAM系統概率模型得出，k時刻狀態量的估計滿足高斯分布，設其后驗概率分布的均值與協方差分別為 ，則有：

由此可得：

設

則有：

同樣可得：

由此完成了k時刻狀態量的后驗概率分布估計，該過程被稱為“更新”。卡爾曼濾波器通過控制量“預測”再利用觀測值“更新”，其結構圖如圖2所示。

圖2 卡爾曼濾波算法

然而在實際應用中所面臨的大部分觀測方程與運動方程為非線性函數。為此，研究人員將卡爾曼濾波器進行推廣，得到適用于非線性系統的擴展卡爾曼濾波器，其基本思想在于將觀測方程運動方程在特定點附近進行泰勒展開，并只保留一階項，從而近似為線性系統，即

則有預測過程：

計算卡爾曼增益：

更新過程為：

在非線性優化方法中，模型將軌跡中離散的所有位姿設為變量，將運動方程與觀測方程作為變量間約束，構造代價函數：

其中，z指代觀測數據，在以視覺傳感器為數據來源的SLAM中指代像素坐標（us，vs），ξ即相機外參R、t對應的李代數形式，也即前面數學模型中的x，p為三維空間點坐標，即數學模型中的的路標m.

3 與機器學習相結合的SL A M研究

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

圖3 神經網絡復合卡爾曼濾波算法

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

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

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

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

[3]常 建.無所不能的SLAM無人車、無人機、AR都離不開它[J].計算機與網絡，2017，43（5）：18-19.

[4]趙一路.移動機器人SLAM問題研究[D].上海：復旦大學，2010.

[5]Charles K.Chui，Guanrong Chen.卡爾曼濾波及其實時應用[M].北京：電子工業出版社清華大學出版社，2013.

[6]王忠善.移動機器人同時定位與地圖創建的研究[D].哈爾濱：哈爾濱工業大學，2008.

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

[8]尹寶才，王文通，王立春.深度學習研究綜述[J].北京工業大學學報，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.