視覺輔助下的室內(nèi)慣導(dǎo)位姿修正

鄒漢達(dá)，袁 洪

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

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

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

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

1 基于合作目標(biāo)的單目視覺定位

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

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

(1) 確定平均攝影距離(大概航高)、內(nèi)方位元素、編碼標(biāo)志的空間坐標(biāo)及對應(yīng)的像點坐標(biāo)。

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

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

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

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

本文的慣導(dǎo)+視覺組合算法，即利用上述介紹的視覺定位方法來獲取視覺定位結(jié)果，以對慣導(dǎo)進(jìn)行修正。

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

2 慣導(dǎo)+視覺組合算法

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

為了描述慣導(dǎo)系統(tǒng)的運動參數(shù)，本文主要用到兩個參考坐標(biāo)系，分別為載體坐標(biāo)系和導(dǎo)航坐標(biāo)系。其中，載體坐標(biāo)系采用右前上坐標(biāo)系，導(dǎo)航坐標(biāo)系采用東北天地理坐標(biāo)系。

2.1 導(dǎo)航狀態(tài)變量

算法選取18維狀態(tài)變量為

(1)

2.2 狀態(tài)方程

2.2.1 姿態(tài)誤差方程

姿態(tài)的誤差方程為

(2)

2.2.2 速度誤差方程

速度的誤差方程為

(3)

2.2.3 位置誤差方程

位置的誤差方程為

(4)

(5)

(6)

2.2.4 狀態(tài)方程

由以上誤差方程可得，整個系統(tǒng)的狀態(tài)方程為

(7)

式中，F(xiàn)SINS/VNS為系統(tǒng)狀態(tài)矩陣；w為預(yù)測噪聲向量(均值為零，方差為Q的白噪聲)。相應(yīng)的預(yù)測噪聲協(xié)方差矩陣Q為

(8)

系統(tǒng)狀態(tài)矩陣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 量測方程

算法選取慣導(dǎo)系統(tǒng)與視覺系統(tǒng)的姿態(tài)、位置之差作為觀測量z，即

(21)

系統(tǒng)量測方程為

z=HSINS/VNSx+v

(22)

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

(23)

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

(24)

3 慣導(dǎo)位姿誤差修正方法

每次觀測量的值更新后，得到當(dāng)前誤差估計的最優(yōu)值，利用該值對慣導(dǎo)的當(dāng)前位姿信息進(jìn)行反饋修正。

3.1 姿態(tài)的修正

采用的修正方程為

(25)

3.2 速度、位置的修正

采用的修正方程為

vn=vn--δvn-

(26)

pn=pn--δpn-

(27)

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

4 試驗驗證

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

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

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

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

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

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

表1 不同觸發(fā)頻率的導(dǎo)航結(jié)果

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

5 結(jié) 論

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