2.2 MPU6050數據讀寫

2.2.1 MPU6050初始化

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

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

2.2.2 濾波算法

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

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

3 飛行器穩定控制的實現

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

3.1 控制定時器的設定

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

　　最后配置定時器3的搶占優先級為2，子優先級為0，這樣防止與通訊串口或超聲波模塊傳輸數據用的串口發生沖突等情況。清除中斷標志位后，使能定時器3定時器，則系統就會每經過2.5ms進入中斷，采集傳感器數據，并進行PID調節。

3.2 控制流程

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

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

4 總結

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

　　在實際編寫中，是用STM32庫函數版本進行代碼編寫，大大簡化了程序編寫的難度，并且可讀性也更強。

　　最后進行了系統調試實驗，定點定姿定高飛行控制姿態角的收斂情況如圖8所示。從實驗結果可以看出該控制系統在有一定的外部干擾情況下，能夠在40秒左右達到控制目標。

參考文獻：

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

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

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

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

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

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

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