衛(wèi)星數(shù)不足條件下GPS/SINS數(shù)據(jù)融合工程化算法

王 楠，周本川，程海彬

(中航工業(yè)空空導(dǎo)彈研究院，河南洛陽(yáng) 471000)

GPS/INS組合導(dǎo)航系統(tǒng)已廣泛應(yīng)用于飛行器、車載系統(tǒng)等領(lǐng)域，GPS/INS數(shù)據(jù)融合的原理是利用GPS提供的觀測(cè)信息估計(jì)INS的導(dǎo)航或器件誤差并對(duì)其進(jìn)行校正，GPS接收機(jī)需要觀測(cè)4顆以上衛(wèi)星才能進(jìn)行導(dǎo)航定位，但是在GPS受到干擾或遮擋時(shí)，觀測(cè)衛(wèi)星數(shù)可能只有2～3顆，這種情況下，傳統(tǒng)的基于GPS定位結(jié)果的GPS/INS組合導(dǎo)航模型由于GPS接收機(jī)失效而無(wú)法進(jìn)行數(shù)據(jù)融合導(dǎo)航，并完全依賴INS導(dǎo)航系統(tǒng)。

事實(shí)上GPS衛(wèi)星數(shù)目不足4顆條件下，GPS仍有可用信息，對(duì)輔助定位或定位推測(cè)有很大作用。為了解決2～3顆衛(wèi)星條件下的GPS/INS數(shù)據(jù)融合問(wèn)題，人們一般采用改進(jìn)的數(shù)據(jù)融合方法或更高階次的系統(tǒng)模型，典型的方法有:“速度位置組合+鐘差聯(lián)合外推方法”［1］，“速度位置組合+構(gòu)建第三方觀測(cè)量方法”［2-3］，“基于偽距偽距率的緊組合導(dǎo)航模型”［4］等方法。這些方法都能一定程度上解決2～3顆衛(wèi)星條件下的GPS/INS數(shù)據(jù)融合問(wèn)題，但也存在著諸如設(shè)備成本增加，精度難以保證，系統(tǒng)階次高計(jì)算耗時(shí)等弊端。

本研究提出一種在衛(wèi)星數(shù)不完備條件下的工程化的數(shù)據(jù)融合方法，能夠克服傳統(tǒng)緊組合濾波算法在2～3顆衛(wèi)星條件下的不足，具有實(shí)現(xiàn)簡(jiǎn)單，可靠性高等特點(diǎn)。

1 現(xiàn)有方法及其不足

接收機(jī)鐘鐘差可采用類似GPS衛(wèi)星鐘鐘差的二次多項(xiàng)式進(jìn)行擬合［3］。常見(jiàn)的鐘差擬合公式為

在正常衛(wèi)星數(shù)不少于4顆時(shí)，利用單點(diǎn)定位獲得接收機(jī)鐘差，然后利用數(shù)個(gè)歷元的鐘差求解擬合系數(shù)a0，a1，a2。當(dāng)運(yùn)動(dòng)載體進(jìn)入遮擋環(huán)境觀測(cè)衛(wèi)星數(shù)小于4顆時(shí)，利用前面正常定位條件計(jì)算出的鐘差系數(shù)外推接收機(jī)鐘差，因此衛(wèi)星觀測(cè)方程只有3個(gè)未知的坐標(biāo)參數(shù)，僅需觀測(cè)3顆衛(wèi)星即可進(jìn)行導(dǎo)航定位。

該方法缺點(diǎn)是鐘差漂移建模不準(zhǔn)確，接收機(jī)的鐘的穩(wěn)定度一般為 10 ～8 s［5］，并存在鐘差跳變的情況［6］;此外，鐘差的擬合系數(shù)估值也存在誤差，因此在短時(shí)間內(nèi)鐘差外推精度可以接受，長(zhǎng)時(shí)間遞推誤差隨時(shí)間增長(zhǎng)很快。

2 速度位置組合+構(gòu)建第三方觀測(cè)量

此類方法主要通過(guò)增加額外傳感器觀測(cè)信息來(lái)克服衛(wèi)星數(shù)目不足4顆情況下可用方程數(shù)目不足的問(wèn)題，常見(jiàn)的方法有引入里程計(jì)、磁力計(jì)、高度表、地圖匹配或高精度INS等附加運(yùn)動(dòng)約束。

除了GPS提供的直接觀測(cè)信息外，根據(jù)載體具體的運(yùn)動(dòng)環(huán)境、觀測(cè)條件等，系統(tǒng)中某些參數(shù)滿足一些特定的條件。這些運(yùn)動(dòng)信息充分利用相當(dāng)于在觀測(cè)模型中增加了虛擬觀測(cè)信息，對(duì)解決組合系統(tǒng)觀測(cè)信息不足，部分參數(shù)難以準(zhǔn)確估計(jì)的問(wèn)題是有利的。

這些共同特點(diǎn)是增加硬件結(jié)構(gòu)，增加成本，或者應(yīng)用范圍有限，不便于工程化應(yīng)用。

3 基于偽距/偽距率的緊組合方法

該方法是目前見(jiàn)諸文獻(xiàn)最多的方法，是目前此類問(wèn)題的通用方法。原理是利用可用衛(wèi)星測(cè)量的偽距和偽距率信息，把載體SINS的積累誤差映射成載體至衛(wèi)星的視距誤差，通過(guò)kalman濾波對(duì)SINS的誤差進(jìn)行濾波修正。系統(tǒng)狀態(tài)階數(shù)和量測(cè)方程數(shù)目較多，計(jì)算過(guò)程比較耗時(shí)。

該方法準(zhǔn)確性受建模影響較大，模型誤差嚴(yán)重時(shí)可能造成相反方向修正的結(jié)果。誤差修正效果不能完全保證。原因在于:

觀測(cè)誤差向?qū)Ш阶鴺?biāo)系投影的大小取決于濾波器中各方向誤差的方差估計(jì)(即P矩陣)，P矩陣體現(xiàn)的是濾波器對(duì)當(dāng)前誤差的評(píng)價(jià)，而通常情況下誤差的估計(jì)與實(shí)際誤差并不完全一致。這樣觀測(cè)誤差在導(dǎo)航系各方向的投影可能與該方向的實(shí)際誤差不一致，會(huì)出現(xiàn)總的合成誤差在收斂，而某一特定方向的誤差可能出現(xiàn)波動(dòng)，即收斂過(guò)程中誤差可能先增大后減小的情況。如果在誤差處于較大的時(shí)間點(diǎn)上，衛(wèi)星接收機(jī)定位中斷，那么該方向較大的誤差會(huì)一直保持，可能會(huì)對(duì)載體導(dǎo)航造成不可接受的影響。

4 工程化新算法

當(dāng)衛(wèi)星數(shù)僅為2～3顆時(shí)，接收機(jī)已不能給出速度位置的定位結(jié)果，但是能夠給出2～3顆衛(wèi)星的偽距和偽距率測(cè)量值，因此該算法就是最大限度地利用這幾顆衛(wèi)星的偽距和偽距率信息，校正理論上能夠觀測(cè)到并消除的誤差。

4.1 GPS偽距/偽距率測(cè)量方程

衛(wèi)星定位接收機(jī)測(cè)量得到的與第j顆衛(wèi)星之間的偽距可以表示為

其中:ρj為理想距離;δtu為等效時(shí)鐘誤差相應(yīng)的距離。由于δtu是偽距測(cè)量中的主要誤差，因此在建立偽距測(cè)量模型時(shí)，主要考慮了該項(xiàng)誤差的影響。νρj為偽距測(cè)量噪聲，由多路徑效應(yīng)、對(duì)流層延遲誤差、電流層誤差等引起?？梢越普J(rèn)為是白噪聲，則慣導(dǎo)計(jì)算的載體位置(xIyIzI)與第j顆衛(wèi)星之間的偽距為

設(shè)(x y z)為載體和衛(wèi)星之間的真實(shí)距離。(δxδyδz)為慣導(dǎo)計(jì)算的載體在導(dǎo)航坐標(biāo)系的誤差。即

將計(jì)算式在(x y z)處進(jìn)行泰勒展開(kāi)，并忽略高階項(xiàng)可以得到:

其中:δtu為接收機(jī)鐘差引起的偽距測(cè)量誤差;νρj表示包括多路徑，電離層延遲誤差在內(nèi)的測(cè)量噪聲。

同理系統(tǒng)偽距率量測(cè)誤差方程為

當(dāng)衛(wèi)星數(shù)為3顆時(shí)，以偽距觀測(cè)方程為例，能夠構(gòu)造一個(gè)觀測(cè)方程組為

將其做線性變化后，可以將公共接收機(jī)鐘差δtu項(xiàng)消除，得到如下形式的方程組

4.2 誤差解算方法

上述方程組3個(gè)未知數(shù)兩個(gè)方程是存在無(wú)窮多個(gè)解的，但觀察方程組形式可以發(fā)現(xiàn)，形如Ax+By+Cz+D=0的方程在解析幾何上表示的是空間的一個(gè)平面，兩個(gè)這樣的平面相交，表示的是空間的一條直線，如將待估計(jì)的狀態(tài)(δxδyδz)T看作是以(0，0，0)為原點(diǎn)的一個(gè)空間向量，則該向量必定指向該空間直線上的某一點(diǎn)，具體指向哪一點(diǎn)，由于方程數(shù)量限制，是無(wú)法觀測(cè)到的。

誤差修正的根本目的是將誤差向量(δxδyδz)T消除，如確實(shí)不能完全消除，則應(yīng)盡可能將其減少到最小，因此雖然(δxδyδz)T不能求解，但可以精確解出原點(diǎn)到空間直線的垂線向量，并將該向量作為誤差消除;顯然消除該垂線向量后，剩余不可消除的誤差部分其實(shí)就是原誤差在空間直線上的投影，屬于不可觀測(cè)的維度。且剩余的誤差一定小于原有誤差，即該方法誤差修正的方向一定是正確的。由于該方法僅從幾何關(guān)系上求解誤差，因此不會(huì)出現(xiàn)前文所述方法中存在的因模型不準(zhǔn)確而造成誤差估算錯(cuò)誤的情況。

點(diǎn) P(x0，y0，z0)到空間直線 L

的距離公式為(證明過(guò)程參見(jiàn)文獻(xiàn)［7］):

根據(jù)該距離公式即可求解出垂線方向上的誤差分量，可以用來(lái)修正慣導(dǎo)誤差。實(shí)際應(yīng)用中可以經(jīng)過(guò)低通濾波器以消除高頻噪聲。

2顆衛(wèi)星情況下與此類似，線性變化后的觀測(cè)方程由2個(gè)變?yōu)?個(gè)，即:

同理，可以精確解出原點(diǎn)到空間平面的垂線向量，并將該向量作為誤差消除;顯然消除該垂線向量后，剩余不可消除的誤差部分其實(shí)就是原誤差在空間平面上的投影，屬于不可觀測(cè)的維度(且是2個(gè)維度不可觀測(cè));且剩余的誤差一定小于原有誤差，誤差修正的方向也一定是正確的。

點(diǎn) P(x0，y0，z0)到空間直線

的距離公式為(證明過(guò)程參見(jiàn)文獻(xiàn)［7］)

根據(jù)該距離公式即可求解出垂線方向上的誤差分量，可以用來(lái)修正慣導(dǎo)誤差。實(shí)際應(yīng)用中可以經(jīng)過(guò)低通濾波器以消除高頻噪聲。

5 仿真結(jié)果

下面給出仿真驗(yàn)證結(jié)果，仿真條件為:采用SINS/GPS組合導(dǎo)航方式，SINS初始速度誤差北天東3個(gè)方向均為3 m/s，位置初始誤差3個(gè)方向均為0 m，初始姿態(tài)誤差設(shè)為0，仿真驗(yàn)證純慣性導(dǎo)航、2～3顆衛(wèi)星下導(dǎo)航、以及衛(wèi)星正常定位狀態(tài)下導(dǎo)航的速度位置誤差收斂趨勢(shì)，結(jié)果如圖1～圖2所示。

圖1 速度誤差

圖2 位置誤差

由仿真結(jié)果可以看出，2～3顆星情況下，該算法能夠?qū)⒁话胍陨系乃俣群臀恢谜`差消除(能消除多少還與3顆衛(wèi)星的分布有重要關(guān)系，沒(méi)有固定指標(biāo)，理論上0% ～100%都有可能，由觀測(cè)幾何關(guān)系確定)。且3顆星情況下略好與2顆衛(wèi)星情況，這2顆衛(wèi)星取的是這3顆衛(wèi)星中的2顆，與上文的推斷符合，因此印證該算法是可行的。

6 結(jié)論

針對(duì)GPS接收機(jī)觀測(cè)衛(wèi)星數(shù)不足4顆條件下的GPS/SINS數(shù)據(jù)融合問(wèn)題，對(duì)目前常見(jiàn)的解決方法進(jìn)行了綜述，分析了現(xiàn)有方法的優(yōu)缺點(diǎn)，并一種新的算法以克服現(xiàn)有算法不足，該算法根據(jù)可見(jiàn)衛(wèi)星的偽距/偽距率信息，利用幾何關(guān)系，將慣導(dǎo)誤差在觀測(cè)方向的投影消除，僅殘留部分不可觀測(cè)的誤差，將誤差修正到理論上的最小值。仿真測(cè)試結(jié)果良好，2～3顆星條件下組合精度相對(duì)衛(wèi)星數(shù)大于4顆的情況會(huì)有所下降，組合后精度與衛(wèi)星視線方向與慣導(dǎo)誤差矢量的夾角有關(guān)，但明顯高于純慣導(dǎo)精度。

［1］甘雨，隋立芬.GPS/SINS緊組合導(dǎo)航中接收機(jī)鐘差建模［J］.大地測(cè)量與地球動(dòng)力學(xué)，2014，34(6):50-54.

［2］柴艷菊，陽(yáng)仁貴.附加運(yùn)動(dòng)約束提高 GPS/INS導(dǎo)航參數(shù)估計(jì)精度［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(1):28-32.

［3］林雪原，何友.GPS/羅蘭C/SINS/AHRS組合導(dǎo)航系統(tǒng)［J］.電子科技大學(xué)學(xué)報(bào)，2008，37(1):4-7.

［4］王楠，王龍.慣性系下GPS/SINS緊組合導(dǎo)航算法研究［J］.現(xiàn)代電子技術(shù)，2011，34(20):93-96.

［5］楊龍，徐冬梅，張守信，等.關(guān)于北斗衛(wèi)星導(dǎo)航系統(tǒng)的被動(dòng)式定位算法比較研究［J］.空間科學(xué)學(xué)報(bào)，2006，26(3):203-208.

［6］李永利.點(diǎn)到平面距離公式的簡(jiǎn)證及相關(guān)結(jié)論［J］.高等數(shù)學(xué)研究，2005，8(2):27-29.

［7］李志明，李宏偉.距離問(wèn)題的垂足法求解［J］.高等數(shù)學(xué)研究，2010，13(2):41-43.