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

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

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

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

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

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

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

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

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

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

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

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

對于具體方法，筆者在文獻[7]中進行了研究分析與試驗評估，這里不再贅述。

1.2 DMI與RTI的匹配定位

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

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

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

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

計算過程如下：1)根據(jù)(1)式，將RTI特征點對應(yīng)物點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)換為像機坐標(biāo)系CP坐標(biāo)CAi(XCAi,YCAi,ZCAi)。

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

3) 通過(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)系中單位長度對應(yīng)的內(nèi)存坐標(biāo)系中的像素行數(shù)和列數(shù),BPI=[BCI,BRI]T為列和行方向主點位置偏移量,f,Nr,Nc,BCI,BRI為像機內(nèi)方位元素, 可事先標(biāo)定。

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

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

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

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

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

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ū)動矩陣,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個方向的位置之差和速度之差為觀察量, 其中, IMU觀測量為:

GPS觀測量為:

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

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

其中GPS觀測噪聲

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

式中，

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

2 實驗與分析

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

實驗平臺主要指標(biāo):①IMU:陀螺漂移1.0°/hr, 加速度計漂移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。

實驗中, 采用3號像機采集RTI作為子系統(tǒng)2的輸入。實時記錄主系統(tǒng)和子系統(tǒng)1、2輸出的姿態(tài)、速度和位置數(shù)據(jù), 并與RLG-INS輸出數(shù)據(jù)進行比對。在實驗中途, 通過人為關(guān)閉GPS, 模擬衛(wèi)星導(dǎo)航信號被干擾或遮擋, 檢驗系統(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) 通過融合2個子系統(tǒng)的信息, 主系統(tǒng)定位、定姿和測速的精度與可靠性得到明顯改善。

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

3) 如圖 8所示, 約780 s處, 因為GPS被關(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)星信號時, 系統(tǒng)定位結(jié)果并未受到明顯影響。

3 結(jié)論

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