四旋翼飛行器控制系統(tǒng)設(shè)計(jì)

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

2.2.1 MPU6050初始化

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

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

2.2.2 濾波算法

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

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

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

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

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

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

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

3.2 控制流程

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

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

4 總結(jié)

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

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

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

參考文獻(xiàn)：

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

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

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

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

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

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

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