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

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

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

0 引 言

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

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

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

1 GNSS/視覺(jué)觀測(cè)緊組合定位方法

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

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

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

圖1為空間中點(diǎn)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)系中的點(diǎn)Pw(Xw,Yw,Zw) 到圖像坐標(biāo)系中的點(diǎn) (u,v) 的投影過(guò)程.

圖1 針孔成像幾何模型圖

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

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

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

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

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

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

通常，GNSS和視覺(jué)觀測(cè)的組合導(dǎo)航解算可以采用卡爾曼濾波器來(lái)進(jìn)行. 設(shè)GNSS/視覺(jué)緊組合的狀態(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/視覺(jué)緊組合狀態(tài)方程的離散形式為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 試驗(yàn)推車(chē)平臺(tái)示意圖

如圖3所示，推車(chē)平臺(tái)的GPS天線位于2個(gè)相機(jī)的正中間，左右相機(jī)光軸保持平行，并與地面平行安裝，在數(shù)據(jù)采集之前，進(jìn)行相機(jī)標(biāo)定實(shí)驗(yàn)，相機(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ù)通過(guò)慣導(dǎo)設(shè)備脈沖觸發(fā)進(jìn)行時(shí)間同步.

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

圖4 小推車(chē)平面軌跡圖

圖5 小推車(chē)速度

采用小推車(chē)上GNSS接收機(jī)采集的偽距觀測(cè)值進(jìn)行單點(diǎn)定位解算，單獨(dú)GPS SPP定位結(jié)果誤差圖如圖6所示，可知在道路兩側(cè)具有樹(shù)木情況下，由于可見(jiàn)衛(wèi)星數(shù)變化以及觀測(cè)條件差導(dǎo)致GPS SPP定位結(jié)果誤差圖中不斷出現(xiàn)跳躍的粗差點(diǎn). 表1統(tǒng)計(jì)了相應(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方向誤差較大，部分跳躍的點(diǎn)誤差可達(dá)十幾米.

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

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

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

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

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

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

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

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

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

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

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

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

3 結(jié) 語(yǔ)

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

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