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

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

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

0 引言

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

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

1 時鐘誤差模型

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

(1)

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

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

(2)

式中，δ為估計出的接收機鐘差平均值。

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

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

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

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

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

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

傳統(tǒng)慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)實現(xiàn)穩(wěn)定的定位測速也至少需要4顆有效衛(wèi)星的數(shù)據(jù)，當(dāng)可見衛(wèi)星少于4顆時，由于觀測量不足，緊組合導(dǎo)航系統(tǒng)可能會發(fā)散。本文通過正常緊組合情況下的鐘差、鐘漂估計值建立鐘差和鐘漂的數(shù)學(xué)模型，在可見衛(wèi)星少于4顆時，用鐘差、鐘漂模型計算系統(tǒng)的鐘差和鐘漂作為真實值，并省去常規(guī)緊組合狀態(tài)下狀態(tài)變量中的鐘差和鐘漂項，減少了待估計量個數(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個狀態(tài)量(3個姿態(tài)失準(zhǔn)角、3個速度誤差、3個位置誤差、3個陀螺常值漂移以及3個加表常值偏置)和衛(wèi)星接收機的2個狀態(tài)量(鐘漂等效距離率誤差、鐘差等效距離誤差)構(gòu)成。

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

(3)

(4)

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

衛(wèi)星接收機解算得到的載體與第顆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顆時，用擬合得到的鐘差作為真實鐘差對衛(wèi)星接收機輸出的偽距進行補償，則式(8)可轉(zhuǎn)化為

(9)

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

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

(10)

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

(11)

(12)

其中

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

(13)

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

(14)

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

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

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

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

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

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

圖3 跑車試驗實物圖Fig.3 Picture of vehicle experiment

圖4 車載試驗水平位置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顆)、建模時間大于25s時，本文方法的定位測速精度明顯優(yōu)于傳統(tǒng)方案；在建模時間小于25s時，鐘差、鐘漂模型的精度明顯降低，定位測速誤差明顯變大。具體結(jié)果如表2所示。

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

5 結(jié)論

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