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

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

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

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

陀螺儀；姿態(tài)控制；卡爾曼濾波，四元數(shù)算法

1.引言

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

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

2.項目說明

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

（1）陀螺儀數(shù)據(jù)采集；

（2）處理器分析數(shù)據(jù)；

（3）四元數(shù)姿態(tài)解算；

（4）飛行控制算法。

3.四元數(shù)算法說明

3.1 基本原理

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

圖1 基本原理圖

3.2 四元數(shù)姿態(tài)解算

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

圖二 四元數(shù)原理圖

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

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

圖三 歐拉角微分方程

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

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

圖四 四元數(shù)算法

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

3.3 目前常用算法解析

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

3．3．1 姿態(tài)監(jiān)測算法

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

實時融合：

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

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

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

長期融合：

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

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

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

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

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

3．3．2 姿態(tài)控制算法

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

圖五 控制算法 PI計算

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

3.4 算法比較與創(chuàng)新

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

3.5 項目成果展示

圖六 傳感器數(shù)據(jù)采集圖

圖七 飛行器靜態(tài)展示

4.總結

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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