Cyber15隊智能汽車競賽技術(shù)報告(節(jié)選)

　　然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式：
       
　　這里，X01即為我們需要的角度卡爾曼濾波值。

　　可以看到的是，卡爾曼濾波方程形式共有14個公式，同時很多參數(shù)的運(yùn)算涉及浮點數(shù)，這對于單片機(jī)的高效運(yùn)行時極為不利的，因此我們需要通過一些方法對卡爾曼濾波的公式進(jìn)行化簡，從而在不失精度的情況下，盡可能減小運(yùn)算量。

　　需要注意的是，Q,R兩個參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應(yīng)變化，對應(yīng)到我們的系統(tǒng)，在車模運(yùn)行狀態(tài)不同(傾角不同，PWM不同)情況下，Q,R都是不同的。

　　根據(jù)相關(guān)參考文獻(xiàn)，Q,R雖然都是關(guān)于時間的變量，但是由于卡爾曼濾波有很好的收斂性，所以可以將Q，R都取比較極端的參數(shù)。用常量來定義。然后再看圖3，注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個方框中都是為了獲得卡爾曼增益(矩陣Kg)，我們設(shè)想，能否使用一個常數(shù)來等效替代卡爾曼增益那?根據(jù)我們在實驗中的觀察，卡爾曼增益是一個收斂的變量，并且針對到我們的這個系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個常數(shù)。(所有這些工作都在MATLAB下完成)

　　所以，我們將方框中的所有公式完全省略，通過實驗整定，選取一個近似Kg來替代方框中的所有運(yùn)算。同時，通過NATLAB觀測各變量的變化趨勢，我們嘗試讓Kg2=0(Kg1，Kg2本身就是非常小的變量，所以可以讓其等于0)。并發(fā)現(xiàn)Kg2對于整個卡爾曼濾波的影響非常有限。所以將Kg2設(shè)定為0。

　　以下就是我們組程序中卡爾曼濾波的簡化算法實現(xiàn)
         

　　X1=X0+gyro*dt;
　　X0=X1+(acc-X1)*kg;
　　化簡后的卡爾曼濾波框圖如圖4。

　　卡爾曼濾波參數(shù)整定

　　化簡后的卡爾曼濾波主要是對dt以及Kg兩個參數(shù)進(jìn)行整定。需要特別指出的是，這里的dt并不只是一個采樣間隔。由于陀螺儀的輸出和加速度計輸出的量綱并不相同，所以陀螺儀采樣值*dt并不直接反應(yīng)一個角度，而是與實際角度相差一個系數(shù)。因此此處的dt可以等效理解為dt=采樣間隔*比例系數(shù)。

　　dt越大，積分速度越快，卡爾曼輸出追隨實際角度的情況越好(當(dāng)然不能太大，不然可能會出現(xiàn)超前相位)。但是dt越大，陀螺儀漂移造成的影響也就越大。

　　Kg決定了加速度計的權(quán)重。Kg越大，實際輸出的漂移就越小，但是濾波效果的噪聲也就越大。

　　所以dt和kg是一對矛盾，不能太過于極端。

　　首先是靜態(tài)整定。

　　將車模保持在穩(wěn)定直立狀態(tài)，讓車輪以恒定PWM(80%以上)轉(zhuǎn)動，然后調(diào)節(jié)參數(shù)。