基于低成本MEMS-INS的車輛變道判別方法

宋 翔，李 旭，張為公

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

基于低成本MEMS-INS的車輛變道判別方法

宋 翔，李 旭，張為公

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

針對傳統(tǒng)車輛變道判別與預(yù)測方法的不足，提出一種基于低成本MEMS-INS的車輛變道判別與預(yù)測方法。利用車載低成本MEMS-INS和輪速傳感器獲取的縱向速度、加速度與橫擺角速度信息，建立兩級卡爾曼濾波器估計(jì)道路曲率，在此基礎(chǔ)上，將所估計(jì)出的道路曲率信息作為觀測量，利用交互多模型算法對車輛是否變道進(jìn)行判別和預(yù)測，同時(shí)作為附帶的效果，能夠準(zhǔn)確估計(jì)出車輛的航向角。實(shí)車試驗(yàn)結(jié)果表明，車輛變道識別率達(dá)到100%，航向角估計(jì)誤差小于2%，延時(shí)小于1 s，具有成本低、精度高、延時(shí)短，環(huán)境適應(yīng)力強(qiáng)的優(yōu)點(diǎn)，克服了傳統(tǒng)方法的不足，滿足車輛碰撞預(yù)警系統(tǒng)的需要。

車輛碰撞預(yù)警；車輛變道；交互多模型；低成本MEMS-INS

隨著汽車工業(yè)的蓬勃發(fā)展和汽車保有量迅速增長，道路交通事故頻繁發(fā)生，統(tǒng)計(jì)資料表明，車輛的碰撞事故是最主要的交通事故形態(tài)[1]。在車輛的碰撞事故中，尤以車輛的追尾碰撞和車道偏離所引起的碰撞最為常見，危害也最大[2]。針對這兩種事故，各種追尾碰撞和車道偏離的預(yù)警系統(tǒng)應(yīng)運(yùn)而生。在這些系統(tǒng)的研究中，如何有效地判別和預(yù)測車輛是否變道是一項(xiàng)關(guān)鍵技術(shù)。在車道偏離報(bào)警系統(tǒng)中，及時(shí)地預(yù)測車輛是否偏移本車道，并提前給予駕駛員提醒，能夠有效防止因車道偏離所引起的碰撞事故；在追尾碰撞預(yù)警系統(tǒng)中，車輛變道與否是一項(xiàng)重要的決策變量，若預(yù)警系統(tǒng)判定存在潛在追尾碰撞危險(xiǎn)，但本車已進(jìn)行變道避險(xiǎn)，則無需進(jìn)行報(bào)警，以避免對駕駛員進(jìn)行不必要的影響，有效降低虛警率。

車輛變道的判別主要通過以下幾種方式：基于機(jī)器視覺的方式[3-4]、基于磁引導(dǎo)的方式[5]和基于高精度GPS/SINS[6-7]的方式等。機(jī)器視覺是當(dāng)前最常用的方式，目前商業(yè)化使用的車道偏離預(yù)警系統(tǒng)都是基于機(jī)器視覺，按照傳感器的安裝方式可分為俯視系統(tǒng)和前視系統(tǒng)。俯視系統(tǒng)只能在結(jié)構(gòu)化道路上使用（必須存在道路標(biāo)識，且道路標(biāo)識能被有效識別），而前視系統(tǒng)雖然可以在沒有道路標(biāo)識的道路上使用，但極易被其他車輛或行人干擾。同時(shí)，機(jī)器視覺受環(huán)境、天氣影響較大，在光線不充足的工況下，判別誤差較大甚至失效，且若考慮道路曲率，車道線模糊等因素并進(jìn)行處理，往往導(dǎo)致圖像處理工作計(jì)算量大，實(shí)時(shí)性不足?；诖乓龑?dǎo)的方式需對現(xiàn)有道路進(jìn)行改造，在道路下埋設(shè)鐵磁體標(biāo)記，道路改造耗資巨大，推廣難度高，因此應(yīng)用較少?；诟呔菺PS/SINS的方法需利用昂貴的高精度的GPS獲取車輛準(zhǔn)確位置，同樣存在著成本較高、難于推廣的缺點(diǎn)，同時(shí)在對彎道行駛的車輛變道的預(yù)測和識別上存在較大誤差。文獻(xiàn)[8]提出利用高精度GPS/SINS結(jié)合高精度的電子地圖實(shí)現(xiàn)車道曲率的判定以實(shí)現(xiàn)彎道車輛變道的預(yù)測與判別，但現(xiàn)有的電子地圖往往無法全面提供所需的信息或地圖精度有限，高精度的電子地圖技術(shù)當(dāng)前在并未普及且價(jià)格同樣昂貴，這也限制了該方法的應(yīng)用。

針對上述幾種傳統(tǒng)方法的不足，本文采用低成本MEMS-INS(Micro Electro Mechanical Systems-Inertial Navigation System，微機(jī)電系統(tǒng)-慣性導(dǎo)航系統(tǒng))與輪速傳感器信息估計(jì)道路曲率，并在此基礎(chǔ)上利用交互多模型算法實(shí)現(xiàn)車輛變道的有效判別。該方法成本低、精度高、環(huán)境適應(yīng)力強(qiáng)，克服了傳統(tǒng)方法的不足。

1 方法概述

本文所提出的車輛變道判別與預(yù)測方法如圖1。

利用車載低成本 MEMS-INS獲取車輛縱向加速度與橫擺角速度信息，結(jié)合由輪速傳感器獲取的縱向車速信息，建立兩級卡爾曼濾波器估計(jì)道路曲率。在此基礎(chǔ)上，將所估計(jì)出的道路曲率信息與縱向車速、加速度和橫擺角速度一起作為觀測量，利用交互多模型[9]-擴(kuò)展卡爾曼濾波算法對車輛是否變道進(jìn)行判別，即針對車道保持工況和車輛變道工況建立不同的擴(kuò)展卡爾曼模型濾波器，兩個(gè)模型濾波器通過估計(jì)狀態(tài)的組合實(shí)現(xiàn)交互，模型之間基于馬爾可夫鏈進(jìn)行切換，依據(jù)模型概率判定車輛是否進(jìn)行變道，同時(shí)作為附帶的效果，能夠估計(jì)出車輛的航向角，并對橫擺角速度等車輛狀態(tài)觀測信息進(jìn)行濾波。

圖1 變道判別方法流程圖Fig.1 The flow chart of proposed method

2 基于兩級卡爾曼濾波的道路曲率確定

道路曲率利用單回旋模型表示。其表達(dá)式為：

式中，c為道路曲率，c0為車輛當(dāng)前所在點(diǎn)的道路曲率，c1為道路曲率的變化系數(shù)，l為自車前方道路上的點(diǎn)離自車的曲線弧長，如圖2所示。

圖2 道路參數(shù)描述Fig.2 Road parameter description

令車輛航向角為θ，θ0為當(dāng)前航向角，則有

由圖2可以推導(dǎo)出x-y坐標(biāo)系下的道路方程為：

式中，x0、y0為車輛當(dāng)前所在點(diǎn)的坐標(biāo)。若有：

據(jù)此，可以得出表示包含道路曲率系數(shù)c0和c1的增量道路方程。當(dāng)前估計(jì)道路曲率的方法有利用視覺傳感器、高精度GPS結(jié)合電子地圖、雷達(dá)等，但這些方法或運(yùn)算量較大，或成本太高，或需要大量先驗(yàn)知識，都限制了其使用。本文采用車載低成本傳感器，通過測量車輛運(yùn)動參數(shù)以估計(jì)道路曲率參數(shù)，假設(shè)在正常行駛工況下（非變道），車輛沿道路中心線行駛，利用輪速傳感器獲取兩個(gè)非轉(zhuǎn)向輪的車輪線速度，取其平均值作為車輛縱向車速v，利用低成本 MEMSINS可獲取車輛橫擺角速度r與縱向加速度a，則道路曲率參數(shù)c0和c1可以根據(jù)簡單的數(shù)學(xué)關(guān)系由計(jì)算出。由于車輛傳感器僅能獲取含噪聲的v、r與a，首先建立車輛狀態(tài)估計(jì)濾波器估計(jì)，進(jìn)而建立道路曲率估計(jì)濾波器以估計(jì)c0和c1。

車輛狀態(tài)估計(jì)濾波器的狀態(tài)方程與觀測方程為：

車輛狀態(tài)濾波器的估計(jì)結(jié)果用于合成道路曲率估計(jì)濾波器的觀測量，通過對公式(3)進(jìn)行一階和二階微分（假設(shè)θ0=0，不失一般性）用以推導(dǎo)道路曲率參數(shù)c0和c1和車輛狀態(tài)參數(shù)之間的關(guān)系：

即

據(jù)此可對車輛前方道路建立模型，每個(gè)時(shí)刻車輛的當(dāng)前位置被認(rèn)為是道路模型的起點(diǎn)，而v,v˙,r,r˙也是每個(gè)時(shí)刻的瞬時(shí)估計(jì)值，令l= 0（起始點(diǎn)），則有：

則可以建立道路曲率估計(jì)濾波器：

據(jù)此可利用標(biāo)準(zhǔn)卡爾曼濾波遞推以實(shí)時(shí)估計(jì)道路曲率參數(shù)c0和c1。

3 基于交互多模型的車輛變道判別

3.1 模型設(shè)置

針對車輛的車道保持（keep-lane，KL）和變道（change-lane，CL）工況，分別建立基于 bicycle 模型的運(yùn)動學(xué)模型，作為交互多模型的狀態(tài)模型，兩個(gè)模型離散化后的狀態(tài)方程和觀測方程可統(tǒng)一記為：

式中，上標(biāo)j表示CL或KL模型（j=CL,KL），狀態(tài)向量，觀測向量，觀測矩陣；表示互不相關(guān)的零均值的系統(tǒng)高斯白噪聲向量和觀測白噪聲向量，且。兩個(gè)系統(tǒng)模型間的轉(zhuǎn)移概率用一個(gè)馬爾科夫鏈表示：

式中，mk是系統(tǒng)模型；pij是轉(zhuǎn)移概率，下標(biāo)i、j表示從狀態(tài)i轉(zhuǎn)移到狀態(tài)j的概率。

3.2 交互估計(jì)計(jì)算

則交互估計(jì)后j濾波器在k時(shí)刻的輸入為：

3.3 模型條件濾波

每個(gè)濾波器各自進(jìn)行擴(kuò)展卡爾曼濾波遞推，j模型mj(k)的濾波過程如下：

時(shí)間更新：

其中，A是系統(tǒng)狀態(tài)函數(shù)向量f對狀態(tài)向量x求偏導(dǎo)數(shù)的雅可比矩陣，即矩陣A的第m行第n列元素A[m,n]可通過下式求得：

測量更新：

3.4 模型概率更新

在每個(gè)模型完成上一步的更新之后，利用最大似然函數(shù)Λj(k)計(jì)算新的模型概率。最大似然函數(shù)計(jì)算如下：

因此，模型j在k時(shí)刻的模型概率由貝葉斯定理：

在計(jì)算出各模型為正確的后驗(yàn)概率之后，即可根據(jù)KL和CL模型的概率大小，判別當(dāng)前車輛是處于車道保持還是變道狀態(tài)。為有效防止誤判，設(shè)定閾值為0.6，即當(dāng)變道概率大于0.6時(shí)判定為變道。

3.5 估計(jì)組合

作為附加的效果，對所有濾波器的狀態(tài)估計(jì)進(jìn)行概率加權(quán)并求和，權(quán)系數(shù)為模型正確的后驗(yàn)概率，得到最終的狀態(tài)估計(jì)為：，即對車輛航向角進(jìn)行估計(jì)，并對車速、加速度、橫擺角速度等含噪聲的傳感器信息進(jìn)行濾波。

4 試驗(yàn)驗(yàn)證與分析

為檢驗(yàn)本文提出的車輛變道判別方法的實(shí)際效果，進(jìn)行了實(shí)車試驗(yàn)。試驗(yàn)所采用的車輛為別克賽歐三廂轎車。安裝 MEMS-INS與輪速傳感器獲取觀測量，同時(shí)安裝雙天線GPS獲取車輛航向角與車輛行駛軌跡，以驗(yàn)證算法的附帶效果-航向角估計(jì)的準(zhǔn)確性。利用基于PC104的工控機(jī)實(shí)時(shí)采集數(shù)據(jù)，如圖3所示。

圖3 試驗(yàn)車輛及傳感器Fig.3 Test vehicle and sensors

4.1 直道行駛試驗(yàn)

對本文方法在直道行駛工況下進(jìn)行試驗(yàn)驗(yàn)證，共進(jìn)行6輪直線試驗(yàn)驗(yàn)證，共計(jì)變道17次，變道識別率100%。以某次試驗(yàn)為例，試驗(yàn)時(shí)間為58 s，采樣頻率20 Hz，圖4為GPS所輸出的車輛運(yùn)行軌跡，以及傳感器所測得的車輛縱向速度、加速度、橫擺角速度，圖5為模型概率，圖 6為本文方法估計(jì)所得的車輛航向角與雙天線GPS測得的車輛航向角（即真實(shí)值）的比較，以及以橫擺角速度為例的濾波效果。

圖4 傳感器測量值Fig.4 Sensors measurements

結(jié)合道路軌跡、橫擺角速度與航向角可見，車輛共進(jìn)行4次變道，即分別在10 s、20 s左右進(jìn)行變道，但變道持續(xù)時(shí)間較短，在30 s和45 s左右進(jìn)行持續(xù)時(shí)間較長的連續(xù)變道，由圖5可以看出，分別在變道對應(yīng)的時(shí)間，變道模型的概率較大，延時(shí)小于1 s，且持續(xù)時(shí)間也吻合實(shí)際情況，有效地實(shí)現(xiàn)了車輛變道的判別，為防止預(yù)判，在實(shí)際應(yīng)用中設(shè)閾值為0.6，即當(dāng)變道模型概率大于0.6時(shí)，認(rèn)為車輛進(jìn)行了變道。同時(shí)，由圖6可見，作為本文方法的附帶效果，航向角估計(jì)與真實(shí)值較為吻合，誤差率小于2%，取得了較好的估計(jì)效果，而橫擺角速度經(jīng)過濾波也更為平滑。

圖5 模型概率Fig.5 Models probability

結(jié)合道路軌跡、橫擺角速度與航向角可見，車輛共進(jìn)行4次變道，即分別在10 s、20 s左右進(jìn)行變道，但變道持續(xù)時(shí)間較短，在30 s和45 s左右進(jìn)行持續(xù)時(shí)間較長的連續(xù)變道，由圖5可以看出，分別在變道對應(yīng)的時(shí)間，變道模型的概率較大，延時(shí)小于1 s，且持續(xù)時(shí)間也吻合實(shí)際情況，有效地實(shí)現(xiàn)了車輛變道的判別，為防止預(yù)判，在實(shí)際應(yīng)用中設(shè)閾值為0.6，即當(dāng)變道模型概率大于0.6時(shí)，認(rèn)為車輛進(jìn)行了變道。同時(shí)，由圖6可見，作為本文方法的附帶效果，航向角估計(jì)與真實(shí)值較為吻合，誤差率小于2%，取得了較好的估計(jì)效果，而橫擺角速度經(jīng)過濾波也更為平滑。

4.2 彎道行駛試驗(yàn)

對本文方法在彎道行駛工況下進(jìn)行試驗(yàn)驗(yàn)證，共進(jìn)行10輪彎道試驗(yàn)驗(yàn)證，共計(jì)變道31次，車輛變道識別率100%。以某次試驗(yàn)為例，試驗(yàn)時(shí)間為52 s，采樣頻率20 Hz，圖7為GPS所輸出的車輛運(yùn)行軌跡及傳感器所測得的車輛縱向速度、加速度、橫擺角速度。

圖8為模型概率，圖9為本文方法估計(jì)所得的車輛航向角與雙天線 GPS測得的車輛航向角（即真實(shí)值）的比較，以及以橫擺角速度為例的濾波效果。

圖7 傳感器測量值Fig.7 Sensors measurements

圖8 模型概率Fig.8 Models probability

車輛共進(jìn)行3次變道，即分別在15 s、25 s和41 s左右進(jìn)行變道，由圖8可以看出，分別在變道對應(yīng)的時(shí)間，變道模型的概率較大，延時(shí)小于1 s，且持續(xù)時(shí)間也吻合實(shí)際情況，有效地實(shí)現(xiàn)了車輛變道的判別。應(yīng)注意的是，在38 s左右車輛為急拐彎而非變道，而此時(shí)變道概率小于閾值0.6，有效地防止了誤判。由圖9可見，航向角估計(jì)與真實(shí)值誤差小于2%，而橫擺角速度經(jīng)過濾波也更為平滑。

圖9 附帶效果Fig.9 Additional effect

5 結(jié) 論

針對傳統(tǒng)方法的缺點(diǎn)和不足，本文提出了一種基于低成本MEMS-INS的車輛變道判別方法。利用車載低成本 MEMS-INS和輪速傳感器獲取的信息估計(jì)道路曲率，在此基礎(chǔ)上，將估計(jì)出的道路曲率信息作為觀測量，利用交互多模型算法對車輛是否變道進(jìn)行判別，同時(shí)作為附帶的效果，能夠估計(jì)出車輛的航向角。實(shí)車試驗(yàn)證明，該方法系統(tǒng)成本低，對環(huán)境的適應(yīng)力強(qiáng)，延時(shí)短，具有較高的測量精度，有效克服了傳統(tǒng)方法所存在的精度較低、成本高等缺點(diǎn)。

（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]李旭，張為公.基于視覺的車道偏離報(bào)警系統(tǒng)的研究[J].儀器儀表學(xué)報(bào)，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]趙琳，王小旭，丁繼成，曹偉.組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J].中國慣性技術(shù)學(xué)報(bào)，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ìn)卡爾曼濾波的汽車路試制動性能檢測方法[J].中國慣性技術(shù)學(xué)報(bào)，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

國家自然科學(xué)基金資助項(xiàng)目（61273236）；江蘇省自然科學(xué)基金資助項(xiàng)目(BK2010239)；教育部博士點(diǎn)基金資助項(xiàng)目（200802861061）

宋翔（1984—），男，博士，研究方向智能導(dǎo)航與控制。E-mail：sx2190105@foxmail.com

聯(lián) 系 人：李旭（1975—），男，副教授，研究方向智能導(dǎo)航與控制，博士生導(dǎo)師。E-mail：lixu.mail@163.com