王 劍，凌 浩，姜 維，蔡伯根

(1.北京交通大學(xué) 電子信息工程學(xué)院,北京 100044；2.北京市軌道交通電磁兼容與衛(wèi)星導(dǎo)航工程技術(shù)研究中心，北京 100044)

隨著我國鐵路運(yùn)輸?shù)陌l(fā)展，基于全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)和慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)構(gòu)成的組合導(dǎo)航系統(tǒng)已經(jīng)成為列車定位的方式之一。然而目前組合導(dǎo)航系統(tǒng)主要應(yīng)用于列車的測速定位，如何繼續(xù)改善和提高其性能，并且應(yīng)用于更多鐵路方向成為組合導(dǎo)航系統(tǒng)的主要研究方向。

GNSS/INS組合導(dǎo)航系統(tǒng)多是利用GNSS輸出的位置對INS的誤差進(jìn)行校正，但I(xiàn)NS位置、速度的解算都需要利用姿態(tài)，并且姿態(tài)的變化可以反映列車振動(dòng)的大小和線路的變化。因此針對姿態(tài)的長期高精度測量，已經(jīng)成為國內(nèi)外學(xué)者研究的重點(diǎn)。王冰等[1]對基于基線和模糊度參數(shù)的單基線，以及基于姿態(tài)和模糊度參數(shù)的多基線兩種模型進(jìn)行了實(shí)驗(yàn)分析。姜維等[2-3]提出基于仰角約束的最優(yōu)分布參考衛(wèi)星選擇方法，以提高雙差整周模糊度的固定效率，從而實(shí)現(xiàn)雙天線相對位置的解算。Zhang等[4]提出通過安裝多個(gè)天線，使用自適應(yīng)Kalman濾波對歐拉角和角速度進(jìn)行估計(jì)，并通過靜態(tài)實(shí)驗(yàn)和仿真實(shí)驗(yàn)進(jìn)行了驗(yàn)證。梅玲玉等[5]則提出INS/GPS/磁力計(jì)三組合，利用磁力計(jì)實(shí)現(xiàn)航向角測量，利用慣性測量單元中加速度計(jì)測得的橫向、垂向加速度實(shí)現(xiàn)橫滾角和俯仰角的測量，進(jìn)而實(shí)現(xiàn)位置、速度、姿態(tài)的全狀態(tài)組合。綜上所述，目前針對除慣性導(dǎo)航外的姿態(tài)測量方法主要集中在多天線測姿、加速度計(jì)和磁力計(jì)上，對GNSS/INS全狀態(tài)組合在列車上的應(yīng)用研究相對較少。而我國鐵路沿線環(huán)境復(fù)雜，列車運(yùn)行時(shí)間較長，單GPS難以滿足使用要求。隨著北斗導(dǎo)航系統(tǒng)(BeiDou Navigation Satellite System, BDS)中北斗三代的成功布置，BDS的研究與應(yīng)用也越來越多。為此本文提出采用BDS和GPS多星座相結(jié)合的方式實(shí)現(xiàn)雙天線測姿，將GPS/BDS雙天線測姿系統(tǒng)的長期穩(wěn)定性和慣性導(dǎo)航短期高精度的姿態(tài)測量相結(jié)合，從而實(shí)現(xiàn)長時(shí)間、高精度的姿態(tài)輸出，進(jìn)而提高定位測速效果。

1 雙天線測姿

1.1 測姿原理

載體的三維姿態(tài)是指載體相對當(dāng)?shù)貙?dǎo)航坐標(biāo)系的3個(gè)角。利用衛(wèi)星導(dǎo)航進(jìn)行載體姿態(tài)測量，是獲取多個(gè)天線相對主天線在當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的坐標(biāo)和在載體坐標(biāo)系下的坐標(biāo)，然后利用這兩坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系進(jìn)行姿態(tài)的求解。

坐標(biāo)旋轉(zhuǎn)轉(zhuǎn)換示意見圖1，當(dāng)?shù)貙?dǎo)航坐標(biāo)系(Xn,Yn,Zn)依次繞著Z、X、Y軸進(jìn)行旋轉(zhuǎn)可以得到載體坐標(biāo)系(Xb,Yb,Zb)[6]，旋轉(zhuǎn)的3個(gè)角度依次為航向角ψ、俯仰角θ和橫滾角φ。

圖1 坐標(biāo)旋轉(zhuǎn)轉(zhuǎn)換示意

載體坐標(biāo)系與當(dāng)?shù)貙?dǎo)航坐標(biāo)系的原點(diǎn)相同，兩坐標(biāo)系之間存在轉(zhuǎn)換關(guān)系為

(1)

(2)

當(dāng)放置一根基線時(shí)，可以根據(jù)坐標(biāo)旋轉(zhuǎn)關(guān)系計(jì)算出2個(gè)姿態(tài)角，基線放置方向?yàn)闄M向，垂直于列車運(yùn)行方向，此時(shí)可以計(jì)算出航向角ψ和橫滾角φ分別為

(3)

(4)

式中：Len為基線長度。

1.2 雙差載波相位

在使用GPS/BDS測姿時(shí)，一根輔助天線和一根主天線構(gòu)成一條基線，通過計(jì)算基線在載體坐標(biāo)系和當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的坐標(biāo)，再利用坐標(biāo)轉(zhuǎn)換關(guān)系即可求解姿態(tài)角。其中，載體坐標(biāo)系下的坐標(biāo)可以在安裝時(shí)通過測量獲得，當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的位置坐標(biāo)則需要利用雙差載波相位進(jìn)行計(jì)算。雙差載波相位原理見圖2。與偽距相比，載波相位具有更高的精度，而站間差分和星間差分可以消除大部分的誤差，故其測量值主要為衛(wèi)星鐘差、電離層和對流層的傳播誤差，以及接收機(jī)鐘差。

圖2 雙差載波相位原理

圖2中，點(diǎn)o為主天線M的位置，認(rèn)為是載體坐標(biāo)系和當(dāng)?shù)貙?dǎo)航坐標(biāo)系的原點(diǎn)；點(diǎn)O(X，Y，Z)為輔助天線S的位置。根據(jù)載波相位的傳播路徑，主輔兩天線的載波相位觀測方程為

(5)

(6)

式中：φ、λ、f分別為載波相位的測量值、波長、頻率；ρM、ρS分別為衛(wèi)星與主天線、輔助天線之間距離的真實(shí)值；I為電離層誤差；T為對流層誤差；tR、ts分別為接收機(jī)、衛(wèi)星的鐘差；N為整周模糊度；ε為其他誤差；i為接受衛(wèi)星的編號。

(7)

(8)

(9)

式中：上標(biāo)i為關(guān)于衛(wèi)星i的站間差分；上標(biāo)j為關(guān)于衛(wèi)星j的站間差分；上標(biāo)i_j為衛(wèi)星i與衛(wèi)星j之間差；下標(biāo)M_S為主天線與輔助天線之間差。

(10)

(11)

將式(11)在原點(diǎn)處展開為泰勒級數(shù)

(12)

式中：(Xi,Yi,Zi)、(X,Y,Z)分別為衛(wèi)星i、輔助天線在當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的位置；ρM為衛(wèi)星到主天線的距離；(Δx,Δy,Δz)為泰勒展開的偏差。由于衛(wèi)星到主天線的距離遠(yuǎn)大于基線長度，可以認(rèn)為輔助天線的位置即為主天線位置泰勒展開的大約位置[7]，因此(Δx,Δy,Δz)也就是基線坐標(biāo)。

對于衛(wèi)星j同樣進(jìn)行泰勒展開

(13)

式中：(Xj,Yj,Zj)為衛(wèi)星j在當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的位置。

將系數(shù)參數(shù)記為

(14)

則式(10)可以簡化為

(15)

式中：l(i_j)為系數(shù)l(j)與l(j)之差；m(i_j)、d(i_j)含義同l(i_j)。將式(15)代入式(9)，忽略其他誤差可得

(16)

式(16)中只有基線坐標(biāo)和雙差整周模糊度未知，當(dāng)主輔天線收到n顆共同衛(wèi)星時(shí)，雙差時(shí)GPS和BDS需要各選取一顆主星，所以存在n-2個(gè)雙差模糊度，其基線坐標(biāo)和模糊度可以通過Kalman濾波計(jì)算。

1.3 多星座融合

相較于單星座系統(tǒng)，多星座系統(tǒng)可以有效增加可視衛(wèi)星數(shù)和改善定位精度因子，提高系統(tǒng)的可用性、穩(wěn)定性和精度。而要想實(shí)現(xiàn)不同導(dǎo)航系統(tǒng)的聯(lián)合解算，需要將其統(tǒng)一到相同的時(shí)空參考框架下，其中空間參考框架的影響較小，本文不進(jìn)行考慮[8]。對于GPS時(shí)間系統(tǒng)(GPS Time System, GPST)的時(shí)間TGPS與BDS時(shí)間系統(tǒng)(BDS Time System, BDST)的時(shí)間TBDS，由于閏秒的原因[9]，兩者之間存在一個(gè)時(shí)間差，其轉(zhuǎn)換關(guān)系為

TBDS=TGPS-14

(17)

BDS相較于GPS增加了GEO、IGSO軌道的衛(wèi)星，IGSO衛(wèi)星的位置計(jì)算與MEO衛(wèi)星相同，而GEO衛(wèi)星位置的計(jì)算需要進(jìn)行轉(zhuǎn)換。衛(wèi)星位置計(jì)算流程見圖3，具體算法見文獻(xiàn)[9-10]。

圖3 衛(wèi)星位置計(jì)算流程

1.4 基線坐標(biāo)解算

基線的計(jì)算需要利用Kalman濾波進(jìn)行，其系統(tǒng)模型為

Xk=FXk-1+Qk

(18)

(19)

(20)

(21)

系統(tǒng)的量測模型為

(22)

(23)

Hk=[l(n-2)×1m(n-2)×1d(n-2)×10(n-2)×3

-λ·I(n-2)×(n-2)]

(24)

為GPS與BDS的雙差載波相位，共n-2個(gè)。

1.5 模糊度固定

雙差載波相位模糊度具有整數(shù)特性，而利用Kalman濾波計(jì)算的模糊度為浮點(diǎn)解，通過固定整周模糊度可以實(shí)現(xiàn)厘米級甚至毫米級的精度，并且由于天線基線長度不變，可以利用基線長度約束提高固定效率和成功率。模糊度固定流程見圖4。

圖4 模糊度固定流程

模糊度固定首先是人為地以Kalman濾波求解的浮點(diǎn)解和誤差協(xié)方差矩陣為中心構(gòu)建一個(gè)置信搜索空間，將所有可能的模糊度整數(shù)組合包含在內(nèi)。但模糊度之間存在相關(guān)性，搜索空間的最長軸與最短軸相差較大，呈橢圓體，模糊度方差矩陣為非對角陣，因此需要進(jìn)行去相關(guān)處理，將其方差矩陣變?yōu)閷顷?。處理后搜索空間轉(zhuǎn)為接近球體，這樣使得搜索空間減小，提高搜索效率。隨后在變換后的搜索空間中進(jìn)行搜索，并依據(jù)一定的約束條件進(jìn)行校驗(yàn)和回代。

首先定義包含所有可能的整周模糊度組合的整數(shù)搜索空間為

(25)

(26)

此時(shí)變換后整周模糊度的搜索空間可以表示為

(27)

變換矩陣Z的求解需要對協(xié)方差矩陣進(jìn)行Cholesky分解[11]

(28)

(29)

式中：L1為下三角矩陣；D1為對角陣，其對角線上的元素為di；Q1為變換矩陣，具有對稱性和正定性。

(30)

通過矩陣Z進(jìn)行變換后，置信空間由橢圓體變?yōu)榻魄蝮w，可以將式(27)展開為[12]

(31)

式(31)就是判斷模糊度是否符合要求的不等式，而模糊度搜索是依次對每個(gè)模糊度單獨(dú)搜索，從第n個(gè)開始，建立搜索邊界，對第n-1個(gè)模糊度進(jìn)行搜索，依次直到第一個(gè)模糊度完成搜索。對式(31)進(jìn)行數(shù)學(xué)變換確立搜索邊界為

(32)

(33)

由于天線基線長度不變，當(dāng)確定好存在多個(gè)整周模糊度集合時(shí)，可以將模糊度集合帶入式(16)進(jìn)行驗(yàn)證，并與實(shí)際基線長度進(jìn)行比較，獲取最優(yōu)解。

2 GPS/BDS/INS融合

2.1 慣性導(dǎo)航遞推算法

慣性導(dǎo)航系統(tǒng)包含慣性測量部分和導(dǎo)航處理器推算導(dǎo)航系統(tǒng)。慣性測量部分包含三軸加速度計(jì)和三軸陀螺儀，可以提供運(yùn)動(dòng)物體的運(yùn)行加速度和角速度。推算導(dǎo)航系統(tǒng)包含姿態(tài)、速度、位置更新，可以計(jì)算出運(yùn)動(dòng)物體的姿態(tài)、速度和位置。

(1)在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，常用四元數(shù)來更新姿態(tài)矩陣，其微分方程為

(34)

(35)

(36)

(37)

(2)慣性導(dǎo)航系統(tǒng)速度更新的微分方程為

(38)

式中：fn為載體上加速度計(jì)測量的比力；gn為當(dāng)?shù)刂亓铀俣龋籚為速度。

(3)慣性導(dǎo)航系統(tǒng)的緯度L、經(jīng)度B、高程H的微分方程為

(39)

(40)

(41)

式中：Vu為天向的速度。

2.2 全狀態(tài)松組合模型

通過接收機(jī)可以直接讀取到位置的一維速度，利用雙天線測姿結(jié)果可以將速度進(jìn)行分解，從而實(shí)現(xiàn)位置、速度、姿態(tài)的全狀態(tài)組合。GPS/BDS/INS全狀態(tài)組合結(jié)構(gòu)見圖5。

圖5 GPS/BDS/INS全狀態(tài)組合結(jié)構(gòu)

將GPS/BDS和INS的計(jì)算結(jié)果傳給松組合Kalman濾波，通過對傳感器的系統(tǒng)誤差和噪聲源進(jìn)行建模，實(shí)現(xiàn)對INS的誤差估計(jì)，從而減小甚至消除INS的漂移誤差[13]。

(42)

對應(yīng)的系統(tǒng)轉(zhuǎn)移矩陣則是根據(jù)捷聯(lián)慣性導(dǎo)航的誤差方程進(jìn)行設(shè)置，位置、姿態(tài)、速度誤差方程依次為

(43)

(44)

(45)

(46)

(47)

系統(tǒng)的觀測向量為

(48)

式中：PI、VI、ψI、φI分別為INS輸出的位置、速度、航向角、橫滾角；PG、VG、ψG、φG分別為雙天線測姿系統(tǒng)輸出的位置、速度、航向角、橫滾角。

量測矩陣為

(49)

由于慣性導(dǎo)航元器件的誤差，慣性系統(tǒng)從載體坐標(biāo)系到當(dāng)?shù)貙?dǎo)航坐標(biāo)系之間的轉(zhuǎn)換矩陣在計(jì)算時(shí)會產(chǎn)生一個(gè)平臺失準(zhǔn)角的誤差，狀態(tài)方程中姿態(tài)部分的設(shè)置與平臺失準(zhǔn)角有關(guān)[14]，所以量測矩陣中HA為

(50)

全狀態(tài)組合系統(tǒng)的噪聲矩陣QAk為

(51)

式中：bf為加速度計(jì)零偏；bω為陀螺儀零偏。

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

為了驗(yàn)證本文算法的有效性，選用在2020年12月6日朔黃線的一段10 min左右的數(shù)據(jù)進(jìn)行實(shí)驗(yàn)。其間列車運(yùn)行狀況正常，位于定州東站—定州西站之間，場景開闊，衛(wèi)星信號良好。

在實(shí)驗(yàn)車車頭前部橫向放置一對天線，兩天線相距2.8 m。實(shí)驗(yàn)設(shè)備包括1臺SPAN INS(NovAtel OEM6 GNSS接收機(jī)和IMAR-FSAS IMU)、1臺UR380接收機(jī)和STIM300 INS。其中SPAN是商用高精度GPS/INS組合導(dǎo)航系統(tǒng)，在開闊環(huán)境下可以作為結(jié)果參考；NovAtel OEM6 GNSS接收機(jī)和UR380接收機(jī)分別記錄兩根天線的數(shù)據(jù)；MEMS慣性導(dǎo)航STIM300是一款較低精度的慣性測量單元，采集頻率為125 Hz，用于GPS/BDS/INS全狀態(tài)組合算法驗(yàn)證。

實(shí)驗(yàn)過程中公共可見衛(wèi)星數(shù)見圖6。從圖6可見，大部分時(shí)刻GPS衛(wèi)星存在5顆，但是其數(shù)目變化頻繁，在此基礎(chǔ)上又選取了5顆高度角大的北斗衛(wèi)星，以增強(qiáng)雙天線測姿系統(tǒng)的穩(wěn)定性和可用性。

圖6 可見衛(wèi)星數(shù)

(1)雙天線測姿結(jié)果見圖7。從圖7中可以看出，在衛(wèi)星數(shù)發(fā)生變化時(shí)，單GPS測姿結(jié)果波動(dòng)較大，橫滾角尤為明顯；當(dāng)衛(wèi)星數(shù)過少時(shí)，如在600 s之后衛(wèi)星顆數(shù)快速降低，單GPS測姿結(jié)果離參考值較大，而采用GPS/BDS聯(lián)合解算可以有效降低衛(wèi)星顆數(shù)過少帶來的誤差增大；在衛(wèi)星數(shù)目穩(wěn)定時(shí)，當(dāng)姿態(tài)發(fā)生較大變化，如在300 s附近，雙天線測姿可以有效跟蹤姿態(tài)的變化。

圖7 雙天線測姿結(jié)果

雙天線測姿誤差分析見表1。從表1中可以看出，相較于單GPS，GPS/BDS聯(lián)合測姿的誤差的標(biāo)準(zhǔn)差STD和均方根誤差RMS均有較大改善，BDS系統(tǒng)的加入可以有效提高多天線測姿系統(tǒng)的穩(wěn)定性和精度。

表1 雙天線測姿誤差分析 (°)

(2)組合測姿結(jié)果見圖8。從圖8(a)～圖8(d)可以看出，單位置組合的姿態(tài)結(jié)果會不斷偏離參考值，誤差不斷增大，而全狀態(tài)組合的姿態(tài)結(jié)果相對穩(wěn)定。由于單位置組合的姿態(tài)主要依靠于慣性導(dǎo)航的遞推，而全狀態(tài)組合則利用雙天線測姿的結(jié)果對慣性導(dǎo)航遞推的姿態(tài)誤差進(jìn)行估計(jì)與修正，從而有效抑制了INS的姿態(tài)漂移，航向角、橫滾角的RMS也分別從0.310 0°、0.590 1°降至0.111 8°、0.080 0°。在300 s附近列車姿態(tài)有較為明顯的改變，此時(shí)列車正在經(jīng)過道岔，因此姿態(tài)有較大變化，兩種組合方式都有效跟蹤了姿態(tài)的變化趨勢，其中全狀態(tài)組合的效果更為明顯，但由于列車在經(jīng)過道岔時(shí)振動(dòng)較大，導(dǎo)致其組合解算的結(jié)果出現(xiàn)了一定的突變。

從圖8(e)～圖8(j)可以明顯看出，全狀態(tài)組合的位置、速度相較于單位置組合更加平滑，具有更高的穩(wěn)定性，其中測速精度的改善較為明顯。

圖8 組合測姿結(jié)果

組合測姿的位置誤差分析見表2，速度誤差分析見表3。從表2、表3中可以看出，全狀態(tài)組合的位置誤差的標(biāo)準(zhǔn)差有明顯下降，均方根誤差有一定的改善；速度測量有明顯提升，其中北向和天向提升較為明顯。

表2 位置誤差分析 m

表3 速度誤差分析 m/s

4 結(jié)論

(1)基于GPS/BDS/INS的全狀態(tài)組合導(dǎo)航系統(tǒng)，可以有效實(shí)現(xiàn)INS的姿態(tài)校正，從而提高組合導(dǎo)航的整體精度。

(2)為了克服雙接收機(jī)接收到的GPS共同衛(wèi)星數(shù)目波動(dòng)頻繁的問題，在系統(tǒng)中加入BDS衛(wèi)星，可以有效提高雙天線測姿的穩(wěn)定性和精度，并在傳統(tǒng)的松組合基礎(chǔ)上加入速度和姿態(tài)實(shí)現(xiàn)全狀態(tài)融合，可以有效抑制INS的發(fā)散，使航向角和橫滾角的均方根誤差分別為0.305 9和0.132 4，有效提高了組合導(dǎo)航系統(tǒng)的精度和穩(wěn)定性。