小型無(wú)人機(jī)飛行姿態(tài)串級(jí)控制系統(tǒng)

程方曉， 王海彪， 劉 璐

(長(zhǎng)春工業(yè)大學(xué) 電氣與電子工程學(xué)院， 吉林 長(zhǎng)春 130012)

小型無(wú)人機(jī)飛行姿態(tài)串級(jí)控制系統(tǒng)

程方曉， 王海彪， 劉 璐

(長(zhǎng)春工業(yè)大學(xué) 電氣與電子工程學(xué)院， 吉林 長(zhǎng)春 130012)

采用四元數(shù)互補(bǔ)濾波算法對(duì)IMU單元等多傳感器采集到的數(shù)據(jù)進(jìn)行解算融合，串級(jí)PID為小型無(wú)人機(jī)飛行姿態(tài)系統(tǒng)的控制算法。設(shè)計(jì)了實(shí)時(shí)監(jiān)測(cè)子系統(tǒng),實(shí)驗(yàn)結(jié)果表明,基于STM32的串級(jí)PID控制算法可以實(shí)現(xiàn)飛行姿態(tài)的有效控制。

無(wú)人機(jī)； 姿態(tài)； STM32； 監(jiān)控

0 引 言

近幾年，隨著科學(xué)技術(shù)的快速發(fā)展，小型無(wú)人機(jī)在軍用、民用以及日常搶險(xiǎn)救災(zāi)中起到了至關(guān)重要的作用,正逐漸成為人們生活中不可或缺的一部分。文中針對(duì)無(wú)人機(jī)涉及的空氣動(dòng)力學(xué)原理、自動(dòng)控制原理以及數(shù)據(jù)融合算法等具有非線性多變量的特點(diǎn),采用捷聯(lián)慣性測(cè)量單元(IMU)進(jìn)行姿態(tài)檢測(cè)，并用多傳感器數(shù)據(jù)融合得到精確的姿態(tài)角[-2]。通過比較獲取的當(dāng)前姿態(tài)和給定姿態(tài)得到偏差，經(jīng)串級(jí)PID調(diào)節(jié)PWM控制信號(hào)實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速的控制[3]，從而達(dá)到姿態(tài)調(diào)節(jié)的目的，通過無(wú)線傳輸與上位機(jī)進(jìn)行通信，達(dá)到動(dòng)態(tài)調(diào)整和實(shí)時(shí)監(jiān)測(cè)的效果。

1 系統(tǒng)工作原理

無(wú)人機(jī)為十字形結(jié)構(gòu)[4]，4個(gè)電機(jī)分別驅(qū)動(dòng)4個(gè)安裝于機(jī)體頂點(diǎn)的螺旋槳，1、3為一組，2、4為一組,分別產(chǎn)生順時(shí)針方向扭矩和逆時(shí)針方向扭矩，順逆方向反轉(zhuǎn)矩相互抵消通過控制電機(jī)轉(zhuǎn)速達(dá)到控制無(wú)人機(jī)飛行控制，如圖1所示。

圖1 四旋翼飛行器結(jié)構(gòu)示意圖

飛行控制由姿態(tài)控制和位置控制兩部分組成，文中主要研究姿態(tài)控制。姿態(tài)控制就是在前期姿態(tài)解算的基礎(chǔ)上對(duì)無(wú)人機(jī)實(shí)施有效的飛行控制，從而達(dá)到想要的控制效果。這就要求算法能夠連續(xù)做決策，不斷選擇最佳的飛行控制，算法的評(píng)價(jià)應(yīng)由其做出選擇的長(zhǎng)期質(zhì)量來判定?？刂七^程以PID反饋控制器為主。飛行器主要通過改變四個(gè)旋翼的角速度來控制四旋翼無(wú)人機(jī)。每個(gè)旋翼產(chǎn)生一個(gè)推力(F1、F2、F3、F4)和一個(gè)力矩，其共同作用產(chǎn)生的合力構(gòu)成四旋翼無(wú)人機(jī)的主推力、偏航力矩、俯仰力矩和滾轉(zhuǎn)力矩。文中以四旋翼無(wú)人機(jī)為控制對(duì)象，其動(dòng)力學(xué)模型可描述為將控制對(duì)象看作有一個(gè)力和三個(gè)力矩的三維剛體。四旋翼無(wú)人機(jī)在小角度變化條件下的動(dòng)力學(xué)模型近似如下：

2 系統(tǒng)總體設(shè)計(jì)

無(wú)人機(jī)系統(tǒng)由主控芯片、電源管理、IMU單元、動(dòng)力和無(wú)線通信5部分組成，系統(tǒng)結(jié)構(gòu)如圖2示。

圖2 系統(tǒng)結(jié)構(gòu)圖

控制部分讀取和處理傳感器數(shù)據(jù)，同時(shí)接收無(wú)線控制信息對(duì)無(wú)人機(jī)進(jìn)行控制；電源為各部分提供電壓；IMU單元由加速度計(jì)、陀螺儀、磁阻傳感器和氣壓傳感器對(duì)無(wú)人機(jī)飛行姿態(tài)進(jìn)行采集,并用四元數(shù)互補(bǔ)濾波法對(duì)無(wú)人機(jī)的飛行高度和姿態(tài)角進(jìn)行計(jì)算[5]，結(jié)合串級(jí)PID控制算法輸出合適的控制信號(hào)；通過驅(qū)動(dòng)動(dòng)力部分的電機(jī)已達(dá)到無(wú)人機(jī)的姿態(tài)調(diào)整；通過無(wú)線模塊與上位機(jī)通信實(shí)現(xiàn)飛行數(shù)據(jù)實(shí)時(shí)更新和飛行姿態(tài)的動(dòng)態(tài)顯示。

3 系統(tǒng)硬件設(shè)計(jì)

3.1 主控芯片

主控部分采用主頻為72 MHz基于ARM Cortex-M3內(nèi)核32位控制器STM32F103VCTE[6]，該控制器功耗低，運(yùn)算速度快，具有多路PWM信號(hào)輸出通道，同時(shí)兼具I2C、SPI、USART等通信方式，方便數(shù)據(jù)采集、控制無(wú)人機(jī)和與上位機(jī)之間的通信等。

3.2 電源設(shè)計(jì)

采用鋰電池供電，使用TI公司的TPS73633穩(wěn)壓芯片來產(chǎn)生穩(wěn)定系統(tǒng)所需要的電壓。

3.3 數(shù)據(jù)采集單元

傳感器對(duì)無(wú)人機(jī)的飛行姿態(tài)進(jìn)行數(shù)據(jù)采集，通過采集到的加速度、角速度、飛行高度和磁阻進(jìn)行姿態(tài)融合算法，從而控制飛行?？紤]到飛行系統(tǒng)的體積和重量要求，系統(tǒng)全部采用高集成度的MEMS器件。姿態(tài)傳感器采用nvensense公司的MPU-9250，該傳感器是融合了三軸加速度、三軸陀螺儀和三軸磁阻傳感器于一體的九軸運(yùn)動(dòng)傳感器[7]。此芯片也可自行配置量程，加速度可設(shè)置為±2、±4、±8、±16 g，角速度可設(shè)置為±250、±500、±1 000、±2 000°/s，磁阻滿量程為±4 800 μT。通信方式采用最高達(dá)到400 kHz的I2C通信。無(wú)人機(jī)的高度信息則采用MS5611MEMS氣壓傳感器提供，該傳感器響應(yīng)時(shí)間為1 ms、測(cè)量精度可達(dá)10 cm，可通過I2C與系統(tǒng)通信。

3.4 電機(jī)選取

作為無(wú)人機(jī)的動(dòng)力來源，該部分選用4個(gè)電子調(diào)速器驅(qū)動(dòng)的新西達(dá)2212型無(wú)刷直流電機(jī)，為提高系統(tǒng)穩(wěn)定性，防止無(wú)刷電機(jī)產(chǎn)生的沖擊電流在STM32的PWM輸出與電機(jī)驅(qū)動(dòng)間加入數(shù)字隔離器，電機(jī)由PWM進(jìn)行調(diào)速。

3.5 無(wú)線通信部分

無(wú)線通信采用SPI通信方式與MCU進(jìn)行數(shù)據(jù)傳輸、工作頻段在2.4～2.5 GHz的nRF24L01芯片，該芯片具有功耗低、信號(hào)穩(wěn)定、傳輸速度快的特點(diǎn)。

3.6 飛行器控制

無(wú)人機(jī)控制部分主要由姿態(tài)控制和位置控制兩部分組成，一般情況下姿態(tài)控制由無(wú)人機(jī)系統(tǒng)本身自動(dòng)完成，而位置控制則由遠(yuǎn)程遙控控制。姿態(tài)控制是無(wú)人機(jī)控制部分的重點(diǎn)，因?yàn)闊o(wú)人機(jī)的姿態(tài)變化可以導(dǎo)致位置變化。姿態(tài)和位置兩個(gè)環(huán)路控制如圖3所示。

圖3 姿態(tài)和位置環(huán)路控制

PID控制器憑借其對(duì)系統(tǒng)模型精確度要求不高和簡(jiǎn)單易懂等特點(diǎn)被廣泛應(yīng)用。無(wú)人機(jī)通過對(duì)4個(gè)無(wú)刷電機(jī)轉(zhuǎn)速的調(diào)節(jié)達(dá)到對(duì)飛行姿態(tài)的調(diào)節(jié)，飛行器俯仰、滾轉(zhuǎn)、偏航等飛行動(dòng)作由4個(gè)電機(jī)不同的轉(zhuǎn)速組合來實(shí)現(xiàn)。通過PID控制器在3個(gè)姿態(tài)控制通道進(jìn)行控制達(dá)到想要的控制功能[8]。由于本系統(tǒng)對(duì)動(dòng)態(tài)要求較高，所以采用串級(jí)PID控制器(PID控制器的一種改進(jìn))，控制結(jié)構(gòu)如圖4所示。

圖4 控制結(jié)構(gòu)

PID、PD控制器分別為主控制器和副控制器，分別實(shí)現(xiàn)對(duì)姿態(tài)角和角速度的變化趨勢(shì)進(jìn)行快速精確調(diào)整，使系統(tǒng)具有穩(wěn)定的飛行姿態(tài)控制性能和動(dòng)態(tài)擾動(dòng)下的控制能力。

在Simulink搭建仿真模型[9]，整定串級(jí)PID控制器參數(shù)，滾轉(zhuǎn)角和俯仰角的階躍響應(yīng)仿真曲線如圖5所示。

從圖5可以看出，串級(jí)PID具有很好的控制能力，能夠達(dá)到控制要求。

(a) 滾轉(zhuǎn)角階躍響應(yīng)曲線 (b) 俯仰角角階躍響應(yīng)曲線

4 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)流程如圖6所示。

圖6 系統(tǒng)軟件流程

5 姿態(tài)解算

姿態(tài)角測(cè)量和解算是控制系統(tǒng)的重要部分，無(wú)人機(jī)飛行姿態(tài)的實(shí)時(shí)性與穩(wěn)定性直接由解算速度和精度決定。由于陀螺儀存在溫漂特性和長(zhǎng)時(shí)間積分后累積的積分誤差，使得數(shù)據(jù)精度在短時(shí)間內(nèi)較高；飛行過程中產(chǎn)生影響加速度計(jì)的震動(dòng),磁阻傳感器易受外部磁場(chǎng)干擾，二者的靜態(tài)誤差不隨時(shí)間變化，應(yīng)采用長(zhǎng)時(shí)間姿態(tài)角采集,而不宜采用短期測(cè)量。所以采用四元數(shù)互補(bǔ)濾波算法對(duì)傳感器采集的數(shù)據(jù)進(jìn)行融合，提高飛行控制精度[10]。

首先通過四元數(shù)互補(bǔ)濾波法將傳感器檢測(cè)到的數(shù)據(jù)進(jìn)行融合，然后利用向量叉積法求得姿態(tài)誤差，最后由一階龍格庫(kù)塔法更新數(shù)據(jù)，得到最終精確的飛行器姿態(tài)值。由于歐拉角參數(shù)表示法物理意義明確、簡(jiǎn)單明了，而四元數(shù)法計(jì)算簡(jiǎn)單方便，所以采用歐拉角和四元數(shù)法結(jié)合使用表達(dá)無(wú)人機(jī)的飛行姿態(tài)。由陀螺儀得到歐拉角更新四元數(shù)矩陣為：

對(duì)陀螺儀積分獲取姿態(tài)角存在的誤差，由加速度計(jì)和電子羅盤得到偏差E1和E2的和對(duì)陀螺儀x、y、z三軸的誤差E(ex,ey,ez)進(jìn)行校正[11]，其叉積求解如下：

由E和式(4)的叉積誤差對(duì)陀螺儀的三軸角速度值進(jìn)行PI修正:

其中式(5)為積分修正，式(6)為比例積分修正。

四元數(shù)法積分校正原理如圖7所示。

圖7 四元數(shù)法積分校正原理

其中,a為加速度計(jì)實(shí)測(cè)矢量，m為數(shù)字羅盤實(shí)測(cè)矢量，a1為四元數(shù)轉(zhuǎn)換后的加速度向量，m1為四元數(shù)轉(zhuǎn)換后磁場(chǎng)向量，E為實(shí)測(cè)矢量與四元數(shù)轉(zhuǎn)換矢量的誤差值，T為誤差校正更新時(shí)間常數(shù)。

由一階龍格庫(kù)塔法對(duì)四元數(shù)互補(bǔ)法積分校正后的精確角速度值姿態(tài)進(jìn)行更新如下:

為方便下次更新，將更新后四元數(shù)規(guī)范化表示如下:

四元數(shù)互補(bǔ)濾波法獲得精確的姿態(tài)值實(shí)現(xiàn)了無(wú)人機(jī)的穩(wěn)定控制，將四元數(shù)轉(zhuǎn)換為歐拉角，用歐拉角表示無(wú)人機(jī)姿態(tài)，如下：

6 實(shí)驗(yàn)結(jié)果與分析

上位機(jī)顯示的飛行姿態(tài)如圖8所示(左側(cè)顯示PWM控制量，右側(cè)顯示姿態(tài)角和更新的數(shù)據(jù))。

圖8 四旋翼姿態(tài)顯示界面

使用Matlab根據(jù)懸停實(shí)驗(yàn)測(cè)得的數(shù)據(jù)繪制的無(wú)人機(jī)在三維空間懸停位置如圖9所示。

圖9 四旋翼懸停坐標(biāo)位置圖

采樣周期和樣本分別取20 ms和100個(gè)。其中，X,Y,Z分別為左右、前后和高低。分析實(shí)驗(yàn)結(jié)果可知,姿態(tài)角控制精度為3°到5°，懸停位置的最大誤差不大于5 cm。

7 結(jié) 語(yǔ)

基于STM32主控芯片設(shè)計(jì)了無(wú)人機(jī)飛行控制系統(tǒng)。由多傳感器進(jìn)行飛行姿態(tài)數(shù)據(jù)采集，然后控制器對(duì)采集到的數(shù)據(jù)進(jìn)行處理，由四元數(shù)互補(bǔ)濾波法進(jìn)行數(shù)據(jù)融合得到穩(wěn)定精確的姿態(tài)角，通過串級(jí)PID控制算法進(jìn)行電機(jī)調(diào)整量計(jì)算，通過微處理器輸出的PWM實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速的控制，達(dá)到調(diào)整無(wú)人機(jī)飛行姿態(tài)的目的。懸停實(shí)驗(yàn)和橫滾角、俯仰角的階躍響應(yīng)仿真曲線表明,該系統(tǒng)飛行姿態(tài)穩(wěn)定，達(dá)到了對(duì)無(wú)人機(jī)飛行姿態(tài)控制的要求和效果。

[1] 劉方滔.多傳感器融合的四旋翼飛行器關(guān)鍵技術(shù)研究[D].天津:天津科技大學(xué)，2011.

[2] 代君，任淑紅，王曉璐.小型無(wú)人機(jī)姿態(tài)航向參考系統(tǒng)信息融合算法[J].長(zhǎng)春工業(yè)大學(xué)學(xué)報(bào)，2016，37(1)：52-55.

[3] 李秀英，劉彥博.基于PWM的四旋翼飛行器控制方法[J].吉林大學(xué)學(xué)報(bào)，2011，29:464-472.

[4] 盧秀和，李建波.基于STM32的四旋翼姿態(tài)控制系統(tǒng)[J].計(jì)算機(jī)測(cè)量與控制，2014,22(3):761-763，772.

[5] 杜杉杉，吳昊，張繼文，等.一種面向AHRS的改進(jìn)補(bǔ)濾波融合算法[J].國(guó)外電子測(cè)量技術(shù)，2015(3)：13-18.

[6] 劉火良,楊森.STM32庫(kù)開發(fā)實(shí)戰(zhàn)指南[M].北京:機(jī)械工業(yè)出版社,2013:1-496.

[7] 鄭健.基于9軸傳感器的姿態(tài)參考系統(tǒng)研究與實(shí)現(xiàn)[D].成都:電子科技大學(xué)，2013.

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

[9] 潘海珠.四旋翼無(wú)人機(jī)自適應(yīng)導(dǎo)航控制[J].計(jì)算機(jī)仿真，2012，29(5):98-102.

[10] 張浩，任芊.四旋翼飛行器航姿測(cè)量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動(dòng)化，2013，32(1)：28-31.

[11] 廖懿華，張鐵民，廖貽泳.基于模糊-比例積分偏差修正的多旋翼飛行器姿態(tài)測(cè)算系統(tǒng)[J]．農(nóng)業(yè)工程學(xué)報(bào)，2014,20：19-27.

ThesystemdesignofsmallunmannedaerialvehicleflightattitudecontrolbasedonSTM32

CHENG Fangxiao, WANG Haibiao, LIU Lu

(School of Electrical & Electronic Engineering, Changchun University of Technology, Changchun 130012， China)

The quaternions complementary filter algorithm is used for data fusion, which are sampled from IMU unit and other sensors. Cascade PID algorithm is applied into the Unmanned Aerial Vehicle (UAV) pose control system with a real-time monitor. Experimental results indicate that the cascade PID algorithm based on STM32 is effective for the UAV pose control.

Unmanned Aerial Vehicle (UAV)； pose； STM32； monitor.

2017-07-28

吉林省科技廳基金資助項(xiàng)目(20170204038SF)

程方曉(1969-)，女，漢族，吉林長(zhǎng)春人，長(zhǎng)春工業(yè)大學(xué)副教授，博士，主要從事測(cè)控技術(shù)與智能系統(tǒng)方向研究,E-mail:chengfangxiao@ccut.edu.cn.

10.15923/j.cnki.cn22-1382/t.2017.5.08

TP 273

A

1674-1374(2017)05-0453-06