蔣 青，張躍仲

(重慶郵電大學(xué) 移動(dòng)通信技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065)

一種高精度的BDS/INS緊耦合測(cè)姿算法*

蔣 青，張躍仲*

(重慶郵電大學(xué) 移動(dòng)通信技術(shù)重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400065)

針對(duì)“北斗”衛(wèi)星姿態(tài)測(cè)量系統(tǒng)在測(cè)姿過(guò)程中測(cè)姿精度和穩(wěn)定性不高的問(wèn)題，提出了“北斗”衛(wèi)星導(dǎo)航系統(tǒng)(BDS)/慣性導(dǎo)航系統(tǒng)(INS)緊耦合姿態(tài)測(cè)量算法。該算法首先利用BDS觀測(cè)量設(shè)計(jì)了BDS系統(tǒng)測(cè)姿誤差模型；然后以INS狀態(tài)誤差方程為濾波系統(tǒng)狀態(tài)方程，以載波相位為主要觀測(cè)量設(shè)計(jì)了擴(kuò)展卡爾曼濾波器，利用濾波器的輸出實(shí)現(xiàn)對(duì)慣性導(dǎo)航測(cè)姿系統(tǒng)的輔助校正；最后采用靜態(tài)測(cè)試、動(dòng)態(tài)測(cè)試和遮擋測(cè)試驗(yàn)證該算法。該系統(tǒng)可以有效提高BDS測(cè)姿精度與輸出頻率，并且在靜態(tài)條件下航向角測(cè)量精度可以達(dá)到0.15°。

“北斗”導(dǎo)航系統(tǒng)；慣性導(dǎo)航系統(tǒng)；組合姿態(tài)測(cè)量；載波相位；擴(kuò)展卡爾曼濾波

1 引 言

近年來(lái)，隨著全球衛(wèi)星導(dǎo)航系統(tǒng)的高速發(fā)展，利用衛(wèi)星對(duì)載體進(jìn)行姿態(tài)測(cè)量越來(lái)越受到人們的重視，而整周模糊度的求解是其中最重要的問(wèn)題。雖然最小二乘模糊度去相關(guān)平差法[1](Least-squares Ambiguity Decorrelation Adjustment，LAMBDA)、約束最小二乘模糊度去相關(guān)平差法[2](Constrained-LAMBDA,CLAMBDA)以及改進(jìn)最小二乘模糊度去相關(guān)平差法[3](Modified-LAMBDA，MLAMBDA)解決了利用載波相位進(jìn)行姿態(tài)測(cè)量出現(xiàn)的整周模糊度求解的問(wèn)題，但是整體算法復(fù)雜，無(wú)法保證較高頻率的姿態(tài)信息輸出。此外，利用衛(wèi)星進(jìn)行姿態(tài)測(cè)量還受限于衛(wèi)星信號(hào)強(qiáng)度、觀測(cè)條件等外界因素。

隨著微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)慣性器件的不斷發(fā)展，國(guó)內(nèi)外針對(duì)微型慣性測(cè)量單元(Micro Inertial Measurement Unit，MIMU)測(cè)姿的研究與應(yīng)用越來(lái)越多，并在此基礎(chǔ)上進(jìn)行了與衛(wèi)星組合測(cè)姿的方法研究。文獻(xiàn)[4]通過(guò)GPS/陀螺組合系統(tǒng)中陀螺的輔助參數(shù)，對(duì)整周模糊度搜索空間進(jìn)行優(yōu)化提高了組合系統(tǒng)的效率和實(shí)用性。文獻(xiàn)[5]利用陀螺儀數(shù)據(jù)設(shè)計(jì)LAMBDA算法的約束條件，獲取更準(zhǔn)確的整周模糊度，提高系統(tǒng)的初始化速率和姿態(tài)角精度。文獻(xiàn)[6]通過(guò)設(shè)計(jì)以偽距和偽距率為觀測(cè)量的融合濾波方案實(shí)現(xiàn)對(duì)衛(wèi)星測(cè)姿系統(tǒng)的輔助校正，提高系統(tǒng)精度。由于偽距觀測(cè)量存在較大誤差，因此利用偽距為觀測(cè)量時(shí)會(huì)導(dǎo)致組合系統(tǒng)的精度較低，不能夠?qū)崟r(shí)提供高精度姿態(tài)信息。

本文在分析現(xiàn)有組合測(cè)姿技術(shù)的基礎(chǔ)上，提出了一種以載波相位為主要觀測(cè)量，設(shè)計(jì)BDS/INS組合方案，提高系統(tǒng)方案中的參數(shù)精度，實(shí)現(xiàn)對(duì)BDS測(cè)姿系統(tǒng)的高精度校正,最后利用單頻“北斗”接收機(jī)和9軸慣性傳感器設(shè)計(jì)了仿真測(cè)試平臺(tái)進(jìn)行數(shù)據(jù)實(shí)測(cè)仿真，驗(yàn)證了該方案的可行性。

2 BDS測(cè)姿及誤差模型

線性化的“北斗”衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou Navigation Satellite System,BDS)雙差觀測(cè)方程為

y=Aa+Bb+e。

(1)

式中：y為m×1的載波和偽距的觀測(cè)量和計(jì)算量的雙差值,a為雙差后的載波相位整周模糊度向量且有a∈n，b為基線向量坐標(biāo)且有b∈3[7]。

利用基線長(zhǎng)度作為約束條件，采用二次特征值法求解約束浮點(diǎn)解如下：

(2)

(3)

由式(3)可以看出，通過(guò)整周模糊度固定解解出的姿態(tài)信息具有較高精度，用模糊度精度因子(Ambiguty Dilution of Precision，ADOP)來(lái)表示整周模糊度的精度衰減因子[8]：

(4)

(5)

式中：σP、σΦ分別為偽距和載波相位的標(biāo)準(zhǔn)差，λf為衛(wèi)星信號(hào)頻率為f時(shí)的波長(zhǎng)，n為衛(wèi)星顆數(shù)，m為基線向量個(gè)數(shù)，精度值可以表示為

(6)

利用ADOP和幾何精度因子(Geometric Dilution of Precision，GDOP)乘積來(lái)表示浮點(diǎn)解精度，用ζ來(lái)表示在當(dāng)前搜索空間中搜索出的固定解的精度，將兩項(xiàng)誤差量結(jié)合來(lái)表示BDS測(cè)姿結(jié)果的精度ξ：

ξ=ADOP×GDOP×eζ，

(7)

3 INS誤差處理模型

慣導(dǎo)系統(tǒng)采用由3個(gè)單軸陀螺儀、1個(gè)三軸加速度計(jì)和1個(gè)三軸磁力計(jì)組成的MIMU器件，其中3個(gè)單軸陀螺儀采用的是芬蘭VTI公司制造的2個(gè)SCR1100和1個(gè)SCC1300，加速度計(jì)采用的是SCC1300內(nèi)置加速度計(jì)。此外，MIMU的輸出頻率為20～100 Hz，本次測(cè)試采用的是20 Hz輸出。

由于MIMU在出廠時(shí)經(jīng)過(guò)嚴(yán)格的零偏誤差、刻度因子誤差以及交叉耦合誤差校正，所以在對(duì)MIMU進(jìn)行誤差處理時(shí)主要處理隨機(jī)誤差。在由MIMU和BDS組成的捷聯(lián)導(dǎo)航系統(tǒng)中，從外部給平臺(tái)輸入姿態(tài)角Φ=[θ,γ,ψ]T，其中θ為俯仰角，γ為橫滾角，ψ為航向角，然后根據(jù)MIMU自身輸出值可以計(jì)算出其自身的姿態(tài)信息，ΦMIMU=[θimu,γimu,ψimu]T，則有

Φ=HΦMIMU。

(8)

式中：H為MIMU器件與BDS平臺(tái)的載體坐標(biāo)系之間的旋轉(zhuǎn)矩陣，根據(jù)姿態(tài)角差值可以直接計(jì)算出旋轉(zhuǎn)矩陣H[9]。

4 緊耦合算法設(shè)計(jì)

本文采用擴(kuò)展卡爾曼濾波方程(Extended Kalman Filtering，EKF)對(duì)BDS和MIMU進(jìn)行數(shù)據(jù)融合，實(shí)現(xiàn)系統(tǒng)間的緊耦合測(cè)姿方案。

4.1 狀態(tài)方程

在系統(tǒng)融合方案中我們采用INS系統(tǒng)的誤差方程作為系統(tǒng)融合方案的狀態(tài)方程：

(9)

式中：X(t)=[δV,δΦ,δp,δε,a]T，δV=[δvx,δvy,δvz]T分別為載體(捷聯(lián)慣導(dǎo)平臺(tái))的運(yùn)動(dòng)速度在東北天3個(gè)方向上的誤差量,δΦ=[δθ,δγ,δψ]分別為載體俯仰角、橫滾角和航向角的3個(gè)姿態(tài)角的狀態(tài)誤差量，其中δp=[δL,δλ,δh]為緯度、經(jīng)度和高度的狀態(tài)誤差量,δε和a分別為陀螺和加速度計(jì)在3個(gè)軸向的隨機(jī)漂移；F(t)為INS誤差狀態(tài)轉(zhuǎn)移矩陣。

4.2 觀測(cè)方程

系統(tǒng)觀測(cè)量主要采用載波相位和偽距率兩個(gè)觀測(cè)量，由于載波相位觀測(cè)值精度較高可以達(dá)到毫米級(jí)，所以在精確求解整周模糊度的情況下，構(gòu)建基于載波相位的觀測(cè)方程可以保證較高精度。

4.2.1 載波相位觀測(cè)方程

(10)

取φi=(Φi+Ni)λ，設(shè)(δx,δy,δz)為慣導(dǎo)給出的飛行器在地球坐標(biāo)系中的位置誤差，則

(11)

式中：下標(biāo)為I表示慣導(dǎo)給出的值，無(wú)下標(biāo)者表示理想值。用泰勒公式將公式(10)在(x,y,z)展開(kāi)，并忽略二階等高階項(xiàng)，可得

(12)

其中：

(13)

(14)

(15)

(16)

則可將公式(13)寫為

(17)

BDS接收機(jī)測(cè)得的第i顆衛(wèi)星的距離可表示為

(18)

由于系統(tǒng)采用基線較短，忽略電離層、對(duì)流層等誤差值，式(1)中的載波相位雙差觀測(cè)方程可以寫為

(19)

兩個(gè)接收機(jī)天線分別1和2，假設(shè)天線2為主天線且天線位置已知，天線1的位置由INS給出為(x,y,z)，根據(jù)公式(11)～(18)可得

(20)

對(duì)上式進(jìn)行化簡(jiǎn)可得

(21)

由于式中[δx,δy,δz]是慣導(dǎo)給出的天線1的位置在大地坐標(biāo)系下的位置誤差，而慣導(dǎo)的狀態(tài)方程中的位置誤差為[δL,δλ,δh]，上式需要進(jìn)行如下坐標(biāo)轉(zhuǎn)換：

(22)

式中：Da為旋轉(zhuǎn)矩陣。則載波相位觀測(cè)方程為

Z1=H1(t)X(t)+Vρ。

(23)

式中：觀測(cè)矩陣H1(t)=[04×6δe·Da04×7Dρ]，Dρ=[1 1 1 1]T。

4.2.2 偽距率觀測(cè)方程

接收機(jī)位置(取天線2的位置)(x,y,z)至第i顆衛(wèi)星距離變化率表示為

(24)

慣導(dǎo)給出的接收機(jī)位置到第i顆衛(wèi)星距離變化為

(25)

將公式(25)進(jìn)行化簡(jiǎn)做差可得[10-12]

(26)

(27)

(28)

5 數(shù)據(jù)仿真及分析

為了驗(yàn)證文本算法在實(shí)際應(yīng)用環(huán)境中的測(cè)量精度，在重慶郵電大學(xué)逸夫科技樓頂樓(29.531 6°N，106.584 9°E)進(jìn)行測(cè)試。為了直觀體現(xiàn)測(cè)試效果，采取利用航向角代替3個(gè)姿態(tài)角來(lái)進(jìn)行仿真驗(yàn)證。測(cè)試平臺(tái)如圖1所示，主要包括雙BDS天線和NovAtel OEM617D接收機(jī)、MIMU、測(cè)試回轉(zhuǎn)臺(tái)以及PC。

圖1 測(cè)姿平臺(tái)Fig.1 Attitude measurement platform

5.1 靜態(tài)測(cè)試

為了驗(yàn)證組合系統(tǒng)的性能，靜態(tài)測(cè)試選擇的是觀測(cè)環(huán)境(天氣因素)較差時(shí)進(jìn)行的。在此環(huán)境下，BDS的觀測(cè)噪聲較大，觀測(cè)量精度降低，求解整周模糊度時(shí)會(huì)出現(xiàn)波動(dòng)，導(dǎo)致無(wú)法保證系統(tǒng)姿態(tài)角的求解精度。

圖2 BDS靜態(tài)姿態(tài)角Fig.2 The BDS attitude of static condition

圖3 組合系統(tǒng)靜態(tài)姿態(tài)角Fig.3 The integrated system attitude of static condition

5.2 動(dòng)態(tài)測(cè)試

利用轉(zhuǎn)臺(tái)進(jìn)行旋轉(zhuǎn)來(lái)測(cè)試系統(tǒng)的動(dòng)態(tài)性能，當(dāng)轉(zhuǎn)臺(tái)旋轉(zhuǎn)一周時(shí)系統(tǒng)的仿真結(jié)果如圖4和圖5所示。

圖4為BDS和組合系統(tǒng)的動(dòng)態(tài)情況下(測(cè)試回轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)一周)的仿真結(jié)果對(duì)比圖。從圖中可以發(fā)現(xiàn)，在正常觀測(cè)條件下，兩個(gè)系統(tǒng)的姿態(tài)角誤差均比較小，無(wú)法通過(guò)姿態(tài)角進(jìn)行直觀對(duì)比。

圖4 BDS和組合系統(tǒng)動(dòng)態(tài)姿態(tài)角Fig.4 The attitude of BDS and integrated system under rotation

圖5為BDS和組合系統(tǒng)在動(dòng)態(tài)情況下的姿態(tài)角誤差對(duì)比圖。從圖中可以看出BDS在載體處于動(dòng)態(tài)情況下，姿態(tài)角誤差明顯增大；而組合系統(tǒng)的姿態(tài)角誤差在動(dòng)態(tài)情況下雖然有所增加，但是相比于BDS，較大程度地降低了姿態(tài)角誤差，提高了系統(tǒng)的穩(wěn)定性。

圖5 BDS和組合系統(tǒng)的動(dòng)態(tài)姿態(tài)角誤差Fig.5 The attitude error of BDS and integrated system under rotation

5.3 遮擋測(cè)試

為了模擬在衛(wèi)星信號(hào)受到遮擋時(shí)的情況，利用電路板的材質(zhì)特性，采用手持雙層廢棄電路板(25 cm×30 cm左右)置于主天線上方的方式，對(duì)衛(wèi)星信號(hào)進(jìn)行遮擋。

圖6為在遮擋情況下觀測(cè)到的衛(wèi)星顆數(shù)，其中紅色為主天線觀測(cè)衛(wèi)星數(shù)，綠色為有效衛(wèi)星數(shù)。通常情況下，接收機(jī)觀測(cè)到的衛(wèi)星顆數(shù)為5顆以上，在圖中的第700～800歷元間，信號(hào)發(fā)生遮擋衛(wèi)星顆數(shù)降低，遮擋嚴(yán)重時(shí)，衛(wèi)星顆數(shù)會(huì)下降至4顆及4顆以下，BDS開(kāi)始無(wú)法正常工作。

圖6 遮擋情況下的衛(wèi)星顆數(shù)Fig.6 The number of available satellite under obscured condition

圖7為BDS測(cè)姿結(jié)果與組合測(cè)姿結(jié)果的對(duì)比圖，在第700～800歷元之間，有效衛(wèi)星顆數(shù)小于4顆，BDS開(kāi)始失效，無(wú)法輸出姿態(tài)角;而組合系統(tǒng)通過(guò)濾波方案，依靠INS進(jìn)行參數(shù)更新，可以維持一定時(shí)間內(nèi)的高精度和高頻率的姿態(tài)角輸出。

圖7 遮擋情況下BDS與組合系統(tǒng)航向角Fig.7 The attitude of BDS and integrated system under obscured condition

5.4 測(cè)試分析

按照上述測(cè)試方法分別在靜態(tài)和動(dòng)態(tài)狀態(tài)下進(jìn)行多次測(cè)試，通過(guò)與系統(tǒng)提供的姿態(tài)角進(jìn)行比較，分別統(tǒng)計(jì)每次測(cè)試的標(biāo)準(zhǔn)差，將BDS測(cè)姿結(jié)果和組合測(cè)姿結(jié)果進(jìn)行比較，如表1所示。

表1 BDS測(cè)姿系統(tǒng)與組合測(cè)姿系統(tǒng)的標(biāo)準(zhǔn)差比較Tab.1 Comparison of the standard deviation between BDS attitude and fusion system attitude

從表1中的統(tǒng)計(jì)結(jié)果可以看出無(wú)論靜態(tài)還是動(dòng)態(tài)情況下，BDS系統(tǒng)輸出的姿態(tài)角在經(jīng)過(guò)組合系統(tǒng)優(yōu)化濾波后，都可以在一定程度上提高系統(tǒng)的姿態(tài)角精度和輸出頻率，提高系統(tǒng)的穩(wěn)定性和實(shí)時(shí)性。即使在衛(wèi)星信號(hào)發(fā)生遮擋，BDS無(wú)法正常工作時(shí)，在短時(shí)間內(nèi)組合系統(tǒng)仍能提供高精度、高頻率的姿態(tài)角信息，大幅改善系統(tǒng)的可用性。

6 結(jié)束語(yǔ)

本文提出的利用載波相位為主要觀測(cè)量的BDS/INS緊耦合測(cè)姿算法有效地提高了目前“北斗”測(cè)姿中的姿態(tài)角精度以及輸出頻率。該算法在觀測(cè)量選取上沒(méi)有采用較為普遍的偽距與偽距率的組合方式，而是采用載波相位和偽距率的觀測(cè)量組合，因此在較大程度上提高了觀測(cè)量精度，從而提高系統(tǒng)姿態(tài)角的求解精度。不同環(huán)境下的多次測(cè)試結(jié)果表明，該算法可以有效提高BDS的測(cè)姿精度和數(shù)據(jù)頻率。但是,本文的BDS測(cè)姿系統(tǒng)與融合測(cè)姿系統(tǒng)都是在載體處于低速運(yùn)動(dòng)的假設(shè)下，對(duì)于高動(dòng)態(tài)載體如炮彈等的姿態(tài)計(jì)算，還需要對(duì)融合系統(tǒng)進(jìn)行進(jìn)一步的研究與改善。

[1] TEUNISSEN P J G. The least-squares ambiguity decorrelation adjustment:a method for fast GPS integer ambiguity estimation[J].Journal of Geodesy,1995,70(70):65-82.

[2] 汪鐿林,田增山. 等式約束最小二乘在“北斗”姿態(tài)測(cè)量中的應(yīng)用[J].電訊技術(shù),2016,56(7)：760-764. WANG Yilin,TIAN Zengshan. Application of equality constrained least squares in BDS attitude determination[J].Telecommunication Engineering,2016,56(7):760-764.(in Chinese)

[3] CHANG X W,YANG X,ZHOU T. MLAMBDA:a modified LAMBDA method for integer least-squares estimation[J].Journal of Geodesy,2005,79(9):552-565.

[4] WANG C. Development of an integrated low-cost GPS/rate gyro system for attitude determination[J].Journal of Navigation,2004,57(1):85-101.

[5] ZHU J,TAO L,WANG J,et al. Rate-gyro-integral constraint for ambiguity resolution in GNSS attitude determination applications[J].Sensors,2013,13(6):7979-99.

[6] CONG L,LI E,QIN H,et al. A performance improvement method for low-cost land vehicle GPS/MEMS-INS attitude determination[J].Sensors,2015,15(3):5722-46.

[7] GIORGI G. GNSS carrier phase-based attitude determ- ination estimation and applications[D].Delft,Nether lands：Delft University of Technology,2011:113-116.

[8] TEUNISSEN P,ODIJK D,DE JONG C. Ambiguity dilution of precision:an additional tool for GPS quality control[J].LGR-Series，2000(21):261-270.

[9] 翟海廷,叢麗,秦紅磊,等. 基于單天線的 MEMS-INS/GPS 組合定姿方法[J].計(jì)算機(jī)工程與設(shè)計(jì),2012,33(10):3999-4003. ZHAI Haiting,CONG Li,QIN Honglei,et al. Method for MEMS-INS /GPS integrated attitude determination based on single antenna[J].Computer Engineering and Design,2012,33(10):3999-4003.(in Chinese)

[10] 馬霞,王永勝. SINS/GPS/TACAN機(jī)載綜合導(dǎo)航定位系統(tǒng)設(shè)計(jì)[J].電訊技術(shù),2011,51(4):16-20. MA Xia,WANG Yongsheng.Design of an airborne integrated navigation and location system based on SINS/GPS/TACAN[J].Telecommunication Engineering,2011,51(4):16-20.(in Chinese)

[11] 崔留爭(zhēng). MEMS-SINS/GPS 組合導(dǎo)航關(guān)鍵技術(shù)研究[D].長(zhǎng)春：中國(guó)科學(xué)院長(zhǎng)春光學(xué)精密機(jī)械與物理研究所,2014:53-56. CUI Liuzheng. Research on the key technologies of MEMS-SINS/GPS integration navigation system[D].Changchun：Changchun Institute of Optics,Fine Mechanics and Physics,Chinese Academy of Sciences,2014:53-56.(in Chinese)

[12] 祝燕華,蔡體菁,楊卓鵬. MEMS-IMU/GPS 組合導(dǎo)航系統(tǒng)的實(shí)現(xiàn)[J].中國(guó)慣性技術(shù)學(xué)報(bào),2009,17(5):552-561. ZHU Yanhua,CAI Tijing,YANG Zhuopeng. MEMS-IMU/GPS integrated navigation system[J].Journal of Chinese Inertial Technology,2009,17(5):552-561.(in Chinese)

A High Precision Attitude Determination Algorithm for Tightly Coupled BDS/INS

JIANG Qing,ZHANG Yuezhong
(Chongqing Key Laboratory of Mobile Communications Technology,Chongqing University of Posts and Telecommunications,Chongqing 400065,China)

For the problem of low accuracy and stability in attitude determination of BeiDou Navigation Satellite System(BDS),this paper proposes a tightly coupled attitude determination algorithm based on BDS and Inertial Navigation System(INS). First,the error model of BDS attitude determination algorithm based on the BDS observation is designed. Then,the extended Kalman filtering(EKF) is designed with the error state equation as the system state equation and the carrier phase as main observation.And the filter output is used to correct the attitude determination of INS.Finally,static tests,dynamic tests and obscured tests are perform to verify the algorithm.The proposed algorithm can dramatically improve the accuracy,stability and output frequency of BDS attitude,and the accuracy of the yaw reaches 0.15°.

BeiDou navigation satellite system(BDS)；inertial navigation system (INS)；integration attitude determination;carrier phase;extended Kalman filtering(EKF)

2016-07-01；

2016-11-14 Received date:2016-07-01；Revised date：2016-11-14

國(guó)家自然科學(xué)基金資助項(xiàng)目(61301126);重慶郵電大學(xué)博士啟動(dòng)基金(A2012-33)

10.3969/j.issn.1001-893x.2017.02.005

蔣青，張躍仲.一種高精度的BDS/INS緊耦合測(cè)姿算法[J].電訊技術(shù)，2017,57(2):151-156.[JIANG Qing,ZHANG Yuezhong.A high precision attitude determination algorithm for tightly coupled BDS/INS[J].Telecommunication Engineering,2017,57(2):151-156.]

TN967;P228

A

1001-893X(2017)02-0151-06

蔣 青(1965—),女，重慶人，碩士，教授，主要研究方向?yàn)閷拵ЬW(wǎng)絡(luò)技術(shù)和信息理論;

張躍仲(1991—)，男，河南人，碩士研究生，主要研究方向?yàn)锽DS/INS組合姿態(tài)測(cè)量系統(tǒng)。

Email:hpuzhang@163.com

*通信作者：hpuzhang@163.com Corresponding author：hpuzhang@163.com