基于超短基線的GPS測姿算法研究

宋雅倩，何 維，任 彥

(重慶郵電大學(xué) 通信與信息工程學(xué)院，重慶 400065)

0 引 言

姿態(tài)角是載體在慣性空間指向過程中的重要導(dǎo)航參數(shù)之一。這些參數(shù)可以幫助了解載體在空中的實(shí)時(shí)姿態(tài)，在軍事、民用等方面都有著重要的作用。

常用的測姿主要分為衛(wèi)星測姿、慣性導(dǎo)航系統(tǒng)(inertial navigation system, INS)測姿兩大類。慣性測量單元是通過加速度計(jì)和陀螺儀測量載體的姿態(tài)，陀螺儀的誤差會隨著時(shí)間的推移而增大，且高精度的INS系統(tǒng)成本較高[1]。同INS測姿相比，全球定位系統(tǒng)(global positioning system, GPS)測姿系統(tǒng)只需要利用低成本的接收機(jī)就可以提供較高精度的姿態(tài)信息，同時(shí)還可以完成載體定位和系統(tǒng)授時(shí)，能夠長時(shí)間進(jìn)行高精度測姿任務(wù)[2]。

正常情況下，衛(wèi)星測姿的精度將隨著基線長度的增加而升高，而例如在無人機(jī)、基站天線、無人駕駛車等超短基線情況下長基線產(chǎn)品無法應(yīng)用。超短基線由于兩端之間非常接近，使得兩端的測量誤差有很強(qiáng)的相關(guān)性，并且很小的整周周跳都將對基線的解算精度產(chǎn)生重大影響，因此，整周模糊度確定是超短基線測姿的核心之一；同時(shí)，基于基線求解的姿態(tài)解算方法將直接影響著姿態(tài)結(jié)果，因此，測姿算法的研究十分必要[3]。文獻(xiàn)[4]提出了一種基于低成本接收機(jī)的雙天線測姿算法，當(dāng)基線長度為1.1 m時(shí)，測姿航向角精度在1°左右，但其需要雙系統(tǒng)并且基線長度較長；文獻(xiàn)[5]提出了一種GPS/INS組合導(dǎo)航算法，但陀螺儀測量精度隨著時(shí)間的推移會逐漸降低，只有在短時(shí)間內(nèi)精度高；文獻(xiàn)[6]通過建立了雙差分相位觀測值與姿態(tài)四元數(shù)之間的函數(shù)關(guān)系使姿態(tài)解算更精準(zhǔn)，在靜態(tài)超短基線為0.55 m浮點(diǎn)模糊度的準(zhǔn)確性得到了提高，姿態(tài)解算更精準(zhǔn)，但是其需要用到多天線。

整周模糊度確定是超短基線測姿的核心之一，目前現(xiàn)存的整周模糊度的方法，都需要求解模糊度實(shí)數(shù)解，構(gòu)建模糊度搜索空間，并利用一定的假設(shè)檢驗(yàn)條件在構(gòu)建的搜索空間內(nèi)搜索出正確的整周模糊度。浮點(diǎn)模糊度一般由偽距觀測值、最小二乘法或卡爾曼濾波平滑獲得。并且浮點(diǎn)模糊度的準(zhǔn)確性會影響整數(shù)模糊度分辨率的性能[7]。整周模糊度的固定方法有模糊度函數(shù)法、最小二乘模糊度搜索法、快速模糊度算法 (fast ambiguty resolution approach,FARA)法、快速模糊度搜索濾波法、模糊度最小二乘去相關(guān)平差法(modified least-squares ambiguity decorrelation adjustment algorithm, M-LAMBDA)[8]。目前有學(xué)者提出改進(jìn)模糊度最小二乘去相關(guān)平差法(modified least-squares ambiguity decorrelation adjustment algorithm, M-LAMBDA)算法，通過提前置換和動(dòng)態(tài)調(diào)整搜索范圍能夠有效提高傳統(tǒng)LAMBDA的計(jì)算效率，且不會對模糊度固定成功率造成影響[9]。

姿態(tài)角的解算將在基線求解的基礎(chǔ)上進(jìn)行，受基線精度影響較大，目前基于衛(wèi)星測姿的姿態(tài)角算法大致分為直接法、最優(yōu)估計(jì)方法、最小二乘法和濾波算法[10]。直接法是直接通過觀測值求解，容易實(shí)現(xiàn)，硬件要求低，但不能同時(shí)利用已知基線長度的先驗(yàn)信息；最優(yōu)估計(jì)算法需要大量的觀測值來進(jìn)行迭代求解最優(yōu)值；最小二乘法是采用最小二乘理論來求解姿態(tài)矩陣，其默認(rèn)為線性估計(jì)，其結(jié)果并不是最優(yōu)值；濾波法是指采用例如卡爾曼濾波、粒子濾波等方法求解姿態(tài)角，但濾波法對初值的選取較為敏感，較為精準(zhǔn)的初值適用于卡爾曼濾波，不太精準(zhǔn)的初始條件適用于粒子濾波[11]。

通過以上分析，區(qū)別于其他GPS測姿算法，本文提出的改進(jìn)GPS測姿算法適用于更短的基線，在求解浮點(diǎn)模糊度時(shí)采用改進(jìn)擴(kuò)展卡爾曼濾波，有效提高測姿系統(tǒng)的可靠性，在姿態(tài)解算時(shí)采用濾波法，有效提高了測姿系統(tǒng)的穩(wěn)定性。

1 GPS測姿算法模型推導(dǎo)

1.1 系統(tǒng)組成及功能

本文提出的基于超短基線的GPS測姿系統(tǒng)框圖如圖1所示。

圖1 GPS測姿系統(tǒng)框架Fig.1 Framework of GPS attitude measurement system

1)轉(zhuǎn)臺：利用轉(zhuǎn)臺來控制基線航向角的變化。

2)接收機(jī)：user、reference接收機(jī)為2臺GPS天線，用來接收衛(wèi)星數(shù)據(jù)。

3)OEM617D板卡：將2個(gè)天線連接板卡，串口1輸入命令獲得觀測數(shù)據(jù)與星歷數(shù)據(jù)。串口2輸入命令獲得觀測數(shù)據(jù)。

4)筆記本：處理數(shù)據(jù)，進(jìn)行算法驗(yàn)證與分析。

算法驗(yàn)證主要分為5部分：數(shù)據(jù)解析、周跳檢測與修復(fù)模塊、擴(kuò)展卡爾曼濾波模塊、MLAMBDA算法模塊和姿態(tài)解算模塊，本文重點(diǎn)研究擴(kuò)展卡爾曼濾波模塊和姿態(tài)解算模塊。

1.2 基于擴(kuò)展卡爾曼濾波的模型推導(dǎo)

在進(jìn)行濾波前，利用載波相位測量值進(jìn)行周跳檢測[12]。在雙頻情況下，reference站的坐標(biāo)通過單點(diǎn)定位得知，user站的坐標(biāo)初始模糊位置通過單點(diǎn)定位可知，則X可以表示為

(1)

(2)

設(shè)在k-1時(shí)刻，頻點(diǎn)為f1，公共衛(wèi)星為1到6。假設(shè)衛(wèi)星1有周跳，這時(shí)候需要將衛(wèi)星1的單差模糊度賦值為0，則

(3)

設(shè)在k時(shí)刻，單差模糊度浮點(diǎn)解為

(4)

將k-1時(shí)刻不為零的單差模糊度與k時(shí)刻對應(yīng)的單差模糊度做差，將每個(gè)差值相加求平均av，有

(5)

(5)式中，n為公共衛(wèi)星的個(gè)數(shù)，即Nur,1,k為

(6)

擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)針對于非線性系統(tǒng)，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，使得系統(tǒng)的估計(jì)值有最小均方誤差。利用泰勒公式對高階項(xiàng)采用忽略或逼近措施，從而使非線性系統(tǒng)轉(zhuǎn)變?yōu)橐粋€(gè)線性系統(tǒng)，再利用卡爾曼濾波方法進(jìn)行最優(yōu)估計(jì)。擴(kuò)展卡爾曼濾波觀測方程與狀態(tài)方程分別為

zk=h(xk)+vk

xk=Fxk-1+u

(7)

(7)式中：zk為觀測向量；xk為當(dāng)前狀態(tài)向量；xk-1是上一時(shí)刻的狀態(tài)后驗(yàn)估計(jì)值矩陣。F是的傳遞矩陣，vk為系統(tǒng)殘差，由載波相位殘差和偽距殘差2部分構(gòu)成，即

(8)

預(yù)測方程為

(9)

(10)

設(shè)本模型的狀態(tài)變量為經(jīng)過整周跳變與修正的Xk，則狀態(tài)方程為

(11)

(12)

當(dāng)Δt為零時(shí)，為靜態(tài)狀態(tài)，Δt不為零時(shí)為動(dòng)態(tài)模型，因此，預(yù)方程為[4]

(13)

(13)式中：Q為上一時(shí)刻k-1到此刻k外界提供給濾波器的過程噪聲方差，根據(jù)經(jīng)驗(yàn)值將狀態(tài)向量的協(xié)方差矩陣初始化為[4，13]

(14)

(15)

(15)式中：

P0的值會隨著擴(kuò)展卡爾曼濾波的每一次的迭代而變化。

已知雙差載波觀測方程和雙差偽距觀測方程為

(16)

模型中選取最高仰角衛(wèi)星作為參考衛(wèi)星，將參考衛(wèi)星與其他衛(wèi)星的雙差觀測方程的左側(cè)作為觀測值z，即選取(16)式左側(cè)部分(GPS接收機(jī)板卡輸出)作為觀測值，若假設(shè)最高仰角衛(wèi)星編號為1，則

(17)

(17)式中，當(dāng)高仰角衛(wèi)星編號不為1時(shí)，Φi與Pi的值會變化。選取(16)式右邊一部分為h(X′)，即

(18)

將h(X′)非線性函數(shù)轉(zhuǎn)化為近似的線性函數(shù)

(19)

(19)式中，偏導(dǎo)項(xiàng)被稱為雅可比式H。測量更新的3個(gè)方程為

(20)

(20)式中，R為測量噪聲矩陣，表示測量值與真實(shí)值之間的差值。

1.3 GPS觀測量誤差隨機(jī)模型

若要符合標(biāo)準(zhǔn)Kalman濾波的條件，那么就要保證有正確的系統(tǒng)模型、量測模型和量測噪聲。同時(shí)正確給定函數(shù)模型和隨機(jī)模型對于GPS定位結(jié)果的估計(jì)和觀測量的粗差探測均至關(guān)重要。在GPS應(yīng)用中，R為測量噪聲矩陣，本文采用不同的模型，應(yīng)用于擴(kuò)展卡爾曼濾波模型中，對GPS基線解算的不同結(jié)果進(jìn)行分析比較。測量噪聲矩陣R表示為

(21)

當(dāng)采用衛(wèi)星高度角的GPS觀測量隨機(jī)模型時(shí)

(22)

(22)式中：i對應(yīng)參考衛(wèi)星；a,b為載波相位觀測誤差；c為基線向量觀測誤差；d為衛(wèi)星時(shí)鐘穩(wěn)定誤差。其中，a,b經(jīng)驗(yàn)固定值0.003，c,d為0，e對應(yīng)衛(wèi)星的高度角。該隨機(jī)模型的實(shí)質(zhì)是利用低高度角衛(wèi)星受到大氣延遲較大，且容易產(chǎn)生多路徑效應(yīng)的特點(diǎn)，將衛(wèi)星高度角較低的觀測值賦較低的權(quán)。

1.4 整周模糊度求解

LAMBDA算法最早是由Teunissen[14]提出，該算法主要是利用Z變化將模糊度的搜索空間去相關(guān)化，使得模糊度搜索空間減小。但傳統(tǒng)的LAMBDA計(jì)算效率低，通過后者不斷修正，提出了M-LAMBDA算法，M-LAMBDA通過改進(jìn)搜索空間，使得計(jì)算效率提高。

由前面的擴(kuò)展卡爾曼濾波得到了雙差模型下的流動(dòng)站坐標(biāo)和單差模糊度浮點(diǎn)解，在M-LAMBDA求整數(shù)解的過程中，將單差通過轉(zhuǎn)換矩陣轉(zhuǎn)化為雙差，提高整周模糊度的求解精度，并提升求解效率。

(23)

已知單差模型下的方差-協(xié)方差矩陣為Pk，則雙差模型下的方差-協(xié)方差矩陣為

(24)

(25)

1.5 姿態(tài)解算

在衛(wèi)星與接收機(jī)位置解算過程中，由GPS接收機(jī)測得參考站在地心地固坐標(biāo)系下的坐(xr,yr,zr)，首先將其轉(zhuǎn)為大地坐標(biāo)系(φ,λ,h)，轉(zhuǎn)換公式為

(26)

(26)式中：N為基準(zhǔn)橢球體的卯酉圓曲率半徑；e為橢球偏心率；a基準(zhǔn)橢球體的長半徑；b為短半徑。基線向量l為

(27)

將基線的地心坐標(biāo)系轉(zhuǎn)化為站心坐標(biāo)系為

(28)

(29)

基線在載體坐標(biāo)系下可表示為[0|bl|0]T可以測量載體的航向角γ和俯仰角β，則

(30)

若采用直接法，則俯仰角和航向角的結(jié)果為

(31)

若采用濾波法，濾波采用擴(kuò)展卡爾曼濾波，對(30)式簡化為

yenu=|l|f(β,γ)

(32)

對(32)式進(jìn)行線性化，其中俯仰角和橫滾角的初始值β0、γ0采用直接法測姿獲得。雅可比陣為

(33)

yenu=Cxθ+vθ,k

(34)

預(yù)測方程為

(35)

校正方程為

(36)

(37)

(37)式中，wk為均值0方差0.1的正態(tài)白噪聲，vk為均值0方差2的正態(tài)白噪聲。

2 算法驗(yàn)證與結(jié)果分析

2.1 測試環(huán)境

本文選取在重慶郵電大學(xué)逸夫樓樓頂進(jìn)行測試。測試平臺如圖2所示，選擇OEM617D板卡進(jìn)行接收數(shù)據(jù)，得到衛(wèi)星數(shù)據(jù)與觀測數(shù)據(jù)，采用精準(zhǔn)轉(zhuǎn)臺固定航向角度，2個(gè)天線同時(shí)工作，天線之間的距離為基線長度，然后通過Python軟件進(jìn)行算法驗(yàn)證。

圖2 測試環(huán)境Fig.2 Testing environment

2.2 系統(tǒng)驗(yàn)證以及結(jié)果分析

圖3、圖4為分別用最小二乘法和擴(kuò)展卡爾曼濾波在20、40、100 cm的120°航向角情況下求解浮點(diǎn)模糊度的航向角的誤差。由圖3，圖4可知，在基線長度為100 cm情況下，最小二乘法和擴(kuò)展卡爾曼濾波產(chǎn)生的航向角誤差相差不大，但隨著基線長度的減小最小二乘法的航向角誤差逐漸增大，在20 cm時(shí)已經(jīng)完全不準(zhǔn)確，因?yàn)殡S著基線的減小觀測誤差逐漸增大，最小二乘法只能利用當(dāng)前的觀測量，不能對觀測量進(jìn)行誤差分析，因此，定位結(jié)果受觀測量的誤差影響比較大，精度不高。擴(kuò)展卡爾曼濾波的誤差相對穩(wěn)定，因?yàn)閿U(kuò)展卡爾曼濾波在時(shí)間上具有相關(guān)性，利用觀測殘差，不斷修正預(yù)測值并利用測量噪聲協(xié)方差矩陣和過程噪聲協(xié)方差矩陣控制最終的結(jié)果中測量值所占的權(quán)重。證明了在超短基線情況下，擴(kuò)展卡爾曼濾波求解模糊度浮點(diǎn)解比最小二乘模糊度浮點(diǎn)解更適用。從圖4也看出，隨著基線的減小，航向角的可靠性和穩(wěn)定性減小，因此，本文提出對擴(kuò)展卡爾曼濾波進(jìn)行改進(jìn)提高航向角的可靠性，采取濾波法提高航向角的穩(wěn)定性。

圖3 最小二乘求解浮點(diǎn)模糊度導(dǎo)致航向角的誤差Fig.3 Least square method for solving the heading angle error caused by floating point ambiguity

圖4 EKF求解浮點(diǎn)模糊度導(dǎo)致航向角誤差Fig.4 Heading angle error caused by floating point ambiguity solved by EKF

在靜態(tài)情況下基線長度為20 cm，每30°為一方位角對等權(quán)模型和基于衛(wèi)星高度角模型產(chǎn)生的航向角均方誤差進(jìn)行對比，如圖5所示。從圖5可以看出，在不同航向角的情況下基于衛(wèi)星高度角模型明顯比等權(quán)模型的均方誤差小。證明靜態(tài)下基于衛(wèi)星高度角的隨機(jī)模型使超短基線的姿態(tài)解算可靠性提高。其原理為若假設(shè)所有觀測量誤差相同的方差，但實(shí)際中存在大量的冗余觀測量，會導(dǎo)致粗差無法檢測，降低參數(shù)估計(jì)精度?；谛l(wèi)星高度角的隨機(jī)模型可以有效削弱大氣殘差，部分減少多路徑效應(yīng)影響，而且該隨機(jī)模型易于實(shí)現(xiàn)，且能較好地反映觀測量誤差，提高參數(shù)估計(jì)精度。

圖5 靜態(tài)時(shí)不同模型情況下航向角的均方誤差Fig.5 Mean square error of heading angle under different model conditions at static state

在靜態(tài)情況下基線長度為20 cm，每30°為一方位角利用直接法和濾波法的航向角誤差方差進(jìn)行對比，如圖6所示。從圖6可以看出，在不同航向角的情況下濾波法比直接法穩(wěn)定性大大提高。圖6證明了濾波法提高了靜態(tài)測姿系統(tǒng)的穩(wěn)定性。其原理為直接法解算姿態(tài)計(jì)算過程簡單，解算速度快，但不能利用基線已知的信息來估計(jì)姿態(tài)角，因此，其估計(jì)值是次優(yōu)的，濾波法利用直接法提供初始值，利用時(shí)間上的相關(guān)性不斷進(jìn)行迭代更新，提高參數(shù)估計(jì)的精度。

圖6 靜態(tài)時(shí)不同測姿算法下的航向角誤差方差Fig.6 Heading Angle error variance of different attitude measurement algorithms at static state

在動(dòng)態(tài)情況下，基線長度分別為100、40、20 cm，航向角每秒6°進(jìn)行變化，分別在等權(quán)模型和基于衛(wèi)星高度角模型下進(jìn)行誤差的均方誤差比較，如圖7所示。從圖7可以看出，隨著基線的減小均方誤差不斷增大，但基于衛(wèi)星高度角模型的均方誤差明顯比等權(quán)模型下的均方誤差小。證明了基于衛(wèi)星高度角模型提高了超短基線下動(dòng)態(tài)測姿系統(tǒng)的可靠性。

圖7 動(dòng)態(tài)時(shí)不同模型情況下航向角的均方誤差Fig.7 Mean square error of heading angle under different model conditions in dynamic condition

在動(dòng)態(tài)情況下，基線長度分別為100、40、20 cm，航向角每秒6°進(jìn)行變化，分別在直接法和濾波法下進(jìn)行誤差的均方誤差比較，如圖8所示。從圖8可以看出，隨著基線長度的減小誤差方差不斷增大，但基于濾波法的誤差方差小于直接法的誤差方差。證明了濾波法提高了超短基線下動(dòng)態(tài)測姿系統(tǒng)的穩(wěn)定性。

圖8 動(dòng)態(tài)時(shí)不同測姿算法下的航向角誤差方差Fig.8 Heading angle error variance of different attitude measurement algorithms in dynamic condition

圖9為靜態(tài)下基線長度為20 cm時(shí)，利用本文算法與其他算法對200個(gè)歷元內(nèi)航向角為150°采集的數(shù)據(jù)進(jìn)行解算的結(jié)果對比圖；圖10為動(dòng)態(tài)時(shí)基線長度為20 cm，利用本文算法與其他算法分別對60個(gè)歷元內(nèi)航向角以每秒6°變化的數(shù)據(jù)進(jìn)行解算的結(jié)果對比圖。從圖9—圖10可以看出，無論是在靜態(tài)狀態(tài)下還是動(dòng)態(tài)狀態(tài)下，本文提出的算法更加適用于超短基線測姿。

圖9 基線為20 cm靜態(tài)時(shí)航向角誤差變化對比圖Fig.9 Comparison chart of heading angle error when the baseline is 20 cm static state

圖10 基線為20 cm動(dòng)態(tài)時(shí)航向角誤差對比圖Fig.10 Comparison chart of heading angle error when the baseline is 20 cm dynamic state

3 結(jié) 論

本文提出一種提高超短基線姿態(tài)測量穩(wěn)定度與可靠性的GPS測姿系統(tǒng)，利用改進(jìn)擴(kuò)展卡爾曼濾波來為MLAMBDA提供精準(zhǔn)的模糊度浮點(diǎn)解，提高測姿系統(tǒng)可靠性。采用濾波法進(jìn)行姿態(tài)解算，有效地提高了測姿系統(tǒng)的穩(wěn)定性。結(jié)果表明，該系統(tǒng)在基線長度為20 cm的靜態(tài)情況下，誤差方差為0.1左右，并且其航向角的精度為2°左右。該系統(tǒng)在基線長度為20 cm的動(dòng)態(tài)情況下，誤差方差為2左右，并且其航向角的精度為3°左右。與其他測姿系統(tǒng)相比，該系統(tǒng)在測姿可靠性和穩(wěn)定性上都有所提高，且結(jié)構(gòu)簡單，易于集成，成本低，可應(yīng)用于大部分的超短基線測姿場景。