李俊杰 張曉龍 王聰 張峻豪

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

關(guān)鍵詞： 四旋翼無(wú)人機(jī);姿態(tài)解算;互補(bǔ)濾波;卡爾曼濾波

中圖分類號(hào)： TP391? ? 文獻(xiàn)標(biāo)識(shí)碼： A? ? DOI：10.3969/j.issn.1003-6970.2020.10.063

本文著錄格式：李俊杰，張曉龍，王聰，等. 兩種四旋翼無(wú)人機(jī)姿態(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? 引言

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

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

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

1? 互補(bǔ)濾波算法

1.1? 互補(bǔ)濾波原理

在無(wú)人機(jī)的姿態(tài)角測(cè)量時(shí)用到的MEMS傳感器因?yàn)闊o(wú)人機(jī)電機(jī)驅(qū)動(dòng)的振動(dòng)會(huì)使傳感信號(hào)帶有噪聲，這些噪聲是不利于無(wú)人機(jī)控制的。所以需要對(duì)MEMS測(cè)到的信號(hào)進(jìn)行濾波處理，在四旋翼無(wú)人機(jī)上測(cè)量角速度的傳感器選用到的是陀螺儀[3]。雖然陀螺儀的動(dòng)態(tài)性能不錯(cuò)，但是它存在零漂和溫漂問(wèn)題，所以在使用陀螺儀的時(shí)候會(huì)有低頻噪聲，可以使用高頻濾波器對(duì)測(cè)得的信號(hào)去噪。在重力加速度測(cè)量選用的是加速度

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

式（1）中，表示旋轉(zhuǎn)角，為前一次計(jì)算的值，為測(cè)得的角速度值，t為兩次測(cè)量的時(shí)間差。

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

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

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

1.2? 互補(bǔ)濾波算法MATLAB仿真

選用上述傳感器測(cè)得的數(shù)據(jù)[3]來(lái)驗(yàn)證互補(bǔ)濾波算法并進(jìn)行MATLAB編程，生成了對(duì)比圖。

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

2? 卡爾曼濾波算法

卡爾曼濾波算法因?yàn)槠錈o(wú)偏性，估計(jì)方差最小，實(shí)時(shí)性好等優(yōu)點(diǎn)廣泛應(yīng)用于各種控制領(lǐng)域，也是四旋翼無(wú)人機(jī)姿態(tài)解算常用的一種算法。

2.1? 卡爾曼濾波算法推導(dǎo)

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

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

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

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

在估值的過(guò)程中Q和R值是很重要的，Q和R的增大或減小都會(huì)對(duì)結(jié)果造成影響。

2.2? MATLAB仿真分析

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

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

3? 結(jié)論

在無(wú)人機(jī)姿態(tài)解算中常用的互補(bǔ)濾波和卡爾曼濾波方法都能對(duì)信號(hào)進(jìn)行處理，兩種方法側(cè)重點(diǎn)各不相同。

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

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

（3）卡爾曼濾波算法使用的關(guān)鍵是配置好Q值和R值，Q值大的時(shí)候?yàn)V波效率較高此時(shí)觀測(cè)信息占比較大，R值大的時(shí)候?qū)υ肼暤奶幚硇Ч么藭r(shí)預(yù)測(cè)信息占比較大。

參考文獻(xiàn)

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

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

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

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

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

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

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

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

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

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