基于卡爾曼濾波的賽道檢測優化算法

劉新寧

（長安大學 汽車學院，陜西 西安 710064）

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

卡爾曼濾波算法在智能輔助駕駛方面有著廣闊的應用前景。卡爾曼濾波算法的關鍵是根據某一時刻“實際測量值”和上一時刻的“預測估計值”以及估計誤差和測量誤差，通過加權處理計算得到該時刻的最優結果，再對下一時刻的數據進行預測。隨著數據的不斷更新，誤差也在不斷迭代，從而實現數據的不斷更新[1]。

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

1 智能車的結構及原理

攝像頭智能車的結構主要包括車模、英飛凌TC264單片機、主板、驅動板、攝像頭、顯示屏、電池、編碼器等[2]。

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

2 硬件系統設計

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

圖1 攝像頭智能車硬件整體設計框圖

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

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

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

3 軟件系統設計

3.1 賽道識別

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

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

3.2 圖像二值化處理

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

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

圖2 圖像二值化

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

3.3 賽道邊線獲取

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

圖3 賽道邊線獲取流程圖

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

3.4 彎道上賽道中線的計算

當攝像頭智能車在彎道上行駛時，會出現丟線的現象，如圖4所示。

圖4 丟線現象

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

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

圖5 賽道中線獲取流程圖

3.5 PID控制算法

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

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

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

4 卡爾曼濾波算法

4.1 數據融合

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

對方差求導可得

推出

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

4.2 狀態空間表達

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

圖6 車身振動的單質量系統模型

進一步整理可得

令μ=+εq，即為系統輸入，故有

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

式（9）和式（10）體現了變量隨時間的連續變化，可以將其離散化為

式中，zk為k時刻的系統狀態量；zk-1為k-1時刻的系統狀態量；μk-1為k-1時刻的系統輸入量；Meak為k時刻的系統測量量。

然而在實際生活中，存在著很多不確定性，這就導致所建立的狀態空間方程會隨著這些不確定性而發生變化。

那么狀態空間方程變為

式中，wk為過程噪音，體現了模型的不準確性；vk為測量噪音，體現了測量的不準確性。

4.3 推導卡爾曼增益

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

由

推出

式中，為測量結果。

根據卡爾曼濾波的思想

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

那么

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

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

4.4 卡爾曼濾波算法應用

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

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

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

嵌入過程總共分為五步：

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

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

3）計算卡爾曼增益：

4）進行校正更新：

這里可以假設測量值Mea1=715。求得

則該值便為k=1時刻的最優占空比分子。

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

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

5 數據分析

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

圖7 舵機的占空比分子

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

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

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

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

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

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

6 結論

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