基于卡爾曼濾波的賽道檢測優(yōu)化算法

劉新寧

（長安大學(xué) 汽車學(xué)院，陜西 西安 710064）

目前，汽車正朝著智能化方向迅猛發(fā)展，高 級駕駛輔助系統(tǒng)發(fā)展迅速，各大汽車廠商都在發(fā)展智能車。主流的智能汽車都在使用攝像頭對前方道路進(jìn)行識別，因此，做好攝像頭智能車是研究智能輔助駕駛的重要前提。

卡爾曼濾波算法在智能輔助駕駛方面有著廣闊的應(yīng)用前景?？柭鼮V波算法的關(guān)鍵是根據(jù)某一時刻“實際測量值”和上一時刻的“預(yù)測估計值”以及估計誤差和測量誤差，通過加權(quán)處理計算得到該時刻的最優(yōu)結(jié)果，再對下一時刻的數(shù)據(jù)進(jìn)行預(yù)測。隨著數(shù)據(jù)的不斷更新，誤差也在不斷迭代，從而實現(xiàn)數(shù)據(jù)的不斷更新[1]。

本文的主要研究工作是通過對比嵌入卡爾曼濾波算法前后，攝像頭智能車對賽道的檢測情況，來判斷卡爾曼濾波算法是否可對攝像頭智能車識別賽道起到優(yōu)化作用。

1 智能車的結(jié)構(gòu)及原理

攝像頭智能車的結(jié)構(gòu)主要包括車模、英飛凌TC264單片機(jī)、主板、驅(qū)動板、攝像頭、顯示屏、電池、編碼器等[2]。

攝像頭智能車以32位的英飛凌TC264單片機(jī)作為主控制器。通過制作主控板及驅(qū)動板，使攝像頭智能車的各個模塊之間相互配合，共同控制小車的運行。通過攝像頭采集賽道信息，并對采集的圖像進(jìn)行矯正處理，以獲取準(zhǔn)確的道路信息[3]。隨后微控制器處理賽道信息和車輛自身運行的各項參數(shù)，計算出輸送給電機(jī)和舵機(jī)的占空比，完成對攝像頭智能車的控制。

2 硬件系統(tǒng)設(shè)計

攝像頭智能車硬件系統(tǒng)主要劃分為6個模塊：主控制器模塊、電源供電模塊、攝像頭圖像信息采集模塊、圖像顯示模塊、方向控制模塊和電機(jī)驅(qū)動模塊[4]，攝像頭智能車的硬件系統(tǒng)整體設(shè)計框圖如圖1所示。

圖1 攝像頭智能車硬件整體設(shè)計框圖

電機(jī)驅(qū)動模塊是保證攝像頭智能車按照賽道軌跡行駛的關(guān)鍵。對于電機(jī)來說，只要給一定的電壓值，電機(jī)就可以轉(zhuǎn)動。但是對于攝像頭智能車來說，希望其電機(jī)轉(zhuǎn)速可以按照期望值變化，同時還可以實現(xiàn)正反轉(zhuǎn)，這時施加給電機(jī)的電壓就不能是某一定值，因此，需要設(shè)計驅(qū)動電路，實現(xiàn)電機(jī)調(diào)速與正反轉(zhuǎn)。

目前最常見的電機(jī)驅(qū)動電路之一就是H橋驅(qū)動電路，通過金屬氧化物半導(dǎo)體管（Metal Oxide Semiconductor, MOS）的開合來控制電機(jī)正反轉(zhuǎn)，還可以直接通過控制占空比的大小來改變電機(jī)兩端電壓，占空比越大，則電機(jī)兩端獲得的電壓值越大，電機(jī)轉(zhuǎn)速越高，從而使得攝像頭智能車可以獲得更快的行駛車速。

組成H橋驅(qū)動電路的方式有很多，可以使用分立元件組裝成H橋驅(qū)動電路，也可以直接使用電機(jī)驅(qū)動芯片。為了保證工作穩(wěn)定可靠，同時降低成本，采用BTN系列的電機(jī)驅(qū)動芯片。只有驅(qū)動芯片還不足以驅(qū)動電機(jī)，需要按照芯片的原理構(gòu)建驅(qū)動電路，將驅(qū)動芯片分別與電阻、電容、電源以及電機(jī)相連。為了防止反向電流過大造成單片機(jī)的損壞，應(yīng)加入隔離芯片。利用 BTN7971電機(jī)驅(qū)動芯片和74LVC245隔離芯片來構(gòu)建H橋電機(jī)驅(qū)動電路，因為74LVC245芯片需要使用5 V供電，而電源電壓為7.2 V，所以還需要使用穩(wěn)壓芯片將7.2 V降低至5 V來給隔離芯片供電，采用AMS1117穩(wěn)壓芯片進(jìn)行降壓。

3 軟件系統(tǒng)設(shè)計

3.1 賽道識別

攝像頭智能車主要依靠攝像頭對賽道進(jìn)行識別。首先由于光線的存在，攝像頭前方的景物會生成光學(xué)圖像，傳感器將光信號轉(zhuǎn)化為電信號，經(jīng)過圖像處理器（Image Signal Process, ISP）和數(shù)字信號處理芯片（Digital Signal Process, DSP）處理后[5]，就可以通過屏幕觀察到拍攝到的圖像。智能車的攝像頭也是一樣的道理。

智能車的攝像頭通過場中斷、行中斷和直接存儲器訪問（Direct Memory Access, DMA）中斷來采集圖像。采集圖像時開啟場中段，當(dāng)檢測完一行之后，開啟行中斷，攝像頭將采集到的數(shù)據(jù)存入數(shù)組中。采集完成后，行中斷和場中段都會關(guān)閉，從而為下一次圖像采集做好準(zhǔn)備[6]。

3.2 圖像二值化處理

圖像二值化處理，簡單來說就是選擇一個合適的閾值，根據(jù)閾值將圖像分割成黑或者白，反映圖像的局部特征，從而實現(xiàn)目標(biāo)物的識別[7]。

圖2展示了五種二值化方法處理后的圖像，其中原圖代表初始拍攝圖像。A圖像的二值化方法為超過閾值部分取最大值255，否則取0；B圖像的二值化方法為超過閾值部分取最小值0，否則取255；C圖像的二值化方法為大于閾值部分設(shè)為閾值，小于閾值部分不變；D圖像的二值化方法為大于閾值部分不變，小于閾值部分設(shè)為0；E圖像的二值化方法為小于閾值部分不變，大于閾值部分設(shè)為0。

圖2 圖像二值化

從圖中可以看出，通過二值化處理，圖像非黑即白。傳統(tǒng)的二值化算法先設(shè)定一個固定的灰度值，圖像按此值進(jìn)行二值化分割，大于該值的設(shè)置為255，小于該值的設(shè)置為0。但是由于賽道環(huán)境千變?nèi)f化，干擾太多，傳統(tǒng)分割方法已經(jīng)無法實現(xiàn)對賽道的準(zhǔn)確分割[8]，故采用大津法，也就是俗稱的最大類間方差法對賽道進(jìn)行二值化處理。

3.3 賽道邊線獲取

首先根據(jù)攝像頭采集到的數(shù)據(jù)獲取該幀圖像的像素矩陣，通過軟件二值化，將灰度圖像像素矩陣轉(zhuǎn)化為黑白圖像像素矩陣，利用該矩陣尋找賽道的黑白黑跳變點。為了保證準(zhǔn)確度，將有效行設(shè)置為18行至57行。圖3為賽道邊線獲取的流程圖。

圖3 賽道邊線獲取流程圖

第一次從第57行的第47列開始向兩側(cè)掃描，直到找到黑白跳變點，停止掃描，記錄這兩條邊線，然后計算出賽道的中線。第二次以第57行的中線位置作為基準(zhǔn)向兩側(cè)掃描。隨著攝像頭不斷采集賽道圖像信息，每幀圖像不斷更新，使得攝像頭智能車不斷計算前方賽道的中線。

3.4 彎道上賽道中線的計算

當(dāng)攝像頭智能車在彎道上行駛時，會出現(xiàn)丟線的現(xiàn)象，如圖4所示。

圖4 丟線現(xiàn)象

從圖4可以看出，攝像頭識別到的圖像只有左側(cè)邊界，而無右側(cè)邊界，如果按照直道上的計算方法獲取中線位置，那么通過計算得到的中線會出現(xiàn)錯誤，影響攝像頭智能車的循跡功能，此時便需要進(jìn)行補(bǔ)線操作。

假如左丟線，則令左邊線為1。接下來再根據(jù)右邊線的位置來判斷右邊線是否越界。如果沒有越界，就根據(jù)該行寬度的一半來推斷中線的位置。如果右邊線越界，那么就以左邊線作為中線的位置。圖5為賽道中線獲取流程圖。

圖5 賽道中線獲取流程圖

3.5 PID控制算法

比例-積分-微分（Proportion Integration Differentiation, PID）控制算法可分為兩種類型：位置式PID控制算法和增量式PID控制算法。在攝像頭智能車電機(jī)控制中，主要采用增量式 PID控制算法。位置式 PID控制算法容易產(chǎn)生累積誤差，而增量式不會產(chǎn)生累積誤差。

在調(diào)位置式速度閉環(huán)控制時先加大Kp值，同時令KI、KD為0，查看編碼器值和目標(biāo)值的差值，當(dāng)反饋回來的編碼器值在目標(biāo)值上下震蕩時加入KI值即可。增量式的KI就是位置式的Kp，因此，對于增量式PID控制先加入KI，再加入Kp。在實際調(diào)試過程中，當(dāng)KI很小時，電機(jī)就能達(dá)到目標(biāo)值。

在對舵機(jī)進(jìn)行控制時，根據(jù)攝像頭采集到的圖像信息獲得賽道實際中線位置，通過其與期望的中線位置做差便可得到偏差，然后再乘以比例系數(shù)Kp便得到此時舵機(jī)應(yīng)該接收到的占空比，從而控制舵機(jī)轉(zhuǎn)向。因為對于舵機(jī)的控制，不需要再去記錄之前的偏差，因此，控制舵機(jī)時不再需要積分環(huán)節(jié)的嵌入。具體的Kp、KD參數(shù)調(diào)節(jié)，應(yīng)該通過多次實際測試獲得。總的來說應(yīng)該先確定Kp值并使KD=0，KI=0，保證攝像頭智能車可以正確循跡。然后逐漸增大Kp的值，以增加攝像頭智能車的轉(zhuǎn)向響應(yīng)速度，但是如果Kp值太大的話會使小車在直線行駛過程中出現(xiàn)震蕩現(xiàn)象，影響攝像頭智能車的行駛穩(wěn)定性，所以此時應(yīng)通過增大KD的值來抑制震蕩。當(dāng)攝像頭智能車不再震蕩后，可以繼續(xù)增加Kp值，直到不管怎樣增大KD值，攝像頭智能車仍會出現(xiàn)震蕩，此時應(yīng)該適當(dāng)降低Kp值。經(jīng)過多次測試，找到最合適的一組參數(shù)。

4 卡爾曼濾波算法

4.1 數(shù)據(jù)融合

假設(shè)現(xiàn)在測量某人身高，獲得兩組數(shù)據(jù)h1=180 cm和h2=185 cm，這兩組數(shù)據(jù)均不準(zhǔn)確，標(biāo)準(zhǔn)差分別為σ1=1 cm和σ2=2 cm，均符合正態(tài)分布。接下來要根據(jù)h1和h2來估計真實值。令?=h1+K(h2-h1)，這就是卡爾曼濾波算法的關(guān)鍵，其中，K為Kalman Gain。所以應(yīng)求得K使得標(biāo)準(zhǔn)差σh?最小，即方差Var(?)最小。

對方差求導(dǎo)可得

推出

將σ1=1 cm，σ2=2 cm帶入得

4.2 狀態(tài)空間表達(dá)

圖6為車身振動的單質(zhì)量系統(tǒng)模型。該模型由車身質(zhì)量m2、彈簧剛度ε、減震器阻尼系數(shù)C的懸架組成。q是輸入的路面不平度函數(shù)[9]；z是車身垂向位移；是車身垂向速度；˙z˙是車身垂向加速度。根據(jù)牛頓第二定律，系統(tǒng)運動的微分方程為

圖6 車身振動的單質(zhì)量系統(tǒng)模型

進(jìn)一步整理可得

令μ=+εq，即為系統(tǒng)輸入，故有

再定義測量量Mea1=z1，且Mea2=z2。

式（9）和式（10）體現(xiàn)了變量隨時間的連續(xù)變化，可以將其離散化為

式中，zk為k時刻的系統(tǒng)狀態(tài)量；zk-1為k-1時刻的系統(tǒng)狀態(tài)量；μk-1為k-1時刻的系統(tǒng)輸入量；Meak為k時刻的系統(tǒng)測量量。

然而在實際生活中，存在著很多不確定性，這就導(dǎo)致所建立的狀態(tài)空間方程會隨著這些不確定性而發(fā)生變化。

那么狀態(tài)空間方程變?yōu)?/p>

式中，wk為過程噪音，體現(xiàn)了模型的不準(zhǔn)確性；vk為測量噪音，體現(xiàn)了測量的不準(zhǔn)確性。

4.3 推導(dǎo)卡爾曼增益

首先由于在計算過程中忽略了過程噪音wk-1，此時zk并不準(zhǔn)確，令先驗估計值，為上一時刻的估計值。

由

推出

式中，為測量結(jié)果。

根據(jù)卡爾曼濾波的思想

式中，?為后驗估計值，G為權(quán)重系數(shù)。為了避免出現(xiàn)求逆運算，再令G=KH，K即為卡爾曼增益。

那么

此外，有P(wk)～(0,Q)，協(xié)方差矩陣且有P(vk)～(0,R)，協(xié)方差矩陣分別是過程噪音和測量噪音的協(xié)方差矩陣。為了獲得最優(yōu)解，要尋找K使得估計值最接近于實際值zk。

令誤差 E rrork=zk-，則有P(Errork)～(0,Pk)，誤差的協(xié)方差矩陣為Pk，從而推出

4.4 卡爾曼濾波算法應(yīng)用

利用卡爾曼濾波的基本原理，對預(yù)測值和測量值進(jìn)行加權(quán)處理，加權(quán)系數(shù)即為卡爾曼增益（Kalman Gain）。預(yù)測值即為賽道二十六行的中線位置，測量值為賽道第三十行的中線位置。為了使攝像頭智能車可以做到提前轉(zhuǎn)向，要對前方賽道進(jìn)行預(yù)測，但是預(yù)測值會與實際值有偏差，所以不能完全相信預(yù)測值，應(yīng)該將預(yù)測值與測量值進(jìn)行數(shù)據(jù)融合，得到一個最優(yōu)的結(jié)果，即最優(yōu)的占空比，使得攝像頭智能車可以提前轉(zhuǎn)向，提高攝像頭智能車的行駛穩(wěn)定性。

以第三十行中線AverageCenter為基準(zhǔn)，與理論中線進(jìn)行分析比對，從而計算出此時應(yīng)該傳遞給舵機(jī)的占空比。為了預(yù)測前方賽道，取第二十六行中線數(shù)據(jù)AverageCenter_Predict作為預(yù)測值。但是該預(yù)測數(shù)據(jù)是不準(zhǔn)確的，故引入卡爾曼濾波算法，對預(yù)測值 AverageCenter_Predict與測量值A(chǔ)verageCenter進(jìn)行加權(quán)處理，既不完全相信預(yù)測值，也不完全相信測量值。

對于卡爾曼濾波算法嵌入，首先應(yīng)設(shè)定兩個初值，令占空比分子=710，由于變量只有一個，所以誤差、過程噪音、測量噪音的協(xié)方差矩陣便成為了一個數(shù)值，即為Po=10，Q=5，R=5。

嵌入過程總共分為五步：

1）可以由k=0時刻的最優(yōu)占空比分子去預(yù)測k=1時刻系統(tǒng)的占空比分子先驗估計，則有=+Bμ0。對于本例可以直接獲取，假設(shè)= 7 20。

2）由k=0時刻的誤差協(xié)方差P0和過程噪聲的協(xié)方差Q預(yù)測k=1時刻的誤差協(xié)方差的先驗估計P1-，可得

3）計算卡爾曼增益：

4）進(jìn)行校正更新：

這里可以假設(shè)測量值Mea1=715。求得

則該值便為k=1時刻的最優(yōu)占空比分子。

5）接下來要對誤差的協(xié)方差矩陣進(jìn)行更新，P1= (I-KH)P1-=3.36，這樣依次迭代，便完成了卡爾曼濾波算法的嵌入。

采用卡爾曼濾波算法的思想，對某一時刻占空比分子進(jìn)行最優(yōu)化求解。對于卡爾曼增益K的選擇，即不可以過大，也不可以過小，即不過分聽信預(yù)測值，也不過分聽信測量值。K取得過大或過小，會影響攝像頭智能車的行駛穩(wěn)定性，故K的選擇必須經(jīng)過多次實際測試獲得。首先賦值0.5，觀察攝像頭智能車行駛穩(wěn)定性，然后逐漸增加，最后發(fā)現(xiàn)當(dāng)卡爾曼增益取值為0.54時，攝像頭智能車可以獲得最佳的占空比，實現(xiàn)轉(zhuǎn)向的最優(yōu)控制。

5 數(shù)據(jù)分析

為了驗證卡爾曼濾波算法在攝像頭智能車上的實際應(yīng)用效果，采用無線串口模塊接收攝像頭智能車返回的卡爾曼濾波算法控制下的占空比分子和非卡爾曼濾波算法控制下的占空比分子，以此分析卡爾曼濾波算法是否能對賽道起到預(yù)測作用，圖7是兩種算法返回的占空比分子。

圖7 舵機(jī)的占空比分子

從圖中可以看出，當(dāng)使用卡爾曼濾波算法時，舵機(jī)的占空比分子的波動幅度要大于非卡爾曼濾波算法，也就是說舵機(jī)轉(zhuǎn)向幅度會變大。

如圖8所示，圖中ServoPWM代表非卡爾曼濾波算法控制下舵機(jī)的占空比分子；ServoPWM_Finall代表卡爾曼濾波算法控制下舵機(jī)的占空比分子。在賽道的同一位置處，非卡爾曼濾波算法所計算出的占空比分子為710，卡爾曼濾波算法計算出的占空比分子為706。

圖8 攝像頭智能車的屏幕顯示

圖9為攝像頭智能車在賽道上的位置。為了保證變量單一的原則，將攝像頭智能車放在賽道上，向電機(jī)輸入的占空比設(shè)置為0。此時攝像頭智能車靜止，通過卡爾曼濾波算法和非卡爾曼濾波算法同時計算出此時舵機(jī)的占空比，觀察卡爾曼濾波算法控制下，舵機(jī)是否可以獲得比非卡爾曼濾波算法控制下更優(yōu)的占空比，從而實現(xiàn)提前轉(zhuǎn)向的功能。

圖9 攝像頭智能車在賽道上的位置

結(jié)合圖8與圖9可以看出，當(dāng)攝像頭智能車前方不遠(yuǎn)處出現(xiàn)彎道時，非卡爾曼濾波算法控制下，攝像頭智能車不能發(fā)現(xiàn)前方出現(xiàn)彎道，仍認(rèn)為前方是直道，所以占空比分子的值為710，舵機(jī)保持中值。但是使用卡爾曼濾波算法之后，在同一位置處攝像頭智能車已經(jīng)能夠識別出前方出現(xiàn)彎道，因此，會令占空比分子的值為706，提前轉(zhuǎn)向，從而提高攝像頭智能車的過彎穩(wěn)定性。

6 結(jié)論

本文以全國大學(xué)生智能汽車競賽為背景，通過車模的搭建、硬件電路設(shè)計，結(jié)合圖像二值化處理、賽道左右邊線識別，使得攝像頭智能車能夠按照賽道軌跡行駛，并加入 PID控制算法，保證攝像頭智能車的行駛穩(wěn)定性。經(jīng)過多次調(diào)試，攝像頭智能車已經(jīng)可以實現(xiàn)循跡功能，速度可達(dá)1.4 m/s且過彎穩(wěn)定。在此基礎(chǔ)上，嵌入卡爾曼濾波算法，選擇合適的卡爾曼增益系數(shù)，將預(yù)測值與測量值進(jìn)行加權(quán)處理，使得攝像頭智能車的舵機(jī)獲得最優(yōu)的占空比。將嵌入卡爾曼濾波算法的程序與未嵌入卡爾曼濾波算法的程序分別寫入單片機(jī)，觀察攝像頭智能車在彎道處的行駛情況。試驗結(jié)果表明，嵌入卡爾曼濾波算法之后，攝像頭智能車可以更早地發(fā)現(xiàn)前方出現(xiàn)彎道，從而提前轉(zhuǎn)向，避免攝像頭智能車在彎道處因轉(zhuǎn)向不及時而沖出賽道。因此，卡爾曼濾波算法對賽道識別可以起到優(yōu)化作用，提高攝像頭智能車的行駛穩(wěn)定性。