基于Kalman濾波的車位側(cè)方距離修正方法

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

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

0 引言

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

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

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

1 線性離散Kalman濾波原理

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

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

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

(1)

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

Zk=HkXk+Vk

(2)

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

(3)

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

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

預(yù)測(cè)過程：

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

(4)

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

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

(5)

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

修正階段：

1)計(jì)算 Kalman增益：

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

(6)

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

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

(7)

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

Pk= (I-KkHk)Pk-

(8)

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

2 Kalman濾波的距離修正原理

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

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

Xk=Xk-1+Wk-1

(9)

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

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

(10)

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

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

(11)

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

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和測(cè)量噪聲Vk。根據(jù)Kalman濾波的實(shí)質(zhì)，可以知道如果要估算k時(shí)刻的實(shí)際值，就要根據(jù)k-1時(shí)刻的值來對(duì)其進(jìn)行估算。

2.2 Kalman濾波的距離修正

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

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

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

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

1)根據(jù)k-1時(shí)刻的值，對(duì)狀態(tài)預(yù)測(cè)

2)計(jì)算協(xié)方差預(yù)測(cè)，

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

3)計(jì)算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ā)平臺(tái)下的部分核心代碼如下：

% 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ū)動(dòng)矩陣G=1；觀測(cè)矩陣H=1；過程噪聲方差Q=0.0001；觀測(cè)噪聲方差R=0.7959；仿真總步數(shù)T=150；過程噪聲為W=sqrt(Q)*randn(1,T)；觀測(cè)噪聲為V=sqrt(R)*randn(1,T)；單位矩陣I=eye(1)；

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

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

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

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

圖2 真實(shí)值、測(cè)量值與濾波值的差異圖

表2 kalman濾波算法樣本值實(shí)驗(yàn)結(jié)果

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

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

(13)

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

圖3 誤差分析圖

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

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

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

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

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

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

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

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

4 結(jié)語

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