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

王萬(wàn)征，李 洪，郭玉勝

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

?

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

王萬(wàn)征，李 洪，郭玉勝

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

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

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

0 引言

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

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

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

組合導(dǎo)航系統(tǒng)以1s為周期，用卡爾曼濾波對(duì)各項(xiàng)誤差進(jìn)行估計(jì)，并對(duì)速度誤差和水平姿態(tài)角誤差進(jìn)行閉環(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)的量測(cè)量。

觀測(cè)矩陣為

2 可觀測(cè)性分析

2.1 加速度恒定時(shí)航向角誤差可觀測(cè)性分析

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

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

在研究加速度對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)變量可觀測(cè)度之前，首先研究加速度為零即勻速運(yùn)動(dòng)時(shí)捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)變量的可觀測(cè)度。勻速運(yùn)動(dòng)時(shí)，系統(tǒng)在導(dǎo)航坐標(biāo)系三個(gè)方向上的加速度分別為fN=0、fU=g、fE=0。用PWCS法進(jìn)行可觀測(cè)性分析可知，在勻速運(yùn)動(dòng)時(shí)，系統(tǒng)可觀測(cè)性矩陣的秩為7，與靜止時(shí)一樣，這是因?yàn)樵诰暥茸兓梢院雎圆挥?jì)的情況下，系統(tǒng)的狀態(tài)矩陣并未發(fā)生變化。

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

2.2 分段恒定加速運(yùn)動(dòng)時(shí)航向角誤差可觀測(cè)性分析

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

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

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

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

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

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

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

2.3 航向角誤差修正方法

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

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

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

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

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

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

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

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

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

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

3 仿真分析

3.1 仿真條件

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

3.2 仿真結(jié)果

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

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

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

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

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

4 結(jié)論

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

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

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

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

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

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

[6] 趙睿.捷聯(lián)慣性系統(tǒng)初始對(duì)準(zhǔn)研究[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。

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

E-mail：MyForverKaKa@163.com

TP273

A

2095-8110(2015)06-0032-06