宋銳++劉宏立++李和平

摘 要：文章介紹了MEMS姿態(tài)傳感器在控制飛行器中的應(yīng)用研究。首先介紹了無(wú)人控制飛行器系統(tǒng)，以STM32F407VGT6（ARM Cortex-M4）處理器為控制核心，利用MEMS姿態(tài)傳感器為姿態(tài)檢測(cè)模塊，通過(guò)PC端應(yīng)用軟件系統(tǒng)規(guī)劃行駛路徑，同時(shí)輔以4G無(wú)線傳輸GPS地理位置定位、實(shí)現(xiàn)飛行器的地理位置反饋和環(huán)境監(jiān)控，通過(guò)對(duì)比仿真對(duì)照ANP控制器和PID控制器的試驗(yàn)結(jié)果，試驗(yàn)表明：ANP控制器比PID控制器在飛行器的應(yīng)用中使得系統(tǒng)性能更穩(wěn)定，精度更高。

關(guān)鍵詞：MEMS姿態(tài)傳感器 ANP控制 PID控制器 無(wú)人駕駛飛行器

中圖分類號(hào)：TP334.3 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1674-098X（2016）08（c）-0008-03

開(kāi)發(fā)無(wú)人駕駛的微型飛行器，對(duì)危險(xiǎn)領(lǐng)域進(jìn)行探測(cè)和搜救即可減少傷害的發(fā)生。在微型飛行器的研究方面，目前歐美等發(fā)達(dá)國(guó)家遠(yuǎn)遠(yuǎn)走在全球的前列，且成功研究了固定翼式、旋翼式和撲翼式各類微型飛行器，我國(guó)高校和研究所雖然也開(kāi)始展開(kāi)了相關(guān)研究，但在救災(zāi)減災(zāi)方面研究極少。監(jiān)控室的監(jiān)控人員事先規(guī)劃路線通過(guò)4G無(wú)線傳輸對(duì)飛行器實(shí)現(xiàn)實(shí)時(shí)路徑導(dǎo)航和無(wú)線控制，當(dāng)飛行器到達(dá)災(zāi)后現(xiàn)場(chǎng)上空時(shí)，通過(guò)所攜帶的攝像頭和環(huán)境傳感器采集將圖像資料和環(huán)境參數(shù)無(wú)線傳輸給監(jiān)控室，給現(xiàn)場(chǎng)決策提供幫助。

1 硬件設(shè)計(jì)

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

系統(tǒng)硬件主要由姿態(tài)解算MEMS器件AHRS（Attitude and Heading Reference System）、控制系統(tǒng)（ARM Cortex-M4處理器、環(huán)境監(jiān)控系統(tǒng)、GPS導(dǎo)航系統(tǒng)、智能相機(jī)系統(tǒng)及4G傳輸系統(tǒng)）構(gòu)成。

主控制芯片選用STM32F407VGT6，采用ARM32-bitCortex?-M4內(nèi)核，通過(guò)STM32內(nèi)部的PLL，處理器最高運(yùn)行主頻為168 MHz，較強(qiáng)的DSP處理能力在姿態(tài)算法上可以保證較高的精度和較快的響應(yīng)速度[1]。

環(huán)境監(jiān)控模塊主要由溫濕度傳感器SHT11、PM2.5粉塵傳感器SM-PWM-0.1A和石油液化氣模塊MS6100組成。GPS定位選用U-box PAM-7Q模塊，具有U-blox 7引擎的高性能，以業(yè)內(nèi)公認(rèn)的封裝形式提供高靈敏度和行內(nèi)最短的捕捉時(shí)間，其最大更新速率為10 Hz，定位精度2.5 mCEP。智能相機(jī)選用Intova SP1 Sport HD 1080p高清攝影相機(jī)；4G視頻數(shù)據(jù)無(wú)線傳輸支持圖像壓縮，保證視頻高速傳遞。

1.2 MEMS姿態(tài)解算器

姿態(tài)解算MEMS器件AHRS（Attitude and Heading Reference System）由三軸角速度陀螺儀、三軸加速度計(jì)、三軸磁阻傳感器、氣壓高度傳感器組成。

MPU6050組合了16位A/D三軸陀螺儀和三軸加速度計(jì)，為全球首例整合性6軸運(yùn)動(dòng)處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時(shí)之軸間差的問(wèn)題，減少了大量包裝空間，產(chǎn)品傳輸可通過(guò)最高至400 kHz的I2C總線。

HMC5883L采用霍尼韋爾各向異性磁阻（AMR）技術(shù)，這些各向異性傳感器具有在軸向高靈敏度和線性高精度的特點(diǎn)。傳感器具有的對(duì)正交軸的低靈敏度的固相結(jié)構(gòu)能用于測(cè)量地球磁場(chǎng)的方向和大小，其測(cè)量范圍從毫高斯到8高斯（gauss）。

BMP180是一款高精度、小體積、超低能耗的壓力傳感器，采用強(qiáng)大的8-pin陶瓷無(wú)引線芯片承載（LCC）超薄封裝。

各傳感器通過(guò)I2C接口與STM32F407VGT6連接，同時(shí)傳感器的數(shù)據(jù)中斷引腳與STM32F407VGT6的IO相連。傳感器在完成ADC轉(zhuǎn)換，STM32F407VGT6在第一時(shí)間讀取最新的數(shù)據(jù)，快速響應(yīng)姿態(tài)的變化。這樣的連接使得控制器擁有最大的主動(dòng)權(quán)，最快地獲取各傳感器的狀態(tài)和轉(zhuǎn)換結(jié)果。

2 算法研究

飛行器采用四元數(shù)加互補(bǔ)濾波作為姿態(tài)解算算法，引入高度的自適應(yīng)神經(jīng)網(wǎng)絡(luò)PID作為控制器，實(shí)現(xiàn)自適應(yīng)環(huán)境飛行和相機(jī)增穩(wěn)等功能。經(jīng)測(cè)試具有靈活輕盈、延展性、適應(yīng)性強(qiáng)等特點(diǎn)。

2.1 姿態(tài)算法

基于互補(bǔ)濾波的四元素算法[2]，利用加速度測(cè)得的重力向量與估計(jì)姿態(tài)得到重力向量的誤差來(lái)矯正陀螺儀積分誤差，從而利用矯正后的陀螺儀積分得到姿態(tài)角等，姿態(tài)解算已經(jīng)在多個(gè)研究中進(jìn)行了詳細(xì)的論證，提出了多種解算算法，故不做詳細(xì)介紹。

2.2 四軸飛行器高度自適應(yīng)神經(jīng)網(wǎng)絡(luò)PID算法

四軸飛行器的飛行高度控制就如姿態(tài)解算一樣重要，如天氣因素對(duì)于飛行器的飛行高度控制系統(tǒng)的影響是一項(xiàng)具有挑戰(zhàn)性的技術(shù)難點(diǎn)，提供一種合適的方法使機(jī)器人在面對(duì)不同的環(huán)境下，更加穩(wěn)定和高效的飛行。

嘗試使用一種自適應(yīng)的方法來(lái)控制機(jī)器人的高度，可以讓機(jī)器人在不同的狀況下做出適當(dāng)?shù)牟僮??？刂葡到y(tǒng)的激勵(lì)已經(jīng)在MATLAB仿真中完成，飛行器等價(jià)為一個(gè)質(zhì)點(diǎn)，因此牛頓-歐拉公式可以在動(dòng)力學(xué)中直接被使用。四轉(zhuǎn)軸機(jī)器人的建模中，四轉(zhuǎn)軸機(jī)器人的動(dòng)力學(xué)方程使用牛頓-歐拉方法，總的方程如下：

m為機(jī)器人質(zhì)量；VB線性速度；V為角速度；R為旋轉(zhuǎn)矩陣；G為重力加速度；TB為引擎力總和。

自適應(yīng)PID控制器控制飛行器的高度，類似于多認(rèn)知對(duì)象神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)中的的神經(jīng)元構(gòu)架。每一個(gè)控制器得到的是一個(gè)用于相乘的比率（W1、W2、W3），而控制器的輸出被限制在sigmund函數(shù)里，而等式（2）中給出了具體的控制器構(gòu)架。

在更新（W1、W2、W3）時(shí)，算法會(huì)將基于預(yù)定軌跡之間相差錯(cuò)誤值來(lái)控制系統(tǒng)的值的最小化值。公式（3）會(huì)給出這個(gè)系統(tǒng)公式。它采用梯度值由遞歸方程也就是最陡下降算法（SD）和方程（4）和（5）示出其細(xì)節(jié)實(shí)時(shí)更新控制器在每個(gè)時(shí)間的步長(zhǎng)系數(shù)。通過(guò)使用在線學(xué)習(xí)算法，一些高錯(cuò)誤值（外力干擾）輸入而造成系統(tǒng)不穩(wěn)定，雖然輸出值Y是一個(gè)在[0，1]區(qū)間的值，在一些特定的環(huán)境下，改變控制器的輸出，可以將其改為幾個(gè)特定的值。

為了優(yōu)化ANP算法的參數(shù)（W1、W2、W3），執(zhí)行標(biāo)準(zhǔn)應(yīng)該最小化，并在每一個(gè)時(shí)間步中使用公式（4）然后再更新參數(shù)。其支持一種再?zèng)]有延時(shí)的情況下快速處理并調(diào)節(jié)算法的系統(tǒng)。

ANP控制器被用來(lái)正在確定的系統(tǒng)中自適應(yīng)的獲得合適的參數(shù)來(lái)控制飛行器的高度。這種方法優(yōu)點(diǎn)在于其不需要使用不同的數(shù)學(xué)方法分析來(lái)定義合適的系數(shù)，適應(yīng)新的環(huán)境和動(dòng)態(tài)的改變系統(tǒng)。

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

軟件設(shè)計(jì)分為上位機(jī)軟件和飛行器底層下位機(jī)軟件。上位機(jī)主要負(fù)責(zé)路徑規(guī)劃及監(jiān)控信息實(shí)時(shí)顯示[5]，下位機(jī)軟件主要負(fù)責(zé)飛行器的姿態(tài)及正常飛行、GPS數(shù)據(jù)獲取、相機(jī)控制、傳感器數(shù)據(jù)采集及4G通信等任務(wù)。

上位機(jī)內(nèi)嵌Google Earth顯示界面，可以直接運(yùn)用Google Earth規(guī)劃路徑，通過(guò)接收GPS信號(hào)，進(jìn)行實(shí)時(shí)定位顯示并將地理位置數(shù)據(jù)發(fā)送給飛行器修正，使飛行器按照上位機(jī)軟件規(guī)劃的路徑行駛，同時(shí)，上位機(jī)實(shí)時(shí)監(jiān)控飛行器的姿態(tài)和相關(guān)傳感器的數(shù)值，并控制攝像頭拍照和云臺(tái)控制，實(shí)現(xiàn)智能判斷和顯示。

4 仿真調(diào)試

4.1 姿態(tài)角調(diào)試

在完成控制器底層的硬件驅(qū)動(dòng)后，開(kāi)始姿態(tài)角算法的調(diào)試。通過(guò)4G傳輸模塊將解算出的姿態(tài)角數(shù)據(jù)發(fā)回上位機(jī)，搖動(dòng)四軸機(jī)體，觀察上位機(jī)數(shù)據(jù)曲線與姿態(tài)曲線。四軸飛行器姿態(tài)參數(shù)如偏航角：Yaw，俯仰角：Pitch，翻滾角：Rool，從曲線可以看出在進(jìn)過(guò)一段調(diào)整后，曲線變?yōu)槠叫星€，說(shuō)明這幾個(gè)量處于一個(gè)相對(duì)平衡狀態(tài)，飛行器處于一個(gè)平衡狀態(tài)。

4.2 整機(jī)測(cè)試

使用LabVIEW編寫(xiě)的上位機(jī)軟件開(kāi)始整機(jī)調(diào)試系統(tǒng)。編寫(xiě)的上位機(jī)主要分為5個(gè)模塊區(qū)域：GPS實(shí)時(shí)地理位置監(jiān)控和路徑規(guī)劃區(qū)域、四軸飛行器姿態(tài)及運(yùn)行狀態(tài)檢測(cè)區(qū)域、視頻撲捉拍攝區(qū)域、飛行器當(dāng)前環(huán)境檢測(cè)區(qū)域、4G信號(hào)狀況顯示區(qū)域、軟件操作區(qū)域等，主要操作步驟：

第一步：開(kāi)啟4G網(wǎng)絡(luò)，使4G網(wǎng)絡(luò)正常工作；

第二步：在內(nèi)嵌的Google Earth（GPS實(shí)時(shí)地理位置監(jiān)控和路徑規(guī)劃區(qū)域），搜索GPS數(shù)據(jù)和開(kāi)始進(jìn)行路徑規(guī)劃設(shè)置，使得GPS功能運(yùn)行正常；

第三步：在四軸飛行器姿態(tài)及運(yùn)行狀態(tài)檢測(cè)區(qū)域，開(kāi)啟四軸飛行器，讓飛行器能夠按照路徑規(guī)劃的指定路徑飛行；

到達(dá)目的地，在視頻撲捉拍攝區(qū)域開(kāi)啟圖像捕捉，或者在環(huán)境監(jiān)測(cè)區(qū)域查看傳感器數(shù)據(jù)等。

測(cè)試過(guò)程中，上位機(jī)檢測(cè)效果良好，特別是數(shù)據(jù)的實(shí)時(shí)響應(yīng)，GPS信號(hào)解析可以很好的和Google Earth兼容，路徑規(guī)劃傳遞實(shí)時(shí)響應(yīng)，3D渲染也表現(xiàn)的不錯(cuò)，4G網(wǎng)絡(luò)傳輸在較為偏僻的地理位置信號(hào)較弱。

5 實(shí)驗(yàn)及其結(jié)果

為了評(píng)估這套控制機(jī)器人高度的方法，在Matlab仿真中，要求這種控制算法研究2中狀態(tài)。其在（W1、W2、W3）系數(shù)中首要的值被認(rèn)為是0，在一個(gè)有合適參數(shù)的時(shí)間自適應(yīng)系統(tǒng)中。在第一個(gè)狀態(tài)中，控制器（Kp，Kd，Ki）首要得到的被認(rèn)為是一個(gè)隨機(jī)值而結(jié)果要求控制系統(tǒng)的輸入是一個(gè)頻率為：1（rad/sec），大小為1 m的正弦波。在這次研究中有一定的嘗試和錯(cuò)誤，首先r為0.3，同樣這個(gè)值也對(duì)（W1、W2、W3）3個(gè)系數(shù)有影響。

為了更好的研究控制器的特性，在輸入中加入5 N的干擾的方波信號(hào)，寬度為0.5個(gè)單位。

但是干擾當(dāng)干擾超過(guò)6 N時(shí)會(huì)造成系統(tǒng)的不穩(wěn)定并難以控制。在第二種狀態(tài)下，系統(tǒng)的反應(yīng)接近正弦輸入但是控制器適當(dāng)?shù)氖滓目刂浦翟谶z傳算法中得到，而r的學(xué)習(xí)比例將存在出兩種不同狀態(tài)下有干擾和無(wú)干擾的區(qū)別。

ANP算法的自適應(yīng)性可以使其通過(guò)函數(shù)自行降噪，在這個(gè)問(wèn)題中，遺傳算法的PID控制函數(shù)是通過(guò)比較得到的。就如先前的實(shí)驗(yàn)中，研究?jī)煞N狀態(tài)下有無(wú)干擾的降噪效果。

實(shí)驗(yàn)的最后一部分是實(shí)驗(yàn)在噪聲情況下的3種測(cè)試，直接測(cè)試其控制飛行器的高度，并追蹤器輸入的正弦信號(hào)路徑，噪聲采用周期0.01 s，范圍[-2，2]的輸入干擾。

6 結(jié)語(yǔ)

在研究中，ANP控制器被用在確定的系統(tǒng)中自適應(yīng)地獲得合適的參數(shù)來(lái)控制飛行器的高度。這種方法最重要的優(yōu)點(diǎn)在于其不需要使用不同的數(shù)學(xué)方法分析來(lái)定義合適的系數(shù)、適應(yīng)新的環(huán)境和動(dòng)態(tài)的改變系統(tǒng)。另一方面來(lái)說(shuō)，這種方法不能保證系數(shù)是近似合適的。另一個(gè)問(wèn)題就是學(xué)習(xí)比率的值是一個(gè)很重要的挑戰(zhàn)，因?yàn)榈偷膶W(xué)習(xí)比率會(huì)降低接近合適參數(shù)的速度而使得系統(tǒng)無(wú)法快速地適應(yīng)環(huán)境，而高的學(xué)習(xí)比率可能會(huì)導(dǎo)致系統(tǒng)變化無(wú)常。因此ANP的首要獲得就是一個(gè)很重要的問(wèn)題，而此次基于問(wèn)題本身的結(jié)果控制函數(shù)非常巨大。然而，適應(yīng)性可以提高控制器去除系統(tǒng)輸入噪聲的能力。ANP控制器可以控制系統(tǒng)中出現(xiàn)的噪聲，而PID控制無(wú)法做得這么好。

參考文獻(xiàn)

[1] 高宗余，方建軍，于麗杰.MEMS傳感器隨機(jī)誤差A(yù)llan方差分析[J].儀器儀表學(xué)報(bào)，2011（12）：2863-2868.

[2] 付夢(mèng)印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].2版.北京：科學(xué)出版社，2010：17-18.

[3] C C Foster，G H Elkaim，UC Santa Cruz. Extension of a two-Step calibration methodology to include nonorthogonal sensor axes[J].IEEE Transactionson Aerospace and Electronic Systems，2008，44（3）：1070-1078.

[4] 趙思浩，陸明泉，馮振明.MEMS慣性器件誤差系數(shù)的Allan方差分析方法[J].中國(guó)科學(xué)：物理學(xué)力學(xué)天文學(xué)，2010（5）：672-675.

[5] Yun JaeMu，Ko Jae-Pyung，Lee JangMyung.An inexpensive and accurate absolute po-sition sensor for driving assistance[J]. IEEE Transactionson Instrumentation and Measurement，2008，57（4）：864-873.

[6] 吳德會(huì)，趙偉，黃松嶺，等.傳感器動(dòng)態(tài)建模FLANN方法改進(jìn)研究[J].儀器儀表學(xué)報(bào)，2009（2）：362-367.

[7] 黃美發(fā)，景暉，匡兵，等.基于擬蒙特卡羅方法的測(cè)量不確定度評(píng)定[J].儀器儀表學(xué)報(bào)，2009（1）：120-125.

[8] 涂剛毅，金世俊，祝雪芬，等.基于改進(jìn)粒子濾波算法的GPS非高斯偽距誤差修正[J].電子測(cè)量與儀器學(xué)報(bào)，2009（6）：24-28.