胡 杰 王子卉 朱倚嫻

1 中國(guó)電子科技集團(tuán)公司第二十八研究所空中交通管理系統(tǒng)與技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，南京市永智路1號(hào)，210014 2 東南大學(xué)微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京市四牌樓2號(hào)，210096 3 南通大學(xué)機(jī)械工程學(xué)院，江蘇省南通市嗇園路9號(hào)，226019

建立飛行區(qū)車(chē)輛管控系統(tǒng)有利于實(shí)現(xiàn)車(chē)輛的可視化、數(shù)字化、精細(xì)化管理[1]，車(chē)載定位系統(tǒng)能夠?qū)崟r(shí)準(zhǔn)確地提供車(chē)輛行駛態(tài)勢(shì)，是實(shí)現(xiàn)車(chē)輛監(jiān)控管理的前提。在車(chē)載定位系統(tǒng)中組合使用捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)與全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)，可以獲得車(chē)輛行駛速度、位置以及姿態(tài)等信息[2-3]。我國(guó)北斗導(dǎo)航系統(tǒng)已完成全球組網(wǎng)，正在推進(jìn)其在民航領(lǐng)域的應(yīng)用，差分北斗與SINS融合可為飛行區(qū)車(chē)輛管控系統(tǒng)提供高精度的位置服務(wù)。

當(dāng)衛(wèi)星信號(hào)有效時(shí)，SINS與GNSS融合能夠獲得車(chē)輛的行駛速度、位置等參數(shù)；當(dāng)衛(wèi)星信號(hào)受到遮擋時(shí)，組合導(dǎo)航系統(tǒng)精度會(huì)降低。劉鵬飛[4]提出一種里程計(jì)(DR)輔助的高精度GNSS/INS組合導(dǎo)航方法，并建立了GNSS/INS和DR/INS兩種組合導(dǎo)航濾波模型，可根據(jù)車(chē)輛行駛環(huán)境切換濾波模式，具有長(zhǎng)時(shí)間、高精度導(dǎo)航定位功能，但需要安裝輔助導(dǎo)航設(shè)備，因此使用范圍受限。張夢(mèng)得等[5]針對(duì)GPS短時(shí)失效的問(wèn)題提出一種基于偏最小二乘輔助高斯過(guò)程回歸的SINS/GPS組合導(dǎo)航方法，但該方法的適用性還需進(jìn)一步驗(yàn)證。Yao等[6]和張倫東等[7]提出一種基于零速修正技術(shù)(ZUPT)的組合導(dǎo)航方法，利用車(chē)輛停止或行人駐足時(shí)速度為0作為約束條件來(lái)估計(jì)SINS誤差。胡杰等[8]提出利用車(chē)輛行駛中側(cè)向和天向速度為0構(gòu)造濾波器量測(cè)約束信息，稱為動(dòng)態(tài)零速修正技術(shù)(DZUPT)，但當(dāng)車(chē)輛急速轉(zhuǎn)彎時(shí)，側(cè)向速度為0的假設(shè)條件不成立。周俊等[9]對(duì)GNSS/SINS組合導(dǎo)航算法進(jìn)行研究，但未考慮衛(wèi)星失效時(shí)引起系統(tǒng)精度降低的問(wèn)題。近年來(lái)，部分學(xué)者[10-11]提出利用機(jī)器學(xué)習(xí)、深度學(xué)習(xí)等技術(shù)輔助SINS導(dǎo)航，但這些算法實(shí)現(xiàn)較為復(fù)雜，在使用中存在局限性。

本文針對(duì)飛行區(qū)車(chē)輛衛(wèi)星信號(hào)斷續(xù)或遮擋時(shí)導(dǎo)航系統(tǒng)精度降低的問(wèn)題，利用ZUPT與DZUPT結(jié)合的運(yùn)動(dòng)學(xué)約束技術(shù)，提出一種基于車(chē)輛運(yùn)動(dòng)檢測(cè)的差分北斗/SINS組合導(dǎo)航方法。針對(duì)車(chē)輛行駛特征給出組合導(dǎo)航系統(tǒng)框架，建立組合導(dǎo)航濾波狀態(tài)方程與量測(cè)方程，根據(jù)SINS輸出信息實(shí)現(xiàn)車(chē)輛運(yùn)動(dòng)狀態(tài)的檢測(cè)，并對(duì)該方法進(jìn)行實(shí)驗(yàn)驗(yàn)證。

1 基于運(yùn)動(dòng)檢測(cè)的差分北斗/SINS組合導(dǎo)航系統(tǒng)框架

圖1為本文提出的飛行區(qū)車(chē)輛差分北斗/SINS組合導(dǎo)航系統(tǒng)框架。當(dāng)北斗衛(wèi)星信號(hào)有效時(shí)，組合導(dǎo)航濾波器利用差分北斗提供的速度、位置信息作為觀測(cè)量進(jìn)行濾波器量測(cè)更新，并根據(jù)估計(jì)得到的誤差值反饋校正SINS。當(dāng)北斗衛(wèi)星信號(hào)受到遮擋時(shí)，要利用SINS提供的導(dǎo)航信息，首先需通過(guò)車(chē)輛運(yùn)動(dòng)狀態(tài)檢測(cè)模塊來(lái)判斷當(dāng)前的車(chē)輛行駛狀態(tài)(停車(chē)狀態(tài)、直線行駛或拐彎行駛)；然后根據(jù)車(chē)輛運(yùn)動(dòng)學(xué)特性構(gòu)造相應(yīng)的運(yùn)動(dòng)約束信息(ZUPT模式、DZUPT模式或僅時(shí)間更新的DZUPT模式)；最后將構(gòu)造得到的運(yùn)動(dòng)學(xué)約束輔助信息與SINS相組合，實(shí)現(xiàn)系統(tǒng)的誤差估計(jì)與校正。

圖1 組合導(dǎo)航系統(tǒng)框架Fig.1 Frame of integrated navigation system

2 組合導(dǎo)航濾波器設(shè)計(jì)

本文導(dǎo)航坐標(biāo)系選取東-北-天地理坐標(biāo)系，記為n系，慣性坐標(biāo)系記為i系，載體坐標(biāo)系記為b系，其中oxb軸沿車(chē)體橫軸指向右方，oyb軸沿車(chē)體縱軸指向前方，ozb軸垂直于車(chē)體向上，即構(gòu)成一個(gè)右-前-上坐標(biāo)系。

2.1 系統(tǒng)狀態(tài)方程

X=[δLδλδhδvEδvNδvU

φEφNφUεxεyεzxyz]T

(1)

進(jìn)一步，由SINS誤差方程可得到組合導(dǎo)航濾波狀態(tài)方程為：

(2)

式中，A為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，可根據(jù)SINS誤差確定；W為系統(tǒng)噪聲向量。

2.2 系統(tǒng)量測(cè)方程

2.2.1 北斗衛(wèi)星信號(hào)有效時(shí)

當(dāng)衛(wèi)星信號(hào)有效時(shí)，將SINS輸出速度、位置與北斗接收機(jī)輸出速度、位置的差值作為Kalman濾波器測(cè)量值，此時(shí)濾波量測(cè)方程可表示為：

Z1=H1X+V1

(3)

式中，Z1為衛(wèi)星信號(hào)有效時(shí)的量測(cè)向量；V1為系統(tǒng)量測(cè)噪聲向量；H1為量測(cè)矩陣，可表示為：

(4)

式中，I3×3表示3×3維單位矩陣；03×3、03×9分別表示3×3和3×9維零矩陣。

2.2.2 北斗衛(wèi)星信號(hào)失效時(shí)

當(dāng)衛(wèi)星信號(hào)失效且檢測(cè)到車(chē)輛處于停止?fàn)顟B(tài)，即組合導(dǎo)航系統(tǒng)處于ZUPT模式時(shí)，利用車(chē)輛n系3個(gè)方向速度為0的特性構(gòu)造濾波器量測(cè)值：

(5)

式中，Z2為衛(wèi)星信號(hào)失效且車(chē)輛停止時(shí)的量測(cè)向量；vE、vN和vU分別為n系3個(gè)方向的速度誤差；vE、vN和vU分別為n系3個(gè)方向的速度值。

由式(5)可得到衛(wèi)星信號(hào)失效且車(chē)輛處于停止?fàn)顟B(tài)時(shí)的量測(cè)方程為：

Z2=H2X+V2

(6)

式中，V2為系統(tǒng)量測(cè)噪聲向量；H2為量測(cè)矩陣，可表示為：

(7)

當(dāng)衛(wèi)星信號(hào)失效且車(chē)輛處于直線行駛，即組合導(dǎo)航系統(tǒng)處于DZUPT模式時(shí)，利用車(chē)輛運(yùn)動(dòng)學(xué)特性約束SINS誤差。假設(shè)車(chē)輛無(wú)側(cè)滑、跳躍等運(yùn)動(dòng)，則b系中側(cè)向和天向速度可近似為0，如圖2所示，該方法被稱為非完整性約束[12]。

圖2 非完整性約束Fig.2 Non-holonomic constraint

(8)

式中，vb、vn分別為b系和n系車(chē)輛速度。由式(8)可得b系速度誤差表達(dá)式為：

(9)

式(9)可進(jìn)一步表示為：

δvb=Hv,1δvn+Hv,2φn

(10)

其中，

(11)

(12)

利用車(chē)輛非完整性約束條件可構(gòu)造濾波器量測(cè)值為：

(13)

由式(10)和式(13)可得量測(cè)方程為：

Z3=H3X+V3

(14)

式中，V3為系統(tǒng)量測(cè)噪聲向量；H3為系統(tǒng)量測(cè)矩陣，可表示為：

H3=

(15)

式中，Hv,1(1,)、Hv,2(1,)表示矩陣第1行；Hv,1(3,)、Hv,2(3,)表示矩陣第3行；01×3表示1×3維零矩陣。

3 運(yùn)動(dòng)狀態(tài)檢測(cè)

根據(jù)分析可知，車(chē)輛狀態(tài)可分為直線行駛、拐彎行駛和停止3種。當(dāng)車(chē)輛停止時(shí)，可采用傳統(tǒng)ZUPT進(jìn)行誤差約束；當(dāng)車(chē)輛直線行駛時(shí)，利用車(chē)體側(cè)向和天向速度為0進(jìn)行非完整性約束；當(dāng)車(chē)輛急速拐彎時(shí)，會(huì)產(chǎn)生較大的向心加速度，車(chē)體側(cè)向和天向存在速度分量，不滿足DZUPT約束條件，但一般情況下車(chē)輛拐彎時(shí)間較短，此時(shí)Kalman濾波算法需暫停量測(cè)更新，即組合導(dǎo)航系統(tǒng)處于僅時(shí)間更新的DZUPT模式。

假設(shè)k時(shí)刻IMU輸出的車(chē)輛角速度和比力分別為ωk=[ωxkωykωzk]T、fk=[fxkfykfzk]T，車(chē)輛運(yùn)動(dòng)狀態(tài)檢測(cè)的目的是根據(jù)IMU輸出的信息確定當(dāng)前車(chē)輛行駛狀態(tài)，具體實(shí)現(xiàn)過(guò)程如下：

1)確定停止?fàn)顟B(tài)閾值。在車(chē)輛初始對(duì)準(zhǔn)階段，采集3～5 min的IMU原始靜態(tài)數(shù)據(jù)計(jì)算角速度和加速度檢測(cè)閾值，表達(dá)式為：

(16)

式中，λG和λA分別為車(chē)輛停止時(shí)陀螺和加速度計(jì)的檢測(cè)閾值；N為數(shù)據(jù)采集時(shí)間長(zhǎng)度；ωi和fi(i=1, 2,…,N)分別為時(shí)間窗內(nèi)i時(shí)刻IMU輸出的陀螺和加速度計(jì)值；‖·‖表示2范數(shù)。

2)判別車(chē)輛是否停止?；瑒?dòng)窗口時(shí)間取值為T(mén)，根據(jù)IMU采樣頻率可求得T時(shí)間段內(nèi)采樣值，假設(shè)為M。實(shí)時(shí)計(jì)算滑動(dòng)窗口內(nèi)角速度和比力平均值，并與式(16)進(jìn)行比較，以此來(lái)判斷車(chē)輛是否停止：

(17)

(18)

式中，LG(·)和LA(·)分別為根據(jù)陀螺和加速度計(jì)輸出求得的車(chē)輛停止檢驗(yàn)值；g為當(dāng)?shù)刂亓铀俣?。判斷?chē)輛是否停止可認(rèn)為是假設(shè)檢驗(yàn)問(wèn)題，當(dāng)式(19)成立時(shí)，車(chē)輛即可被認(rèn)為處于停止?fàn)顟B(tài)：

(19)

3)判別車(chē)輛是否拐彎。由牛頓運(yùn)動(dòng)定律可確定車(chē)輛向心加速度為：

a(t)=V(t)×ωz(t)

(20)

式中，a(t)為t時(shí)刻車(chē)輛向心加速度；V(t)為車(chē)輛行駛速度；ωz(t)為陀螺輸出沿垂向拐彎角速度。

如果|a(t)|≥λa，說(shuō)明車(chē)體急速拐彎，此時(shí)將誤差約束切換至僅時(shí)間更新的DZUPT模式，其中檢測(cè)閾值λa由車(chē)輛實(shí)際行駛路況及車(chē)輛拐彎幅度確定。

4 車(chē)載實(shí)驗(yàn)

車(chē)載驗(yàn)證實(shí)驗(yàn)裝置由差分北斗/MEMS SINS組合導(dǎo)航系統(tǒng)、北斗差分基準(zhǔn)站、北斗接收天線等組成，SINS中采用的MEMS陀螺儀零偏穩(wěn)定性(1 s平滑，1σ，室溫)優(yōu)于5°/h，加速度計(jì)零偏穩(wěn)定性(1 s平滑，1σ，室溫)優(yōu)于100 μg。采用高精度Septentrio PolaRx5型GNSS接收機(jī)保存的星歷和觀測(cè)數(shù)據(jù)進(jìn)行事后RTK差分處理，獲得車(chē)輛參考行駛路線，數(shù)據(jù)更新頻率為1 Hz，位置精度優(yōu)于0.05 m。

實(shí)驗(yàn)地點(diǎn)為某機(jī)場(chǎng)飛行區(qū)，利用專用移動(dòng)通信網(wǎng)絡(luò)實(shí)現(xiàn)北斗差分站與行駛車(chē)輛以及行駛車(chē)輛與監(jiān)控席位之間的數(shù)字通信。圖3為車(chē)輛行駛軌跡，圖4為該時(shí)間段內(nèi)北斗可見(jiàn)衛(wèi)星個(gè)數(shù)及其幾何精度因子曲線(GDOP)，圖5為差分北斗/SINS組合導(dǎo)航系統(tǒng)位置誤差曲線，表1為位置誤差統(tǒng)計(jì)結(jié)果。

圖3 車(chē)輛行駛軌跡Fig.3 Vehicle track

圖4 可見(jiàn)衛(wèi)星個(gè)數(shù)及其GDOPFig.4 Numbers of visible satellites and their GDOP

圖5 差分北斗/SINS組合導(dǎo)航位置誤差Fig.5 Position errors of differential Beidou/SINS integrated navigation

表1 組合導(dǎo)航位置誤差數(shù)值統(tǒng)計(jì)

由圖4可以看出，該時(shí)間段內(nèi)可見(jiàn)衛(wèi)星個(gè)數(shù)最少為14顆，最多為16顆，滿足差分定位需求；GDOP最大值為2.35，衛(wèi)星幾何分布優(yōu)。由圖5及表1可以看出，組合導(dǎo)航系統(tǒng)的最大位置誤差小于1 m，表明差分北斗與SINS組合后能夠?yàn)轱w行區(qū)車(chē)輛管控系統(tǒng)提供亞米級(jí)精度位置服務(wù)。

為進(jìn)一步模擬車(chē)輛在機(jī)場(chǎng)停機(jī)坪、廊橋等遮擋環(huán)境下的組合導(dǎo)航系統(tǒng)定位精度，利用保存的SINS和差分北斗數(shù)據(jù)，通過(guò)人為切斷北斗接收機(jī)的衛(wèi)星信號(hào)對(duì)本文提出的基于運(yùn)動(dòng)檢測(cè)的DZUPT約束組合導(dǎo)航方法進(jìn)行驗(yàn)證。行駛里程約為2.5 km，時(shí)刻為605～919 s，對(duì)應(yīng)圖3中A-B行駛路段。該路段車(chē)輛行駛狀態(tài)包括直線行駛、停車(chē)以及拐彎行駛等，傳統(tǒng)DZUPT約束與本文提出的基于運(yùn)動(dòng)檢測(cè)的DZUPT約束的位置誤差對(duì)比曲線如圖6所示，表2為2種約束方法的最大位置誤差對(duì)比統(tǒng)計(jì)。

圖6 衛(wèi)星信號(hào)失效時(shí)組合導(dǎo)航位置誤差對(duì)比Fig.6 Comparison of position errors of integrated navigation under the satellite signal outage

表2 衛(wèi)星信號(hào)失效時(shí)組合導(dǎo)航的最大位置誤差對(duì)比

由圖6及表2可以看出，當(dāng)中斷衛(wèi)星信號(hào)并采用傳統(tǒng)DZUPT約束輔助時(shí)，組合導(dǎo)航系統(tǒng)的經(jīng)度誤差最大為5.21 m，引起誤差的主要原因?yàn)楣諒潟r(shí)車(chē)輛側(cè)向速度不為0，傳統(tǒng)DZUPT的約束條件不成立；當(dāng)采用本文提出的基于運(yùn)動(dòng)檢測(cè)的DZUPT約束輔助時(shí)，通過(guò)運(yùn)動(dòng)狀態(tài)檢測(cè)算法能夠?qū)崟r(shí)檢測(cè)車(chē)輛的運(yùn)動(dòng)狀態(tài)，當(dāng)車(chē)輛拐彎時(shí)則切換濾波器至僅時(shí)間更新的DZUPT模式，可隔離車(chē)輛拐彎所引起的量測(cè)誤差，經(jīng)度最大誤差減小為1.35 m，從而提高了組合導(dǎo)航系統(tǒng)精度。

5 結(jié) 語(yǔ)

為進(jìn)一步提高機(jī)場(chǎng)飛行區(qū)車(chē)輛組合導(dǎo)航精度，本文對(duì)北斗導(dǎo)航系統(tǒng)衛(wèi)星信號(hào)中斷時(shí)SINS位置誤差隨時(shí)間積累問(wèn)題進(jìn)行分析和研究。提出一種基于運(yùn)動(dòng)檢測(cè)的DZUPT輔助約束SINS組合導(dǎo)航方法，當(dāng)衛(wèi)星信號(hào)中斷時(shí)，利用慣性測(cè)量單元輸出的陀螺和加速度計(jì)信息以及SINS輸出的車(chē)輛速度來(lái)實(shí)現(xiàn)運(yùn)動(dòng)狀態(tài)在線檢測(cè)，從而切換組合導(dǎo)航系統(tǒng)至對(duì)應(yīng)的輔助約束模式。

車(chē)載組合導(dǎo)航驗(yàn)證實(shí)驗(yàn)結(jié)果表明，當(dāng)北斗導(dǎo)航系統(tǒng)衛(wèi)星信號(hào)有效時(shí)，差分北斗與SINS組合定位精度優(yōu)于1 m；當(dāng)衛(wèi)星信號(hào)中斷時(shí)，本文提出的基于運(yùn)動(dòng)檢測(cè)的DZUPT輔助約束方法相比于傳統(tǒng)DZUPT約束方法定位精度更高。