基于低成本MEMS-INS的车辆变道判别方法

宋 翔，李 旭，张为公

（东南大学 仪器科学与工程学院，南京 210096）

基于低成本MEMS-INS的车辆变道判别方法

宋 翔，李 旭，张为公

（东南大学 仪器科学与工程学院，南京 210096）

针对传统车辆变道判别与预测方法的不足，提出一种基于低成本MEMS-INS的车辆变道判别与预测方法。利用车载低成本MEMS-INS和轮速传感器获取的纵向速度、加速度与横摆角速度信息，建立两级卡尔曼滤波器估计道路曲率，在此基础上，将所估计出的道路曲率信息作为观测量，利用交互多模型算法对车辆是否变道进行判别和预测，同时作为附带的效果，能够准确估计出车辆的航向角。实车试验结果表明，车辆变道识别率达到100%，航向角估计误差小于2%，延时小于1 s，具有成本低、精度高、延时短，环境适应力强的优点，克服了传统方法的不足，满足车辆碰撞预警系统的需要。

车辆碰撞预警；车辆变道；交互多模型；低成本MEMS-INS

随着汽车工业的蓬勃发展和汽车保有量迅速增长，道路交通事故频繁发生，统计资料表明，车辆的碰撞事故是最主要的交通事故形态[1]。在车辆的碰撞事故中，尤以车辆的追尾碰撞和车道偏离所引起的碰撞最为常见，危害也最大[2]。针对这两种事故，各种追尾碰撞和车道偏离的预警系统应运而生。在这些系统的研究中，如何有效地判别和预测车辆是否变道是一项关键技术。在车道偏离报警系统中，及时地预测车辆是否偏移本车道，并提前给予驾驶员提醒，能够有效防止因车道偏离所引起的碰撞事故；在追尾碰撞预警系统中，车辆变道与否是一项重要的决策变量，若预警系统判定存在潜在追尾碰撞危险，但本车已进行变道避险，则无需进行报警，以避免对驾驶员进行不必要的影响，有效降低虚警率。

车辆变道的判别主要通过以下几种方式：基于机器视觉的方式[3-4]、基于磁引导的方式[5]和基于高精度GPS/SINS[6-7]的方式等。机器视觉是当前最常用的方式，目前商业化使用的车道偏离预警系统都是基于机器视觉，按照传感器的安装方式可分为俯视系统和前视系统。俯视系统只能在结构化道路上使用（必须存在道路标识，且道路标识能被有效识别），而前视系统虽然可以在没有道路标识的道路上使用，但极易被其他车辆或行人干扰。同时，机器视觉受环境、天气影响较大，在光线不充足的工况下，判别误差较大甚至失效，且若考虑道路曲率，车道线模糊等因素并进行处理，往往导致图像处理工作计算量大，实时性不足。基于磁引导的方式需对现有道路进行改造，在道路下埋设铁磁体标记，道路改造耗资巨大，推广难度高，因此应用较少。基于高精度GPS/SINS的方法需利用昂贵的高精度的GPS获取车辆准确位置，同样存在着成本较高、难于推广的缺点，同时在对弯道行驶的车辆变道的预测和识别上存在较大误差。文献[8]提出利用高精度GPS/SINS结合高精度的电子地图实现车道曲率的判定以实现弯道车辆变道的预测与判别，但现有的电子地图往往无法全面提供所需的信息或地图精度有限，高精度的电子地图技术当前在并未普及且价格同样昂贵，这也限制了该方法的应用。

针对上述几种传统方法的不足，本文采用低成本MEMS-INS(Micro Electro Mechanical Systems-Inertial Navigation System，微机电系统-惯性导航系统)与轮速传感器信息估计道路曲率，并在此基础上利用交互多模型算法实现车辆变道的有效判别。该方法成本低、精度高、环境适应力强，克服了传统方法的不足。

1 方法概述

本文所提出的车辆变道判别与预测方法如图1。

利用车载低成本 MEMS-INS获取车辆纵向加速度与横摆角速度信息，结合由轮速传感器获取的纵向车速信息，建立两级卡尔曼滤波器估计道路曲率。在此基础上，将所估计出的道路曲率信息与纵向车速、加速度和横摆角速度一起作为观测量，利用交互多模型[9]-扩展卡尔曼滤波算法对车辆是否变道进行判别，即针对车道保持工况和车辆变道工况建立不同的扩展卡尔曼模型滤波器，两个模型滤波器通过估计状态的组合实现交互，模型之间基于马尔可夫链进行切换，依据模型概率判定车辆是否进行变道，同时作为附带的效果，能够估计出车辆的航向角，并对横摆角速度等车辆状态观测信息进行滤波。

图1 变道判别方法流程图Fig.1 The flow chart of proposed method

2 基于两级卡尔曼滤波的道路曲率确定

道路曲率利用单回旋模型表示。其表达式为：

式中，c为道路曲率，c0为车辆当前所在点的道路曲率，c1为道路曲率的变化系数，l为自车前方道路上的点离自车的曲线弧长，如图2所示。

图2 道路参数描述Fig.2 Road parameter description

令车辆航向角为θ，θ0为当前航向角，则有

由图2可以推导出x-y坐标系下的道路方程为：

式中，x0、y0为车辆当前所在点的坐标。若有：

据此，可以得出表示包含道路曲率系数c0和c1的增量道路方程。当前估计道路曲率的方法有利用视觉传感器、高精度GPS结合电子地图、雷达等，但这些方法或运算量较大，或成本太高，或需要大量先验知识，都限制了其使用。本文采用车载低成本传感器，通过测量车辆运动参数以估计道路曲率参数，假设在正常行驶工况下（非变道），车辆沿道路中心线行驶，利用轮速传感器获取两个非转向轮的车轮线速度，取其平均值作为车辆纵向车速v，利用低成本 MEMSINS可获取车辆横摆角速度r与纵向加速度a，则道路曲率参数c0和c1可以根据简单的数学关系由计算出。由于车辆传感器仅能获取含噪声的v、r与a，首先建立车辆状态估计滤波器估计，进而建立道路曲率估计滤波器以估计c0和c1。

车辆状态估计滤波器的状态方程与观测方程为：

车辆状态滤波器的估计结果用于合成道路曲率估计滤波器的观测量，通过对公式(3)进行一阶和二阶微分（假设θ0=0，不失一般性）用以推导道路曲率参数c0和c1和车辆状态参数之间的关系：

即

据此可对车辆前方道路建立模型，每个时刻车辆的当前位置被认为是道路模型的起点，而v,v˙,r,r˙也是每个时刻的瞬时估计值，令l= 0（起始点），则有：

则可以建立道路曲率估计滤波器：

据此可利用标准卡尔曼滤波递推以实时估计道路曲率参数c0和c1。

3 基于交互多模型的车辆变道判别

3.1 模型设置

针对车辆的车道保持（keep-lane，KL）和变道（change-lane，CL）工况，分别建立基于 bicycle 模型的运动学模型，作为交互多模型的状态模型，两个模型离散化后的状态方程和观测方程可统一记为：

式中，上标j表示CL或KL模型（j=CL,KL），状态向量，观测向量，观测矩阵；表示互不相关的零均值的系统高斯白噪声向量和观测白噪声向量，且。两个系统模型间的转移概率用一个马尔科夫链表示：

式中，mk是系统模型；pij是转移概率，下标i、j表示从状态i转移到状态j的概率。

3.2 交互估计计算

则交互估计后j滤波器在k时刻的输入为：

3.3 模型条件滤波

每个滤波器各自进行扩展卡尔曼滤波递推，j模型mj(k)的滤波过程如下：

时间更新：

其中，A是系统状态函数向量f对状态向量x求偏导数的雅可比矩阵，即矩阵A的第m行第n列元素A[m,n]可通过下式求得：

测量更新：

3.4 模型概率更新

在每个模型完成上一步的更新之后，利用最大似然函数Λj(k)计算新的模型概率。最大似然函数计算如下：

因此，模型j在k时刻的模型概率由贝叶斯定理：

在计算出各模型为正确的后验概率之后，即可根据KL和CL模型的概率大小，判别当前车辆是处于车道保持还是变道状态。为有效防止误判，设定阈值为0.6，即当变道概率大于0.6时判定为变道。

3.5 估计组合

作为附加的效果，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到最终的状态估计为：，即对车辆航向角进行估计，并对车速、加速度、横摆角速度等含噪声的传感器信息进行滤波。

4 试验验证与分析

为检验本文提出的车辆变道判别方法的实际效果，进行了实车试验。试验所采用的车辆为别克赛欧三厢轿车。安装 MEMS-INS与轮速传感器获取观测量，同时安装双天线GPS获取车辆航向角与车辆行驶轨迹，以验证算法的附带效果-航向角估计的准确性。利用基于PC104的工控机实时采集数据，如图3所示。

图3 试验车辆及传感器Fig.3 Test vehicle and sensors

4.1 直道行驶试验

对本文方法在直道行驶工况下进行试验验证，共进行6轮直线试验验证，共计变道17次，变道识别率100%。以某次试验为例，试验时间为58 s，采样频率20 Hz，图4为GPS所输出的车辆运行轨迹，以及传感器所测得的车辆纵向速度、加速度、横摆角速度，图5为模型概率，图 6为本文方法估计所得的车辆航向角与双天线GPS测得的车辆航向角（即真实值）的比较，以及以横摆角速度为例的滤波效果。

图4 传感器测量值Fig.4 Sensors measurements

结合道路轨迹、横摆角速度与航向角可见，车辆共进行4次变道，即分别在10 s、20 s左右进行变道，但变道持续时间较短，在30 s和45 s左右进行持续时间较长的连续变道，由图5可以看出，分别在变道对应的时间，变道模型的概率较大，延时小于1 s，且持续时间也吻合实际情况，有效地实现了车辆变道的判别，为防止预判，在实际应用中设阈值为0.6，即当变道模型概率大于0.6时，认为车辆进行了变道。同时，由图6可见，作为本文方法的附带效果，航向角估计与真实值较为吻合，误差率小于2%，取得了较好的估计效果，而横摆角速度经过滤波也更为平滑。

图5 模型概率Fig.5 Models probability

结合道路轨迹、横摆角速度与航向角可见，车辆共进行4次变道，即分别在10 s、20 s左右进行变道，但变道持续时间较短，在30 s和45 s左右进行持续时间较长的连续变道，由图5可以看出，分别在变道对应的时间，变道模型的概率较大，延时小于1 s，且持续时间也吻合实际情况，有效地实现了车辆变道的判别，为防止预判，在实际应用中设阈值为0.6，即当变道模型概率大于0.6时，认为车辆进行了变道。同时，由图6可见，作为本文方法的附带效果，航向角估计与真实值较为吻合，误差率小于2%，取得了较好的估计效果，而横摆角速度经过滤波也更为平滑。

4.2 弯道行驶试验

对本文方法在弯道行驶工况下进行试验验证，共进行10轮弯道试验验证，共计变道31次，车辆变道识别率100%。以某次试验为例，试验时间为52 s，采样频率20 Hz，图7为GPS所输出的车辆运行轨迹及传感器所测得的车辆纵向速度、加速度、横摆角速度。

图8为模型概率，图9为本文方法估计所得的车辆航向角与双天线 GPS测得的车辆航向角（即真实值）的比较，以及以横摆角速度为例的滤波效果。

图7 传感器测量值Fig.7 Sensors measurements

图8 模型概率Fig.8 Models probability

车辆共进行3次变道，即分别在15 s、25 s和41 s左右进行变道，由图8可以看出，分别在变道对应的时间，变道模型的概率较大，延时小于1 s，且持续时间也吻合实际情况，有效地实现了车辆变道的判别。应注意的是，在38 s左右车辆为急拐弯而非变道，而此时变道概率小于阈值0.6，有效地防止了误判。由图9可见，航向角估计与真实值误差小于2%，而横摆角速度经过滤波也更为平滑。

图9 附带效果Fig.9 Additional effect

5 结 论

针对传统方法的缺点和不足，本文提出了一种基于低成本MEMS-INS的车辆变道判别方法。利用车载低成本 MEMS-INS和轮速传感器获取的信息估计道路曲率，在此基础上，将估计出的道路曲率信息作为观测量，利用交互多模型算法对车辆是否变道进行判别，同时作为附带的效果，能够估计出车辆的航向角。实车试验证明，该方法系统成本低，对环境的适应力强，延时短，具有较高的测量精度，有效克服了传统方法所存在的精度较低、成本高等缺点。

（References）：

[1]Lee K,Peng H.Evaluation of automotive forward collision warning and collision avoidance algorithms[J].Vehicle System Dynamics,2005,10(43):735-751.

[2]Chang T H,Hsu C S,Wang C,Yang L K.Onboard measurement and warning module for irregular vehicle behavior[J].IEEE Transactions on Intelligent Transportation Systems,2008,9(3):501-513

[3]李旭，张为公.基于视觉的车道偏离报警系统的研究[J].仪器仪表学报，2008，7(29)：1554-1558.LI Xu,ZHANG Wei-gong.Research on lane departure warning system based on machine vision[J].Chinese Journal of Scientific Instrument,2008,7(29):1554-1558.

[4]Hsiao P Y,Yeh C W,Huang S S.A portable vision-based real-time lane departure warning system:Day and night[J].IEEE Transactions on Vehicular Technology,2009,58(4):2089-2094.

[5]Enache N M,Mammar S,Netto M.Driver steering assistance for lane-departure avoidance based on hybrid automata and composite Lyapunov function[J].IEEE Transactions on Intelligent Transportation Systems,2010,11(1):28-39.

[6]Joshua M C,David M B.A low-cost solution for an integrated multisensor lane departure warning system[J].IEEE Transactions on Intelligent Transportation Systems,2009,10(1):47-39.

[7]Rafael T M,Miguel A Z.IMM-based lane-change prediction in highways with low-cost GPS/INS[J].IEEE Transactions on Intelligent Transportation Systems,2009,10(1):180-185.

[8]Rafael T M,Miguel A Z.Collision avoidance support in roads with lateral and longitudinal maneuver prediction by fusing GPS/IMU and digital maps[J].Transportation Research Part C,2010,18:611-625.

[9]赵琳，王小旭，丁继成，曹伟.组合导航系统非线性滤波算法综述[J].中国惯性技术学报，2009，17(1)：46-52.ZHAO Lin,WANG Xiao-XU,DING Ji-cheng.Overview of nonlinear filter methods applied in integrated navigation system[J].Journal of Chinese Inertial Technology,2009,17(1):46-52.

[10]李旭，宋翔，张国胜，于家河，张为公.基于改进卡尔曼滤波的汽车路试制动性能检测方法[J].中国惯性技术学报，2011，19(6)：760-764.LI Xu,SONG Xiang,ZHANG Guo-sheng,YU Jia-he,ZHANG Wei-gong.Test method of vehicle braking performance based on improved Kalman filtering[J].Journal of Chinese Inertial Technology,2011,19(6):760-764.

Identification method of vehicle lane changes based on low-cost MEMS-INS

SONG Xiang,LI Xu,ZHANG Wei-gong
(School of Instrument Science and Engineering,Southeast University,NanJing 210096,China)

To overcome the shortcomings of traditional vehicle lane changes identification and prediction methods,a novel identification and prediction method of vehicle lane-change based on low-cost MEMS-INS is proposed.The speed,acceleration and yaw rate from MEMS-INS and wheel speed sensor are taken as the observed information of Kalman filter.By two-level Kalman filter recursion algorithm,the road curvature parameters are acquired.Then the road curvature parameters are considered as observed information,and the interactive multiple model method is employed for predicting lane changes.As an additional effect,the

heading angle is estimated with high precision.The real vehicle tests demonstrate that the identification rate of lane changes is 100%,the estimation error of heading angle is less than 2%,and the latency times is less than 1 s.The proposed method has such advantages as low cost,high precision,environmental adaptability and very short latency times,which overcomes the shortcomings of traditional methods and meets the acquirements of vehicle collision warning system.

vehicle collision warning;vehicle lane changes;interactive multiple model;low-cost MEMS-INS

TP273

：A

1005-6734(2014)01-0067-07

10.13695/j.cnki.12-1222/o3.2014.01.014

2013-09-28；

：2013-11-19

国家自然科学基金资助项目（61273236）；江苏省自然科学基金资助项目(BK2010239)；教育部博士点基金资助项目（200802861061）

宋翔（1984—），男，博士，研究方向智能导航与控制。E-mail：sx2190105@foxmail.com

联 系 人：李旭（1975—），男，副教授，研究方向智能导航与控制，博士生导师。E-mail：lixu.mail@163.com