基于Kalman濾波的車位側方距離修正方法

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

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

0 引言

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

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

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

1 線性離散Kalman濾波原理

Kalman濾波由系統狀態方程和系統觀測方程兩組基本方程組成。

系統狀態方程是被估計狀態Xk與受噪聲驅動Wk-1的關系式：

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

(1)

對Xk的觀測滿足線性關系，觀測方程為：

Zk=HkXk+Vk

(2)

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

(3)

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

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

預測過程：

1)根據上一狀態預測的當前系統狀態：

(4)

2)對誤差協方差的預測：

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

(5)

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

修正階段：

1)計算 Kalman增益：

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

(6)

Kalman增益的校正狀態值得誤差協方差的重要參數，用來權衡系統預測的傳感器量測。其中Hk是觀測矩陣，Rk是觀測噪聲序列的方差陣。

2)對系統狀態預測值的修正：

(7)

3)對誤差協方差的修正：

Pk= (I-KkHk)Pk-

(8)

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

2 Kalman濾波的距離修正原理

2.1 測距系統建模

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

Xk=Xk-1+Wk-1

(9)

小車側方傳感器與障礙物的實時測量部分數據如表1所示。

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

(10)

由此得超聲波第k次測的的數據不一定是準確的，因含有測量噪聲Vk，所以得到系統的觀測方程為：

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

(11)

綜上所述，該系統的狀態方程和觀測方程為：

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]。

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

2.2 Kalman濾波的距離修正

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

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

圖1 Kalman濾波的數據修正過程

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

1)根據k-1時刻的值，對狀態預測

2)計算協方差預測，

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

3)計算Kalman增益，

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

4)狀態更新，由

得：

5)協方差更新，

Pk= (I-KkHk)Pk-=

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

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

% 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

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

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

3 距離修正仿真結果與分析

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

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

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

表2 kalman濾波算法樣本值實驗結果

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

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

(13)

同樣地，將其用于Matlab的“Figure”中自帶的數據標尺工具在圖3中將第1次和第159次的偏差標出，結果發現其測量偏差值由1逐漸減小到0.4779，且其偏差逐漸趨于穩定值。

圖3 誤差分析圖

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

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

表3 不同真實距離下的測量數據濾波修正結果

由表3數據發現，經過150次迭代計算后的測量值，其絕對誤差不會隨著距離增加而增加，且絕對誤差可以保證在2 cm以內，實驗的平均絕對誤差僅1.575 cm，準確度高。

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

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

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

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

4 結語

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