楊述斌,2，蔣宗霖，劉 寒

(1.武漢工程大學(xué) 電氣信息學(xué)院， 武漢 430205; 2.智能機(jī)器人湖北省重點實驗室，武漢 430205)

0 引言

傳統(tǒng)國外汽車廠商所采用的車位識別方案都基于超聲波傳感器或者毫米波雷達(dá)獲取車位信息識別，如豐田、奧迪、奔馳、寶馬等公司，利用安裝于車身保險杠兩端的超聲波傳感器進(jìn)行車位的檢測，但由于存在傳感器本身的固有特性以及在測量過程中因波束角的跳變等問題帶來的隨機(jī)干擾，導(dǎo)致傳感器不能直接得到所需要的測量真實值。

針對超聲波傳感器測量數(shù)據(jù)準(zhǔn)確度不高、實時性較差的特性問題，學(xué)者們提出了不同的解決辦法。如陳喬松等提出采用雙探頭平均值法和雙探頭融合數(shù)據(jù)法來降低測量誤差，以此來提高超聲波傳感器的探測精度，雖計算簡單，但因瞬時測量值絕對誤差大，導(dǎo)致實時性差[1]。又如畢清磊通過大量實驗數(shù)據(jù)得到障礙物距離與誤差值之間的關(guān)系式，利用傳感器誤差補(bǔ)償公式來進(jìn)行修正誤差，但公式運算復(fù)雜，大幅度降低單片機(jī)的運算速度，且傳感器測量誤差會隨著測量距離的增大而增大，導(dǎo)致該方法準(zhǔn)確性低[2]。因此，如何提高超聲波測距的準(zhǔn)確性和實時性，并有效降低由傳感器帶來的隨機(jī)噪聲干擾是自動泊車系統(tǒng)中車位識別技術(shù)的一個新研究趨勢。

Kalman濾波就是這種能夠有效降低隨機(jī)噪聲影響的工具。近年來，隨著科學(xué)技術(shù)的日益發(fā)展，Kalman濾波與相關(guān)改進(jìn)算法都能夠在計算機(jī)的輔助下得以快速實現(xiàn)，無論是在機(jī)器人的軌跡規(guī)劃還是決策領(lǐng)域或者是在航天航空領(lǐng)域等重大軍工業(yè)領(lǐng)域都有廣泛地應(yīng)用[3]。總之，Kalman濾波在信號處理與控制技術(shù)方面得到了均可有效地運用，所以文章將以Kalman濾波算法為重點研究對象，闡述其在自動泊車系統(tǒng)中車位識別技術(shù)的應(yīng)用與實現(xiàn)。

1 線性離散Kalman濾波原理

Kalman濾波由系統(tǒng)狀態(tài)方程和系統(tǒng)觀測方程兩組基本方程組成。

系統(tǒng)狀態(tài)方程是被估計狀態(tài)Xk與受噪聲驅(qū)動Wk-1的關(guān)系式：

Xk=Ak,k-1Xk+BUk-1+Wk-1

(1)

對Xk的觀測滿足線性關(guān)系，觀測方程為：

Zk=HkXk+Vk

(2)

Xk是k時刻的系統(tǒng)狀態(tài)，Uk是k時刻對系統(tǒng)的控制量。Ak,k-1是從k-1時刻到k時刻的狀態(tài)轉(zhuǎn)矩陣。Hk是觀測矩陣。Wk是系統(tǒng)的激勵噪聲，Vk是觀測噪聲。在Kalman濾波中，要求Wk和Vk是正態(tài)分布的白色噪聲。因此有：

(3)

其中，Qk和Rk分別是系統(tǒng)噪聲Wk和觀測噪聲Vk的方差矩陣。

Kalman濾波信息更新由時間更新和觀測更新這兩個過程組成，其中時間更新過程也稱為預(yù)測過程，觀測更新過程也稱為修正過程。

預(yù)測過程：

1)根據(jù)上一狀態(tài)預(yù)測的當(dāng)前系統(tǒng)狀態(tài)：

(4)

2)對誤差協(xié)方差的預(yù)測：

Pk-=Ak,k-1Pk-1Ak,k-1T+Qk

(5)

上述兩個方程是系統(tǒng)的預(yù)測過程，式(4)根據(jù)k-1時刻的狀態(tài)最優(yōu)估計預(yù)測k時刻的狀態(tài)估計，式(5)描述了這種預(yù)測的均方差，對預(yù)測的好壞程度做定量描述，其值也是由上一時刻的誤差協(xié)方差來做預(yù)測。從時間角度來分析，即這兩個式子將時間從k-1時刻推進(jìn)到k時刻，于是這個過程稱為Kalman濾波的預(yù)測過程。

修正階段：

1)計算 Kalman增益：

Kk=Pk-HkT(HkPk-HkT+Rk)-1

(6)

Kalman增益的校正狀態(tài)值得誤差協(xié)方差的重要參數(shù)，用來權(quán)衡系統(tǒng)預(yù)測的傳感器量測。其中Hk是觀測矩陣，Rk是觀測噪聲序列的方差陣。

2)對系統(tǒng)狀態(tài)預(yù)測值的修正：

(7)

3)對誤差協(xié)方差的修正：

Pk= (I-KkHk)Pk-

(8)

式(6)和式(8)利用狀態(tài)預(yù)測的質(zhì)量優(yōu)劣(Pk-)、觀測信息的質(zhì)量優(yōu)劣(Rk)、觀測與狀態(tài)的關(guān)系(Hk)以及觀測信息Zk修正時間上的預(yù)測，構(gòu)造改進(jìn)后的最優(yōu)估計，描述了Kalman濾波的修正過程。得到的最優(yōu)估計又會作為先驗數(shù)據(jù)供下次預(yù)測使用，由此行成遞歸推算。其中，Kalman濾波也就是由式(4)～式(8)這個五項基本方程組成[4-7]。

2 Kalman濾波的距離修正原理

2.1 測距系統(tǒng)建模

超聲波測距模塊采樣時間為25 ms，在3.75 s內(nèi)采樣到150組距離數(shù)據(jù)信息，對這150組數(shù)據(jù)濾波及修正，便可以穩(wěn)定輸出一個比較接近真實值的數(shù)值。經(jīng)過多次測量，并取平均值得到小車離側(cè)方障礙物大約為5 cm，這個距離可能會受到傳感器固有特性等問題帶來一些外部因素的干擾，于是將這個擾動則為過程噪聲Wk，其方差矩陣為Q，大小假定為Q=0.000 1(如果不考慮過程噪聲的影響，此時Q=0)。因為考慮系統(tǒng)中的Xk是在第k時刻采樣時的距離信息是一維的，而且無控制量。由此對照式子(1)，可以得出該系統(tǒng)的狀態(tài)方程是:

Xk=Xk-1+Wk-1

(9)

小車側(cè)方傳感器與障礙物的實時測量部分?jǐn)?shù)據(jù)如表1所示。

取前20組數(shù)據(jù)作為實驗輸入，經(jīng)過如下式(10)計算得測量誤差方差為：R=0.7959。

(10)

由此得超聲波第k次測的的數(shù)據(jù)不一定是準(zhǔn)確的，因含有測量噪聲Vk，所以得到系統(tǒng)的觀測方程為：

Z(k)=X(k)+V(k)

(11)

綜上所述，該系統(tǒng)的狀態(tài)方程和觀測方程為：

Xk=Ak,k-1Xk-1+Wk-1

Zk=HkXk+Vk

(12)

表1 小車與障礙物距離信息表

式中，Xk是距離信息為一維變量，則Ak,k-1=1，Hk=1，Wk-1和Vk的方差矩陣分別為Q和R[8-10]。

建好系統(tǒng)之后,即可用Kalman濾波來處理過程噪聲Wk和測量噪聲Vk。根據(jù)Kalman濾波的實質(zhì)，可以知道如果要估算k時刻的實際值，就要根據(jù)k-1時刻的值來對其進(jìn)行估算。

2.2 Kalman濾波的距離修正

1)假定前20組值測得第一組距離值為k-1時刻的值Lk-1，距離的真實值為S，那么該測量值的偏差是S-Lk-1，即該時刻的協(xié)方差Pk-1=(S-Lk-1)2=M。

2)在k時刻，超聲波測距傳感器的測量值由于傳感器的固有特性，測得第二組值Lk，偏差為S-Lk?，F(xiàn)在用于估算第kk時刻的測量值由兩個測量值，分別是k-1時刻的Lk和k時刻的S-Lk，將以上兩組測量值進(jìn)行融合使其逼近真實值，Kalman濾波也非常適合循環(huán)迭代運算，因此也適合采用計算機(jī)程序?qū)崿F(xiàn)，實現(xiàn)流程如圖1所示。

圖1 Kalman濾波的數(shù)據(jù)修正過程

因為Ak,k-1及Hk均為1，所以具體操作步驟如下。根據(jù)Kalman濾波的五項基本方程(4～8)，這里分5步來分析Kalman濾波的數(shù)據(jù)修正流程。

1)根據(jù)k-1時刻的值，對狀態(tài)預(yù)測

2)計算協(xié)方差預(yù)測，

Pk-=Pk-1+Q(S-Lk-1)2+Q=M+QPk-

3)計算Kalman增益，

Kk=Pk-/(Pk-+R) = (M+Q)/[(M+Q) +R]

4)狀態(tài)更新，由

得：

5)協(xié)方差更新，

Pk= (I-KkHk)Pk-=

{I-[(M+Q)/(M+Q+R)]*Hk}*(M+Q)

其中，Matlab開發(fā)平臺下的部分核心代碼如下：

% State initialization

S=zeros(1,T);S(1,1)=5.00;

% observation initialization

L=zeros(1,T);L(1,1)=3.97;

%State initialization by Kalman filtering

Skf=zeros(1,T);Skf(1,1)=L(1,1);

% covariance initialization

P=zeros(1,T);P(1,1)=1.0609;

% The measured target is one-dimensional information

for k=2:T

% X is the true distance value, which consists of the real value and the disturbance caused by the disturbance.

% Equation of state S(1,k)=F*S(1,k-1)+G*W(1,k);

% The range finder can only be measured by the sensor, the measurement information is Z, and the filter starts according to the measurement information.

% observation equation

L(1,k)=H*X(1,k)+V(1,k);

% Step 1: Status Forecast

Spre=H*Skf(1,k-1);

% Step 2: Covariance prediction

Ppre=F*P(k-1)*F'+Q;

% Step 3: Calculate the Kalman gain

K=Ppre*inv(H*Ppre*H'+R);

% calculation information

e=L(k)-H*Xpre;

% Step 4: Status Update

Skf(1,k)=Spre+K*e;

% Step 5: Covariance update

P(1,k)=(I-K*H)*Ppre;

end

其中，狀態(tài)轉(zhuǎn)移矩陣F=1；噪聲驅(qū)動矩陣G=1；觀測矩陣H=1；過程噪聲方差Q=0.0001；觀測噪聲方差R=0.7959；仿真總步數(shù)T=150；過程噪聲為W=sqrt(Q)*randn(1,T)；觀測噪聲為V=sqrt(R)*randn(1,T)；單位矩陣I=eye(1)；

從Kalman濾波的本質(zhì)來看，Step 1和Step 2將時間從k-1時刻推進(jìn)至k時刻，描述了Kalman濾波的預(yù)測過程，具體的距離修正量由時間更新的質(zhì)量優(yōu)劣(Pk-)、觀測信息的質(zhì)量優(yōu)劣(R)、觀測與狀態(tài)的關(guān)系(H)及具體的觀測信息L(k)得以確定，這些步驟都是以如何正確合理的利用觀測L(k)為目的的，同時這5步也恰恰描述了Kalman濾波的修正過程。

3 距離修正仿真結(jié)果與分析

應(yīng)用以上的系統(tǒng)的狀態(tài)方程和測量方程，在Inter(R) Core(TM) CPU主頻為2.3 GHz，安裝內(nèi)存4 GB的計算機(jī)上運行Matlab R2018a進(jìn)行仿真實驗，分析小車與側(cè)方障礙物距離信息的真實值、測量值和濾波修正值的差異，同時也計算了測量偏差與濾波偏差，部分核心代碼已在上文給出。

圖2中，實線代表的是真實值，圓圈代表的是在隨機(jī)噪聲干擾下的超聲波傳感器測量值，而星號線則代表的是濾波修正值。由圖2可見，超聲波傳感器的測量值準(zhǔn)確度低，絕對誤差的波動非常大，但濾波修正值逐漸地逼近真實值，且趨于穩(wěn)定。標(biāo)記出6組樣本值，記錄如表2所示。

圖2 真實值、測量值與濾波值的差異圖

表2 kalman濾波算法樣本值實驗結(jié)果

圖3中，將各個采樣時刻、濾波結(jié)果和真實值去做差，計算其絕對值，這個值就是偏差值，定義為：

Xdev(k)=|Xn(k)-X(k)|

(13)

同樣地，將其用于Matlab的“Figure”中自帶的數(shù)據(jù)標(biāo)尺工具在圖3中將第1次和第159次的偏差標(biāo)出，結(jié)果發(fā)現(xiàn)其測量偏差值由1逐漸減小到0.4779，且其偏差逐漸趨于穩(wěn)定值。

圖3 誤差分析圖

表2中的6組樣本值分別代表第25、 50、75、100、125、150次的濾波結(jié)果，由表2可知經(jīng)過150次迭代計算后的測量值已經(jīng)達(dá)4.68 cm，相對誤差由0.98 cm減少到0.32 cm，可見反復(fù)迭代計算值的絕對誤差逐漸降低，并將繼續(xù)減小，使其測量值逼近真實值，且趨于穩(wěn)定。

為了驗證Kalman濾波在自動泊車系統(tǒng)中車位識別技術(shù)的普適性，在真實距離S為15 cm，25 cm，35 cm，45 cm，55 cm，65 cm的不同情況下，利用上述原理進(jìn)行測量數(shù)據(jù)的濾波修正，并記錄其結(jié)果，如表3所示。

表3 不同真實距離下的測量數(shù)據(jù)濾波修正結(jié)果

由表3數(shù)據(jù)發(fā)現(xiàn)，經(jīng)過150次迭代計算后的測量值，其絕對誤差不會隨著距離增加而增加，且絕對誤差可以保證在2 cm以內(nèi)，實驗的平均絕對誤差僅1.575 cm，準(zhǔn)確度高。

同上，運行在真實距離S為15 cm，25 cm，35 cm，45 cm，55 cm，65 cm的不同情況下，并用MATLAB的“Etime”函數(shù)獲取各數(shù)據(jù)修正所需的時間，并記錄如表4所示。

表4 不同真實距離下的測量數(shù)據(jù)濾波修正所需時間

由表4可計算得到這6次實驗的數(shù)據(jù)修正所需平均時間為0.028 s，而且Kalman濾波計算過程為一個不斷“預(yù)測—修正”的過程，在數(shù)據(jù)修正時不需要存儲大量的數(shù)據(jù)，且一旦觀測到新的數(shù)據(jù)，隨時可以計算得濾波修正值，有非常好的實時處理性。

綜上所述，該方法使用的kalman濾波算法可有效降低了隨機(jī)噪聲干擾，使車位識別技術(shù)中側(cè)方障礙物距離數(shù)據(jù)得以修正，準(zhǔn)確性高且實時性好，同時也完全符合安全泊車距離±5 cm的標(biāo)準(zhǔn)，進(jìn)而使車輛泊車可達(dá)到理想效果。

4 結(jié)語

本文通過合理地設(shè)計狀態(tài)方程及觀測方程，采用Kalman濾波的車位側(cè)方距離修正方法得到的距離平均修正誤差小，運行時間更短，表明該算法具有更好的準(zhǔn)確性和實時性。逐步迭代的計算使濾波修正值繼續(xù)逼近真實值，可以有效應(yīng)用于實際的車位識別系統(tǒng)中。