李博峰，陳廣鄂

同濟大學(xué)測繪與地理信息學(xué)院，上海 200092

高精度導(dǎo)航定位技術(shù)推動了時空信息賦能行業(yè)的迅猛發(fā)展，支撐無人駕駛[1]、智能交通[2]、智慧農(nóng)業(yè)[3]等領(lǐng)域的深度應(yīng)用。GNSS/INS組合導(dǎo)航綜合利用了導(dǎo)航衛(wèi)星的測距信號和IMU的角速度和比力觀測值，實現(xiàn)二者優(yōu)勢互補，有效提升了定位定姿的精度和可用性，已被廣泛應(yīng)用于自動駕駛等領(lǐng)域。然而在復(fù)雜的城市環(huán)境下，GNSS信號被長時間干擾或遮擋時，組合系統(tǒng)將缺少足夠的外部高精度觀測值來抑制INS誤差，進(jìn)而造成組合定位發(fā)散而無法確保高精度定位的連續(xù)性。

針對復(fù)雜環(huán)境下GNSS/INS組合定位發(fā)散的問題，諸多學(xué)者開展了相關(guān)研究工作并發(fā)表了一系列論著。這些方法從本質(zhì)上可分為兩類：一類是通過引入外部其他觀測信息來改善定位效果，比如引入車輛自身運動信息[4-5]、加入其他傳感器輔助等[6-7]；另一類方法是在不增加外部傳感器信息的情況下，通過共享車輛之間及車輛與道路之間的信息來實現(xiàn)車輛的協(xié)同定位[8]。目前主流的協(xié)同定位技術(shù)包括協(xié)同地圖匹配技術(shù)(cooperative map matching,CMM)和車輛協(xié)同相對位置約束技術(shù)(vehicles cooperative relative position constant,VCRPC)。CMM技術(shù)基于一定區(qū)域內(nèi)車輛受到的定位誤差相似及車輛始終位于道路上這兩個基本假設(shè)，通過共享此區(qū)域內(nèi)所有車輛的道路約束信息，求解并修正該定位誤差，從而提升所有車輛的定位精度[9]。為了提高未參與CMM的車輛的定位精度，文獻(xiàn)[10]提出了基于CMM和動對動RTK的協(xié)同定位技術(shù)，擴寬了常規(guī)CMM技術(shù)的應(yīng)用范圍。在前人的基礎(chǔ)上，文獻(xiàn)[11]定性研究了道路約束對CMM精度的影響，為車輛協(xié)同網(wǎng)絡(luò)的設(shè)計與優(yōu)化提供了參考。顯然，CMM方法僅適用于對定位精度要求不高的場景，無法滿足自動駕駛等場景的高精度定位需求。在VCRPC相關(guān)研究方面，文獻(xiàn)[12]利用專用短程通信技術(shù)(dedicated short-range communication,DSRC)在協(xié)同車輛定位方面開展了大量的研究工作，最早提出基于載波頻偏的DSRC定位解算方法，建立車輛間的相對空間關(guān)系，從而有效提升車輛在弱GNSS信號場景中的定位表現(xiàn)[12]。文獻(xiàn)[13]利用激光雷達(dá)點云匹配的方式，獲取車輛間的相對位置關(guān)系，從而實現(xiàn)車輛間觀測值的共享，試驗結(jié)果證明該方法可以有效提高車輛在復(fù)雜環(huán)境中的定位性能[13]。VCRPC通過整體構(gòu)建車輛自身及車輛之間的觀測模型，建立車輛間待估參數(shù)的相對關(guān)系，實現(xiàn)車輛間觀測值的共享，理論模型嚴(yán)密，是實現(xiàn)協(xié)同精密定位的關(guān)鍵。

本文提出一種基于GNSS和INS組合的車輛協(xié)同定位技術(shù)。首先建立單車GNSS/INS組合定位模型，在此基礎(chǔ)上建立了GNSS/INS組合傳感器的多車協(xié)同定位模型，并給出了相應(yīng)的解算方法。通過搭建雙車試驗場景，測試分析了本文方法的效果，為協(xié)同精密定位的發(fā)展提供參考。便于下文表述簡潔，BeiDou/GPS/Galileo/QZSS分別簡記為C/G/E/J，北-東-地坐標(biāo)系(North-East-Down)記為N-E-D；i、e、b和n分別表示慣性系、地心地固系、IMU測量坐標(biāo)系和當(dāng)?shù)貙?dǎo)航坐標(biāo)系；en表示元素全為1的n維列向量，In表示n維單位陣；E、D和?分別表示期望、方差和克羅內(nèi)克積算子。

1 協(xié)同定位數(shù)學(xué)模型

1.1 單車GNSS/INS緊組合定位數(shù)學(xué)模型

給定載體初始姿態(tài)、速度和位置，通過對IMU的角速度和比力觀測量積分實現(xiàn)姿態(tài)、速度和位置信息的更新。e系下的INS遞推公式為[4,13]

(1)

(2)

式中

(3)

式中，bω、bf、sω、sf、εω和εf分別表示陀螺儀和加表的殘余零偏項、比例因子項和量測噪聲。由于IMU每次上電后的殘余零偏項和比例因子項都不盡相同，因此無法通過先驗校正，而需要通過參數(shù)估計的方式對其進(jìn)行補償。通常對殘余零偏項和比例因子項建立一階馬爾可夫過程[4]

(4)

(5)

式中

(6)

(7)

在GNSS相對定位中，短基線雙差載波相位和偽距觀測方程為[19-23]

(8)

(9)

(10)

GNSS/INS組合在算法層面上包括松組合和緊組合兩種方式。松組合是結(jié)果層面上的組合，要求至少同時觀測到4顆衛(wèi)星；而緊組合是觀測值層面的組合，即使衛(wèi)星數(shù)小于4顆也可進(jìn)行組合解算[25]?？紤]到復(fù)雜環(huán)境中觀測衛(wèi)星數(shù)不足4顆的情況時常發(fā)生，導(dǎo)致松組合頻繁失效，因此本文將采用GNSS/INS緊組合進(jìn)行公式推導(dǎo)。

GNSS觀測值在GNSS/INS組合導(dǎo)航中起到量測更新的作用，因此組合導(dǎo)航算法的核心是建立INS誤差與GNSS觀測值的數(shù)學(xué)關(guān)系。由于IMU中心與GNSS天線相位中心不重合，二者之間存在一個在b系中維持恒定的距離，稱之為桿臂l，如圖1所示。

圖1 GNSS天線相位中心與IMU中心空間關(guān)系Fig.1 The spatial relationship between GNSS antenna phase center and IMU center

根據(jù)空間向量關(guān)系，在e系中有

(11)

對其進(jìn)行誤差擾動，得到e系下的GNSS天線相位中心的坐標(biāo)誤差與IMU中心的坐標(biāo)誤差之間的關(guān)系

(12)

聯(lián)合式(5)、式(9)和式(12)，并顧及GNSS和INS對誤差定義相反[13,15]，得GNSS/INS緊組合的觀測方程為

(13)

δxins,k=Φk-1,kδxins,k-1+wk-1

(14)

(15)

1.2 多車協(xié)同定位數(shù)學(xué)模型

本文以4輛車的情形為例闡述多車協(xié)同定位方法，多車之間的協(xié)同定位原理類似，如圖2所示。

圖2 多車協(xié)同定位Fig.2 Multi-vehicle cooperative positioning

(16)

此時車輛之間的量測更新方程為

(17)

量測方程的隨機模型為

diag(Σv1,Σv2,Σv3,Q,Q,Q,Q,Q,Q)

(18)

式中，Σv1、Σv2、Σv3表示3輛車的GNSS觀測向量的方差-協(xié)方差矩陣，其形式類似于式(10)。Q=σ2I3表示每個約束式(16)的隨機模型，σ反映了約束式(16)的先驗精度，根據(jù)經(jīng)驗其取值范圍為0.05～0.1 m。結(jié)合式(17)可以看到，協(xié)同定位模式相比于單獨解算模式增加了更多的約束，相當(dāng)于將這3輛車視為一個整體，從而不僅實現(xiàn)各輛車之間的觀測值共享，也大大增加了觀測冗余，提高了定位質(zhì)量。

2 試驗與分析

2.1 試驗設(shè)計

為了驗證提出算法的有效性，本文開展了一組半仿真雙車協(xié)同定位試驗。試驗數(shù)據(jù)于2021年7月13日在同濟大學(xué)校園內(nèi)采集，測試環(huán)境較為開闊，試驗測試軌跡如圖3所示。兩輛小車(記為VA、VB)分別用于采集各自的導(dǎo)航數(shù)據(jù)(GNSS和IMU觀測值)。VA上搭載了型號為Sepetentrio asterX-D的GNSS/INS組合設(shè)備，其中，GNSS采樣率為1 Hz，內(nèi)置IMU型號為ADIS16505，且采樣率為100 Hz；VB上搭載了Xsens MTIG710和Trimble Alloy，其中IMU采樣率為100 Hz，Alloy的采樣頻率為1 Hz。MTIG710和ADIS16505的參數(shù)見表1?；鶞?zhǔn)站為Trimble Alloy接收機，架設(shè)在300 m以內(nèi)的測繪館樓頂，采樣頻率為1 Hz。需要注意的是，IMU說明手冊上提供的參數(shù)通常代表產(chǎn)品在理想環(huán)境下的指標(biāo)，實際應(yīng)用環(huán)境中可能會偏優(yōu)，用戶可根據(jù)實際情況對參數(shù)進(jìn)行優(yōu)化。

圖3 測試路線與設(shè)備安裝Fig.3 Test route and equipment installation

表1 MTIG710和ADIS16505設(shè)備參數(shù)*Tab.1 The equipment parameters of MTIG710 and ADIS16505

實現(xiàn)協(xié)同精密定位的前提是精確確定車輛之間的相對空間關(guān)系，這也是協(xié)同定位的核心和難點，特別是當(dāng)車輛處于復(fù)雜的高遮擋環(huán)境中時，如何結(jié)合車輛之間的多傳感器實現(xiàn)高精度的空間相對位置確定依然是未來研究關(guān)注點。本文旨在驗證協(xié)同定位思想的效果，本文試驗中VA和VB的相對位置關(guān)系將通過動對動RTK事先計算得到，兩輛車之間的空間位置變化如圖4所示。為了對比協(xié)同技術(shù)在不同環(huán)境下對定位的改善效果，通過控制雙差觀測值個數(shù)來模擬不同條件的觀測環(huán)境。

圖4 可見衛(wèi)星數(shù)及VA和VB間基線變化情況Fig.4 The number of visible satellites and the baseline between VA and VB

(1)T1(1～500 s)：不改變VA和VB的雙差觀測值個數(shù)。

(2)T2(501～1000 s)：保留VB高度角最高的3對雙差觀測值，不改變VA的雙差觀測值個數(shù)。

(3)T3(1001～1500 s)：保留VA和VB高度角最高的3對雙差觀測值。

(4)T4(1501～2000 s)：不改變VA的雙差觀測值個數(shù)，為了測試在單載體衛(wèi)星信號完全失鎖情況下的協(xié)同定位性能，在此時段末端20 s內(nèi)丟棄VB所有的雙差觀測值，其余時段不改變VB的雙差觀測值個數(shù)。

另外，將VA和VB在正常觀測環(huán)境下GNSS/INS雙向平滑的緊組合結(jié)果作為各自軌跡的參考值。

2.2 結(jié)果與分析

圖5給出了VA和VB在獨立模式和協(xié)同模式下的位置誤差序列，圖6展示了三維位置誤差累積分布函數(shù)(cumulative distribution function，CDF)。表2和表3給出了相應(yīng)的精度統(tǒng)計結(jié)果。圖7和8展示了VA和VB在獨立模式和協(xié)同模式下的速度與姿態(tài)誤差序列。

圖5 位置誤差時間序列Fig.5 The position error series

圖6 三維位置誤差累積分布函數(shù)Fig.6 CDF of 3D position error series

圖7 速度誤差時間序列Fig.7 The velocity error series

表2 不同時段內(nèi)VA定位誤差統(tǒng)計信息Tab.2 The statistics of each period 3D positioning error of VA m

表3 不同時段內(nèi)VB定位誤差統(tǒng)計信息Tab.3 The statistics of each period 3D positioning error of VB m

由整體試驗結(jié)果可知，協(xié)同定位的精度顯著優(yōu)于載體獨立定位效果，VA和VB獨立定位的RMS分別為0.169和0.864 m，協(xié)同定位的RMS分別為0.045和0.071 m，精度提升了73.37%和91.78%；VA和VB獨立定位68%對應(yīng)的CDF分別為0.059和0.859 m，協(xié)同模式下該指標(biāo)分別為0.031和0.034 m，提升了47.46%和96.04%；95%對應(yīng)的CDF分別從0.416、2.792 m提升至0.112、0.160 m，分別得到了73.87%和94.27%的改善。

從不同環(huán)境下的結(jié)果看，當(dāng)載體在較為開闊的環(huán)境中運動時(T1和T4前段)，無論是單獨導(dǎo)航解算還是協(xié)同定位，VA與VB均取得了較高的定位精度，三維定位精度均達(dá)到厘米級。因為在開闊環(huán)境下，即使基于載體本身傳感器的定位，也能觀測到足夠數(shù)量的GNSS衛(wèi)星，能夠有效抑制INS誤差的累積。但當(dāng)載體在可用GNSS觀測值較少的環(huán)境中運動時(T2、T3和T4最后20 s)，獨立模式下的定位誤差會隨著時間增加而快速累積，其累積速度取決于IMU設(shè)備的精度與在惡劣環(huán)境中的運動時長。而采用了協(xié)同定位模式后，兩輛車在此環(huán)境下的定位精度均得到了顯著提高。對于VA而言，當(dāng)采用單獨導(dǎo)航模式時，在T3時段內(nèi)三維定位的RMS、CDF(68%)和CDF(95%)對應(yīng)的誤差分別為0.329、0.339和0.534 m，而當(dāng)采用協(xié)同定位模式時，3個統(tǒng)計量降至0.073、0.067和0.142 m，提升了77.81%、80.24%和73.41%。以VB定位效果改善最為明顯的T3時段為例，當(dāng)采用單獨導(dǎo)航模式時，三維定位誤差的RMS、CDF(68%)和CDF(95%)對應(yīng)的誤差分別為1.435、1.838和2.117 m，當(dāng)采用協(xié)同定位模式時，3個統(tǒng)計量降至0.130、0.059和0.367 m，提升百分比分別為90.94%、96.79%和82.66%。

本文結(jié)合圖7和圖8簡要分析協(xié)同與獨立模式在測速與定姿精度上的差異。當(dāng)可用GNSS觀測值較多時，無論是VA還是VB，協(xié)同與獨立模式最終都取得了相當(dāng)?shù)臏y速與定姿精度。此外，觀察VA的T1時段與VB的T4時段，可以發(fā)現(xiàn)協(xié)同相比于獨立模式明顯縮短了速度與姿態(tài)的收斂時間；當(dāng)載體在可用GNSS觀測值較少的環(huán)境中運動并且采用獨立模式時，無論是VA還是VB，其速度與姿態(tài)精度都會有不同程度的發(fā)散，發(fā)散的速度取決于IMU器件的精度。而當(dāng)采用協(xié)同定位模式時，VA與VB即使在惡劣環(huán)境中也能保持非常高的測速與定姿精度。這說明協(xié)同模式不僅可以改善車輛的定位精度，也可以有效改善車輛的測速與定姿精度，并且加快待估參數(shù)的收斂速度。

圖8 姿態(tài)誤差時間序列Fig.8 The velocity error series

以上結(jié)果表明在復(fù)雜環(huán)境下，獨立載體即使采用GNSS/INS組合方式也會導(dǎo)致定位、測速與定姿結(jié)果發(fā)散，其根本原因是觀測衛(wèi)星數(shù)有限導(dǎo)致長時間無法提供外部絕對輔助信息從而無法抑制INS誤差。而協(xié)同定位技術(shù)通過共享車輛之間的觀測值，在車輛之間建立相對位置約束關(guān)系，有效增加載體可用觀測量以抑制INS誤差的發(fā)散。此外，一旦車輛間的相對位置實現(xiàn)高精度協(xié)同，多載體就會被固連為一個剛體，此時相當(dāng)于一個載體上搭載了多臺多元傳感器設(shè)備，能有效提高定位的可信度。需要說明的是，由于試驗硬件有限，盡管本文僅采用了兩輛車驗證協(xié)同定位效果，但多車協(xié)同的實現(xiàn)過程是類似的，因此不妨礙對本文算法可行性的驗證。當(dāng)然，參與協(xié)同的車輛越多，單個車輛能夠利用的觀測值便越多，越有利于提高協(xié)同定位的效果。

3 結(jié) 論

本文基于目前最常用的GNSS/INS組合導(dǎo)航技術(shù)，推導(dǎo)了GNSS/INS多車協(xié)同緊組合定位模型和解算方法，通過建立車輛之間的相對位置關(guān)系，實現(xiàn)車輛之間的信息共享，從而提升多車定位精度。通過設(shè)計雙車協(xié)同定位試驗，模擬不同復(fù)雜觀測環(huán)境，對比分析了協(xié)同定位模式相比于單獨導(dǎo)航模式在定位、測速與定姿精度方面的差異。結(jié)果表明，協(xié)同定位模式能顯著改善載體在復(fù)雜環(huán)境下的定位精度，兩輛車的獨立定位RMS分別被提升了73.37%和91.78%，同時也能夠改善測速與定姿的精度并加快參數(shù)的收斂速度。本文可為后續(xù)深化協(xié)同定位技術(shù)在智慧交通和無人駕駛領(lǐng)域中的應(yīng)用拋磚引玉。