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

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

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

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

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

0　引言

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

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

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

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

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

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

定義四個(gè)獨(dú)立的控制輸入U(xiǎn)=（U1，U2，U3，U4），分別控制四旋翼飛行器的爬升、橫滾、俯仰、偏航四種基本運(yùn)動(dòng)方式，其他的運(yùn)動(dòng)方式都可分解為這四種運(yùn)動(dòng)

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

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

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

2.1模型的推導(dǎo)

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

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

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

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

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

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

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

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

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

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

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

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

其狀態(tài)方程

觀測(cè)方程

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

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

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

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

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

4　結(jié)論

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

［1］殷強(qiáng).四旋翼無(wú)人機(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］付夢(mèng)印，鄧志紅，閆莉萍.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］徐田來(lái)，游文虎，崔平遠(yuǎn).基于模糊自適應(yīng)卡爾曼濾波的INS/ GPS組合導(dǎo)航系統(tǒng)算法研究［J］.宇航學(xué)報(bào)，2005，9（5）：571-575.

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

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

［10］陳航科，張東升，盛曉超，等.基于Kalman濾波算法的姿態(tài)傳感器信號(hào)融合技術(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)化項(xiàng)目（2012CK1003）；國(guó)家科技支撐計(jì)劃課題資助項(xiàng)目（2014BAD06B07）

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