車輛導(dǎo)航系統(tǒng)橫擺角速度傳感器零點偏移的自動校正*

李江濤，楊殿閣，楊 揚(yáng)，張 濤，連小珉

(清華大學(xué)汽車工程系，汽車安全與節(jié)能國家重點實驗室，北京 100084)

?

2015183

車輛導(dǎo)航系統(tǒng)橫擺角速度傳感器零點偏移的自動校正*

李江濤，楊殿閣，楊 揚(yáng)，張 濤，連小珉

(清華大學(xué)汽車工程系，汽車安全與節(jié)能國家重點實驗室，北京 100084)

本文中提出了一種在車輛正常行駛時動態(tài)自動校正橫擺角速度傳感器零點偏移的方法，該方法利用衛(wèi)星定位信息和車速識別行駛車輛的靜止或直線行駛兩種可校正行駛工況，并在該工況下利用橫擺角速度傳感器的測量值校正其零點偏移。采用壞點清空的單隊列從衛(wèi)星定位點中提取連續(xù)好點序列，針對該好點序列采用最小二乘直線擬合和標(biāo)準(zhǔn)差條形分布直行判據(jù)識別直線行駛工況。結(jié)果表明，本方法能在車輛行駛中正確地自動識別可校正工況，校正后的零點偏移精度滿足汽車導(dǎo)航系統(tǒng)的應(yīng)用要求。

汽車導(dǎo)航；橫擺角速度傳感器；零點偏移；自動校正；直線行駛

前言

具有自主導(dǎo)航、抗干擾等特點的慣性導(dǎo)航系統(tǒng)與常用衛(wèi)星導(dǎo)航系統(tǒng)結(jié)合構(gòu)成的組合導(dǎo)航系統(tǒng)是近年來汽車導(dǎo)航領(lǐng)域的研究熱點。橫擺角速度傳感器是組合導(dǎo)航系統(tǒng)的核心元件[1]，其精度直接關(guān)系到組合導(dǎo)航的定位性能。由于成本及使用環(huán)境的限制，一般車輛導(dǎo)航中都采用低成本的角速度傳感器，例如微機(jī)械陀螺，決定該類傳感器精度的重要參數(shù)有零點偏移和標(biāo)度因子，其中標(biāo)度因子相對穩(wěn)定[2]，因此，本文中主要研究零點偏移的自動校正問題。角速度傳感器的零點偏移反映了測量角速度值中存在的與運(yùn)動狀態(tài)無關(guān)的偏移量[3]，通常通過零角速度輸入狀態(tài)下一段時間測量角速度的均值獲取[4-6]。

最初的零點偏移校正多在室內(nèi)利用試驗臺實現(xiàn)[3]，然而，在車輛行駛過程中零點偏移會發(fā)生不規(guī)則變化，導(dǎo)致測量的角速度存在誤差，在航位推算過程中，該偏差累積會導(dǎo)致方位推算精度迅速惡化[7-8]，因此，零點偏移需要在使用中動態(tài)校正。

文獻(xiàn)[9]中提出了在車停時自動校正零點偏移的方法，但當(dāng)車輛連續(xù)長時間行駛時，該方法無法及時完成校正。文獻(xiàn)[10]中將零點偏移作為參數(shù)采用卡爾曼濾波進(jìn)行估計，該算法的實時性和精度均較好，但計算量較大，會給嵌入式導(dǎo)航設(shè)備造成較大計算壓力。文獻(xiàn)[11]中利用GPS結(jié)合地圖匹配校正零點偏移，當(dāng)發(fā)現(xiàn)車輛在直線道路上行駛時實施校正，該方法思路簡單，適用性較好，但由于電子地圖精度較低，使得校正精度存在不確定性。此外，文獻(xiàn)[2]中提出了采用溫度補(bǔ)償角速度測量誤差的方法，但系統(tǒng)須要增加溫度傳感器。

綜上所述，須要設(shè)計一種計算復(fù)雜度低，校正所需信息容易從現(xiàn)有組合導(dǎo)航系統(tǒng)獲取，適用于普通車載導(dǎo)航平臺，能夠在車輛行駛過程中動態(tài)自動校正橫擺角速度傳感器零點偏移的方法。本文中對此提出了利用車載GPS、車速傳感器進(jìn)行零點偏移自動校正的方法，該方法能夠在車輛行駛中自動識別靜止和直線行駛兩種可校正工況，并利用該工況下的角速度測量值校正零點偏移。其中，直線行駛動態(tài)識別算法是本文中的主要內(nèi)容，利用連續(xù)的GPS位置信息判斷車輛行駛狀態(tài)的思路在一些研究中已經(jīng)得到了應(yīng)用[12]。

本文中，首先介紹自動校正系統(tǒng)的整體架構(gòu)，隨后說明連續(xù)好點序列的提取方法，之后闡述用于校正零點偏移的直線行駛工況判別方法，最后是實驗結(jié)果和結(jié)論。

1 零點偏移校正原理及系統(tǒng)架構(gòu)

零點偏移自動校正旨在車輛行駛過程中自動識別一定時長的零橫擺角速度行駛工況，即可校正工況，利用該工況下測得的角速度均值來校正零點偏移。在車輛行駛中，常見的零橫擺角速度輸入工況為停車和直線行駛[13]。

自動校正系統(tǒng)的結(jié)構(gòu)如圖1所示。考慮系統(tǒng)在實際工作中是以一定時間間隔離散計算的，因此該系統(tǒng)模型為離散模型。每次接收到GPS信息時執(zhí)行一次計算，記執(zhí)行計算的離散時刻為tk，簡記為k，系統(tǒng)的輸入包括k時刻的GPS定位信息(位置p(k)、在用衛(wèi)星數(shù)cp(k)和水平精度因子hp(k))、車速集V(k)和測量角速度集Ω(k)。V(k)和Ω(k)分別由[k-1,k]時間區(qū)間內(nèi)車速傳感器測量的車速v(i)和角速度傳感器得到的測量角速度ωk(i)構(gòu)成。一般情況下，這兩種傳感器的采樣頻率要高于GPS。不同的組合導(dǎo)航系統(tǒng)采用的車速傳感器不同，本文中車速信息來源于CAN總線，精度較高。注意到，即使組合導(dǎo)航系統(tǒng)沒有專門的車速傳感器，從GPS定位信息中也可以得到車速信息，滿足本文中校正方法的需求。

d(k)為車輛停止標(biāo)志，代表車輛是否停止足夠長時間，由車停判斷函數(shù)fD利用一段時間的車速判斷得到：

(1)

式中：Vs是車停閾值，取0.01m/s；max(V(i))表示車速集V(i)的最大元素；H為靜止時長，當(dāng)車輛靜止超過該時長后，靜止時段的測量角速度就可用于計算新的零點偏移；d(k)為“1”表示檢測到車停可校正工況。如果系統(tǒng)無法獲取精確車速，采用GPS車速時應(yīng)調(diào)整相應(yīng)閾值。

本文中重點討論如何在行駛中利用GPS定位信息識別直線行駛工況。不同于車停識別，直行工況的識別更為復(fù)雜，可分為兩個子過程：連續(xù)好點序列的提取和直行判斷，分別由序列提取過程fG和直行判別函數(shù)fS實現(xiàn)。其中，k時刻的連續(xù)好點序列提取過程為

Pc(k)=fG(p(i),cp(i),hp(i),v(i)),

i∈[k-M+1,k],k≥M

(2)

式中：v(i)是速度集V(i)中最靠近時刻k的元素。好點序列包含M個符合條件的連續(xù)GPS位置點，是后續(xù)直行判斷的數(shù)據(jù)基礎(chǔ)。如果k時刻沒有滿足條件的連續(xù)好點序列，則函數(shù)fG輸出無效標(biāo)志。

直行判別式為

s(k)=fS(Pc(k)),k≥1

(3)

直行標(biāo)志s(k)為“1”表示直行工況，“0”表示無效工況，由直行判斷函數(shù)fS利用連續(xù)好點序列Pc(k)得到，具體見第3節(jié)。

若可校正工況對應(yīng)的時間區(qū)間為[h,h+M-1]，其中，h為可校正工況的起始時刻編號，系統(tǒng)啟動后，每接收1個GPS位置點，時刻編號會增加1；M為可校正工況包含的GPS位置點個數(shù)。則零點偏移的計算公式為

(4)

式中：bc是校正后的零點偏移；Nr(j)為j-1到j(luò)時間段內(nèi)測量角速度ωj的個數(shù)。

2 連續(xù)好點序列的提取

從原始GPS定位點中提取符合精度要求和連續(xù)性條件的一段GPS定位點序列作為直行判斷的數(shù)據(jù)基礎(chǔ)，稱為連續(xù)好點序列的提取。

2.1 提取原理

連續(xù)好點序列從原始GPS定位點中提取，原理如圖2 所示。

圖2中，p為原始GPS位置點，實心點表示好點，用pc表示，空心點表示壞點，k0為起點時刻，每個起點時刻對應(yīng)一個從零開始的新坐標(biāo)系，稱為好點動坐標(biāo)系，該坐標(biāo)系的時間用m表示(相對時間)。

連續(xù)好點序列的提取過程即為好點動坐標(biāo)系的生長過程。首先須要確定起點時刻k0：

k0=k,fp(k)=1

(5)

式中fp(k)為星車組合的好點判據(jù)：

(6)

式中：C為GPS星數(shù)閾值，保證提取的GPS位置精度較高，其值為8；Vd為車速閾值，進(jìn)而保證提取后的序列長度滿足距離要求，保證直線行駛判斷的準(zhǔn)確性。

起點時刻k0標(biāo)志著好點動坐標(biāo)系的生成，也標(biāo)志著連續(xù)好點序列生長的開始。在圖2中，k0(h)表示第h層好點動坐標(biāo)系的起點時刻，上層動坐標(biāo)系的起點時刻較早，因此其包含的位置點會最早生成連續(xù)好點序列。此外，當(dāng)車輛連續(xù)快速行駛時，GPS定位精度更高，定位穩(wěn)定性更好，本文中車速閾值取10m/s；Hd為水平精度因子閾值，作用同星數(shù)閾值類似，本文中取值為2。當(dāng)GPS定位點滿足設(shè)定的多閾值條件后，其精度一般較高，稱為好點。

確定起點時刻后，生成好點動坐標(biāo)系下連續(xù)好點序列：

pc(m)=p(k),fp(k)=1,k∈[k0,k0+M-1],m=k-k0

(7)

式中pc(m)為起點時刻k0對應(yīng)的好點動坐標(biāo)系下m時刻的好點。

由于好點的連續(xù)性要求和序列長度限制，好點動坐標(biāo)系具有一定的生命周期。當(dāng)其中好點的連續(xù)性被破壞，或者好點個數(shù)超過M時，該動坐標(biāo)系即消亡，因此，某一時刻至多同時存在M個好點動坐標(biāo)系。

當(dāng)好點動坐標(biāo)系中點的個數(shù)達(dá)到M個，即可得到連續(xù)好點序列Pc：

Pc=[pc(0),pc(1),…,pc(M-1)]

(8)

式中好點pc(i)包含車輛的平面坐標(biāo)，即

pc(i)=[xc(i),yc(i)]

(9)

在上述原理的具體實現(xiàn)中，根據(jù)圖2各動坐標(biāo)系的特點，可以采用一個特殊的隊列完成連續(xù)好點序列的提取過程。

2.2 壞點清空的單隊列

圖2中各動坐標(biāo)系存在重復(fù)好點，采用長度為M的定長隊列即可完成多個動坐標(biāo)系的統(tǒng)一存儲，實現(xiàn)連續(xù)好點序列提取，稱該隊列為壞點清空的單隊列，用Q表示。

隊列Q包括初始態(tài)和一般態(tài)兩種狀態(tài)。隊列未存滿時稱為初始態(tài)，此時各好點動坐標(biāo)系在逐漸生成和生長，但尚未滿足提取連續(xù)好點序列的條件；一段時間后，隊列存滿，進(jìn)入一般態(tài)，此時，隊列中的所有元素可構(gòu)成連續(xù)好點序列。

一般態(tài)下的壞點清空單隊列如圖3所示。

一般態(tài)下隊列包含的所有位置點均為動坐標(biāo)系起點時刻k0對應(yīng)的位置點，也是連續(xù)的好點，可以稱此時的隊列為起點隊列。隊列中第h個位置點的時刻對應(yīng)圖2中第h層動坐標(biāo)的起點時刻。

起點隊列中的所有位置點構(gòu)成連續(xù)好點序列Pc，若下一位置點p(h+M)仍為好點，則提取當(dāng)前的連續(xù)好點序列Pc，并執(zhí)行好點p(h+M)的入隊操作。對應(yīng)圖2中的過程為：第一層動坐標(biāo)系銷毀，下層各動坐標(biāo)系依次升層，以新接收好點的時刻作為起點時刻生成新的第M層動坐標(biāo)系；若下一位置點為壞點，因隊列中好點連續(xù)性被破壞，須將隊列Q清空。

初始態(tài)的壞點清空單隊列如圖4所示。

該狀態(tài)下，當(dāng)下一時刻接收到好點時，執(zhí)行該點入隊操作；當(dāng)接收到壞點時，清空隊列。

3 直線行駛工況的判別

得到連續(xù)好點序列Pc后，就可以利用它進(jìn)行直線行駛工況的判斷。

首先對連續(xù)好點序列Pc采用最小二乘法進(jìn)行直線擬合[14]，通過標(biāo)準(zhǔn)差條形分布直行判據(jù)判斷Pc對應(yīng)的車輛行駛階段是否為直線行駛，如圖5所示。

圖5中，L為擬合直線，l為連續(xù)好點序列擬合直線段的長度，δp為擬合標(biāo)準(zhǔn)差。

連續(xù)好點序列Pc中各位置點的橫坐標(biāo)構(gòu)成向量X：

X=[xc(0),xc(1),…,xc(M-1)]

(10)

縱坐標(biāo)構(gòu)成向量Y：

Y=[yc(0),yc(1),…,yc(M-1)]

(11)

M維單位向量I：

I=[1,1,…,1]

(12)

設(shè)擬合直線L的方程式為

y=P1x+P2

(13)

式中：P1和P2為擬合直線的待定參數(shù)：

(14)

經(jīng)演算得

(15)

則點到直線距離意義下的標(biāo)準(zhǔn)差[15]為

(16)

標(biāo)準(zhǔn)差條形分布直行判據(jù)為

l≥lt

(17)

δP

(18)

式中：lt為直線長度閾值；S為標(biāo)準(zhǔn)差閾值。

由于連續(xù)好點序列Pc中每個位置點的速度均有下限，因此可以保證擬合直線長度近似滿足式(17)。

若擬合標(biāo)準(zhǔn)差同時也滿足式(18)，則認(rèn)為Pc對應(yīng)時間段內(nèi)車輛沿直線行駛。此時，橫擺角速度可以近似認(rèn)為等于零，進(jìn)而利用此階段測得的角速度按式(4)進(jìn)行零點偏移校正。

4 實驗結(jié)果

提出的自動校正方法用C語言實現(xiàn)，并運(yùn)行于嵌入式車載導(dǎo)航平臺上，程序可以實時校正零點偏移，同時記錄實驗數(shù)據(jù)，支持離線分析和測試。

實驗中采用的橫擺角速度傳感器為日本村田公司生產(chǎn)的微機(jī)械陀螺MEV-50A，采樣頻率為50Hz，在某次正常行駛中，該傳感器的零點偏移如圖6所示。

圖6中，圓點代表零點偏移，是在車輛靜止時利用1min的測量角速度平均后得到的；虛線為采用最小二乘法擬合得到的零點偏移變化趨勢線。

可以看到，在行駛時間并不長的情況下(4.17h)，該傳感器的零點偏移仍會有0.091°/s的變化，由于車輛行駛環(huán)境復(fù)雜，在長時間行駛時零點偏移的變化會更大。

采用本文中介紹的方法識別的一個直行可校正工況如圖7所示，連續(xù)好點序列的點數(shù)M為30個，在好點滿足式(6)所示速度條件和式(18)所示直線性條件的前提下，連續(xù)好點序列所包含GPS位置點的個數(shù)M就能保證式(17)所示長度條件的成立。其中，定位點的坐標(biāo)已經(jīng)轉(zhuǎn)換為平面坐標(biāo)。

在圖7工況下校正零點偏移后，采用后續(xù)測量角速度結(jié)合新的零點偏移進(jìn)行行駛方位角的推算，并與RT3000測得的方位角進(jìn)行對比，如圖8所示，其中，RT3000的方位角測量精度在1σ(66.7%)情況下小于0.1°。

方位角推算值與RT3000測量值的偏差見圖9。

可以看到，方位角的偏差隨著推算時間近似線性增大，表明零點偏移的校正值與真值之間存在常數(shù)誤差，而偏差直線的斜率即為該誤差值，利用0～300s的數(shù)據(jù)直線擬合后，可得該校正誤差為0.056 6°/s，與一些復(fù)雜算法的校正精度類似。

5 結(jié)論

提出的橫擺角速度零點偏移自動校正方法，能夠在車輛行駛過程中自動辨別可校正行駛工況并校正零點偏移。校正系統(tǒng)結(jié)構(gòu)簡單，算法計算量小，校正精度滿足一般汽車導(dǎo)航應(yīng)用的要求。其中，標(biāo)準(zhǔn)差條形分布直行判據(jù)能夠利用較高精度的GPS連續(xù)定位點列識別車輛的直線行駛工況，所識別的工況能夠滿足零點偏移校正對行駛直線性的要求；星車組合的好點判據(jù)利用GPS定位精度信息和車速對GPS定位點進(jìn)行過濾，保證提取到的GPS點列滿足精度和距離要求，提高了直線行駛識別的準(zhǔn)確性；壞點清空的單隊列可以在占用很少運(yùn)算資源的情況下實現(xiàn)連續(xù)好點序列的采集。

[1] Barbour N, Schmidt G. Inertial Sensor Technology Trends[J]. IEEE Sensors Journal,2001,1(4):332-339.

[2] 黃智,鐘志華.車載導(dǎo)航系統(tǒng)陀螺的自適應(yīng)校正方法[J].汽車工程,2006,28(6):550-553.

[3] Park M. Error Analysis and Stochastic Modeling of MEMS Based Inertial Sensors for Land Vehicle Navigation Applications[D]. Alberta: Univ. of Calgary,2004.

[4] Borenstein J, Ojeda L, Kwanmuang S. Heuristic Reduction of Gyro Drift for Personnel Tracking Systems[J]. The Journal of Navigation,2009,62(1):41-58.

[5] Godha S. Performance Evaluation of Low Cost MEMS-Based IMU Integrated with GPS for Land Vehicle Navigation Application[D]. Alberta: Univ. of Calgary,2006.

[6] Lee D, Lee S, Park S, et al. Test and Error Parameter Estimation for MEMS - Based Low Cost IMU Calibration[J]. International Journal of Precision Engineering and Manufacturing,2011,12(4):597-603.

[7] Foxlin E. Motion Tracking Requirements and Technologies[G]. Stanney K. Handbook of Virtual Environment Technology, New Jersey: Lawrence Erlbaum Associates,2002:163-210.

[8] Artese G, Trecroci A. Calibration of a Low Cost MEMS INS Sensor for an Integrated Navigation System[C]. Proceeding of 23st ISPRS Congress, Beijing, China,2008:877-882.

[9] Davidson P, Hautam?ki J, Collin J, et al. Improved Vehicle Positioning in Urban Environment Through Integration of GPS and Low-cost Inertial Sensors[C]. Proceedings of European Navigation Conference, Naples, Italy,2009:101-107.

[10] Zhao L, Ochieng W Y, Quddus M A, et al. An Extended Kalman Filter Algorithm for Integrating GPS and Low Cost Dead Reckoning System Data for Vehicle Performance and Emissions Monitoring[J]. The Journal of Navigation,2003,56:257-275.

[11] Zhang Xiaoguo, Wang Qing, Wan Dejun. Map Matching in Road Crossings of Urban Canyons Based on Road Traverses and Linear Heading-Change Model[J]. IEEE Transactions on Instrumentation and Measurement,2007,56(6):2795-2803.

[12] Yang Diange, Zhang Tao, Li Jiangtao, et al. Synthetic Fuzzy Evaluation Method of Trajectory Similarity in Map-Matching[J]. Journal of Intelligent Transportation Systems,2011,15(4):193-204.

[13] Zhao Yilin. Vehicle Location and Navigation Systems[M]. Boston: Artech House,1997.

[14] 李平舟,陳秀華,吳興林.大學(xué)物理實驗[M].西安:西安電子科技大學(xué)出版社,2002.

[15] 葉其孝,沈永歡.實用數(shù)學(xué)手冊[M].北京:科學(xué)出版社,2006.

Automatic Correction for the Zero Offset of YawRate Sensor in Vehicle Navigation System

Li Jiangtao, Yang Diange, Yang Yang, Zhang Tao & Lian Xiaomin

StateKeyLaboratoryofAutomotiveSafetyandEnergy,DepartmentofAutomotiveEngineering,TsinghuaUniversity,Beijing100084

A dynamic automatic correction method for the zero offset of yaw rate sensor during the normal driving of vehicle is proposed in this paper. The method uses satellite positioning information and vehicle speed to identify two correctable driving conditions of running vehicle: standstill and straight-line driving, and the values measured by yaw rate sensor are used to correct zero offset in those conditions. Then the successive valid point series are extracted from satellite positioning points by using single array with invalid points cleared, and with these valid point series, the straight-line driving condition is identified by using least square line fitting and the straight-line driving criterion with standard deviation strip distribution. The results show that the method proposed can automatically and correctly identify correctable conditions during vehicle driving, with the accuracy of zero offset after correction meeting the application requirements of vehicle navigation system.

vehicle navigation; yaw rate sensor; zero offset; automatic correction; straight-line driving

*國家863計劃項目(2012AA111901)資助。

原稿收到日期為2013年12月31日。