基于改進(jìn)UKF的組合導(dǎo)航系統(tǒng)航向角估計方法研究

程建華，王 諾，尚修能

(哈爾濱工程大學(xué)自動化學(xué)院，哈爾濱 150001)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)與全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System，GNSS)在誤差傳播性能上互補，組合之后的系統(tǒng)精度、性能和可靠性等多個方面均優(yōu)于單獨的子系統(tǒng)，具有良好的應(yīng)用前景[1]。SINS/GNSS通常用于陸地車輛導(dǎo)航，為其提供可靠的速度、位置和姿態(tài)信息。伴隨著微機電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)技術(shù)的快速發(fā)展，基于MEMS技術(shù)的慣性器件因具有低成本、低功耗、小體積以及可大批量生產(chǎn)等優(yōu)勢[2]，受到了廣泛關(guān)注。但由于成本的降低導(dǎo)致MEMS陀螺儀的輸出存在許多不確定性誤差，使得陀螺儀的姿態(tài)精度降低，從而導(dǎo)致整個導(dǎo)航系統(tǒng)的精度下降[3]。因此，必須提出一種能夠提高M(jìn)EMS-SINS/GNSS組合導(dǎo)航系統(tǒng)姿態(tài)估計精度的解決方案。

采用速度、位置和姿態(tài)全組合方式進(jìn)行組合導(dǎo)航，雖然相較于傳統(tǒng)的位置和速度組合方式的姿態(tài)角精度有了較大改善，但在實際應(yīng)用中實現(xiàn)復(fù)雜，抗干擾性差，且只適用于多天線衛(wèi)星導(dǎo)航系統(tǒng)[4-5]。文獻(xiàn)[1]在簡化組合導(dǎo)航模型的基礎(chǔ)上進(jìn)行姿態(tài)估計，使用常規(guī)組合導(dǎo)航的算法解算橫搖角與縱搖角，對于航向角則采用級聯(lián)卡爾曼濾波算法進(jìn)行單獨估計。這種方法雖然能夠得到精度較高的姿態(tài)角，但是系統(tǒng)運算量增大，從而導(dǎo)致其實時性變差[6]。而文獻(xiàn)[7]則研究了線性系統(tǒng)下，基于改進(jìn)常規(guī)卡爾曼濾波的姿態(tài)估計算法，但是該算法并不能直接應(yīng)用于非線性系統(tǒng)中[8]。因此，目前需要一種針對非線性系統(tǒng)的導(dǎo)航估計算法，在提高導(dǎo)航精度的同時減少計算量，提升系統(tǒng)的實時性。

針對上述問題，本文建立了以速度、位置和航向角誤差為系統(tǒng)狀態(tài)矢量的組合導(dǎo)航模型。首先對傳統(tǒng)的速度和位置組合導(dǎo)航非線性系統(tǒng)進(jìn)行可觀測性分析，為了增強航向角的可觀測性、抑制航向角易發(fā)散的問題，輔助以GNSS航向角作量測量；采用轉(zhuǎn)彎判斷規(guī)則和自適應(yīng)算法降低了GNSS測量誤差對姿態(tài)估計的影響。上述方法具有良好的適應(yīng)性，導(dǎo)航精度高、實時性強，滿足運載體的導(dǎo)航需求。

1 SINS/GNSS非線性誤差模型

由于捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型本質(zhì)上為非線性的，并受限于慣性器件本身的測量誤差等因素，使得系統(tǒng)顯示出較強的非線性和時變性。直接建立非線性誤差模型，能夠減少約束條件，使得誤差模型能夠更準(zhǔn)確地描述系統(tǒng)的誤差傳播特性，有助于分析并提高系統(tǒng)的導(dǎo)航性能。

(1)

式中，各項變量定義見文獻(xiàn)[6]。

由誤差方程建立濾波狀態(tài)微分方程為[11]

(2)

由于陸地車輛導(dǎo)航不需要考慮天向位置和速度，所以量測方程為

(3)

式中

(4)

其中，Z(t)是l×1觀測向量；ZV和ZP分別由SINS和GNSS的速度和位置的差值得出；V為零均值噪聲向量[12]。離散化狀態(tài)方程和量測方程之后，對SINS/GNSS進(jìn)行積分，獲得長時精度的位置和速度。系統(tǒng)原理如圖1所示。

圖1 MEMS-SINS/GNSS系統(tǒng)原理Fig.1 System principle of MEMS-SINS/GNSS

2 非線性系統(tǒng)可觀測性分析

對系統(tǒng)進(jìn)行可觀測性分析能夠確定卡爾曼濾波效果的好壞，系統(tǒng)狀態(tài)的可觀測性可以反映該狀態(tài)的估計精度，并確定收斂速度的快慢[13]。對于不可觀測的狀態(tài)，系統(tǒng)無法精確估計甚至不能估計。可觀測性分析中的定性分析為確定狀態(tài)是否可觀測，定量分析為確定狀態(tài)的可觀測程度，即可觀測度[14]。通常，對于線性時變系統(tǒng)采用由Goshen-Meskin和Bar-Itzhack提出的分段線性定常系統(tǒng)(Piece Wise Constant System，PWCS)可觀測性分析方法，即PWCS方法[15]。可觀測度分析通常采用基于PWCS的奇異值分解(Singular Value Decomposition，SVD)方法，即SVD方法[16]。但是目前所有線性系統(tǒng)的可測性分析方法都無法直接應(yīng)用于非線性系統(tǒng)，并且非線性系統(tǒng)的可觀測性分析現(xiàn)在沒有統(tǒng)一的定義方法。由于現(xiàn)有的非線性系統(tǒng)的分析方法實現(xiàn)復(fù)雜，難于應(yīng)用，因此通過將非線性系統(tǒng)線性化為線性系統(tǒng)，然后使用PWCS方法和SVD方法對其進(jìn)行可觀測性和可觀測度分析。

2.1 可觀測性分析

對式(2)狀態(tài)微分方程中的t在[tk,tk+1]上積分得

(5)

(6)

將f(X(t))在Xk處進(jìn)行Taylor展開得

?f(Xk)+F(Xk)(X(t)-Xk)

(7)

將X(t)在tk處進(jìn)行Taylor展開得

(8)

將式(8)代入式(7)得

f(X(t))=f(Xk)+F(Xk)f(Xk)(t-tk)

(9)

將式(9)代入式(6)并積分得

(10)

式中，T=tk+1-tk。此即為得到的離散線性化狀態(tài)方程。

(11)

所以，可以得出

(12)

對式(3)量測方程直接進(jìn)行離散化得

Zk+1=HXk+1+Vk+1

(13)

結(jié)合式(13)得到

(14)

所有時間段i=1,2,…,j的總觀測矩陣(Total Observability Matrix, TOM)構(gòu)造如下

(15)

(16)

2.2 SVD可觀測度分析

(17)

式中

(18)

(19)

其中，X(0)是初始狀態(tài)矢量，經(jīng)過矩陣變換可得

(20)

其中，Xi(0)是奇異值σi的初始狀態(tài)矢量。所以第k個狀態(tài)的可觀測度定義為

(21)

其中，σi是使Xi(0)取得最大值的奇異值，σ0為觀測量對應(yīng)的奇異值。表1給出了在不同的外部觀測量條件下的非線性系統(tǒng)可觀測度分析結(jié)果。

表1 不同外部觀測量條件下的系統(tǒng)可觀測度

分析表1可知，增加外部觀測量有效地提高了系統(tǒng)狀態(tài)量的可觀測程度。

3 航向角精確估計

3.1 GNSS航向角約束

速度位置組合導(dǎo)航系統(tǒng)通過常規(guī)無跡卡爾曼濾波器(Unscented Kalman Filter，UKF)濾波后可得到精度較高的俯仰角和橫滾角。但是由于航向角可觀測性弱，當(dāng)載體為勻速直線行駛或者靜止?fàn)顟B(tài)時，航向角誤差較大，即使是在機動情況下，航向角的估計誤差也不容忽略。所以，可通過增加航向角觀測約束，提高航向角的可觀測性和姿態(tài)角的精度。

單天線GNSS在多數(shù)情況下都可以給出速度、位置和航跡角。單天線GNSS航跡角為

(22)

式中，vE和vN分別為地理坐標(biāo)系下單天線GNSS測得的東向和北向速度。在量測方程中加入航向約束，此時，量測量變?yōu)?/p>

(23)

式中，Zk為k時刻的量測向量；ΔVk、Δpk分別為k時刻MEMS傳感器積分得到的速度、位置信息和GNSS獲取的速度、位置信息的差值；Δφk為單天線GNSS航跡角與MEMS陀螺儀航向角的差值。

通過加入單天線GNSS航向約束可以解決航向角可觀測性弱的問題，提高估計精度。

3.2 改進(jìn)型濾波器

當(dāng)運載體處于直線運動狀態(tài)時，GNSS得到的航跡角與運載體的航向角是一致的，不存在偏流角的影響；但是當(dāng)運載體處于轉(zhuǎn)彎狀態(tài)時，由于偏流角的存在，導(dǎo)致測量得到的航跡角與真實的航向角之間存在誤差。由于系統(tǒng)先天存在這種誤差，所以運載體在測量到轉(zhuǎn)彎狀態(tài)后，需要關(guān)閉GNSS航向輔助，短時間內(nèi)僅通過陀螺積分來維持航向角的輸出。但是由于MEMS-IMU的精度低，即使是短時間內(nèi)關(guān)閉GNSS航向輔助也可能造成比較大的航向誤差。為解決這個問題，對UKF進(jìn)行了改進(jìn)，從而有效地抑制了由于MEMS精度低而導(dǎo)致的航向誤差增大。

3.2.1 轉(zhuǎn)彎判斷規(guī)則

通過單天線GNSS航向約束改善了航向角可觀測性弱的問題，但是在載體轉(zhuǎn)彎時會產(chǎn)生偏流角，GNSS測得的載體航跡角實際上為真實的航向角與偏流角之和，使得測量值與實際值之間產(chǎn)生了偏差。為了消除偏流角對系統(tǒng)的影響，在檢測到車輛轉(zhuǎn)彎時，關(guān)閉航向角觀測。對于車輛轉(zhuǎn)彎的判斷可以通過測量轉(zhuǎn)彎角速率的航向角陀螺的輸出值導(dǎo)出，即

(24)

3.2.2 自適應(yīng)UKF

常規(guī)UKF算法見文獻(xiàn)[6]。對于系統(tǒng)中未知的噪聲，利用基于Sage-Husa的極大后驗估值算法對Qk和Rk進(jìn)行估計。

(25)

(26)

結(jié)合UKF中的Pk/k-1則式(26)為

(27)

(28)

(29)

在這個基礎(chǔ)上再采用漸消指數(shù)加權(quán)的方法，推導(dǎo)出非線性系統(tǒng)時變噪聲統(tǒng)計估計器為

(30)

式中，dk=(1-b)/(1-bk)，b為遺忘因子，0

3.2.3 改進(jìn)模糊自適應(yīng)UKF

Sage-Husa濾波器通常會出現(xiàn)濾波發(fā)散的問題，為了解決這個問題，考慮在協(xié)方差匹配方法的基礎(chǔ)上引入對濾波發(fā)散趨勢的判斷規(guī)則，即

(31)

其中，α為一可調(diào)的儲備系數(shù)(α≥1)，依據(jù)實際情況進(jìn)行設(shè)定；tr()為矩陣的跡。當(dāng)式(31)成立時，表示濾波效果穩(wěn)定;當(dāng)該式不成立時，就需要對Pk/k-1進(jìn)行修正，使其變?yōu)?/p>

(32)

其中，λk如下所示

(33)

其中，λ0為模糊UKF的殘差理論值與實際值之間的比值。

模糊UKF的殘差方差理論值為

(34)

實際殘差方差為

(35)

所以，λ0為

(36)

由此，通過自適應(yīng)加權(quán)系數(shù)λk對Pk/k-1的修正，來抑制濾波的發(fā)散情況。

4 仿真驗證

為驗證本文所提出的算法的有效性，進(jìn)行了車載實驗仿真驗證，設(shè)置初始地理位置為北緯45°、東經(jīng)126°，陀螺漂移誤差為20(°)/h，加速度計零偏5×10-3g，GNSS的速度測量誤差為0.01m/s，位置測量誤差為10m，MEMS-SINS解算頻率為100Hz，系統(tǒng)濾波頻率為1Hz，仿真時長為3h，運行軌跡包括靜止、勻速直線、加速、減速和轉(zhuǎn)彎等狀態(tài)。勻速直線運動時，北向速度為3m/s；加速運動狀態(tài)時，北向加速度為2m/s2；減速時，北向加速度為-2m/s2；轉(zhuǎn)彎狀態(tài)時，航向角速率為-30(°)/s，轉(zhuǎn)彎時長為3s，GNSS航向角量測存在偏流角。結(jié)果如下所示，圖2～圖4分別為常規(guī)UKF在勻速直線狀態(tài)下的姿態(tài)、速度和位置誤差曲線圖；圖5～圖7分別為加入航向約束并在330s加入轉(zhuǎn)彎狀態(tài)的UKF算法與加入航向約束后改進(jìn)UKF所得誤差曲線的對比圖。

圖2 UKF姿態(tài)誤差曲線圖Fig.2 UKF attitude error curve

圖3 UKF速度誤差曲線圖Fig.3 UKF speed error curve

圖4 UKF位置誤差曲線圖Fig.4 UKF position error curve

圖5 航向約束UKF和改進(jìn)UKF姿態(tài)誤差曲線圖Fig.5 Course constraint UKF and improved UKF attitude error curve

圖6 航向約束UKF和改進(jìn)UKF速度誤差曲線圖Fig.6 Course constraint UKF and improved UKF speed error curve

圖7 航向約束UKF和改進(jìn)UKF位置誤差曲線圖Fig.7 Course constraint UKF and improved UKF position error curve

分析上述仿真結(jié)果，由圖2可以看出，在無航向輔助常規(guī)UKF算法中，勻速直線狀態(tài)下，由于航向角可觀測性弱，UKF之后航向角誤差與理想誤差值之間始終存在差值，且無法通過改變初始值的取值而改善濾波的估值。由圖5～圖7可知，在加入航向角約束輔助后的直線行駛階段，同無輔助姿態(tài)估計算法相比，航向角能夠較好的收斂，但是幅值振蕩較大，且在轉(zhuǎn)彎狀態(tài)下，姿態(tài)角出現(xiàn)了不同程度的發(fā)散，這是由于GNSS航向角在轉(zhuǎn)彎時出現(xiàn)的偏流角等其他因素導(dǎo)致的量測誤差較大引起的。所以，為了能夠使轉(zhuǎn)彎時得到更好的估算效果，則需要改進(jìn)濾波算法，通過引入轉(zhuǎn)彎判斷規(guī)則和改進(jìn)自適應(yīng)UKF來增強算法的適應(yīng)性，提高估計精度。由圖5可以看出，姿態(tài)角的估計精度得到明顯的提升，水平姿態(tài)誤差控制在0.01°以內(nèi)，航向角誤差控制在0.1°以內(nèi)，速度誤差控制在0.02m/s以內(nèi)，位置誤差控制在1m以內(nèi)。由此可以證明，該算法能夠很好地完成運載體的長航時、高精度的導(dǎo)航需求。

5 結(jié)論

本文針對系統(tǒng)可觀測性弱導(dǎo)致的姿態(tài)估計差等問題，提出了一種基于MEMS的非線性組合導(dǎo)航系統(tǒng)航向角估計算法。算法分析與實驗結(jié)果證明：

1)通過采用單天線GNSS航跡角對系統(tǒng)進(jìn)行航向約束的方法，解決了因航向角可觀測性弱引起的姿態(tài)估計精度差的問題，實現(xiàn)了姿態(tài)角高精度估計。

2)通過采用轉(zhuǎn)彎判斷規(guī)則判斷車輛是否處于轉(zhuǎn)彎狀態(tài)，并在檢測到轉(zhuǎn)彎狀態(tài)后采用改進(jìn)的Sage-Husa自適應(yīng)無跡卡爾曼濾波器，解決了由于轉(zhuǎn)彎時GNSS航向角量測存在偏流角而影響姿態(tài)估計精度的問題，從而得到了較好的估計效果。

3)通過仿真實驗結(jié)果可以證明，本文提出的基于單天線GNSS的非線性系統(tǒng)航向角估計方法滿足了低成本導(dǎo)航系統(tǒng)高可靠性和高精度的要求，具有重要的工程應(yīng)用價值。