DIY高大上！用STM32單片機(jī)搞定四旋翼無人機(jī)飛控

嘗試制作這個(gè)四旋翼飛控的過程，感觸頗多，整理了思緒之后，把重要的點(diǎn)一一記下來；

這個(gè)飛控是基于STM32，整合了MPU6050，即陀螺儀和重力加速計(jì)，但沒有融合電子羅盤；

另外，四旋翼飛行器的運(yùn)動方式請百度百科，不太復(fù)雜，具體不再贅述；

這是飛控程序的控制流程（一個(gè)執(zhí)行周期）：

比較重要的地方：

1.i2c通信方式；

因?yàn)槲也皇菍W(xué)電類專業(yè)，最開始對i2c這些是沒有一點(diǎn)概念，最后通過Google了解了一些原理，然后發(fā)現(xiàn)STM32的開發(fā)庫是帶有i2c通信的相關(guān)函數(shù)的，但是我最后還是沒有用這些函數(shù)。

我通過GPIO模擬i2c,這樣也能獲得mpu6050的數(shù)據(jù)，雖然代碼多了一些，但是比較好的理解i2c的原理。

STM32庫實(shí)現(xiàn)的模擬i2c代碼（注釋好像因?yàn)榫幋a問題跪了）：

2.mpu6050;

然后用寫好的模擬i2c函數(shù)讀取mpu6050，根據(jù)mpu6050手冊的各寄存器地址，讀取到了重力加速計(jì)和陀螺儀的各分量;

傳感器采樣率設(shè)置為200Hz,這個(gè)值是因?yàn)槲译娬{(diào)頻率為200Hz，也就是說，我的程序循環(huán)一次0.005s,一般來說，采樣率高點(diǎn)沒問題，別比執(zhí)行一次閉環(huán)控制的周期長就行了；

陀螺儀量程±2000°/s，加速計(jì)量程±2g， 量程越大，取值越不精確；

這里注意，由于我們沒有采用磁力計(jì)，而陀螺儀存在零偏，所以最終在yaw方向上沒有絕對的參考系，不能建立絕對的地理坐標(biāo)系，這樣最好的結(jié)果也僅僅是在yaw上存在緩慢漂移。

3.互補(bǔ)濾波；

融合時(shí)，陀螺儀的積分運(yùn)算很大程度上決定了飛行器的瞬時(shí)運(yùn)動情況，而重力加速計(jì)通過長時(shí)間的累積不斷矯正陀螺儀產(chǎn)生的誤差，最終得到準(zhǔn)確的機(jī)體姿態(tài)。

這里我們采用Madgwick提供的UpdateIMU算法來得到姿態(tài)角所對應(yīng)的四元數(shù)，之后只需要經(jīng)過簡單運(yùn)算便可轉(zhuǎn)換為實(shí)時(shí)歐拉角。感謝Madgwick大大為開源做出的貢獻(xiàn)。

4.獲取期望姿態(tài)；

也就是遙控部分了，讓用戶介入控制。

本著拿來主義的原則，用上”圓點(diǎn)博士開源項(xiàng)目”提供的安卓的開源藍(lán)牙控制端。

圓點(diǎn)博士給出了數(shù)據(jù)包格式，同過HC-06藍(lán)牙模塊接連到STM32串口1，再無線連接到控制端，這樣我們就可以獲得控制端不斷發(fā)送的數(shù)據(jù)包了，并實(shí)時(shí)更新期望姿態(tài)角，這里只需要注意輸出的姿態(tài)角和實(shí)時(shí)姿態(tài)角方向一致以及數(shù)據(jù)包的校驗(yàn)就行了。

5.PID控制算法；

由于簡單的線性控制不可能滿足四軸飛行器這個(gè)靈敏的系統(tǒng)，引入PID控制器來更好的糾正系統(tǒng)。

簡介：PID實(shí)指“比例proportional”、“積分integral”、“微分derivative”，這三項(xiàng)構(gòu)成PID基本要素。每一項(xiàng)完成不同任務(wù)，對系統(tǒng)功能產(chǎn)生不同的影響。

以Pitch為例：

error為期望角減去實(shí)時(shí)角度得到的誤差；

iState為積分i參數(shù)對應(yīng)累積過去時(shí)間里的誤差總和；

if語句限定iState范圍，繁殖修正過度；

微分d參數(shù)為當(dāng)前姿態(tài)減去上次姿態(tài)，估算當(dāng)前速度（瞬間速度）；

總調(diào)整量為p,i,d三者之和；

這樣，P代表控制系統(tǒng)的響應(yīng)速度，越大，響應(yīng)越快。

I，用來累積過去時(shí)間內(nèi)的誤差，修正P無法達(dá)到的期望姿態(tài)值（靜差）；

D，加強(qiáng)對機(jī)體變化的快速響應(yīng)，對P有抑制作用。

PID各參數(shù)的整定需要綜合考慮控制系統(tǒng)的各個(gè)方面，才能達(dá)到最佳效果。

輸出PWM信號：

PID計(jì)算完成之后，便可以通過STM32自帶的定時(shí)資源很容易的調(diào)制出四路pwm信號，采用的電調(diào)pwm格式為50Hz，高電平持續(xù)時(shí)間0.5ms-2.5ms；

我以1.0ms-2.0ms為每個(gè)電機(jī)的油門行程，這樣，1ms的寬度均勻的對應(yīng)電調(diào)的從最低到最高轉(zhuǎn)速。

至此，一個(gè)用stm32和mpu6050搭建的飛控系統(tǒng)就算實(shí)現(xiàn)了。