3D RISS/GPS組合導(dǎo)航在衛(wèi)星失鎖下的定位精度分析

魏漲汀，錢素琴

3D RISS/GPS組合導(dǎo)航在衛(wèi)星失鎖下的定位精度分析

魏漲汀1，錢素琴2

（1. 東華大學(xué) 信息科學(xué)與技術(shù)學(xué)院，上海 201620；2. 東華大學(xué) 數(shù)字化紡織服裝技術(shù)教育部工程研究中心，上海 201620）

針對車輛組合導(dǎo)航在衛(wèi)星信號失鎖條件下定位精度較低的問題，提出了一種輸出校正和反饋校正同時(shí)進(jìn)行的三維簡化慣性傳感器系統(tǒng)（3D RISS）/全球定位系統(tǒng)（GPS）組合導(dǎo)航模型。該模型在衛(wèi)星信號正常的時(shí)候進(jìn)行3D RISS和GPS的組合導(dǎo)航，由擴(kuò)展卡爾曼濾波對二者的輸出進(jìn)行融合，并通過輸出校正和反饋校正同時(shí)進(jìn)行的方式得到當(dāng)前的位置、姿態(tài)、速度以及傳感器誤差等信息，在衛(wèi)星信號失鎖后切換至3D RISS自主導(dǎo)航，并補(bǔ)償之前所估計(jì)的傳感器誤差。實(shí)驗(yàn)結(jié)果表明，該組合導(dǎo)航模型可以保證車輛在衛(wèi)星信號正常和失鎖階段都有較高的定位精度。

組合導(dǎo)航；車輛定位；衛(wèi)星信號失鎖；擴(kuò)展卡爾曼濾波；三維簡化慣性傳感器系統(tǒng)

0 引言

隨著社會(huì)的發(fā)展，人們對車輛的需求越來越大，因此對于車輛導(dǎo)航的定位精度要求也越來越高。經(jīng)過幾十年的發(fā)展，導(dǎo)航技術(shù)也從之前的單一導(dǎo)航逐漸發(fā)展成組合導(dǎo)航。組合導(dǎo)航可以將各種單一導(dǎo)航的特點(diǎn)進(jìn)行結(jié)合，實(shí)現(xiàn)優(yōu)勢互補(bǔ)，其中應(yīng)用最為廣泛的就是全球定位系統(tǒng)（global positioning system，GPS）與慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）相結(jié)合的模式[1]。GPS適用范圍廣，但比較容易受外界環(huán)境因素的影響且輸出頻率較低，而INS有著不受外界條件限制，自主性強(qiáng)，輸出頻率高，短時(shí)導(dǎo)航效果好等特點(diǎn)，因此，二者結(jié)合能大大提高導(dǎo)航的適用范圍和精度[2]。

INS/GPS組合導(dǎo)航的數(shù)據(jù)融合方式，如今已經(jīng)比較成熟，隨著卡爾曼濾波、擴(kuò)展卡爾曼濾波、粒子濾波、無跡卡爾曼濾波等濾波算法的提出，組合導(dǎo)航輸出數(shù)據(jù)的精度越來越高[3]。然而，組合導(dǎo)航的問題卻往往集中在衛(wèi)星信號失鎖階段，車輛在實(shí)際行駛過程中，會(huì)出現(xiàn)很多衛(wèi)星信號不佳的情況，比如地庫，城市峽谷，惡劣天氣等，這時(shí)候傳統(tǒng)的濾波器就會(huì)失效，慣性導(dǎo)航的誤差會(huì)隨著時(shí)間迅速積累而導(dǎo)致系統(tǒng)發(fā)散。為了保證車輛組合導(dǎo)航系統(tǒng)在衛(wèi)星信號失鎖條件下仍然能保持一段時(shí)間的高精度定位，文獻(xiàn)[4-5]將神經(jīng)網(wǎng)絡(luò)引入組合導(dǎo)航系統(tǒng)，對系統(tǒng)誤差模型進(jìn)行動(dòng)態(tài)學(xué)習(xí)，可以在衛(wèi)星信號失鎖后保證一定的導(dǎo)航精度，但由于實(shí)際行車路況非常復(fù)雜，利用神經(jīng)網(wǎng)絡(luò)的方法有時(shí)候無法達(dá)到精度要求，這時(shí)就需要從另一個(gè)角度進(jìn)行分析，通過對慣性導(dǎo)航環(huán)節(jié)做誤差補(bǔ)償或者尋找新的觀測量來提高組合導(dǎo)航的定位精度。陸地車輛可以將輪速當(dāng)作另一觀測量，因此，車輛組合導(dǎo)航往往采用的是INS/GPS/輪速的組合形式[6-10]，在衛(wèi)星信號失鎖條件下，采用車載慣性和里程計(jì)組合的導(dǎo)航模式，保證短時(shí)間內(nèi)的導(dǎo)航精度。然而，由于慣性器件以及里程計(jì)的測量存在誤差，失鎖階段的定位精度依舊會(huì)受到一定的影響，因此需要對這部分誤差進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償。本文基于三維簡化慣性傳感器系統(tǒng)（three-dimensional reduced inertial sensor system，3D RISS）這一航位推算（dead reckoning，DR）方法，設(shè)計(jì)了一種輸出校正和反饋校正同時(shí)進(jìn)行的3D RISS/GPS組合導(dǎo)航系統(tǒng)，在引入輪速輔助的同時(shí)，估計(jì)出傳感器誤差，可以使車輛組合導(dǎo)航在衛(wèi)星信號失鎖后仍然具有較高的定位精度。

1 3D RISS/GPS組合導(dǎo)航

1.1 3D RISS模型

3D RISS就是利用車輛的里程計(jì)、一個(gè)單軸陀螺儀和兩個(gè)加速度計(jì)來實(shí)現(xiàn)三維導(dǎo)航的一種DR方法，陀螺儀與車輛的垂直軸對齊，俯仰角和橫滾角不再通過陀螺儀計(jì)算，而是通過前向和側(cè)向的加速度計(jì)算得，同時(shí)速度也不再通過加速度計(jì)來計(jì)算，而是直接采用里程計(jì)測得的速度[11]。3D RISS模型如圖1所示。

圖1 3D RISS模型

與傳統(tǒng)的DR[12-13]相比，3D RISS結(jié)合了俯仰角以及橫滾角，實(shí)現(xiàn)了三維導(dǎo)航，可以推算出更準(zhǔn)確的航向角。而與INS相比，3D RISS引入了里程計(jì)的速度并直接參與計(jì)算，避免了加速度計(jì)帶來的誤差，使得位置的推算更加準(zhǔn)確。

3D RISS的優(yōu)點(diǎn)在于，它是通過加速度計(jì)來計(jì)算俯仰角和橫滾角而非陀螺儀，因此可以避免陀螺儀的積分誤差所帶來的影響。而在速度的計(jì)算上，也不再使用加速度計(jì)而是直接采用了里程計(jì)所測得的速度，從而避免了加速度計(jì)的積分誤差所帶來的影響。所以在3D RISS中，主要的誤差就是垂直方向的陀螺儀漂移和里程計(jì)測量誤差。

1.2 3D RISS航位推算

車輛運(yùn)動(dòng)的姿態(tài)包括俯仰角、橫滾角以及航向角。俯仰角和橫滾角可以通過車輛的前向加速度以及側(cè)向加速度與重力加速度的夾角算得，當(dāng)車輛運(yùn)動(dòng)時(shí)，前向加速度計(jì)測量的是車輛前向加速度以及重力分量。為了計(jì)算俯仰角，需要在前向加速度的測量值中減去由里程計(jì)算得的車輛加速度，可得俯仰角的計(jì)算公式為

計(jì)算橫滾角時(shí)也要對橫向加速度進(jìn)行補(bǔ)償，可得橫滾角的計(jì)算公式為

航向角定義為與正北的夾角，沿順時(shí)針方向?yàn)檎?。在航向角更新過程中，需要考慮到地球自轉(zhuǎn)引起的分量以及速度在坐標(biāo)系轉(zhuǎn)換中引起的分量。航向角的計(jì)算公式為

根據(jù)車輛姿態(tài)和地球模型可以求得車輛當(dāng)前時(shí)刻的位置信息，位置計(jì)算公式為

根據(jù)車輛的姿態(tài)信息可以得到從載體坐標(biāo)系到東北天坐標(biāo)系的旋轉(zhuǎn)矩陣為

車輛速度由里程計(jì)提供，經(jīng)過旋轉(zhuǎn)矩陣轉(zhuǎn)化到東北天坐標(biāo)系之后可得東向、北向和天向的速度分別為

1.3 3D RISS/GPS組合導(dǎo)航數(shù)據(jù)融合

為了便于實(shí)現(xiàn)，GPS與3D RISS二者的組合方式為松耦合，間接法濾波，數(shù)據(jù)融合上采用的是擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）[14]。相比于傳統(tǒng)的INS/GPS/輪速的組合導(dǎo)航，3D RISS/GPS組合導(dǎo)航不需要設(shè)計(jì)2個(gè)濾波器在衛(wèi)星信號正常和失鎖之間進(jìn)行切換，同時(shí)3D RISS/GPS組合導(dǎo)航的誤差狀態(tài)量只有9維，簡化了計(jì)算，降低了對處理器的要求。

3D RISS EKF的誤差狀態(tài)方程可表示為

將誤差狀態(tài)方程線性離散化后可得

為了能夠更準(zhǔn)確地估計(jì)出陀螺儀的動(dòng)態(tài)漂移，將GPS的航向角信息也作為觀測量，形成7維觀測量為

離散卡爾曼濾波方程為

2 傳感器誤差估計(jì)和補(bǔ)償

在利用誤差狀態(tài)量對組合導(dǎo)航系統(tǒng)進(jìn)行校正時(shí)會(huì)用到輸出校正和反饋校正2種方式，這2種方式也被稱為開環(huán)法和閉環(huán)法[15]。輸出校正和反饋校正的形式如圖2所示。

圖2 輸出校正和反饋校正

所謂輸出校正就是利用濾波器估計(jì)的導(dǎo)航參數(shù)誤差值去校正導(dǎo)航輸出的參數(shù)，從而得到組合系統(tǒng)導(dǎo)航參數(shù)的最優(yōu)估計(jì)值。反饋校正則是把濾波器估計(jì)出的導(dǎo)航參數(shù)誤差反饋到慣導(dǎo)系統(tǒng)內(nèi)對誤差狀態(tài)進(jìn)行校正，慣導(dǎo)重新解算出更為精確的導(dǎo)航參數(shù)。2種校正方式各有優(yōu)缺點(diǎn)，輸出校正的優(yōu)勢在于系統(tǒng)和濾波器是獨(dú)立工作的，因此輸出校正的方式可以得到GPS輸出、慣性導(dǎo)航輸出以及組合導(dǎo)航輸出3種結(jié)果，另外輸出校正的方式不在誤差系統(tǒng)模型內(nèi)部對參數(shù)進(jìn)行修正，這也就意味著通過輸出校正，可以得到系統(tǒng)誤差在一段時(shí)間內(nèi)的變化情況，這有利于傳感器誤差的估計(jì)，但是輸出校正的缺點(diǎn)在于其系統(tǒng)誤差的狀態(tài)模型是經(jīng)過一階線性化的，即使誤差是相對較小的量，長時(shí)間工作下依舊會(huì)導(dǎo)致一定程度的失真，從而使得濾波器的精度下降。反饋校正則相反，在對誤差狀態(tài)進(jìn)行校正后，系統(tǒng)誤差可以始終保持在比較小的量級，實(shí)際上在每次濾波后，誤差狀態(tài)量都被置零，因此可以保證濾波器長時(shí)間的精度，可是這種做法卻不利于傳感器誤差的估計(jì)。

上文提到對于3D RISS模型，主要的誤差來源是垂直方向陀螺儀的漂移以及里程計(jì)的測量誤差，通過卡爾曼濾波器可以實(shí)時(shí)估計(jì)這兩項(xiàng)誤差。利用反饋校正很容易實(shí)現(xiàn)系統(tǒng)在3D RISS/GPS組合導(dǎo)航和3D RISS自主導(dǎo)航之間的切換，便于程序的設(shè)計(jì)，而利用輸出校正則能估計(jì)出更準(zhǔn)確的傳感器誤差，為了結(jié)合二者的優(yōu)點(diǎn)，這里利用了2種校正方式同時(shí)進(jìn)行的方法，它們利用相同的導(dǎo)航參數(shù)進(jìn)行卡爾曼濾波，并且獨(dú)立進(jìn)行，只在時(shí)間上同步，反饋校正后的輸出作為組合導(dǎo)航的輸出，輸出校正只用來估計(jì)傳感器誤差，實(shí)現(xiàn)流程如圖3所示。

圖3 開環(huán)和閉環(huán)同時(shí)進(jìn)行的3D RISS/GPS組合導(dǎo)航流程

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

為了驗(yàn)證方法的有效性，這里分別采集了幾組不同路況下衛(wèi)星信號失鎖前后的路測數(shù)據(jù)進(jìn)行仿真，其中包括空曠無遮擋道路、隧道、地庫以及林蔭路等路況。硬件系統(tǒng)包含主控芯片，五軸慣性傳感器單元，輪速傳感器，衛(wèi)星接收機(jī)模塊，GPS雙天線等。

圖4（a）、圖4（b）分別為車輛行駛在空曠無遮擋道路時(shí)的路徑和局部放大圖，圖中組合導(dǎo)航定位輸出頻率為100 Hz，實(shí)時(shí)差分定位（real-time kinematic, RTK）輸出為2 Hz,3D RISS/GPS推算路徑與RTK固定解定位路徑基本一致，誤差在厘米級別。由此可以看出在衛(wèi)星質(zhì)量較好的情況下，文中設(shè)計(jì)的3D RISS/GPS組合導(dǎo)航具有較高的定位精度。

圖4 車輛行駛在空曠無遮擋道路上的路徑

圖5（a）、圖5（b）、圖5（c）分別為汽車行駛在隧道前后的路徑和局部放大情況。圖5（b）為有衛(wèi)星信號時(shí)車輛的運(yùn)動(dòng)路徑，從圖5（b）中可以看出，在隧道外有衛(wèi)星信號的時(shí)候系統(tǒng)推算路徑與GPS路徑基本重合，也再次說明了在衛(wèi)星信號正常階段該組合導(dǎo)航系統(tǒng)可以提供非常精確的導(dǎo)航定位。圖5（c）為衛(wèi)星信號失鎖階段即隧道內(nèi)的路徑對比，由于陀螺儀的漂移以及里程計(jì)的誤差使得系統(tǒng)在3D RISS自主導(dǎo)航環(huán)節(jié)推算的路徑（圖中左邊路徑）與實(shí)際路徑（圖中右邊離散點(diǎn)路徑為參考路徑）產(chǎn)生了偏差，失鎖總長約為1768 m，失鎖時(shí)長在150 s左右，失鎖偏移距離為16.2 m，誤差率在0.9%左右。但上述路徑是并未進(jìn)行開環(huán)誤差補(bǔ)償?shù)?D RISS所推算的，本文所提出的方案中可以利用獨(dú)立的開環(huán)卡爾曼濾波器來估計(jì)傳感器誤差，因此可以在衛(wèi)星失鎖后補(bǔ)償之前估計(jì)出的傳感器誤差，推算出更為精確的路徑（圖5（c）中中間靠近右邊的路徑），補(bǔ)償后失鎖偏移距離由原來的16.2 m縮短到了5 m左右，誤差率降低到了0.3%，大大提高了失鎖階段的導(dǎo)航精度。

圖5 車輛行駛在隧道前后的路徑

圖6（a）、圖6（b）為車輛在地庫內(nèi)外行駛的情況。測試車輛在進(jìn)入地庫后繞著地庫連續(xù)跑4圈后出庫，地庫內(nèi)衛(wèi)星信號失鎖，圖6（b）為車輛在地庫內(nèi)的路徑推算圖。從圖6（b）中可以非常明顯地看出，由于陀螺儀的漂移導(dǎo)致了系統(tǒng)推算的路徑有較大的偏差，本應(yīng)該重合的4圈地庫變成了梅花鏈的形狀，出庫的偏移距離也非常大，但是經(jīng)過傳感器誤差的補(bǔ)償后可以看出地庫內(nèi)車輛的行駛情況已經(jīng)非常接近真實(shí)路徑了。

圖7為車輛行駛在小區(qū)林蔭道里的情況。林蔭道最大的特點(diǎn)就是衛(wèi)星信號斷斷續(xù)續(xù)，也就是說車輛在行駛過程中會(huì)出現(xiàn)很多次衛(wèi)星失鎖的情況，并且每次失鎖的時(shí)間都很短，大概幾秒鐘。從圖7中可以看出，在整個(gè)行駛過程中，系統(tǒng)推算的路徑幾乎沒有偏離，這也說明了3D RISS航位推算在短時(shí)間內(nèi)的導(dǎo)航精度是非常高的。

圖7 車輛行駛在林蔭道上的路徑圖

車輛在實(shí)際行駛過程中，可能會(huì)由于一些不確定因素而導(dǎo)致系統(tǒng)在誤差估計(jì)和導(dǎo)航路徑推算上出現(xiàn)問題，為了驗(yàn)證系統(tǒng)的性能，這里對一個(gè)地庫和2個(gè)隧道進(jìn)行了多次測試，統(tǒng)計(jì)結(jié)果如表1所示。另外由于在衛(wèi)星信號正常階段，組合導(dǎo)航輸出的結(jié)果基本上沒有偏差，所以這里不做統(tǒng)計(jì)。表1中的偏移距離為衛(wèi)星信號恢復(fù)后3D RISS推算位置與GPS給出位置的距離，誤差率為偏移距離除以失鎖行駛距離，從表1中可以看出，本文所采用的組合導(dǎo)航系統(tǒng)在地庫和隧道的定位誤差率可以基本維持在1%以內(nèi)，具有較高的精度。

表1 車輛行駛在地庫和隧道的定位誤差統(tǒng)計(jì)

4 結(jié)束語

組合導(dǎo)航在衛(wèi)星信號失鎖階段的定位精度決定了整個(gè)系統(tǒng)的導(dǎo)航性能。本文從3D RISS出發(fā)，設(shè)計(jì)了一種低成本，易于實(shí)現(xiàn)的組合導(dǎo)航系統(tǒng)，可以根據(jù)衛(wèi)星信號的質(zhì)量來切換3D RISS/GPS組合導(dǎo)航和3D RISS自主導(dǎo)航這2種模式，經(jīng)誤差補(bǔ)償后的3D RISS自主導(dǎo)航，可以使車輛在衛(wèi)星失鎖階段的定位誤差率基本維持在1%以內(nèi)，在城市車輛定位方面有一定的工程應(yīng)用價(jià)值。文中只對主要的誤差進(jìn)行了估計(jì)和補(bǔ)償，當(dāng)然系統(tǒng)還有一些其他的誤差會(huì)影響導(dǎo)航定位精度，比如IMU的安裝誤差，桿臂誤差以及GPS雙天線的安裝誤差等，對這些誤差進(jìn)行補(bǔ)償，可以使得系統(tǒng)在衛(wèi)星失鎖下有更高的定位精度。

[1] 王海生. SINS/GPS組合導(dǎo)航的捷聯(lián)算法研究[D]. 沈陽: 沈陽理工大學(xué), 2019.

[2] 韓正陽. 衛(wèi)星導(dǎo)航與慣性導(dǎo)航組合定位技術(shù)研究[D]. 石家莊: 河北科技大學(xué), 2020.

[3] 趙雪峰. 衛(wèi)星信號短暫缺失時(shí)的組合導(dǎo)航技術(shù)研究與實(shí)現(xiàn)[D]. 成都: 電子科技大學(xué), 2017.

[4] NOURELDIN A, EL-SHAFIE A, BAYOUMI M. GPS/INS integration utilizing dynamic neural networks for vehicular navigation[J]. Information Fusion, 2011, 12(1): 48-57

[5] 張新民, 張維. GPS失鎖情況下基于神經(jīng)網(wǎng)絡(luò)的組合導(dǎo)航算法研究[J]. 彈箭與制導(dǎo)學(xué)報(bào), 2017, 37(3): 13-16.

[6] 白亮, 秦永元, 嚴(yán)恭敏, 等. 車載航位推算組合導(dǎo)航算法研究[J]. 計(jì)算機(jī)測量與控制, 2010, 18(10): 2379-2381, 2384.

[7] 李旦, 秦永元, 張金亮. 車載慣導(dǎo)航位推算組合導(dǎo)航系統(tǒng)誤差補(bǔ)償研究[J]. 計(jì)算機(jī)測量與控制, 2011, 19(2): 389-391.

[8] 高福隆, 石然, 劉洋, 等. 車載高精度里程計(jì)輔助捷聯(lián)慣性組合導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)驗(yàn)[J]. 導(dǎo)航與控制, 2018, 17(6): 42-47.

[9] 劉鵬飛. 里程計(jì)輔助的高精度車載GNSS/INS組合導(dǎo)航系統(tǒng)[J]. 光學(xué)精密工程, 2020, 28(4): 979-987.

[10] 王可, 劉立剛, 周斌, 等. 基于SINS/GNSS/輪速的列車組合導(dǎo)航算法研究[J]. 系統(tǒng)仿真學(xué)報(bào), 2021, 33(1): 189-195.

[11] COSSABOOM M, GEORGY J, KARAMAT T, et al. Augmented Kalman filter and map matching for 3D RISS/GPS integration for land vehicles[EB/OL]. [2021-08-21]. https: //downloads. hindawi. com/archive/2012/576807. pdf.

[12] 張博. GPS/DR車輛組合導(dǎo)航系統(tǒng)設(shè)計(jì)與研究[D]. 北京: 北京交通大學(xué), 2013.

[13] 沈圓, 倪福生, 魏長赟, 等. 基于粒子濾波方法的GPS/DR組合導(dǎo)航仿真研究[J]. 計(jì)算機(jī)與數(shù)字工程, 2017, 45(8): 1539-1542.

[14] NOURELDIN A, KARAMAT T B, GEORGY J. Two case studies: full IMU/GPS and 3D RISS/GPS integration[EB/OL]. [2021-08-21]. https: //sci-hub. se/10. 1007/978-3-642-30466-8_10.

[15] 尤杰, 高紅蓮, 曹松銀. 基于魯棒濾波的SINS/GPS松組合導(dǎo)航系統(tǒng)的校正方法[J]. 揚(yáng)州大學(xué)學(xué)報(bào)(自然科學(xué)版), 2020, 23(6): 73-78.

Positioning accuracy analysis of 3D RISS/GPS integrated navigation system under satellite lock-out

WEI Zhangting1, QIAN Suqin2

(1. School of Information Science and Technology, Donghua University, Shanghai 201620, China;2. Engineering Research Center of Digitized Textile & Apparel Technology, Ministry of Education, Donghua University, Shanghai 201620, China)

Aiming at the problem of low positioning accuracy of vehicle integrated navigation under the condition of satellite signal lock-out, the paper propose an integrated navigation model in which output correction and feedback correction are carried out simultaneously. This model combined Three-Dimensional Reduced Inertial Sensor System (3D RISS) and Global Positioning System (GPS) navigation when the satellite signal is normal, and their output is fused by Extended Kalman Filter (EKF), at the same time, the current position, posture, speed and sensor error can be obtained by output correction and feedback correction. When the satellite signal is lost, the system can switch to 3D RISS autonomous navigation and compensate for the previously estimated sensor error. The experimental result showed that this integrated navigation model can guarantee the high positioning accuracy of the vehicle in the phase of normal satellite signal and GPS outage.

integrated navigation; vehicle positioning; navigation signal outage; extended Kalman filter; three-dimensional reduced inertial sensor system

P228

A

2095-4999(2022)04-0049-07

魏漲汀，錢素琴. 3D RISS/GPS組合導(dǎo)航在衛(wèi)星失鎖下的定位精度分析[J].導(dǎo)航定位學(xué)報(bào),2022,10(4): 49-55.（WEI Zhangting, QIAN Suqin. Positioning accuracy analysis of 3D RISS/GPS integrated navigation system under satellite lock-out[J].Journal of Navigation and Positioning, 2022, 10(4): 49-55.）DOI:10.16547/j.cnki.10-1096.20220407.

2021-09-27

魏漲?。?997—），男，浙江紹興人，碩士研究生，研究方向?yàn)榍度胧较到y(tǒng)和組合導(dǎo)航。

錢素琴（1971—），女，上海人，碩士，副教授，研究方向?yàn)榍度胧较到y(tǒng)和人工智能。