基于雙OD修正的GNSS/MIMU導(dǎo)航算法研究

葉忠昌, 趙忠華, 葉 鑫

(上海交通大學(xué) 電子信息與電氣工程學(xué)院, 上海 200240 )

0 引言

隨著自動(dòng)駕駛、移動(dòng)測(cè)繪等技術(shù)的發(fā)展,車輛定位的精度與可靠性成為汽車導(dǎo)航的重要因素[1]。全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)信號(hào)易受建筑物遮蔽,造成信號(hào)失鎖。使用高精度的慣性導(dǎo)航系統(tǒng)(INS)可解算出精準(zhǔn)的位姿信息[2],但考慮價(jià)格因素,汽車上多使用低成本的微慣性測(cè)量單元(MIMU)進(jìn)行姿態(tài)測(cè)量。低成本的MIMU性能較差,位姿解算存在誤差累積,長(zhǎng)期的導(dǎo)航結(jié)果可靠性不高,因此需要使用其他傳感器輔助導(dǎo)航[3]。輪速里程計(jì)(OD)組合MIMU導(dǎo)航在車輛導(dǎo)航系統(tǒng)中應(yīng)用最廣泛。文獻(xiàn)[4]推導(dǎo)了單OD輔助的GNSS/INS松組合模型,并實(shí)驗(yàn)驗(yàn)證了OD可輔助較高精度的INS解算速度與位置。文獻(xiàn)[5]提出使用OD可估計(jì)車體運(yùn)動(dòng)轉(zhuǎn)彎時(shí)的非完整性約束噪聲,使車輛在120 s內(nèi)最大水平誤差減小68.4%。已有研究大多使用單OD測(cè)速校正,但GNSS信號(hào)失鎖時(shí),低成本 MIMU解算的航向角仍會(huì)產(chǎn)生誤差累積,導(dǎo)致定位出現(xiàn)偏差。針對(duì)城市內(nèi)GNSS信號(hào)短時(shí)失鎖情況,本文基于卡爾曼濾波(KF)建立了一種使用雙天線GNSS/MIMU/OD組合導(dǎo)航模型。該模型使用雙天線GNSS、雙OD修正,實(shí)現(xiàn)了對(duì)OD刻度系數(shù)誤差及MIMU解算姿態(tài)角誤差的估計(jì)。

1 車輛姿態(tài)解算及里程計(jì)誤差分析

為便于汽車導(dǎo)航參數(shù)分析,將常用的坐標(biāo)系做如下規(guī)定:三軸指向汽車右、前、上方向的載體坐標(biāo)系(b系)、車載MIMU坐標(biāo)系(m系)、地球慣性坐標(biāo)系(i系)、與當(dāng)?shù)貣|、北、天方向重合的導(dǎo)航坐標(biāo)系(n系)和地心地固坐標(biāo)系(e系)。

1.1 車輛姿態(tài)角解算

使用慣性傳感器計(jì)算姿態(tài),常用方法[6]有歐拉角法、方向余弦矩陣法和四元數(shù)法。四元數(shù)法計(jì)算相對(duì)簡(jiǎn)單,且無萬向鎖等缺陷,選擇使用該方法計(jì)算車體的姿態(tài)。引入四元數(shù)q=[q0q1q2q3]T, 則陀螺儀解算姿態(tài)的微分方程為

(1)

式(1)展開形式為

(2)

式中:ωb=[ωxωyωz]T為在b系下陀螺儀輸出的角速度;Ω為ωb的反對(duì)稱矩陣。整理得到四元數(shù)的傳遞方程:

(3)

式中:I為單位矩陣;T為陀螺儀的輸出時(shí)間間隔。

通過式(3)解算的四元數(shù)按下式轉(zhuǎn)換為姿態(tài)角:

(4)

式中:ψ為航向角;θ為俯仰角;γ為橫滾角。

在汽車靜止時(shí),使用加速度計(jì)可計(jì)算θ、γ。在汽車運(yùn)動(dòng)時(shí),存在外部運(yùn)動(dòng)加速度,通過OD可估計(jì)該外部運(yùn)動(dòng)加速度。

(5)

式中TOD為里程計(jì)輸出周期。重力加速度在m系的投影為

(6)

此時(shí)根據(jù)重力模型[7]計(jì)算θ、γ為

(7)

關(guān)于車輛的航向角解算,雙天線GNSS通過載波相位差分可精確解算兩天線中心位置的坐標(biāo)變化,進(jìn)而根據(jù)與雙天線的幾何變化關(guān)系解算航向角[8]為

(8)

當(dāng)GNSS信號(hào)失鎖時(shí),左右OD的輪速差[9]可用于估計(jì)載體航向角的變化量Δψ為

(9)

通過式(9)可知,OD可在GNSS信號(hào)失鎖時(shí)提供航向信息,但OD的刻度系數(shù)受路況、車輪狀態(tài)影響。為了提高OD測(cè)量的準(zhǔn)確度,需要根據(jù)路況動(dòng)態(tài)估計(jì)里程計(jì)的測(cè)量噪聲特性,并在雙天線GNSS信號(hào)有效時(shí)修正OD誤差。

1.2 里程計(jì)誤差分析

(10)

(11)

對(duì)式(11)兩邊取微分可得:

(12)

視m系與b系坐標(biāo)軸重合,將式(12)變換到n系可得:

(13)

(14)

(15)

(16)

(17)

(18)

(19)

通過式(19)即可表征里程計(jì)的位置誤差變化。

2 GNSS/MIMU/OD組合導(dǎo)航模型

2.1 GNSS信號(hào)失鎖前導(dǎo)航模型

GNSS可以精確獲得汽車的速度和位置,從而校正MIMU與OD的測(cè)量誤差。卡爾曼濾波(KF)是一種通過系統(tǒng)狀態(tài)的觀測(cè)數(shù)據(jù)對(duì)系統(tǒng)狀態(tài)校正的最優(yōu)估計(jì)算法。針對(duì)非線性的汽車導(dǎo)航模型,使用擴(kuò)展卡爾曼濾波(EKF)可將GNSS、MIMU和OD數(shù)據(jù)融合,得到汽車導(dǎo)航信息的最優(yōu)估計(jì)。

(20)

(21)

式中:Fnav為系統(tǒng)矩陣;Wnav為隨機(jī)噪聲矩陣;Gnav為噪聲變換矩陣;Znav為觀測(cè)向量;Hnav為觀測(cè)矩陣;Nnav為GNSS接收機(jī)相應(yīng)的測(cè)量噪聲,噪聲大小參考器件手冊(cè)。相應(yīng)矩陣為

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

按以下步驟對(duì)導(dǎo)航參數(shù)估計(jì):

Step1:模型離散化:

(30)

Step2:對(duì)系統(tǒng)狀態(tài)量先驗(yàn)估計(jì)與觀測(cè)校正。

a.狀態(tài)單步估計(jì):

Xk/k-1=Ak/k-1Xk-1

(31)

b.協(xié)方差計(jì)算:

(32)

c.卡爾曼濾波增益:

(33)

d.狀態(tài)二次校正:

Xk=Xk/k-1+Kk(Zk-HkXk/k-1)

(34)

e.更新協(xié)方差:

Pk=(I-KkHk)Pk/k-1

(35)

Step3:使用估計(jì)的誤差修正當(dāng)前時(shí)刻的導(dǎo)航參數(shù):

(36)

校正下一時(shí)刻的陀螺儀、加速度計(jì)、OD的輸出:

(37)

Step4:將所有誤差置0,即:

Xk-1=0

(38)

2.2 GNSS信號(hào)失鎖后導(dǎo)航模型

在GNSS信號(hào)失鎖時(shí),保留GNSS信號(hào)失鎖前一時(shí)刻的姿態(tài)、速度和位置等導(dǎo)航參數(shù)作為起始值。使用OD輪速差修正航向角及加速度計(jì)修正橫滾角與俯仰角。

(39)

對(duì)應(yīng)的狀態(tài)方程:

(40)

其中:

(41)

(42)

(43)

觀測(cè)量Zcal定義及量測(cè)方程:

(44)

其中:

(45)

(46)

式中:Nov為里程計(jì)速度的觀測(cè)噪聲;Naθ,Naγ分別為根據(jù)加速度信息解算的俯仰角和橫滾角的觀測(cè)噪聲,其常被看作高斯白噪聲;Noψ為根據(jù)OD輪速差解算得到的航向角觀測(cè)噪聲。

(47)

式中i為選中的速度基準(zhǔn)輪(i=R,L)。根據(jù)式(9),計(jì)算Noψ為

(48)

統(tǒng)計(jì)時(shí)間預(yù)設(shè)為10 s,若GNSS有效修正時(shí)間不足10 s,預(yù)設(shè)時(shí)間相應(yīng)減少。

按照以下步驟對(duì)導(dǎo)航參數(shù)進(jìn)行估計(jì):

Step1:根據(jù)式(47)、(48)構(gòu)建噪聲,并初始化GNSS信號(hào)失鎖時(shí)刻的起始導(dǎo)航參數(shù)。

Step2:與GNSS信號(hào)失鎖前Step1、Step2階段一致,使用卡爾曼濾波估計(jì)導(dǎo)航誤差。

Step3:輸出修正后的速度與姿態(tài)信息。

(49)

(50)

Step5:更新結(jié)束,將所有誤差狀態(tài)置0,即:

Xk-1=0

(51)

3 車載實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)設(shè)計(jì)

在上海市浦東區(qū)進(jìn)行車載實(shí)驗(yàn)。跑車上裝載MIMU、雙輪里程計(jì)及外接雙天線的SPAN-CPT高精度導(dǎo)航系統(tǒng)。MIMU的噪聲參數(shù)如表1所示。汽車的后驅(qū)雙輪裝載輪速里程計(jì)。由加拿大NovAtel公司生產(chǎn)的SPAN-CPT內(nèi)部搭載高精度的慣導(dǎo)設(shè)備, GNSS雙天線間距1 m,信號(hào)的輸出頻率為10 Hz(可升級(jí)為20 Hz)。通過實(shí)時(shí)動(dòng)態(tài)載波相位差分技術(shù),在GNSS信號(hào)良好時(shí),SPAN-CPT的定位精度達(dá)厘米級(jí),輸出的航向角精度(RMS)低于0.1°,組合輸出的航向角頻率為100 Hz,滿足高精度、低時(shí)延的定位與定向要求,可作為位置與航向信息的基準(zhǔn)值。

表1 MIMU參數(shù)

跑車道路實(shí)測(cè)的總時(shí)長(zhǎng)為139 min,選擇汽車上高架路段作為驗(yàn)證路段1,選擇汽車經(jīng)過天橋與城市隧道時(shí)GNSS信號(hào)連續(xù)失鎖路段作為驗(yàn)證路段2。GNSS解算的汽車行駛軌跡如圖1所示。

圖1 GNSS解算的汽車行駛軌跡

3.2 路段1實(shí)驗(yàn)

路段1(310～430 s)期間模擬GNSS信號(hào)失鎖,在汽車行駛至路段1前,左右輪OD的刻度比例系數(shù)誤差估計(jì)如圖2、3所示。

圖2 左輪OD的刻度比例系數(shù)誤差估計(jì)

圖3 右輪OD的刻度比例系數(shù)誤差估計(jì)

經(jīng)統(tǒng)計(jì),在GNSS信號(hào)失鎖前10 s內(nèi),左輪OD的刻度比例系數(shù)誤差方差為8.145×10-4,右輪OD的刻度比例系數(shù)誤差方差為6.855×10-4。失鎖前左輪OD的刻度比例系數(shù)為1.012 9,右輪OD的刻度比例系數(shù)為1.011 8。

使用不同算法解算得到的航向角如圖4所示。圖中,WRC為輪速比例修正。圖5為使用不同算法解算的航向角誤差(SPAN-CPT輸出的航向角作為參考值)。

圖4 航向角解算對(duì)比

由圖4可見,各算法解算的航向角變化趨勢(shì)基本一致。由圖5可看出,MIMU解算的航向角因零偏不穩(wěn)定及隨機(jī)游走而產(chǎn)生偏差,在2 min內(nèi),MIMU解算的航向角最終偏移為2.53°。

由圖5中MIMU/OD曲線可知,雙OD輪速差解算的航向角誤差累積較小,但存在兩點(diǎn)缺陷:

1) 解算的航向角對(duì)較大轉(zhuǎn)彎具有滯后效應(yīng),轉(zhuǎn)彎誤差較大。

2) 對(duì)路況變化較敏感,部分時(shí)刻存在航向角突變。

由圖5中MIMU/OD+WRC曲線可知,使用WRC算法后解算的航向角,車輛轉(zhuǎn)彎的敏感度有效提升。通過估計(jì)OD的觀測(cè)噪聲,融合后的航向角一定程度上弱化了雙OD解算航向角的突變,同時(shí)有效地減小了MIMU解算航向角的誤差發(fā)散。

各算法解算的汽車軌跡如圖6所示。由圖可知,使用MIMU、MIMU/OD解算的航向角將使汽車軌跡與真實(shí)值產(chǎn)生較大偏移,在MIMU/OD組合的基礎(chǔ)上使用WRC算法解算的車輛軌跡與真實(shí)值最接近。

圖6 不同算法解算位置對(duì)比

各方案解算的誤差參數(shù)對(duì)比如表2所示。其中方案1～3分別為MIMU解算、MIMU/OD解算、MIMU/OD+WRC解算。

表2 不同方案誤差數(shù)值對(duì)比

經(jīng)數(shù)據(jù)分析,使用MIMU/OD +WRC算法解算位置,最大定位誤差相對(duì)無WRC減小63.9%,定位誤差標(biāo)準(zhǔn)差減小80.1%。同時(shí),最大航向角誤差減小3.22°,航向角誤差標(biāo)準(zhǔn)差減小0.21°。由此可見,所設(shè)計(jì)的WRC算法可有效地提高組合導(dǎo)航系統(tǒng)的穩(wěn)定性與準(zhǔn)確性。

3.3 路段2實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證WRC算法的可用性,選取了路段2(6 050～6 250 s)進(jìn)行驗(yàn)證。汽車將在該路段連續(xù)兩次經(jīng)過天橋,一次經(jīng)過地下隧道,經(jīng)歷3個(gè)時(shí)間段的GNSS信號(hào)失鎖,失鎖時(shí)長(zhǎng)分別為12.4 s、5.6 s和78.1 s,總測(cè)試時(shí)長(zhǎng)200 s。使用MIMU/OD +WRC的導(dǎo)航結(jié)果如圖7、8所示。圖中,綠色軌跡代表GNSS定位結(jié)果,紅色軌跡代表使用MIMU/OD+WRC定位結(jié)果,汽車行駛方向?yàn)榧^方向。

圖7 路段2的汽車軌跡解算

汽車局部軌跡放大效果如圖8所示。圖中,方框處為GNSS信號(hào)恢復(fù)正常時(shí)的定位點(diǎn)。

圖8 汽車軌跡局部放大圖

汽車航向角的變化如圖9所示。在GNSS信號(hào)恢復(fù)正常時(shí)刻,MIMU/OD+WRC算法解算的航向角與SPAN-CPT輸出的航向角差值分別為0.10°,0.35°和0.28°,定位差值分別為0.29 m,0.27 m和0.55 m,該定位結(jié)果可用于城市內(nèi)精準(zhǔn)導(dǎo)航。

圖9 汽車航向角解算

4 結(jié)束語

本文基于雙天線GNSS修正MIMU/OD的導(dǎo)航模型,設(shè)計(jì)了雙OD輪速比例修正算法。通過車載實(shí)驗(yàn)證明,針對(duì)城市隧道等GNSS信號(hào)失鎖環(huán)境,相較于無WRC的MIMU/OD組合導(dǎo)航算法,MIMU/OD+WRC算法在抑制航向角發(fā)散與提升航向角測(cè)量敏感度方面具有更好的效果。在120 s的高架轉(zhuǎn)彎路段,最大定位誤差減小63.9%,定位誤差標(biāo)準(zhǔn)差減小80.1%。在200 s的GNSS信號(hào)頻繁失鎖路段,車輛定位保持在車道內(nèi),GNSS信號(hào)恢復(fù)時(shí)的最大位置誤差為0.55 m。本文所設(shè)計(jì)的算法能有效抑制航向角的誤差發(fā)散,進(jìn)而提高車輛的定位精度。