GPS/INS緊組合導(dǎo)航姿態(tài)計(jì)算方法選取分析

黃運(yùn)乾，程鵬飛，成英燕

(1.武漢大學(xué)測(cè)繪學(xué)院，湖北武漢430079;2.國(guó)家測(cè)繪工程中心，北京100039;3.國(guó)家測(cè)繪產(chǎn)品質(zhì)量檢驗(yàn)測(cè)試中心，北京100830;4.中國(guó)測(cè)繪科學(xué)研究院，北京100830)

一、引 言

GPS/INS緊組合導(dǎo)航是一種僅僅依靠GPS原始觀測(cè)數(shù)據(jù)(偽距、偽距率)來(lái)和INS數(shù)據(jù)進(jìn)行組合導(dǎo)航的一種模式。它的優(yōu)點(diǎn)在于直接提取衛(wèi)星接機(jī)中偽距、偽距率信息作為觀測(cè)量，降低了量測(cè)噪聲的相關(guān)性，并能在GPS可見(jiàn)衛(wèi)星數(shù)小于4顆的情況下確保一定的導(dǎo)航精度;同時(shí)用補(bǔ)償后的偽距、偽距率信息輔助接收機(jī)進(jìn)行衛(wèi)星捕獲，有助于提高系統(tǒng)的導(dǎo)航精度、魯棒性及抗干擾性能。

在慣性導(dǎo)航系統(tǒng)(INS)中，通過(guò)慣性測(cè)量單元陀螺和加速度計(jì)分別測(cè)得載體的角運(yùn)動(dòng)和線運(yùn)動(dòng)信息，導(dǎo)航計(jì)算機(jī)根據(jù)這些測(cè)量信息解算出載體的航向、姿態(tài)、速度和位置等導(dǎo)航參數(shù)。其中，姿態(tài)矩陣的解算相當(dāng)于建立起數(shù)學(xué)平臺(tái)，姿態(tài)矩陣一方面把加速度計(jì)的輸出從載體坐標(biāo)系變換到導(dǎo)航坐標(biāo)系，然后進(jìn)行導(dǎo)航計(jì)算;另一方面利用姿態(tài)矩陣的元素可以提取水平姿態(tài)角與航向角信息。姿態(tài)矩陣的計(jì)算，涉及載體姿態(tài)的實(shí)時(shí)解算，也關(guān)系到慣性導(dǎo)航平臺(tái)的即時(shí)修正，姿態(tài)矩陣實(shí)時(shí)更新的速度和精度對(duì)導(dǎo)航系統(tǒng)的性能有著直接的影響，因此姿態(tài)更新算法性能的優(yōu)劣直接影響捷聯(lián)慣性導(dǎo)航系統(tǒng)的導(dǎo)航精度，是捷聯(lián)慣性導(dǎo)航系統(tǒng)算法的核心。

姿態(tài)更新算法有方向余弦法、四元數(shù)法等，本文將方向余弦算法和四元數(shù)算法應(yīng)用在GPS/INS緊組合仿真數(shù)據(jù)處理過(guò)程中，并對(duì)其結(jié)果進(jìn)行比較，從而選擇一種更適用于GPS/INS緊組合數(shù)據(jù)處理的姿態(tài)更新算法。

二、方法分析

1.方向余弦法

方向余弦矩陣(direction cosine matrix，DCM)用示，為3×3階矩陣，矩陣的列表示載體坐標(biāo)系中單位矢量在參考坐標(biāo)系中的投影

DCM表示矢量變換

DCM隨時(shí)間變化率

載體系b系相對(duì)導(dǎo)航系n系在載體軸系的轉(zhuǎn)動(dòng)角速度為

其分量形式為

從上式可看出，方向余弦矩陣求解姿態(tài)矩陣時(shí)規(guī)避了歐拉方程退化的現(xiàn)象，未分方程是線性且非常簡(jiǎn)單的，可以全姿態(tài)工作，但同時(shí)要求解9個(gè)一階微分方程，計(jì)算量大。

2.四元數(shù)法

四元數(shù)最早由Hamilton于1843年提出，四元數(shù)由1個(gè)實(shí)數(shù)和3個(gè)虛數(shù)組成，它是四維空間中的一個(gè)矢量，但其3個(gè)虛數(shù)又具有三維空間中的矢量性質(zhì)。因此，任何三維空間中的一個(gè)矢量，都可以看作是一個(gè)實(shí)部為零的四元數(shù)，這個(gè)四元數(shù)是三維空間中的一個(gè)矢量在四維空間中的映象。

四元數(shù)是由4個(gè)元素構(gòu)成的數(shù)

式中，q0、q1、q2、q3為實(shí)數(shù);i、j、k 既是相互正交的單位向量，又是虛數(shù)單位。

并由它建立的微分方程組為

上述微分方程組中沒(méi)有奇點(diǎn)，因此可以表述載體的所有姿態(tài)，同時(shí)其約束方程為

三、GPS/INS緊組合模式下的姿態(tài)方法選擇

GPS/INS緊組合是一種依靠GPS原始觀測(cè)數(shù)據(jù)(即偽距/偽距率)來(lái)進(jìn)行組合導(dǎo)航的模式。它以INS相對(duì)GPS衛(wèi)星的距離、速度與接收機(jī)測(cè)量獲得的偽距、偽距率之差作為觀測(cè)量，采用Kalman濾波等算法對(duì)慣導(dǎo)誤差進(jìn)行估計(jì)，并對(duì)慣導(dǎo)進(jìn)行補(bǔ)償。其算法流程如圖1所示。

圖1 GPS/INS緊組合模式

GPS/INS緊組合導(dǎo)航狀態(tài)方程為

一般取15個(gè)狀態(tài)變量，即

式中，δVx、δVy、δVz為慣導(dǎo) 3 個(gè)軸向的速度誤差;δφ、δλ、δh分別為捷聯(lián)慣導(dǎo)緯度、經(jīng)度和高度誤差;δk為游移角誤差;Φx、Φy、Φz(mì)為平臺(tái)姿態(tài)角誤差;εx、εy、εz為陀螺漂移;δρ為接收機(jī)鐘偏對(duì)應(yīng)的偽距誤差;δρv為接收機(jī)頻漂對(duì)應(yīng)的偽距率誤差。

緊組合模式下，INS計(jì)算過(guò)程最主要的作用是通過(guò)計(jì)算姿態(tài)矩陣來(lái)給出載體的姿態(tài)和導(dǎo)航參數(shù)，以便將誤差傳遞到濾波過(guò)程，并使GPS接收機(jī)向INS提供精確的位置和速度信息，輔助并幫助克服INS的長(zhǎng)時(shí)間漂移誤差積累;INS同時(shí)向GPS接收機(jī)提供實(shí)時(shí)的位置和速度信息，輔助GPS接收機(jī)內(nèi)部的碼/載波跟蹤回路，提高GPS接收機(jī)的抗干擾能力和動(dòng)態(tài)跟蹤能力。

為了更好地對(duì)兩種算法進(jìn)行評(píng)估，應(yīng)對(duì)姿態(tài)誤差進(jìn)行量化，并簡(jiǎn)約到幾階以內(nèi)，可使結(jié)果保持在一個(gè)較好的精度水平。

1.方向余弦算法計(jì)算姿態(tài)矩陣誤差

式中，Ck為第k次計(jì)算b系相對(duì)于n系的方向余弦矩陣;Ak為循環(huán)變換間的方向余弦矩陣;σ=為時(shí)間間隔內(nèi)載體的旋轉(zhuǎn)量，取

對(duì)式(10)進(jìn)行指數(shù)項(xiàng)展開為

它的精度評(píng)估指標(biāo)定義為Ddc，Ddc/δt可用來(lái)度量計(jì)算姿態(tài)矩陣的漂移。單一軸旋轉(zhuǎn)情況下有

角增量(σ)最大值為0.1 rad 和0.05 rad 的情況下，假定載體最大角速度為10 rad/s，分別計(jì)算1、2、3、4階下的漂移誤差，可以得到姿態(tài)漂移誤差在不同階次使用方向余弦矩陣算法的大小變化，如圖2所示。

圖2 不同階次下方向余弦算法計(jì)算的姿態(tài)漂移誤差

2.四元數(shù)法計(jì)算姿態(tài)矩陣誤差

當(dāng)四元數(shù)表示的微分方程建立之后，可以通過(guò)在給定條件下用數(shù)值法來(lái)求解。設(shè)漂移誤差參數(shù)為Dq，其表達(dá)式定義為

其中

采用第1種算法在相同參數(shù)下，計(jì)算使用四元數(shù)算法的姿態(tài)漂移誤差，可以得到姿態(tài)漂移誤差在不同階次四元數(shù)算法的大小變化，如圖3所示。

四、結(jié) 論

1)通過(guò)以上對(duì)比分析可以得出，角增量最大值越小，計(jì)算的更新間隔越小，得到的精度也越高，這說(shuō)明減小更新間隔可以得到更好的精度。在考慮軸旋轉(zhuǎn)條件下，相同舍位階數(shù)下四元數(shù)方法計(jì)算后得到的姿態(tài)漂移值要小于方向余弦法得到的結(jié)果。比如，同樣計(jì)算三階，最大角增量為0.1 rad的情況下，使用四元數(shù)算法得到的姿態(tài)漂移誤差比方向余弦算法少 6.4°/h。

圖3 不同階次下四元數(shù)算法計(jì)算的姿態(tài)漂移誤差

2)從算法解算方程可以看出，方向余弦算法需要解求9個(gè)參數(shù)，參數(shù)之間有6個(gè)約束方程，同時(shí)要解9個(gè)一階微分方程，計(jì)算量大。而四元數(shù)所用參數(shù)為4個(gè)，參數(shù)之間有1個(gè)約束方程，而且四元數(shù)不論剛體處于任何狀態(tài)都不會(huì)退化，所得到的方程組線性化程度高。因此，在GPS/INS組合導(dǎo)航過(guò)程中，四元數(shù)方法在適當(dāng)環(huán)境下要優(yōu)于方向余弦矩陣算法。

［1］周坤芳，李德武，周湘蓉.干擾環(huán)境下GPS/INS組合模式的研究［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2004，12(4):24-27.

［2］郭杭，劉經(jīng)南.GPS/INS組合系統(tǒng)數(shù)據(jù)處理方法［J］.測(cè)繪通報(bào)，2002(2):21-23.

［3］陳哲.捷聯(lián)慣導(dǎo)系統(tǒng)原理［M］.北京:宇航出版社，1986.

［4］秦永元.慣性導(dǎo)航原理［M］.北京:科學(xué)出版社，2006.

［5］李斐，束蟬方，陳武.遙感技術(shù)中GPS/INS組合系統(tǒng)的應(yīng)用［J］.測(cè)繪通報(bào)，2004(12):1-4.

［6］鄭辛，付夢(mèng)印.SINS/GPS緊耦合組合導(dǎo)航［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(1):33-37.

［7］張曉亮.GPS/SINS組合導(dǎo)航系統(tǒng)應(yīng)用研究［D］.南京:南京理工大學(xué)，2013.

［8］GREWAL M，WEIL L，ANDREWS P.Global Positioning Systems，Inertial Navigation and Integration［M］.New York:Wiley，2001.

［9］TITTERTON D H，WESTON J L.Strapdown Inertial Navigation Technology［M］.London:Institution of Electrical Engineers，1997.

［10］WANG C.Development of a Low-cost GPS-based Attitude Determination System［D］.Calgavy:University of Calgary，2003.

［11］ROGERS R M.Applied Mathematics in Integrated Navigation Systems［M］.2rd ed.［S.l.］:American Institute of Aeronautics and Astronautics Inc.，2003.