利用視覺(jué)信息輔助GNSS 緊耦合定位方法

王 磊，樹(shù)玉泉，付文舉 ，馬愛(ài)龍，陳銳志，蔚保國(guó)

（1. 衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，石家莊 050081；2. 武漢大學(xué) 測(cè)繪遙感信息工程國(guó)家重點(diǎn)實(shí)驗(yàn)室，武漢 430079）

0 引言

全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system, GNSS）的脆弱性是其服務(wù)性能的短板之一，在復(fù)雜地理和電磁環(huán)境條件下，GNSS 信號(hào)受到遮擋或者電磁干擾，往往無(wú)法正常地提供導(dǎo)航定位服務(wù)。另一方面，隨著電子戰(zhàn)、導(dǎo)航戰(zhàn)等電子對(duì)抗形式的出現(xiàn)，僅依賴衛(wèi)星導(dǎo)航定位技術(shù)獲得位置的可靠性逐漸降低，近年來(lái)開(kāi)展GNSS 干擾和GNSS 欺騙的事件時(shí)有發(fā)生[1-3]。未來(lái)的彈性定位、導(dǎo)航、授時(shí)（positioning, navigation and time, PNT）框架就是要集成多源異構(gòu)的多種導(dǎo)航源實(shí)現(xiàn)彈性的、堅(jiān)韌的導(dǎo)航定位服務(wù)[4]。而可信PNT，則要求利用多源融合的方法達(dá)到導(dǎo)航定位服務(wù)高可用的狀態(tài)[5]。為了提升GNSS 在復(fù)雜和受限環(huán)境下的定位性能，本文提出1 種將攝影測(cè)量技術(shù)和衛(wèi)星導(dǎo)航技術(shù)進(jìn)行融合的方法，提升導(dǎo)航定位服務(wù)在復(fù)雜場(chǎng)景下的可用性。

利用影像進(jìn)行定位的方法，已廣泛地應(yīng)用到室內(nèi)場(chǎng)景的導(dǎo)航定位中，例如機(jī)器人定位、視覺(jué)即時(shí)定位與地圖構(gòu)建（simultaneous localization and mapping, SLAM）等[6]，在室外也主要用于視覺(jué)里程計(jì)（visual odometer, VO）等近距離定位場(chǎng)景[7]。受到相機(jī)分辨率限制，如果物方目標(biāo)與相機(jī)距離變遠(yuǎn)，會(huì)導(dǎo)致定位精度顯著下降，因此在室外和大范圍場(chǎng)景下，一般不直接使用視覺(jué)信息進(jìn)行絕對(duì)定位[8]。視覺(jué)定位手段與GNSS 定位技術(shù)的融合方法主要分為松耦合和緊耦合2 種。松耦合的方式是2 種定位手段分別提供定位結(jié)果，然后利用因子圖等方法對(duì)多源定位結(jié)果進(jìn)行融合[9]。而目前緊耦合的方式又分為2 種[10]：①利用對(duì)天的相機(jī)實(shí)時(shí)獲取信號(hào)遮擋信息，然后輔助GNSS 拒絕接收到的衍射和繞射信號(hào)，降低非視距信號(hào)（none line-of-sight, NLOS）帶來(lái)的誤差，從而提升GNSS 在城市峽谷等區(qū)域的定位精度[11]；②將GNSS 信號(hào)與視覺(jué)里程計(jì)等傳感器融合，利用視覺(jué)里程計(jì)提供的相對(duì)位置變化信息，對(duì)GNSS 軌跡進(jìn)行約束來(lái)提升定位精度[12]。本文提出1 種觀測(cè)值域的緊耦合定位方法，即利用相片與物方平面之間的相似關(guān)系，計(jì)算相機(jī)到物方特征點(diǎn)的距離，再與GNSS 測(cè)距信號(hào)聯(lián)合定位解算。

1 緊耦合組合定位原理

在遮擋或者復(fù)雜環(huán)境條件下，單獨(dú)使用衛(wèi)星導(dǎo)航技術(shù)不足以直接定位，此時(shí)可通過(guò)攝影測(cè)量的方式增加觀測(cè)值，實(shí)現(xiàn)協(xié)同定位。具體的思路是利用攝影測(cè)量的原理，確定用戶終端到相片上若干標(biāo)志物之間的距離，再聯(lián)合衛(wèi)星測(cè)距和攝影測(cè)量測(cè)距的方式求解用戶終端的位置，其基本原理如圖1 所示。在 GNSS 無(wú)法單獨(dú)提供定位解的時(shí)候，利用影像提取若干測(cè)距信號(hào)，進(jìn)行聯(lián)合定位解算。

圖1 衛(wèi)星導(dǎo)航與攝影測(cè)量緊耦合定位技術(shù)原理

目前已有 GNSS 和偽衛(wèi)星等地基增強(qiáng)手段融合定位的先例。對(duì)于視覺(jué)而言，可以將相片上的特征點(diǎn)當(dāng)做“視覺(jué)基站”。如果這些特征點(diǎn)在世界坐標(biāo)系下的 3 維坐標(biāo)已知，則可通過(guò)測(cè)量相片來(lái)確定用戶到目標(biāo)的距離，此時(shí)相機(jī)也變成了 1 種測(cè)距設(shè)備。如果被攝目標(biāo)在世界坐標(biāo)系下的坐標(biāo)已知，并且可以測(cè)量被攝目標(biāo)到用戶的距離，那么被攝目標(biāo)相當(dāng)于1 個(gè)基站或者偽衛(wèi)星，稱作“視覺(jué)基站”。聯(lián)合處理GNSS 信號(hào)和視覺(jué)基站的信號(hào)，可以實(shí)現(xiàn)信號(hào)層面的聯(lián)合定位，稱為衛(wèi)星導(dǎo)航與攝影測(cè)量的緊耦合定位（如圖2 所示）。緊耦合定位的過(guò)程主要分 2 個(gè)步驟，即從視覺(jué)影像中提取距離觀測(cè)值，再與GNSS 聯(lián)合定位解算。

圖2 利用攝影測(cè)量的方法測(cè)量終端到“視覺(jué)基站”的方法示意

1.1 利用視覺(jué)影像提取距離觀測(cè)值

根據(jù)影像覆蓋范圍內(nèi)一定數(shù)量分布合理的像控點(diǎn)（已知其像點(diǎn)和地面點(diǎn)的坐標(biāo)），利用共線條件方程求解像片外方位元素θ=[XS, YS, ZS,ψ,ω,κ]T。在求解外方位元素前，需要利用格網(wǎng)標(biāo)志點(diǎn)對(duì)相機(jī)的內(nèi)方位元素進(jìn)行精確標(biāo)定，以削弱鏡頭畸變帶來(lái)的影響[13]。在后續(xù)論述中，假設(shè)相機(jī)已經(jīng)事先完成了內(nèi)參數(shù)的標(biāo)定并對(duì)鏡頭畸變進(jìn)行了合理的校準(zhǔn)。

如圖2 所示，物方已知點(diǎn)A和相機(jī)投影中心S在地面攝影測(cè)量坐標(biāo)系中的坐標(biāo)分別是（XA ,YA,ZA）、（XS,YS,ZS）；物方已知點(diǎn)A在相片上對(duì)應(yīng)的點(diǎn)a在像空間坐標(biāo)系中的坐標(biāo)是（x,y,-f）。由共線條件方程知，物方已知點(diǎn)的觀測(cè)方程[14]可表示為

為保證參數(shù)估計(jì)的精度和可靠性，需要測(cè)量4 個(gè)或更多像控點(diǎn)和對(duì)應(yīng)像點(diǎn)的坐標(biāo)，采用最小二乘平差方法解算。此時(shí)像點(diǎn)坐標(biāo)（x,y）作為觀測(cè)值，可列出每個(gè)像點(diǎn)的誤差方程為

利用最小二乘法求解相機(jī)外方位元素為

外方位元素的求解精度與物方距離遠(yuǎn)近有關(guān)。在相機(jī)分辨率一定的條件下，相機(jī)和物方距離越遠(yuǎn)，物方分辨率越低，像點(diǎn)觀測(cè)精度越低，外方位元素求解精度越低，位置求解精度越差。此時(shí)可利用求解的相機(jī)近似坐標(biāo)和相片目標(biāo)點(diǎn)的準(zhǔn)確坐標(biāo)計(jì)算相機(jī)至目標(biāo)點(diǎn)的距離，這樣就利用相片形成了若干個(gè)到目標(biāo)點(diǎn)的距離觀測(cè)值。利用相片提取距離觀測(cè)值的流程如圖3 所示。在求解相機(jī)外方位元素后，即可反算若干個(gè)相機(jī)到物方特征點(diǎn)之間的近似距離。該距離觀測(cè)值的方差可根據(jù)觀測(cè)距離遠(yuǎn)近建立經(jīng)驗(yàn)?zāi)Ｐ痛_定。

圖3 利用相機(jī)求解相機(jī)到目標(biāo)點(diǎn)距離算法流程

1.2 緊耦合定位方法

在可見(jiàn)衛(wèi)星數(shù)不足的條件下，單獨(dú)使用GNSS無(wú)法實(shí)現(xiàn)定位，此時(shí)可聯(lián)合相機(jī)計(jì)算得到的距離觀測(cè)值和 GNSS 觀測(cè)信號(hào)求解用戶坐標(biāo)。聯(lián)合影像與GNSS 定位求解過(guò)程為

式中：PG和PC分別為GNSS 測(cè)量的偽距和相機(jī)測(cè)量的距離；ρ為衛(wèi)星或者視覺(jué)基站和接收機(jī)之間的幾何距離（以米為單位）；δorb為 GNSS 衛(wèi)星軌道誤差；c為真空中的光速；分別為衛(wèi)星和接收機(jī)的時(shí)鐘誤差；Ii和δtrop分別為第i個(gè)頻率信號(hào)的電離層延遲和對(duì)流層延遲； ?Pi和 ?C分別為GNSS 偽距測(cè)量誤差和相機(jī)測(cè)距誤差。

GNSS 信號(hào)受到多種誤差源的影響，但其測(cè)距精度與用戶位置基本無(wú)關(guān)。相機(jī)通過(guò)視覺(jué)測(cè)距的方法得到的距離觀測(cè)值，基本不受其他誤差的影響，但其觀測(cè)精度與物方距離有關(guān)，距離越遠(yuǎn)，測(cè)距精度越差。

對(duì)GNSS 信號(hào)而言，采用標(biāo)準(zhǔn)定位方法時(shí)，即可忽略軌道誤差；衛(wèi)星鐘差通過(guò)廣播星歷修正；電離層、對(duì)流層誤差均利用經(jīng)驗(yàn)?zāi)Ｐ托拚挥脩糇鴺?biāo)和接收機(jī)鐘差利用參數(shù)估計(jì)。通過(guò)這種方法，可以獲得米級(jí)的GNSS 定位精度。

相機(jī)距離觀測(cè)值僅和用戶坐標(biāo)有關(guān)，無(wú)需估計(jì)鐘差參數(shù)，將幾何距離ρ按泰勒級(jí)數(shù)展開(kāi)，可得

式中：ρ0為用戶接收機(jī)到衛(wèi)星或視覺(jué)基站的近似幾何距離；ε為非線性誤差；為第i個(gè)衛(wèi)星或視覺(jué)基站的坐標(biāo)；(dx,dy,dz)為用戶接收機(jī)坐標(biāo)的增量。忽略式（6）的非線性誤差項(xiàng)后可得

式中E(·)為數(shù)學(xué)期望運(yùn)算符。利用泰勒級(jí)數(shù)線性化后，GNSS 觀測(cè)信號(hào)和相機(jī)觀測(cè)數(shù)據(jù)緊耦合的聯(lián)合定位問(wèn)題可近似地轉(zhuǎn)化為 1 個(gè)線性問(wèn)題，該線性系統(tǒng)為

式中：前m個(gè)觀測(cè)值為 GNSS 距離觀測(cè)值；后n個(gè)觀測(cè)值為相機(jī)距離觀測(cè)值。

對(duì)應(yīng)求解的4 個(gè)參數(shù)分別為用戶3 維坐標(biāo)和GNSS 接收機(jī)鐘差。此方程的最小二乘解為

2 實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證上述方法的正確性，進(jìn)行了仿真計(jì)算。仿真計(jì)算的場(chǎng)景如圖4 所示。仿真計(jì)算使用了3 顆可見(jiàn)的GNSS 衛(wèi)星，聯(lián)合1 張包含4 個(gè)特征點(diǎn)的影像。

圖4 仿真計(jì)算場(chǎng)景示意

仿真過(guò)程中，相機(jī)焦距設(shè)為100 mm，相機(jī)內(nèi)參數(shù)經(jīng)過(guò)預(yù)先標(biāo)定。為了研究物方距離對(duì)定位的影響，采用1 張真實(shí)照片，計(jì)算其外方位元素后固定相機(jī)位置，根據(jù)需求變換物方距離，解算出仿真的物方坐標(biāo)。利用仿真的物方坐標(biāo)研究物方坐標(biāo)遠(yuǎn)近對(duì)其定位精度的影響。進(jìn)一步仿真了3 顆可見(jiàn)的 GNSS 衛(wèi)星和對(duì)應(yīng)的偽距觀測(cè)值。GNSS 偽距觀測(cè)值的精度為0.3 m，其他誤差在仿真過(guò)程中忽略不計(jì)。筆者比較了不同物方距離條件下，聯(lián)合GNSS 信號(hào)和相機(jī)測(cè)距信號(hào)求解坐標(biāo)的精度，如圖5 所示。

圖5 不同物方距離條件下定位精度比較

圖5 顯示利用GNSS 與相機(jī)測(cè)距信號(hào)能夠顯著提高定位精度。為了量化定位精度，筆者對(duì)仿真采用的近100 個(gè)算例的定位精度進(jìn)行了統(tǒng)計(jì)，其均方根誤差（root mean squares, RMS）如表1 所示。

表1 相機(jī)定位與GNSS+相機(jī)緊耦合定位精度對(duì)比

表1 表明：在仿真的條件下，使用單GNSS無(wú)法定位，但與相機(jī)組合后，可使東西（E）和南北（N）方向平均定位精度從80～90 m 提升至1～4 m；高程（U）方向提升沒(méi)有平面明顯，也可將平均定位精度從41 m 精度提升至30 m 左右。

3 結(jié)束語(yǔ)

本文提出了 1 種視覺(jué)和 GNSS 緊耦合的定位技術(shù)，可提升 GNSS 在復(fù)雜場(chǎng)景下定位服務(wù)的可用性和抗干擾性。該方法的思路是利用相方坐標(biāo)和物方坐標(biāo)之間的相似關(guān)系，利用若干物方已知點(diǎn)求解相機(jī)的近似坐標(biāo)，再反算相機(jī)到已知點(diǎn)之間的距離。受到相機(jī)分辨率的限制，在室外場(chǎng)景進(jìn)行遠(yuǎn)距離定位時(shí)，視覺(jué)定位精度不高。利用影像反算得到的距離觀測(cè)值，聯(lián)合少量的 GNSS 距離觀測(cè)值進(jìn)行定位：一方面，視覺(jué)與GNSS 緊耦合定位的方式可以提升視覺(jué)定位的精度；另一方面，利用影像獲取的距離觀測(cè)值可以作為 GNSS 測(cè)距信號(hào)的補(bǔ)充，在GNSS 單獨(dú)無(wú)法定位時(shí)，聯(lián)合影像測(cè)距的方式即可實(shí)現(xiàn)用戶定位，提升了導(dǎo)航定位服務(wù)的可用性。仿真計(jì)算結(jié)果表明，聯(lián)合GNSS 測(cè)距信號(hào)與相機(jī)緊耦合定位，能夠有效地提升相機(jī)遠(yuǎn)距離定位的精度，同時(shí)在 GNSS 系統(tǒng)可見(jiàn)衛(wèi)星數(shù)不滿足定位條件時(shí)，仍可提供有效的定位結(jié)果。同時(shí)，仿真計(jì)算結(jié)果也驗(yàn)證了本文所述方法原理上的可行性。在城市峽谷等區(qū)域，目前單獨(dú)依靠GNSS 技術(shù)無(wú)法提供連續(xù)可靠的定位結(jié)果，但如果利用 3 維實(shí)景城市模型輔助，提供物方特征點(diǎn)坐標(biāo)，再利用本文所述的方法，則有望提升衛(wèi)星導(dǎo)航定位服務(wù)的可用性。