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

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

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

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

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

0　引言

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

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

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

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

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

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

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

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

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

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

2.1模型的推導

系統(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分別為反映動力學模型和觀測值的誤差。根據(jù)最小二乘法，構(gòu)造損失函數(shù)

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

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

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

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

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

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

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

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

其狀態(tài)方程

觀測方程

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

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

圖1　標準卡爾曼濾波估計誤差Fig 1　Estimation error of standard Kalman filtering

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

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

4　結(jié)論

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

［1］殷強.四旋翼無人機自主控制系統(tǒng)研究［D］.天津：天津大學，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濾波理論及其在導航系統(tǒng)中的應(yīng)用［M］.北京：科學出版社，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］徐田來，游文虎，崔平遠.基于模糊自適應(yīng)卡爾曼濾波的INS/ GPS組合導航系統(tǒng)算法研究［J］.宇航學報，2005，9（5）：571-575.

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

［9］韓厚增，王堅.自適應(yīng)UKF在GNSS/INS緊組合導航中的應(yīng)用研究［J］.大地測量與地球動力學，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）

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