低成本MIMU/GPS車載組合測(cè)量系統(tǒng)的導(dǎo)航算法設(shè)計(jì)*

劉 江,呂妍紅

(北京航空航天大學(xué),北京 100191)

低成本MIMU/GPS車載組合測(cè)量系統(tǒng)的導(dǎo)航算法設(shè)計(jì)*

劉 江,呂妍紅

(北京航空航天大學(xué),北京 100191)

文中的組合測(cè)量系統(tǒng)用于測(cè)量汽車的位置、速度和姿態(tài)信息,為提高測(cè)量精度,文中設(shè)計(jì)的導(dǎo)航算法為:系統(tǒng)做近似直線運(yùn)動(dòng)時(shí),采用基于位置、速度、航向組合的導(dǎo)航算法;系統(tǒng)做轉(zhuǎn)彎運(yùn)動(dòng)時(shí),采用基于位置、速度組合的導(dǎo)航算法。實(shí)際跑車實(shí)驗(yàn)表明,系統(tǒng)位置精度為3 m(CEP)、速度精度為0.25 m/s、航向精度為1°,相對(duì)單純的位置、速度組合導(dǎo)航算法,系統(tǒng)的測(cè)量精度有所提高,尤其航向測(cè)量精度提高了1倍,從而說(shuō)明文中所述導(dǎo)航算法的有效性。

微慣性測(cè)量單元;航向組合;卡爾曼濾波;組合導(dǎo)航

0 引言

慣導(dǎo)系統(tǒng)作為一種重要的自主式導(dǎo)航設(shè)備,其具有隱蔽性好、抗干擾、不受氣象條件限制、輸出導(dǎo)航參數(shù)全面且輸出頻率高的優(yōu)點(diǎn),已廣泛應(yīng)用于航空、航天、航海和陸地導(dǎo)航中[1-2]。但是,慣導(dǎo)系統(tǒng)高昂的成本,使其主要用于國(guó)防領(lǐng)域,在民用領(lǐng)域的應(yīng)用很少。慣導(dǎo)系統(tǒng)的成本主要受限于慣性器件的成本,特別是陀螺儀的成本。近年來(lái),慣性器件特別是MEMS陀螺儀和加速度計(jì)取得了非常顯著的進(jìn)步,其以尺寸小、重量輕、低成本、微功耗、可靠性高、易于批量加工、耐振動(dòng)和沖擊等優(yōu)點(diǎn)而被大量用于許多民用領(lǐng)域。目前基于慣性測(cè)量、衛(wèi)星測(cè)量及其組合測(cè)量特別是低成本的MEMS陀螺儀和加速度計(jì)與GPS的組合測(cè)量在汽車位置姿態(tài)測(cè)量中應(yīng)用發(fā)展迅速[3]。

SINS/GPS的組合方式包括淺組合、緊組合和深組合,其中淺組合由于工程實(shí)現(xiàn)容易,因而應(yīng)用廣泛[4]。淺組合是基于位置、速度量測(cè)構(gòu)建卡爾曼濾波,其對(duì)慣導(dǎo)系統(tǒng)位置、速度誤差具有很好的抑制作用,但對(duì)航向誤差的抑制作用小,航向仍會(huì)因誤差積累而發(fā)散。文中將在基于位置、速度組合的導(dǎo)航算法基礎(chǔ)上,再添加基于位置、速度、航向組合的導(dǎo)航算法;根據(jù)當(dāng)前系統(tǒng)的運(yùn)動(dòng)狀態(tài),實(shí)現(xiàn)兩種導(dǎo)航算法之間的切換,以提高對(duì)汽車位置、速度、姿態(tài)的測(cè)量精度。

1 傳感器簡(jiǎn)介

系統(tǒng)采用的MIMU為荷蘭Xsens Technologies公司的MTI-10 IMU,其集成了3個(gè)單軸的MEMS陀螺儀、兩個(gè)雙軸MEMS加速度計(jì)和一個(gè)溫度傳感器;其MEMS陀螺儀的實(shí)測(cè)零偏穩(wěn)定性為100°/h,長(zhǎng)期零偏重復(fù)性為0.2°/s;其MEMS加速度計(jì)的實(shí)測(cè)零偏穩(wěn)定性為80μg,長(zhǎng)期零偏重復(fù)性為0.03 m/s2。

系統(tǒng)采用的GPS接收機(jī)為星網(wǎng)宇達(dá)科技開(kāi)發(fā)有限公司的XW-GPS1001衛(wèi)星接收機(jī),其標(biāo)稱的定位精度為2.5 m(CEP)、速度精度為0.1 m/s,其輸出的航向?yàn)檐壽E航向。軌跡航向是根據(jù)相鄰兩個(gè)時(shí)刻的位置坐標(biāo)計(jì)算得到的,而一般導(dǎo)航系統(tǒng)輸出航向?yàn)樗俣群较颉Ｓ纱丝芍?當(dāng)系統(tǒng)做近似直線運(yùn)動(dòng)時(shí),GPS接收機(jī)輸出航向是可信的;當(dāng)系統(tǒng)做轉(zhuǎn)彎運(yùn)動(dòng)時(shí),其輸出航向是不可信的。

2 組合導(dǎo)航算法設(shè)計(jì)

設(shè)計(jì)基于卡爾曼濾波的組合導(dǎo)航算法時(shí),需要建立描述組合導(dǎo)航系統(tǒng)動(dòng)態(tài)特性的系統(tǒng)狀態(tài)方程和反映量測(cè)與狀態(tài)關(guān)系的量測(cè)方程[5],下面將分別予以介紹。

2.1 系統(tǒng)狀態(tài)方程

系統(tǒng)的狀態(tài)向量選為12維,包括3個(gè)平臺(tái)誤差角,東向、北向速度誤差,緯度、經(jīng)度誤差,X、Y、Z軸陀螺隨機(jī)常值漂移和X、Y軸加計(jì)隨機(jī)常值零偏。根據(jù)慣導(dǎo)系統(tǒng)的平臺(tái)誤差角方程、速度誤差方程和位置誤差方程[5],并將MEMS慣性器件誤差簡(jiǎn)化為隨機(jī)常值誤差與白噪聲之和,可以寫出系統(tǒng)狀態(tài)方程為：

(1)式中:

其中,FN為對(duì)應(yīng)于SINS的7個(gè)誤差參數(shù)(3個(gè)姿態(tài)誤差,2個(gè)速度誤差,2個(gè)位置誤差)的系統(tǒng)動(dòng)態(tài)矩陣,其非零元素為:

FN(5,1)=fU,FN(5,3)=-fE,

2.2 基于位置、速度組合的量測(cè)方程

系統(tǒng)基于位置、速度組合的量測(cè)向量的維數(shù)為4,包括東向、北向速度誤差量測(cè)量,東向、北向位置誤差量測(cè)量。量測(cè)向量的計(jì)算式為

(2)

(3)式中:

其中:ME、MN分別為GPS量測(cè)的東向、北向速度誤差,NN、NE分別為GPS量測(cè)的東向、北向位置誤差。

2.3 基于位置、速度、航向組合的量測(cè)方程

系統(tǒng)基于位置、速度、航向組合的量測(cè)向量的維數(shù)為5,即在前述位置、速度組合的基礎(chǔ)上添加航向誤差量測(cè)量,其位置、速度誤差量測(cè)方程同2.2,下面詳細(xì)推導(dǎo)航向誤差量測(cè)方程。

由系統(tǒng)三軸姿態(tài)角可得到姿態(tài)矩陣為:

(4)

由坐標(biāo)變換有:

(5)

式中:

根據(jù)式(4)、式(5)可得:

(6)

設(shè)捷聯(lián)慣導(dǎo)系統(tǒng)輸出的航向角為ψSINS=ψ+δψ,則根據(jù)式(4)、式(6)可得:

(7)

將式(7)的右邊按泰勒級(jí)數(shù)展開(kāi)并忽略誤差角的二次項(xiàng)得:

(8)

由于δψ是一個(gè)小量,可以近似認(rèn)為tanδψ=δψ,則式(7)的左邊可寫為:

(9)

將式(9)按泰勒級(jí)數(shù)展開(kāi)并忽略誤差角的二次項(xiàng),得:

(10)

另外,有:

(11)

連同式(8)、式(10)、式(11)可得:

(12)

式(12)即為航向角誤差量測(cè)方程。

需要注意的是:在計(jì)算航向角誤差量測(cè)時(shí),不能簡(jiǎn)單地由ψSINS與ψGPS作差得到。航向角誤差的方向定義為:以ψGPS按小角度逆時(shí)針轉(zhuǎn)到ψSINS為正,以ψGPS按小角度順時(shí)針轉(zhuǎn)到ψSINS為負(fù)。其具體計(jì)算流程為:

step1將ψSINS和ψGPS的定義均轉(zhuǎn)換為從北向算起、逆時(shí)針轉(zhuǎn)動(dòng)一周的變化范圍為[0°,360°);

step2計(jì)算航向角誤差δψ=ψSINS-ψGPS;

step3若ψSINS指示系統(tǒng)方位為北偏西,而ψGPS指示北偏東,則修正航向誤差角δψ=δψ+2π,轉(zhuǎn)step5;

step4若ψSINS指示系統(tǒng)方位為北偏東,而ψGPS指示北偏西,則修正航向誤差角δψ=δψ-2π;

step5結(jié)束。

2.4 兩種組合導(dǎo)航算法的切換條件

XW-GPS1001輸出的航向?yàn)檐壽E航向,即當(dāng)系統(tǒng)做近似直線運(yùn)動(dòng)時(shí),航向是可信的;當(dāng)系統(tǒng)做轉(zhuǎn)彎運(yùn)動(dòng)時(shí),航向是不可信的。根據(jù)此特性,確定兩種組合導(dǎo)航算法的切換條件為:當(dāng)|ωz|<2°/s(ωz為系統(tǒng)轉(zhuǎn)彎角速度)時(shí),采用基于位置、速度、航向組合的導(dǎo)航算法;否則,采用基于位置、速度組合的導(dǎo)航算法。

3 跑車實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證文中提出的導(dǎo)航算法的有效性,進(jìn)行了實(shí)際跑車實(shí)驗(yàn)。實(shí)驗(yàn)過(guò)程中,在保存系統(tǒng)輸出的數(shù)據(jù)的同時(shí),還保存NovAtel公司的DL-V3型GPS接收機(jī)輸出的導(dǎo)航數(shù)據(jù),其定位精度為1.5 m(CEP)、速度精度為0.03 m/s。在評(píng)價(jià)系統(tǒng)測(cè)量精度時(shí),將以其輸出數(shù)據(jù)作為參考基準(zhǔn)。

跑車實(shí)驗(yàn)重復(fù)進(jìn)行3次,圖1～圖3給出了第一次跑車實(shí)驗(yàn)的測(cè)量效果。使用第一次跑車實(shí)驗(yàn)數(shù)據(jù),離線進(jìn)行基于位置、速度組合的導(dǎo)航解算,其航向測(cè)量效果如圖4所示,兩種組合導(dǎo)航算法的測(cè)量精度對(duì)比如表1所示(導(dǎo)航算法1為文中所述導(dǎo)航算法,導(dǎo)航算法2為只采用位置、速度組合的導(dǎo)航算法)。由表1可知:相對(duì)單純的位置、速度組合導(dǎo)航算法,采用文中所述的導(dǎo)航算法,系統(tǒng)的位置、速度、航向測(cè)量精度均有所提高,尤其航向測(cè)量精度提高了1倍。

采用文中所述導(dǎo)航算法,系統(tǒng)3次實(shí)驗(yàn)的測(cè)量精度如表2所示,由表2可知:系統(tǒng)的位置精度為3 m(CEP),速度精度(1σ)為0.25 m/s,航向精度(1σ)為1°,從而說(shuō)明了導(dǎo)航算法的有效性。

圖1 位置誤差曲線

圖2 速度誤差曲線

圖3 航向及航向誤差曲線1

圖4 航向及航向誤差曲線2

導(dǎo)航算法位置精度/m速度精度/(m/s)航向精度/(°)12.87330.20220.962623.56780.21032.0309

表2 系統(tǒng)測(cè)量精度

4 結(jié)論

文中針對(duì)低精度MIMU和GPS組合的測(cè)量技術(shù)在車載測(cè)量系統(tǒng)中的應(yīng)用,提出在基于位置、速度組合的導(dǎo)航算法基礎(chǔ)上,再添加基于位置、速度、航向組合的導(dǎo)航算法,根據(jù)當(dāng)前系統(tǒng)的運(yùn)動(dòng)狀態(tài),實(shí)現(xiàn)兩種導(dǎo)航算法之間的切換,以提高對(duì)汽車位置、速度、姿態(tài)的測(cè)量精度。通過(guò)進(jìn)行實(shí)際跑車實(shí)驗(yàn),以NovAtel公司的DL-V3型GPS接收機(jī)輸出的數(shù)據(jù)作為參考基準(zhǔn),得到系統(tǒng)的位置精度為3 m(CEP),速度精度為0.25 m/s,航向精度為1°。相對(duì)采用單純的位置、速度組合導(dǎo)航算法,采用文中所述的導(dǎo)航算法,系統(tǒng)的位置、速度、航向測(cè)量精度均有所提高,尤其航向測(cè)量精度提高了1倍。實(shí)驗(yàn)結(jié)果表明,文中所述的導(dǎo)航算法是有效的。

[1] 楊艷娟, 卞鴻巍, 田蔚風(fēng), 等. 一種新的INS/GPS組合導(dǎo)航技術(shù) [J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2004, 12(2): 23-26.

[2] 以光衢. 慣性導(dǎo)航原理 [M]. 北京: 航空工業(yè)出版社, 1987.

[3] 張小龍. 車輛主動(dòng)安全性能道路試驗(yàn)系統(tǒng)及評(píng)價(jià)方法研究 [D]. 南京: 東南大學(xué), 2006.

[4] 李倩. GPS/INS組合導(dǎo)航系統(tǒng)研究及實(shí)現(xiàn) [D]. 上海: 上海交通大學(xué), 2010.

[5] 秦永元, 張洪鉞，汪叔華. 卡爾曼濾波與組合導(dǎo)航原理 [M]. 2版. 西安: 西北工業(yè)大學(xué)出版社, 2012.

Navigation Algorithm Design on Low-cost MIMU/GPS Integrated Measurement System for Vehicles

LIU Jiang,LYU Yanhong

(Beijing University of Aeronautics and Astronautics, Beijing 100191, China)

Integrated measurement system is used to measure vehicle’s position, velocity and attitude. To improve the measurement accuracy, a navigation algorithm is designed as follows:the algorithm based on position, velocity and yaw integration is used when the system is moving approximate linearly; If the system is turning, the algorithm based on position and velocity integration is applied. The car experiments show that the system’s measurement accuracy of positon is 3 m(CEP), that of velocity is 0.25 m/s and that of yaw is 1°. Compared with the navigation algorithm simply based on position and velocity integration, the designed navigation algorithm can achieve more accurate measurement. Especially, the measurement accuracy of yaw is doubling. The results indicate that the navigation algorithm designed is available.

micro-inertial measurement unit; yaw integration; Kalman filter; integrated navigation

2014-07-30

劉江(1989-),男,湖南人,碩士研究生,研究方向:慣性導(dǎo)航、組合導(dǎo)航。

V249.322

A