雙因子自適應濾波算法在四旋翼上的應用*

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

雙因子自適應濾波算法在四旋翼上的應用*

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

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

四旋翼；自適應濾波；非線性濾波

0　引言

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

本文分析了四旋翼動力學模型后，在抗差自適應卡爾曼濾波的基礎上引入兩個自適應因子。它們分別調節四旋翼的動力學模型誤差和觀測系統誤差。仿真和實驗表明，雙因子自適應濾波算法顯著地提高了四旋翼的精度和穩定性。

1　建立多旋翼數學模型

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

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

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

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

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

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

2　雙因子自適應濾波算法

2.1模型的推導

系統的狀態方程和觀測方程，用一般式表示如下［7］

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

狀態向量和觀測向量的誤差方程

式中ΥXk和Υk分別為反映動力學模型和觀測值的誤差。根據最小二乘法，構造損失函數

式中Nk，Mk分別為狀態估計向量和狀態預測向量的權矩陣；λk和μk分別為k時刻的觀測自適應因子和動力學模型自適應因子。

2.2自適應因子的選取

自適應因子的選取方法有很多，指數型兩段法具有結構簡單明了等優點。本文參照指數型兩段法選取動力學模型的自適應因子和觀測模型的自適應因子［10］為

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

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

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

3　仿真建模與結果分析

結合四旋翼的動力學模型構建狀態方程和觀測方程。狀態變量

其狀態方程

觀測方程

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

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

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

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

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

4　結論

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

［1］殷強.四旋翼無人機自主控制系統研究［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］于飛，阮雙雙.自適應數字濾波技術在光纖陀螺SINS中的應用［J］.傳感器與微系統，2014，33（4）：154-157.

［4］付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導航系統中的應用［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］徐田來，游文虎，崔平遠.基于模糊自適應卡爾曼濾波的INS/ GPS組合導航系統算法研究［J］.宇航學報，2005，9（5）：571-575.

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

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

［10］陳航科，張東升，盛曉超，等.基于Kalman濾波算法的姿態傳感器信號融合技術研究［J］.傳感器與微系統，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

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

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