兩種四旋翼無人機姿態(tài)解算算法仿真分析

李俊杰 張曉龍 王聰 張峻豪

摘? 要： 近年來隨著微電子技術和控制技術的發(fā)展，四旋翼無人機也得到了快速的發(fā)展，為了精確的得到四旋翼無人機的姿態(tài)角，從而提高無人機控制系統(tǒng)的控制精度，由傳感器測到的信息需要進行解算處理，常用的解算算法有互補濾波算法和卡爾曼濾波算法。本文對兩種常用的姿態(tài)解算算法原理進行推導并進行了仿真實驗，實驗結果表明兩種解算算法均能優(yōu)化所測得的信息，互補濾波算法可以有效融合陀螺儀和加速度計的特性從而提高濾波性能，在卡爾曼濾波算法中可以自由配置Q和R的值來調整觀測信息和預測信息的權重。

關鍵詞： 四旋翼無人機;姿態(tài)解算;互補濾波;卡爾曼濾波

中圖分類號： TP391? ? 文獻標識碼： A? ? DOI：10.3969/j.issn.1003-6970.2020.10.063

本文著錄格式：李俊杰，張曉龍，王聰，等. 兩種四旋翼無人機姿態(tài)解算算法仿真分析[J]. 軟件，2020，41（10）：250252

【Abstract】： In recent years， with the development of microelectronics and control technology， quadrotor UAV has also been rapid development. In order to accurately obtain the attitude angle of quadrotor UAV and improve the control accuracy of UAV control system， the information measured by sensors needs to be solved. The commonly used algorithms are complementary filtering algorithm and Kalman filtering algorithm. In this paper， the principle of two commonly used attitude calculation algorithms is derived and simulated. The experimental results show that the two algorithms can optimize the measured information， and the complementary filtering algorithm can effectively integrate the characteristics of gyroscope and accelerometer， so as to improve the filtering performance. In Kalman filter algorithm， the values of Q and R can be freely configured to adjust the observation information and prediction information Weight.

【Key words】： Quadrotor UAV; Attitude solution; Complementary filtering; Kalman filter

0? 引言

無人機是一種可以回收利用的可執(zhí)行各種飛行任務的飛行器。它的主要特點是可以遠程遙控、成本較低且有著不錯的性能，許多飛行任務可以用無人機去替代人類執(zhí)行。自第一臺無人機問世到現(xiàn)在，無人機技術得到了飛速的發(fā)展應用面也越來越廣。早期的無人機主要是軍工企業(yè)研發(fā)和使用，但隨著微型計算機控制技術和傳感技術的發(fā)展，無人機技術的日趨成熟，民用和商業(yè)使用無人機也得到了很大的發(fā)展[1]。

多旋翼無人機是靠控制旋翼是驅動飛機執(zhí)行各種飛行動作，按旋翼數(shù)量又可以分為四旋翼、六旋翼、八旋翼無人機等。不同的旋翼數(shù)量影響著無人機執(zhí)行任務的性能，其中最常見的是四旋翼無人機，作為一種小型無人機不僅結構簡單，而且適應性很強，相較于固定翼的無人機它有著許多不可比擬的優(yōu)勢，還可以做到固定翼無人機做不到的動作如懸停、垂直起飛或降落等，被廣泛應用于攝影、地形測繪等領域。

隨著微機電系統(tǒng)的發(fā)展，四旋翼無人機的控制技術也隨之發(fā)展。為了能得到好的控制效果，在無人機上裝有傳感器實時監(jiān)測無人機的姿態(tài)數(shù)據(jù)信息。但是由于傳感器自身的問題或機體振動噪聲的影響，傳感數(shù)據(jù)會存在不可避免的誤差，為了盡可能的接近無人機真實的姿態(tài)需要建立相應的算法去對傳感器的測量值進行修正。

1? 互補濾波算法

1.1? 互補濾波原理

在無人機的姿態(tài)角測量時用到的MEMS傳感器因為無人機電機驅動的振動會使傳感信號帶有噪聲，這些噪聲是不利于無人機控制的。所以需要對MEMS測到的信號進行濾波處理，在四旋翼無人機上測量角速度的傳感器選用到的是陀螺儀[3]。雖然陀螺儀的動態(tài)性能不錯，但是它存在零漂和溫漂問題，所以在使用陀螺儀的時候會有低頻噪聲，可以使用高頻濾波器對測得的信號去噪。在重力加速度測量選用的是加速度

計傳感器，測量地球磁場的磁力使用的是磁力計。加速度傳感器和磁力計在動態(tài)特性方面較差，但是靜態(tài)性能很好，它們在測量時存在高頻噪聲，可以使用低頻濾波器去噪。陀螺儀測量公式如下：

式（1）中，表示旋轉角，為前一次計算的值，為測得的角速度值，t為兩次測量的時間差。

由式（1）可知陀螺儀測量會累積誤差，而磁力計和加速度計不會，所以在較短的時間段內可以用陀螺儀測量姿態(tài)角。若測量時間跨度較大則需要加速度計和磁力計對其進行修正，確保測量結果盡可能接近真實值。互補濾波算法原理如圖1。

為了使結果更加準確，先取加速度計測得的數(shù)據(jù)的平均值，然后把陀螺儀所測數(shù)據(jù)和加速度計融合。加速度計算公式為：

由式（5）知系數(shù)s和k決定了估計值和真實之間的差值。

1.2? 互補濾波算法MATLAB仿真

選用上述傳感器測得的數(shù)據(jù)[3]來驗證互補濾波算法并進行MATLAB編程，生成了對比圖。

由圖2兩曲線對比，陀螺儀所測的信號既沒有高頻誤差也沒有高頻噪聲，但陀螺儀相較于互補濾波后的信號存在誤差。在20 s之前兩曲線沒太大差別，但20 s之后開始出現(xiàn)較大偏差且隨時間累積。由圖3兩曲線對比，加速度計曲線十分雜亂波動很大，但和互補濾波曲線走勢一樣，互補濾波后曲線平緩了許多。

2? 卡爾曼濾波算法

卡爾曼濾波算法因為其無偏性，估計方差最小，實時性好等優(yōu)點廣泛應用于各種控制領域，也是四旋翼無人機姿態(tài)解算常用的一種算法。

2.1? 卡爾曼濾波算法推導

系統(tǒng)觀測方程和狀態(tài)方程如下式：

式中是轉移矩陣，前一時刻系統(tǒng)輸入為，觀測矩陣。系統(tǒng)的觀測噪聲，系統(tǒng)的過程噪聲，其中服從期望為0方差為Q的正態(tài)分布服從期望為0方差為R的正態(tài)分布。

使用上一時刻系統(tǒng)的狀態(tài)加輸入和當前時刻觀測到的值來預估當前的系統(tǒng)狀態(tài)，這就是卡爾曼濾波算法的原理[7]。

式（15）里面，是用所估算的上一時刻的系統(tǒng)狀態(tài)和模型來預估系統(tǒng)在k時刻的狀態(tài)，是k時刻觀測量的預估值，它是k時刻的預估值使用觀測模型估算出來的。上式的意義為狀態(tài)的估計值是觀測的信息乘上卡爾曼系數(shù)再加上對狀態(tài)的預測值。

在估值的過程中Q和R值是很重要的，Q和R的增大或減小都會對結果造成影響。

2.2? MATLAB仿真分析

需要驗證卡爾曼濾波算法在四旋翼無人姿態(tài)解算上是否有效，使用到陀螺儀測到的一組數(shù)據(jù)[3]，在matlab軟件中用卡爾曼濾波算法程序。結果如圖4。

在解算圖4a的基礎上增大Q值得到圖b，在圖a的基礎上增大R值得到圖c。圖a可以看出卡爾曼濾波算法可以很好的優(yōu)化傳感器所測結果，在b圖中原始數(shù)據(jù)線條和卡爾曼濾波后的線條無太大差異，也就是在該算法中觀測值占比較大。所以可以知道，在卡爾曼濾波算法中如果需要提高觀測信息的占比只需要增大Q值即可，此時還會加快濾波響應速度和收斂速度。相應的，若需要提高預測值的占比則需要增大R值，此時對噪聲處理能力也會增強。

3? 結論

在無人機姿態(tài)解算中常用的互補濾波和卡爾曼濾波方法都能對信號進行處理，兩種方法側重點各不相同。

（1）陀螺儀在前20 s內測得的數(shù)據(jù)較為準確，? ?但在20 s后由于累計誤差導致數(shù)據(jù)偏差越來越大，? ?互補濾波算法能有效得融合陀螺儀和加速度計所測得數(shù)據(jù)。

（2）加速度計曲線和結算后曲線總體走勢相同，但是十分曲折所測數(shù)據(jù)很難使用，互補濾波算法能很好的去除噪音使曲線圖平緩。在使用時，前面短時間段內可以用陀螺儀數(shù)據(jù)，之后需要兩個傳感器數(shù)據(jù)進行互補濾波進行融合。

（3）卡爾曼濾波算法使用的關鍵是配置好Q值和R值，Q值大的時候濾波效率較高此時觀測信息占比較大，R值大的時候對噪聲的處理效果好此時預測信息占比較大。

參考文獻

[1]常于敏. 無人機技術研究現(xiàn)狀及發(fā)展趨勢[J]. 電子技術與軟件工程， 2014（1）： 242-243.

[2]余后明. 四旋翼無人機姿態(tài)控制研究[D]. 中北大學， 2019.

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

[4]賈文哲， 王劍平. 兩類改進非線性濾波器 UKF 算法綜述[J]. 化工自動化及儀 表， 2016， 43（10）： 1011-1014.

[5]卞月根， 張偉. 基于非線性卡爾曼濾波的車輛定位優(yōu)化算法[J]. 計算機技術與發(fā)展， 2015， 25（8）： 80-83+89.

[6]汪大理， 王先春. 四旋翼自主飛行器設計[J]. 電子世界， 2016（23）： 105-106.

[7]王寬田， 黨選舉， 鄭飛宇， 等. 基于卡爾曼濾波四旋翼無人機控制系統(tǒng)設計[J]. 電子世界， 2017（8）： 103-104.

[8]李松柏. 四旋翼飛行器姿態(tài)控制模型研究[J]. 信息技術與信息化， 2019（11）： 238-240.

[9]吳杰， 閆建國. 基于修正的卡爾曼濾波的姿態(tài)估計算法研究[J]. 計算機仿真， 2012， 29（2）： 54-57.

[10]朱巖， 付巍. 基于無跡卡爾曼濾波的四旋翼無人飛行器姿態(tài)估計算法[J]. 測試技術學報， 2014， 28（3）： 194-198.