基于Fisher信息的室內(nèi)主動SLAM

席永輝，胡士強(qiáng)

上海交通大學(xué) 航空航天學(xué)院，上海 200240

同時定位與地圖創(chuàng)建（simultaneous localization and mapping，SLAM）是指機(jī)器人在未知的環(huán)境中，依靠自身攜帶的傳感器完成自主定位與周圍環(huán)境地圖的創(chuàng)建[1]。近年來，基于視覺傳感器的SLAM 在機(jī)器人和無人機(jī)的室內(nèi)定位中得到了廣泛的應(yīng)用[2-4]。第一個實時的單目視覺SLAM系統(tǒng)[5]是在2007年提出的，前端跟蹤稀疏的特征點，以擴(kuò)展卡爾曼濾波為后端，實時更新相機(jī)的狀態(tài)和路標(biāo)點的位置，但是前端特征點容易跟丟，應(yīng)用場景有限。同年，Klein 等人[6]提出了PTAM（parallel tracking and mapping），是第一個使用非線性優(yōu)化的后端，并且實現(xiàn)了跟蹤與地圖創(chuàng)建的并行化，后續(xù)許多優(yōu)秀的開源方案如Fast-SLAM[7]、ORB-SLAM2[8]都是在此基礎(chǔ)上完成。然而，這些算法過度依賴環(huán)境的紋理信息，當(dāng)遇到弱紋理的環(huán)境，會找不到足夠多的匹配特征點，從而導(dǎo)致算法的精度降低甚至定位失敗[9]。

針對弱紋理環(huán)境下特征缺失帶來的問題，國內(nèi)外的學(xué)者也做了大量的研究。李海豐等人[10]通過提取環(huán)境中的點、線、面特征，彌補(bǔ)環(huán)境中點特征不足帶來的影響，但是匹配算法復(fù)雜，只適用于具有結(jié)構(gòu)化的人造環(huán)境；Engel等人[11-12]利用直接法，直接對像素進(jìn)行操作，不依賴紋理信息，但是算法容易受到光照變化的影響，相機(jī)不能運動過快，魯棒性較弱。除了上述基于圖像層面的研究方案，還有不少學(xué)者從系統(tǒng)結(jié)構(gòu)的層面出發(fā)，引入慣性傳感器去彌補(bǔ)視覺特征的缺失[13-14]，或者設(shè)計冗余結(jié)構(gòu)，采用多位姿測量系統(tǒng)進(jìn)行位姿估計[15]，但是前者在視覺特征長期缺失的情況下會產(chǎn)生較大的誤差漂移，后者傳感器的標(biāo)定較難，系統(tǒng)結(jié)構(gòu)龐大，不易于移植和維護(hù)。

不同于上述傳統(tǒng)的視覺SLAM算法，越來越多的學(xué)者開始研究弱紋理環(huán)境下運動規(guī)劃和位姿估計聯(lián)合優(yōu)化的主動SLAM算法[16-18]，但是，這類方法計算復(fù)雜度高或者需要事先知道環(huán)境中的特征點分布，對載體的機(jī)動性能也有一定的要求。考慮到Fisher信息[19]可作為衡量參數(shù)估計性能的定量指標(biāo)，被廣泛應(yīng)用于特征點篩選[20]、最優(yōu)化設(shè)計[21]等方面，本文針對弱紋理環(huán)境下的位姿估計問題提出了一種基于Fisher 信息的室內(nèi)主動SLAM算法，對載體的運動軌跡和機(jī)動性能沒有過多限制，主要貢獻(xiàn)及創(chuàng)新性如下：

（1）提出了一種新的主動SLAM 框架，在ORBSLAM2 的基礎(chǔ)上增加了Fisher 信息場構(gòu)建模塊與云臺控制模塊，算法能夠?qū)⑶岸说奶卣鞲櫊顟B(tài)反饋到云臺相機(jī)控制中，在特征丟失時通過改變相機(jī)的觀測方向，使得整個系統(tǒng)能夠進(jìn)行主動重定位，得到穩(wěn)定且準(zhǔn)確的結(jié)果。

（2）為了找到最優(yōu)位姿估計對應(yīng)的最優(yōu)觀測方向，理論上分析了Fisher信息與參數(shù)估計性能之間的關(guān)系，并推導(dǎo)了位姿估計中Fisher信息矩陣的計算公式。

（3）在Gazebo仿真平臺中搭建了室內(nèi)環(huán)境，將所提算法運用到四旋翼無人機(jī)的室內(nèi)位姿估計中，驗證了算法的有效性。

1 改進(jìn)后系統(tǒng)框架

以四旋翼無人機(jī)為載體，改進(jìn)后的系統(tǒng)框架大致可分為位姿估計、Fisher 信息場構(gòu)建和云臺控制這三個部分，具體框圖如圖1所示。

圖1 改進(jìn)后系統(tǒng)框架Fig.1 Overall architecture of improved system

位姿估計部分通過跟蹤地圖中的ORB[22]特征點，實現(xiàn)對相機(jī)位姿的實時估計；Fisher 信息場構(gòu)建部分根據(jù)特征點的空間位置分布實時更新每個體素的Fisher信息并計算出最優(yōu)的觀測方向；云臺控制部分在相機(jī)所獲得的圖像遇到弱紋理區(qū)域而跟蹤失敗時可以通過改變相機(jī)的觀測方向來進(jìn)行主動的重定位，增強(qiáng)了系統(tǒng)的魯棒性。相機(jī)與無人機(jī)的相對位姿可以用一個三維的歐式變換SE（3）來表示，該變換隨著云臺的轉(zhuǎn)動而改變，但是能直接獲得，由位姿估計模塊輸出的相機(jī)位姿和相機(jī)相對于無人機(jī)的位姿可以得到無人機(jī)的六自由度位姿。每個部分的具體內(nèi)容將在后續(xù)小節(jié)中詳細(xì)介紹。

2 位姿估計

本文以O(shè)RB-SLAM2開源方案作為位姿估計模塊，ORB-SLAM2是現(xiàn)代SLAM系統(tǒng)中較為完善易用的系統(tǒng)之一，許多研究工作都以其為標(biāo)準(zhǔn)，或者在它的基礎(chǔ)上繼續(xù)開發(fā)。ORB-SLAM2 支持單目相機(jī)、雙目相機(jī)、RGB-D 相機(jī)3 種傳感器形式，由于RGB-D 相機(jī)可以直接獲取場景的深度，初始化簡單，特征點的空間位置容易獲得，所以本文采用RGB-D 深度相機(jī)作為視覺位姿估計的傳感器。ORB-SLAM2 分為3 個并行的線程：跟蹤線程，局部優(yōu)化線程以及閉環(huán)檢測線程。其中，跟蹤線程將空間中的特征點和當(dāng)前幀中的特征點進(jìn)行匹配并投影到當(dāng)前幀中，通過最小化重投影誤差來實現(xiàn)相機(jī)位姿的極大似然估計：

ui表示觀測的數(shù)據(jù)，即特征點在圖像中的坐標(biāo)，τ為相機(jī)坐標(biāo)系（原點為相機(jī)的光心，x軸與y軸與圖像的X、Y軸平行，z軸為相機(jī)光軸）到圖像坐標(biāo)系的投影函數(shù)，Pi表示特征點在世界坐標(biāo)系（把最初的相機(jī)坐標(biāo)系固定為世界坐標(biāo)系）下的坐標(biāo)，exp(ξ^)為李代數(shù)的指數(shù)映射[9]。為了減小累計誤差，引入了局部優(yōu)化和閉環(huán)檢測線程，具體實現(xiàn)過程可以參考文獻(xiàn)[8]，這里就不再贅述。

3 Fisher信息場構(gòu)建

3.1 Fisher信息的概念

Fisher信息是用來度量含參隨機(jī)變量所攜帶參數(shù)信息多寡的量。假設(shè)估計的參數(shù)為θ，觀測數(shù)據(jù)x的分布依賴于參數(shù)θ，用P(x|θ)表示，參數(shù)θ的無偏估計為θ′，由無偏估計的定義可得：

由式（13）可知，從估計誤差的角度來看，F(xiàn)isher信息矩陣作為衡量參數(shù)估計性能的定量指標(biāo)，給出了參數(shù)估計的精度上限，通過最大化Fisher信息矩陣的某一度量函數(shù)（常用矩陣的跡或者行列式）可以實現(xiàn)參數(shù)估計的協(xié)方差最小化[23]。

3.2 位姿估計中Fisher信息的計算

相機(jī)的位姿估計問題本質(zhì)上也是一個參數(shù)估計問題，可以從以下3 個方面評價某個參數(shù)估計方法的性能：無偏性，指估計量的數(shù)學(xué)期望等于被估計參數(shù)的真實值；有效性，指無偏估計量的方差，方差越小的估計越有效；相合性，指當(dāng)觀測數(shù)據(jù)足夠多時，估計值會逐漸收斂于真值。對于形如式（1）的極大似然估計來說，由極大似然估計的漸近正態(tài)分布性質(zhì)可知其滿足相合性，但是在有限樣本的情況下，因為非線性誤差的存在達(dá)不到無偏性，不過偏置量會隨著Fisher信息的增大而減小[24]，另一方面，位姿估計的方差難以直接獲得，但是由3.1節(jié)中Fisher 信息的性質(zhì)可知，估計方差也會隨著Fisher 信息的增大而減小，所以某個位置Fisher信息量最大的方向可作為相機(jī)的最優(yōu)觀測方向，以該方向進(jìn)行觀測，可以達(dá)到位姿估計無偏性與有效性的最大化。以下是位姿估計中Fisher信息的具體計算過程：

其中b為二值函數(shù)，當(dāng)位姿為ξ的相機(jī)能觀測到特征點Pi時取1，否則取0。

由于受到相機(jī)視場角的影響，即使在同一位置，不同觀測方向的Fisher信息量也不一樣，根據(jù)公式（24）可知，要計算某個位置Fisher 信息量最大的方向，需要在每個方向?qū)λ刑卣鼽c進(jìn)行迭代，然而，實際過程中特征點數(shù)目往往是巨大的，會帶來較大的計算量，無法滿足實時性的要求，參考文獻(xiàn)[25]的思想，用旋轉(zhuǎn)矩陣Rwc和平移向量twc表示相機(jī)位姿，首先對相機(jī)的可觀測模型進(jìn)行如下近似：

其中a0、a1、a2是對相機(jī)可觀測模型進(jìn)行近似后的參數(shù)，和相機(jī)的視場角有關(guān)，θ?,i為相機(jī)光心到特征點Pi的射線與相機(jī)光軸正方向之間的夾角，它的余弦值為：

上式是一個帶約束的非線性優(yōu)化問題，可以使用序列二次規(guī)劃算法（SQP）對其求解，將原問題轉(zhuǎn)化為一系列二次規(guī)劃子問題，經(jīng)過數(shù)次迭代之后最終收斂到原問題的最優(yōu)解[26]。

3.3 Fisher信息場

通過上述分析可知，F(xiàn)isher 信息表征了觀測量與被估計參數(shù)之間的內(nèi)在聯(lián)系，而對于特征點法的SLAM算法來說，相機(jī)在某個位姿的觀測數(shù)據(jù)只和特征點的空間位置分布有關(guān)，故只需利用特征點的世界坐標(biāo)更新該位姿的Fisher信息量。但是由于空間位置的連續(xù)性，不可能對每個位置都進(jìn)行Fisher信息的計算，這時候需要把連續(xù)的空間離散化成若干個體素（即空間中的小立方體）。具體步驟如下：首先確定離散化后的體素數(shù)量，然后根據(jù)室內(nèi)空間的大小和體素數(shù)量計算每個體素的邊長，按照該邊長把三維空間均勻分割，得到一系列的體素，最后用體素中心點處的Fisher信息代替整個體素內(nèi)所有點的Fisher信息。在設(shè)置體素數(shù)量的時候，為了最大化空間降采樣后的精度，希望在保證與原有算法具有相同運算效率的同時讓體素的數(shù)量盡可能的多。假設(shè)體素的數(shù)量為Nv，計算一個特征點給一個體素帶來的Fisher 信息需要的時間為t，特征點隨著關(guān)鍵幀的插入而增加，關(guān)鍵幀插入的平均時間間隔為T，每次平均增加的特征點個數(shù)為Nf，那么在關(guān)鍵幀插入的時間間隔中，要完成每個體素Fisher 信息的更新，體素個數(shù)需要滿足：

在本節(jié)中，暫不考慮實際的物理尺寸與運算效率，給出一個離線仿真的結(jié)果，把10×10×10 的三維空間等分為1 000 個體素（每個體素的大小為1×1×1），為了更加直觀地顯示效果，把200個特征點都隨機(jī)分布在x=0的平面上，特征點的空間坐標(biāo)已知，在Matlab 中進(jìn)行仿真，得到的結(jié)果如圖2所示。

圖2 Fisher信息場示意圖Fig.2 Diagram of Fisher information field

圖2中五角星代表空間中的特征點，每個體素中箭頭的方向為該體素Fisher信息量最大的方向，把這樣一種表示方式稱為Fisher 信息場[25]，F(xiàn)isher 信息場提供了每個體素的最優(yōu)觀測方向，為弱紋理環(huán)境下定位失敗時相機(jī)的偏轉(zhuǎn)提供了基礎(chǔ)。

4 云臺控制

4.1 偏轉(zhuǎn)角度計算

為了簡化實驗，假設(shè)相機(jī)只能繞著y軸（垂直于地面的軸）轉(zhuǎn)動。將三維空間分為M個體素，每個體素中心點在世界坐標(biāo)系下的坐標(biāo)為ti，垂直相機(jī)y軸平面上的Fisher 信息量最大的方向為R_best(ti)，i=1,2,…,M。當(dāng)相機(jī)在空間中某一位姿（具有旋轉(zhuǎn)量Rwc和平移量twc=[x,y,z]T）遇到弱紋理區(qū)域而定位失敗，先找到距離當(dāng)前位置歐式距離最近的體素t*，然后再以該體素Fisher信息量最大的方向作為此刻相機(jī)的最優(yōu)觀測方向

通過上式解出旋轉(zhuǎn)矩陣R(θ)后，再轉(zhuǎn)化為歐拉角θ，即待求的偏轉(zhuǎn)角度。

4.2 控制規(guī)律設(shè)計

本文選擇經(jīng)典的PID控制器控制云臺相機(jī)運動，調(diào)整相應(yīng)的控制參數(shù)后，可使得云臺相機(jī)無超調(diào)地快速運動到期望位置，控制律如下：

其中f為作用于云臺的控制力矩，θc為云臺的期望角度，θ為云臺的實際角度，Kp，Kd，Ki為控制參數(shù)，具體選取的數(shù)值如表1所示。

表1 控制參數(shù)的取值Table 1 Value of control parameters

5 實驗分析

5.1 仿真模型與場景

Gazebo 作為一個功能強(qiáng)大的物理仿真平臺，具備高保真的三維物理引擎，廣泛應(yīng)用于機(jī)器人、無人機(jī)等設(shè)備的開發(fā)，所以本文的所有實驗均是在Gazebo 平臺上實現(xiàn)的。

在仿真環(huán)境中，四旋翼無人機(jī)和云臺相機(jī)的物理模型參數(shù)如表2和表3所示。

表2 四旋翼無人機(jī)模型參數(shù)Table 2 Model parameters of quadrotor UAV

表3 云臺相機(jī)模型參數(shù)Table 3 Model parameters of tilt camera

云臺相機(jī)模型通過一個轉(zhuǎn)動關(guān)節(jié)和四旋翼無人機(jī)模型相連，可繞著垂直地面的軸轉(zhuǎn)動，如圖3所示，利用4.2節(jié)設(shè)置的PID控制器可以操控相機(jī)轉(zhuǎn)動到期望角度。

圖3 仿真模型Fig.3 Simulation model

仿真場景是一個12 m×8 m×3 m 的室內(nèi)環(huán)境，其中有一面無紋理的墻面，如圖4所示。

圖4 室內(nèi)仿真場景Fig.4 Indoor simulation scene

5.2 仿真結(jié)果與分析

無人機(jī)按照事先設(shè)定好的軌跡，先垂直飛到距離地面2 m的空中，然后沿著一個8 m×2 m的矩形繞室內(nèi)飛行一周，首先用傳統(tǒng)的ORB-SLAM2算法對無人機(jī)的位姿進(jìn)行估計，得到的實驗結(jié)果如圖5所示。

圖5（a）截取了算法前端跟蹤特征點的圖像，綠色的點代表匹配的特征點。從圖中可以看出，當(dāng)相機(jī)運動到弱紋理的墻面時，算法由于缺少特征而跟蹤失敗，對應(yīng)的定位效果如圖5（b）所示，該圖表示位姿估計過程中的地圖構(gòu)建，包括特征點地圖和關(guān)鍵幀地圖，關(guān)鍵幀之間的連線表示它們的共視關(guān)系，由圖可知，算法在弱紋理區(qū)域無法定位。為解決此問題，需要將空間離散化，經(jīng)過測試，本文實驗中的T大概為610 ms，Nf大概為30 個，t為0.007 5 ms，利用公式（31）可得體素個數(shù)Nv≤2 711，若選擇2 711個體素，根據(jù)室內(nèi)空間的大小，可得到體素的邊長為0.474 m，為了簡便運算，設(shè)置體素的邊長為0.5 m，一共得到2 304 個體素，用本文所提出的主動SLAM算法，在相同環(huán)境下的運行效果如圖6所示。

圖5 ORB-SLAM2算法實驗效果Fig.5 Experimental results of ORB-SLAM2

圖6 主動SLAM算法實驗效果Fig.6 Experimental results of active SLAM

分析圖6可知，本文所提的算法在特征丟失的情況下可以完成自主重定位，進(jìn)一步比較兩種算法在空間中3個方向上的位置與航向角的誤差（無人機(jī)在飛行的過程中俯仰和滾轉(zhuǎn)角始終保持為零，不做控制），在飛行的過程中均勻地采樣200 個數(shù)據(jù)，真值由仿真環(huán)境提供，結(jié)果如圖7～10所示。

圖7 ORB-SLAM2算法位置估計結(jié)果Fig.7 Results of location estimation with ORB-SLAM2

分析圖7 和圖9 可知，ORB-SLAM2 算法在弱紋理區(qū)域定位失?。ū憩F(xiàn)為圖中虛線框部分，跟蹤失敗后的位姿一直是失敗前一時刻的位姿）；由圖8和圖10可知，所提算法在弱紋理區(qū)域可以通過相機(jī)的偏轉(zhuǎn)進(jìn)行特征捕捉，完成整個定位過程，注意到相機(jī)在偏轉(zhuǎn)之后，無人機(jī)與相機(jī)的相對位移仍然保持不變，可用相機(jī)的空間位置代替無人機(jī)的空間位置，但二者之間的相對角度發(fā)生了變化，需要在航向上做一個坐標(biāo)變換；如圖10 所示，在第137個采樣點的時候，相機(jī)轉(zhuǎn)動了115.2°，因此需要在相機(jī)角度的基礎(chǔ)上減去一個115.2°的補(bǔ)償作為無人機(jī)的偏航（云臺的響應(yīng)時間可忽略）。表4 統(tǒng)計了改進(jìn)前與改進(jìn)后的算法在3 個方向上的位置和航向角的RMSE（均方根誤差）。實驗結(jié)果表明，在紋理豐富的區(qū)域，兩個算法的定位效果差異不大，然而在傳統(tǒng)ORBSLAM2 算法遇到弱紋理區(qū)域而失效的情況下，改進(jìn)后的算法仍能夠有效地完成位姿估計，且3個方向的位置誤差都在10 cm 以內(nèi)，航向定位精度在4°以內(nèi)，證明了所提算法的有效性。

圖8 主動SLAM算法位置估計結(jié)果Fig.8 Results of location estimation with active SLAM

圖9 ORB-SLAM2算法角度估計結(jié)果Fig.9 Results of angle estimation with ORB-SLAM2

圖10 主動SLAM算法角度估計結(jié)果Fig.10 Results of angle estimation with active SLAM

表4 均方根誤差分析Table 4 Analysis of RMSE

6 結(jié)束語

本文研究了視覺SLAM 在弱紋理區(qū)域定位失敗的問題，以O(shè)RB-SLAM2 框架為基礎(chǔ)，增加了Fisher 信息場構(gòu)建模塊和云臺控制模塊，將室內(nèi)的三維空間劃分成若干個大小相同的體素，在定位的同時構(gòu)建Fisher信息場，在特征缺失之后可以根據(jù)當(dāng)前體素Fisher信息量最大的方向，計算出相機(jī)的偏轉(zhuǎn)角度，然后控制相機(jī)運動到最優(yōu)的觀測方向進(jìn)行主動重定位，提高了系統(tǒng)的魯棒性，并在仿真中驗證了算法的有效性。由于本文在構(gòu)建Fisher信息場時并未考慮到障礙物的遮擋對特征點可觀測性的影響，所以下一步將考慮用深度相機(jī)對遮擋物進(jìn)行建模，在計算空間中每個體素的Fisher信息時排除某些可能被遮擋的特征點，以達(dá)到更加精準(zhǔn)的定位效果。