車輛導航系統橫擺角速度傳感器零點偏移的自動校正*

李江濤，楊殿閣，楊 揚，張 濤，連小珉

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

?

2015183

車輛導航系統橫擺角速度傳感器零點偏移的自動校正*

李江濤，楊殿閣，楊 揚，張 濤，連小珉

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

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

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

前言

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

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

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

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

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

1 零點偏移校正原理及系統架構

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

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

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

(1)

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

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

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個符合條件的連續GPS位置點，是后續直行判斷的數據基礎。如果k時刻沒有滿足條件的連續好點序列，則函數fG輸出無效標志。

直行判別式為

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

(3)

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

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

(4)

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

2 連續好點序列的提取

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

2.1 提取原理

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

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

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

k0=k,fp(k)=1

(5)

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

(6)

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

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

確定起點時刻后，生成好點動坐標系下連續好點序列：

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

(7)

式中pc(m)為起點時刻k0對應的好點動坐標系下m時刻的好點。

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

當好點動坐標系中點的個數達到M個，即可得到連續好點序列Pc：

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

(8)

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

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

(9)

在上述原理的具體實現中，根據圖2各動坐標系的特點，可以采用一個特殊的隊列完成連續好點序列的提取過程。

2.2 壞點清空的單隊列

圖2中各動坐標系存在重復好點，采用長度為M的定長隊列即可完成多個動坐標系的統一存儲，實現連續好點序列提取，稱該隊列為壞點清空的單隊列，用Q表示。

隊列Q包括初始態和一般態兩種狀態。隊列未存滿時稱為初始態，此時各好點動坐標系在逐漸生成和生長，但尚未滿足提取連續好點序列的條件；一段時間后，隊列存滿，進入一般態，此時，隊列中的所有元素可構成連續好點序列。

一般態下的壞點清空單隊列如圖3所示。

一般態下隊列包含的所有位置點均為動坐標系起點時刻k0對應的位置點，也是連續的好點，可以稱此時的隊列為起點隊列。隊列中第h個位置點的時刻對應圖2中第h層動坐標的起點時刻。

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

初始態的壞點清空單隊列如圖4所示。

該狀態下，當下一時刻接收到好點時，執行該點入隊操作；當接收到壞點時，清空隊列。

3 直線行駛工況的判別

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

首先對連續好點序列Pc采用最小二乘法進行直線擬合[14]，通過標準差條形分布直行判據判斷Pc對應的車輛行駛階段是否為直線行駛，如圖5所示。

圖5中，L為擬合直線，l為連續好點序列擬合直線段的長度，δp為擬合標準差。

連續好點序列Pc中各位置點的橫坐標構成向量X：

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

(10)

縱坐標構成向量Y：

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

(11)

M維單位向量I：

I=[1,1,…,1]

(12)

設擬合直線L的方程式為

y=P1x+P2

(13)

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

(14)

經演算得

(15)

則點到直線距離意義下的標準差[15]為

(16)

標準差條形分布直行判據為

l≥lt

(17)

δP

(18)

式中：lt為直線長度閾值；S為標準差閾值。

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

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

4 實驗結果

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

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

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

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

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

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

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

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

5 結論

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

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

[2] 黃智,鐘志華.車載導航系統陀螺的自適應校正方法[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] 李平舟,陳秀華,吳興林.大學物理實驗[M].西安:西安電子科技大學出版社,2002.

[15] 葉其孝,沈永歡.實用數學手冊[M].北京:科學出版社,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日。