MEMS姿態傳感器在控制飛行器中的應用研究

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

摘 要：文章介紹了MEMS姿態傳感器在控制飛行器中的應用研究。首先介紹了無人控制飛行器系統，以STM32F407VGT6（ARM Cortex-M4）處理器為控制核心，利用MEMS姿態傳感器為姿態檢測模塊，通過PC端應用軟件系統規劃行駛路徑，同時輔以4G無線傳輸GPS地理位置定位、實現飛行器的地理位置反饋和環境監控，通過對比仿真對照ANP控制器和PID控制器的試驗結果，試驗表明：ANP控制器比PID控制器在飛行器的應用中使得系統性能更穩定，精度更高。

關鍵詞：MEMS姿態傳感器 ANP控制 PID控制器 無人駕駛飛行器

中圖分類號：TP334.3 文獻標識碼：A 文章編號：1674-098X（2016）08（c）-0008-03

開發無人駕駛的微型飛行器，對危險領域進行探測和搜救即可減少傷害的發生。在微型飛行器的研究方面，目前歐美等發達國家遠遠走在全球的前列，且成功研究了固定翼式、旋翼式和撲翼式各類微型飛行器，我國高校和研究所雖然也開始展開了相關研究，但在救災減災方面研究極少。監控室的監控人員事先規劃路線通過4G無線傳輸對飛行器實現實時路徑導航和無線控制，當飛行器到達災后現場上空時，通過所攜帶的攝像頭和環境傳感器采集將圖像資料和環境參數無線傳輸給監控室，給現場決策提供幫助。

1 硬件設計

1.1 系統結構

系統硬件主要由姿態解算MEMS器件AHRS（Attitude and Heading Reference System）、控制系統（ARM Cortex-M4處理器、環境監控系統、GPS導航系統、智能相機系統及4G傳輸系統）構成。

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

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

1.2 MEMS姿態解算器

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

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

HMC5883L采用霍尼韋爾各向異性磁阻（AMR）技術，這些各向異性傳感器具有在軸向高靈敏度和線性高精度的特點。傳感器具有的對正交軸的低靈敏度的固相結構能用于測量地球磁場的方向和大小，其測量范圍從毫高斯到8高斯（gauss）。

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

各傳感器通過I2C接口與STM32F407VGT6連接，同時傳感器的數據中斷引腳與STM32F407VGT6的IO相連。傳感器在完成ADC轉換，STM32F407VGT6在第一時間讀取最新的數據，快速響應姿態的變化。這樣的連接使得控制器擁有最大的主動權，最快地獲取各傳感器的狀態和轉換結果。

2 算法研究

飛行器采用四元數加互補濾波作為姿態解算算法，引入高度的自適應神經網絡PID作為控制器，實現自適應環境飛行和相機增穩等功能。經測試具有靈活輕盈、延展性、適應性強等特點。

2.1 姿態算法

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

2.2 四軸飛行器高度自適應神經網絡PID算法

四軸飛行器的飛行高度控制就如姿態解算一樣重要，如天氣因素對于飛行器的飛行高度控制系統的影響是一項具有挑戰性的技術難點，提供一種合適的方法使機器人在面對不同的環境下，更加穩定和高效的飛行。

嘗試使用一種自適應的方法來控制機器人的高度，可以讓機器人在不同的狀況下做出適當的操作。控制系統的激勵已經在MATLAB仿真中完成，飛行器等價為一個質點，因此牛頓-歐拉公式可以在動力學中直接被使用。四轉軸機器人的建模中，四轉軸機器人的動力學方程使用牛頓-歐拉方法，總的方程如下：

m為機器人質量；VB線性速度；V為角速度；R為旋轉矩陣；G為重力加速度；TB為引擎力總和。

自適應PID控制器控制飛行器的高度，類似于多認知對象神經網絡結構中的的神經元構架。每一個控制器得到的是一個用于相乘的比率（W1、W2、W3），而控制器的輸出被限制在sigmund函數里，而等式（2）中給出了具體的控制器構架。

在更新（W1、W2、W3）時，算法會將基于預定軌跡之間相差錯誤值來控制系統的值的最小化值。公式（3）會給出這個系統公式。它采用梯度值由遞歸方程也就是最陡下降算法（SD）和方程（4）和（5）示出其細節實時更新控制器在每個時間的步長系數。通過使用在線學習算法，一些高錯誤值（外力干擾）輸入而造成系統不穩定，雖然輸出值Y是一個在[0，1]區間的值，在一些特定的環境下，改變控制器的輸出，可以將其改為幾個特定的值。

為了優化ANP算法的參數（W1、W2、W3），執行標準應該最小化，并在每一個時間步中使用公式（4）然后再更新參數。其支持一種再沒有延時的情況下快速處理并調節算法的系統。

ANP控制器被用來正在確定的系統中自適應的獲得合適的參數來控制飛行器的高度。這種方法優點在于其不需要使用不同的數學方法分析來定義合適的系數，適應新的環境和動態的改變系統。

3 軟件設計

軟件設計分為上位機軟件和飛行器底層下位機軟件。上位機主要負責路徑規劃及監控信息實時顯示[5]，下位機軟件主要負責飛行器的姿態及正常飛行、GPS數據獲取、相機控制、傳感器數據采集及4G通信等任務。

上位機內嵌Google Earth顯示界面，可以直接運用Google Earth規劃路徑，通過接收GPS信號，進行實時定位顯示并將地理位置數據發送給飛行器修正，使飛行器按照上位機軟件規劃的路徑行駛，同時，上位機實時監控飛行器的姿態和相關傳感器的數值，并控制攝像頭拍照和云臺控制，實現智能判斷和顯示。

4 仿真調試

4.1 姿態角調試

在完成控制器底層的硬件驅動后，開始姿態角算法的調試。通過4G傳輸模塊將解算出的姿態角數據發回上位機，搖動四軸機體，觀察上位機數據曲線與姿態曲線。四軸飛行器姿態參數如偏航角：Yaw，俯仰角：Pitch，翻滾角：Rool，從曲線可以看出在進過一段調整后，曲線變為平行曲線，說明這幾個量處于一個相對平衡狀態，飛行器處于一個平衡狀態。

4.2 整機測試

使用LabVIEW編寫的上位機軟件開始整機調試系統。編寫的上位機主要分為5個模塊區域：GPS實時地理位置監控和路徑規劃區域、四軸飛行器姿態及運行狀態檢測區域、視頻撲捉拍攝區域、飛行器當前環境檢測區域、4G信號狀況顯示區域、軟件操作區域等，主要操作步驟：

第一步：開啟4G網絡，使4G網絡正常工作；

第二步：在內嵌的Google Earth（GPS實時地理位置監控和路徑規劃區域），搜索GPS數據和開始進行路徑規劃設置，使得GPS功能運行正常；

第三步：在四軸飛行器姿態及運行狀態檢測區域，開啟四軸飛行器，讓飛行器能夠按照路徑規劃的指定路徑飛行；

到達目的地，在視頻撲捉拍攝區域開啟圖像捕捉，或者在環境監測區域查看傳感器數據等。

測試過程中，上位機檢測效果良好，特別是數據的實時響應，GPS信號解析可以很好的和Google Earth兼容，路徑規劃傳遞實時響應，3D渲染也表現的不錯，4G網絡傳輸在較為偏僻的地理位置信號較弱。

5 實驗及其結果

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

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

但是干擾當干擾超過6 N時會造成系統的不穩定并難以控制。在第二種狀態下，系統的反應接近正弦輸入但是控制器適當的首要的控制值在遺傳算法中得到，而r的學習比例將存在出兩種不同狀態下有干擾和無干擾的區別。

ANP算法的自適應性可以使其通過函數自行降噪，在這個問題中，遺傳算法的PID控制函數是通過比較得到的。就如先前的實驗中，研究兩種狀態下有無干擾的降噪效果。

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

6 結語

在研究中，ANP控制器被用在確定的系統中自適應地獲得合適的參數來控制飛行器的高度。這種方法最重要的優點在于其不需要使用不同的數學方法分析來定義合適的系數、適應新的環境和動態的改變系統。另一方面來說，這種方法不能保證系數是近似合適的。另一個問題就是學習比率的值是一個很重要的挑戰，因為低的學習比率會降低接近合適參數的速度而使得系統無法快速地適應環境，而高的學習比率可能會導致系統變化無常。因此ANP的首要獲得就是一個很重要的問題，而此次基于問題本身的結果控制函數非常巨大。然而，適應性可以提高控制器去除系統輸入噪聲的能力。ANP控制器可以控制系統中出現的噪聲，而PID控制無法做得這么好。

參考文獻

[1] 高宗余，方建軍，于麗杰.MEMS傳感器隨機誤差Allan方差分析[J].儀器儀表學報，2011（12）：2863-2868.

[2] 付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導航系統中的應用[M].2版.北京：科學出版社，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慣性器件誤差系數的Allan方差分析方法[J].中國科學：物理學力學天文學，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] 吳德會，趙偉，黃松嶺，等.傳感器動態建模FLANN方法改進研究[J].儀器儀表學報，2009（2）：362-367.

[7] 黃美發，景暉，匡兵，等.基于擬蒙特卡羅方法的測量不確定度評定[J].儀器儀表學報，2009（1）：120-125.

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