(華北科技學院電子信息工程學院 河北 廊坊 065201)

引言

1960年,卡爾曼發(fā)表了用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文(A New Approach to Linear Filtering and Prediction Problems)[1]?？柭鼮V波應用廣泛且功能強大,它可以估計信號的過去和當前狀態(tài),甚至能估計將來的狀態(tài),即使并不知道模型的確切性質(zhì)[1]。

卡爾曼濾波器又稱為最佳線性濾波器[2]，它的好處有很多，比如：簡單易實現(xiàn)、又是一個純時域的濾波器，不需要進行頻域變換?？柭鼮V波(Kalman filtering)一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法[3]。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。

一、卡爾曼濾波的原理

(一)預測

假設移動機器人正在運動，用它的位置和速度來表示它當前的狀態(tài)，t時刻的位置為pt，速度為vt，加速度為ut，那么在t時刻移動機器人的狀態(tài)向量可以用一個二維的列向量xt=[pt,vt]T表示。假設移動機器人的加速度量由外部控制量(油門加速或控制命令)得到[4]，如果已知上一時刻的狀態(tài)xt-1可推出其在t時刻的速度與位置如下:

移動機器人的速度：vt=vt-1+ut×Δt

其中Δt是兩個時刻的時間間隔，觀察上公式可知輸出變量都只是輸入變量的線性組合，這就是卡爾曼濾波器是最佳的線性濾波器原因，因為它只能描述狀態(tài)與狀態(tài)之間的線性關(guān)系，既然是線性關(guān)系，則可改寫成矩陣的形式如下：

vt=0×pt-1+1×vt-1+ut×Δt

把兩個狀態(tài)變換矩陣提取出來，公式可以簡化成如下：

由狀態(tài)轉(zhuǎn)移矩陣Ft和控制矩陣Bt表示的移動機器人的狀態(tài)轉(zhuǎn)移公式為：

(二)更新

卡爾曼系數(shù)Kt作用主要有兩方面，一是權(quán)衡預測狀態(tài)協(xié)方差P和觀測的協(xié)方差矩陣R的大小來決定預測模型更可靠，還是相信觀察模型更可靠，若預測模型更可靠，則殘殺的權(quán)重就會小一點，若觀察模型更可靠，則殘差的權(quán)重就會大一點。第二個作用是把殘差的表現(xiàn)形式從觀察域轉(zhuǎn)換到狀態(tài)域[8]。

最后一步是更新最佳估計值的噪聲分布，即噪聲協(xié)方差矩陣的更新，用公式表示為：

二、卡爾曼濾波器的系統(tǒng)模型

綜上所述，我們已經(jīng)得到了卡爾曼濾波器中的五個公式，在這五個公式中前兩個公式是通過上一時刻的狀態(tài)來預測當前時刻的狀態(tài)，這表示這并不是最佳的估計值。再把當前時刻的觀測值考慮進去即為更新，經(jīng)過更新后的值就是最佳的觀測值。

預測：

更新：

三、結(jié)束語

由于卡爾曼濾波在機器人SLAM中主要用于位置估計，所謂的估計值是估計位置坐標，輸入傳感器數(shù)據(jù)包括碼盤估計的位置，陀螺儀的角速度等。采用卡爾曼濾波的方法來實現(xiàn)移動機器人位置的估計，利用卡爾曼濾波的良好的跟蹤性能，它可以根據(jù)當前的觀測量和之前的觀測數(shù)據(jù)，根據(jù)線下那個最小方差原理，求出最優(yōu)估計。采用遞歸方法解決線性濾波問題，只需要當前的測量值和前一個采樣周期的估計值就能夠進行狀態(tài)估計，不需要大量的存儲空間，每一步的計算量小，計算步驟清晰，非常適合計算機處理。最終輸出的最佳估計值用作機器人的當前位置，其由除導航算法之外的其他程序調(diào)用。