一種基于自適應(yīng)濾波的GPS滾轉(zhuǎn)角估計方法

劉陽，李懷建，杜小菁

（北京理工大學(xué) 宇航學(xué)院，北京100081）

全球定位系統(tǒng)（GPS）測姿技術(shù)對于衛(wèi)星導(dǎo)航系統(tǒng)和測姿領(lǐng)域都具有非常重要的價值。在GPS系統(tǒng)開發(fā)的早期階段，就認(rèn)識到GPS載波用于載體姿態(tài)測量的可能性。不管是單天線還是多天線的GPS測姿系統(tǒng)，結(jié)構(gòu)都較簡單，成本低廉，國內(nèi)外已經(jīng)提出了大量行之有效的算法［1］。隨著眾多學(xué)者不斷提出和改進(jìn)算法，測量精度不斷提高，GPS測姿技術(shù)已經(jīng)能夠滿足大部分實(shí)際工程需要［2］。但是，當(dāng)前GPS測姿技術(shù)研究沒有充分利用接收機(jī)天線的測量信息，只利用信號功率或者載波相位一種測量信息。

Luo等［3］提出了一種利用單接收機(jī)天線確定旋轉(zhuǎn)載體滾轉(zhuǎn)角速度和滾轉(zhuǎn)角的方法，其利用了接收到的衛(wèi)星信號I／Q幅值和相位受到滾轉(zhuǎn)角調(diào)制的特性，以衛(wèi)星信號I／Q幅值和相位作為量測量，采用擴(kuò)展卡爾曼濾波器對滾轉(zhuǎn)角和滾轉(zhuǎn)角速度進(jìn)行實(shí)時測量，測量精度在300 Hz轉(zhuǎn)速情況下，滾轉(zhuǎn)角均方根誤差為1.8°，轉(zhuǎn)速均方根誤差為0.02Hz。Velde等［4］還提出一種使用2個或多個天線接收的GPS衛(wèi)星信號之間的功率差和相位差來確定旋轉(zhuǎn)載體滾轉(zhuǎn)角速度和滾轉(zhuǎn)角的方法，適合于測量具有短基線的快速旋轉(zhuǎn)載體的滾轉(zhuǎn)角速度和滾轉(zhuǎn)角，在轉(zhuǎn)速為300 Hz時，滾轉(zhuǎn)角測量精度為（7±15）°，轉(zhuǎn)速測量精度為（0.1±0.3）Hz。

本文利用當(dāng)前統(tǒng)計模型進(jìn)行系統(tǒng)建模，將信號功率和多普勒頻率信息作為量測量，并根據(jù)滾轉(zhuǎn)角預(yù)測估計值合理選取，使用無跡卡爾曼濾波（UKF）算法融合2種測量信息得到載體滾轉(zhuǎn)角和滾轉(zhuǎn)角速度，在此基礎(chǔ)上，提出自適應(yīng)濾波算法，自適應(yīng)調(diào)整系統(tǒng)噪聲方差陣，提高濾波估計精度。

1 測量原理及模型

1.1 基于信號功率的滾轉(zhuǎn)角測量

GPS接收機(jī)天線對不同角度的衛(wèi)星入射信號增益不同，形成天線方向圖，正因?yàn)樘炀€方向圖的存在，載體的姿態(tài)變化會對接收到的衛(wèi)星信號功率產(chǎn)生很大影響［5］。如圖1所示，由于載體的遮擋，只有載體的上半表面能接收到衛(wèi)星信號，為了保證在旋轉(zhuǎn)時，有2個天線能接收到信號，應(yīng)將至少4個天線等弧度地安裝在載體表面。每個天線單獨(dú)接收衛(wèi)星信號進(jìn)行處理，信號在各個天線的入射角度不同，每個天線的增益不同，接收的信號功率也不同，將相鄰2個天線接收的信號功率求差，利用信號功率差值與滾轉(zhuǎn)角φ的關(guān)系，即可估計出載體的滾轉(zhuǎn)角。

信號從衛(wèi)星天線發(fā)射到接收機(jī)天線，衛(wèi)星信號功率P的自由空間傳播公式為［6］

式中：PS為衛(wèi)星信號的發(fā)射功率；GS為衛(wèi)星天線在信號發(fā)射方向上的增益；G為接收機(jī)天線在信號入射方向上的增益；λ為信號波長；r為衛(wèi)星與接收機(jī)天線的幾何距離；LA為大氣損耗。接收機(jī)天線增益G與信號入射方向有關(guān)，信號入射方向在天線坐標(biāo)系中由入射方位角α和入射仰角β表示，天線坐標(biāo)系如圖2所示。

圖1 滾轉(zhuǎn)角測量原理Fig.1 Principle of roll angle measurement

圖2 天線坐標(biāo)系Fig.2 Antenna coordinate system

接收機(jī)天線增益G可以以函數(shù)［7］形式表示為

對于同一顆衛(wèi)星在2個天線坐標(biāo)系下的方位角和仰角不同，幾何距離可近似相等，以天線1和天線2為例，有

則天線1和天線2接收的信號功率差值P2，1為

信號功率差值P2，1的變化與接收機(jī)天線增益G相關(guān)。實(shí)際天線的增益特性是通過實(shí)驗(yàn)測量，以表格形式確定。如果已知入射方位角和入射仰角，可以采用二維插值方法從表格中得到天線增益。圖3為某天線的實(shí)測天線方向圖，天頂方向約有8 dB的增益，隨著仰角β減小，天線增益逐漸降低，當(dāng)仰角β小于0°時，迅速變?yōu)閷π盘栍?0 dB的衰減。

衛(wèi)星信號入射方位角α和入射仰角β與衛(wèi)星在天線坐標(biāo)系的坐標(biāo)raS有關(guān)。GPS定位可以得到衛(wèi)星在地心地固直角坐標(biāo)系的坐標(biāo)reS，通過地理坐標(biāo)系和載體坐標(biāo)系，建立reS到raS的坐標(biāo)變換模型，用CX（γ）、CY（γ）和CZ（γ）表示直角坐標(biāo)系分別繞X軸、Y軸和Z軸旋轉(zhuǎn)γ角的坐標(biāo)變換矩陣。

在地理坐標(biāo)系的衛(wèi)星坐標(biāo)rgS為

式中：θ為俯仰角；ψ為航向角。

在天線坐標(biāo)系的衛(wèi)星坐標(biāo)raS為

圖3 天線方向圖Fig.3 Antenna pattern

式中：γA為天線坐標(biāo)系OaYaZa平面與載體坐標(biāo)系OYbZb平面之間的夾角，天線相位中心為天線坐標(biāo)系原點(diǎn)；rbA為原點(diǎn)在載體坐標(biāo)系的坐標(biāo)。

得到衛(wèi)星的坐標(biāo)raS＝［xaSyaSzaS］T，則

接收機(jī)天線可以同時接收多顆可見衛(wèi)星信號，對多顆衛(wèi)星的信號功率求差可以提高滾轉(zhuǎn)角測量的精度。當(dāng)測量n顆衛(wèi)星的信號功率時，有

式中：P為功率測量值，上標(biāo)為衛(wèi)星編號，下標(biāo)為天線編號。

分析可知，接收信號、功率差值是衛(wèi)星位置、載體質(zhì)心位置、載體姿態(tài)角、天線相位中心位置和夾角等參數(shù)的復(fù)雜函數(shù)。在GPS定位過程中，如果通過其他測姿方法得到俯仰角和航向角，則利用接收信號功率差值可以反推出載體的滾轉(zhuǎn)角。

1.2 基于多普勒頻率的滾轉(zhuǎn)角速度測量

接收機(jī)天線接收的衛(wèi)星信號產(chǎn)生多普勒頻率是由于衛(wèi)星運(yùn)動、載體質(zhì)心運(yùn)動和載體繞質(zhì)心轉(zhuǎn)動引起的。由于相鄰2個天線的安裝位置不同，載體的姿態(tài)變化使2個天線相對于載體質(zhì)心的速度不同，導(dǎo)致天線測量的多普勒頻率不同，將多普勒頻率求差，可以消去衛(wèi)星和載體質(zhì)心運(yùn)動產(chǎn)生的多普勒。對于高速旋轉(zhuǎn)的載體，轉(zhuǎn)速每秒可達(dá)幾轉(zhuǎn)，甚至幾十轉(zhuǎn)，俯仰角速度和航向角速度遠(yuǎn)小于滾轉(zhuǎn)角速度，因此，可以認(rèn)為多普勒頻率差值是由于滾轉(zhuǎn)角速度引起的，利用多普勒頻率差值與滾轉(zhuǎn)角速度的關(guān)系，即可估計出載體的滾轉(zhuǎn)角速度。

多普勒頻率f與天線速度V的關(guān)系為［8］

式中：VS為衛(wèi)星速度；δf為接收機(jī)鐘漂；δfS為衛(wèi)星鐘漂；ε為多普勒頻率的測量誤差；I為單位觀測矢量，由衛(wèi)星位置和載體質(zhì)心位置給出：

2個天線對于同一顆衛(wèi)星的觀測矢量I可近似相等，對于天線1和天線2在地心地固直角坐標(biāo)系的速度分別為Ve1和Ve2，有

則天線1和天線2的多普勒頻率差值f2，1為

則利用多普勒頻率差值可以反推出載體的滾轉(zhuǎn)角速度。

2 系統(tǒng)建模

2.1 狀態(tài)方程

在機(jī)動目標(biāo)動力學(xué)理論中，對于機(jī)動目標(biāo)跟蹤，當(dāng)前統(tǒng)計模型被認(rèn)為是一種行之有效的自適應(yīng)跟蹤模型［9］。當(dāng)前統(tǒng)計模型由學(xué)者周宏仁提出，將加速度的一步預(yù)測當(dāng)作是當(dāng)前加速度，并采用該加速度作為修正瑞利分布的均值，實(shí)時地修正加速度的分布，從而實(shí)現(xiàn)閉環(huán)自適應(yīng)跟蹤［10］。本文利用當(dāng)前統(tǒng)計模型對滾轉(zhuǎn)角和滾轉(zhuǎn)角速度測量進(jìn)行系統(tǒng)建模。

根據(jù)當(dāng)前統(tǒng)計模型對滾轉(zhuǎn)角建立狀態(tài)方程：

式中：ˉa為當(dāng)前滾轉(zhuǎn)角加速度的預(yù)測值；Wk為離散時間過程噪聲；

其中：T為采樣周期；α*為機(jī)動頻率，只有通過實(shí)測才能得到α*的值，通常根據(jù)機(jī)動形式取經(jīng)驗(yàn)值，范圍為0＜α*≤1。

式中：amax和a-max分別為滾轉(zhuǎn)角加速度的最大值和最小值。

2.2 量測量的選取

對于GPS接收天線，為了抑制多路徑影響，降低對地面反射的衛(wèi)星信號的接收，通常設(shè)計天線時會減小低仰角方向上的增益，同時考慮到載體對衛(wèi)星信號的遮擋，最好選擇載體上表面的2個天線。天線的選擇可以根據(jù)滾轉(zhuǎn)角，如圖1所示，當(dāng)滾轉(zhuǎn)角為45°時，天頂方向上的衛(wèi)星正好位于天線1和天線2之間，所以選擇這2個天線。

在濾波算法遞推時，根據(jù)k-1時刻到k時刻的滾轉(zhuǎn)角預(yù)測估計值選取2個天線，量測量為2個天線接收的GPS可見星的載波信號功率差值和多普勒頻率差值：

2.3 UKF算法

由于量測方程具有較強(qiáng)的非線性，本文采用UKF進(jìn)行濾波和估計。UKF是sigma點(diǎn)卡爾曼濾波的一種，通過無跡非線性變換逼近非線性系統(tǒng)狀態(tài)的后驗(yàn)均值和協(xié)方差［11］。UKF既可以保證狀態(tài)估計的精度，又能避免對非線性方程的線性化。UKF算法的具體過程可參考文獻(xiàn)［12］。

3 自適應(yīng)濾波算法

式中：C為比例系數(shù)。在文獻(xiàn)［14-15］的基礎(chǔ)上，本文提出滾轉(zhuǎn)角加速度估計自適應(yīng)濾波算法，利用前后時刻滾轉(zhuǎn)角加速度估計值的差值，對滾轉(zhuǎn)角加速度方差的改進(jìn)。如果載體的滾轉(zhuǎn)角加速度改變較小，前后時刻滾轉(zhuǎn)角加速度估計值相差較小，滾轉(zhuǎn)角加速度的分布相對于平均值的偏離程度也較小，即滾轉(zhuǎn)角加速度方差較??；反之，如果載體的滾轉(zhuǎn)角加速度改變較大，前后時刻滾轉(zhuǎn)角加速度估計值相差較大，滾轉(zhuǎn)角加速度的分布相對于平均值的偏離程度也較大，即滾轉(zhuǎn)角加速度方差較大?？梢钥闯觯瑵L轉(zhuǎn)角加速度方差應(yīng)該和前后時刻滾轉(zhuǎn)角加速度估計值的差值相關(guān)，因此可以利用滾轉(zhuǎn)角加速度估計值的差值來估計滾轉(zhuǎn)角加速度方差，參考式（22）和式（23），可以得到一種簡單的自適應(yīng)調(diào)整公式來估計滾轉(zhuǎn)角加速度方差：

仿真中C取1時估計精度較高。從式（24）中可以看出，在一個采樣周期內(nèi)，載體滾轉(zhuǎn)角加速度的變化趨勢能夠反映滾轉(zhuǎn)角加速度的偏離程度，即滾轉(zhuǎn)角加速度方差的大小。通過對滾轉(zhuǎn)角加速度方差的自適應(yīng)調(diào)整，避免了預(yù)先設(shè)定滾轉(zhuǎn)角加速度最值對模型性能的限制。

4 實(shí)驗(yàn)與仿真

為了驗(yàn)證本文提出的基于自適應(yīng)濾波的GPS滾轉(zhuǎn)角估計方法的有效性，設(shè)計實(shí)驗(yàn)將接收機(jī)天線靜止置于室外，接收實(shí)際衛(wèi)星信號10m in，某顆GPS衛(wèi)星的信號功率和多普勒頻率的測量結(jié)果如圖4所示。

GPS衛(wèi)星是在不停運(yùn)動的，會引起多普勒頻率測量值變化，由于測量時間較短，認(rèn)為其他因素的影響較小，可以估計出該天線信號功率的測量均方根誤差約為0.2 dB·W，多普勒頻率測量均方根誤差約為0.4 Hz。

設(shè)計載體轉(zhuǎn)速為勻加速和正弦2種機(jī)動形式的仿真，對UKF算法和本文提出的自適應(yīng)濾波算法進(jìn)行比較。利用導(dǎo)航衛(wèi)星數(shù)仿軟件產(chǎn)生GPS衛(wèi)星和接收機(jī)天線數(shù)據(jù)，根據(jù)實(shí)驗(yàn)結(jié)果設(shè)定仿真中的量測噪聲，載體以100m／s的速度向北飛行，俯仰角和航向角保持不變，仿真時間為10 s，采樣周期T＝100ms，機(jī)動頻率α*＝0.025，在UKF算法中amax和a-max分別取400和-400（°）／s2。

勻加速機(jī)動模擬載體轉(zhuǎn)速從0增大到10 Hz，滾轉(zhuǎn)角加速度設(shè)為）＝360（°）／s2。勻加速機(jī)動的估計結(jié)果如圖5所示。

對于勻加速機(jī)動，滾轉(zhuǎn)角加速度保持不變，自適應(yīng)濾波的滾轉(zhuǎn)角加速度方差比UKF的小，收斂速度更快。

圖4 某顆GPS衛(wèi)星的測量結(jié)果Fig.4 Measurement results of a GPS satellite

正弦機(jī)動模擬載體在10 Hz轉(zhuǎn)速上有一個正弦波動，滾轉(zhuǎn)角加速度設(shè)置為φ¨（t）＝300cos（πt／5）（°）／s2。正弦機(jī)動估計結(jié)果如圖6所示。

對于正弦機(jī)動，滾轉(zhuǎn)角加速度不斷變化，2種濾波都有不同程度的滯后，其中UKF的滾轉(zhuǎn)角加速度方差與當(dāng)前滾轉(zhuǎn)角加速度的預(yù)測值ˉa有關(guān)，在第2.5 s和第7.5 s時，滾轉(zhuǎn)角加速度為0，誤差明顯增大。

2種濾波算法對2種轉(zhuǎn)速機(jī)動形式的滾轉(zhuǎn)角、滾轉(zhuǎn)角速度和滾轉(zhuǎn)角加速度估計的均方根誤差如表1所示。

在實(shí)際使用過程中，載體運(yùn)動可能會對天線的測量精度產(chǎn)生影響，以自適應(yīng)濾波的正弦機(jī)動為例，在其他條件不變的情況下，加入2倍和3倍的量測噪聲，得到估計誤差如圖7所示。

圖5 勻加速機(jī)動的估計結(jié)果Fig.5 Estimation results of constant-acceleration maneuver

圖6 正弦機(jī)動的估計結(jié)果Fig.6 Estimation results of sinusoidal maneuver

表1 2種算法的均方根誤差Table 1 Root mean square error of two algorithms

圖7 不同噪聲下的估計誤差Fig.7 Estimation error under different noises

2倍量測噪聲下的滾轉(zhuǎn)角和滾轉(zhuǎn)角速度的均方根誤差分別為7.2°和34.5（°）／s，3倍量測噪聲下的滾轉(zhuǎn)角和滾轉(zhuǎn)角速度的均方根誤差分別為12.6°和48.7（°）／s。由圖7可以看出，同樣在第2.5 s和第7.5 s時，滾轉(zhuǎn)角加速度為0，但是前后時刻的滾轉(zhuǎn)角加速度改變最大，所以量測噪聲越大，誤差也越大，滯后也越明顯。

5 結(jié) 論

1）本文分析了GPS信號功率與載體滾轉(zhuǎn)角、多普勒頻率與載體滾轉(zhuǎn)角速度的關(guān)系，采用了UKF算法融合GPS信號功率和多普勒頻率測量信息，并提出了滾轉(zhuǎn)角加速度估計自適應(yīng)濾波算法改進(jìn)模型，仿真結(jié)果表明，濾波算法融合2種信息是可以對載體的滾轉(zhuǎn)角和滾轉(zhuǎn)角速度進(jìn)行測量的，且天線的信號功率和多普勒頻率測量精度對估計精度影響很大。

2）與UKF算法相比，自適應(yīng)濾波算法不僅能對各種轉(zhuǎn)速機(jī)動形式進(jìn)行估計，而且避免了滾轉(zhuǎn)角加速度最值選取導(dǎo)致對濾波性能的影響，更有效地抑制了滾轉(zhuǎn)角、滾轉(zhuǎn)角速度和滾轉(zhuǎn)角加速度的誤差跳變幅度，減小了均方根誤差，具有更好的濾波估計能力，大幅度地提高了測量精度。