一種基于GPS單差模型的姿態(tài)解算算法

秦紅磊，陳萬通，金 天，叢 麗

(北京航空航天大學(xué)電子信息工程學(xué)院，100191北京，qhlmmm@sina．com)

一種基于GPS單差模型的姿態(tài)解算算法

秦紅磊，陳萬通，金 天，叢 麗

(北京航空航天大學(xué)電子信息工程學(xué)院，100191北京，qhlmmm@sina．com)

針對(duì)采用雙差模型無法克服雙差觀測(cè)量的強(qiáng)相關(guān)性這一問題，以觀測(cè)量不相關(guān)的單差模型為基礎(chǔ)，提出一種可遞歸處理的定姿算法．該方法用碼觀測(cè)量輔助載波相位觀測(cè)值，增加了信息量，通過遞歸估計(jì)模糊度浮點(diǎn)解及其方差協(xié)方差矩陣，從而可以利用LAMBDA算法估計(jì)整周模糊度．該算法計(jì)算量較小，且具有數(shù)值穩(wěn)定性．實(shí)驗(yàn)結(jié)果表明，該算法的初始化時(shí)間僅占雙差模型所用時(shí)間的1/50左右．該算法可以有效縮短初始化階段，能夠高效地用于動(dòng)態(tài)姿態(tài)解算．

全球定位系統(tǒng);姿態(tài)解算;整周模糊度;短基線

全球定位系統(tǒng)(GPS)具有全球性、全天候和連續(xù)的精密三維定位能力．應(yīng)用GPS載波相位測(cè)量技術(shù)來確定載體姿態(tài)，因具有精度高、長(zhǎng)期穩(wěn)定的準(zhǔn)確性、低成本和低能量耗損等優(yōu)點(diǎn)而日益成為導(dǎo)航領(lǐng)域研究的熱點(diǎn)［1］．但由于載波是一種周期性的正弦信號(hào)，進(jìn)行相位測(cè)量時(shí)存在未知的整周模糊度問題，必須先求得初始時(shí)刻的整周模糊度，才能獲得高精度的基線坐標(biāo)［2］．采用雙差載波相位觀測(cè)方程能夠有效地減小電離層和對(duì)流層誤差、軌道誤差、衛(wèi)星和接收機(jī)時(shí)鐘誤差，是姿態(tài)測(cè)量中通常使用的基本模型［3］．靜態(tài)情況下，基線坐標(biāo)在任何時(shí)刻保持不變，可以利用最小二乘求解浮點(diǎn)解和LAMBDA算法估計(jì)初始整周模糊度;動(dòng)態(tài)情況下，基線坐標(biāo)在不同時(shí)刻都對(duì)應(yīng)著不同的未知量，經(jīng)典最小二乘法和LAMBDA算法不能直接應(yīng)用，而采用“直接收斂法”由于雙差觀測(cè)量的強(qiáng)相關(guān)性，通常會(huì)有十幾分鐘的初始化時(shí)間，不利于實(shí)際姿態(tài)測(cè)量應(yīng)用．

本文避開雙差模型，采用觀測(cè)量不相關(guān)的單差模型，遞歸求解模糊度浮點(diǎn)解及其方差協(xié)方差矩陣，使得可以應(yīng)用LAMBDA算法進(jìn)行整周模糊度估計(jì)，并在有約束條件的情況下，通過擴(kuò)大候選值空間后再利用約束條件“識(shí)別”模糊度的方法，將初始化時(shí)間縮減至十幾秒，并給出了詳細(xì)的算法步驟和相應(yīng)的實(shí)驗(yàn)對(duì)比．

1 基本觀測(cè)方程

1.1 碼觀測(cè)方程

接收機(jī)s對(duì)衛(wèi)星i的碼觀測(cè)方程為

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

接收機(jī)s對(duì)衛(wèi)星i的載波相位觀測(cè)方程為

式中:φis為載波相位測(cè)量的小數(shù)部分;Nis為未知的整周模糊度;λ為L(zhǎng)1載波的波長(zhǎng);vis為載波相位測(cè)量噪聲．

1.3 碼單差方程

對(duì)于短基線而言，采用單差方法可以消去電離層和對(duì)流層誤差、衛(wèi)星軌道誤差和衛(wèi)星時(shí)鐘誤差［4］．若在接收機(jī)A和接收機(jī)B之間針對(duì)同一顆衛(wèi)星i進(jìn)行碼觀測(cè)量的單差，則碼單差方程為

1.4 載波相位單差方程

碼觀測(cè)量的缺點(diǎn)是噪聲較大，因此在高精度測(cè)量中主要是利用載波相位觀測(cè)量，其觀測(cè)精度相對(duì)于碼觀測(cè)量要高2 ～3個(gè)數(shù)量級(jí)［5－6］．對(duì)于以A、B兩個(gè)天線為端點(diǎn)的短基線，其對(duì)衛(wèi)星i的單差載波相位觀測(cè)方程表述為

根據(jù)式(5)，則整理方程為

1.5 載波相位雙差方程

將2個(gè)單差方程相減，可以消去接收機(jī)鐘差項(xiàng)．假設(shè)衛(wèi)星1仰角最高，作為參考星，則由式(5)可知參考星的單差方程為

式(5)與式(7)相減可以得到衛(wèi)星i對(duì)參考星的雙差載波相位觀測(cè)方程為

式中:▽?duì)う読AB為A、B兩個(gè)天線到衛(wèi)星i的雙差載波相位觀測(cè)值的小數(shù)部分;▽?duì)iAB為未知的雙差整周模糊度．

2 雙差觀測(cè)模型求解載體姿態(tài)

2.1 雙差觀測(cè)模型

對(duì)于m顆衛(wèi)星而言，則存在m－1個(gè)載波相位雙差方程，寫成矩陣形式為

一般認(rèn)為噪聲服從均值為零的正態(tài)分布，對(duì)于雙差載波相位觀測(cè)值向量y而言，其各個(gè)分量存在一定的相關(guān)性，y的方差協(xié)方差矩陣為

式中:σ2φ為接收機(jī)載波相位測(cè)量噪聲方差，一般認(rèn)為 σφ=0.025周［7］;Em－1為 m － 1 階矩陣，其所有元素均為1;Im－1為m－1階單位矩陣．可見雙差觀測(cè)值y各個(gè)分量之間存在強(qiáng)相關(guān)性．

式(9)是虧秩的，單歷元的情況下無法得到唯一的解，必須將多個(gè)歷元的觀測(cè)方程進(jìn)行聯(lián)立，通過最小二乘估計(jì)未知參數(shù)．但最小二乘估計(jì)的基線和模糊度參數(shù)均為實(shí)數(shù)解，而模糊度是整數(shù)，解決此問題通常是先求解模糊度的浮點(diǎn)解，然后根據(jù)其方差協(xié)方差矩陣估計(jì)模糊度固定解，再利用固定解重新修正基線坐標(biāo)．

2.2 靜態(tài)觀測(cè)模型

對(duì)于靜態(tài)基線，b是保持不變的，對(duì)于連續(xù)的q個(gè)歷元有

則其數(shù)學(xué)模型和統(tǒng)計(jì)模型可以看成

即得到基線向量坐標(biāo)和模糊度浮點(diǎn)解及其相應(yīng)的方差協(xié)方差矩陣為

然后通過目標(biāo)函數(shù)最小來求得模糊度固定解為

LAMBDA算法是求解式(15)的有效算法．

2.3 動(dòng)態(tài)觀測(cè)模型

對(duì)于動(dòng)態(tài)情況，每個(gè)歷元k都對(duì)應(yīng)一個(gè)不同的基線坐標(biāo)bk，則其批處理模型為

則式(16)中矩陣階數(shù)會(huì)隨著k的增加而增加，特別是求解各歷元的浮點(diǎn)解時(shí)，矩陣求逆計(jì)算量會(huì)非常大，無法滿足實(shí)時(shí)處理的要求．一種解法是將方程組進(jìn)行解耦變換［9］，即

移項(xiàng)有Bi(yi－vi)=Biz，但是觀測(cè)噪聲是未知的，若忽略噪聲，可得到模糊度的近似浮點(diǎn)解為

這種做法雖然可以避免大矩陣求逆求得模糊度的浮點(diǎn)解，也易遞歸實(shí)現(xiàn)，但是需要多個(gè)歷元來等待模糊度浮點(diǎn)解的收斂，即直接收斂法．其收斂的速度曲線基本呈指數(shù)衰減的趨勢(shì)，在開始的2～3 min內(nèi)，向量NDD可迅速收斂到距真實(shí)值2～3個(gè)波長(zhǎng)λ的位置，以后的收斂速度就變得十分緩慢．要到400～600 s后，NDD才逼近到距真實(shí)值NDD＜0.5λ的位置，從而獲得正確的NDD．這主要是由于短時(shí)間內(nèi)雙差觀測(cè)量的強(qiáng)相關(guān)性造成的，由于式(19)的特殊結(jié)構(gòu)無法準(zhǔn)確求得浮點(diǎn)解的方差協(xié)方差矩陣，所以無法直接使用LAMBDA算法進(jìn)行估計(jì)．

3 單差觀測(cè)模型求解姿態(tài)模型

3.1 求解單差整周模糊度

假設(shè)存在m顆衛(wèi)星，則根據(jù)式(6)和式(4)將分別存在m個(gè)載波相位單差方程與m個(gè)碼單差方程，寫成矩陣形式分別為

式中E為接收機(jī)到衛(wèi)星的設(shè)計(jì)矩陣．為了將噪聲統(tǒng)計(jì)特性歸一化［10］，令 σr≡ σφ/σρ，其中:σφ，σρ分別為載波和碼噪聲的標(biāo)準(zhǔn)差;σr為其比值，一般取為0. 001，將其乘在式(21)的兩端，則有

則式(23)等價(jià)于

其中a的各個(gè)分量均為整數(shù)．將k個(gè)歷元的上述方程進(jìn)行聯(lián)立，其矩陣形式為

通過Householder變換進(jìn)行QR分解可以得到正交陣Tk，式(28)兩邊同時(shí)乘以TTk，使得:

LAMBDA算法對(duì)初始模糊度進(jìn)行Z變換，變換結(jié)果為

在變換域求解到z，在通過反變換求得原始的雙差整周模糊度為

3.2 模糊度檢驗(yàn)

由于各種誤差的作用，并不能保證所求出的模糊度就是正確的．因此需要對(duì)模糊度候選值進(jìn)行檢驗(yàn)，以得到正確的固定解．工程中最為常用的一種是比率檢驗(yàn)，即式中Z矩陣及其轉(zhuǎn)置的作用是對(duì)搜索空間進(jìn)行不同方向的“擠壓”，使搜索空間由拉長(zhǎng)的高維橢球近似于球，以提高搜索效率，經(jīng)過變換，式(34)的目標(biāo)函數(shù)等價(jià)為

式中:as為次優(yōu)模糊度固定解;α為常數(shù)，其如何選取該關(guān)鍵值仍是一個(gè)開放的問題［11］，目前多通過經(jīng)驗(yàn)選?。疄榱舜_保模糊度驗(yàn)證的可靠性，本文采用的方法為如果連續(xù)n個(gè)歷元的基線坐標(biāo)都通過了基線長(zhǎng)度檢驗(yàn)且初始整周模糊度的固定解都一致，就可以認(rèn)為該模糊度求解正確，即看是否滿足

否則，將采用更多個(gè)歷元繼續(xù)遞歸求解，直到可以確定初始整周模糊度．

3.3 求解基線坐標(biāo)和鐘差

求得整周模糊度a，代入式(23)即有

3.4 算法遞歸實(shí)現(xiàn)

在動(dòng)態(tài)載體應(yīng)用中，數(shù)據(jù)處理常采用遞歸的方式，為了便于實(shí)現(xiàn)模糊度浮點(diǎn)解的遞歸估計(jì)，利用第k歷元估計(jì)的結(jié)果，則第k+1歷元有

通過 Householder變換實(shí)現(xiàn) QR分解，即式(41)兩邊同時(shí)乘以正交陣TTk+1，即

用k+1個(gè)歷元的數(shù)據(jù)得到的初始整周模糊度的浮點(diǎn)估計(jì)值為，那么

式(45)與式(31)形式統(tǒng)一，即在各歷元都可以采用LAMBDA算法對(duì)浮點(diǎn)解進(jìn)行整周模糊度估計(jì)．另外，遞歸算法使用QR分解來完成最小二乘計(jì)算，具有數(shù)值穩(wěn)定性，而若使用遞歸最小二乘法，則會(huì)出現(xiàn)隨著時(shí)間的增長(zhǎng)，不可避免的截?cái)嗾`差或舍入誤差導(dǎo)致結(jié)果偏離真值．

4 附加約束條件下的算法改進(jìn)

4.1 歷元數(shù)與模糊度正確性的關(guān)系

4.2 擴(kuò)大候選值空間

減少歷元數(shù)將導(dǎo)致模糊度浮點(diǎn)解的精度降低，雖然低精度的浮點(diǎn)解經(jīng)變換后可能落到正確z的鄰近整數(shù)點(diǎn)的歸屬域中，但其一定在z的周圍．假設(shè)其落入z'的歸屬域，那么以z'為中心一定半徑的覆蓋范圍內(nèi)的全部歸屬域所對(duì)應(yīng)的整數(shù)點(diǎn)會(huì)包含z，如果不包含，則擴(kuò)大覆蓋半徑到一定程度，必定包含z．如果能夠?qū)νㄟ^擴(kuò)大候選值空間得到的所有候選點(diǎn)進(jìn)行符合某些準(zhǔn)則的判別和檢驗(yàn)，從而篩選出正確的整周模糊度，那么則對(duì)k的精度也放低．幸運(yùn)的是，LAMBDA算法能夠按照“最佳程度”依次給出多個(gè)候選值，這里用“正確候選點(diǎn)出現(xiàn)的位置”表示正確的候選值在多個(gè)候選值中的次序．例如若第3個(gè)候選值是真實(shí)的模糊度，可記作“正確候選點(diǎn)出現(xiàn)的位置為3”．

4.3 根據(jù)約束條件篩選候選值

基線長(zhǎng)度或俯仰角等約束信息可以輔助篩選正確的整周模糊度，令LAMBDA算法給出的候選值個(gè)數(shù)為N，具體篩選步驟為:對(duì)于某個(gè)候選值a'，代入式(40)，求得對(duì)應(yīng)的基線坐標(biāo)記為b'k，則其基線長(zhǎng)度為

式中:δbl為預(yù)先設(shè)定的閾值;bl為基線的真實(shí)長(zhǎng)度．在姿態(tài)測(cè)量應(yīng)用中，δbl的選取是基線長(zhǎng)度約束的關(guān)鍵．因?yàn)樘炀€相位中心的變化和噪聲影響，不能將δbl的值設(shè)置的過小，同時(shí)過大的δbl又無法有效地識(shí)別a是否為正確的整周模糊度．該值一般為基線真實(shí)長(zhǎng)度的1%．

如果由a'得到的b'k滿足式(47)，則可利用b'k求解的姿態(tài)角繼續(xù)進(jìn)行約束檢驗(yàn)，即對(duì)于地面載體，滿足|θ|≤10°;對(duì)于有慣性器件輔助的載體，則應(yīng)滿足|θ－|≤δθ并且|ψ－|≤δψ，其中:和分別為慣性器件給出的俯仰角和航向角觀測(cè)值;δθ和δψ分別為設(shè)定的誤差變化范圍．如果通過該檢驗(yàn)，則可認(rèn)為a'在很大概率上為正確的模糊度．如果連續(xù)幾個(gè)歷元計(jì)算初始整周模糊度相同，則基本可以確定其為正確解．

如果由a'得到的b'k沒有通過基線長(zhǎng)度檢驗(yàn)或者沒有通過姿態(tài)角檢驗(yàn)，則需要試驗(yàn)下一個(gè)模糊度候選值，直到篩選出正確的整周模糊度;如果所有模糊度候選值均不滿足，則需擴(kuò)大候選值個(gè)數(shù)中的N值，或者需要更多歷元繼續(xù)按照上述方法檢測(cè)，直至找出滿足約束條件的模糊度候選解及其解算得到的姿態(tài)角．

5 實(shí)際數(shù)據(jù)測(cè)試

為了比較新算法的優(yōu)勢(shì)，進(jìn)行了單基線姿態(tài)測(cè)量的試驗(yàn)．?dāng)?shù)據(jù)采集地點(diǎn)為北京航空航天大學(xué)新主樓樓頂，即盡可能避免信號(hào)被遮擋，OEM板型號(hào)為NavAtel公司的Super Star II，其載波相位輸出頻率為1 Hz，天線型號(hào)為GPS－701GG，其內(nèi)含扼流圈，具有多徑抑制功能．基線長(zhǎng)度為2 m，保持水平放置，持續(xù)跟蹤的可見星數(shù)目為7顆．為了得到初始時(shí)刻的雙差整周模糊度真值，以作為參考基準(zhǔn)，首先采用靜態(tài)觀測(cè)模型算法進(jìn)行求解，通過1 200個(gè)歷元的觀測(cè)數(shù)據(jù) 求 得 NDD= ［ 1， － 9， 7，－ 8，－ 8，7］T，b =［0.868 3，1.805 8，0.017 0］T，ψ =64.320 3°，θ=0.484 9°．

采用動(dòng)態(tài)觀測(cè)模型的直接收斂法求解，利用式(19)得到的模糊度浮點(diǎn)解的收斂情況如圖 1，2所示，其中:“?”為該時(shí)刻浮點(diǎn)解四舍五入后沒有映射到正確的整周模糊度上;“·”為映射成功．可以看出直接收斂法確定模糊度分量分別至少需要 452， 465， 543， 335， 478，517 s才會(huì)收斂到正確整周模糊度的0.5λ范圍內(nèi)，也即需要近550 s才能保證所有的模糊度分量浮點(diǎn)解四舍五入后收斂到正確的整周模糊度上，進(jìn)而求解出姿態(tài)．

圖1 模糊度第 1， 2，3分量收斂情況

圖2 模糊度第 4， 5，6分量收斂情況

采用基于單差模型的算法進(jìn)行解算，首先假設(shè)無基線長(zhǎng)度和姿態(tài)角約束等先驗(yàn)信息．實(shí)驗(yàn)結(jié)果如圖3所示．結(jié)果顯示，僅用31 s的觀測(cè)數(shù)據(jù)，就可以得到正確的姿態(tài)解，解算過程中求得初始時(shí)刻的單差整周模糊度為

以第1顆星為參考星，可以得到對(duì)應(yīng)的雙差整周模糊度N(1)DD，與真實(shí)值一致．

圖3 姿態(tài)角解算結(jié)果

對(duì)比圖3和圖4可知，基于單差模型的算法求解姿態(tài)的時(shí)間要遠(yuǎn)小于直接收斂法，其原因是單差模型的觀測(cè)量各個(gè)分量之間不相關(guān)，同時(shí)利用了多個(gè)歷元的數(shù)據(jù)遞歸求得初始時(shí)刻整周模糊度的浮點(diǎn)解及其方差協(xié)方差矩陣，進(jìn)而使用LAMBDA算法準(zhǔn)確地估計(jì)出單差整周模糊度，從而縮短了初始化時(shí)間．

圖4 基于約束條件的算法結(jié)果

由于基線長(zhǎng)度事先可以測(cè)量，對(duì)于地面載體，俯仰角也存在約束，則可采用附加約束條件下的算法改進(jìn)的算法求解．為了避免計(jì)算量過大，候選值個(gè)數(shù)N選定為500．實(shí)驗(yàn)結(jié)果如圖4所示，此時(shí)初始化時(shí)間僅為10 s，小于直接收斂法所用時(shí)間的1/50．對(duì)比圖3可知，比不用約束條件時(shí)的初始化時(shí)間還要少20 s，從而說明基于約束條件的算法更加有效．圖5顯示了前100個(gè)歷元識(shí)別到的正確模糊度的位置，在第31 s之前，模糊度正確解是通過約束條件篩選出來的，出現(xiàn)位置雖然較遠(yuǎn)，但可以看出隨著時(shí)間的推移其逐漸“向前”靠攏，直至第31 s以后，第1個(gè)候選值均為正確解．

圖5 利用基線長(zhǎng)度篩選候選點(diǎn)

6 結(jié)論

1)采用觀測(cè)值無關(guān)的單差模型，克服了雙差模型由于觀測(cè)值存在相關(guān)性而導(dǎo)致的模糊度浮點(diǎn)解收斂速度太慢的缺點(diǎn)．

2)可利用LAMBDA算法提高模糊度估計(jì)的準(zhǔn)確性．

3)該算法可遞歸實(shí)現(xiàn)，數(shù)值計(jì)算穩(wěn)定，計(jì)算量較小，能夠滿足實(shí)時(shí)應(yīng)用．

［1］JOHN R．Current issues in the use of the global positioning system aboard satellites［J］．Acta Astronautica， 2000，47(2/9):377－387．

［2］ XIA Kewen，ZHANG Xinying，GAO Jinyong，et al．Study on GPS attitude determination technology based on QPSO algorithm［C］//Proceedings of the 7th World Congress on Intelligent Control and Automation．Washington DC:IEEE，2008:1869－1873．

［3］ CHANG Xiaowen，PAIGE C C，YIN Lan．Code and carrier phase based short baseline GPS positioning:Computational aspects ［J］．GPS Solutions， 2004，7(4):230－240．

［4］HOFMANN-WELLENHOF B．GNSS-Global Navigation Satellite Systems GPS，GLONASS，Galileo，and More［M］．New York:Springer Wien，2008:227－234．

［5］CHANG Xiaowen，PAIGE C C．An orthogonal transformation algorithm for GPS Positioning［J］．SIAM Journal on Scientific Computing， 2002，24(5):1710－1732．

［6］CHANG Xiaowen，PAIGE C C．An algorithm for combined code and carrier phase based GPS positioning［J］．BIT Numerical Mathematics， 2003，43(5):915 －927．

［7］ MISRA P，ENGE P．Global Positioning System:Signals，Measurements，and Performance［M］．2nd ed．Lincoln MA:Ganga-Jamuna Press，2006:238 －239．

［8］VERHAGEN S．The GNSS Integer Ambiguities:Estimation and Validation［D］．Delft:Delft University of Technology，2003:34－35．

［9］俞文伯，高國(guó)江，趙剡，等．單頻GPS動(dòng)態(tài)相對(duì)定位的模糊度逼近搜索解法［J］．北京航空航天大學(xué)學(xué)報(bào)， 2002，28(2):242－244．

［10］CHANG Xiaowen，GUO Ying．Huber＇s M-estimation in relative GPS positioning:Computational aspects［J］．Journal of Geodesy， 2005，79(6):351－362．

［11］VERHAGEN S．Integer ambiguity validation:An open problem［J］．GPS Solution， 2004，8(1):36－43．

An attitude determination algorithm based on GPS single-differenced mode

QIN Hong-lei，CHEN Wan-tong，JIN Tian，CONG Li

(School of Electronic and Information Engineering，Beihang University，100191 Beijing，China，qhlmmm@sina．com)

It is difficult to overcome the high correlation of double differential observation using double-differenced model and the initialization procedure costs a long time with makes it difficult to apply in practice．To deal this problem，a recursive algorithm based on the single-differenced model is presented in which the observations are not correlated．The new method increases observation information with the aid of code for original carrier phase observation．At the same time the float ambiguity solutions and its variance-covariance matrix can be obtained recursively and it makes the LAMBDA method be used to estimate the integer ambiguities．The method is also less computation with numerical stability．The experiment results show that the time of initialization is reduced to about 1/50 of that using the double-differenced method，so this method can be effectively applied in practical attitude determination for shortening initialization procedure．

GPS;attitude determination;integer ambiguity;short baseline

V249.3

A

0367－6234(2011)09－0105－07

2010－01－08．

國(guó)家高技術(shù)研究發(fā)展計(jì)劃資助項(xiàng)目(2009AA12Z313)．

秦紅磊(1975—)，男，博士，副教授．

(編輯 張 紅)