雙因子自適應(yīng)濾波算法在四旋翼上的應(yīng)用*

謝習(xí)華，唐　順（.中南大學(xué)高性能復(fù)雜制造國家重點實驗室，湖南長沙40083；2.山河智能裝備股份有限公司，湖南長沙4000；3.南方糧油作物協(xié)同創(chuàng)新中心，湖南長沙4000）

雙因子自適應(yīng)濾波算法在四旋翼上的應(yīng)用*

謝習(xí)華1，2，3，唐順1
（1.中南大學(xué)高性能復(fù)雜制造國家重點實驗室，湖南長沙410083；2.山河智能裝備股份有限公司，湖南長沙410100；3.南方糧油作物協(xié)同創(chuàng)新中心，湖南長沙410100）

針對動力學(xué)模型的不精準(zhǔn)和觀測存在誤差問題，建立四旋翼的動力學(xué)模型和雙因子自適應(yīng)濾波模型，采用兩個因子分別調(diào)節(jié)動力學(xué)模型和觀測模型對濾波的影響，通過仿真，對比卡爾曼、擴(kuò)展卡爾曼濾波算法。結(jié)果表明：雙因子自適應(yīng)濾波算法在誤差與穩(wěn)定性方面均有所提高。

四旋翼；自適應(yīng)濾波；非線性濾波

0　引言

四旋翼（quadrotor）是一種特殊的無人機(jī)，具有垂直起降、結(jié)構(gòu)簡單、操作容易等特點，廣泛地應(yīng)用于偵查、救援、航拍等任務(wù)中［1］。濾波技術(shù)是制約四旋翼高精度飛行的一個重要問題，在濾波問題上，國內(nèi)外很多學(xué)者進(jìn)行了廣泛的研究，提出了一系列的濾波算法，如卡爾曼濾波、擴(kuò)展卡爾曼濾波、自適應(yīng)濾波、粒子濾波等，并取得了顯著的成果［2～5］。四旋翼的動力學(xué)模型復(fù)雜，具有強耦合和非線性等特點，卡爾曼濾波廣泛應(yīng)用與線性模型中，對于四旋翼其濾波誤差較大。擴(kuò)展卡爾曼濾波可適用于非線性模型，但其高階截斷誤差會較大程度地影響四旋翼的精度和穩(wěn)定性［6］。自適應(yīng)濾波是引入一個調(diào)節(jié)因子來均衡調(diào)節(jié)動力學(xué)模型的不精準(zhǔn)和觀測模型的誤差對濾波估計值的影響［7］?？共钭赃m應(yīng)濾波的關(guān)鍵步驟是：1）求解狀態(tài)參數(shù)抗差解；2）求出自適應(yīng)因子；3）根據(jù)自適應(yīng)因子求解狀態(tài)參數(shù)［8，9］。

本文分析了四旋翼動力學(xué)模型后，在抗差自適應(yīng)卡爾曼濾波的基礎(chǔ)上引入兩個自適應(yīng)因子。它們分別調(diào)節(jié)四旋翼的動力學(xué)模型誤差和觀測系統(tǒng)誤差。仿真和實驗表明，雙因子自適應(yīng)濾波算法顯著地提高了四旋翼的精度和穩(wěn)定性。

1　建立多旋翼數(shù)學(xué)模型

為了簡單、方便、精準(zhǔn)地建立四旋翼的動力學(xué)模型，現(xiàn)做以下幾個假設(shè)：1）四旋翼是理想的剛體結(jié)構(gòu)，不變形；2）四旋翼沿不相鄰的電機(jī)之間的軸完全對稱，切質(zhì)點與幾何中心重合；3）四旋翼所受的阻力、重力加速度gn和飛行高度無關(guān)。

建立兩個北東天坐標(biāo)系，機(jī)體坐標(biāo)系B（ObXbYbZb）固連在四旋翼上和地面坐標(biāo)系E（OeXeYeZe）原點與四旋翼起飛前的質(zhì)點重合。定義P=（x，y，z）為四旋翼飛行器在地面坐標(biāo)系中的位置；（φ，θ，ψ）分別是滾轉(zhuǎn)角、俯仰角和偏航角；Ω=（p，q，r）為機(jī)體坐標(biāo)系的機(jī)體角速度。分析四旋翼在飛行過程中受到的升力、重力、空氣阻力、向心力，由牛頓第二定律和歐拉方程得到四旋翼飛行器的動力學(xué)方程為［1］

飛行器的歐拉角和機(jī)體角速度之間的關(guān)系為［1］

定義四個獨立的控制輸入U=（U1，U2，U3，U4），分別控制四旋翼飛行器的爬升、橫滾、俯仰、偏航四種基本運動方式，其他的運動方式都可分解為這四種運動

綜合式（1）、式（2）、式（3），得出四旋翼飛行器的動力學(xué)［1］

式中ξ=ω1+ω2-ω3-ω4。

2　雙因子自適應(yīng)濾波算法

2.1模型的推導(dǎo)

系統(tǒng)的狀態(tài)方程和觀測方程，用一般式表示如下［7］

式中Xk為系統(tǒng)的k時刻的狀態(tài)變量；Γ（k，k-1）為系統(tǒng)的狀態(tài)矩陣；Υ（k，k-1）為系統(tǒng)的噪音矩陣；Yk為系統(tǒng)的觀測變量；H為觀測矩陣；ek為觀測噪音。

狀態(tài)向量和觀測向量的誤差方程

式中ΥXk和Υk分別為反映動力學(xué)模型和觀測值的誤差。根據(jù)最小二乘法，構(gòu)造損失函數(shù)

式中Nk，Mk分別為狀態(tài)估計向量和狀態(tài)預(yù)測向量的權(quán)矩陣；λk和μk分別為k時刻的觀測自適應(yīng)因子和動力學(xué)模型自適應(yīng)因子。

2.2自適應(yīng)因子的選取

自適應(yīng)因子的選取方法有很多，指數(shù)型兩段法具有結(jié)構(gòu)簡單明了等優(yōu)點。本文參照指數(shù)型兩段法選取動力學(xué)模型的自適應(yīng)因子和觀測模型的自適應(yīng)因子［10］為

式中λki∈［0，1］，μki∈［0，1］。

選取觀測模型自適應(yīng)因子λk，觀測向量誤差的第i項Υki反映了k時刻，狀態(tài)變量的第i項的觀測殘差，對其進(jìn)行標(biāo)準(zhǔn)化處理。，δ為Υki的均方差。閾值Λλ的取值定義為［1.0，4.0］

動力學(xué)模型自適應(yīng)因子μk的選取與觀測模型自適應(yīng)因子選取過程一樣

3　仿真建模與結(jié)果分析

結(jié)合四旋翼的動力學(xué)模型構(gòu)建狀態(tài)方程和觀測方程。狀態(tài)變量

其狀態(tài)方程

觀測方程

設(shè)定四旋翼三個空間位置的初始值 x（0）=40 m，y（0）=40 m，z（0）=3 m，在初始位置附近進(jìn)行小范圍的運動，分別使用卡爾曼、擴(kuò)展卡爾曼、雙因子自適應(yīng)三種方法進(jìn)行濾波。根據(jù)抗差自適應(yīng)卡爾曼的核心公式編寫時間更新和測量更新的代碼進(jìn)行仿真。觀測模型自適應(yīng)因子Λλ取1.5，動力學(xué)模型自適應(yīng)因子Λμ取2。水平與高度方向的誤差曲線類似，本文選取高度方向的誤差曲線作為分析對象。

從圖1中可以看出：標(biāo)準(zhǔn)卡爾曼濾波算法對四旋翼位置濾波上有偏差，且波動大，不穩(wěn)定，出現(xiàn)了發(fā)散點的情況。對比圖2、圖3，擴(kuò)展卡爾曼濾波算法和雙因子自適應(yīng)濾波算法對四旋翼的位置濾波都有較好的效果。對比它們的誤差平均值和均方差，可以看出雙因子自適應(yīng)濾波在偏差和穩(wěn)定性上比擴(kuò)展卡爾曼濾波提高了5%以上。擴(kuò)展卡爾曼濾波算法能適用用于四旋翼的非線性系統(tǒng)，但它截斷了高階非線性特征，導(dǎo)致濾波的偏差和穩(wěn)定性有所下降。雙因子自適應(yīng)濾波算法從觀測誤差和動力學(xué)模型不精準(zhǔn)的兩個方面共同抑制誤差，提升了濾波的偏差和穩(wěn)定性。

圖1　標(biāo)準(zhǔn)卡爾曼濾波估計誤差Fig 1　Estimation error of standard Kalman filtering

圖2　擴(kuò)展卡爾曼濾波估計誤差Fig 2　Estimation error of extended Kalman filtering

圖3　雙因子自適應(yīng)濾波估計誤差Fig 3　Estimation error of two-factor adaptive filtering

4　結(jié)論

本文建立四旋翼的動力學(xué)模型，并將雙因子自適應(yīng)濾波應(yīng)用于所建立模型的位置濾波上。通過兩個因子分別調(diào)整四旋翼的動力學(xué)誤差和觀測方面的誤差，并對四旋翼進(jìn)行了動態(tài)模型仿真。結(jié)果表明：雙因子自適應(yīng)濾波算法能較好地處理四旋翼的非線性濾波問題，在誤差與穩(wěn)定性方面也優(yōu)于擴(kuò)展卡爾曼濾波算法。

［1］殷強.四旋翼無人機(jī)自主控制系統(tǒng)研究［D］.天津：天津大學(xué)，2011.

［2］Cabecinhas D，Cunha R，Silvestre C.A nonlinear quadrotor trajectory tracking controller with disturbance rejection［J］.Control Engineering Practice，2014，26：1-10.

［3］于飛，阮雙雙.自適應(yīng)數(shù)字濾波技術(shù)在光纖陀螺SINS中的應(yīng)用［J］.傳感器與微系統(tǒng)，2014，33（4）：154-157.

［4］付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京：科學(xué)出版社，2010：4.

［5］Kova X，Kacmarik P R P.Interoperable GPS，GLONASS and Galileo software receiver［J］.IEEE Aerospace and Electronic Systems Magazine，2011，26（4）：24-30.

［6］Song Qi，Han Jianda.An adaptive UKF algorithm for the state and parameter estimations of a mobile robot［J］.Acta Automatica Sinica，2008，34（1）：72-79.

［7］徐田來，游文虎，崔平遠(yuǎn).基于模糊自適應(yīng)卡爾曼濾波的INS/ GPS組合導(dǎo)航系統(tǒng)算法研究［J］.宇航學(xué)報，2005，9（5）：571-575.

［8］李輝，蘆利斌，金國棟.基于Kinect的四旋翼無人機(jī)體感控制［J］.傳感器與微系統(tǒng)，2015，34（8）：99-102.

［9］韓厚增，王堅.自適應(yīng)UKF在GNSS/INS緊組合導(dǎo)航中的應(yīng)用研究［J］.大地測量與地球動力學(xué)，2013，12（6）：98-102.

［10］陳航科，張東升，盛曉超，等.基于Kalman濾波算法的姿態(tài)傳感器信號融合技術(shù)研究［J］.傳感器與微系統(tǒng)，2013，32（12）：82-85，89.

DOI：10.13873/J.1000—9787（2016）06—0149—02

Application of two-factor adaptive filtering algorithm in quadrotor*

XIE Xi-hua1，2，3，TANG Shun1
（1.State Key Laboratory of High Performance Complicated Manufacturing，Central South University，Changsha 410083，China；2.Sunward Intelligent Equipment Co Ltd，Changsha 410100，China；3.Collaborative Innovation Center for Southern Grain and Oil Crop，Changsha 410100，China）

Aiming at problems of inaccurate dynamic model and observation error，quadrotor dynamic model and two-factor adaptive filtering model are set up，using two factors seperately adjust effects of dynamic and observation model on filtering，through simulation，compared with Kalman filtering，extended Kalman filtering algorithm. Results show that two-factor adaptive filtering algorithm are improved in error and stability.

quadrotor；adaptive filtering；non-linear filtering

TP18

A

1000—9787（2016）06—0146—03

10.13873/J.1000—9787（2016）06—0146—03

2015—09—07

湖南省重大科技成果轉(zhuǎn)化項目（2012CK1003）；國家科技支撐計劃課題資助項目（2014BAD06B07）

謝習(xí)華（1969-），男，博士，碩士生導(dǎo)師，研究方向為工業(yè)機(jī)器人控制、機(jī)電液一體化。