摘  要： 四旋翼飛行器控制系統(tǒng)的性能決定了飛行效果的優(yōu)劣，如何改善飛行控制系統(tǒng)使其擁有更良好的表現(xiàn)成為近幾年的研究熱點(diǎn)。根據(jù)四旋翼飛行器的飛行原理，設(shè)計(jì)了一種新型四旋翼飛行器控制系統(tǒng)。該系統(tǒng)以STM32作為主控制器，配合各姿態(tài)傳感器實(shí)現(xiàn)飛行器姿態(tài)及位置的控制，并結(jié)合以姿態(tài)角為主要誤差源的雙環(huán)結(jié)構(gòu)PID控制器，提高了飛行器的平穩(wěn)性。經(jīng)實(shí)際飛行驗(yàn)證，該飛行控制系統(tǒng)方案能夠取得較穩(wěn)定的飛行效果。

　　關(guān)鍵詞： 四旋翼；飛行控制；STM32；PID控制

0 引言

　　目前，以精確的電子技術(shù)取代復(fù)雜的機(jī)械結(jié)構(gòu)已成為一種趨勢(shì)，四旋翼飛行器（four-rotor）以機(jī)載電子設(shè)備控制機(jī)身的姿態(tài)及運(yùn)動(dòng)，機(jī)械結(jié)構(gòu)得到簡(jiǎn)化，降低了生產(chǎn)成本及維護(hù)成本。它可以實(shí)現(xiàn)垂直起降、懸停等空中動(dòng)作，飛行速度為每秒幾米甚至十幾米，具有較高的機(jī)動(dòng)性與操控性，與其他類型飛行器相比具有較明顯的優(yōu)勢(shì)，在軍工、警用、民用等諸多領(lǐng)域有廣泛的應(yīng)用前景。

　　四旋翼飛行器具有4個(gè)輸入力，但卻有6個(gè)狀態(tài)輸出，是一個(gè)欠驅(qū)動(dòng)系統(tǒng)，具有非線性、強(qiáng)耦合、多變量等特性，因此對(duì)控制器的設(shè)計(jì)要求較高[1]。根據(jù)四旋翼飛行器結(jié)構(gòu)特點(diǎn)及實(shí)際需求，選用意法半導(dǎo)體公司生產(chǎn)的STM32F405RG微處理器作為控制系統(tǒng)的主控制器，其強(qiáng)大的運(yùn)算處理能力及豐富的片上資源能夠滿足四旋翼控制的需求。

1 四旋翼的飛行原理

　　四旋翼飛行器結(jié)構(gòu)形式是一種最直觀、最簡(jiǎn)單的穩(wěn)定控制形式。四旋翼飛行器機(jī)身為兩根剛性支架呈十字形交叉結(jié)構(gòu)，飛控板、外部設(shè)備及電池等安裝在支架的交叉處，作為飛行器動(dòng)力來源的4個(gè)電機(jī)及旋翼軸對(duì)稱地安裝在支架的4個(gè)末端，相鄰兩個(gè)旋翼旋轉(zhuǎn)方向相反，空中平衡飛行時(shí)相鄰旋翼產(chǎn)生的反作用力相互抵消，防止機(jī)身自旋[2]。通過控制4個(gè)旋翼的轉(zhuǎn)速，產(chǎn)生升力的變化，便可實(shí)現(xiàn)飛行器空中姿態(tài)的控制。四旋翼飛行器有6種基本的運(yùn)動(dòng)狀態(tài)，分別是：垂直運(yùn)動(dòng)、前后運(yùn)動(dòng)、側(cè)向運(yùn)動(dòng)、俯仰運(yùn)動(dòng)、滾轉(zhuǎn)運(yùn)動(dòng)和偏航運(yùn)動(dòng)。基于以上基本運(yùn)動(dòng)狀態(tài)，四旋翼可以完成各種復(fù)雜動(dòng)作[3]。四旋翼的結(jié)構(gòu)形式如圖1所示。

2 系統(tǒng)硬件選型與電路設(shè)計(jì)

　　四旋翼飛行器的控制系統(tǒng)主要由主控制器、各種姿態(tài)傳感器、數(shù)據(jù)傳輸模塊、電機(jī)及其驅(qū)動(dòng)模塊組成。主控器通過SPI、IIC總線采集各姿態(tài)傳感器的數(shù)據(jù)，實(shí)時(shí)計(jì)算飛行器相對(duì)于地面的姿態(tài)角和航向角的變化，結(jié)合相應(yīng)的控制律與任務(wù)指令輸出恰當(dāng)?shù)腜WM波信號(hào)，電調(diào)根據(jù)PWM波處理產(chǎn)出相應(yīng)的電壓信號(hào)輸入給4個(gè)電機(jī)，以此控制電機(jī)轉(zhuǎn)速，實(shí)現(xiàn)自動(dòng)調(diào)整飛行器在空中的姿態(tài)與位置。四旋翼飛行器飛行控制系統(tǒng)設(shè)計(jì)方案如圖2所示。

　　2.1 主控制器

　　四旋翼控制系統(tǒng)對(duì)實(shí)時(shí)性、穩(wěn)定性及低功耗具有較高要求，因此選用一款高性能的芯片作為其主控制器尤為重要。STM32F405RG基于32位ARM Cortex-M4內(nèi)核開發(fā)，主頻可達(dá)168 MHz，處理單條指令速度可達(dá)1.25 DMIPS/MHz，滿足對(duì)數(shù)據(jù)處理速度的要求。它具有功耗動(dòng)態(tài)調(diào)整功能，能夠在運(yùn)行模式下和從Flash存儲(chǔ)器執(zhí)行時(shí)實(shí)現(xiàn)低至238 A/MHz的電流消耗。多達(dá)15個(gè)高速通信接口，包括6個(gè)USART、3個(gè)SPI和3個(gè)IIC等，可用于將各傳感器模塊采集的姿態(tài)數(shù)據(jù)傳輸給主控制器。STM32集成多個(gè)PWM定時(shí)器，可實(shí)現(xiàn)多路PWM脈沖信號(hào)的輸出。用戶可使用SWD或JTAG兩種模式下載和調(diào)試程序。

　　2.2 陀螺儀傳感器

　　陀螺儀是飛行器慣導(dǎo)系統(tǒng)中的核心器件，用于檢測(cè)俯仰、滾轉(zhuǎn)和偏航3個(gè)方向的角速度，積分后可得到相應(yīng)的傾斜角度[4]?？刂破鲗?duì)傾斜角的控制使飛行器在受到外界干擾（如氣流等）時(shí)能保持姿態(tài)的平穩(wěn)。

　　系統(tǒng)采用體積小、功耗低的MAX21000型號(hào)3軸數(shù)字輸出陀螺儀。用戶可設(shè)定±31.25~±2 000 dps的量程范圍，靈敏度可達(dá)960 digit/dps。通過10 MHz的SPI接口向STM32提供測(cè)得的角速度數(shù)據(jù)。陀螺儀工作時(shí)存在一定的溫度漂移，需通過軟件對(duì)陀螺儀中立點(diǎn)進(jìn)行實(shí)時(shí)修正來提高數(shù)據(jù)的精準(zhǔn)度。MAX21000陀螺儀電路原理如圖3所示。

　　2.3 加速度傳感器

　　本系統(tǒng)采用ST公司的LSM303D加速度計(jì)。該芯片整合了一個(gè)3軸加速度計(jì)和一個(gè)3軸磁場(chǎng)傳感器。通過SPI接口向STM32提供數(shù)據(jù)。LSM303D加速度計(jì)電路原理如圖4所示。

　　加速度傳感器用于檢測(cè)飛行器軸向的加速度，積分可得飛行速度與距離。陀螺儀的數(shù)據(jù)因?yàn)榇嬖凇皽仄?，在積分作用下，較長(zhǎng)時(shí)間后數(shù)據(jù)將不可靠，加速度計(jì)傳感器的數(shù)據(jù)可以對(duì)陀螺儀的誤差進(jìn)行矯正。但同時(shí)加速度傳感器對(duì)震動(dòng)比較敏感，加入簡(jiǎn)單的濾波會(huì)降低數(shù)據(jù)的實(shí)時(shí)性，因此精確的姿態(tài)采集需多傳感器配合實(shí)現(xiàn)。

　　2.4 高度計(jì)

　　HC-SR04超聲波測(cè)距模塊只需提供一個(gè)10 ?滋m以上的脈沖觸發(fā)信號(hào)，將自動(dòng)發(fā)射8個(gè)40 kHz方波，同時(shí)檢測(cè)是否有返回波，根據(jù)發(fā)射信號(hào)與檢測(cè)到返回信號(hào)的時(shí)間差可計(jì)算模塊與障礙物的距離。該模塊測(cè)量飛行器離地高度精度可達(dá)3 mm，在精度上優(yōu)于氣壓計(jì)，但是也有其缺陷，當(dāng)?shù)孛鏋椴萜夯虮粶y(cè)障礙物表面不平整時(shí)，返回的信號(hào)太弱。同時(shí)該模塊測(cè)距范圍為2~400 cm，只能用于較低高度的檢測(cè)。

　　飛行器在戶外飛行時(shí)，高度易超出超聲波模塊的測(cè)量范圍，則系統(tǒng)采用MS5611氣壓傳感器測(cè)量高度[5]。上電時(shí)，氣壓傳感器測(cè)量當(dāng)前的穩(wěn)定氣壓并保存為0 m氣壓，此后飛行器高度改變，在此參數(shù)基礎(chǔ)上進(jìn)行疊加。該芯片集成了一個(gè)24位AD轉(zhuǎn)換器，將氣壓值轉(zhuǎn)換成數(shù)字信號(hào)輸出，測(cè)距精度可達(dá)10 cm。

3 四旋翼飛行器建模

　　選擇合適的坐標(biāo)系有利于四旋翼飛行器飛行狀態(tài)的分析。如圖1所示，本文選擇地面坐標(biāo)系為E（OXYZ），機(jī)體坐標(biāo)系為B（oxyz）。地面坐標(biāo)系與機(jī)體坐標(biāo)系的關(guān)系為滾轉(zhuǎn)角、俯仰角和偏航角，表示分別繞Bx、By、Bz旋轉(zhuǎn)到Ex、Ey、Ez軸的角度。從機(jī)體坐標(biāo)系B到地面坐標(biāo)系E的旋轉(zhuǎn)矩陣R可表示為：

　　定義Fi（i=1，2，3，4）表示第i個(gè)旋翼產(chǎn)生的升力。在機(jī)體坐標(biāo)系下，飛行器所受的拉力為：

　　代入旋轉(zhuǎn)矩陣R可轉(zhuǎn)換為飛行器在地面坐標(biāo)系中的受力：

　　F

　　四旋翼飛行器在地面坐標(biāo)系中的線性位移為[x y z]T，根據(jù)牛頓第二定理F=ma，得線性加速度方程為：

　　定義l為飛行器臂長(zhǎng)，M、M、M分別為飛行器繞機(jī)體坐標(biāo)系3個(gè)軸的轉(zhuǎn)矩，I、I、I分別為飛行器繞機(jī)體坐標(biāo)系3個(gè)軸的轉(zhuǎn)動(dòng)慣量，i為旋翼i轉(zhuǎn)速，則飛行器在滾轉(zhuǎn)、俯仰、偏航3個(gè)方向上的轉(zhuǎn)矩為：

　　由歐拉方程可得，四旋翼飛行器在地面坐標(biāo)系中的角加速度方程式為：

　　令：

　　式（7）為四旋翼飛行器控制系統(tǒng)的4個(gè)輸入力，U1、U2、U3、U4分別為飛行器的滾轉(zhuǎn)力、俯仰力、偏航力及升力。結(jié)合式（4）、式（6）和式（7）可得四旋翼飛行器的動(dòng)力學(xué)模型為：

　　該動(dòng)力學(xué)模型忽略了空氣阻力、機(jī)體形變、重力加速度隨高度的改變而變化等對(duì)系統(tǒng)影響較小的因素，相對(duì)于實(shí)際飛行狀態(tài)，該模型進(jìn)行了簡(jiǎn)化，有利于控制算法的設(shè)計(jì)。

4 PID控制器設(shè)計(jì)

　　本系統(tǒng)采用雙環(huán)PID設(shè)計(jì)，內(nèi)回路為姿態(tài)PID回路，外回路為位置PID回路[6]。給出預(yù)期位置指令后，位置PID回路計(jì)算出所需改變的姿態(tài)角，輸入給姿態(tài)回路，姿態(tài)回路計(jì)算出電機(jī)的控制量。姿態(tài)的改變可引起飛行器線性的運(yùn)動(dòng)，因此實(shí)施閉環(huán)設(shè)計(jì)以精確控制飛行器的姿態(tài)。位置控制回路的設(shè)計(jì)可以使飛行更精確、快速地按照設(shè)定軌跡飛行。PID控制器結(jié)構(gòu)如圖5所示。

　　四旋翼平衡飛行時(shí)，PID控制器以傾斜角作為其主要的誤差來源，通過將偏差量的比例、積分和微分進(jìn)行線性組合，構(gòu)成控制量。比例參數(shù)的大小決定了控制器的響應(yīng)速度，但參數(shù)過大會(huì)使系統(tǒng)不穩(wěn)定；對(duì)角速度積分后可得飛行器的傾斜角度，從而產(chǎn)生抵抗力，消除傾斜角，直到飛行器達(dá)到平衡位置；飛行器偏差的產(chǎn)生與控制器的響應(yīng)之間存在微秒級(jí)的滯后，微分環(huán)節(jié)能提前抑制傾斜角的偏差，但是過強(qiáng)的微分會(huì)放大傳感器的噪聲干擾，降低控制器的抗干擾性能。因此設(shè)定恰當(dāng)?shù)腜ID控制參數(shù)才能保證飛行器達(dá)到穩(wěn)定的飛行狀態(tài)。

5 飛行試驗(yàn)結(jié)果

　　飛行實(shí)驗(yàn)在室內(nèi)進(jìn)行，實(shí)驗(yàn)過程中，地面監(jiān)控站接收飛行器通過串口模塊傳送的實(shí)時(shí)飛行數(shù)據(jù)，并將其保存，同時(shí)設(shè)定懸停高度為2 m。手動(dòng)操控四旋翼飛行器起飛至2 m左右，切換到平衡模式，使其進(jìn)行懸停飛行，在此過程中，飛控系統(tǒng)對(duì)飛行器的滾轉(zhuǎn)角、俯仰角及高度進(jìn)行自主控制。飛行實(shí)驗(yàn)時(shí)間約為2 min，實(shí)驗(yàn)結(jié)果如圖6~8所示。

　　從圖6和圖7可以看出，飛行器的滾轉(zhuǎn)角度和俯仰角度基本控制在±2°以內(nèi)，俯仰角的穩(wěn)定性略高于滾轉(zhuǎn)角，兩個(gè)姿態(tài)角都達(dá)到了較理想的控制效果。

　　圖8為高度曲線，從圖中可以看出，飛行器在最初的幾秒內(nèi)自主調(diào)整到設(shè)定的高度位置，之后的時(shí)間穩(wěn)定在1.9 m到2.1 m之間。飛行器高度的自主調(diào)整在0.2 m的范圍波動(dòng)，但總體仍在目標(biāo)高度附近。

6 結(jié)論

　　本文提出了一套四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)方案，經(jīng)懸停飛行實(shí)驗(yàn)驗(yàn)證，飛行器能夠成功實(shí)現(xiàn)滾轉(zhuǎn)、俯仰和高度的自主控制，各個(gè)模塊性能可靠、穩(wěn)定，達(dá)到了設(shè)計(jì)要求。飛行控制系統(tǒng)以STM32為主控制器，與傳統(tǒng)單片機(jī)相比具有較明顯優(yōu)勢(shì)，其豐富的片上資源有利于系統(tǒng)功能的進(jìn)一步擴(kuò)展，為后續(xù)課題的研究提供了良好的平臺(tái)。四旋翼飛行器操控難度低、飛行角度靈活，在軍事偵查、安全巡檢、低空航拍等眾多領(lǐng)域具有廣泛的應(yīng)用前景。

　　參考文獻(xiàn)

　　[1] 劉乾，孫志鋒.基于ARM的四旋翼無人飛行器控制系統(tǒng)[J].機(jī)電工程，2011（10）：1237-1240.

　　[2] 殷強(qiáng).四旋翼無人機(jī)自主控制系統(tǒng)研究[D].天津：天津大學(xué)，2011.

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

　　[4] 韓濤.四傾轉(zhuǎn)旋翼機(jī)直升機(jī)模式飛行控制系統(tǒng)研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2012.

　　[5] 冀明，盧京潮.基于Cortex-M4的四旋翼飛行控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2013，21（7）：1816-1821.

　　[6] 宿敬亞，樊鵬輝，蔡開元.四旋翼飛行器的非線性PID姿態(tài)控制[J].北京航空航天大學(xué)學(xué)報(bào)，2011，37（9）：1054-1058.