南余榮,宛冬晴,潘 帥,姚亞楠

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

二階互補(bǔ)濾波姿態(tài)求解器的設(shè)計

南余榮,宛冬晴,潘 帥,姚亞楠

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

為了降低小型四旋翼姿態(tài)解算的成本和提高姿態(tài)輸出精度，采用了低成本MPU-9250作為IMU，設(shè)計了一種二階互補(bǔ)濾波姿態(tài)求解器.二階互補(bǔ)濾波算法所產(chǎn)生的計算量不大并且對處理芯片的性能要求不高，從而達(dá)到降低成本的目的.通過實(shí)際測試,以成品AHRS100作為航姿參考系統(tǒng)，得出姿態(tài)角度動態(tài)誤差在3°以內(nèi).實(shí)驗(yàn)結(jié)果表明：所設(shè)計的姿態(tài)求解器能較精確地解算出四旋翼的姿態(tài)角度，滿足了小型四旋翼對輸出姿態(tài)精度的要求.

四旋翼；二階互補(bǔ)濾波；姿態(tài)角度

隨著科技的進(jìn)步和時代的發(fā)展，無人機(jī)憑借自身特有的優(yōu)勢，在民用軍事和農(nóng)業(yè)中得到了廣泛的應(yīng)用[1]，例如可以用無人機(jī)進(jìn)行航拍攝影，電力巡檢，新聞報告，噴灑農(nóng)藥，偵查敵情，還可以用無人機(jī)來送快遞，在這些方面無人機(jī)有著人工所不具備的特有優(yōu)勢，無人機(jī)成為當(dāng)下科技行業(yè)研究的熱點(diǎn).一直以來，在諸多研究問題中，飛行器的姿態(tài)解算問題都是研究的重點(diǎn)[2-3].在小型四旋翼姿態(tài)解算中一般采用低成本捷聯(lián)慣性導(dǎo)航測量單元，主要由陀螺儀，磁力計和加速度計組成，陀螺儀具有很好的動態(tài)跟蹤特性[4-5]，在飛行器飛行過程中測得的角速度會比較準(zhǔn)確，實(shí)際上單獨(dú)用陀螺儀也能解算出四旋翼的姿態(tài)，但是由陀螺儀測量得到的積分角度會產(chǎn)生誤差的累積，如果不加以補(bǔ)償，長時間可能引起嚴(yán)重的偏差，所以筆者利用加速度計和磁力計對陀螺儀解算的角度進(jìn)行誤差補(bǔ)償[6-7].

計算四旋翼在三維空間的旋轉(zhuǎn)有很多種等價的表示方法，常見的有方向余弦、四元數(shù)和歐拉角[8].歐拉角雖然簡單易于表示和理解，但是由于其自身的姿態(tài)表示特點(diǎn)導(dǎo)致萬向節(jié)死鎖，不能全方位地解算出四旋翼的姿態(tài)方位，方向余弦法雖然能滿足四旋翼全方位姿態(tài)解算的要求，但是需要的運(yùn)算量太大[9-10]，在采用低成本處理芯片的平臺中，導(dǎo)致姿態(tài)不能實(shí)時輸出.故筆者采用四元數(shù)來表示姿態(tài)變化，四元數(shù)有一些其他角位移表示方法所沒有的優(yōu)點(diǎn)如平滑插值，能和矩陣形式快速轉(zhuǎn)換等[11-12].融合加速度計，陀螺儀和磁力計測得數(shù)據(jù)的方法有一階互補(bǔ)濾波，二階互補(bǔ)濾波和擴(kuò)展卡爾曼濾波等[13]，擴(kuò)展卡爾曼算法雖然比較先進(jìn)，解算出來的角度能很好地跟蹤飛行器真實(shí)的角度，但其設(shè)計復(fù)雜，計算量大，在小型四旋翼中普遍使用的低成本慣性測量單元不滿足其對IMU性能的要求[14].一階互補(bǔ)濾波算法雖然設(shè)計簡單，易于調(diào)試，只需調(diào)節(jié)一個參數(shù)，但解算精確度差.故筆者采用了二階互補(bǔ)濾波算法，以STM32F103C8T6為CPU，IMU采用MPU-9250，這兩個模塊累計成本在30元左右，而卡爾曼濾波算法采用的CPU和IMU模塊累計成本普遍在80～120元之間，加入PI環(huán)節(jié)補(bǔ)償單獨(dú)用陀螺儀測量所產(chǎn)生的誤差，通過調(diào)節(jié)兩個參數(shù)KP和Ki最終解算出了較精確的姿態(tài)[15-16]，滿足了小型低成本四旋翼飛行器求解姿態(tài)過程中對成本和精度的綜合要求.

1 系統(tǒng)建模

對四旋翼進(jìn)行姿態(tài)解算前首先要建立導(dǎo)航坐標(biāo)系n系和機(jī)體坐標(biāo)系b系，建立的導(dǎo)航坐標(biāo)系坐標(biāo)原點(diǎn)位于四旋翼的質(zhì)心，xn軸指向北，yn指向東，zn指向地即東北地坐標(biāo)系，三條軸之間都是倆倆垂直的，同理建立四旋翼的機(jī)體坐標(biāo)系，機(jī)體坐標(biāo)系和四旋翼固聯(lián)，隨著四旋翼的旋轉(zhuǎn)而旋轉(zhuǎn)，坐標(biāo)系如圖1所示.

圖1 機(jī)體坐標(biāo)系和導(dǎo)航坐標(biāo)系Fig.1 Body axes coordinate system and navigation coordination system

(1)

四元數(shù)是一種高階復(fù)數(shù)，能很方便地刻畫飛行器繞任意軸的旋轉(zhuǎn)，且計算量小.四元數(shù)q的表達(dá)式為

(2)

(3)

2 二階互補(bǔ)濾波算法設(shè)計和姿態(tài)解算過程

陀螺儀動態(tài)響應(yīng)好，積分后可測傾角，但在解算姿態(tài)的過程中，會產(chǎn)生累積誤差和零漂等，在低頻段信號不好.磁力計和加速度在低頻段表現(xiàn)優(yōu)秀，但是瞬時特性很差，當(dāng)四旋翼姿態(tài)劇烈變化時，不能滿足四旋翼實(shí)時解算的要求.因此可以結(jié)合它們分別在高頻段和低頻段的優(yōu)勢，利用互補(bǔ)濾波原理對陀螺儀測得的姿態(tài)角度進(jìn)行高通濾波，對磁力計和加速度計測得的姿態(tài)角度進(jìn)行低通濾波，得到在高頻段和低頻段都較好的信號，濾波原理如圖2所示.

圖2 二階互補(bǔ)濾波原理Fig.2 The principle of second order complementary filter algorithm

由圖(2)可以進(jìn)一步得到二階互補(bǔ)濾波公式為

(4)

(5)

(6)

總的誤差向量e為

(7)

式中：e1為加速度計測得的單位向量與從四元數(shù)那里獲得的重力向量之間的誤差向量；e2為磁力計測得的單位向量與從四元數(shù)那里得到的單位向量之間的誤差向量.令δ=(Kp+Ki/s)·e，陀螺儀測得的角速度為wg，將δ與wg進(jìn)行疊加得到修正后的角速度w，最后來更新四元數(shù)，得

(8)

式中w=0+wxi+wyj+wzk，由式(8)可以得到四元數(shù)的迭代方程式為

(9)

由式(9)可知：只需傳入角速度在三個軸的分量和周期T即可求得下一周期的四元數(shù)值，將求得的四元數(shù)進(jìn)行單位化，得到最終表示四旋翼姿態(tài)的歐拉角為

(10)

詳細(xì)的姿態(tài)解算過程，如圖3所示.

圖3 姿態(tài)解算過程Fig.3 The process of attitude calculation

3 實(shí)驗(yàn)測試和結(jié)果分析

3.1 實(shí)驗(yàn)平臺

我們選用STM32F103C8T6為CPU，IMU采用MPU-9250，MPU-9250是一個QFN封裝的復(fù)合芯片，它由兩部分組成，一組是3軸加速度計和3軸陀螺儀，另一組則是AKM公司的AK896 3軸磁力計.采用AHRS100成品航姿系統(tǒng)作為參考系統(tǒng)進(jìn)行對比，AHRS100在各種惡劣條件下均能精確解算出四旋翼的姿態(tài).

3.2 測試結(jié)果和分析

將一階互補(bǔ)濾波算法和二階互補(bǔ)濾波算法的廉價IMU單元與AHRS100固定在四旋翼上并在實(shí)際場景下飛行，通過計算機(jī)串口同時讀取3個測量單元的數(shù)據(jù)進(jìn)行實(shí)時姿態(tài)解算，采樣時間為60s，以AHRS100成品航姿系統(tǒng)測得的姿態(tài)數(shù)據(jù)為參考標(biāo)準(zhǔn)進(jìn)行MATLAB繪圖.圖4為一階互補(bǔ)濾波姿態(tài)求解器、二階互補(bǔ)濾波姿態(tài)求解器和參考航姿系統(tǒng)所測得的俯仰角輸出對比情況，進(jìn)一步利用MATLAB內(nèi)置的計算工具，求出俯仰角誤差，如圖5所示，可以很明顯看到大部分時間里一階解算角度誤差都是大于二階解算角度誤差，二階解算角度誤差第4s和第6s左右達(dá)到最大，接近2°.而一階解算角度最大誤差在第25s達(dá)到了3.2°.

圖4 俯仰角對比情況Fig.4 Contrast of pitch angle

圖5 俯仰角誤差Fig.5 Error of pitch angle

圖6為測得的橫滾角輸出對比情況，進(jìn)一步繪制出橫滾角誤差(圖7)，可以很明顯看到大部分時間里一階解算角度誤差都是大于二階解算角度誤差.在幾個特殊的時間點(diǎn)由于飛行器姿態(tài)變化劇烈導(dǎo)致姿態(tài)求解角度和真實(shí)角度偏差較大，在第18s左右二階解算角度誤差達(dá)到最大，達(dá)到2.3°.而一階解算角度最大誤差在第39s左右達(dá)到了4.3°.

圖6 橫滾角對比情況Fig.6 Contrast of roll angle

圖7 橫滾角誤差Fig.7 Error of roll angle

圖8為測得的偏航角輸出對比情況，進(jìn)一步繪制出偏航角誤差，如圖9所示，可以很明顯看到大部分時間里一階解算角度誤差都是大于二階解算角度誤差.二階解算角度誤差大多在1.5°以內(nèi).而一階解算角度誤差在很多時刻都超過了1.5°，解算精度明顯低于二階解算精度.

圖8 偏航角對比情況圖Fig.8 Contrast of yaw angle

圖9 偏航角誤差Fig.9 Error of yaw angle

可以得到四旋翼在飛行過程中，無論是俯仰角，橫滾角還是偏航角，一階解算的精確度都是明顯低于二階解算的精確度.大多數(shù)時間內(nèi)二階解算俯仰角誤差，橫滾角誤差和偏航角誤差分別控制在2°，1.5°，1.5°以內(nèi)，姿態(tài)角最大動態(tài)誤差小于2.5°，滿足小型低成本四旋翼飛行器對輸出姿態(tài)精度的要求.

4 結(jié) 論

針對小型四旋翼姿態(tài)解算這一問題進(jìn)行了詳細(xì)的分析，通過對比一階互補(bǔ)濾波算法，卡爾曼濾波算法，二階互補(bǔ)濾波算法最后設(shè)計了一種基于四元數(shù)的二階互補(bǔ)濾波姿態(tài)求解器，并對解算的原理和流程做了詳細(xì)介紹.以四旋翼飛行器為實(shí)驗(yàn)平臺，通過實(shí)際飛行測試，對比二階互補(bǔ)濾波姿態(tài)求解器和一階互補(bǔ)濾波姿態(tài)求解器解算出的角度誤差，得出了二階解算精度明顯高于一階解算精度的結(jié)論，驗(yàn)證了筆者所設(shè)計的二階互補(bǔ)濾波算法在提升輸出姿態(tài)精度上的有效性.

[1] 郭曉鴻，楊忠，陳喆，等.EKF和互補(bǔ)濾波在飛行姿態(tài)確定中的應(yīng)用[J].傳感器與微系統(tǒng)，2011，30(11):56-58.

[2] 王立波.捷聯(lián)慣導(dǎo)系統(tǒng)設(shè)計與數(shù)據(jù)處理技術(shù)研究[M].北京：科學(xué)出版社，2006.

[3] 史智寧，韓波，徐玉，等.基于重力場自適應(yīng)互補(bǔ)濾波的無人直升機(jī)水平姿態(tài)估計方法[J].傳感技術(shù)學(xué)報，2009，22(7):993-996.

[4] 方路平，高坤，潘清，等.基于陀螺儀的穿刺手術(shù)導(dǎo)航系統(tǒng)設(shè)計[J].浙江工業(yè)大學(xué)學(xué)報，2016，44(2)：129-133.

[5] 王宇，董輝，徐建明，等.一種新的智能小車控制策略[J].浙江工業(yè)大學(xué)學(xué)報，2012，40(2)：217-221.

[6] 胥芳，倪紫京，占紅武.自平衡小車的模型分析與數(shù)據(jù)處理[J].浙江工業(yè)大學(xué)學(xué)報，2016，44(3)：288-291.

[7] 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程.2008，16(10):1963-1970.

[8] 張承岫，李鐵鷹，王耀力.基于MPU6050和互補(bǔ)濾波的四旋翼飛控系統(tǒng)設(shè)計[J].傳感技術(shù)學(xué)報，2016，29(7)：1011-1015.

[9] 蔡磊，牛小驥，張?zhí)嵘?，?MEMS慣導(dǎo)在四旋翼飛行器中導(dǎo)航性能的實(shí)驗(yàn)分析[J].傳感技術(shù)學(xué)報，2016，29(5)：711-715.

[10] 彭孝東，張鐵民，李繼宇，等.基于傳感器校正與融合的農(nóng)用小型無人機(jī)姿態(tài)估計算法[J].自動化學(xué)報，2015，41(4)：854-860.

[11] 王延?xùn)|，賈宏光.組合導(dǎo)航系統(tǒng)濾波器截斷誤差抑制方法[J].傳感技術(shù)學(xué)報，2014，27(5)：616-621.

[12] 鄭君里，楊為理，應(yīng)啟珩.信號與系統(tǒng)[M].北京：高等教育出版社，2000.

[13] 文常保，高麗紅，方吉善，等.基于改進(jìn)型限幅平均濾波法的高精度稱重系統(tǒng)研究[J].傳感技術(shù)學(xué)報，2014，27(5)：649-653.

[14] 張洪濤.四旋翼微型飛行器位姿及控制策略的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2014.

[15] 傅忠云，劉文波，孫金秋，等.自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計中的應(yīng)用[J].傳感技術(shù)學(xué)報，2014，27(5)：698-703.

[16] 彭孝東，張鐵民，李繼宇，等.基于傳感器校正與融合的農(nóng)用小型無人機(jī)姿態(tài)估計算法[J].自動化學(xué)報，2015，41(4)：854-860.

(責(zé)任編輯：陳石平)

Design of second order complementary filter attitude solver

NAN Yurong, WAN Dongqing, PAN Shuai, YAO Yanan

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

A second order complementary filter solver is designed in order to reduce the cost and improve the output precision of small quad-rotors in attitude calculation process. The Low-cost MPU-9250 is adopted as IMU in this paper. The second-order complementary filtering algorithm produces little computation and the performance requirement of the processing chip isn’t high. It will achieve the purpose of reducing cost. Through the practical test, using AHRS100 as attitude reference system, and the dynamic error of the attitude angle is less than 3°. The experimental result show that the solver designed in this paper can calculate the attitude angle fairly accurately and meet the requirement of output precision of small quad-rotors.

quad-rotors; second order complementary filter; attitude angle

2016-11-30

浙江省自然科學(xué)基金資助項(xiàng)目(Z12E070003)

南余榮(1966—)，男，浙江樂清人，教授，博士，研究方向?yàn)殡娏鲃蛹捌渥詣踊?，E-mail：nyr@zjut.edu.cn.

TP273

A

1006-4303(2017)04-0416-05