基于mems九軸傳感器的智能自主飛行器

華南理工大學電子與信息學院 侯昶宇 劉彥超 周澤群 吳克乾

基于mems九軸傳感器的智能自主飛行器

華南理工大學電子與信息學院 侯昶宇 劉彥超 周澤群 吳克乾

本文通過對飛行器常用算法的大量研究，提出了改進后的四元數算法，可以通過系統狀態量和加速度計測量值對系統狀態進行驗證，從而得到該系統的最優狀態量，并能實時更新參數矩陣。并在加速度計偵測到各種有害加速度的時候，使姿態監測更加準確。

陀螺儀；姿態控制；卡爾曼濾波，四元數算法

1.引言

飛行器目前在工業、農業、軍事上都得到了廣泛的應用，地址探測、災害監測、氣象探測的研究等等，飛行器也迎來了大發展的時期。飛行器的飛行狀態等相關因素也成為了研究的重點。

目前多數無人機都是采用數據積分并融合的方式進行姿態控制，本文所探討的是另外一種姿態控制模式即四元數控制算法。通過這種算法來改善飛行狀況進行穩定的飛行。

2.項目說明

本項目試圖通過改變飛行控制算法來改善飛行器的飛行狀況，即：每個采樣周期通過讀取陀螺儀，加速度計和電子羅盤的數據進行迭代計算，實現四元數隨著時間進行更新，進而求得三個姿態角（俯仰，滾轉，偏航）。由處理器處理姿態角變化并將指令發送給電子調速器進而控制無刷電機實現飛行控制。項目研究內容：

（1）陀螺儀數據采集；

（2）處理器分析數據；

（3）四元數姿態解算；

（4）飛行控制算法。

3.四元數算法說明

3.1 基本原理

由于載體的姿態方位角速率較大，所以針對姿態矩陣的實時計算提出了更高的要求。通常假定捷聯系統“數學平臺”模擬地理坐標系，即導航坐標系;而確定載體的姿態矩陣即為研究載體坐標系和導航坐標系的空間轉動關系，一般用載體坐標系相對導航坐標系的三次轉動角確定。

圖1 基本原理圖

3.2 四元數姿態解算

四元數的數學概念是1843年由哈密頓首先提出的，它是代數學中的內容之一。隨著捷聯式慣性導航技術的發展，為了更簡便地描述剛體的角運動，采用了四元數這個數學工具，用它來彌補通常描述剛體角運動的3個歐拉角參數在設計控制系統時的不足。

圖二 四元數原理圖

一個動坐標系相對參考坐標系的方位可以完全由動坐標系依次繞3個不同的軸轉動的3個角度來確定。如把載體坐標系作為動坐標系，把導航坐標系作為參考坐標系，則姿態角即為一組歐拉角，按一定的轉動順序得到導航坐標系到載體坐標系的關系。

上述方法就是通過歐拉角設計飛行控制算法。

圖三 歐拉角微分方程

通過上面的歐拉角微分方程，可以求得三個姿態角，但由于只有三個方程而且每個方程都含有三角函數運算所以這種算法可能出現奇點，導致向量方向不確定從而不能全姿態工作。

四元數可以描述一個坐標系或一個矢量相對某一個坐標系的旋轉，四元數的標量部分表示了轉角的一半余弦值，而其矢量部分則表示瞬時轉軸的方向、瞬時轉動軸與參考坐標系軸間的方向余弦值。

圖四 四元數算法

四元數算法只用解算四個一階微分方程式，因此還滿足了飛行控制算法中實時性的要求。

3.3 目前常用算法解析

目前多數飛行器使用的是由德國首創的四周控制算法，其算法并沒有使用通常意義上的四元數算法，而是采用了一種獨特的積分算法。

3．3．1 姿態監測算法

首先，將陀螺儀和加速度計的測量值減常值誤差，得到角速度和加速度，并對角速度進行積分，然后對陀螺儀積分和加速度計的數值進行融合。融合分為兩部分，實時融合和長期融合。實時融合每一次算法周期都要執行，而長期融合設256個檢測周期執行一次。

實時融合：

（1）將陀螺儀積分和加表濾波后的值做差;

（2）按照情況對差值進行衰減，并作限幅處理;

（3）將衰減值加入到角度中。

長期融合：

長期融合主要包括兩個部分，一是對角速度的漂移進行估計(估計值是要在每一個控制周期都耦合到角度中的)，二是對陀螺儀的常值誤差(也就是陀螺儀中立點)進行實時的修正。

（1）將陀螺儀積分的積分和加速度積分做差，這里之所以要使用加表積分和陀螺儀積分的積分，是因為在256個檢測周期內，有一些加速度計的值含有有害的加速度分量，如果只使用一個時刻的加表值對陀螺儀漂移進行估計，顯然估計值不會準確，使用多個周期的值進行疊加后平均處理，可以減小隨機的有害加速度對估計陀螺儀漂移過程中所產生的影響。

（2）將上面兩個值進行衰減，得到估計的陀螺儀漂移

（3）對使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，如果這兩個值較大，說明陀螺儀在前段時間內測到的角速率不夠準確，需要對差值誤差(也就是陀螺儀中立點)進行修正，修正幅度和差值有關

這種姿態監測方法并不是四元數算法，而且計算量也不比四元數算法小。但是編程是更加直觀，易于理解，而且參數的調節也會更加方便。這種方法是在嘗試著對四元數算法這一復雜相互耦合的多狀態變量的線性系統狀態估計過程進行了簡單的解耦，從而將姿態的最優估計和陀螺儀漂移的最優估計分隔開，這樣，就可以通過比較直觀的觀測手段對兩個部分的參數進行調整。而且避開了高難度的卡爾曼濾波設計，因此在設計性上也會更加的合理和易于實現。

3．3．2 姿態控制算法

控制算法的核心是對角速度做PI計算，P的作用是使四軸能夠產生對于外界干擾的抵抗力矩，I的作用是讓四軸產生一個與角度成正比的抵抗力。

圖五 控制算法 PI計算

對角速度做I積分運算實際得到的就是角度，如果四軸有一個傾斜角度，那么四軸就會自己進行調整，直到四軸的傾角為零，它所產生的抵抗力是與角度成正比的，但是，如果只有I的作用，會使四軸迅速產生振蕩，因此，必須將P和I結合起來一起使用，是四軸非常快速的產生抵抗力矩。

3.4 算法比較與創新

目前常用算法對于系統模型的準確性、模型參數的標定和系統參數的選取的準確性沒有很高的要求，而且修改參數時不需要對系統中的很多地方進行大量改變，不需要進行長時間調試。而通過我們改進的算法可以通過系統狀態量和加速度計測量值對系統狀態進行驗證，從而得到該系統的最優狀態量，并能實時更新參數矩陣。并在加速度計偵測到各種有害加速度的時候，使姿態監測更加準確。但我們的算法需要大量的實踐數據，只通過上位機觀測很難得到合適的方法。

3.5 項目成果展示

圖六 傳感器數據采集圖

圖七 飛行器靜態展示

4.總結

隨著技術的發展，飛行器也不再“神秘”，但對于飛行姿態檢測的準確性，飛行狀態的穩定性，靈活性提出了更高的要求，本文對四元數控制算法與飛行器飛行狀態之間的研究，通過一系列真實的飛行實驗，從第一階段的飛行器的姿勢檢測，到第二階段的姿勢控制，采用PI算法，通過STM32F103輸出數據給電子調速器，通過改變無刷電機電流，快速生成合理力矩，使飛行器維持穩定并改變姿態飛行（見圖七）。

[1]朱華,劉衛東,趙淳生．變體飛行器及其變形驅動技術[J]．機械制造與自動化,2010,02:8-14+125．

[2]史建國,高曉光．飛行器突發威脅下的智能自主航路規劃技術[J]．火力與指揮控制,2010,03:39-41+45．

[3]牛軼峰,沈林成,龍濤．攻擊型無人飛行器自主控制技術研究綜述[J]．系統工程與電子技術,2007,03:391-395．

[4]劉峰等．四軸飛行器姿態控制系統設計[J]．計算機測量與控制,2011,19(3)．

[5]王東來等．小型四軸飛行器動力學參數測定方法設計[J]．科技導報,2011,29(36)．

[6]鄧矛．關于四軸飛行器的姿態動力學建模[J]．科技創新導報,2012(9)．

[7]王萍．駐伊美軍裝備無人機[J]．輕兵器,2003(11)．

[8]馮琦．軍用無人機發展趨勢[J]．電光與控制,2003(02)．

[9]S Bouabdallah．Design and Control of quadrotors with application to autonomous flying．Epfl:2007．

[10]常敏,崔永進,何蓓薇,等．四軸飛行器姿態控制系統設計[J]．通信電源技術,2015(5):144-147．

[11]劉峰,呂強,王國勝,等．四軸飛行器姿態控制系統設計[J]．計算機測量與控制,2011,19(3):583-585．

[12]孫嬌．關于四軸飛行器姿態控制系統設計與問題分析[J]．工程技術:文摘版,2016(3):00225-00225．

[13]何瑜．四軸飛行器控制系統設計及其姿態解算和控制算法研究[D]．電子科技大學,2015．

[14]http://cdmd．cnki．com．cn/Article/CDMD-10336-1013218065．htm．

[15]http://www．ck365．cn/lunwen/9/42230．html．