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

引言

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

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

一、卡爾曼濾波的原理

(一)預(yù)測(cè)

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

移動(dòng)機(jī)器人的速度：vt=vt-1+ut×Δt

其中Δt是兩個(gè)時(shí)刻的時(shí)間間隔，觀察上公式可知輸出變量都只是輸入變量的線性組合，這就是卡爾曼濾波器是最佳的線性濾波器原因，因?yàn)樗荒苊枋鰻顟B(tài)與狀態(tài)之間的線性關(guān)系，既然是線性關(guān)系，則可改寫(xiě)成矩陣的形式如下：

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

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

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

(二)更新

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

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

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

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

預(yù)測(cè)：

更新：

三、結(jié)束語(yǔ)

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