基于低成本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