摘要：利用可量測(cè)影像具有高精度空間位置信息的特點(diǎn)，提出了一種可量測(cè)影像與GPS、IMU組合導(dǎo)航方法。提出了系統(tǒng)框架，研究了可量測(cè)影像與道路網(wǎng)數(shù)據(jù)一體化組織方法，可量測(cè)影像與實(shí)時(shí)影像匹配定位算法，可量測(cè)影像與GPS、IMU聯(lián)邦卡爾曼濾波導(dǎo)航模型。實(shí)驗(yàn)表明，新方法可以彌補(bǔ)車(chē)輛導(dǎo)航系統(tǒng)衛(wèi)星信號(hào)失鎖、慣性器件誤差隨時(shí)間累積發(fā)散等問(wèn)題，提高了系統(tǒng)在復(fù)雜環(huán)境中定位精度和穩(wěn)健性。

衛(wèi)星/慣性組合導(dǎo)航是目前車(chē)輛導(dǎo)航系統(tǒng)(vehicle navigation system, VNS)的主要導(dǎo)航定位手段，但是，在復(fù)雜環(huán)境中，衛(wèi)星導(dǎo)航會(huì)因信號(hào)遮擋或多路徑干擾而失效，慣性導(dǎo)航誤差則會(huì)隨著時(shí)間而累積發(fā)散，因此，如何實(shí)現(xiàn)可靠、連續(xù)導(dǎo)航是車(chē)輛導(dǎo)航領(lǐng)域的一個(gè)研究熱點(diǎn)。

近年來(lái)，無(wú)人駕駛技術(shù)、車(chē)聯(lián)網(wǎng)、地面移動(dòng)測(cè)量等汽車(chē)平臺(tái)的精確自主導(dǎo)航與測(cè)量估計(jì)技術(shù)發(fā)展迅猛，技術(shù)應(yīng)用范圍不斷擴(kuò)大，國(guó)內(nèi)外相關(guān)機(jī)構(gòu)對(duì)相關(guān)技術(shù)開(kāi)展了大量研究工作。Dupuis等利用車(chē)載低成本GPS的少量信號(hào)即快速實(shí)現(xiàn)了地圖初始估計(jì)，以實(shí)現(xiàn)自主車(chē)任務(wù)準(zhǔn)備的低成本、快速、準(zhǔn)確初始化[1]。Yun等進(jìn)行了移動(dòng)測(cè)量系統(tǒng)(MMS)進(jìn)行道路管理的可用性測(cè)試，其中IMU、GNSS、可量測(cè)影像(DMI)組合輸出車(chē)輛軌跡，利用與GNSS同步的圖像及點(diǎn)云數(shù)據(jù)高效提取道路標(biāo)志及道路損壞部分[2]。

Suhr等提出和實(shí)現(xiàn)了一種基于傳感器融合的低成本車(chē)輛定位系統(tǒng)，其中融合了GPS、IMU、碼盤(pán)、單個(gè)前置相機(jī)、以及通過(guò)粒子濾波得到的少量數(shù)字地圖，道路標(biāo)志僅通過(guò)最小數(shù)量點(diǎn)進(jìn)行表示，計(jì)算代價(jià)小，可在低成本嵌入式系統(tǒng)中實(shí)時(shí)執(zhí)行計(jì)算[3]。Meng等研究了基于GNSS/IMU/碼盤(pán)/LiDAR傳感器融合的自主車(chē)魯棒定位方法[4], 提出了多約束出錯(cuò)檢測(cè)方法以平滑GNSS信號(hào)跳變時(shí)的車(chē)輛定位結(jié)果，同時(shí)定位誤差可利用點(diǎn)云定位方法進(jìn)一步補(bǔ)償改進(jìn)。

李德仁院士在文獻(xiàn)[5]中提出了一個(gè)從天到地、影像到模型、靜止到動(dòng)態(tài)的城市環(huán)境監(jiān)測(cè)系統(tǒng)，可滿(mǎn)足建筑變化監(jiān)測(cè)、安全巡航、監(jiān)控分析等多種需求。盧秀山等提出基于真彩激光點(diǎn)云，構(gòu)建城市自然數(shù)字模型；基于車(chē)載移動(dòng)測(cè)量、無(wú)人機(jī)航攝、手機(jī)拍照、高清視頻監(jiān)控及云計(jì)算平臺(tái)等技術(shù)手段，形成城市自然數(shù)字模型的快速更新技術(shù)體系，來(lái)構(gòu)建最優(yōu)現(xiàn)勢(shì)性的城市地理信息平臺(tái)，以奠定城市管理智能化的數(shù)據(jù)基礎(chǔ)[6]。筆者在文獻(xiàn)[7]中對(duì)DMI的導(dǎo)航數(shù)據(jù)組織模型進(jìn)行了研究。這些研究中分別采用了GNSS、IMU、DMI、LiDAR、碼盤(pán)等多種傳感器搭建所需系統(tǒng)，以實(shí)現(xiàn)高精度的實(shí)時(shí)導(dǎo)航與測(cè)量目標(biāo)。

可量測(cè)影像(DMI)是一種新興的地面立體影像信息產(chǎn)品，包含時(shí)空序列上絕對(duì)外方位元素信息，可以支持對(duì)環(huán)境實(shí)景的直接瀏覽、對(duì)目標(biāo)地物高度、寬度、面積等信息的相對(duì)測(cè)量，以及絕對(duì)位置解析測(cè)量和目標(biāo)屬性信息挖掘等應(yīng)用[8-9]。利用車(chē)輛導(dǎo)航系統(tǒng)采集實(shí)時(shí)影像(real-time image)，并與預(yù)先獲取的可量測(cè)影像進(jìn)行匹配，將匹配上的可量測(cè)影像空間位置信息傳遞至實(shí)時(shí)影像，通過(guò)空間坐標(biāo)變換推算出運(yùn)動(dòng)載體的當(dāng)前位置。因此，將可量測(cè)影像引入到車(chē)輛導(dǎo)航系統(tǒng)中，并與實(shí)時(shí)影像進(jìn)行匹配定位計(jì)算，計(jì)算結(jié)果可以作為一種新的定位數(shù)據(jù)源和衛(wèi)星、慣導(dǎo)的定位信息進(jìn)行數(shù)據(jù)融合，從而提高復(fù)雜環(huán)境下的組合導(dǎo)航系統(tǒng)的可靠性和連續(xù)性。

本文提出了一種基于可量測(cè)影像與衛(wèi)星、慣性組合導(dǎo)航方法，研究了系統(tǒng)框架、實(shí)現(xiàn)原理和關(guān)鍵技術(shù)，并用實(shí)驗(yàn)驗(yàn)證了方法的有效性，以期為復(fù)雜環(huán)境下車(chē)輛導(dǎo)航提供新的技術(shù)途徑。

1 技術(shù)框架和工作原理

GPS/IMU/DMI組合導(dǎo)航原型系統(tǒng)安裝衛(wèi)星定位(GPS)、慣性測(cè)量單位(IMU)、里程計(jì)、CCD像機(jī)等設(shè)備，在導(dǎo)航計(jì)算機(jī)中裝載DMI數(shù)據(jù)庫(kù)、二維導(dǎo)航數(shù)據(jù)庫(kù)和導(dǎo)航軟件。通過(guò)可量測(cè)影像與衛(wèi)星導(dǎo)航定位、慣性導(dǎo)航、里程計(jì)等集成，實(shí)現(xiàn)多源信息融合的導(dǎo)航定位，同時(shí)支持環(huán)境實(shí)景顯示、查詢(xún)、分析、量算等導(dǎo)航輔助功能，總體技術(shù)框架如圖 1所示。

1.1 DMI與道路網(wǎng)數(shù)據(jù)一體化組織[7]

DMI與RTI匹配定位的前提，是需要從可量測(cè)影像數(shù)據(jù)庫(kù)中快速查詢(xún)到當(dāng)前定位點(diǎn)前趨方向的影像。因此，建立DMI高效索引以滿(mǎn)足導(dǎo)航定位的實(shí)時(shí)性要求是一個(gè)關(guān)鍵。

本文利用DMI數(shù)據(jù)沿道路呈線(xiàn)性分布的特點(diǎn)，采用線(xiàn)性參考系統(tǒng)下的基于道路的動(dòng)態(tài)分段索引結(jié)構(gòu)，建立影像與所在路段、路段偏距的空間拓?fù)潢P(guān)聯(lián)，并利用大文件模型存儲(chǔ)索引表、影像主體和內(nèi)、外方位元素參數(shù)。在檢索時(shí)，根據(jù)定位點(diǎn)概略位置計(jì)算出道路、路段，進(jìn)而查詢(xún)到定位點(diǎn)可能分布區(qū)域的DMI序列，再分別與獲取的RTI進(jìn)行匹配定位計(jì)算。

對(duì)于具體方法，筆者在文獻(xiàn)[7]中進(jìn)行了研究分析與試驗(yàn)評(píng)估，這里不再贅述。

1.2 DMI與RTI的匹配定位

DMI與RTI匹配定位的關(guān)鍵，是從檢索到的定位點(diǎn)可能分布區(qū)域的影像序列中，查詢(xún)到與RTI匹配的DMI。由于DMI與RTI拍攝時(shí)間不同、拍攝時(shí)刻像機(jī)位置和姿態(tài)不同，2幅影像分辨率和光照不同，因此，需要可靠的匹配方法實(shí)現(xiàn)2幅不同條件下所成影像的精確配準(zhǔn)。

本文采用SIFT特征匹配[10-13]和RANSAC抗差處理[14]相結(jié)合的影像特征點(diǎn)提取和匹配方法解決DMI和RTI的匹配問(wèn)題，算法框架如圖 2所示。

DMI與RTI匹配的實(shí)質(zhì)是在不同尺度空間上查找特征點(diǎn)(或關(guān)鍵點(diǎn))，也就是具有方向信息的局部極值點(diǎn)，如暗區(qū)中的亮點(diǎn)、亮區(qū)中的暗點(diǎn)、角點(diǎn)、邊緣點(diǎn)等。這些點(diǎn)能夠?qū)τ谟跋裥D(zhuǎn)、縮放和仿射變換保持不變，可以滿(mǎn)足在不同光照、不同位置姿態(tài)、不同分辨率等條件下所成的DMI和RTI的匹配需求。首先，通過(guò)2幅影像的高斯差分尺度空間(DoG scale-space)中采樣點(diǎn)與其影像域和尺度域中的所有鄰點(diǎn)進(jìn)行比較，從而檢測(cè)提取出特征點(diǎn)；然后，根據(jù)特征點(diǎn)鄰域像素的梯度方向分布特性，為每個(gè)特征點(diǎn)附加一個(gè)主方向和多個(gè)輔方向的描述性信息，即特征向量；接下來(lái)，利用特征點(diǎn)特征向量的歐氏距離，對(duì)兩幅影像特征點(diǎn)進(jìn)行相似性度量，確定匹配上的特征點(diǎn)；最后，為保證算法魯棒性，采用隨機(jī)抽樣一致性算法RANSAC(random sample consensus)，通過(guò)構(gòu)建特征點(diǎn)估計(jì)模型，剔除可能存在的錯(cuò)配點(diǎn)。當(dāng)匹配上的特征點(diǎn)滿(mǎn)足一定數(shù)量(一般在5個(gè)以上)，且均勻分布，則認(rèn)為兩幅影像匹配成功，同時(shí)獲得RTI特征點(diǎn)坐標(biāo)，包括特征點(diǎn)像素坐標(biāo)和對(duì)應(yīng)物點(diǎn)的空間坐標(biāo)。

由RTI特征點(diǎn)坐標(biāo)到載體當(dāng)前位置坐標(biāo)的推算過(guò)程，涉及空間直角坐標(biāo)系、載體本體坐標(biāo)系、像機(jī)坐標(biāo)系、影像坐標(biāo)系、內(nèi)存坐標(biāo)系的轉(zhuǎn)換，如圖 3所示。

計(jì)算過(guò)程如下：1)根據(jù)(1)式，將RTI特征點(diǎn)對(duì)應(yīng)物點(diǎn)Ai在空間直角坐標(biāo)系GP中坐標(biāo)GAi(XGAi,YGAi,ZGAi)轉(zhuǎn)換為載體本體坐標(biāo)系VP中坐標(biāo)VAi(XVAi,YVAi,ZVAi)。

式中,α為載體航向角, ΔXG, ΔYG為未知量。

2) 根據(jù)(2)式、(3)式, 將VAi轉(zhuǎn)換為像機(jī)坐標(biāo)系CP坐標(biāo)CAi(XCAi,YCAi,ZCAi)。

式中，VPC(VXC,VYC,VZC),ψ,θ,φ表示像機(jī)在VP中的安裝位置和姿態(tài), 可事先標(biāo)定,VCR為旋轉(zhuǎn)變換矩陣。

3) 通過(guò)(4)式、(5)式, 將CAi轉(zhuǎn)換為影像坐標(biāo)系IP中坐標(biāo)IAi(xI,yI)和內(nèi)存坐標(biāo)系BP中坐標(biāo)BAi(xBAi,yBAi)。

式中,Nr和Nc分別是影像坐標(biāo)系中單位長(zhǎng)度對(duì)應(yīng)的內(nèi)存坐標(biāo)系中的像素行數(shù)和列數(shù),BPI=[BCI,BRI]T為列和行方向主點(diǎn)位置偏移量,f,Nr,Nc,BCI,BRI為像機(jī)內(nèi)方位元素, 可事先標(biāo)定。

4) 根據(jù)(5)式, 利用5個(gè)以上匹配的特征點(diǎn)構(gòu)建方程, 方程中包含3個(gè)未知量ΔXG, ΔYG和α, 采用最小二乘法求解方程, 解得3個(gè)未知量。

5) 將載體本體坐標(biāo)系原點(diǎn)位置代入(1)式, 反算得到運(yùn)動(dòng)載體的空間直角坐標(biāo)。

1.3 GNSS/IMU/DMI聯(lián)邦卡爾曼濾波導(dǎo)航模型

原形系統(tǒng)采用聯(lián)邦卡爾曼濾波將基于DMI的匹配定位結(jié)果與GPS、IMU數(shù)據(jù)進(jìn)行融合, 濾波器結(jié)構(gòu)如圖 4所示。

采用IMU作為參考子系統(tǒng), 分別與GPS和DMI定位輸出構(gòu)成2個(gè)子系統(tǒng):子濾波器1包括IMU和GPS; 子濾波器2包括DMI匹配定位、IMU和里程計(jì)。,?分別為2個(gè)子濾波器的局部估計(jì)值,?P1,?P2為協(xié)方差陣,?和Pg為全局最優(yōu)估計(jì)。

1) 公共參考系統(tǒng)基于IMU構(gòu)建捷聯(lián)慣導(dǎo)系統(tǒng), 狀態(tài)參數(shù)如(6)式所示, 狀態(tài)方程如(7)式所示。

式中,ψN,ψE,ψD為姿態(tài)誤差,δVN,δVE,δVD為速度誤差,δλ,δL,δh為位置誤差。

(7)

式中,FI(t)為狀態(tài)轉(zhuǎn)換矩陣,GI(t)為噪聲驅(qū)動(dòng)矩陣,WI(t)為系統(tǒng)噪聲。

2) 子系統(tǒng)1中GPS狀態(tài)參數(shù)如(8)式所示, IMU/GPS組合定位系統(tǒng)狀態(tài)方程如(9)式所示。

(8)

δλg,δLg,δhg為經(jīng)度、緯度和高程方向GPS位置誤差,δVEg,δVNg,δVDg為東、北和地向GPS速度誤差。

式中

子濾波器1采用位置和速度組合方式, 分別取3個(gè)方向的位置之差和速度之差為觀察量, 其中, IMU觀測(cè)量為:

GPS觀測(cè)量為:

λ,L,h為載體實(shí)際位置,VN,VE,VD為載體實(shí)際速度, 其他為相應(yīng)誤差。

子系統(tǒng)1量測(cè)方程如(10)式所示。

其中GPS觀測(cè)噪聲

3) 子系統(tǒng)2的觀測(cè)量主要依據(jù)DMI與RTI匹配定位信息和里程計(jì)速度信息, 并與IMU輸出信息比對(duì)。其狀態(tài)方程參照公共參考系統(tǒng), 如(7)式所示, 觀測(cè)方程如(11)式所示。

式中，

,λD,LD,hD是DMI與RTI匹配定位信息,VON,VOE,VOD是里程計(jì)輸出的速度信息,VIOD是相應(yīng)傳感器觀測(cè)噪聲構(gòu)成的6維觀測(cè)量。

2 實(shí)驗(yàn)與分析

采用某型地面移動(dòng)測(cè)量車(chē)作為實(shí)驗(yàn)平臺(tái)。平臺(tái)上集成有計(jì)算機(jī)、GPS、IMU和6臺(tái)CCD像機(jī)(編號(hào)1~6)等設(shè)備, 計(jì)算機(jī)中裝載預(yù)先采集的實(shí)驗(yàn)區(qū)域DMI數(shù)據(jù), 此外, 在實(shí)驗(yàn)平臺(tái)上加裝某型高精度激光陀螺慣性導(dǎo)航系統(tǒng)(ring laser gyroscope inertial navigation system, RLG-INS), 作為實(shí)驗(yàn)精度的比對(duì)基準(zhǔn), 如圖 5所示。

實(shí)驗(yàn)平臺(tái)主要指標(biāo):①I(mǎi)MU:陀螺漂移1.0°/hr, 加速度計(jì)漂移0.000 2g; ②GPS:平面定位精度為(10±1×10-6)mm, 高程精度為20 mm±1ppm; ③CCD:幀率16 frame/s, 分辨率1 392×1 040;④DMI:位置精度優(yōu)于0.5 m, 姿態(tài)精度優(yōu)于0.3°, 分辨率1 392×1 040, 影像采集間隔≤8 m。

實(shí)驗(yàn)中, 采用3號(hào)像機(jī)采集RTI作為子系統(tǒng)2的輸入。實(shí)時(shí)記錄主系統(tǒng)和子系統(tǒng)1、2輸出的姿態(tài)、速度和位置數(shù)據(jù), 并與RLG-INS輸出數(shù)據(jù)進(jìn)行比對(duì)。在實(shí)驗(yàn)中途, 通過(guò)人為關(guān)閉GPS, 模擬衛(wèi)星導(dǎo)航信號(hào)被干擾或遮擋, 檢驗(yàn)系統(tǒng)在衛(wèi)星失鎖狀態(tài)下工作的穩(wěn)定性。原型系統(tǒng)(主系統(tǒng)、子系統(tǒng)1、子系統(tǒng)2)與RLG-INS基準(zhǔn)的輸出姿態(tài)誤差、輸出速度誤差、輸出位置誤差分別如圖 6~圖 8所示。

可以看出:

1) 通過(guò)融合2個(gè)子系統(tǒng)的信息, 主系統(tǒng)定位、定姿和測(cè)速的精度與可靠性得到明顯改善。

2) 如圖 8、圖 6所示, 在定位性能上, 子系統(tǒng)2的反饋能夠降低IMU隨時(shí)間漂移帶來(lái)的誤差, 但定姿性能不夠理想。因此, 在對(duì)2個(gè)子系統(tǒng)信息融合處理中, 需要進(jìn)行數(shù)據(jù)置信度評(píng)判, 將子系統(tǒng)2對(duì)主系統(tǒng)定姿精度影響降至最低。

3) 如圖 8所示, 約780 s處, 因?yàn)镚PS被關(guān)閉, 子系統(tǒng)1工作異常, 定位誤差較大; 但子系統(tǒng)2不受影響, 工作正常, 主系統(tǒng)定位精度在允許范圍內(nèi)。

綜上所述, 采用本文方法, 將DMI與RTI匹配定位引入GPS/IMU組合導(dǎo)航系統(tǒng), 可以提升系統(tǒng)精度和穩(wěn)健性, 其中, 北向定位誤差由2.814 m(1σ)提升到1.914 m(1σ), 東向定位誤差由4.553 m(1σ)提升到3.161 m(1σ), 地向定位誤差由3.274 m(1σ)提升到2.847m(1σ), 當(dāng)失去衛(wèi)星信號(hào)時(shí), 系統(tǒng)定位結(jié)果并未受到明顯影響。

3 結(jié)論

本文提出的GPS/IMU/DMI組合導(dǎo)航方法充分利用了可量測(cè)影像具有高精度空間位置信息的特點(diǎn)優(yōu)勢(shì), 可以彌補(bǔ)傳統(tǒng)車(chē)輛導(dǎo)航系統(tǒng)衛(wèi)星信號(hào)失鎖、慣性器件隨時(shí)間誤差累積發(fā)散等帶來(lái)的問(wèn)題。在下一步工程化應(yīng)用中, 還需重點(diǎn)研究DMI數(shù)據(jù)組織、影像匹配、多源定位數(shù)據(jù)融合誤差控制等技術(shù), 進(jìn)一步提高系統(tǒng)效率與精度。