3.3確定卡爾曼濾波算法的初始值

　　卡爾曼濾波算法作為一個(gè)迭代過程，需要賦予其初值，初值的選擇至關(guān)重要，如果初值選擇不合適，就不能滿足收斂性的要求。在CV模型中，P(0|0)的確定方法已經(jīng)由模型給出，這里只需給出X(0|0)的取值，本文中取X(0|0)=[10，-0.4]'.

　　4仿真實(shí)驗(yàn)與結(jié)果分析

　　在完成上述準(zhǔn)備工作后，筆者在Matlab軟件中進(jìn)行仿真實(shí)驗(yàn)，仿真實(shí)驗(yàn)流程如圖3所示。

　　圖3仿真實(shí)驗(yàn)流程圖

　　根據(jù)仿真實(shí)驗(yàn)流程圖，在Matlab中先對(duì)模擬出的目標(biāo)位置信息進(jìn)行卡爾曼濾波處理，如圖4和圖5所示，這里的目標(biāo)指的是所測(cè)障礙物。首先，從圖4和圖5可以看出：經(jīng)卡爾曼濾波處理后的目標(biāo)位置的估計(jì)值在前2 s偏離真實(shí)值較遠(yuǎn)，從第4 s以后，無(wú)論觀測(cè)值如何波動(dòng)，估計(jì)值曲線均能很好地跟蹤真實(shí)值曲線，說明卡爾曼濾波算法起到了良好的濾波效果。

　　將融合處理前后，目標(biāo)位置的估計(jì)值曲線和目標(biāo)位置估計(jì)值的方差曲線分別置于同一幅圖中，如圖6所示，通過對(duì)比反映STF融合算法的優(yōu)點(diǎn)。從圖6( b)中可以看出：融合處理后，目標(biāo)位置估計(jì)值的方差變小，說明融合處理后對(duì)目標(biāo)位置的估計(jì)更加準(zhǔn)確。從圖6中可以發(fā)現(xiàn)，融合曲線介于雙目視覺傳感器的估計(jì)值曲線和超聲測(cè)距傳感器的估計(jì)值曲線之間，且更加靠近準(zhǔn)確度高的超聲測(cè)距傳感器的估計(jì)值曲線。

　　在本文所引文獻(xiàn)中，實(shí)驗(yàn)驗(yàn)證環(huán)節(jié)均在具體的應(yīng)用場(chǎng)景下進(jìn)行，實(shí)驗(yàn)結(jié)果是移動(dòng)機(jī)器人能夠進(jìn)行無(wú)礙行走，文中均未給出具體的測(cè)量精度。本文仿真實(shí)驗(yàn)的結(jié)果表明：融合處理后，測(cè)量精度可達(dá)4.6 cm，滿足了仿生四足機(jī)器人對(duì)測(cè)距的精度要求。

　　圖4雙目視覺傳感器系統(tǒng)的卡爾曼濾波

　　圖5超聲測(cè)距傳感器系統(tǒng)的卡爾曼濾波

　　圖6融合前后目標(biāo)位置估計(jì)值曲線和方差曲線的對(duì)比