王萬征，李 洪，郭玉勝

(北京自動化控制設(shè)備研究所，北京100074)

?

一種車載組合導(dǎo)航系統(tǒng)航向修正方法研究

王萬征，李 洪，郭玉勝

(北京自動化控制設(shè)備研究所，北京100074)

針對車載組合導(dǎo)航系統(tǒng)航向誤差難以修正的問題，對航向角誤差可觀測性進行分析，并提出一種基于加速度變化的航向誤差可觀測性分析的誤差修正方法。首先利用分段線性定常系統(tǒng)(PWCS)可觀測性分析法和奇異值(SVD)分析法對不同加速度變化情況下的組合導(dǎo)航系統(tǒng)航向角誤差進行可觀測性分析，得出航向角誤差的可觀測度與加速度變化的劇烈程度正相關(guān)的結(jié)論。在不同加速度變化情況下，對航向角誤差的濾波估計進行仿真，總結(jié)出加速度變化過程中的收斂規(guī)律，從而提出根據(jù)加速度變化情況對航向誤差進行修正的方法。最后實驗結(jié)果表明，通過該方法能夠有效地減小航向角誤差導(dǎo)致的位置誤差，提高導(dǎo)航精度。

車載組合導(dǎo)航系統(tǒng)；可觀測性；加速度；航向角誤差

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)是一種自主式導(dǎo)航系統(tǒng)，能夠自主提供姿態(tài)、速度和位置信息，在導(dǎo)航領(lǐng)域得到了廣泛應(yīng)用，但是其誤差會隨著時間不斷積累。由于里程計誤差不會隨時間發(fā)散，航位推算系統(tǒng)(DR)導(dǎo)航誤差隨時間變化相對SINS較小，所以在車載導(dǎo)航系統(tǒng)中，常用航位推算更新系統(tǒng)的位置信息。如果載車上已經(jīng)裝有捷聯(lián)慣導(dǎo)系統(tǒng)，利用SINS/DR組合導(dǎo)航系統(tǒng)將具有較高的精度和容錯性能[1-2]。但由于SINS和DR共用姿態(tài)矩陣，所以SINS/DR組合導(dǎo)航系統(tǒng)難以對系統(tǒng)的航向做出有效估計。SINS/GPS組合導(dǎo)航系統(tǒng)可以有效抑制位置發(fā)散，但是系統(tǒng)的速度或者位置會發(fā)生跳變，在需要精確路線時無法滿足要求。在SINS/DR組合導(dǎo)航系統(tǒng)中，可以通過卡爾曼濾波對水平姿態(tài)誤差角進行精確估計從而減小水平姿態(tài)角引起的航位推算誤差，而航向角誤差引起的位置誤差無法消除。SINS/GPS組合導(dǎo)航系統(tǒng)由于其航向角誤差在很多情況下具有不可觀測性，故而難以實時估計，而傳統(tǒng)的通過判斷濾波收斂的方式對航向誤差進行估計和修正的方法，在閉環(huán)系統(tǒng)中由于速度實時修正，航向誤差引起的速度誤差信息難以積累，難以準確估計航向角誤差。本文在不同加速度變化情況下對系統(tǒng)的航向角誤差進行可觀測性分析，得出加速度變化與航向角誤差可觀測度的聯(lián)系，利用GPS量測信息對航向角進行修正，降低了航向角誤差誤判率，提高了航位推算系統(tǒng)導(dǎo)航精度。

1 車載組合導(dǎo)航系統(tǒng)

選取北—天—東(NUE)地理坐標系為導(dǎo)航坐標系，記為n系；IMU與載車固聯(lián)，認為IMU坐標系與載車坐標系重合，記為b系；里程計坐標系記為m系，其Ox軸在和載車車輪相接觸地平面內(nèi)并指向行駛方向，Oz向右，Oy向上。

組合導(dǎo)航系統(tǒng)以1s為周期，用卡爾曼濾波對各項誤差進行估計，并對速度誤差和水平姿態(tài)角誤差進行閉環(huán)修正。其結(jié)構(gòu)示意圖如圖1所示。

圖1 車載組合導(dǎo)航系統(tǒng)結(jié)構(gòu)示意圖Fig.1 Structure of vehicle integrated navigation system

組合導(dǎo)航系統(tǒng)狀態(tài)量為

X=[δVNδVEφNφUφEεxεy

εzxyz]

組合導(dǎo)航系統(tǒng)的狀態(tài)矩陣為

其中，

以航位推算解算的速度與捷聯(lián)慣導(dǎo)系統(tǒng)解算的速度做差，將該差值作為系統(tǒng)的量測量。

觀測矩陣為

2 可觀測性分析

2.1 加速度恒定時航向角誤差可觀測性分析

組合導(dǎo)航系統(tǒng)濾波模型為時變系統(tǒng)，時變系統(tǒng)可以應(yīng)用PWCS可觀測性分析方法對其可觀測性進行定性分析[3-4]。要知道航向的可觀測程度，則需要對系統(tǒng)狀態(tài)變量可觀測度概念進行定量分析，可用基于系統(tǒng)可觀測性矩陣奇異值分解的可觀測度分析的奇異值方法，即SVD法。

用離散PWCS的SOM代替TOM研究離散系統(tǒng)的可觀測性可以大大簡化分析，降低狀態(tài)量[5]。離散PWC提取的可觀測性矩陣(SOM)為

在研究加速度對捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)變量可觀測度之前，首先研究加速度為零即勻速運動時捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)變量的可觀測度。勻速運動時，系統(tǒng)在導(dǎo)航坐標系三個方向上的加速度分別為fN=0、fU=g、fE=0。用PWCS法進行可觀測性分析可知，在勻速運動時，系統(tǒng)可觀測性矩陣的秩為7，與靜止時一樣，這是因為在緯度變化可以忽略不計的情況下，系統(tǒng)的狀態(tài)矩陣并未發(fā)生變化。

勻加速運動時，假設(shè)系統(tǒng)在導(dǎo)航坐標系三個方向上的加速度分別為fN=1、fU=g、fE=1。進行可觀測性分析可知，在勻加速運動時，系統(tǒng)可觀測性矩陣的秩提升為8，但是航向角誤差的可觀測度很小，原因在于速度不大時，速度變化對狀態(tài)矩陣的影響很小。例如：VN=10m/s，系統(tǒng)狀態(tài)矩陣中，VN/(RM+H)=1.7291×10-6rad/s，比地球的自轉(zhuǎn)角速度ωie小一個數(shù)量級以上，所以并不能顯著提升航向角誤差的可觀測性。

2.2 分段恒定加速運動時航向角誤差可觀測性分析

用分段恒定加速運動來代替系統(tǒng)連續(xù)的線性機動進行可觀測性分析[6]。假設(shè)四種不同的勻加速運動，在地理坐標系下各方向上加速度如表1所示。

表1 分段恒定加速度Tab.1 Piecewise constant acceleration

表2 航向角誤差可觀測度Tab.2 Observability of heading error

從表2中可以看出，在勻速運動或勻加速運動時，可以認為航向角誤差不可觀測；只有當(dāng)加速度變化時，航向角誤差才可觀測。而且可以看出航向角誤差的可觀測度與加速度變化向量的大小有關(guān)。由于在車載組合導(dǎo)航系統(tǒng)中，系統(tǒng)在線運動時只有前向速度，所以航向角誤差的可觀測度與組合導(dǎo)航系統(tǒng)的前向加速度變化有關(guān)，而且可以較為容易地判定載車加速度變化情況。

本文只對加速度方向不變而大小發(fā)生變化時的情況進行分析。假設(shè)只存在北向加速度，在不同的加速度變化情況下航向角誤差的可觀測度如表3所示。

表3 航向角誤差可觀測度分析Tab.3 Observability analysis of heading error

從表3中可以看出，在一段時間內(nèi)，航向角誤差可觀測度與加速度變化的劇烈程度正相關(guān)，而與加速度本身的大小關(guān)系不大。在加速度變化趨勢一致時，變化幅度越大，航向角誤差可觀測度越高；在加速度變化的幅度一樣時，變化越劇烈，則航向角誤差可觀測度越高，所以可通過加速度的變化情況來間接判斷組合導(dǎo)航系統(tǒng)航向角誤差的可觀測度。

2.3 航向角誤差修正方法

在已知航向角誤差與加速度變化劇烈程度相關(guān)的情況下，對分段恒定加速運動時航向角誤差的收斂情況進行分析。假設(shè)航向角存在0.2°的誤差，在不同的加速度變化情況下其航向誤差估計如圖2所示(濾波周期為1s)。

圖2 航向角誤差的卡爾曼濾波估計Fig.2 Kalman filtering estimation of heading error

在慣導(dǎo)系統(tǒng)載體運動過程中，一定存在一個加速度從無到有和從有到無的變化過程，這個過程中，航向角誤差具有可觀測性。從圖2可以得出以下結(jié)論：

1)在系統(tǒng)產(chǎn)生加速度即系統(tǒng)由靜止或勻速運動變?yōu)榧铀龠\動時，航向角誤差迅速收斂逼近真實值，如果加速度不發(fā)生變化，其估計精度不會進一步提高。當(dāng)加速度再變?yōu)?即由加速運動再變化為靜止或勻速運動時，航向角誤差繼續(xù)收斂，此時收斂速度較慢，在加速度變?yōu)?后航向角誤差估計值逐漸穩(wěn)定。

2)在同樣的加速度變化幅度下，相對于加速度在某個值保持一段時間，如果加速度只發(fā)生一個擾動，則航向角誤差的估計精度較低。前面分析航向角誤差可觀測度與加速度變化劇烈程度正相關(guān)，這是在分段恒定加速運動情況下分析的，如果加速度只是發(fā)生一個擾動，由于短時間內(nèi)在該加速度下航向角誤差引起的速度誤差較小，所以濾波器不能精確估計航向角誤差。

3)在同樣的變化趨勢下，加速度變化幅度越大，則航向角誤差估計精度越高。這與前面加速度變化時航向角誤差的可觀測度分析結(jié)果相吻合。

由上面的分析，可以得出在組合導(dǎo)航系統(tǒng)工作過程中判斷航向角誤差可觀測性并修正誤差的方法，按照以下步驟進行：

1)在加速度發(fā)生明顯變化時，停止水平姿態(tài)和慣導(dǎo)速度的閉環(huán)修正。因為短時間的導(dǎo)航過程中，閉環(huán)修正不利于速度輸出中關(guān)于姿態(tài)誤差的信息濃度的積累[7]，影響航向角誤差的估計精度。

2)監(jiān)測加速度值的變化，如果加速度變化值在一定時間內(nèi)超過臨界值，且沒有立即恢復(fù)到0，則準備修正航向誤差，否則繼續(xù)閉環(huán)修正。前面分析已知，航向角誤差的可觀測性與加速度變化劇烈程度有關(guān)，由此可以間接判斷航向角誤差的可觀測性，而且要排除加速度只是發(fā)生一個擾動的情況。

3)系統(tǒng)加速度值再次穩(wěn)定，并且濾波收斂后，對航向角進行修正，此后繼續(xù)系統(tǒng)對水平姿態(tài)角和慣導(dǎo)速度的閉環(huán)修正。由前面分析已知，加速度穩(wěn)定時，航向角誤差仍在收斂過程中，在一段時間后才達到穩(wěn)定值。

3 仿真分析

3.1 仿真條件

圖3 仿真軌跡及速度Fig.3 Experimental trajectory and speed

3.2 仿真結(jié)果

如果在載車行進過程中只對水平姿態(tài)誤差做閉環(huán)修正，其航向角誤差和位置誤差如圖4所示。

圖4 航向角及位置誤差Fig.4 Heading error and position error

從圖4中可以看出，如果不對航向角進行修正，航向失準角和陀螺漂移帶來的誤差會使導(dǎo)航系統(tǒng)的位置誤差發(fā)散，尤其在與載車行進速度相垂直的方向上，影響更為明顯。按前文所述方法對航向做修正后，其航向角誤差和位置誤差如圖5所示。

圖5 航向角及位置誤差Fig.5 Heading error and position error

從圖5中可以看出，在有加速度變化的情況下，組合導(dǎo)航系統(tǒng)估計出航向角誤差，對航向進行修正，不但消除了航向失準角的影響，而且有效抑制了陀螺漂移引起的航向角誤差，位置精度明顯提高。

4 結(jié)論

實驗數(shù)據(jù)分析表明，通過該方法使車載組合導(dǎo)航系統(tǒng)的航向角誤差能夠在其可觀測度較好時得到有效估計，從而提高組合導(dǎo)航系統(tǒng)的精度。而且該方法是通過加速度變化間接判定航向角誤差可觀測性，增加的計算量小。該方法適用于載體有加速度變化的情況，運動平穩(wěn)的載體航向誤差估計可通過羅經(jīng)回路實現(xiàn)。

[1] 李旦，秦永元，張金亮.車載慣導(dǎo)航位推算組合導(dǎo)航系統(tǒng)誤差補償研究[J].計算機測量與控制，2011，19(2)：389-391.

[2] 嚴恭敏，秦永元，楊波.車載航位推算系統(tǒng)誤差補償技術(shù)研究[J].西北工業(yè)大學(xué)學(xué)報，2006，24(2)：26-30.

[3] 嚴濤，王躍鋼，楊波，等.SINS/DR組合導(dǎo)航系統(tǒng)可觀測性研究[J].現(xiàn)代防御技術(shù)，2012，40(3)：83-87.

[4] 孔星煒，董景新，吉慶昌,等.一種基于PWCS的慣導(dǎo)系統(tǒng)可觀測度分析方法[J].中國慣性技術(shù)學(xué)報，2011，19(6)：631-636.

[5] 付夢印，鄭辛，鄧志紅，等.傳遞對準理論與應(yīng)用[M].北京：科學(xué)出版社，2012.

[6] 趙睿.捷聯(lián)慣性系統(tǒng)初始對準研究[D].南京：東南大學(xué)，2006.

[7] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.

Research on Heading Error Correction of Vehicle Intergrated Navigation System

WANG Wan-zheng，LI Hong，GUO Yu-sheng

(Beijing Institute of Automatic Control Equipment，Beijing 100074，China)

Due to heading error on vehicle integrated navigation system can not be corrected，a method is proposed to judge the observability of the heading error by acceleration and correct the heading error when the error is observable.Firstly the observability of heading error is analyzed by the analysis method of piece-wise constant systems(PWCS)and method of singular value decomposition(SVD).Positive correlation is found between the observabilities of heading error and the changing amplitude of acceleration.Carry out simulation test upon Kalman filter on different acceleration standards，the correct method is proposed.Finally the results of experiment indicate that the method can correct heading error and improve the precision of position effectively.

Vehicle integrated navigation system；Observability；Acceleration；Heading error

2015 - 08 - 22；

2015 - 09 - 23。

王萬征(1991 - )，男，碩士，主要從事慣性導(dǎo)航及組合導(dǎo)航方面的研究。

E-mail：MyForverKaKa@163.com

TP273

A

2095-8110(2015)06-0032-06