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

程建華，王 諾，尚修能

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

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

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

1 SINS/GNSS非線性誤差模型

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

(1)

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

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

(2)

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

(3)

式中

(4)

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

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

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

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

2.1 可觀測(cè)性分析

對(duì)式(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)

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

Zk+1=HXk+1+Vk+1

(13)

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

(14)

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

(15)

(16)

2.2 SVD可觀測(cè)度分析

(17)

式中

(18)

(19)

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

(20)

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

(21)

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

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

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

3 航向角精確估計(jì)

3.1 GNSS航向角約束

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

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

(22)

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

(23)

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

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

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

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

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

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

(24)

3.2.2 自適應(yīng)UKF

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

(25)

(26)

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

(27)

(28)

(29)

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

(30)

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

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

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

(31)

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

(32)

其中，λk如下所示

(33)

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

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

(34)

實(shí)際殘差方差為

(35)

所以，λ0為

(36)

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

4 仿真驗(yàn)證

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

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

5 結(jié)論

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

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

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

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