視覺輔助下的室內慣導位姿修正

鄒漢達，袁 洪

(1. 中國科學院大學，北京 100094; 2. 中國科學院空天信息研究院，北京 100094)

慣性導航系統是一種自主式的導航系統。其優點是可提供連續的導航信息，短期的定位精度和穩定性好，但是由于導航信息是由積分產生，定位誤差隨時間而增大，不能長期獨立工作，而多源融合導航方法則可以有效彌補慣性導航系統的缺點。傳統的方法是采用慣導+衛星組合導航方法[1-3]來修正慣導累積誤差，但是在大型倉庫、地下停車場、城市地下交通路網等室內、地下等衛星信號薄弱的場景中該方法就不再適用。而視覺導航方法在光源充足、紋理特征點豐富的條件下具有精度高、穩定性好、可靠性高等優點，并且在室內場景中效果同樣很好[4]。但是視覺導航方法計算復雜度較高，有時不能保持連續性。利用慣導的短時連續性可以提高視覺導航方法中的匹配搜索效率，因此視覺與慣導組合定位[5]具有廣闊的發展前景。

本文研究的INS+視覺組合的定位方法中，采用基于合作目標[6]的單目視覺定位方法，主要作用是修正慣性導航系統的姿態、位置積累誤差，從而提高整個系統的導航精度。視覺定位系統首先獲取含有編碼標志的圖像，提取其中的特征信息，由對應編碼獲得其在參考坐標系中的坐標，繼而得到自身的位姿信息，最后采用基于卡爾曼濾波[7]的方法將慣導的導航信息與視覺的位姿信息進行融合處理，實現對慣導誤差的修正。

對于慣導誤差的修正方法，傳統方法是采用直接位置校正方法[8-15]，即檢測到慣導系統的誤差后，直接更改導航解算過程中的位姿數據，這種方法不能有效抑制位置誤差曲線的增長。為了解決這一問題，本文利用視覺定位的位姿信息建立觀測方程，進行卡爾曼濾波，并通過分析算法實際應用場景，選用合適的試驗設備，進行實際試驗對比驗證了該算法對慣導系統誤差的修正具有良好的效果，同時也具有一定的工程參考價值。

1 基于合作目標的單目視覺定位

本文采用的視覺定位方法是基于合作目標的單目視覺定位方法，主要原理為：采用附有編碼標志圖片的人工標志物作為定位的合作目標。首先在室內場景(如走廊、地下車庫)的墻頂、側墻面等位置每隔一定距離布置并固定人工標志物，該標志物為人工編碼且具有唯一編碼的標志卡片，用單目相機對標志物進行連續拍照后完成該場景中人工標志物的建模，即獲得每個人工標志物在該相對參考系中的坐標。之后在融合定位的過程中，每當單目相機收到信號進行拍照獲得含有編碼標志的圖像，就提取其中的特征點信息，由對應編碼獲得其在參考坐標系中的坐標，繼而計算得到相機自身的位姿信息。

這種基于合作目標的單目視覺定位方法輸出的具體參數有相機自身的姿態角(橫滾角、俯仰角、偏航角)以及三軸坐標位置(x,y,z)。視覺定位求解相機自身位姿信息的基本原理為攝影測量中的空間后方交會求解外方位元素，其基本解算步驟為：

(1) 確定平均攝影距離(大概航高)、內方位元素、編碼標志的空間坐標及對應的像點坐標。

(2) 確定相機位置的初值及相機姿態的初值。

(3) 對每個編碼標志點，依次計算出像點坐標的近似值。

(4) 求解相機姿態與位置未知量的誤差，與對應的近似值相加，可得未知量的近似值。

(5) 當所求誤差小于規定閾值時，停止迭代。

本文的慣導+視覺組合算法，即利用上述介紹的視覺定位方法來獲取視覺定位結果，以對慣導進行修正。

另外，通過利用慣導的短時連續定位結果，可以優化視覺定位中搜索匹配算法的計算復雜度。具體來說，由前一時刻慣導定位的結果推算出當前時刻慣導的相對位置，進一步推導出編碼標志在相片上像點坐標的大概位置，以這個位置為中心進行一定范圍的窗口搜索，即可提高編碼標志識別的搜索效率。

2 慣導+視覺組合算法

本文的慣導+視覺組合算法采用卡爾曼濾波算法，利用視覺定位獲得的姿態和位置信息對整個系統的誤差進行估計，并修正慣導系統的位姿誤差，降低姿態和位置誤差的變化率，從而提高慣導定位精度。

為了描述慣導系統的運動參數，本文主要用到兩個參考坐標系，分別為載體坐標系和導航坐標系。其中，載體坐標系采用右前上坐標系，導航坐標系采用東北天地理坐標系。

2.1 導航狀態變量

算法選取18維狀態變量為

(1)

2.2 狀態方程

2.2.1 姿態誤差方程

姿態的誤差方程為

(2)

2.2.2 速度誤差方程

速度的誤差方程為

(3)

2.2.3 位置誤差方程

位置的誤差方程為

(4)

(5)

(6)

2.2.4 狀態方程

由以上誤差方程可得，整個系統的狀態方程為

(7)

式中，FSINS/VNS為系統狀態矩陣；w為預測噪聲向量(均值為零，方差為Q的白噪聲)。相應的預測噪聲協方差矩陣Q為

(8)

系統狀態矩陣FSINS/VNS按下式計算[16]

(9)

其中，每個子塊按如下方式計算

(10)

(11)

Map=M1+M2

(12)

(13)

(14)

Mvp=(vn×)(2M1+M2)+M3

(15)

(16)

(17)

(18)

(19)

(20)

2.3 量測方程

算法選取慣導系統與視覺系統的姿態、位置之差作為觀測量z，即

(21)

系統量測方程為

z=HSINS/VNSx+v

(22)

式中，HSINS/VNS為系統量測矩陣；v為觀測噪聲向量(均值為零，方差為R的白噪聲)，相應的觀測噪聲協方差矩陣為

(23)

式中，φ0、r0為提前確定的姿態、位置觀測噪聲。系統量測矩陣HSINS/VNS按下式計算

(24)

3 慣導位姿誤差修正方法

每次觀測量的值更新后，得到當前誤差估計的最優值，利用該值對慣導的當前位姿信息進行反饋修正。

3.1 姿態的修正

采用的修正方程為

(25)

3.2 速度、位置的修正

采用的修正方程為

vn=vn--δvn-

(26)

pn=pn--δpn-

(27)

式中，vn-、pn-分別為慣導系統解算出的速度、位置向量；δvn-、δpn-分別為濾波器估計的速度、位置誤差向量；vn、pn分別為修正后的更精確的速度、位置向量。

4 試驗驗證

為了驗證本文所提出的慣導位姿修正方法，在地下車庫進行了試驗驗證。地下車庫已經事先布置好編碼標志并完成建模，對應地下車庫中的一塊方形區域，南北方向的兩條通道布滿了編碼標志，而東西方向的兩條通道則沒有布置編碼標志。每個編碼標志對應唯一的編碼序列，并且確定了每個編碼標志在同一個參考坐標系下的三維坐標，可以直接進行基于合作目標的單目視覺定位。

該試驗主要使用一臺手推車作為載體放置試驗設備，試驗設備包括一臺筆記本電腦，安裝有時間同步軟件、慣導記錄軟件、視覺定位軟件及組合定位算法Matlab程序，一臺MTI慣導和一臺單目視覺相機分別與電腦連接以采集原始數據， 100C與接收機連接作為軌跡基準。試驗開始時，首先對慣導系統和相機進行對準，然后設定相機觸發頻率，在0.2 Hz的條件下繞地下車庫兩周進行定位試驗以檢驗慣導精度。

在地下車庫中整個系統的運動軌跡如圖1所示，慣導+視覺組合算法的誤差曲線如圖2所示。

由系統的誤差曲線圖2可知，經過慣導+視覺組合算法對慣導系統的位姿修正后，整個系統的姿態、速度、位置誤差都較小；橫滾角、俯仰角誤差最后穩定在2角分以內，偏航角誤差最后穩定在10角分以內；東向、北向、天向速度最后都穩定在0.1 m/s以內；緯度、經度誤差最后穩定在2 m以內，高度誤差最后穩定在5 m以內。同時，各誤差曲線均逐漸呈收斂趨勢，由于算法采用閉環反饋修正的形式，因此，誤差曲線在一定的誤差范圍內呈現出振蕩趨勢，這也說明了該算法的優越性。

由圖1可知，純慣導定位軌跡的累計誤差會隨時間而變得很大，純視覺定位結果雖然精度較高，但卻不能保持連續性，而相較之下慣導+視覺組合定位軌跡與真實軌跡呈逐漸貼合趨勢，說明該導航結果能滿足一般需求。

改變相機觸發頻率，分別在0.2 Hz、0.1 Hz、不觸發的條件下進行試驗，其他條件不變，仍然繞地下車庫一周進行定位試驗，在每種頻率各進行3次試驗，對比幾次試驗的結果見表1。

表1 不同觸發頻率的導航結果

由表1可知，對于相同的一次試驗，相機不同的觸發頻率會導致導航結果的誤差不同，并且觸發頻率越高，導航結果的誤差越小，因此在考慮計算成本與相機硬件限制的前提下，采用0.2 Hz的觸發頻率效果最好；同時，有觸發相機進行修正時的導航結果誤差明顯小于不觸發相機進行修正時的誤差，說明本算法對慣導誤差的修正具有良好的效果。

5 結 論

本文針對在室內場景中衛星信號薄弱導致慣導+衛星組合導航方法不再適用的問題，提出了一種新的基于慣導+視覺組合的室內慣導位姿修正方法，主要工作可以總結為：①分析基于合作目標的單目視覺定位原理，并利用其定位結果建立慣導系統姿態誤差、位置誤差的量測方程對慣導系統的位姿進行修正；②在特定室內場景中選取符合相應應用場景的試驗設備多次進行試驗驗證算法的有效性與優越性。試驗結果表明，本文提出的修正算法能夠有效解決室內場景中慣導位姿誤差的修正問題，具有良好的效果且具有一定的工程參考價值。