李漢博，秦文華

(曲阜師范大學(xué) 物理工程學(xué)院，山東 曲阜 273165)

四旋翼慣性導(dǎo)航姿態(tài)解算算法的改進(jìn)與仿真

李漢博，秦文華

(曲阜師范大學(xué) 物理工程學(xué)院，山東 曲阜 273165)

為了實(shí)現(xiàn)四旋翼飛行器的高精度導(dǎo)航，提出了互補(bǔ)濾波法和四元數(shù)算法對(duì)傳感器獲得的數(shù)據(jù)進(jìn)行修正，最大限度的抑制干擾誤差并提高姿態(tài)角解算的準(zhǔn)確度;首先簡(jiǎn)單推導(dǎo)了捷聯(lián)式慣性導(dǎo)航系統(tǒng)的算法基本原理并利用互補(bǔ)濾波算法進(jìn)行改進(jìn)，然后給出了慣性導(dǎo)航系統(tǒng)的力學(xué)編排模型分析旋翼飛行器的運(yùn)動(dòng)姿態(tài);最后仿真驗(yàn)證數(shù)據(jù)選用慣性儀表MPU6050和HMC5883所得到實(shí)測(cè)數(shù)據(jù)采集并進(jìn)行仿真分析，平臺(tái)處理器選用STM32來(lái)仿真慣性儀表的測(cè)量速度，最終得到實(shí)驗(yàn)結(jié)果證明算法可行性。

四旋翼飛行器；互補(bǔ)濾波法；四元數(shù)算法；傳感器；姿態(tài)角

0 引言

近些年來(lái)，隨著智能化產(chǎn)業(yè)迅速的發(fā)展，無(wú)人飛行器開始逐漸的走進(jìn)人們視線。四旋翼飛行器(quadrotor)擁有4個(gè)旋轉(zhuǎn)螺旋槳,前后的兩個(gè)螺旋槳按照順時(shí)針?lè)较蛐D(zhuǎn),左右的兩個(gè)螺旋槳按照逆時(shí)針?lè)较蛐D(zhuǎn)[1]。同時(shí)兩個(gè)軸向電機(jī)馬達(dá)反向的旋轉(zhuǎn)方式相互抵消彼此的轉(zhuǎn)矩,使得四旋翼飛行器能夠在半空中保持懸停。前后的螺旋槳所產(chǎn)生的推力差別可以使其做俯仰動(dòng)作,而翻滾動(dòng)作則是由于左右的螺旋槳推力的差別所產(chǎn)生的;其本身通過(guò)改變2個(gè)軸向轉(zhuǎn)速來(lái)實(shí)現(xiàn)飛行器的自旋。四旋翼飛行器結(jié)構(gòu)簡(jiǎn)單并且可靠，具有定點(diǎn)懸停、垂直起降、在小空間內(nèi)飛行還有穩(wěn)定低速飛行等特點(diǎn)， 受到了廣泛關(guān)注，并且在很多個(gè)領(lǐng)域獲得到了廣泛應(yīng)用。

但是由于四旋翼無(wú)人飛行器所使用的 MEMS 傳感器的精度較低，尤其受 MEMS 的陀螺儀逐次的啟動(dòng)參數(shù)誤差以及零偏不穩(wěn)定等因素的影響，系統(tǒng)所得到姿態(tài)參數(shù)發(fā)散較快，難以滿足對(duì)導(dǎo)航精度和速度的要求[2]，所以本設(shè)計(jì)基于以上情況，創(chuàng)新性的提出了使用互補(bǔ)濾波算法[3]對(duì)傳感器的采集數(shù)據(jù)進(jìn)行修正。給出了設(shè)計(jì)中慣性導(dǎo)航系統(tǒng)的力學(xué)編排模型和分析，系統(tǒng)解算過(guò)程的影響因素，對(duì)算法進(jìn)行了改進(jìn)來(lái)消除對(duì)系統(tǒng)的影響。最后通過(guò)Matlab軟件對(duì)本設(shè)計(jì)中使用的算法進(jìn)行了仿真驗(yàn)證。

1 導(dǎo)航解算算法的原理與分析

1.1 導(dǎo)航解算算法基礎(chǔ)

導(dǎo)航算法包含獲取姿態(tài)的算法、得到速度算法、獲得位置的算法等。我們采用獲取姿態(tài)的算法進(jìn)行導(dǎo)航解算來(lái)獲取飛行器的位置，獲取姿態(tài)的過(guò)程又稱為姿態(tài)更新，姿態(tài)更新又包括以下幾種算法：旋轉(zhuǎn)矢量法、歐拉角法、方向余弦法和四元數(shù)法，這些算法各有利弊。其中四元數(shù)法計(jì)算量相對(duì)來(lái)說(shuō)比較小，簡(jiǎn)單易于實(shí)現(xiàn)且精度相對(duì)比較高[4]。

要使用四元數(shù)法對(duì)四旋翼進(jìn)行導(dǎo)航解算，通常要選擇合適的坐標(biāo)進(jìn)行運(yùn)算處理，即慣性測(cè)量?jī)x表測(cè)得的載體相關(guān)數(shù)據(jù)必需通過(guò)一定的轉(zhuǎn)換關(guān)系轉(zhuǎn)換到地理坐標(biāo)系才能應(yīng)用于導(dǎo)航[5-6]。常用的坐標(biāo)系有地心坐標(biāo)系(i系，慣性參考坐標(biāo)系[7])，地球坐標(biāo)系(e系)，地理坐標(biāo)系(n系，導(dǎo)航坐標(biāo)系)，地平坐標(biāo)系(t系)，載體坐標(biāo)系(b系)。除此之外，在本設(shè)計(jì)還定義了姿態(tài)解算過(guò)程中的3個(gè)姿態(tài)角，用θ表示姿態(tài)角中的俯仰角(Pitch)，用γ表示姿態(tài)角中的橫滾角(Roll)，用φ表示姿態(tài)角中的航向角(Yaw)。

在導(dǎo)航的慣性系統(tǒng)中，需要進(jìn)行坐標(biāo)系的轉(zhuǎn)換來(lái)獲取姿態(tài)信息，姿態(tài)矩陣也就是地理坐標(biāo)系n與載體坐標(biāo)系b之間的轉(zhuǎn)換關(guān)系[8]。地理坐標(biāo)系0xnynzn坐標(biāo)繞zn軸方向旋轉(zhuǎn)，航向角φ得到坐標(biāo)0x'y'z'，0x'y'z'繞y'軸旋轉(zhuǎn)俯仰角θ得到坐標(biāo)0xnynzn，0x"y"z"再繞x"軸旋轉(zhuǎn)橫滾角γ則得到載體坐標(biāo)系0xbybzb，根據(jù)整個(gè)運(yùn)算旋轉(zhuǎn)過(guò)程的值，可以得到載體坐標(biāo)系與地理坐標(biāo)系之間的旋轉(zhuǎn)角度的運(yùn)算關(guān)系式，我們可以用方向余弦陣表示為式(1)， 由于姿態(tài)矩陣是3個(gè)姿態(tài)角的函數(shù)，即可有姿態(tài)矩陣確定姿態(tài)角主值的四元數(shù)表達(dá)式，由式(2)表示，在得到姿態(tài)角的主值之后就可以根據(jù)姿態(tài)角的定義域來(lái)確定姿態(tài)角的真值，(3)式為姿態(tài)角的真值式

(1)

(2)

(3)

1.2 導(dǎo)航解算算法中的互補(bǔ)濾波原理與應(yīng)用

在實(shí)際環(huán)境中，我們需要慣性儀表如陀螺儀和加速度計(jì)輸出的數(shù)據(jù)來(lái)進(jìn)行飛行器的導(dǎo)航姿態(tài)解算，然而它們存在一定的誤差。而在解算的過(guò)程中，這兩個(gè)器件的輸出數(shù)據(jù)是否準(zhǔn)確關(guān)系十分重大。所以對(duì)兩個(gè)器件的輸出必須經(jīng)過(guò)一定的處理以提高其精度[9-10]，就是本設(shè)計(jì)所使用的關(guān)鍵利用互補(bǔ)濾波算法來(lái)去噪濾波處理提升輸出數(shù)據(jù)的精度。

由于陀螺儀漂移的存在，會(huì)導(dǎo)致其測(cè)得的數(shù)據(jù)越來(lái)越不準(zhǔn)?；パa(bǔ)濾波就是在短時(shí)間內(nèi)較多參考陀螺儀的數(shù)據(jù)，而隨著時(shí)間的增加，對(duì)陀螺儀測(cè)得的數(shù)據(jù)的信任度降低的時(shí)候，采用加速度計(jì)測(cè)得的數(shù)據(jù)對(duì)其數(shù)據(jù)進(jìn)行補(bǔ)償校正。從原理上說(shuō)來(lái)，是由于陀螺儀和加速度計(jì)其各自的特性決定的。如加速度計(jì)由于其動(dòng)態(tài)響應(yīng)慢，在高頻段存在誤差，可以通過(guò)低通濾波器來(lái)抑制其高頻誤差；而陀螺儀由于常值零漂低頻段存在較大誤差，所以需濾波器來(lái)抑制其低頻誤差。而互補(bǔ)濾波器就是根據(jù)其在頻域上的互補(bǔ)特性來(lái)設(shè)計(jì)，以抑制干擾誤差。這樣就能夠?qū)蓚€(gè)慣性器件的優(yōu)點(diǎn)結(jié)合起來(lái)，得到更加精確的數(shù)據(jù)。通過(guò)加速計(jì)測(cè)得矢量a(三軸測(cè)量值)和參考矢量v(重力加速度在載體坐標(biāo)系的比例的分量)做叉乘可得到加速度計(jì)的測(cè)量誤差，同時(shí)對(duì)導(dǎo)航中另一傳感器電子羅盤測(cè)量矢量(三軸測(cè)量值)和參考矢量(磁場(chǎng)矢量在載體坐標(biāo)系的比例分量)做叉乘得到電子羅盤測(cè)量誤差，以上兩者相加可以得到修正陀螺儀的誤差矢量

(4)

設(shè)陀螺儀的采樣起始時(shí)刻為tk、更新周期為T，那么測(cè)量修正值可用下式表示

(5)

其中：Kp為誤差比例系數(shù)常量，Ki為誤差積分系數(shù)常量。一般的，Kp為Ki的10—100倍[11-12]。

1.3 導(dǎo)航解算算法中的四元數(shù)與姿態(tài)矩陣

上面已經(jīng)提到過(guò)利用四元數(shù)對(duì)導(dǎo)航姿態(tài)矩陣進(jìn)行解算，四元數(shù)理論是數(shù)學(xué)的一個(gè)分支，是一個(gè)實(shí)數(shù)單位為1和3個(gè)虛數(shù)單位組成的含有4個(gè)元的數(shù)，可用下式表達(dá)Q=q0+q1i+q2j+q3k。一個(gè)坐標(biāo)系相對(duì)于另外一個(gè)坐標(biāo)系的轉(zhuǎn)動(dòng)，可以用四元數(shù)唯一的表示出來(lái)。那么載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系運(yùn)動(dòng)時(shí)可如下表示

(6)

即可以得到利用四元數(shù)算法表示的姿態(tài)矩陣

(7)

也就是說(shuō)明姿態(tài)矩陣中九個(gè)元素都可以使用四元數(shù)一一對(duì)應(yīng)表示出來(lái)進(jìn)行進(jìn)一步的解算。

2 導(dǎo)航解算的力學(xué)分析與算法改進(jìn)

旋翼飛行器具有十字交叉的數(shù)個(gè)螺旋槳，它可以通過(guò)改變4個(gè)螺旋槳升力實(shí)現(xiàn)不同的運(yùn)動(dòng)，其主要方法是改變螺旋槳的轉(zhuǎn)速。旋翼有且僅有4個(gè)輸入力，卻需要產(chǎn)生6個(gè)自由度方向的運(yùn)動(dòng)，屬于典型的欠驅(qū)動(dòng)系統(tǒng)。而且四旋翼飛行器具有高度的耦合動(dòng)特性，一個(gè)螺旋槳速度發(fā)生變化，將會(huì)同時(shí)改變6個(gè)狀態(tài)量。四旋翼的姿態(tài)控制特性是發(fā)散的，升力的偏斜會(huì)使得姿態(tài)偏離中立位置越來(lái)越遠(yuǎn)，直到升力不足以支撐重力導(dǎo)致飛行器墜落。

2.1 建立理想狀態(tài)的動(dòng)力學(xué)模型

使用Simulink建立如圖1所示的旋翼飛行器的慣導(dǎo)系統(tǒng)的開環(huán)響應(yīng)模型。開環(huán)響應(yīng)模型中的姿態(tài)解算部分如圖2所示，采用了上文中所設(shè)計(jì)的算法。

圖1 慣性導(dǎo)航系統(tǒng)的開環(huán)響應(yīng)模型

圖2 姿態(tài)解算的理論模型

2.2 慣性導(dǎo)航算法的改進(jìn)

將控制信號(hào)輸入仿真模型，產(chǎn)生的三軸姿態(tài)角如圖3所示。

圖3 三軸姿態(tài)角輸出

姿態(tài)角3個(gè)方向間耦合度很小，姿態(tài)響應(yīng)較控制信號(hào)的輸入慢，姿態(tài)變化導(dǎo)致升力方向變化進(jìn)而引起旋翼機(jī)運(yùn)動(dòng)。在旋翼飛行器進(jìn)行傾斜運(yùn)動(dòng)時(shí)，需要及時(shí)進(jìn)行垂直速度補(bǔ)償，否則將會(huì)減速而下降。若消除機(jī)體姿態(tài)角解算自然發(fā)散的趨勢(shì)，就需要對(duì)輸入數(shù)據(jù)和輸出數(shù)據(jù)進(jìn)行特殊耦合，即使用前文所使用的互補(bǔ)濾波算法進(jìn)行處理互補(bǔ)濾波脈沖做為歐拉角的輸入。輸入和輸出分別如圖4和5所示，可以發(fā)現(xiàn)經(jīng)過(guò)輸入信號(hào)加入互補(bǔ)濾波

后姿態(tài)角的輸出值高度耦合，達(dá)到算法改進(jìn)的目的。

圖4 互補(bǔ)濾波控制信號(hào)輸出

圖5 改進(jìn)后姿態(tài)角輸出

3 導(dǎo)航算法的仿真和驗(yàn)證

算法的仿真驗(yàn)證選擇使用直接由慣性儀表(陀螺儀、加速度計(jì)和電子羅盤)得到的實(shí)測(cè)數(shù)據(jù)進(jìn)行仿真分析。實(shí)測(cè)數(shù)據(jù)能真實(shí)反映隨機(jī)誤差和量化誤差，如果能得到誤差相對(duì)比較小的算法，那么在實(shí)際中執(zhí)行時(shí)遇到的困難和問(wèn)題比較少。

加速度計(jì)和陀螺儀選用了InvenSence公司的6軸運(yùn)動(dòng)處理組件MPU6050，該芯片集成了3軸MEMS陀螺儀和3軸MEMS加速度計(jì)，免除了組合陀螺儀與加速器時(shí)之軸間差的問(wèn)題。

電子羅盤選用Honeywell公司的HMC5883L芯片。該芯片采用各向異性磁阻技術(shù)，具有在軸向高靈敏度和線性高精度的特點(diǎn)，測(cè)量范圍從1毫高斯到8高斯。

飛行器的采集數(shù)據(jù)平臺(tái)處理器選用STM32芯片，將已經(jīng)改進(jìn)加入互補(bǔ)濾波的算法加入芯片，進(jìn)行數(shù)據(jù)的采集和解算分析。

每次采集數(shù)據(jù)共有十二個(gè)值，分別為加速度計(jì)三軸測(cè)量值、電子羅盤三軸測(cè)量值、陀螺儀三軸測(cè)量值和通過(guò)處理器解算得到的3個(gè)姿態(tài)角，此處以俯仰角為例，波形圖如圖6第一行波形所示。

同時(shí)為了驗(yàn)證互補(bǔ)濾波算法對(duì)系統(tǒng)誤差的影響，也使用Matlab對(duì)無(wú)互補(bǔ)濾波補(bǔ)償修正算法進(jìn)行仿真驗(yàn)證，如圖6第二行波形所示，第三行為前兩行波形的誤差波形

圖6 俯仰角實(shí)測(cè)值與無(wú)互補(bǔ)濾波修正的解算值比較

圖7與圖6不同的是第二行波形是在Matlab仿真過(guò)程中已經(jīng)加入了互補(bǔ)濾波算法，可以得到已經(jīng)加入算法的實(shí)測(cè)值與仿真值誤差基本為零，證明該算法在實(shí)際操作中的可行性。

圖7 俯仰角實(shí)測(cè)值與互補(bǔ)濾波修正的解算值比較

4 結(jié)語(yǔ)

四旋翼飛行器導(dǎo)航系統(tǒng)首先需要保證導(dǎo)航姿態(tài)角的精度和可靠性，才能夠有效地融合其他傳感器的數(shù)據(jù)來(lái)估計(jì)出位置和速度。本文創(chuàng)新之處在于實(shí)現(xiàn)了一種基于互補(bǔ)濾波的算法，有效的解決了傳感器噪聲所造成的誤差問(wèn)題，提高了姿態(tài)角的準(zhǔn)確度。通過(guò)對(duì)導(dǎo)航控制系統(tǒng)的力學(xué)模型分析，以及解算過(guò)程的影響因素，對(duì)算法進(jìn)行了改進(jìn)消除了噪聲對(duì)飛行導(dǎo)航解算的影響。通過(guò)算法實(shí)測(cè)與，Matlab仿真對(duì)本設(shè)計(jì)中的算法進(jìn)行了驗(yàn)證，證明了該算法能夠有效的消除噪聲，為后續(xù)進(jìn)一步實(shí)現(xiàn)飛行控制系統(tǒng)硬線電路設(shè)計(jì)提供了基礎(chǔ)。

[1] 馬遠(yuǎn)超.四旋翼飛行器導(dǎo)航及控制技術(shù)研究 [D]. 哈爾濱:哈爾濱工程大學(xué)，2013.

[2] Brockhaus R.Flugregelung[M].NewYork:Springer-Verlag, 1994: 5-14.

[3] Robert M，Tarek H，Jean M P．Complimentary filter design on the special orthogonal group SO(3)[A]. Proceedings of the 44thIEEE Conference on Decision and Control and European Control Conference 2005[C]. Seville, Spain, Dec 12-15, 2005: 1477-1484.

[4] Madgwick S, Harrison A, Vaidyananthan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[A].IEEE International Conference on Rehabilitation Robotics[C]. Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29-July 1, 2011.

[5] 賈勝文. 基于ARM7CPU和FPGA的低成本小型捷聯(lián)慣導(dǎo)系統(tǒng)的設(shè)計(jì)[D].西安: 西北工業(yè)大學(xué), 2006.

[6] 栗瑞江. 低成本SINS/GPS組合導(dǎo)航系統(tǒng)研究[D]. 西安: 西北工業(yè)大學(xué), 2001.

[7] 鄧正隆. 慣性技術(shù)[M]. 哈爾濱: 哈爾濱工業(yè)大學(xué)出版社, 2006.

[8] 黃溪流. 一種四旋翼無(wú)人直升機(jī)飛行控制器的設(shè)計(jì)[D]. 南京: 南京理工大學(xué), 2010.

[9] Mellinger D, Michael N, Kumar V. Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors[J]. The International Journal of Robotics Research,2012,31(5): 664-674.

[10] Euston M,Coote P, Mahony R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[A]. 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems[C]. Nice, France, Sept 22-26, 2008: 340-345．

[11] 黃德鳴, 程 祿. 慣性導(dǎo)航系統(tǒng)[M]. 北京: 國(guó)防工業(yè)出版社, 1986.

[12] 雷 芳, 王 華, 焦國(guó)太, 等. 彈道修正彈藥的姿態(tài)測(cè)量技術(shù)研究[J]. 彈箭與制導(dǎo)學(xué)報(bào), 2009, 29(4): 123-125.

Improvement and Simulation of Four Rotor Aircraft Inertial Navigation Attitude Algorithm

Li Hanbo, Qin Wenhua

(Physics Engineering College, Qufu Normal University, Qufu 273165,China)

In order to implement high precision four rotor aircraft navigation that puts forward the complementary filter method and the quaternion algorithm for sensor data amended, it can restrain the interference of the maximum error and improve the accuracy of attitude Angle calculating. First of all, a simple algorithm of strapdown inertial navigation system was deduced basic principle and use the complementary filter algorithm was improved, the inertial navigation system is given and then the mechanical layout model analysis of rotor aircraft motion. Finally simulation data selection of inertial instrument MPU6050 and HMC5883 simulation analysis was made on the measured data, platform selection STM32 processor to simulate inertial instrument measurement speed, eventually get the experimental results show feasibility of the algorithm.

four-rotor aircraft ;the complementary filter ; quaternion-algorithm; sensor; attitude-angle

2015-12-14；

2016-02-15。

山東省科技發(fā)展計(jì)劃項(xiàng)目(2014GGX101027)；山東省高校計(jì)劃項(xiàng)目(J15LN08)。

李漢博(1991-)，女，山東濟(jì)寧人，碩士研究生，主要從事智能信息處理方向的研究。

秦文華(1968-)，男，山東聊城人，碩士，副教授，主要從事無(wú)線通信技術(shù)、信號(hào)的采集與處理方向的研究。

1671-4598(2016)07-0187-03

10.16526/j.cnki.11-4762/tp.2016.07.051

TP391 文獻(xiàn)標(biāo)識(shí)碼：A