车辆导航系统横摆角速度传感器零点偏移的自动校正*

李江涛，杨殿阁，杨 扬，张 涛，连小珉

(清华大学汽车工程系，汽车安全与节能国家重点实验室，北京 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日。