楊洪亞　裴文卉

摘 要：近些年微機(jī)電系統(tǒng)（MEMS）的快速發(fā)展，現(xiàn)代控制的理論算法在四旋翼飛行器控制得到廣范運(yùn)用。文章從四個(gè)部分對(duì)四旋翼飛行器的硬件設(shè)計(jì)與控制算法進(jìn)行研究，首先分析飛行器原理與結(jié)構(gòu)，建立動(dòng)力學(xué)方程模型，然后對(duì)主控電路STM32F411CE和發(fā)射、接收電路NRF51822+RFX2401C的設(shè)計(jì)進(jìn)行介紹，之后利用卡爾曼濾波算法將獲取MPU9250（加速度計(jì)和陀螺儀）的姿態(tài)數(shù)據(jù)進(jìn)行融合，設(shè)計(jì)出一款超高性能PID控制器，最后利用數(shù)學(xué)工具M(jìn)ATLAB/Simulink對(duì)濾波前后的信號(hào)波形進(jìn)行比較，突顯卡爾曼濾波算法先進(jìn)性。

關(guān)鍵詞：動(dòng)力學(xué)方程；NRF51822+RFX2401C；卡爾曼濾波算法； MATLAB/Simulink

中圖分類號(hào)：V249.1 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：2095-2945（2018）17-0034-05

Abstract： In recent years， with the rapid development of Micro Electromechanical System （MEMS）， modern control theory and algorithm have been widely used in the control of four-rotor aircraft. In this paper， the hardware design and control algorithm of the four-rotor aircraft is studied in four parts. Firstly， the principle and structure of the vehicle are analyzed， and the dynamic equation model is established. Then the design of main control circuit STM32F411CE and transmitting and receiving circuit NRF51822 + RFX2401C is introduced. Then the attitude data obtained from MPU9250 （accelerometer and gyroscope） are fused by Kalman filter algorithm， and a super-high performance PID controller is designed. Finally， MATLAB/Simulink is used to compare the signal waveforms before and after filtering， which highlights the advancement of Kalman filtering algorithm.

Keywords： kinetic equation； NRF51822+RFX2401C； Kalman filter algorithm； MATLAB/Simulink

四旋翼飛行器因具備體積小，質(zhì)量輕，結(jié)構(gòu)簡單，可懸停和垂直起降等優(yōu)點(diǎn)，可用于國防軍事，海洋執(zhí)法監(jiān)測(cè)和民用生活航拍領(lǐng)域。本文從工程設(shè)計(jì)角度建立卡爾曼狀態(tài)方程和測(cè)量方程，將姿態(tài)角度進(jìn)行精密解算來對(duì)電機(jī)精確控制，實(shí)現(xiàn)空中翻轉(zhuǎn)、懸停等。

1 四旋翼飛行器的結(jié)構(gòu)與原理

四旋翼飛行器模型采用十字交叉式，由四個(gè)直流電機(jī)驅(qū)動(dòng)風(fēng)葉組成的六自由度，欠驅(qū)動(dòng)系統(tǒng)。因存在旋轉(zhuǎn)力矩與平衡系統(tǒng)的耦合問題，傳統(tǒng)的直升機(jī)通過借助尾槳調(diào)節(jié)，來平衡反扭矩， 因此四旋翼飛行器在設(shè)計(jì)時(shí)需要考慮多變量，非線性系統(tǒng)的問題。工作原理如下：

1.1 垂直上升、下降運(yùn)動(dòng)：飛行器自穩(wěn)后，電機(jī)1，2，3，4號(hào)保持同樣速度，同時(shí)加速或減速，飛行器會(huì)發(fā)生垂直上升或下降運(yùn)動(dòng)。如圖1所示。

1.2 俯仰、橫滾（前后、左右）運(yùn)動(dòng)：飛行器自穩(wěn)后，1號(hào)電機(jī)增速（減速），3號(hào)電機(jī)減速（加速），同時(shí)保持其他兩個(gè)電機(jī)轉(zhuǎn)速不變，這樣機(jī)體會(huì)存在拉力差，使機(jī)身前后傾斜，拉力產(chǎn)生水平分量，使機(jī)體向前運(yùn)動(dòng)。如圖2所示，同理，同樣的方法作用到2號(hào)、4號(hào)電機(jī)會(huì)發(fā)發(fā)生橫滾（左右）運(yùn)動(dòng)，如圖3所示

1.3 偏航運(yùn)動(dòng)：增大（減小）1號(hào)、3號(hào)電機(jī)，減?。ㄔ龃螅?號(hào)、4號(hào)電機(jī)轉(zhuǎn)速，機(jī)體會(huì)發(fā)生順時(shí)針偏航運(yùn)動(dòng)，反之為逆時(shí)針偏航運(yùn)動(dòng)。如圖4所示。

2 運(yùn)動(dòng)方程模型

為獲得四旋翼飛行器數(shù)學(xué)模型，我們可以假定a.四旋翼飛行器模型是均勻?qū)ΨQ的剛體；b.飛行器各個(gè)方向的拉力與推進(jìn)器轉(zhuǎn)速的平方成正比。設(shè)立兩個(gè)基坐標(biāo)參考系：慣性坐標(biāo)系E（OXYZ）和飛行器坐標(biāo)系B（OXYZ），如圖5所示。首先設(shè)定歐拉角？啄=[？準(zhǔn) ？茲 ？鬃]T，將飛行器經(jīng)過三次簡單轉(zhuǎn)動(dòng)得到模型的三個(gè)姿態(tài)角依次為？鬃→？茲→？準(zhǔn)。這三次旋轉(zhuǎn)分別得到從機(jī)體坐標(biāo)系到地面坐標(biāo)系XYZ軸的轉(zhuǎn)換矩陣如下：

3 硬件電路設(shè)計(jì)

3.1 飛行器硬件系統(tǒng)主要組成

系統(tǒng)電路主要包括氣壓計(jì)定高模塊MS5611-01BA，無線收發(fā)模塊，陀螺加速度計(jì)模塊MPU9250，核心MCU STM32F411電路，空心杯直流電機(jī)等幾個(gè)重要模塊組成。

3.2 主控電路最小系統(tǒng)組成

本設(shè)計(jì)中采用ST公司的STM32F411RE最為主控芯片，該芯片是Cortex-M4內(nèi)核，帶FPU（Float Point Unit，源點(diǎn)運(yùn)算單元）和DSP（Digital Signal Processing，數(shù)字信號(hào)處理）指令集，主頻可達(dá)到168MHz（可獲得210DMIPS的處理能力，并且擁有128KB的SRAM（Static Random Access Memory，靜態(tài)隨機(jī)存取存儲(chǔ)器，帶攝像頭DCMI接口。還擁有ART（Adaptive Real-Time Accelerator，自適應(yīng)實(shí)施加速器），可以達(dá)到相當(dāng)于Flash零等待周期的性能。電路如圖6所示。

芯片采用雙晶振設(shè)計(jì)，8M晶振經(jīng)過鎖相環(huán)可到180MHz，RTC實(shí)時(shí)時(shí)鐘采用32.768MHz晶振。

3.3 通訊電路設(shè)計(jì)

本文飛行器通訊電路采用NORDIC公司的低功耗藍(lán)牙芯片Nrf51822，該款芯片是32位ARM Cortex-M0，有256KB FLASH存儲(chǔ)，16KB RAM存儲(chǔ)，睡眠模式功耗電流僅為 0.4uA，正常工作電壓范圍為1.8-3.6V，最高輸出功率為4dbm，靈敏度為92.5dBm與RFX2401C結(jié)合可將發(fā)射距離擴(kuò)大到100m。如圖7所示。

3.4 PCB設(shè)計(jì)

PCB設(shè)計(jì)采用四層板設(shè)計(jì)，材料選用FR-4，在RF 射頻電路設(shè)計(jì)時(shí)采用50歐阻抗匹配，通過PCB參數(shù)計(jì)算工具Saturn PCB Design Toolkit 如圖8所示。

差分電路阻抗匹配為100Ω，PCB層間距如圖9所示。

用Altium Designer軟件設(shè)計(jì)出的PCB如圖10所示。

4 算法部分-卡爾曼濾波器的PID控制

擴(kuò)展卡爾曼濾波算法是一種高效的遞推算法，它被廣泛應(yīng)用在工業(yè)和科研領(lǐng)域中，特別是飛行器控制中，具有很高的姿態(tài)解算精度。建立狀態(tài)方程，通過對(duì)系統(tǒng)狀態(tài)進(jìn)行估算得到下一時(shí)刻的姿態(tài)角。卡爾曼濾波器的設(shè)計(jì)要求系統(tǒng)模型為離散模型。定義的線性狀態(tài)方程狀態(tài)空間模型為：

X（k）=AX（k-1）+BU（k）+W（k）

式中：X（k）為當(dāng)前系統(tǒng)狀態(tài)的估算值，A為前一狀態(tài)X（k）和后一狀態(tài)X（k+1）的狀態(tài)轉(zhuǎn)移矩陣，U（k）為k時(shí)刻控制輸入；B為輸入控制，W（k）為k時(shí)刻系統(tǒng)的過程噪聲及系統(tǒng)誤差，當(dāng)W（k）無限逼近于0，意味著過程噪聲W（k）為高斯白噪聲N（0，Qk），Qk為過程噪聲的協(xié)方差矩陣，反之，W（k）為有色噪聲。

觀測(cè)方程即系統(tǒng)輸出方程為 z（k）=HX（k）+V（k）

式中：H為測(cè)量相關(guān)狀態(tài)矩陣；z（k）為k時(shí)刻的觀測(cè)值，當(dāng)加速度計(jì)的觀測(cè)噪聲V（k）逼近于0，意味著高斯白噪聲N（0，Rk），Rk為觀測(cè)噪聲協(xié)方差矩陣，反之，V（k）為有色噪聲。

基于卡爾曼濾波的PID控制結(jié)構(gòu)圖如圖11所示。

建立狀態(tài)空間模型為：

xk=f（xk-1'+uk+wk）Zk=h（xk，Vk）

其中W（k）和V（k）均為隨機(jī)信號(hào)服從正態(tài)分布：

p（w）～N（0，Qk）p（v）～N（0，Rk）

協(xié)方差系數(shù)Rk與Qk是影響濾波效果和響應(yīng)速度的重要參數(shù)，Rk取值越小，濾波響應(yīng)和收斂越快；Qk取值越小，抑制、濾除噪聲的能力就越強(qiáng)。

卡爾曼濾波的基本公式為：

（1）狀態(tài)預(yù)測(cè)方程：由系統(tǒng)狀態(tài)變量k-1時(shí)刻的最優(yōu)值X（k-1|k-1）和系統(tǒng)輸入U(xiǎn)（k）可以求出k時(shí)刻系統(tǒng)預(yù)測(cè)值X（k|k-1）為

X（k|k-1）=AX（k-1|k-1）+BU（k）

（2）協(xié)方差預(yù)測(cè)方程：根據(jù)k-1時(shí)刻系統(tǒng)協(xié)方差矩陣 預(yù)測(cè)k時(shí)刻系統(tǒng)協(xié)方差矩陣P（k|k-1）為

P（k|k-1）=AP（k-1|k-1）AT+Q

（3）卡爾曼增益計(jì)算方程：根據(jù)狀態(tài)變量預(yù)測(cè)值和協(xié)方差矩陣預(yù)測(cè)值計(jì)算卡爾曼增益Kg（k）為

Kg（k）=P（k|k-1）HT/（HP（k|k-1）HT+R）

（4）最優(yōu)值更新方程：由狀態(tài)變量預(yù)測(cè)值和系統(tǒng)測(cè)量值計(jì)算k時(shí)刻狀態(tài)變量最優(yōu)值X（k|k）為

X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））

（5）協(xié)方差更新方程：更新k時(shí)刻協(xié)方差矩陣P（k|k）為

P（k|k）=（1-Kg（k）H）P（k|k-1）

最優(yōu)值和協(xié)方差更新方程都計(jì)算結(jié)束后，開始進(jìn)行下一輪循環(huán)，往復(fù)（1）-（5）步。遞推算法的本質(zhì)體現(xiàn)在估計(jì)過程，每次只需根據(jù)以前的測(cè)量值遞歸計(jì)算，就能得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)?？柭鼮V波器的工作流程如圖12所示。

用MATLAB/Simulink仿真結(jié)果，如圖13所示。

5 結(jié)束語

本文通過對(duì)四旋翼飛行器硬件電路設(shè)計(jì)以及姿態(tài)傳感器獲取姿態(tài)角的數(shù)據(jù)解算，建立飛行器的數(shù)學(xué)模型，利用估計(jì)后的姿態(tài)角設(shè)計(jì)PID控制器實(shí)現(xiàn)對(duì)飛行器角度擾動(dòng)的糾正。利用MATLAB仿真和分析，驗(yàn)證卡爾曼濾波算法在飛行器控制應(yīng)用中可以極大減小誤差。

參考文獻(xiàn)：

[1]唐建杰，王鑫.基于PI-PD控制器的四旋翼姿態(tài)控制[J].電子技術(shù)與軟件工程，2014.

[2]張棟，焦嵩鳴，劉延泉.互補(bǔ)濾波和卡爾曼濾波的融合姿態(tài)解算方法[J].傳感器與微系統(tǒng)2017，36（3）：62-66.

[3]馮新宇，范紅剛，辛亮.四旋翼無人飛行器設(shè)計(jì)[M].北京：清華大學(xué)出版社，2017：20.

[4]張帥.四旋翼飛行器穩(wěn)定性的非線性控制[D].上海：上海電力學(xué)院，2017.

[5]王偉.四旋翼飛行器姿態(tài)控制系統(tǒng)設(shè)計(jì)[J].科學(xué)技術(shù)與工程，2013，13（19）：5514-5519.

[6]歷小偉，郭玉英.四旋翼飛行器的動(dòng)力學(xué)建模與飛行控制[J].自動(dòng)化與儀器儀表，2017，01：130-132.

[7]楊廣杰，白寶明，李凱.四旋翼無人飛行器飛行控制系統(tǒng)設(shè)計(jì)[D].西安：西安電子科技大學(xué)，2013.

[8]張麗娟，孫戈.四旋翼飛行器的姿態(tài)角估計(jì)與控制[D].西安：西安科技大學(xué)，2016.

[9]胡琦逸，王建中，鄒洪波.四旋翼飛行器的姿態(tài)估計(jì)與優(yōu)化控制研究[D].杭州：杭州電子科技大學(xué)，2013.