基于Fisher信息的室內主動SLAM

席永輝，胡士強

上海交通大學 航空航天學院，上海 200240

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

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

不同于上述傳統的視覺SLAM算法，越來越多的學者開始研究弱紋理環境下運動規劃和位姿估計聯合優化的主動SLAM算法[16-18]，但是，這類方法計算復雜度高或者需要事先知道環境中的特征點分布，對載體的機動性能也有一定的要求?？紤]到Fisher信息[19]可作為衡量參數估計性能的定量指標，被廣泛應用于特征點篩選[20]、最優化設計[21]等方面，本文針對弱紋理環境下的位姿估計問題提出了一種基于Fisher 信息的室內主動SLAM算法，對載體的運動軌跡和機動性能沒有過多限制，主要貢獻及創新性如下：

（1）提出了一種新的主動SLAM 框架，在ORBSLAM2 的基礎上增加了Fisher 信息場構建模塊與云臺控制模塊，算法能夠將前端的特征跟蹤狀態反饋到云臺相機控制中，在特征丟失時通過改變相機的觀測方向，使得整個系統能夠進行主動重定位，得到穩定且準確的結果。

（2）為了找到最優位姿估計對應的最優觀測方向，理論上分析了Fisher信息與參數估計性能之間的關系，并推導了位姿估計中Fisher信息矩陣的計算公式。

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

1 改進后系統框架

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

圖1 改進后系統框架Fig.1 Overall architecture of improved system

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

2 位姿估計

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

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

3 Fisher信息場構建

3.1 Fisher信息的概念

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

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

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

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

其中b為二值函數，當位姿為ξ的相機能觀測到特征點Pi時取1，否則取0。

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

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

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

3.3 Fisher信息場

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

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

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

圖2中五角星代表空間中的特征點，每個體素中箭頭的方向為該體素Fisher信息量最大的方向，把這樣一種表示方式稱為Fisher 信息場[25]，Fisher 信息場提供了每個體素的最優觀測方向，為弱紋理環境下定位失敗時相機的偏轉提供了基礎。

4 云臺控制

4.1 偏轉角度計算

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

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

4.2 控制規律設計

本文選擇經典的PID控制器控制云臺相機運動，調整相應的控制參數后，可使得云臺相機無超調地快速運動到期望位置，控制律如下：

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

表1 控制參數的取值Table 1 Value of control parameters

5 實驗分析

5.1 仿真模型與場景

Gazebo 作為一個功能強大的物理仿真平臺，具備高保真的三維物理引擎，廣泛應用于機器人、無人機等設備的開發，所以本文的所有實驗均是在Gazebo 平臺上實現的。

在仿真環境中，四旋翼無人機和云臺相機的物理模型參數如表2和表3所示。

表2 四旋翼無人機模型參數Table 2 Model parameters of quadrotor UAV

表3 云臺相機模型參數Table 3 Model parameters of tilt camera

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

圖3 仿真模型Fig.3 Simulation model

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

圖4 室內仿真場景Fig.4 Indoor simulation scene

5.2 仿真結果與分析

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

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

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

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

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

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

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

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

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

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

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

6 結束語

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