基于MEMS輔助的單基線北斗融合測姿算法

蔣 青，刁春帆，田增山，周 牧

(重慶郵電大學 移動通信重點實驗室 重慶 400065)

基于MEMS輔助的單基線北斗融合測姿算法

蔣 青，刁春帆，田增山，周 牧

(重慶郵電大學 移動通信重點實驗室 重慶 400065)

載體的姿態(tài)信息是導航的重要參數(shù)，隨著北斗衛(wèi)星導航系統(tǒng)(Beidou navigation satellite system, BDS)和微機電系統(tǒng) (micro-electro-mechanical systems，MEMS)慣性傳感器的發(fā)展與完善，高精度、低成本、自主化的融合測姿技術(shù)具有廣闊的應用前景，因此，提出MEMS輔助單基線北斗融合測姿算法。根據(jù)MEMS慣性傳感器解算出的姿態(tài)信息確定基線向量的搜索范圍，從而輔助模糊度函數(shù)法(ambiguity function method, AFM)減小整周模糊度搜索空間，提高整周模糊度快速求解的成功率和計算效率。將BDS輸出的姿態(tài)角信息作為觀測信息，對MEMS陀螺儀解算出的姿態(tài)信息進行實時校正，實現(xiàn)BDS和MEMS傳感器二者的數(shù)據(jù)融合算法。通過實測數(shù)據(jù)仿真驗證，該算法能夠解決信號失鎖帶來的整周模糊度求解困難的問題，并且測姿系統(tǒng)能在遮擋和動態(tài)等復雜環(huán)境下提供高質(zhì)量姿態(tài)測量結(jié)果。

北斗衛(wèi)星導航系統(tǒng)/微機電系統(tǒng)(BDS/MEMS)測姿；整周模糊度；模糊度函數(shù)法；擴展卡爾曼濾波器

0 引 言

如何充分利用各系統(tǒng)的優(yōu)點，實現(xiàn)北斗衛(wèi)星導航系統(tǒng)(Beidou navigation satellite system, BDS)和微機電系統(tǒng) (micro-electro-mechanical systems，MEMS)慣性傳感器信息融合，使姿態(tài)測量系統(tǒng)能在高動態(tài)環(huán)境下保證高可靠性和準確性已成為近年來的研究熱點。一個單基線的BDS測姿系統(tǒng)需要高效、可靠的整周模糊度求解算法，主要有基于模糊度域和基于位置域的搜索算法。模糊度域的搜索算法主要有最小二乘搜索算法[1]，最小二乘降相關(guān)平差(least squares ambiguity decor-relation adjustment, LAMBDA)算法[2]，帶約束的LAMBDA算法[3]等。然而這些算法主要是在高精度浮點解的基礎(chǔ)上提高姿態(tài)解算的正確率，并沒有解決在低精度浮點解情況下姿態(tài)解算錯誤的問題?；谖恢糜虻乃阉饕阅：群瘮?shù)法(ambiguity function method, AFM)[4]為代表，AFM算法雖然不受周跳影響，但其包含整個角度搜索空間，導致運算復雜，無法保證算法的實時性。近年來，利用慣性傳感器提高衛(wèi)星整周模糊度求解成功率的想法被提出，例如信息工程大學甘雨博士[5]對單全球定位系統(tǒng)(global position system，GPS)天線與三軸陀螺儀組合測姿進行了研究，但該方法受到陀螺漂移的制約，無法長時間保持系統(tǒng)的穩(wěn)定性。Roth等[6]提出了一種低成本慣性傳感器與磁力計信息用于提高衛(wèi)星的姿態(tài)測量性能的算法，但磁力計容易受到周圍磁場環(huán)境的影響，會導致利用磁力計輸出的測量值計算出的姿態(tài)角誤差增大，從而降低了系統(tǒng)測量精度?？哲姽こ檀髮W謝志成[7]對高超聲速飛行器全球定位系統(tǒng)/慣性導航系統(tǒng)(global positioning system /inertial navigation system，GPS/INS)組合測姿原理進行了理論推導。

傳統(tǒng)的BDS測姿算法在BDS信號受到建筑物遮擋，信號失鎖的情況下，不能保證整周模糊度求解成功率，即BDS系統(tǒng)無法保證穩(wěn)定提供高精度的姿態(tài)信息。同樣基于MEMS傳感器的測姿算法也未能解決傳感器長期使用累積誤差越來越嚴重的問題，也無法長期穩(wěn)定提供高精度姿態(tài)角。因此，本文提出了基于MEMS輔助的單基線北斗融合測姿算法，測試結(jié)果表明，整周模糊度的求解成功率能提升到85%以上，數(shù)據(jù)融合算法也提升了測姿系統(tǒng)的穩(wěn)定性，在短時BDS信號惡劣的條件下，仍然能夠持續(xù)、穩(wěn)定地提供高精度姿態(tài)信息。

1 算法總體結(jié)構(gòu)

本文主要有2個模塊，第1個模塊利用傳感器測量得到的姿態(tài)信息輔助AFM算法，減小搜索空間，從而大大降低運算的復雜度，提高整周模糊度的求解成功率；第2個模塊提出了一種基于擴展卡爾曼濾波的BDS/MEMS慣性傳感器融合姿態(tài)測量算法，克服BDS測姿系統(tǒng)因有效衛(wèi)星顆數(shù)不足，無法提供穩(wěn)定的高精度姿態(tài)角問題。算法框架如圖1所示。

圖1 系統(tǒng)算法框架圖Fig.1 Frame diagram of whole method

2 MEMS輔助AFM 算法

AFM是一種在坐標域進行搜索的算法，其原理是越接近正確的姿態(tài)角反推出的模糊度帶入適應度函數(shù)后其值越接近1[8]。MEMS姿態(tài)信息可以確定載體姿態(tài)角的搜索范圍，從而減小整周模糊度的搜索空間，大大減少計算量，提高整周模糊度的求解效率和有效性[9]。根據(jù)MEMS的姿態(tài)角(φI,θI)確定AFM算法搜索空間，其相應的誤差分別為Δφ和Δθ(均大于0)，即(φI-Δφ,φI+Δφ)為航向角的搜索空間，(θI-Δθ,θI+Δθ)為俯仰角的搜索空間，選取(τφ,τθ)作為搜索步長。搜索空間中根據(jù)搜索步長確定的任一姿態(tài)角組合(φ,θ)代入(1)式，求得對應的基線向量b為[10]

b=[LcosθsinφLcosθcosφLsinθ]T

(1)

(1)式中：L表示基線長度；(φ,θ)表示搜索空間中一組姿態(tài)角組合。雙差載波相位觀測方程為

(2)

(3)

將雙差整周模糊度的浮點解代入AFM算法中的適應度函數(shù)，其表示為

(4)

(4)式中，n為觀測到的衛(wèi)星顆數(shù)。

盡管減小了搜索空間，但是仍然會有幾組相似的適應度函數(shù)值接近最大值1，因此，需要通過幾個約束條件進行篩選才能確定最精確的整周模糊度。選取適應度函數(shù)取值大于0.9所對應的浮點解，再將這些浮點解取整后代入(5)式，通過最小二乘計算出相應的基線向量bLS，進而求出姿態(tài)角(φBDS,θBDS)。

(5)

則最小二乘的殘差為

(6)

綜上歸納出以下2個約束條件。

1)基線長度與真值的誤差小，即‖bLS‖與真值L的誤差不超過0.02L；

2)最小二乘殘差最小的x個候選解中選出適應度函數(shù)值最大的固定解。

候選解數(shù)量通過約束條件1)的篩選逐漸減少，再根據(jù)條件2)最終確定模糊度固定解。

利用確定的固定解代入(2)式，通過最小二乘算法求出姿態(tài)角。之后利用此姿態(tài)信息對應的四元數(shù)建立擴展卡爾曼濾波器的觀測方程。

3 基于EKF的BDS/MEMS融合測姿

3.1 BDS/MEMS融合測姿濾波器設(shè)計

擴展卡爾曼濾波器估計最優(yōu)姿態(tài)角所對應的四元數(shù)[11]，將旋轉(zhuǎn)四元數(shù)更新姿態(tài)的離散時間模型作為狀態(tài)方程

(7)

(7)式中：ωk為k時刻載體旋轉(zhuǎn)角速率；Qk-1為k-1時刻最優(yōu)姿態(tài)角估計值所對應的四元數(shù)；Ω(ωk)為4×4的反對稱矩陣；T為傳感器數(shù)據(jù)采樣間隔；wk為系統(tǒng)噪聲。

(8)

(9)

(10)

(11)

(12)

3.2 自適應協(xié)方差構(gòu)建

3.2.1 加速度協(xié)方差構(gòu)建

當載體處于靜止狀態(tài)時，其加速度等于重力加速度，即

(13)

(13)式中：‖·‖為取模運算；g≈9.8 m/s2。

(14)

(14)式中：ka1和ka2為權(quán)重因子；var()為求解方差函數(shù)；D為方差求解窗口大小。ka1，ka2和D的取值均由實驗得到。

3.2.2 四元數(shù)協(xié)方差構(gòu)建

Ratio值是模糊度檢驗和確認的重要參數(shù)。根據(jù)第2節(jié)的分析可知，每組候選姿態(tài)角對應一組模糊度浮點解，因此，姿態(tài)角搜索空間中的多組候選姿態(tài)角可以求出多組浮點解。選取一組浮點解使目標函數(shù)J取得最小值Jmin，同時，也可找到一組模糊度組合使Jsub取次小值。

(15)

(16)

3.3 根據(jù)四元數(shù)求對應的姿態(tài)角

根據(jù)四元數(shù)理論，求擴展卡爾曼濾波估計出的最優(yōu)四元數(shù)對應的姿態(tài)角計算公式為

θ=arcsin(-2(q0q1+q2q3))

(17)

(17)式中：γ為橫滾角；θ為俯仰角；φ為航向角。

4 實驗驗證及結(jié)果分析

2015年6月9日在重慶郵電大學逸夫科技樓頂(北緯29.5°，東經(jīng)106.5°)搭建實驗平臺分別進行靜態(tài)和動態(tài)測試驗證如上算法的可行性。讓載體坐標系下的y軸與2個BDS天線組成的基線方向重合，固定在鐵桿上，放置在水平桌面，如圖2所示。MEMS慣性測量單元包含三軸加速度計感器(STLIS3DH)和三軸陀螺儀(STL3G4200D)。測試過程中，基線長度為1.108 m，BDS接收機數(shù)據(jù)輸出頻率為1 Hz，MEMS傳感器的原始數(shù)據(jù)采樣頻率為20 Hz。

4.1 靜態(tài)測試

模糊度求解成功率的高低是決定BDS測姿算法能否解算出正確姿態(tài)角的關(guān)鍵因數(shù)之一。在靜態(tài)條件下，保存BDS接收機和MEMS傳回的數(shù)據(jù)，并通過時間戳進行數(shù)據(jù)同步。2種算法整周模糊度求解成功率如表1所示，對5次仿真結(jié)果的統(tǒng)計發(fā)現(xiàn)，MEMS輔助AFM算法能夠在很大程度上提升整周模糊度的求解成功率，成功率一般在85%以上，大大高于LAMBDA算法。

圖2 融合測姿硬件Fig.2 Fusion attitude measurement hardware

次數(shù)測試算法總歷元數(shù)求解成功歷元數(shù)求解成功率/%1LAMBDAMEMS?AFM輔助77542862664436．9185．682LAMBDAMEMS?AFM輔助1827 341182618．6599．93LAMBDAMEMS?AFM輔助28961361289346．9999．94LAMBDAMEMS?AFM輔助20701492207072．08100 5LAMBDAMEMS?AFM輔助1534 875153357．0499．93

4.2 動態(tài)測試

仿真驗證動態(tài)情況下，EKF融合算法與BDS測姿算法的動態(tài)性能如圖3所示。

從圖3a中可以看出，由于受到接收信號不穩(wěn)定影響，整周求解的成功率不高，導致解算出來的姿態(tài)角精度不高，特別是在載體處于運動狀態(tài)時，姿態(tài)解算出現(xiàn)了錯誤。并且由于BDS接收機的輸出頻率為1 Hz，不能很好展現(xiàn)載體的運動狀態(tài)，具體表現(xiàn)為在83歷元時，載體的航向角跨過360°，其姿態(tài)角沒有回到0°。從圖3b可以看出，在單獨用BDS進行姿態(tài)解算出現(xiàn)的錯誤的歷元，經(jīng)過數(shù)據(jù)融合后解算出正確姿態(tài)角，解決了因為BDS信號質(zhì)量不佳導致的姿態(tài)解算不正確的問題。并且由于數(shù)據(jù)融合后輸出頻率提高到20 Hz，系統(tǒng)的動態(tài)性能得到了很大的提升，基本能展現(xiàn)載體整個運動過程。

圖3 動態(tài)情況下BDS和融合算法解算出來的航向角Fig.3 Heading of BDS and fusion algorithm under dynamic circumstance

接下來主要分析數(shù)據(jù)融合后的姿態(tài)精度。在動態(tài)環(huán)境下分別采用BDS單獨測姿和MEMS輔助單基線BDS融合測姿，統(tǒng)計俯仰角和橫滾角均方根誤差如表2所示。

表2 BDS及MEMS輔助BDS測姿的俯仰角和橫滾角均方根誤差

從表2可以看出，通過MEMS輔助單基線BDS測姿后，航向角和俯仰角的精度都有明顯提高。輔助后的BDS/MEMS融合測姿算法更能滿足人們在動態(tài)環(huán)境下對高精度的需求。

4.3 遮擋測試

遮擋實驗是在靜態(tài)試驗的基礎(chǔ)上使用隔離板對其中一個BDS天線進行遮擋持續(xù)大約100 s，比較融合算法和BDS測姿算法姿態(tài)解算的結(jié)果，如圖4所示。

圖4 遮擋情況下融合算法前后航向角和俯仰角對比Fig.4 Comparison of heading and pitch under blocking circumstance

從圖4a和圖4c可以看出，在0～400歷元，載體處于靜止狀態(tài)，也沒有對天線進行遮擋，但采用單獨BDS算法進行姿態(tài)測量時，解算出的航向角和俯仰角均出現(xiàn)了錯誤的情況。而在第700歷元附近時，因人為對某個天線進行了遮擋，北斗信號質(zhì)量非常差，甚至出現(xiàn)無法接收到北斗衛(wèi)星信號的情況，導致可觀測到的衛(wèi)星顆數(shù)不足，BDS測姿系統(tǒng)無法正常工作。當遮擋結(jié)束后，雙天線觀測到的有效衛(wèi)星顆數(shù)超過4顆，BDS測姿模塊重新開始正常工作，但整周模糊度未能快速鎖定，導致輸出姿態(tài)角精度不高，甚至錯誤。從圖4b和圖4d可以看出，通過EKF算法將MEMS數(shù)據(jù)與BDS衛(wèi)星數(shù)據(jù)融合后，由于MEMS能在較短時間不受累計誤差影響，仍能輸出高精度姿態(tài)角，并且還可以利用該姿態(tài)角信息輔助整周模糊度的快速鎖定。在0～400歷元間均解算出正確的航向角和俯仰角，沒有出現(xiàn)在某個歷元解算錯誤的情況。并且在第700歷元附近時，BDS測姿系統(tǒng)無法正常工作的情況下，仍然能穩(wěn)定地提供正確姿態(tài)信息。

5 結(jié)束語

本文提出了一種利用MEMS信息輔助BDS求解整周模糊度和BDS/MEMS融合測姿的算法，利用MEMS傳感器系統(tǒng)解算出來的姿態(tài)角縮小了整周模糊度的搜索空間，大大減少了AFM算法的計算量?；贓KF的融合算法，姿態(tài)角的輸出頻率高于單一的BDS測姿系統(tǒng)，所以動態(tài)性能好，同時，不僅提高了測姿系統(tǒng)在靜態(tài)和動態(tài)情況下的精度，而且也能在BDS信號受到外界條件干擾的情況下持續(xù)提供較為精確的姿態(tài)角信息。未來的工作將在以下2個方面進行展開：① 研究在船載、森林等更加復雜的環(huán)境下提高算法測姿精度與穩(wěn)定性；②研究GPS/BDS/GLONASS/MEMS多系統(tǒng)的融合算法，利用各個系統(tǒng)優(yōu)勢互補，向高可靠性、高精度發(fā)展。

[1] ZHAO Z, GAO Q, HU W. An efficient method for carrier phase ambiguity resolution in GPS attitude determination[C]//IEEE.Wireless, Mobile and Multimedia Networks,2006 IET International Conference on IET. Piscataway, NJ：IEEE Press，2006:1-3.

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

[3] NADARAJAH N, TEUNISSEN P J G, RAZIQ N.Instantaneous GPS-Galileo attitude determination: single-frequency performance in satellite-deprived environments[J].Vehicular Technology, IEEE Transactions on, 2013, 62(7): 2963-2976.

[4] WANG Y, ZHAN X, ZHANG Y.Improved ambiguity function method based on analytical resolution for GPS attitude determination[J].Measurement Science and Technology, 2007, 18(9): 2895-2990.

[5] 甘雨,隋立芬,戚國賓,等.陸地導航中GNSS/陀螺儀組合實時測姿方法[J].武漢大學學報:信息科學版, 2015, 40(3):379-383. GAN Yu,SUI Lifen,QI Guobing,et al.A Real-Time Attitude Determination Approach for Land Navigation Based on GPS/Gyroscope Integration[J].Geomatics and Information Science of Wuhan University,2015,40(3):379-383.

[6] ROTH J, KASCHWICH C, TROMMER G F. Improving GNSS attitude determination using inertial and magnetic field sensors[J].Inside GNSS, 2013, 9(10): 54-62.

[7] 謝志成,雷虎民,張大元,等.高超聲速飛行器雙GPS/INS松組合導航姿態(tài)估計[J].飛行力學,2014,32(6):12-15. XIE Zhicheng，LEI Humin，ZHANG dayuan，et al.Estimation of loosely-coupled dual GPS /INS integrated navigation attitude for hypersonic flight vehicles[J]. Flight Dynamic, 2014,32(6):12-15.

[8] CHEN W, QIN H, ZHANG Y, et al. Accuracy assessment of single and double difference models for the single epoch GPS compass[J]. Advances in Space Research, 2012, 49(4): 725-738.

[9] CAPORALI A,DALLA T A,PRATICELLI N.Interferometric attitude and direction sensor using GPS carrier phase data[J].Reports on Geodesy,2003,65(2):173-193.

[10] ELING C, ZEIMETZ P, KUHLMANN H. Development of an instantaneous GPS/MEMS attitude determination system[J].GPS Solut，2013,17(1)：129-138.

[11] 羅磊,田增山,陳俊亞.EKF定位跟蹤算法研究[J].重慶郵電大學學報：自然科學版, 2009，21(1):50-52. LUO Lei,TIAN Zengshan,CHEN Junya. Algorithm of EKF positioning and tracking[J]. Journal of Chongqing University of Posts and Telecommunications:Natural Science Edition, 2009,21(1):50-52.

蔣 青(1965-),女，重慶人，教授，研究生導師，主要研究方向為寬帶網(wǎng)絡技術(shù)。E-mail:jiangq@cqupt.edu.cn。

刁春帆(1991-)，女，重慶人，碩士研究生，主要研究方向為無線定位與慣性導航。E-mail：409105807@qq.com。

田增山(1968-)，男，河南固始人，教授，博士，主要研究方向為移動通信、個人通信、GPS及蜂窩網(wǎng)定位系統(tǒng)及其應用技術(shù)研究。E-mail:tianzs@cqupt.edu.cn。

周 牧(1984-)，男，重慶人，教授，博士，主要研究方向為無線定位技術(shù)、機器學習與人工智能、凸優(yōu)化理論。

(編輯：王敏琦)

Single baseline Beidou fusion attitude determination algorithm based on MEMS inertial sensors

JIANG Qing, DIAO Chunfan, TIAN Zengshan, ZHOU Mu

(Chongqing Key Lab of Mobile Communication Technology, Chongqing University of Posts and Telecommunications,Chongqing 400065, P. R. China)

The attitude information of a platform is an important parameter for modern navigation. With the development and improvement of the Beidou navigation satellite system(BDS)and micro-electro-mechanical systems(MEMS)inertial sensor, high-precision, low-cost and independent fusion attitude determination technology has broad application prospects. Based on this,a single baseline Beidou fusion attitude determination algorithm aided by MEMS Inertial sensors has been proposed to solve the real-time attitude determination problem. Firstly, using the MEMS attitude information deduce the range of baseline vector in order to aide ambiguity function method (AFM), the success rate of fast ambiguity solution is increased. Secondly the BDS attitude is taken as the observation information to eliminate the MEMS gyroscope drift during long-time attitude measurement. The results of simulation test experiments on measured data demonstrate the algorithm can solve ambiguity resolution problem when the signal is unlock and can offer high precision attitude information under complex circumstance.

Beidou navigation satellite system/micro-electro-mechanical systems(BDS/MEMS) attitude determination; ambiguity resolution; ambiguity function method; extended Kalman filter

10.3979/j.issn.1673-825X.2016.06.009

2015-12-24

2016-10-08

刁春帆 409105807@qq.com

國家自然科學基金(61471077)

Foundation Item：The National Natural Science Foundation of China(61471077)

TN914.53

A

1673-825X(2016)06-0804-06