范君健，吳國東，王志軍

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 接口電路設(shè)計

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

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

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

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

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

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

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

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

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

根據(jù)傳感器的輸出特性和系統(tǒng)的數(shù)據(jù)傳輸需求，同時借助FPGA的高度并行和線性處理高速信號的特點，設(shè)計截止頻率為20 kHz的31階（長度為32）低通線性相位FIR濾波器，采樣頻率100 kHz，量化系數(shù)為16比特。為提高設(shè)計效率和系統(tǒng)可靠性，首先通過Matlab設(shè)計出濾波器系數(shù)，而后將其導(dǎo)入到Quartus II包含的FIR IP核當(dāng)中，直接生成FIR模塊。為獲得更高的運算速度，選擇全并行分布式算法結(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軟件仿真出具有白噪聲特性的輸入信號，以及由10 kHz和30 kHz單頻信號疊加的輸入信號，進而仿真測試數(shù)據(jù)經(jīng)濾波器濾波后的輸出數(shù)據(jù)，以便和FPGA設(shè)計實現(xiàn)后的結(jié)果進行比較。Maltab仿真的信號濾波前后頻譜如圖5所示。

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

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

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

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

圖7 FPGA仿真的信號濾波前后的時域波形Fig.7 FPGA simulation of the signal before and after the time domain waveform

3 軟件算法設(shè)計

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

該控制系統(tǒng)屬于由內(nèi)外環(huán)構(gòu)成的控制系統(tǒng)，采用雙環(huán)控制方法設(shè)計控制律[13]。位置子系統(tǒng)為外環(huán)，姿態(tài)子系統(tǒng)為內(nèi)環(huán)。在內(nèi)外環(huán)控制中，內(nèi)環(huán)的動態(tài)性能影響外環(huán)的穩(wěn)定性，從而會影響整個閉環(huán)控制系統(tǒng)的穩(wěn)定性。為實現(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è)計中采用了較大的PD增益，保證內(nèi)環(huán)收斂速度大于外環(huán)收斂速度。

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

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

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

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

4 結(jié)語

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

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

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

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

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

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

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

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

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

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

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

[11]吳友強，紀(jì)浩，鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸?？刂蒲芯縖J].自動化與儀表，2013，28（11）：1-5.

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

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