時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法研究

董 亮，許東歡，臧中原，孫昭行，羅雪溶

(上海航天控制技術(shù)研究所，上海 201109)

0 引言

組合導(dǎo)航系統(tǒng)就是把具有不同特點(diǎn)的導(dǎo)航系統(tǒng)組合在一起，取長補(bǔ)短，以提高導(dǎo)航系統(tǒng)的精度。由于衛(wèi)星導(dǎo)航系統(tǒng)和慣性導(dǎo)航系統(tǒng)各有其優(yōu)缺點(diǎn)，全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System，GNSS)能夠提供實(shí)時(shí)的位置、速度信息，并且精度不隨時(shí)間變化。捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)是一種完全自主的導(dǎo)航系統(tǒng)，具有不受外界電磁干擾、數(shù)據(jù)更新率高等特點(diǎn)。由于SINS和GNSS具有很強(qiáng)的互補(bǔ)性，因此，對二者進(jìn)行組合可以獲得更好的性能。

當(dāng)接收機(jī)在城市或者峽谷等復(fù)雜環(huán)境中，衛(wèi)星信號容易受到遮擋或者干擾，從而導(dǎo)致衛(wèi)星信號丟失。當(dāng)可見衛(wèi)星數(shù)少于4顆時(shí)，GNSS接收機(jī)不能正常定位，此時(shí)慣性/衛(wèi)星松組合導(dǎo)航系統(tǒng)只能工作于純捷聯(lián)慣性導(dǎo)航解算模式，而基于偽距/偽距率的緊組合導(dǎo)航系統(tǒng)可以正常工作，但由于觀測量不足，系統(tǒng)仍然會(huì)發(fā)散。本文針對上述問題，提出了時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法，該算法通過正常情況下的鐘差、鐘漂估計(jì)值建立鐘差和鐘漂的數(shù)學(xué)模型，在可見衛(wèi)星少于4顆時(shí)，用鐘差、鐘漂模型計(jì)算得到的鐘差和鐘漂作為系統(tǒng)的真實(shí)值，并省去狀態(tài)變量中的鐘差和鐘漂項(xiàng)，增加了系統(tǒng)的可觀測性。最后，通過采集跑車試驗(yàn)數(shù)據(jù)進(jìn)行離線數(shù)據(jù)仿真試驗(yàn)，比較了傳統(tǒng)方法和本文方法在可見衛(wèi)星少于4顆情況下的定位、測速精度。試驗(yàn)結(jié)果表明，本文提出的時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法在可見衛(wèi)星不足的情況下取得了較好的效果。

1 時(shí)鐘誤差模型

單模衛(wèi)星接收機(jī)定位測速最少需要4顆可見衛(wèi)星，通過4顆可見衛(wèi)星的位置、速度以及偽距、偽距率信息就可以解算出載體的三維位置、速度以及接收機(jī)的鐘差和鐘漂等信息。以載體位置計(jì)算為例，具體公式如下

(1)

式中，(=1,2,3,4)為第顆衛(wèi)星到載體的偽距；(,,)(=1,2,3,4)為第顆衛(wèi)星的位置；(,,)為載體的位置；為光速；δ為衛(wèi)星接收機(jī)時(shí)鐘誤差。

為了在可見衛(wèi)星少于4顆時(shí)計(jì)算載體的位置，需減少式(1)中的未知量個(gè)數(shù)。通常，同一個(gè)接收機(jī)的鐘差在一定時(shí)間內(nèi)是一個(gè)隨機(jī)常值。因此，本文將正常緊組合狀態(tài)下估計(jì)出的接收機(jī)鐘差的平均值作為接收機(jī)的實(shí)際鐘差，即將正常緊組合狀態(tài)下的鐘差估計(jì)值進(jìn)行累加，當(dāng)可見衛(wèi)星少于4顆時(shí)，計(jì)算估計(jì)出的鐘差平均值并作為接收機(jī)的實(shí)際鐘差，這樣式(1)就可簡化為

(2)

式中，δ為估計(jì)出的接收機(jī)鐘差平均值。

通過式(2)可以實(shí)現(xiàn)可見衛(wèi)星為3顆時(shí)的定位解算。

同樣，接收機(jī)鐘漂可以認(rèn)為是隨時(shí)間線性變化的值，可以利用正常緊組合情況下估計(jì)出的接收機(jī)鐘漂對接收機(jī)的時(shí)鐘漂移進(jìn)行擬合。本文通過一階線性模型=+擬合衛(wèi)星接收機(jī)的鐘漂，選取2個(gè)時(shí)刻的鐘漂估計(jì)值計(jì)算出擬合模型，在可見衛(wèi)星數(shù)為3顆時(shí)，用擬合得到的鐘漂作為接收機(jī)的實(shí)際鐘漂進(jìn)行解算。這樣就可以在可見衛(wèi)星數(shù)為3顆時(shí)解算出載體的速度。

試驗(yàn)中用到的衛(wèi)星接收機(jī)的鐘差、鐘漂的正常估計(jì)值以及模型擬合值如圖1和圖2所示。模型擬

圖1 衛(wèi)星接收機(jī)時(shí)鐘誤差Fig.1 The clock error of satellite receiver

圖2 衛(wèi)星接收機(jī)時(shí)鐘漂移Fig.2 The clock drift of satellite receiver

合曲線前100s為正常組合狀態(tài)，100s后設(shè)置衛(wèi)星數(shù)少于4顆，正常估計(jì)值曲線全程可見衛(wèi)星數(shù)不少于4顆。

傳統(tǒng)慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)穩(wěn)定的定位測速也至少需要4顆有效衛(wèi)星的數(shù)據(jù)，當(dāng)可見衛(wèi)星少于4顆時(shí)，由于觀測量不足，緊組合導(dǎo)航系統(tǒng)可能會(huì)發(fā)散。本文通過正常緊組合情況下的鐘差、鐘漂估計(jì)值建立鐘差和鐘漂的數(shù)學(xué)模型，在可見衛(wèi)星少于4顆時(shí)，用鐘差、鐘漂模型計(jì)算系統(tǒng)的鐘差和鐘漂作為真實(shí)值，并省去常規(guī)緊組合狀態(tài)下狀態(tài)變量中的鐘差和鐘漂項(xiàng)，減少了待估計(jì)量個(gè)數(shù)，增加了系統(tǒng)的可觀測性。

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

常規(guī)的慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)Kalman濾波狀態(tài)變量維數(shù)為17維，由捷聯(lián)慣導(dǎo)子系統(tǒng)的15個(gè)狀態(tài)量(3個(gè)姿態(tài)失準(zhǔn)角、3個(gè)速度誤差、3個(gè)位置誤差、3個(gè)陀螺常值漂移以及3個(gè)加表常值偏置)和衛(wèi)星接收機(jī)的2個(gè)狀態(tài)量(鐘漂等效距離率誤差、鐘差等效距離誤差)構(gòu)成。

考慮可見衛(wèi)星數(shù)少于4顆時(shí)，通過鐘差、鐘漂模型計(jì)算系統(tǒng)當(dāng)前時(shí)刻的鐘差、鐘漂值作為真實(shí)值，因此，衛(wèi)星接收機(jī)的2個(gè)狀態(tài)量(鐘差和鐘漂)不作為少星情況下的狀態(tài)量，則選取的慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)的狀態(tài)變量為15維，即捷聯(lián)慣性導(dǎo)航子系統(tǒng)的15個(gè)基本狀態(tài)量。具體狀態(tài)方程如下

(3)

(4)

3 系統(tǒng)觀測方程

衛(wèi)星接收機(jī)解算得到的載體與第顆GNSS衛(wèi)星之間的偽距可以表示為

(5)

捷聯(lián)慣性導(dǎo)航解算得到的載體位置[]與第顆衛(wèi)星之間的偽距為

(6)

(7)

結(jié)合式(5)和式(7)，可得慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)的偽距觀測方程如下

(8)

在可見衛(wèi)星數(shù)少于4顆時(shí)，用擬合得到的鐘差作為真實(shí)鐘差對衛(wèi)星接收機(jī)輸出的偽距進(jìn)行補(bǔ)償，則式(8)可轉(zhuǎn)化為

(9)

式(9)即為可見衛(wèi)星數(shù)少于4顆時(shí)的衛(wèi)星/慣性緊組合偽距觀測方程。

同理，衛(wèi)星接收機(jī)解算得到的載體與第顆衛(wèi)星之間的偽距率可以表示為

(10)

根據(jù)慣性導(dǎo)航解算得到的載體速度[]，可以計(jì)算出與第顆衛(wèi)星之間的偽距率為

(11)

(12)

其中

結(jié)合式(10)和式(12),可得慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)的偽距率觀測方程為

(13)

在可見衛(wèi)星數(shù)少于4顆時(shí)，用數(shù)學(xué)模型擬合得到的鐘漂作為真實(shí)鐘漂對衛(wèi)星接收機(jī)輸出的偽距率進(jìn)行補(bǔ)償，則式(13)可轉(zhuǎn)化為

(14)

式(14)即為可見衛(wèi)星數(shù)少于4顆時(shí)的慣性/衛(wèi)星緊組合偽距率觀測方程。

結(jié)合式(9)和式(14)，可得時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法的觀測方程。

4 試驗(yàn)結(jié)果與分析

為了驗(yàn)證本文提出的時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法的正確性和有效性，進(jìn)行了跑車數(shù)據(jù)離線仿真試驗(yàn)，比較了傳統(tǒng)慣性/衛(wèi)星緊組合導(dǎo)航方法和本文提出的時(shí)鐘模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法在少星情況下的定位、測速精度。

首先，在進(jìn)行跑車試驗(yàn)的過程中采集GNSS接收機(jī)數(shù)據(jù)和慣性測量數(shù)據(jù)，陀螺儀和加速度計(jì)的數(shù)據(jù)5ms存儲一次，GNSS接收機(jī)數(shù)據(jù)0.1s存儲一次。跑車試驗(yàn)過程中的慣性測量單元(Inertial Measurement Unit，IMU)和GNSS接收機(jī)的參數(shù)如表1所示，跑車試驗(yàn)實(shí)物圖如圖3所示，跑車試驗(yàn)位置二維軌跡如圖4所示。用跑車試驗(yàn)中采集的離線數(shù)據(jù)進(jìn)行離線仿真試驗(yàn)，建模時(shí)間分別設(shè)置為100s、80s、60s、30s、25s，建模完成后將有效衛(wèi)星數(shù)分別設(shè)為3顆和2顆。試驗(yàn)結(jié)果如圖5～圖8所示。

表1 慣性測量單元和衛(wèi)星接收機(jī)參數(shù)

圖3 跑車試驗(yàn)實(shí)物圖Fig.3 Picture of vehicle experiment

圖4 車載試驗(yàn)水平位置Fig.4 Horizontal position of vehicle experiment

圖5 3顆可見衛(wèi)星下位置曲線Fig.5 Position curve in case of 3 visible satellites

圖6 3顆可見衛(wèi)星下速度曲線Fig.6 Velocity curve in case of 3 visible satellites

圖7 2顆可見衛(wèi)星下位置曲線Fig.7 Position curve in case of 2 visible satellites

圖8 2顆可見衛(wèi)星下速度曲線Fig.8 Velocity curve in case of 2 visible satellites

通過圖5～圖8和表2可以看出，在可見衛(wèi)星少于4顆(3顆和2顆)、建模時(shí)間大于25s時(shí)，本文方法的定位測速精度明顯優(yōu)于傳統(tǒng)方案；在建模時(shí)間小于25s時(shí)，鐘差、鐘漂模型的精度明顯降低，定位測速誤差明顯變大。具體結(jié)果如表2所示。

表2 傳統(tǒng)方法和本文方法試驗(yàn)結(jié)果對比

5 結(jié)論

本文針對可見衛(wèi)星少于4顆時(shí)的組合導(dǎo)航方案，提出了一種基于鐘差、鐘漂模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法，該算法通過正常情況下的鐘差、鐘漂估計(jì)值建立鐘差和鐘漂的數(shù)學(xué)模型，在可見衛(wèi)星少于4顆時(shí)，用鐘差、鐘漂模型計(jì)算系統(tǒng)的鐘差和鐘漂，并作為鐘差、鐘漂的真實(shí)值。通過采集跑車試驗(yàn)數(shù)據(jù)進(jìn)行離線仿真試驗(yàn)，比較了傳統(tǒng)方法和本文方法在可見衛(wèi)星少于4顆情況下的定位、測速精度。結(jié)果表明，本文設(shè)計(jì)的基于鐘差、鐘漂模型輔助的慣性/衛(wèi)星緊組合導(dǎo)航算法在可見衛(wèi)星數(shù)少于4顆時(shí)具有較好的效果，在建模時(shí)間大于25s時(shí)，定位精度比傳統(tǒng)方法提高了2倍以上，定位和測速精度明顯高于傳統(tǒng)方法。