Cyber15隊(duì)智能汽車競(jìng)賽技術(shù)報(bào)告(節(jié)選)

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

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

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

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

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

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

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

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

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

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

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

　　所以dt和kg是一對(duì)矛盾，不能太過(guò)于極端。

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

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