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

楊　萌，雷建和，胡廷軒，宮汝林（青島理工大學(xué)　自動(dòng)化工程學(xué)院，山東　青島　266520）

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

楊萌，雷建和，胡廷軒，宮汝林
（青島理工大學(xué)自動(dòng)化工程學(xué)院，山東青島266520）

四旋翼飛行器控制系統(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)定的飛行效果。

四旋翼；飛行控制；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)得到簡化，降低了生產(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)形式是一種最直觀、最簡單的穩(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所示。

圖1　四旋翼的結(jié)構(gòu)形式

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　飛行控制系統(tǒng)設(shè)計(jì)方案

2.1主控制器

四旋翼控制系統(tǒng)對(duì)實(shí)時(shí)性、穩(wěn)定性及低功耗具有較高要求，因此選用一款高性能的芯片作為其主控制器尤為重要。STM32F405RG基于32位ARMCortex-M4內(nèi)核開發(fā)，主頻可達(dá)168MHz，處理單條指令速度可達(dá)1.25DMIPS/MHz，滿足對(duì)數(shù)據(jù)處理速度的要求。它具有功耗動(dòng)態(tài)調(diào)整功能，能夠在運(yùn)行模式下和從Flash存儲(chǔ)器執(zhí)行時(shí)實(shí)現(xiàn)低至238A/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)中的核心器件，用于檢測俯仰、滾轉(zhuǎn)和偏航3個(gè)方向的角速度，積分后可得到相應(yīng)的傾斜角度［4］。控制器對(duì)傾斜角的控制使飛行器在受到外界干擾（如氣流等）時(shí)能保持姿態(tài)的平穩(wěn)。

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

圖3陀螺儀電路

2.3加速度傳感器

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

圖4　加速度計(jì)電路

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

2.4高度計(jì)

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

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

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)系中的受力：

定義l為飛行器臂長，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)行了簡化，有利于控制算法的設(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所示。

圖5　PID控制器結(jié)構(gòu)

四旋翼平衡飛行時(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è)定懸停高度為2m。手動(dòng)操控四旋翼飛行器起飛至2m左右，切換到平衡模式，使其進(jìn)行懸停飛行，在此過程中，飛控系統(tǒng)對(duì)飛行器的滾轉(zhuǎn)角、俯仰角及高度進(jìn)行自主控制。飛行實(shí)驗(yàn)時(shí)間約為2min，實(shí)驗(yàn)結(jié)果如圖6～8所示。

圖6　滾轉(zhuǎn)角曲線

圖7　俯仰角曲線

圖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.9m到2.1m之間。飛行器高度的自主調(diào)整在0.2m的范圍波動(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)用前景。

［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ī)測量與控制，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ī)測量與控制，2013，21（7）：1816-1821.

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

Design for control system of four-rotor aircraft based on STM32

Yang Meng，Lei Jianhe，Hu Tingxuan，Gong Ruling
（School of Automation Engineering，Qingdao Technological University，Qingdao 266520，China）

The performance of control system of four-rotor aircraft determines the flight effect，how to improve the flight control system enables it to have better performance has become the hotspot of research in recent years.According to the flight principle of four-rotor aircraft，a four-rotor aircraft control system is designed.The system uses STM32 as the main controller，with each attitude sensor to achieve the control of posture and position of aircraft，and combined with the double loop structure of PID controller which the main error source depends on attitude angle，improving stability of aircraft.The actual flight shows that the flight control system can obtain stable flight effect.

four-rotor；flight control；STM32；PID control

TP391.8

A

1674-7720（2015）12-0076-04

2015-01-30）

楊萌（1990-），通信作者，男，碩士研究生，主要研究方向：控制理論及應(yīng)用。E-mail：33385024@qq.com。

雷建和（1961-），男，副教授，碩士生導(dǎo)師，主要研究方向：控制理論及應(yīng)用、機(jī)器人技術(shù)、模式識(shí)別與智能系統(tǒng)。