謝習(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)濾波;非線性濾波
四旋翼(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)定性。
為了簡單、方便、精準(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.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)因子選取過程一樣
結(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
本文建立四旋翼的動力學(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ī)電液一體化。