基于FPGA和ARM的飛行器控制系統(tǒng)設(shè)計(jì)

范君健，吳國東，王志軍

（中北大學(xué) 機(jī)電工程學(xué)院，太原 030051）

隨著科學(xué)技術(shù)的不斷進(jìn)步，無人機(jī)無論是在軍事還是民用領(lǐng)域都有著廣闊的應(yīng)用前景。近年來，對(duì)無人機(jī)的研究越來越得到各國研究機(jī)構(gòu)與科技公司的重視，在軍事偵察、空中拍攝、交通監(jiān)控及自然災(zāi)害勘察等領(lǐng)域，小型無人機(jī)由于其體積小巧、使用靈活的特點(diǎn)，將逐步取代人工成為主要的信息源。然而，無人機(jī)的核心控制系統(tǒng)不可避免的要涉及到飛行器的姿態(tài)、速度、位置這幾個(gè)方面的控制運(yùn)算，因此對(duì)于控制器的運(yùn)算能力有很高的要求[1]。

現(xiàn)階段常見的控制系統(tǒng)一般采用ARM、DSP等高速高性能處理器作為控制芯片。對(duì)于單控制器飛控系統(tǒng)，要在指令周期內(nèi)完成無人機(jī)姿態(tài)數(shù)據(jù)采集、計(jì)算控制量并輸出到執(zhí)行機(jī)構(gòu)、接收地面控制指令以及回傳位置信息等操作[2]，因此對(duì)于控制系統(tǒng)的可靠性提出了嚴(yán)峻的挑戰(zhàn)。針對(duì)這一問題，本文設(shè)計(jì)了一種基于FPGA和STM32的飛行控制系統(tǒng)，使用FPGA作為數(shù)據(jù)采集與處理單元，使用STM32芯片作為控制、通訊以及人機(jī)交互單元。由于FPGA并行處理的特點(diǎn)，對(duì)于多個(gè)傳感器的數(shù)據(jù)可以實(shí)現(xiàn)同步采集，能夠更加精確地實(shí)現(xiàn)無人機(jī)的姿態(tài)與位置計(jì)算，保證了控制系統(tǒng)的高效穩(wěn)定。同時(shí)STM32由于其低廉的成本、靈活多樣的接口設(shè)置以及高效的開發(fā)流程，對(duì)于數(shù)據(jù)的存儲(chǔ)與傳輸、人機(jī)交互、操作系統(tǒng)等的設(shè)計(jì)提供了極大的便利。

本文首先介紹了控制系統(tǒng)的總體方案，然后對(duì)關(guān)鍵部分進(jìn)行詳細(xì)分析與驗(yàn)證。通過Matlab設(shè)計(jì)算法，并對(duì)其進(jìn)行仿真和FPGA的硬件實(shí)現(xiàn)，設(shè)計(jì)FPGA與STM32通訊接口，實(shí)現(xiàn)數(shù)據(jù)的高速傳輸。

1 四旋翼飛行器整體設(shè)計(jì)

該設(shè)計(jì)中使用的FPGA采用Altera公司Cyclone IV E系列的EP4CE10F17I7N，ARM芯片采用ST公司的STM32F407系列，STM32與FPGA通過可變靜態(tài)存儲(chǔ)控制器FSMC（flexible static memory controller）接口連接，并加入256 MB的SDRAM作為數(shù)據(jù)緩沖器。FPGA部分有陀螺儀傳感器、三軸加速度傳感器、地磁傳感器、氣壓傳感器，STM32部分有GPS模塊、遙控器無線模塊以及SD卡存儲(chǔ)模塊。系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System block diagram

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

2.1 系統(tǒng)硬件選型

模擬量傳感器三個(gè)單軸陀螺儀（ENC-03MB）焊接為 X、Y、Z三個(gè)方向，三軸加速度傳感器（ADXL335）。模擬量傳感器可以高速而精確地進(jìn)行數(shù)據(jù)采集，配合高精度多通道AD芯片AD7606，滿足姿態(tài)控制的頻率要求。

數(shù)字量傳感器電子羅盤（HMC5983），氣壓傳感器（MS5611）。所選數(shù)字量傳感器使用相對(duì)簡單，便于數(shù)據(jù)處理，同時(shí)滿足姿態(tài)解算的速度要求。

無線收發(fā)模塊選用 nRF905，GPS模塊選用NEO-6M，二者均連接到STM32，使得開發(fā)流程高效有序。

數(shù)據(jù)存儲(chǔ)模塊SDRAM選用MT48LC16M16A2，容量256 MB，其含有16位數(shù)據(jù)總線，4個(gè)Bank，存儲(chǔ)架構(gòu)為 4 Banks×4 Mbits×16。 加入 SD 卡模塊，用于存儲(chǔ)飛行數(shù)據(jù)，便于上位機(jī)對(duì)飛行過程進(jìn)行回放。

2.2 接口電路設(shè)計(jì)

FSMC是STM32系列采用的一種新型的存儲(chǔ)器擴(kuò)展技術(shù)，支持不同的外部存儲(chǔ)器類型，其中包括多種類型的靜態(tài)存儲(chǔ)器[3]，因此，可將FPGA當(dāng)作STM32F407的外部SRAM來配置，通過擴(kuò)展出的數(shù)據(jù)/地址/控制三總線來實(shí)現(xiàn)操作，這樣既能保證較快的操作速度，又具有很高的靈活性[4]。接口硬件連接如圖2所示。

圖2 STM32與FPGA連接原理Fig.2 STM32 and FPGA connection schematics

將FPGA直接掛載到STM32的FSMC總線上，F(xiàn)SMC提供了 4個(gè)Bank用于連接不同的外部存儲(chǔ)器，F(xiàn)SMC_NE1為Bank1第一區(qū)的片選信號(hào)，F(xiàn)SMC_NOE和FAMC_NWE分別為接口讀寫信號(hào)，根據(jù)設(shè)計(jì)的數(shù)據(jù)要求，設(shè)計(jì)數(shù)據(jù)位寬16位，地址位寬 10 位，將數(shù)據(jù)線 AD［15：0］，地址線 A［25：16］連接到FPGA的I/O端口，還包括一些中斷信號(hào)FSMC_NBL［0：1］，用于 FPGA 向 STM32 發(fā)送中斷請(qǐng)求。這樣FPGA就作為STM32的外設(shè)接入，通過存儲(chǔ)器讀寫指令訪問FPGA。

在FPGA的數(shù)據(jù)接口設(shè)計(jì)當(dāng)中，AD芯片的數(shù)據(jù)采集與FSMC總線的數(shù)據(jù)傳輸處于不同的時(shí)鐘域，而數(shù)據(jù)在不同時(shí)鐘域之間傳遞，容易產(chǎn)生亞穩(wěn)態(tài)[5]。通過IP核設(shè)計(jì)異步FIFO用于數(shù)據(jù)的緩存與傳輸，采用2個(gè)時(shí)鐘信號(hào)來控制其讀寫操作，分別為寫時(shí)鐘（wrclk）和讀時(shí)鐘（rdclk）。FIFO的模塊框圖如圖3所示。其中異步FIFO模塊包括8個(gè)端口[6]，分別為數(shù)據(jù)輸入（data［15：0］）、寫請(qǐng)求（wrreq）、寫時(shí)鐘（wrcl-k）、讀請(qǐng)求（rdreq）、讀時(shí)鐘（rdclk）、緩沖器滿信號(hào)（wrfull）、緩沖器空信號(hào)（rdempty）、數(shù)據(jù)輸出（q［15：0］）。

圖3 異步FIFO的BlockFig.3 Asynchronous FIFO Block

2.3 數(shù)字濾波器設(shè)計(jì)

陀螺儀與加速度計(jì)均為模擬量傳感器，其擁有較高的頻率響應(yīng)和高精度的測(cè)量數(shù)據(jù)，但其不可避免的會(huì)將電路中的噪聲信號(hào)一同送往ADC芯片進(jìn)行模數(shù)轉(zhuǎn)換，這些噪聲在數(shù)字系統(tǒng)的運(yùn)算當(dāng)中可能會(huì)被放大，影響測(cè)量精度，因此在FPGA內(nèi)部創(chuàng)建數(shù)字濾波器，以進(jìn)一步提高信噪比。

相比于完全依靠電阻、電容、晶體管等電子元件組成的模擬濾波器，數(shù)字濾波器具有更高精度、更高信噪比、無可比擬的可靠性等優(yōu)點(diǎn)，雖然在其搭建過程當(dāng)中工作量大、調(diào)試設(shè)計(jì)復(fù)雜，但其換來的靈活性與可擴(kuò)展性可大大降低硬件電路板的設(shè)計(jì)及制作成本[7]。有限脈沖響應(yīng)FIR（finite impulse response）濾波器由于其具有良好的線性相位和極高的穩(wěn)定性，在數(shù)字信號(hào)處理領(lǐng)域得到了極為廣泛的應(yīng)用[8]。n階FIR濾波器的輸出y（n）可表示為輸入序列 x（n）與單位取樣響應(yīng) h（n）的線性卷積，如式（1）所示：

每一次的輸出要完成N次的乘法和N-1次的加法，當(dāng)階數(shù)N較大時(shí)，系統(tǒng)延遲會(huì)很大。

根據(jù)傳感器的輸出特性和系統(tǒng)的數(shù)據(jù)傳輸需求，同時(shí)借助FPGA的高度并行和線性處理高速信號(hào)的特點(diǎn)，設(shè)計(jì)截止頻率為20 kHz的31階（長度為32）低通線性相位FIR濾波器，采樣頻率100 kHz，量化系數(shù)為16比特。為提高設(shè)計(jì)效率和系統(tǒng)可靠性，首先通過Matlab設(shè)計(jì)出濾波器系數(shù)，而后將其導(dǎo)入到Quartus II包含的FIR IP核當(dāng)中，直接生成FIR模塊。為獲得更高的運(yùn)算速度，選擇全并行分布式算法結(jié)構(gòu)[9]，F(xiàn)IR濾波器系數(shù)設(shè)置如表1所示，F(xiàn)IR濾波器系數(shù)量化前后頻譜如圖4所示。

表1 FIR濾波器1～16階系數(shù)設(shè)置Tab.1 FIR filter 1～16 order coefficient setting

圖4 FIR濾波器系數(shù)量化前后的幅頻響應(yīng)Fig.4 FIR filter coefficients are quantized before and after quantization

采用Matlab軟件仿真出具有白噪聲特性的輸入信號(hào)，以及由10 kHz和30 kHz單頻信號(hào)疊加的輸入信號(hào)，進(jìn)而仿真測(cè)試數(shù)據(jù)經(jīng)濾波器濾波后的輸出數(shù)據(jù)，以便和FPGA設(shè)計(jì)實(shí)現(xiàn)后的結(jié)果進(jìn)行比較。Maltab仿真的信號(hào)濾波前后頻譜如圖5所示。

在FPGA設(shè)計(jì)完成后，編寫Testbench文件，將仿真的輸入信號(hào)作為ModelSim的激勵(lì)源，生成經(jīng)過FPGA濾波器的輸出信號(hào)，對(duì)輸出信號(hào)數(shù)據(jù)進(jìn)行時(shí)域及頻域分析，如圖6和圖7所示。

從圖6可以看出，F(xiàn)PGA實(shí)現(xiàn)的濾波器輸入、輸出頻譜與Matlab直接仿真的結(jié)果相同。從圖7可以看出，濾波后的白噪聲在時(shí)域上的變化趨勢(shì)明顯緩和，即濾除了高頻分量，濾波后的合成單頻信號(hào)已形成規(guī)則的頻率為10 kHz的信號(hào)。因此，從仿真結(jié)果看，該濾波器的FPGA實(shí)現(xiàn)滿足設(shè)計(jì)要求。

圖5 Maltab仿真的信號(hào)濾波前后頻譜圖Fig.5 Matlab simulation of signal filtering before and after spectrum

圖6 FPGA仿真的信號(hào)濾波前后頻譜圖Fig.6 FPGA simulation of the signal before and after filtering spectrum

圖7 FPGA仿真的信號(hào)濾波前后的時(shí)域波形Fig.7 FPGA simulation of the signal before and after the time domain waveform

3 軟件算法設(shè)計(jì)

3.1 飛行器姿態(tài)解算

該設(shè)計(jì)所選用的傳感器都可以單獨(dú)完成姿態(tài)角度的測(cè)量，但陀螺儀存在溫度漂移誤差，加速度計(jì)受機(jī)身振蕩會(huì)產(chǎn)生測(cè)量白噪聲，電子羅盤易受外部磁場(chǎng)的干擾，因此單個(gè)傳感器很難得到相對(duì)準(zhǔn)確的姿態(tài)角信息[10]，為實(shí)時(shí)準(zhǔn)確地獲得飛行器的姿態(tài)角，采用卡爾曼濾波算法，將傳感器所測(cè)數(shù)據(jù)進(jìn)行融合，解算飛行器3個(gè)軸向的姿態(tài)角信息[11-12]。

卡爾曼濾波對(duì)數(shù)據(jù)的處理包括預(yù)測(cè)和更新2個(gè)過程。預(yù)測(cè)過程是通過前一時(shí)刻的狀態(tài)估算現(xiàn)在的狀態(tài)，更新過程是通過前一時(shí)刻的預(yù)測(cè)值和現(xiàn)在的觀測(cè)值確定狀態(tài)估測(cè)。不同的姿態(tài)估算方法具有不同的性能評(píng)估標(biāo)準(zhǔn)，卡爾曼濾波是以系統(tǒng)的最小協(xié)方差為標(biāo)準(zhǔn)。對(duì)于四旋翼飛行器系統(tǒng)，將系統(tǒng)連續(xù)信號(hào)離散化為計(jì)算機(jī)能處理的離散信號(hào)，令系統(tǒng)的采樣周期為T，離散系統(tǒng)對(duì)應(yīng)的狀態(tài)方程X（k）和測(cè)量方程 V（k）如式（2）所示：

式中：ωgyro為陀螺儀輸出的角速度；ωg為陀螺儀的測(cè)量白噪聲；νa為加速度計(jì)和電子羅盤的測(cè)量白噪聲。

需根據(jù)k-1時(shí)刻的角度值來估算k時(shí)刻的實(shí)際角度，再根據(jù)預(yù)測(cè)得到的k時(shí)刻的角度值得到k時(shí)刻的高斯噪聲的方差，在此基礎(chǔ)上卡爾曼濾波器進(jìn)行遞歸運(yùn)算直至估算出最優(yōu)的角度值。系統(tǒng)的過程噪聲協(xié)方差矩陣Q以及測(cè)量誤差的協(xié)方差矩陣R 如式（3）所示：

式中：q_acce和q_gyro分別為加速度計(jì)和陀螺儀的協(xié)方差。該系統(tǒng)中陀螺儀的值更接近準(zhǔn)確值，因此取q_gyro的值小于q_acce的值。

預(yù)測(cè)狀態(tài)方程：

更新狀態(tài)方程：

式中：X（k|k-1）為上一時(shí)刻狀態(tài)預(yù)測(cè)值；X（k-1|k-1）為k-1時(shí)刻的最優(yōu)值；X（k|k）為當(dāng)前k時(shí)刻的最優(yōu)估算值；P（k|k-1）為狀態(tài) X（k|k-1）的協(xié)方差；P（k|k）為狀態(tài)X（k|k）的協(xié)方差；K為卡爾曼增益；I為單位陣計(jì)算完更新方程后，再次重復(fù)上一次的后驗(yàn)估計(jì)，作為下一次計(jì)算的先驗(yàn)估計(jì)，這樣不斷地循環(huán)運(yùn)算直至找到最優(yōu)結(jié)果。

3.2 飛行器PD控制設(shè)計(jì)

四旋翼飛行器的動(dòng)力學(xué)模型特點(diǎn)為具有多入多出，帶有強(qiáng)耦合的欠驅(qū)動(dòng)系統(tǒng)，其動(dòng)力學(xué)型表示如式（9）所示，式中［φ，θ，ψ］為飛行器的 3 個(gè)姿態(tài)的歐拉角，分別代表滾轉(zhuǎn)、俯仰和偏航；［x，y，z]為飛行器質(zhì)心在慣性坐標(biāo)系中的位置；l為飛行器每個(gè)旋翼末端到飛行器重心的距離；m為飛行器的負(fù)載總質(zhì)量；Ii為圍繞每個(gè)軸的轉(zhuǎn)動(dòng)慣量；Ki為阻力系數(shù)。由于欠驅(qū)動(dòng)特性的存在，不可能對(duì)所有的6個(gè)自由度都進(jìn)行跟蹤，一個(gè)合理的控制目標(biāo)方案為跟蹤航跡［x，y，z]和滾轉(zhuǎn)角 φ，同時(shí)保證另外 2個(gè)角度。

采用Matlab對(duì)控制系統(tǒng)進(jìn)行仿真，通過輸出波形評(píng)估該系統(tǒng)的穩(wěn)定性，整個(gè)控制系統(tǒng)的結(jié)構(gòu)如圖8所示。

圖8 閉環(huán)系統(tǒng)結(jié)構(gòu)Fig.8 Closed-loop system structure

圖9 三個(gè)位置狀態(tài)的收斂過程Fig.9 Convergence of three position states

圖10 三個(gè)姿態(tài)的收斂過程Fig.10 Convergence of three gestures

該控制系統(tǒng)屬于由內(nèi)外環(huán)構(gòu)成的控制系統(tǒng)，采用雙環(huán)控制方法設(shè)計(jì)控制律[13]。位置子系統(tǒng)為外環(huán)，姿態(tài)子系統(tǒng)為內(nèi)環(huán)。在內(nèi)外環(huán)控制中，內(nèi)環(huán)的動(dòng)態(tài)性能影響外環(huán)的穩(wěn)定性，從而會(huì)影響整個(gè)閉環(huán)控制系統(tǒng)的穩(wěn)定性。為實(shí)現(xiàn)收斂速度快的內(nèi)環(huán)控制，采用內(nèi)環(huán)收斂速度大于外環(huán)收斂速度的方法，以保證閉環(huán)系統(tǒng)的穩(wěn)定性。在本算法中通過調(diào)整內(nèi)環(huán)控制其增益系數(shù)，在設(shè)計(jì)中采用了較大的PD增益，保證內(nèi)環(huán)收斂速度大于外環(huán)收斂速度。

針對(duì)模型式（9），設(shè)定微型四旋翼無人機(jī)相關(guān)參數(shù)[14]。 擾動(dòng)取 d4=d5=d6=0.010，控制目標(biāo) x→0，y→0，z→zd，φ→φd，被控對(duì)象的初始狀態(tài)取 ［2 0 1 0 0 0］，被控對(duì)象角度初始狀態(tài)?。? 0 0 0 0 0］，取 zd=3，φd=π/3。

仿真結(jié)果如圖9～圖11所示，分別為飛行器的位置、姿態(tài)收斂過程和4個(gè)控制輸入的變化過程。

從仿真圖9、圖10可以看出，采用雙閉環(huán)PD控制方式，在3 s的時(shí)候系統(tǒng)達(dá)到設(shè)定控制目標(biāo)，進(jìn)入穩(wěn)態(tài)，且之后沒有震蕩，完全跟蹤上期望值。從仿真圖11可以看出，四個(gè)控制的輸入大約在1 s之后完成控制的輸入，雖然之后有小幅波動(dòng)，但整體系統(tǒng)處于穩(wěn)定狀態(tài)，驗(yàn)證了該控制算法的可行性。

圖11 四個(gè)控制輸入的變化過程Fig.11 Four control inputs change process

4 結(jié)語

針對(duì)單一傳感器測(cè)量四旋翼飛行器姿態(tài)角和位置信息的不準(zhǔn)確問題，本文采用了多傳感器采集飛行器數(shù)據(jù)，借助FPGA強(qiáng)大的數(shù)據(jù)并行處理能力，在保證數(shù)據(jù)實(shí)時(shí)性的基礎(chǔ)上設(shè)計(jì)FIR數(shù)字濾波器，提高傳感器數(shù)據(jù)的可用性，通過仿真驗(yàn)證了濾波器的可行性。在主控制器STM32上設(shè)計(jì)了卡爾曼濾波器用于多傳感器的數(shù)據(jù)融合，進(jìn)而對(duì)飛行器的姿態(tài)進(jìn)行解算，結(jié)合四旋翼飛行器的動(dòng)力學(xué)模型特點(diǎn)，設(shè)計(jì)了雙閉環(huán)PD控制系統(tǒng)，并在Matlab下進(jìn)行了相關(guān)仿真，仿真結(jié)果顯示該控制算法響應(yīng)速度快、穩(wěn)定性好，為微小型四旋翼飛行器的設(shè)計(jì)奠定了基礎(chǔ)。

[1]袁安富，徐金琦，王偉，等.基于雙STM32多旋翼無人機(jī)控制系統(tǒng)設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2013，39（11）：136-138.

[2]李偵，田夢(mèng)君，趙菲菲.基于DSP的無人飛行器飛行控制系統(tǒng)設(shè)計(jì)[J].微處理機(jī)，2010，31（4）：125-128.

[3]潘輝.STM32-FSMC機(jī)制的NOR Flash存儲(chǔ)器擴(kuò)展技術(shù)[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2009，9（10）；31-34.

[4]曹彬乾，程遠(yuǎn)增，楊青.基于STM32+FPGA的數(shù)據(jù)采集系統(tǒng)的設(shè)計(jì)[J].計(jì)算機(jī)工程與設(shè)計(jì)，2014，35（4）：1231-1235.

[5]楊會(huì)建，田成軍，楊志娟，等.基于FPGA的SDRAM乒乓讀寫操作設(shè)計(jì)[J].長春理工大學(xué)學(xué)報(bào)：自然科學(xué)版，2015（2）：67-71.

[6]應(yīng)進(jìn)，潘浩曼，代冀陽，等.FPGA與ARM的無人機(jī)陀螺儀數(shù)據(jù)接口設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2014，40（1）：9-12.

[7]杜勇.數(shù)字濾波器的MATLAB與FPGA實(shí)現(xiàn)-Altera/Verilog版[M].北京：電子工業(yè)出版社，2015.

[8]唐博，李錦明，李士照.基于FPGA的高階FIR濾波器強(qiáng)抗干擾數(shù)據(jù)采集系統(tǒng)[J].電子技術(shù)應(yīng)用，2012，38（9）：89-92.

[9]李偉，武鵬飛，陳宏霖.基于DSP Builder的分布式算法的FIR數(shù)字低通濾波器設(shè)計(jì)[J].計(jì)算機(jī)應(yīng)用，2015（S2）：335-338.

[10]馮智勇，曾瀚，張力，等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J].西南師范大學(xué)學(xué)報(bào)：自然科學(xué)版，2011，36（4）：137-141.

[11]吳友強(qiáng)，紀(jì)浩，鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸停控制研究[J].自動(dòng)化與儀表，2013，28（11）：1-5.

[12]萬曉鳳，康利平，余運(yùn)俊，等.基于多傳感器數(shù)據(jù)融合的四旋翼飛行器的姿態(tài)解算[J].科技導(dǎo)報(bào)，2014，32（19）：31-35.

[13]劉金琨.先進(jìn)PID控制MATLAB仿真.第4版[M].北京：電子工業(yè)出版社，2016.