基于雙處理器的四旋翼飛行控制系統研究

吳思穎+李亞楠+王年豐+姚遠

摘要摘要：為提高四旋翼飛行器工作可靠性，采用STM32F407VGT和R5F100LE處理器，設計了四旋翼飛行器控制系統。加速度陀螺儀MPU6050模塊采集飛行姿態數據，超聲測距模塊、激光傳感模塊和攝像頭模塊提供導航參數，采用濾波算法對傳感器所采集的數據進行處理，使用PID控制算法實現尋線前進。實驗測試表明，飛行器能一鍵起飛，完成空投任務，精確降落。

關鍵詞關鍵詞：四旋翼飛行器；姿態控制；雙處理器；濾波算法

DOIDOI：10.11907/rjdk.162226

中圖分類號：TP319文獻標識碼：A文章編號文章編號：16727800（2017）001004403

引言

四旋翼飛行器是一個多傳感器以及眾多子系統（導航、通信、飛行控制等）構成的高集成控制系統。近年來，隨著嵌入式處理器、傳感技術、控制技術的發展，尤其是智能控制算法的應用，其性能得到了極大提高，在測控、巡檢等軍事和民用領域得到了廣泛應用[12]，具有極高的研究價值和應用潛力。

四旋翼飛行器有4個輸入量、6個自由度輸出，調整飛行姿態和實現室內定位是四旋翼飛行器設計的難點之一[3]。現有的飛行控制系統一般采用ARM7、DSP等高速處理器作為控制芯片。這類單芯片飛控系統，在一個控制周期內要完成數據采集、數據處理、控制運算及指令輸出，還需將數據輸出到監控系統，實現室內定位和尋跡飛行，會產生過重負荷，導致系統可靠性下降。為提高系統可靠性，本文采用ST公司的STM32F407VGT負責主控，瑞薩單片機R5F100LE處理器負責數據處理，調制PWM（Pulse Width Modulation）信號輸出占空比驅動無刷直流電機，姿態傳感器（MPU6050集成模塊）檢測飛行器的加速度和角速度，攝像頭尋跡模塊檢測地面導航線，US-100超聲測距模塊獲取飛行器的飛行高度，運用卡爾曼濾波算法對傳感器數據進行分析融合，串級PID算法對飛行器進行控制，實現四旋翼飛行器的雙處理器飛行控制系統設計。

1飛行控制系統

本設計采用分工協作機制，由ST公司的STM32F407VGT負責主控，瑞薩單片機R5F100LE負責數據處理以提高控制可靠性。其中STM32F407VGT采用先進的Coretex-M4內核，最高處理速度可達210DMIPS的處理能力，存儲空間大，具有浮點運算能力和增強處理指令的優點，帶有多種外設接口，如照相機接口、加密處理器和帶FIFO的DMA控制器等。而R5F100LE單片機內置高速振蕩器時鐘，最高頻率可達32MHZ，其最短指令執行時間可在高速至超低速間更改，自帶單電源閃存，支持自編程功能，內置上電復位、看門狗定時器、按鍵中斷、時鐘輸出/蜂鳴器輸出控制電路等功能，使用極其方便。

四旋翼飛行器的4個動力臂呈十字交叉狀固定在四旋翼飛行器中心部件上，每個動力臂末端的電機座上固定一個電機和螺旋槳。通過改變電機轉速，可使飛行器在其慣性參考坐標系中產生六自由度運動，包括3個坐標軸方向的線運動（進退、左右側飛和升降）和3個坐標軸方向的角運動（偏航、俯仰和橫滾），可實現在有限區域的垂直起降、穩定盤旋以及精確目標移動等飛行姿態控制[1]。

姿態控制是整個飛行控制的基礎。姿態控制系統通過檢測飛行器在機體坐標系下3個軸向的角速度、角度和相對地面的高度等相應姿態信息，采用相應的控制算法解算出4個電機的轉速，發送給電機調速器調整電機轉速，實現對飛行器飛行姿態的調整。

2相關控制方法

四旋翼飛行器姿態調整的控制算法很多，如DI（digital input）控制、PID（比例微分積分）控制以及LQ控制等。其中，PID控制算法具有適用面廣、控制參數相互獨立、參數選定相對簡單等優點[45]，本系統采用PID控制算法實現飛行姿態控制。該算法可以分別計算出飛行器的自轉調節量、左右傾斜調節量、油門控制調節量、前后俯仰調節量等，使系統穩定偏差最小，保證飛行器能按照動作指令有效完成指定動作，如尋線和定高飛行等。PID控制系統原理：①比例環節可以準實時成比例地反映控制系統的偏差信號變化；②積分環節主要用于消除靜差，提高系統的無差度；③微分環節反應偏差信號的變化趨勢（變化速率），加快系統的動作速度，減小調節時間，如圖1所示。

理[67]，計算出當前的飛行姿態和地面控制需要的目標姿態，比較實際姿態和目標姿態的差異，估計過程狀態，并使估計均方差最小；利用PID算法計算出每個電機的調整量，調整電機轉速，從而實現飛行姿態的調整。為了得到穩定、準實時的飛行姿態，以定高飛行為例，利用超聲波傳回的高度值作為外環，將加速度計及加速度計積分得出的Z軸方向速度值作為內環，外環的輸出量解算后成為內環的期望值，內環通過PID控制器將計算得出的量疊加在油門上控制高度，通過調節PID參數值，用先整定內環后整定外環的方法確定PID參數，完成穩定的定高控制。在實際飛行中需調節KP、TI、TD和t這4個常數，這是采用PID算法進行控制的重點和難點。本系統借助實驗對這4個常數進行調節，得到一組較可靠的飛行姿態控制數據。借助視頻傳感器（攝像頭）采集圖像，通過對圖像的二值化處理，統計每一行和每一列的灰度值，算出灰度直方圖，根據行（列）的灰度總值變化趨勢，分析灰度出現突變的行列位置，得出導航線橫向的縱坐標，豎線的橫坐標，然后根據坐標進行飛行器的循線導航控制。

3硬件電路設計

系統硬件分為6個部分[67]，如圖2所示。超聲傳感器負責測量高度，和陀螺儀模塊一起實現定高控制；視頻傳感器（攝像頭）負責尋跡控制；激光傳感器完成電子視高控制；電機模塊負責按照主控信號驅動電機；姿態傳感部分負責獲取飛行姿態。為了提高處理速度，激光傳感器和視頻傳感器傳回的數據先傳入瑞薩單片機（R5F100LE）計算處理，處理結果作為引導量輸出到主控，再由主控使用PID算法完成穩定的尋跡前進，并實現物體的拾取和定點空投。

考慮到超聲波傳感器具有方向性好、能定向傳播的優點，超聲測距模塊US100被用來測量四旋翼飛行器的飛行高度。該模塊使用簡單、測量準確，而且能使用串口指令讀取數據[810]，與陀螺儀模塊結合可校正高度數據，實現定高飛行控制。為實現姿態獲取和控制，本系統集成了三軸加速度計和三軸陀螺儀MPU6050模塊實現飛行姿態感知，使用卡爾曼濾波處理數據后輸出比較穩定的姿態角，輸出加速度和陀螺儀的值。數字運動處理（DMP：Digital Motion Processing）引擎可減少復雜的融合演算數據，較容易地獲取和矯正飛行姿態；MPU6050模塊集成的陀螺儀測量范圍為±500dps，加速度計測量范圍為±2g。對陀螺儀和加速度計分別使用3個16位的ADC進行模數變化，采集速度快，能以數字輸出6軸或9軸融合演算數據并可程式控制，消除加速器與陀螺儀軸間敏感度，降低設定的影響度及感測器飄移。

由于直流電機工作電流大，硬件電路設計時需注意MCU與直流電機的隔離，若隔離措施不到位，容易燒壞MCU的端口。由于主控要連接較多的外圍電路，為了減少反接意外情況，將所有供電系統都通過保險絲控制，具體電路如圖3所示。

4軟件工作流程

四旋翼飛行器軟件主要分為3部分：①電機驅動部分。利用處理器的多路PWM輸出功能驅動電機；②數據接收和處理部分。獲取并處理飛行姿態數據；③姿態控制部分。根據接收到的傳感器數據，運用相應算法，使飛行器姿態平衡，實現特定狀態飛行。主程序根據要求設定不同的飛行控制模式，具體工作模式流程如圖4所示。系統軟件工作流程如圖5所示。

為驗證系統的可靠性和穩定性，分3種情況進行測試：①測試尋跡往返飛行。在飛行高度70cm時，測試降落地與設定中心的最大偏差為38cm；②測試尋跡定高載物定點投放往返飛行。在飛行高度70-79cm時，測試物體落地與定點投放中心的最大偏差為50cm，降落地與設定中心的偏差為38cm，在5次測試中碰觸定高線引發報警1次；③測試尋跡定點拾取物體往返飛行。測試降落地與設定中心的最大偏差為40cm。測試結果表明本文設計的四旋翼飛行器飛行高度穩定、姿態解算正確，能實現尋跡、定高飛行、定點投放、拾取重物等多種需求。

6結語

本文采用ST公司的STM32F407VGT處理器和瑞薩R5F100LE單片機，設計實現了四旋翼飛行器控制系統，可實現定向飛行和定高飛行，在規定線路內的自主飛行，并且能夠通過小型電磁鐵吸取薄鐵片飛至指定區域完成空投。測試表明飛行器能一鍵起飛，完成空投任務，精確降落。

參考文獻：

[1]楊云高，鮮斌，殷強，等.四旋翼無人飛行器架構及飛行控制的研究現狀[C].第三十屆中國控制會議，中國煙臺， IEEE Press，2010：448453.

[2]黃智偉.全國大學生電子設計競賽常用電路模塊制作[M].北京：北京航空航天大學出版社，2011.

[3]聶博文.微小型四旋翼飛行器的研究現狀與關鍵技術[J].電光與控制，2007，14（6）：113117.

[4]楊明志，王敏.四旋翼微型飛行器控制系統設計[J].計算機測量與控制，2008，16（4）485490.

[5]白立群，李成鐵，周劍峰.基于 STM32 的飛行控制器系統設計[J].計算機應用，2013，32（2）：1533.

[6]HOFFMANN F，GODDEMEIER N，BERTRAM T.Attitude estimation and control of a quadrocopter [C].The 2010 IEEE/RSJ International conference on Intelligent Robots and Systems.Taipei：IEEE，2010：10721077 .

[7]王學斌，徐建宏，張章.卡爾曼濾波器參數分析與應用方法研究[J].計算機應用與軟件，2012，29（6）：212215.

[8]李強，林明耀.無傳感器無刷直流電機控制系統及其起動分析[J].微電機，2003，36（3）：3335.

[9]周權，黃向華，朱理化.四旋翼微型飛行平臺姿態穩定控制試驗研究[J].傳感器與微系統，2009，28（5）：7279.

[10]黃敦華，朱青松.基于微小型四旋翼飛行器的目標監測與識別綜述[J].機電產品開發與創新，2011，24（6）：1618.