一種GNSS/視覺觀測緊組合導(dǎo)航定位算法研究

馮祎，涂銳，韓軍強(qiáng)，張垠，李芳馨，洪菊，張世旋

（ 1. 中國科學(xué)院國家授時中心, 西安 710600；2. 中國科學(xué)院大學(xué), 北京 100049 ）

0 引 言

定位、導(dǎo)航和授時(PNT)服務(wù)的需求已經(jīng)滲透到國民經(jīng)濟(jì)和軍事服務(wù)等各個領(lǐng)域. 全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)作為重要的基礎(chǔ)設(shè)施，在開闊的環(huán)境下能夠提供全天候、全球性和持續(xù)的PNT服務(wù). 然而GNSS信號易受到遮擋或者電磁干擾，到達(dá)地面的信號非常微弱，嚴(yán)重影響導(dǎo)航定位的可用性、可靠性、連續(xù)性和精度. 為保障持續(xù)且可靠的PNT服務(wù)，我國開始推進(jìn)以北斗衛(wèi)星導(dǎo)航系統(tǒng)(BDS)為核心的國家綜合PNT體系的建設(shè). 基于多種傳感器并融合多源數(shù)據(jù)，旨在獲得具有抗干擾、防欺騙、連續(xù)且可靠的PNT服務(wù)信息[1].

基于圖像的視覺同步定位與建圖(V-SLAM)技術(shù)已經(jīng)廣泛地應(yīng)用到室內(nèi)外環(huán)境的導(dǎo)航定位中，如機(jī)器人、無人機(jī)、智能駕駛車輛等的導(dǎo)航定位[2-3]. V-SLAM技術(shù)雖然能夠在特征豐富的復(fù)雜環(huán)境中實現(xiàn)持續(xù)的導(dǎo)航定位，但會產(chǎn)生誤差. 具有隨著載體的運(yùn)動距離快速積累的特性，在室外環(huán)境下不直接使用V-SLAM進(jìn)行導(dǎo)航定位. 由于位于GNSS弱信號環(huán)境中的接收機(jī)周圍環(huán)境通常較復(fù)雜且特征豐富，并且GNSS定位誤差不會逐漸積累，可知GNSS和V-SLAM技術(shù)具有優(yōu)劣互補(bǔ)的特性，可將其進(jìn)行組合提升復(fù)雜環(huán)境下導(dǎo)航定位能力.

GNSS與V-SLAM組合方法主要分為松組合與緊組合[4-5]，松組合中GNSS和V-SLAM系統(tǒng)分別單獨計算出位置信息后再進(jìn)行組合，其限制條件是每個系統(tǒng)要有足夠的觀測量并能進(jìn)行單系統(tǒng)的定位. 緊組合中GNSS采用偽距或載波相位原始觀測值，在觀測值域與V-SLAM結(jié)果進(jìn)行組合，相比松組合更具有優(yōu)勢[6]. 基于此，本文研究了一種觀測值域的緊組合方法，即先基于圖像數(shù)據(jù)利用ORB-SLAM2開源平臺求解得到視覺位置結(jié)果增量，再聯(lián)合GNSS偽距觀測數(shù)據(jù)采用卡爾曼濾波(KF)進(jìn)行組合定位解算，實現(xiàn)持續(xù)可靠的導(dǎo)航定位.

1 GNSS/視覺觀測緊組合定位方法

GNSS在開闊的環(huán)境下能夠?qū)崿F(xiàn)連續(xù)的高精度導(dǎo)航定位并且誤差不會逐漸積累，但是在GNSS弱信號或者遮擋環(huán)境下，存在定位精度嚴(yán)重下降甚至定位中斷的問題. 相反V-SLAM技術(shù)在特征豐富的復(fù)雜環(huán)境下能夠?qū)崿F(xiàn)連續(xù)導(dǎo)航定位，但隨著運(yùn)動距離的增加定位誤差積累迅速[7]，因此將GNSS和VSLAM技術(shù)聯(lián)合導(dǎo)航定位，不僅有望提升單個導(dǎo)航系統(tǒng)的可靠性與精度，還能夠在GNSS弱信號環(huán)境下實現(xiàn)持續(xù)的導(dǎo)航定位.

V-SLAM技術(shù)是一種相對定位方法，基本原理為：圖像經(jīng)過校正后，提取圖像中具有代表性的特征點，跟蹤這些特征點從而估計相機(jī)的位姿，在這個過程中還同時進(jìn)行位姿優(yōu)化與閉環(huán)檢測[8]. ORBSLAM2系統(tǒng)是一種基于ORB(Oriented FAST and Rotated BRIEF)特征的同步三維定位與地圖構(gòu)建算法[9]，可使用開源平臺的雙目模式解算得到相機(jī)的位姿變化. 本文將ORB-SLAM2系統(tǒng)得到的相機(jī)位姿變化結(jié)果與GNSS偽距觀測數(shù)據(jù)聯(lián)合來進(jìn)行緊組合導(dǎo)航定位，緊組合定位方法主要分為兩個步驟，首先從視覺圖像中獲取相機(jī)的前后幀位姿變化結(jié)果，再與GNSS偽距觀測數(shù)據(jù)進(jìn)行緊組合導(dǎo)航定位解算.

1.1 相機(jī)位姿估計原理

圖1為空間中點P的針孔成像幾何模型圖，其中O-u,v為圖像坐標(biāo)系，O-x,y為物理成像平面坐標(biāo)系，O-Xc,Yc,Zc為 相機(jī)坐標(biāo)系，O-Xw,Yw,Zw為世界坐標(biāo)系，針孔相機(jī)模型就是從世界坐標(biāo)系中的點Pw(Xw,Yw,Zw) 到圖像坐標(biāo)系中的點 (u,v) 的投影過程.

圖1 針孔成像幾何模型圖

此投影過程主要包含了從世界坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系，以及從相機(jī)坐標(biāo)系轉(zhuǎn)換到圖像坐標(biāo)系，此過程可表示為

式中：Zc為空間點P在相機(jī)坐標(biāo)系下的Z軸坐標(biāo)；(u,v) 為點P的圖像坐標(biāo)；K為相機(jī)的內(nèi)參數(shù)矩陣，常在相機(jī)出廠時確定，或者后續(xù)采用相機(jī)標(biāo)定算法進(jìn)行標(biāo)定獲得[10]；Pc為 點P的相機(jī)坐標(biāo)系坐標(biāo)；Pw為點P的世界坐標(biāo)系坐標(biāo)；R、t分別為相機(jī)坐標(biāo)系和世界坐標(biāo)系之間變換的旋轉(zhuǎn)矩陣和平移矢量，R為3×3的矩陣，t為3×1的矢量，其也稱為相機(jī)的外參數(shù)；T=為相機(jī)外參數(shù)矩陣，也稱為變換矩陣，用于描述世界坐標(biāo)系中的點相對于相機(jī)的位姿變化.

在雙目相機(jī)模型中可以利用提取的特征點在兩副圖像坐標(biāo)系中的坐標(biāo)求解得到Zc，當(dāng)提取到多個特征點時，則可構(gòu)建多個如式(1)的方程來解算位姿參數(shù)R和t，或采用非線性優(yōu)化手段求解，從而估計得到相機(jī)前后幀的位姿變化. 同時在這個過程中還進(jìn)行著位姿優(yōu)化和閉環(huán)檢測，從而得到全局一致的相機(jī)位姿結(jié)果.

1.2 緊組合框架與數(shù)學(xué)模型

GNSS和視覺緊組合模型下，GNSS原始觀測數(shù)據(jù)直接與視覺導(dǎo)航結(jié)果進(jìn)行組合，可以實現(xiàn)觀測值層面的組合. 在GNSS觀測衛(wèi)星數(shù)少于4顆的情況下，仍可以進(jìn)行緊組合解算，充分利用GNSS觀測信息，圖2為緊組合系統(tǒng)框架圖.

圖2 GNSS/視覺緊組合系統(tǒng)框架圖

通常，GNSS和視覺觀測的組合導(dǎo)航解算可以采用卡爾曼濾波器來進(jìn)行. 設(shè)GNSS/視覺緊組合的狀態(tài)向量XTC為

式中：L、B、h分別表示大地坐標(biāo)系下的經(jīng)度、緯度和大地高；VE、VN、VU分別表示導(dǎo)航坐標(biāo)系下東(E)、北(N)、天頂(U)方向的速度； dt表示接收機(jī)鐘差的等效距離； δ (?) 表示其相應(yīng)的誤差模式.

設(shè)GNSS/視覺緊組合狀態(tài)方程的離散形式為

式中：XTC,k+1為 緊組合tk+1時 刻的狀態(tài)向量；XTC,k為緊組合tk時刻的狀態(tài)向量； ΦTC,k+1,k為 從tk時刻到tk+1時刻的狀態(tài)轉(zhuǎn)移矩陣；wk為過程噪聲矩陣，當(dāng)采樣間隔Δt=tk+1-tk內(nèi)系統(tǒng)的動力學(xué)模型矩陣F(t) 可以視為不變或 Δt足夠短時， ΦTC,k+1,k可以使用下式計算

式中，動力學(xué)模型矩陣F(tk) 為

其中，F(xiàn)pp和Fpv矩陣為：

式中，RM和RN分別為卯酉圈曲率半徑和子午圈曲率半徑.

對于緊組合濾波器，GNSS系統(tǒng)對應(yīng)的觀測向量是每顆衛(wèi)星偽距的誤差，采用 ρ?i表示根據(jù)當(dāng)前估計位置計算的第i顆衛(wèi)星的偽距，則GNSS系統(tǒng)對應(yīng)的觀測向量 δzTC,G可表示為

式中：Pn表示第n顆 衛(wèi)星的偽距觀測值； ρ?n表示根據(jù)當(dāng)前估計位置計算的第n顆衛(wèi)星的偽距，其他符號同上.

由于視覺導(dǎo)航為相對定位方法，視覺導(dǎo)航結(jié)果位于初始相機(jī)位置為原點的相機(jī)坐標(biāo)系中，因此需要先利用GNSS定位結(jié)果初始化視覺導(dǎo)航系統(tǒng)，在初始化后利用視覺導(dǎo)航位置增量結(jié)果進(jìn)行位置推算，同時由于GNSS位置結(jié)果與視覺位置結(jié)果存在旋轉(zhuǎn)關(guān)系，因此采用最近迭代法將視覺導(dǎo)航結(jié)果轉(zhuǎn)換到與GNSS位置結(jié)果對應(yīng)的導(dǎo)航坐標(biāo)系下.

視覺導(dǎo)航系統(tǒng)對應(yīng)的觀測向量 δzTC,V可以表示為

式 中： ΔrE、ΔrN、ΔrU分別表示導(dǎo)航坐標(biāo)系下E、N、U方向前后幀的相對位置變化分別表示緊組合濾波器中估計的E方向、N方向、U方向的速度誤差； Δt表示前后幀的時間間隔.

聯(lián)合GNSS偽距單點定位(SPP)的觀測方程

式中：上標(biāo)s與下標(biāo)r分別表示衛(wèi)星和接收機(jī)表示GNSS偽距觀測值表示衛(wèi)星與接收機(jī)間的幾何距離；c表示光速； δtr表 示接收機(jī)鐘差； δts表示衛(wèi)星鐘差；表示對流層誤差表示電離層誤差表示觀測噪聲.

將式(10)進(jìn)行泰勒展開并線性化，綜上可得，GNSS/視覺觀測緊組合觀測方程為

式中：enn表示接收機(jī)到第 顆衛(wèi)星的單位余弦矩陣；式(11)在大地坐標(biāo)系誤差中還隱含了從大地坐標(biāo)系到地心地固坐標(biāo)系的轉(zhuǎn)換.

式(11)構(gòu)建了GNSS/視覺觀測緊組合系統(tǒng)的數(shù)學(xué)模型，采用卡爾曼濾波器即可解算得到載體的位置參數(shù)，實現(xiàn)載體的導(dǎo)航定位.

2 試驗與結(jié)果分析

為了驗證上述提出的緊組合方法的可行性，搭建了圖3的小推車平臺，小推車上主要包含的實驗設(shè)備如下.

圖3 試驗推車平臺示意圖

如圖3所示，推車平臺的GPS天線位于2個相機(jī)的正中間，左右相機(jī)光軸保持平行，并與地面平行安裝，在數(shù)據(jù)采集之前，進(jìn)行相機(jī)標(biāo)定實驗，相機(jī)在慣導(dǎo)設(shè)備iMAR-FASA的脈沖觸發(fā)下，以10 Hz的幀率拍攝雙目圖像，慣導(dǎo)器件數(shù)據(jù)采集頻率為200 Hz，GNSS數(shù)據(jù)采集頻率為1 Hz，圖像數(shù)據(jù)與GNSS數(shù)據(jù)通過慣導(dǎo)設(shè)備脈沖觸發(fā)進(jìn)行時間同步.

試驗選取某校園道路為數(shù)據(jù)采集地點，此環(huán)境中由于道路兩側(cè)存在樹木，會造成GNSS信號出現(xiàn)遮擋，同時此環(huán)境中特征點豐富，但是道路中存在較大的人流量和車流量，可能會造成視覺特征點的誤匹配. 圖4為小推車推動的平面軌跡圖，圖5給出了推車的E、N、U方向的速度. 在采集圖像與GNSS數(shù)據(jù)后，基于ORB-SLAM2系統(tǒng)運(yùn)行得到視覺結(jié)果，同時將GNSS接收機(jī)采集的數(shù)據(jù)、對應(yīng)的基站數(shù)據(jù)和慣導(dǎo)數(shù)據(jù)融合處理，得到GNSS/慣性緊組合結(jié)果作為此試驗的參考值.

圖4 小推車平面軌跡圖

圖5 小推車速度

采用小推車上GNSS接收機(jī)采集的偽距觀測值進(jìn)行單點定位解算，單獨GPS SPP定位結(jié)果誤差圖如圖6所示，可知在道路兩側(cè)具有樹木情況下，由于可見衛(wèi)星數(shù)變化以及觀測條件差導(dǎo)致GPS SPP定位結(jié)果誤差圖中不斷出現(xiàn)跳躍的粗差點. 表1統(tǒng)計了相應(yīng)的誤差結(jié)果，由圖6和表1可知E、N、U方向最大誤差分別達(dá)到了7.19 m、8.60 m、15.57 m，平面上E方向和N方向誤差大都在4 m內(nèi)，E方向和N方向誤差均方根(RMS)值分別為1.92 m和1.53 m，U方向誤差較大，部分跳躍的點誤差可達(dá)十幾米.

圖6 GPS SPP定位結(jié)果誤差圖

表1 四種方法的誤差結(jié)果統(tǒng)計 m

基于ORB-SLAM2系統(tǒng)解算得到的視覺導(dǎo)航結(jié)果如圖7所示，在小推車推動過程中由于相機(jī)的抖動和圖像中特征的不斷移動使得視覺導(dǎo)航結(jié)果誤差積累較快.由表1可知，E、N、U方向最大誤差分別達(dá)到了4.58 m、10.49 m、2.51 m，與單獨GPS SPP定位相比，純視覺導(dǎo)航的結(jié)果在平面方向上較差，但是由于相機(jī)在U方向上沒有移動較大的距離，視覺導(dǎo)航的U方向的誤差RMS值要明顯小于SPP的結(jié)果.

圖7 純視覺導(dǎo)航結(jié)果誤差圖

將上述得到的GPS SPP定位結(jié)果和視覺導(dǎo)航結(jié)果采用已研究的松組合算法進(jìn)行松組合解算[11]，圖8展示了GNSS/視覺松組合結(jié)果的誤差圖，可知圖8中不再有圖6中較大的粗差跳變點. 其中U方向效果最明顯，松組合結(jié)果與圖7的純視覺結(jié)果相比，其誤差也不隨時間積累. 由表1誤差結(jié)果統(tǒng)計可知，松組合誤差RMS值相較純視覺結(jié)果誤差RMS值在E、方向分別降低了44.57%和86.82%，松組合誤差RMS值相較GPS SPP定位結(jié)果誤差RMS值在E、N、U方向分別降低了25.52%、43.14%、54.37%.

圖8 GNSS/視覺松組合結(jié)果誤差圖

GNSS/視覺緊組合系統(tǒng)采用GPS衛(wèi)星的偽距觀測值，與視覺導(dǎo)航結(jié)果采用上述介紹的緊組合方法進(jìn)行緊組合解算，將解算得到的定位結(jié)果轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下，與參考值進(jìn)行比較. 圖9為GNSS/視覺緊組合結(jié)果誤差圖，表1同樣統(tǒng)計了緊組合系統(tǒng)誤差的相關(guān)信息. 通過對圖9和表1的分析可得：與GNSS/視覺松組合類似，GNSS/視覺緊組合也克服了單獨視覺導(dǎo)航系統(tǒng)誤差不斷積累的問題，可以實現(xiàn)持續(xù)的導(dǎo)航定位，較單獨的GPS SPP解算結(jié)果而言，緊組合的精度較好，利用視覺導(dǎo)航短時精度高的特性，緊組合也能較好的消除明顯的粗差跳躍點，在U方向效果最明顯.

圖9 GNSS/視覺緊組合結(jié)果誤差圖

為了更顯著的說明緊組合的優(yōu)勢，模擬了GNSS弱信號環(huán)境并進(jìn)行了相應(yīng)的松組合和緊組合解算. 在圖10中，模擬了40～70 s共30 s的GNSS弱信號環(huán)境，此時可觀測到的衛(wèi)星數(shù)少于4顆，由于缺少足夠的GNSS信息，不能進(jìn)行松組合解算，只能采用視覺導(dǎo)航結(jié)果進(jìn)行位置推算并使用RTS(Rauch-Tung-Striebel)算法平滑. 由圖10可得，雖然經(jīng)過RTS算法平滑，中斷后隨著導(dǎo)航時間的增加，松組合的位置結(jié)果誤差也顯著增大. 雖然可見衛(wèi)星數(shù)少于4顆，由本文提出的緊組合方法可知，此時仍能進(jìn)行緊組合解算. 圖11為40～70 s模擬可觀測衛(wèi)星數(shù)少于4顆時GNSS/視覺緊組合解算結(jié)果誤差圖，表2統(tǒng)計了模擬可見衛(wèi)星數(shù)少于4顆時松組合和緊組合誤差結(jié)果，可得在GNSS弱信號環(huán)境下可觀測衛(wèi)星數(shù)不足時，緊組合誤差RMS優(yōu)于松組合，在E、N和U方向分別提升了43.75%、88.42%、11.76%. 由表2統(tǒng)計結(jié)果可知，緊組合方法誤差最大值在E、N、U三個方向均大于松組合方法，這是由于松組合此時不能解算，由視覺結(jié)果濾波得到位置，而緊組合方法由于存在粗差點，可能造成出現(xiàn)較大誤差點.

圖10 40～70 s模擬衛(wèi)星數(shù)量少于4顆松組合結(jié)果

圖11 40～70 s模擬衛(wèi)星數(shù)量少于4顆緊組合結(jié)果

表2 模擬衛(wèi)星數(shù)量少于4顆時松組合和緊組合誤差結(jié)果統(tǒng)計m

3 結(jié) 語

本文研究了一種GNSS/視覺緊組合導(dǎo)航算法，不僅能有效地提升GNSS弱信號環(huán)境下導(dǎo)航定位的連續(xù)性和精度，并能在可觀測衛(wèi)星數(shù)少于4顆時持續(xù)導(dǎo)航定位. 首先基于圖像數(shù)據(jù)利用ORB-SLAM2開源平臺求解得到視覺位置結(jié)果增量，然后與GNSS偽距觀測數(shù)據(jù)采用KF進(jìn)行聯(lián)合解算，不僅能夠改善單個導(dǎo)航系統(tǒng)導(dǎo)航定位的精度和連續(xù)性，而且提升了導(dǎo)航定位服務(wù)的可靠性和可用性. 實測數(shù)據(jù)測試結(jié)果表明：此GNSS/視覺緊組合導(dǎo)航算法具有可行性，聯(lián)合GNSS偽距觀測數(shù)據(jù)和視覺導(dǎo)航結(jié)果緊組合導(dǎo)航定位，不僅能夠有效地提升弱GNSS環(huán)境下導(dǎo)航定位的連續(xù)性和精度，并能在可觀測衛(wèi)星數(shù)少于4顆時持續(xù)提供有效的導(dǎo)航定位結(jié)果. 視覺導(dǎo)航在特征點變化劇烈或缺乏足夠特征點的環(huán)境下也會失效，后續(xù)考慮在緊組合系統(tǒng)中加入慣性導(dǎo)航數(shù)據(jù)，融合三種傳感器進(jìn)行可靠、持續(xù)且精度高的導(dǎo)航定位.

致謝：感謝武漢大學(xué)李星星教授團(tuán)隊提供的測試數(shù)據(jù). 本文的研究得到了國家自然科學(xué)基金項目(41974032)、中組部、陜西省、中科院高層次人才項目以及王寬誠教育基金會的支持.