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

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

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

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

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

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

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

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

0 引 言

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

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

1 算法總體結構

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

圖1 系統算法框架圖Fig.1 Frame diagram of whole method

2 MEMS輔助AFM 算法

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

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

(1)

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

(2)

(3)

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

(4)

(4)式中，n為觀測到的衛星顆數。

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

(5)

則最小二乘的殘差為

(6)

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

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

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

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

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

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

3.1 BDS/MEMS融合測姿濾波器設計

擴展卡爾曼濾波器估計最優姿態角所對應的四元數[11]，將旋轉四元數更新姿態的離散時間模型作為狀態方程

(7)

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

(8)

(9)

(10)

(11)

(12)

3.2 自適應協方差構建

3.2.1 加速度協方差構建

當載體處于靜止狀態時，其加速度等于重力加速度，即

(13)

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

(14)

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

3.2.2 四元數協方差構建

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

(15)

(16)

3.3 根據四元數求對應的姿態角

根據四元數理論，求擴展卡爾曼濾波估計出的最優四元數對應的姿態角計算公式為

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

(17)

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

4 實驗驗證及結果分析

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

4.1 靜態測試

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

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

次數測試算法總歷元數求解成功歷元數求解成功率/%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 動態測試

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

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

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

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

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

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

4.3 遮擋測試

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

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

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

5 結束語

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

[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松組合導航姿態估計[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-),女，重慶人，教授，研究生導師，主要研究方向為寬帶網絡技術。E-mail:jiangq@cqupt.edu.cn。

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

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

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

(編輯：王敏琦)

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