2.2 MPU6050數(shù)據(jù)讀寫

2.2.1 MPU6050初始化

　　首先，我們要解除I2C休眠狀態(tài)，(電源管理，典型值：0x00(正常啟用))，然后確定陀螺儀采樣率，典型值：0x07(125Hz)，陀螺儀自檢及測量范圍，典型值：0x18(不自檢，2000deg/s)，以及加速計自檢、測量范圍及高通濾波頻率，典型值：0x09(不自檢，4G)。如圖4所示。

　　除了對MPU6050進(jìn)行工作模式的設(shè)定以外，還需設(shè)定芯片的零漂誤差，在每次采集數(shù)據(jù)后，都應(yīng)作差。這是因為MPU6050自身會受電磁場、溫度等的影響，會產(chǎn)生偏差。該值通過事先固定好MPU6050并進(jìn)行多次采樣，通過加權(quán)濾波算法求取平均值得到。

2.2.2 濾波算法

　　由于原始的歐拉角沒有經(jīng)過濾波，陀螺儀的值會隨著時間產(chǎn)生累積誤差，即經(jīng)過一定的時間之后會出現(xiàn)姿態(tài)錯誤的現(xiàn)象。因此，我們采用卡爾曼濾波算法對得到的原始的歐拉角進(jìn)行濾波和數(shù)據(jù)融合，來消除噪聲和累積誤差，最終得到真實歐拉角。如圖5所示。

　　在該系統(tǒng)中，我們從三軸陀螺儀得到的姿態(tài)數(shù)據(jù)有兩組：X、Y軸的加速度轉(zhuǎn)換的角度值和陀螺儀采集的X、Y軸的角速度，我們將加速度轉(zhuǎn)化的角度值設(shè)為temp，角速度轉(zhuǎn)化的角度值設(shè)為angle，angle的值等于上一時刻的真實角度值(即濾波得到的值)加上這一時刻角速度的值乘以時間積分dt，假設(shè)最初的協(xié)方差值為P[2][2] = [1, 0;0, 1]，加速度過程協(xié)方差為Q_angle=0.001，加速度過程協(xié)方差為Q_gyro=0.003, 陀螺儀誤差初值q_bias設(shè)為0.05，測量協(xié)方差R_angle=0.5, 積分時間dt=0.0025;我們通過上面的五個公式不斷地更新協(xié)方差P和卡爾曼增益Kg，獲得最終的真實角度值(即卡爾曼濾波角度)。MPU6050與卡爾曼濾波的結(jié)合流程圖如圖6所示。

3 飛行器穩(wěn)定控制的實現(xiàn)

　　所有傳感器以及驅(qū)動部分均已配置好后開始配置主控程序的流程。本實驗通過配置一個通用定時器，每經(jīng)過一定時間后，進(jìn)入一次中斷服務(wù)函數(shù)，我們稱之為控制中斷。在中斷服務(wù)函數(shù)里，提取傳感器數(shù)據(jù)，進(jìn)行PID運(yùn)算以及調(diào)整PWM波。此外還要讀取通過無線接收到的數(shù)據(jù)，以及通過無線向上位機(jī)發(fā)送信息。

3.1 控制定時器的設(shè)定

　　設(shè)置定時器3為控制定時器，設(shè)置其每2.5ms中斷一次。設(shè)定定時器3的自動重裝載值TIM_Peri為2500，分頻系數(shù)TIM_Prescaler為72-1，采用向上計數(shù)。因為不需要比較輸出，只需要計數(shù)并按照設(shè)定進(jìn)入中斷即可，因此不需要設(shè)置0Cx相關(guān)寄存器。

　　最后配置定時器3的搶占優(yōu)先級為2，子優(yōu)先級為0，這樣防止與通訊串口或超聲波模塊傳輸數(shù)據(jù)用的串口發(fā)生沖突等情況。清除中斷標(biāo)志位后，使能定時器3定時器，則系統(tǒng)就會每經(jīng)過2.5ms進(jìn)入中斷，采集傳感器數(shù)據(jù)，并進(jìn)行PID調(diào)節(jié)。

3.2 控制流程

　　控制流程是指定時器3計時結(jié)束，觸發(fā)中斷后，進(jìn)入到中斷服務(wù)函數(shù)里進(jìn)行的操作。在進(jìn)入中斷后，首先清除定時器3的中斷標(biāo)志位，進(jìn)入超聲波測距子程序，并返回高度值。進(jìn)入高度PID控制器進(jìn)行控制量運(yùn)算，然后輸出PWM波。之后通過I2C協(xié)議開始讀取MPU6050的數(shù)據(jù)，并進(jìn)行數(shù)據(jù)融合，然后經(jīng)過卡爾曼濾波得到比較穩(wěn)定的姿態(tài)角信息，再分別經(jīng)過俯仰PID控制器、橫滾PID控制器以及偏航PID控制器進(jìn)行控制量調(diào)整，得到的PWM控制變量進(jìn)行疊加，最終將疊加后的四個電機(jī)的PWM控制變量輸出到四個電機(jī)。

　　另一方面，為了實現(xiàn)無線數(shù)據(jù)的發(fā)送與接收，在這里，定義一個計數(shù)變量ms，每次進(jìn)入中斷后進(jìn)行自加，根據(jù)該變量的值進(jìn)行判定，是通過無線發(fā)送數(shù)據(jù)，還是通過無線接收數(shù)據(jù)，這樣可以有效地防止無線通訊發(fā)生混亂。在中斷服務(wù)函數(shù)里，控制流程圖如圖7所示。

4 總結(jié)

　　本文實現(xiàn)了對四旋翼飛行器控制的軟件程序編寫。主要研究了PWM波的輸出以及對MPU6050的數(shù)據(jù)采集與處理。重點(diǎn)是控制流程的設(shè)計，即包含多個PID控制器的協(xié)同工作。與上位機(jī)和遙控之間的通訊是通過SPI協(xié)議進(jìn)行讀寫，在本論文中不做深入研究。

　　在實際編寫中，是用STM32庫函數(shù)版本進(jìn)行代碼編寫，大大簡化了程序編寫的難度，并且可讀性也更強(qiáng)。

　　最后進(jìn)行了系統(tǒng)調(diào)試實驗，定點(diǎn)定姿定高飛行控制姿態(tài)角的收斂情況如圖8所示。從實驗結(jié)果可以看出該控制系統(tǒng)在有一定的外部干擾情況下，能夠在40秒左右達(dá)到控制目標(biāo)。

參考文獻(xiàn)：

　　[1]楊明志，王敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計[J],計算機(jī)測量與控制,2008,16(4):485-490.

　　[2]李勁松,宋立博,顏國正.基于自適應(yīng)逆控制方法的小型四旋翼無人直升機(jī)姿態(tài)控制[J].上海交大學(xué)報,2012,46(6):956~961

　　[3]王璐,李光春,王兆龍等.欠驅(qū)動四旋翼無人飛行器的滑模控制[J].哈爾濱工程大學(xué)學(xué)報,2012,33(10):1249~1253

　　[4]甄紅濤,齊曉慧,夏明旗等.四旋翼無人機(jī)魯棒自適應(yīng)姿態(tài)控制[J].控制工程,2013,20(5):916~919.

　　[5]王麗新.基于滑模理論的四旋翼直升機(jī)的姿態(tài)控制研究[D].沈陽:東北大學(xué),2009.

　　[6]汪紹華,基于卡爾曼濾波的四旋翼飛行器姿態(tài)估計和控制算法研究(英文)[J].控制理論與應(yīng)用,2013,30(9):1109~1115

本文來源于中國科技期刊《電子產(chǎn)品世界》2016年第9期第48頁，歡迎您寫論文時引用，并注明出處。