山西中北大學(xué)機(jī)電工程學(xué)院　郭魯奇　顧強(qiáng)　趙意鵬

四旋翼無人機(jī)飛行控制系統(tǒng)設(shè)計

山西中北大學(xué)機(jī)電工程學(xué)院郭魯奇顧強(qiáng)趙意鵬

本文設(shè)計了以STM32F103C8為主控芯片的控制系統(tǒng)，該系統(tǒng)通過MPU6050整合性六軸運(yùn)動處理組件采集數(shù)據(jù)并進(jìn)行濾波處理，再利用四元數(shù)姿態(tài)解算算法得到姿態(tài)角，使用HC-SR04超聲波來測量飛行器高度，通過PID控制實(shí)現(xiàn)飛行器姿態(tài)和高度的控制。測試結(jié)果表明，該系統(tǒng)能使飛行器完成起飛、懸停、降落一系列動作，且穩(wěn)定可靠。

飛行控制系統(tǒng)；MPU6050；HC-SR04；四元數(shù)姿態(tài)解算；PID控制

引言

四旋翼無人機(jī)是一種依靠4個呈十字形或X形分布的電動螺旋槳提供動力的飛行器，特別適合在近地面環(huán)境中執(zhí)行監(jiān)視、偵察等任務(wù)，具有廣闊的軍事和民用前景。因此，設(shè)計一套可靠性高的飛行控制系統(tǒng)就顯得尤為重要。

1　控制系統(tǒng)組成及工作原理

本系統(tǒng)主要由電源電路、主控芯片STM32F103C8、無線射頻模塊nRF2401、超聲波測距模塊HC-SR04、運(yùn)動處理組件MPU-6050及電機(jī)等部分組成，其結(jié)構(gòu)示意圖如圖1所示。

圖1　控制系統(tǒng)結(jié)構(gòu)示意圖

本系統(tǒng)以STM32F103C8為主控芯片，通過無線射頻模塊nRF2401向主控芯片發(fā)送起飛、懸停及降落等命令，當(dāng)其收到命令后，主控芯片通過提取HC-SR04和MPU-6050的信號，利用四元數(shù)姿態(tài)解算法得出飛行器當(dāng)前的狀態(tài)，再通過PID算法，利用PWM調(diào)節(jié)電機(jī)的轉(zhuǎn)速，直到達(dá)到所要求的狀態(tài)［1］。

2　控制系統(tǒng)硬件設(shè)計

2.1主控芯片STM32F103C8

STM32F103C8是基于32位ARM Cotex-M3內(nèi)核，最高主頻率72MHZ，具有高達(dá)90DMips、1.25DMips/MHz的處理能力，Cotex-M3內(nèi)核采用哈佛結(jié)構(gòu)及流水線技術(shù)，很大限度上提高了控制器的運(yùn)算能力，能夠滿足該控制系統(tǒng)復(fù)雜而快速的計算要求，同時該芯片功耗小、性價比高，能夠滿足本系統(tǒng)的設(shè)計要求。

2.2無線通信模塊nRF2401

該模塊是集收發(fā)一體的無線通信模塊，工作在2.4～2.5GHz頻段，具有高達(dá)2M的數(shù)據(jù)傳輸速率，在功率放大器的配合下，可以達(dá)到100m的傳輸距離，滿足設(shè)計要求［2］。

2.3超聲波測距模塊HC-SR04

HC-SR04是可測量2cm～400cm范圍的非接觸式測距模塊，包括超聲波發(fā)射器、接收器及控制電路。工作時，通過給TRIG引腳大于10us的高電平，觸發(fā)模塊開始工作，自動發(fā)送8個40KHz的方波，并檢測是否有返回信號，若有信號返回，則通過ECHO引腳發(fā)出一個高電平，高電平引腳持續(xù)的時間就是超聲波信號從發(fā)射到返回接受的時間，則測量距離=（時間*聲速）/2。

2.4運(yùn)動檢測模塊MPU-6050

MPU-6050是集3軸MEMS陀螺儀、3軸MEMS加速度計及1個可擴(kuò)展的數(shù)字運(yùn)動處理器的運(yùn)動控制傳感器，具有I2C和SPI兩種接口形式，可以滿足多方面的需要［3］，其連接電路圖如圖2所示。

圖2　MPU-6050連接電路圖

3　控制系統(tǒng)軟件設(shè)計

3.1主程序設(shè)計

通過對飛行器進(jìn)行起飛、懸停及降落三個動作的飛行來驗(yàn)證所設(shè)計控制系統(tǒng)的可靠性。在給系統(tǒng)上電后，首先進(jìn)行主

圖3　主程序流程圖

3.2四元數(shù)姿態(tài)解算

飛行器的高度通過超聲波測距模塊HC-SR04來獲取，姿態(tài)由MPU-6050通過I2C總線傳輸，用姿態(tài)角來表示。姿態(tài)角是載體坐標(biāo)系b相對于導(dǎo)航坐標(biāo)系n的狀態(tài)。假設(shè)飛行器上一點(diǎn)在載體坐標(biāo)系b中的坐標(biāo)是（Xb，Yb，Zb），在導(dǎo)航坐標(biāo)系n中的坐標(biāo)是（Xn，Yn，Zn），則有姿態(tài)變換矩陣Cnb滿足：

四元數(shù)是由一個實(shí)數(shù)和三個虛數(shù)i j、k單位組成的超復(fù)數(shù)［5］。如果將坐標(biāo)系b、n的三維矢量擴(kuò)展成四維矢量，可以表示為：

結(jié)合式（1）和式（3）以及四元數(shù)的乘法運(yùn)算法則可得到如下的轉(zhuǎn)換關(guān)系：Cnb=

根據(jù)坐標(biāo)變化理論，導(dǎo)航坐標(biāo)系B依次旋轉(zhuǎn)φ、θ、γ角度后可以得到載體坐標(biāo)系A(chǔ)，從而得到方向余弦矩陣［6］：

那么綜合（5）和（6）可得：

［7］得如下關(guān)系式：

其中ωABA為四元數(shù)形式的向量，表示載體相對于導(dǎo)航坐標(biāo)系轉(zhuǎn)動的角速度在載體坐標(biāo)系上的投影，Qc是CBA對應(yīng)的四元數(shù)。方程即為四元數(shù)微分方程，也可表示為：

求解出該方程可得到四元數(shù)Qc。求出實(shí)時四元數(shù)之后結(jié)合式（4）（5）（6）（7）即可得到姿態(tài)角。

3.3PID控制算法設(shè)計

PID控制是根據(jù)系統(tǒng)的誤差，利用比例、積分和微分計算出控制量進(jìn)行反饋控制的算法。其控制框圖如圖4所示。

圖4　PID控制框圖

PID控制器的完整公式［8］是：

其中Kp、Ti、Td分別是比例系數(shù)、積分系數(shù)和微分系數(shù)，為滿足數(shù)字控制系統(tǒng)的要求還需將其離散化：

其中k是采樣次數(shù)，T是采樣周期，進(jìn)一步可以得到增量式PID控制：

其中，Ki=Kp（T/Ti），Kd=Kp（Td/T）從而可根據(jù)每次采樣結(jié)束時ek、ek-1、ek-2的值可以計算出控制器的輸出量。

本系統(tǒng)高度控制采用雙閉環(huán)控制，內(nèi)環(huán)控制的采樣周期是5ms，外環(huán)是10ms，外環(huán)直接控制高度，為了避免外力影響，內(nèi)環(huán)控制Z軸加速度，通過給定的重力加速度與Z軸加速度比較，輸出控制量調(diào)節(jié)油門變化。高度PID控制如圖5所示。

圖5　高度PID控制

本系統(tǒng)姿態(tài)控制采用雙閉環(huán)控制，內(nèi)環(huán)控制的采樣周期是2ms，外環(huán)是5ms，使得角速度控制較為靈敏，且抗干擾能力強(qiáng)，姿態(tài)PID控制如圖6所示。

圖6　姿態(tài)PID控制

4　系統(tǒng)測試

給系統(tǒng)上電后，先輕輕晃動飛行器，在Watch Windows窗口中，可以看到MPU-6050傳回的數(shù)據(jù)，說明正常工作，如圖7所示。接著給飛行器起飛命令，飛行器起飛，到達(dá)大約3m處，懸停10s，降落，完成預(yù)定的飛行軌跡。

5　結(jié)論

本系統(tǒng)以四元數(shù)姿態(tài)解算為載體，利用PID控制算法來控制飛行器的飛行，具有較高的精度和可靠性，對飛行器控制系統(tǒng)的后續(xù)改進(jìn)具有一定的參考價值。

參考文獻(xiàn)：

［1］謝義建，陳躍東，舒圣焱.基于STM32的四旋翼飛行器的設(shè)計與實(shí)現(xiàn)［J］.四川理工學(xué)院學(xué)報（自然科學(xué)版），2014，27（3）：42-45.

［2］陳志旺.STM32嵌入式微控制器快速上手［M］.廈門大學(xué)出版社，2014：57-60.

［3］王福超.四旋翼無人飛行器控制系統(tǒng)設(shè)計與實(shí)現(xiàn)［D］.哈爾濱工程大學(xué)，2013：8-9.

［4］鄭震璇.基于PIC單片機(jī)的汽車空調(diào)控制器設(shè)計［J］.機(jī)電技術(shù)，2009，2：37-40.

［5］孫貴斌，楊福清，方遒等.基于模糊PD控制的客車前大燈隨動轉(zhuǎn)向研究［J］.機(jī)電技術(shù)，2015，8：121-126.

［6］史瑩晶.航空飛行器控制與仿真［M］.電子科技大學(xué)出版社，2011：28-31.

［7］李堯.四旋翼飛行器控制系統(tǒng)設(shè)計［D］.大連理工大學(xué)，2013：37-38.

［8］胡壽松.自動控制原理［M］.科學(xué)出版社，2007：121-134.

郭魯奇，山東菏澤人，在讀研究生，研究方向：機(jī)電系統(tǒng)設(shè)計與分析?？匦酒珿PIO、無線通信模塊、電機(jī)及測量模塊等的初始化，然后發(fā)送起飛命令，使飛行器飛行到3m高處懸停大約10s，最后降落地面［4］，其程序流程圖如圖3所示。