基于INS輔助CLAMBDA與AFM的GPS／INS組合導航測姿方法

叢 麗，李二翠，張立楊，秦紅磊，薛 瑞

（北京航空航天大學電子信息工程學院，北京100191）

基于INS輔助CLAMBDA與AFM的GPS／INS組合導航測姿方法

叢 麗，李二翠，張立楊，秦紅磊，薛 瑞

（北京航空航天大學電子信息工程學院，北京100191）

為了提高全球定位系統(tǒng)（global positioning system，GPS）定姿的性能及成功率，將帶約束的最小二乘降相關平差法（constrained least squares ambiguity decorrelation adjustment，CLAMBDA）與模糊度函數(shù)法（ambiguity function method，AFM）兩種算法相結(jié)合，同時利用慣性導航系統(tǒng)（inertial navigation system，INS）姿態(tài)信息進行輔助，提出了一種GPS／INS組合定姿方法。在CLAMBDA解算過程中，將INS姿態(tài)信息與GPS載波相位和碼偽距聯(lián)合解算浮點解，然后再進行固定解搜索，如果姿態(tài)解算錯誤，則用AFM算法得到的姿態(tài)角進行替換。其間，用INS姿態(tài)信息縮小AFM的搜索范圍。之后，GPS／INS進行組合濾波。通過實際系統(tǒng)跑車實驗表明，相比于原算法，該算法能有效提高GPS定姿的成功率。

全球定位系統(tǒng)；慣性導航系統(tǒng)；帶約束最小二乘降相關平差；模糊度函數(shù)法

0 引 言

如今，許多導航應用領域不但需要導航系統(tǒng)提供準確的位置、速度，還要提供準確的姿態(tài)。傳統(tǒng)的測姿方法使用慣性導航輸出的姿態(tài)角，但存在誤差隨時間不斷累加、長時間工作時姿態(tài)角精度較差的問題。由于利用全球定位系統(tǒng)（global positioning system，GPS）信號測姿具有體積小、成本低、無累積誤差、精度高等優(yōu)勢，已成為當前姿態(tài)測量的重要手段。然而，GPS定姿也存在自身缺點，如易受外界環(huán)境影響，在高動態(tài)時易于丟星等。因此，利用慣性導航對GPS進行輔助，可提高姿態(tài)測量的可靠性及準確性。

GPS測姿利用載波相位作為觀測量，雖然它的精度高，但存在相位的整周模糊。如何準確地確定整周模糊度，一直是載波相位高精度定姿的關鍵問題。在過去的十幾年中，人們對整周模糊度的求解進行了很多研究。如位置域的搜索，以模糊度函數(shù)法（ambiguity function method， AFM）［13］為代表，龐大的計算量是該類算法的瓶頸。模糊度域的搜索，有最小二乘搜索算法［4］，優(yōu)化Cholesky分解［5］，最小二乘降相關平差（least squares ambiguity decorrelation adjustment，LAMBDA）算法［6］等。當前最流行的是LAMBDA算法，它已經(jīng)被證明是性能較好、理論體系最為完善的算法之一［7］。在LAMBDA的基礎上，繼而發(fā)展成熟的帶約束的最小二乘降相關平差算法（constrained LAMBDA，CLAMBDA）［8-10］進一步提高了姿態(tài)解算成功率。然而CLAMBDA算法，只能將高精度的浮點解映射到正確的整數(shù)解，在浮點解精度較低時，姿態(tài)解算常會出錯［11］。不少相關研究者在GPS／慣性導航系統(tǒng)（inertial navigation system，INS）組合測姿方面展開研究［12-15］，但大多數(shù)GPS／INS組合測姿主要是利用組合濾波方法，沒有解決整周模糊度浮點解精度較低時，雙差整周模糊度搜索成功率低的問題。對此，本文提出了一種基于INS輔助GPS單頻單歷元姿態(tài)解算算法，將INS姿態(tài)觀測量和GPS的載波以及碼偽距觀測量聯(lián)合解算浮點解，再進行固定解搜索，解算姿態(tài)角。若姿態(tài)角解算錯誤，則用AFM解算的姿態(tài)角進行替換。然而，AFM算法搜索的范圍大，計算效率較低。因此，可以利用慣導輸出的姿態(tài)角來縮小AFM的搜索范圍，提高其計算效率。

1 CLAMBDA算法簡介

線性化的載波相位觀測方程可概括為

式中，y是觀測向量；a是整周模糊度向量；b是基線向量，屬于3維實數(shù)向量R3；A是整周模糊度向量的參數(shù)矩陣；B是基線向量的參數(shù)矩陣。

CLAMBDA算法通過使式（2）的目標函數(shù)最小來求得模糊度固定解。

式中

整周模糊度候選解的搜索空間為

式中，χ2表示搜索空間的大小。為了選擇一個合適的搜索空間，可以使用無約束LAMBDA算法尋找一個整數(shù)最小二乘解aILS，根據(jù)該解計算初始的空間大小如果空間內(nèi)無解，則將空間按照不斷擴大。此過程不斷進行，直到空間內(nèi)有解。

2 INS輔助單頻單歷元GPS解算浮點解

以A、B兩個天線為端點的短基線，對于第i顆衛(wèi)星的單差載波相位觀測方程和單差碼偽距觀測方程分別為

式中，λ是載波波長；ΔφiAB和ΔNiAB分別是單差小數(shù)部分載波相位和整周模糊度；si是第i顆衛(wèi)星的單位矢量。

式（4）矩陣模型為

為削減鐘差項βφAB，需進行Householder變換，式（6）兩邊左乘矩陣，得

式中，ˉe＝（1，1，…，1）T（m－1）×1；m是可見衛(wèi)星數(shù)目。將不含鐘差項的部分提取，有

式中，NSD和NDD分別表示單差和雙差整周模糊度向量。

則式（9）等價于

與式（10）類似，得到

定義σ1≡σφ／σρ，則有

原始的GPS測姿方法，是通過式（12），用最小二乘方法求得浮點解。為了提高浮點解的精度，本文提出了利用INS進行輔助求算浮點解，下面將進行具體介紹。

式中，ε為地理坐標系下基線矢量的觀測噪聲。

定義σ2≡σφ／σI，將式（13）兩邊同時乘以再和式（12）聯(lián)立可得

得到式（14）后，同樣采用最小二乘方法求得浮點解。也就是說，通過利用INS信息，增加了求浮點解的方程的冗余度來提高浮點解的精度，進而提高GPS定姿的成功率。同時，在INS輔助求算浮點解時，增加了權(quán)重的概念，在式（13）中乘以系數(shù)σ2，可以根據(jù)INS的性能調(diào)整該參數(shù)，當INS性能較差時，減小其在方程中的權(quán)重，當INS性能較好時，增大其在方程中的權(quán)重。

3 INS輔助AFM及CLMBDA與AFM的結(jié)合

雖然利用慣導信息與GPS的載波相位和碼偽距觀測量聯(lián)合，通過增加方程對未知數(shù)的冗余度，一定程度上提高了整周模糊度浮點解的解算精度。經(jīng)過實際驗證，此方法確實提高了姿態(tài)解算的成功率，但仍會有一部分姿態(tài)角解算錯誤。此時，我們可利用其他搜索方法解算姿態(tài)角。AFM的基本思想是對于正確的姿態(tài)角，雙差方程中的模糊度最接近整數(shù)。對于以A、B兩個天線為端點的短基線，其雙差載波相位觀測方程表述為

整周模糊度的浮點解為

對于N個雙差整周模糊度，AFM算法將觀測值適應度函數(shù)定義為

在搜索過程中，需要解決的一個關鍵問題是如何有效識別出高精度的整周模糊度浮點解。這就需要利用更多已知條件進行約束。其實，單獨使用AFM算法時，為了提高姿態(tài)角解算精度，將搜索到的姿態(tài)角代入式（17），可得到整周模糊度的浮點解，將浮點解進行四舍五入可得到模糊度的整數(shù)解，已知全部衛(wèi)星的整周模糊度后，就可代入雙差觀測方程式（15）中，采用最小二乘方法，計算高精度的基線向量，然后求得高精度的姿態(tài)角。設式（15）的線性形式為

則最小二乘的殘差為

由最小二乘法得到的殘差，可作為一個約束條件。這樣，對于正確的解，用其求得的基線向量應滿足如下約束條件：

（1）基線長度與真值的誤差很??；

（2）對于地面載體，基線向量的俯仰角通常一般在－10°～10°；

（3）最小二乘殘差最小或接近最小。

因此，可根據(jù)約束條件對模糊度候選解進行篩選，逐步縮小候選解個數(shù)。另外，對于約束條件（2），如果將其去掉，姿態(tài)解算的正確率只會稍有下降，影響不大。因此，本文提出的方法，同樣適用于非地面載體。

這樣，先用由慣導信息輔助的CLAMBDA算法解算載體姿態(tài)，CLAMBDA根據(jù)其適應度函數(shù)及基線長度約束條件選擇出正確的整周模糊度。同時增加了另外的約束條件，利用INS的短期高精度，CLAMBDA解算出的當前時刻的基線坐標與上一時刻GPS得到的基線坐標之間的夾角d1，與INS當前時刻得到的基線坐標與上一時刻INS得到的基線坐標之間的夾角d2，兩者相比較，如果d1與d2的差值相差太大，則舍棄候選解，如果相差很小，則保留。但當采用膨脹法時，得到的CLAMBDA候選解個數(shù)超出一定范圍，例如800時，仍未找到滿足CLAMBDA約束條件的整周模糊度，此時，即判斷姿態(tài)解算錯誤，然后再用由INS信息輔助的AFM得到的姿態(tài)角作為GPS接收機的輸出姿態(tài)角。

由于當浮點解精度較高時，用CLAMBDA搜索固定解時，找到正確解的時間會很短，效率高。因此，本文利用INS輔助提高浮點解精度，縮短了CLAMBDA的搜索時間。另外，降低了CLAMBDA終止條件的候選解個數(shù)，使得消耗在用質(zhì)量差的浮點解尋求固定解的時間減少，使其盡早進入INS輔助的AFM算法。原始的AFM算法的一個主要問題是計算量較大，搜索效率低。所以，使用INS縮小AFM搜索范圍，提高了AFM搜索效率。因此，整個算法的實時性可以得到保證。整體設計方案的框圖如圖1所示。

圖1 總體方案設計框圖

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

在低成本GPS接收機與慣導構(gòu)成的組合導航系統(tǒng)中，接收機采用Novatel的SUPERSTAR II，其測碼精度為0．75 m，差分載波相位精度為0．01 m，輸出頻率為1 Hz；慣導為Nov Atel生產(chǎn)的IMU-CPT，陀螺零偏穩(wěn)定性是0．03°／s，加速度計零偏穩(wěn)定性為2 m g，輸出頻率為100 Hz。為驗證GPS／INS測姿方法的性能，將組合導航系統(tǒng)固定于汽車載體上進行動態(tài)的跑車實驗，實驗地點為北京市的大屯路，時間為十分鐘左右。圖2為GPS各天線安裝方式示意圖，沿汽車縱向安裝的兩個天線確定的基線為主基線，其長度測量值為1．0 m；沿汽車橫向安裝的兩個天線確定的基線為輔基線，其長度測量值為0．9 m。圖3為本次跑車實驗路線圖，其與實際的跑車路線是相符合的。

圖2 GPS天線安裝示意圖

圖3 跑車實驗路線圖

為驗證GPS／INS組合測姿方法的性能，首先分析INS輔助GPS定姿方法的性能?；诒敬闻苘噷嶒灢杉臄?shù)據(jù)，分別使用單純的GPS單頻單歷元算法和INS輔助的GPS單頻單歷元算法解算主基線和輔基線矢量，其俯仰角、橫滾角、航向角如圖4～圖6所示。

圖4 兩種姿態(tài)解算方法的俯仰角

圖5 兩種姿態(tài)解算方法的橫滾角

圖6 兩種姿態(tài)解算方法的航向角

在實驗過程中，汽車均在較為平坦的路面上進行靜止或圓周運動，在一定的誤差允許范圍內(nèi)，兩個基線的俯仰角可以認為是恒定不變的。靜止情況下INS初始對準得出的載體俯仰角和橫滾角分別為2．5°和0．5°，以此作為主基線和輔基線的俯仰角真值的絕對值。通常情況下，基線長度為1 m左右，整周模糊度的搜索成功時，基線解算的姿態(tài)角誤差約為2°以內(nèi)，因此，本文以主基線俯仰角絕對值不大于4．5°，輔基線的俯仰角絕對值不大于2．5°為依據(jù)，來統(tǒng)計無慣導輔助的CLAMBDA、有INS輔助的AFM及基于INS輔助的CLAMBDA與AFM相結(jié)合的3種姿態(tài)解算方法所對應的載波相位整周模糊度搜索成功率，如表1所示。由于CLAMBDA算法只能將高精度的浮點解映射到正確的固定解，當浮點解精度不高時，CLAMBDA并不能很好的找出正確解。此次實驗用到的是低成本GPS接收機，性能很差。當利用它的載波和碼偽距進行定姿時，求得的浮點解精度是很差的。而且，此次實驗是動態(tài)實驗，更加影響CLAMBDA解算的成功率。因此CLAMBDA解算出的姿態(tài)成功率并沒有很高。

表1 3種姿態(tài)解算方法的載波相位整周模糊度搜索成功率

由圖4～圖6和表1可知，提出的基于INS輔助的CLAMBDA與AFM相結(jié)合的定姿算法，相對于單純的CLAMBDA算法和INS輔助的AFM算法在解算成功率上有顯著提高。當GPS接收機性能較差時，由于碼偽距的精度較低，導致GPS采用CLAMBDA單獨定姿時，成功率很低。當采用了INS輔助及與AFM結(jié)合的方法時，使得我們能夠有效找到正確的整周模糊度，解算出正確的姿態(tài)角，從而大大提高了姿態(tài)解算的成功率，這與理論分析相符。

為了進一步驗證組合定姿的性能，在同樣的地點，進行第2組實驗。通常情況下，高精度的慣導成本比較高，因此，此組實驗主要是采用更低成本的慣導，測試組合定姿的效果。接收機采用的是Novatel生產(chǎn)的OEMStar，其對于GPS的測碼精度為0．05 m，差分載波相位精度為0．006 m，輸出頻率為1 Hz；慣導為北京星網(wǎng)宇達生產(chǎn)的XWIMU5220，陀螺零偏穩(wěn)定性是0．02°／s，加速度計零偏穩(wěn)定性為8 mg，輸出頻率為100 Hz。

主基線的長度是0．98 m，輔基線的長度0．93 m，實驗時間同樣為10分鐘。我們分別使用單純的GPS單頻單歷元算法和INS輔助的GPS單頻單歷元算法解算主基線和輔基線矢量，其俯仰角、橫滾角和航向角分別如圖7～圖9所示。

圖7 兩種姿態(tài)解算方法所得俯仰角

圖9 兩種姿態(tài)解算方法所得航向角

接下來統(tǒng)計了有無INS輔助時GPS姿態(tài)解算的成功率，如表2所示。

表2 兩種姿態(tài)解算的成功率

由圖7～圖9以及表2可以看出，有INS輔助的GPS定姿比無INS輔助的GPS定姿，成功率有了大幅度的提高。

5 結(jié) 論

本文提出了一種GPS／INS緊密組合測姿方法。該方法首先將INS姿態(tài)信息和GPS的載波相位和碼偽距觀測量聯(lián)合，增加方程對未知數(shù)的冗余度，解算浮點解；同時利用CLAMBDA與AFM相結(jié)合進行搜索。對于用CLAMBDA解算失敗的情況下，用AFM進行姿態(tài)角的解算，并用INS姿態(tài)信息縮小AFM的搜索范圍，減小計算量，提高效率。經(jīng)過實際系統(tǒng)跑車實驗證明了由于INS姿態(tài)信息的輔助，GPS單頻單歷元姿態(tài)解算方法的整周模糊度搜索成功率顯著提高。在后續(xù)工作中，需要做的是進一步提高姿態(tài)角的精度。這可以通過設計濾波器，增加對陀螺和加速度計的誤差量進行濾波來實現(xiàn)，當出現(xiàn)丟星，GPS數(shù)據(jù)不可用時，載體姿態(tài)仍會維持的較好。

［1］Chen W T，Qin H L，Cong L，et al．Error analysis and improve of the ambiguity function method based on the intersection point on sphere［J］．Journal of Electronics and Information Technology，2010，32（9）：2220- 2225．（陳萬通，秦紅磊，叢麗，等．基于球面交點的AFM算法的誤差分析與改進方法［J］．電子與信息學報，2010，32（9）：2220- 2225．）

［2］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）：2985- 2990．

［3］Eling C，Zeimetz P，Kuhlmann H．Development of an instantaneous GNSS／MEMSattitude determination system［J］．GPSSolutions，2013，17（1）：129- 138．

［4］Wang C．Development of a low-cost GPS-based attitude determination system［D］．Calgary：University of Calgary，2003．

［5］Liu N，Xiong Y L，F(xiàn)eng W，et al．An algorithm for rapid integer ambiguity resolution in single frequency GPS kinematical positioning［J］．Acta Geodaetica et Cartographica Sinica，2013，42（2）：211- 217．（劉寧，熊永良，馮威，等．單頻GPS動態(tài)定位中整周模糊度的一種快速解算方法［J］．測繪學報，2013，42（2）：211- 217．）

［6］Teunissen P J G．Integer least-squares theory for the GNSS compass［J］．Journal of Geodesy，2010，84（7）：433- 447．

［7］Verhagen S，Teunissen P J G．New global navigation satellite system ambiguity resolution method compared to existing approaches［J］．Guidance，Control，and Dynamics，2006，29（4）：981 -991．

［8］Teunissen P J G，Giorgi G，Buist P J．Testing a new single-frequency GNSS carrier phase determination method：land，ship and aircraft experiments［J］．GPS Solutions，2011，15（1）：15- 28．

［9］Chansik P，Teunissen P J G．Integer least squares with quadratic equality constraints and its application to GPS attitude determination systems［J］．International Journal of Control，Auto-mation，and Systems，2009，7（4）：566- 576．

［10］Wang B，Miao L J，Wang S T．A constrained LAMBDA method for GPS attitude determination［J］．GPS Solutions，2009，13（2）：97- 107．

［11］Hofmann W．GPS-global navigation satellite systems GPS，GLONASS，Galileo，and more［M］．New York：Springer Wien，2008：227- 234．

［12］Li Y，Efatmaneshnik M，Dempster A G．Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications［J］．GPS Solutions，2012，16（1）：41- 52．

［13］Heon O S，Hwang D H，Park C，et al．Attitude determination GPS／INS integration system design using triple difference technique［J］．Journal of Electrical Engineering and Technology，2012，7（4）：615- 625．

［14］Chiang K W，Lin Y C，Huang Y W，et al．An ANN-RTS smoother scheme for accurate INS／GPS integrated attitude determination［J］．GPS Solutions，2009，13（3）：199- 208．

［15］Lai Y C，Jan S S．Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration［J］．GPS Solutions，2011，15（1）：67- 77．

GPS／INSintegrated navigation attitude determination method based on CLAMBDA and AFM aided by INS

CONG Li，LI Er-cui，ZHANG Li-yang，QIN Hong-lei，XUE Rui
（School of Electronic and Information Engineering，Beihang University，Beijing 100191，China）

To improve the success rate and performance of global positioning system（GPS）attitude determination，an attitude determination method is proposed，which combines the constrained least squares ambiguity decorrelation adjustment（CLAMBDA）and ambiguity function method（AFM），and makes use of the inertial navigation system（INS）attitude information．In the process of CLAMBDA，the INS information is combined with the GPScarrier phase and code measurements，increasing the redundancy of equations to solve the float solution，and then searching the fixed solution．If the true ambiguity solutions are not found and the attitude is false，the false attitude is replaced by the attitude using AFM．Meanwhile，the INS information is used to shrink the search space of AFM．After that，the integrated filter of GPS／INSis made．Through the land vehicle experiments，compared with the original algorithm，the proposed algorithm can effectively improve the success rate of attitude determination．

global positioning system（GPS）；inertial navigation system（INS）；constrained least squares ambiguity decorrelation adjustment（CLAMBDA）；ambiguity function method（AFM）

TN 967．2

A

10．3969／j．issn．1001-506X．2015．04．24

叢 麗（1981 ），女，講師，博士，主要研究方向為衛(wèi)星導航、組合導航和戰(zhàn)術導航。E-mail：congli_hlj＠163．com

李二翠（1989-），通訊作者，女，碩士，主要研究方向為INS／GNSS組合導航定姿。E-mail：buaa_liercui＠163．com

張立楊（1986-），男，碩士，主要研究方向為GNSS姿態(tài)測量。E-mail：liyang_zhang＠126．com

秦紅磊（1975 ），男，教授，博士，主要研究方向為衛(wèi)星導航、組合導航和自動測試系統(tǒng)E-mail：qhlmmm＠sina．com

1001-506X（2015）04-0882-06

2014- 04- 28；

2014- 09- 26；網(wǎng)絡優(yōu)先出版日期：2014- 10- 30。

網(wǎng)絡優(yōu)先出版地址：http：／／w ww．cnki．net／kcms／detail／11．2422．TN．20141030．0942．005．html

國家重點基礎研究發(fā)展計劃（973計劃）（2011CB707004）；國家高技術研究發(fā)展計劃（863計劃）（2012AA121801）；國家科技支撐計劃（2011BAH24B02）；國家自然科學基金（61101077）資助課題