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

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

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

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

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

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

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

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

　　卡爾曼濾波參數整定

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

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

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

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

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

　　將車(chē)模保持在穩定直立狀態(tài)，讓車(chē)輪以恒定PWM(80%以上)轉動(dòng)，然后調節參數。