李 可

(河南工程學院 工程訓練中心，河南 鄭州 451191)

基于擴展Kalman濾波的無人機位姿校正

李 可

(河南工程學院 工程訓練中心，河南 鄭州 451191)

無人機姿態(tài)控制問題是無人機穩(wěn)定性飛行的關鍵，針對無人機在位姿參數(shù)不確定條件下制導控制器姿態(tài)定位精度不高的問題，提出了一種基于擴展Kalman濾波的無人機位姿校正方法，進行制導系統(tǒng)穩(wěn)定性控制律設計.建立無人機飛行動力學模型，構建飛行彈道方程，分析無人機制導系統(tǒng)被控對象的約束參量，采用加速度計、陀螺計和磁力計進行位姿參量測量.考慮到位姿參數(shù)的不確定性，采用擴展Kalman濾波算法進行姿態(tài)參數(shù)的整定性處理，實現(xiàn)角度校正.將校正后的位姿參數(shù)輸入模糊神經網絡系統(tǒng)中，實現(xiàn)無人機制導控制律的優(yōu)化設計.仿真結果表明，采用該方法進行無人機位姿校正和飛行制導控制，定姿精度較高、抗干擾能力較強，實現(xiàn)了飛行的穩(wěn)定性控制.

無人機；制導系統(tǒng)；穩(wěn)定性控制；擴展Kalman濾波

無人機是一種采用遙控裝備進行遠程制導控制的無人駕駛飛行器，在軍事和民用領域都展示了較好的應用價值.對無人機準確、可靠地定姿控制并通過位姿參數(shù)的優(yōu)化解算和自整定性融合處理，輸入制導控制系統(tǒng)中以實現(xiàn)飛行穩(wěn)定性控制是保證飛行穩(wěn)定性的關鍵，故研究無人機制導系統(tǒng)的穩(wěn)定性控制技術在促進無人機技術革新方面具有重要意義[1].

無人機制導系統(tǒng)穩(wěn)定性控制建立在位姿參數(shù)測量和姿態(tài)修正的基礎上.無人機在飛行過程中容易受到電子雷達探測和電磁等干擾，導致位姿參數(shù)存在不確定性，需要進行穩(wěn)定性控制設計.將無人機的姿態(tài)數(shù)據通過慣導系統(tǒng)實時采集并傳輸?shù)阶丝叵到y(tǒng)，與航向陀螺儀進行信息交互，輸出控制指令，以實現(xiàn)飛行控制.傳統(tǒng)方法中，對無人機的控制算法主要有模糊PID控制[2]、自適應反演控制[3]、滑膜積分控制和姿態(tài)融合濾波控制[4]等，通過姿態(tài)參量解算設計控制器取得了較好的姿態(tài)穩(wěn)定性.文獻[5]提出了一種基于姿態(tài)融合濾波的無人機抗干擾控制算法，構建無人機在飛行時的姿態(tài)參量約束運動方程，通過姿態(tài)信息融合，以捷聯(lián)慣導傳輸?shù)綀?zhí)行器實現(xiàn)飛行控制優(yōu)化，但該方法對氣流小擾動等不確定因素的抗干擾能力不強；文獻[6]提出了一種基于推力矢量的飛機縱向魯棒動態(tài)逆控制模型，通過推力矢量的坐標變換得到飛行縱向模型，在線性不確定系統(tǒng)中進行無人機動態(tài)逆控制，改進的控制方法回避了時標分離問題，進行小擾動耦合處理，提高了飛行控制的抗干擾能力，但該方法計算開銷較大且控制的實時性不好.

本研究針對上述問題提出了一種基于擴展Kalman濾波(extended Kalman filter,EKF)的無人機制導系統(tǒng)穩(wěn)定性控制律設計方法，首先進行飛行動力學和被控對象分析，然后進行控制約束參量分析，構建改進的控制律，最后進行仿真測試，得出有效性結論.

1 飛行動力學分析與被控對象描述

1.1無人機飛行動力學方程

(1)

(2)

(3)

在采用姿態(tài)傳感器進行飛行位姿參量測量和采集時，綜合考慮飛機自身重力、流體動力、飛行推力等慣性力矩的影響，得到飛行彈道的非線性方程組：

(4)

1.2被控對象描述

采用加速度計、陀螺計和磁力計進行位姿參量測量，得到位姿參數(shù)模型，針對位姿參數(shù)的不確定性進行誤差補償和濾波處理，進行位姿參數(shù)的自整定性控制，采用改進的Kalman濾波算法實現(xiàn)角度校正，得到被動對象結構，如圖1所示.

圖1 無人機穩(wěn)定性控制被控對象結構模型Fig.1 Structural model of controlled object for UAV stability control

(5)

采用四階龍格庫塔法求解約束狀態(tài)方程，選擇最優(yōu)控制性能指標，得到隨機最優(yōu)導引律：

(6)

無人機外形是軸對稱的，根據瞬時平衡假設，在水平面范圍內，當位姿參數(shù)為一組隨機序列時，基于Kalman濾波進行姿態(tài)參量的最優(yōu)狀態(tài)估計設計控制律.

2 控制律設計

2.1擴展Kalman濾波算法

在飛行動力學分析與被控對象描述的基礎上進行無人機穩(wěn)定性控制律改進設計，提出了一種基于擴展Kalman濾波的無人機制導系統(tǒng)穩(wěn)定性控制律設計方法.首先，采用Kalman濾波技術求出飛行狀態(tài)參量的最小方差估計值[8]，得到Kalman濾波函數(shù)：

(7)

式中：φα，ψα，γ為無人機在橫向、側向和橫滾運動的末彈道參數(shù)；b1，b2，b3，d3為被控姿態(tài)參量的已知系數(shù)；Δb1，Δb2，Δb3，Δd3為未確定系數(shù)；fd1，fd2，fd3為擾動力矩；δφ，δψ，δγ為加速度計、磁力計和陀螺儀角度的估計誤差.

對融合Kalman方程進行擴展，構建最優(yōu)線性遞推濾波方差：

(8)

從式(8)得知，融合濾波后的最優(yōu)姿態(tài)角具有動態(tài)加速穩(wěn)定性，可將穩(wěn)定性特征描述為

(9)

式中：b>0；φa為兩時刻角度估計的差值；u為姿態(tài)角的輸入；fd為外加的白噪聲序列.

在擴展Kalman濾波模型中，當前的飛行位姿狀態(tài)變量和誤差協(xié)方差估計可用以下方程計算：

(10)

(11)

M=Mn+ΔM,

(12)

(13)

在擴展Kalman濾波模型下，被控系統(tǒng)的參數(shù)自整定性預測方程可表示為

(14)

圖2 模糊神經網絡Fig.2 Fuzzy neural network system structure

2.2模糊神經網絡控制律設計

將校正后的位姿參數(shù)輸入模糊神經網絡系統(tǒng)，實現(xiàn)無人機制導控制律的優(yōu)化設計[9]，模糊神經網絡如圖2所示.

圖2給出的神經網絡為3層網絡結構，各層權值的迭代方程為

(15)

輸入層至隱含層的權重值為

(16)

且學習步長η滿足

(17)

(18)

考慮位姿參數(shù)存在不確定性因素，對測量組合系統(tǒng)進行修正，將融合Kalman擴展濾波后的最優(yōu)姿態(tài)角輸入測量系統(tǒng)，建立一個線性離散隨機過程[10]，得到飛行控制的最優(yōu)導引律：

(19)

最后,進行控制律的穩(wěn)定性證明，定義Lyapunov函數(shù)為

(20)

(21)

根據Lyapunove穩(wěn)定性原理[11]，控制系統(tǒng)的穩(wěn)定性得證.

3 仿真測試分析

為了測試本設計的應用性能，進行仿真實驗分析，仿真平臺為Matlab 7.飛行姿態(tài)參量數(shù)據來自于前期通過數(shù)字磁力計和數(shù)字加速度計實際采集的數(shù)據，位姿參數(shù)測量的靈敏度為8.44/15.30/60 mdps/digit，無人機飛行控制傳感器設備的工作溫度為-40～85 ℃，能有效滿足各種飛行條件要求.設置飛行位姿參數(shù)的采樣周期為0.02 s，Kalman濾波的階數(shù)為4，濾波周期為0.02 s,中心頻率取1 200 Hz，擾動干擾強度為0～10 dB，飛行的俯仰角和航向角初始值分別為Δ1=5°，Δ2=8°,轉彎半徑為120 rad，下降速度為10 m/s.根據上述仿真環(huán)境和參量設定，進行無人機飛行姿態(tài)校正和穩(wěn)定性控制仿真分析.采用本設計的EKF融合濾波方法和傳統(tǒng)的直接計算方法進行無人機飛行控制俯仰角、橫滾角和航向角的測量，結果如圖3所示.

圖3 飛行位姿參數(shù)參量結果Fig.3 Parameters of flight attitude

分析圖3得知，采用本方法進行擴展Kalman濾波實現(xiàn)飛行位姿參數(shù)的整定性融合處理和角度校正，角度的振蕩和偏差更小，采樣曲線更加平滑，說明本方法在提高飛行姿態(tài)參量穩(wěn)定性和準確性方面具有明顯的效果.

以圖3所示的EKF方法采集的飛行位姿參量數(shù)據為研究對象進行誤差分析，得到結果如圖4所示.

分析圖4得知，采用本方法能夠有效降低俯仰角和橫滾角的誤差，對磁力計采集的航向角的誤差控制卻相對較弱.

采用不同算法進行無人機控制，得到姿態(tài)角校正的解算結果對比，如圖5所示.

圖4 姿態(tài)角與真值的偏差Fig.4 Deviation of attitude angle and true value

圖5 無人機飛行姿態(tài)角解算結果Fig.5 UAV flight attitude angle calculation results

圖5給出的飛行姿態(tài)角解算結果分別描述了靜態(tài)直航飛行階段和大迎角飛行階段的位姿參量.其中，前后兩段相對較平穩(wěn)的部分表示靜態(tài)直航，中間段姿態(tài)角劇烈振蕩階段表示大迎角飛行.根據圖5的校正結果，在相同實驗參數(shù)下，對不同算法進行位姿參數(shù)校正的最大誤差、平均誤差和均方差統(tǒng)計，結果見表1.

表1 不同算法位姿參數(shù)校正統(tǒng)計分析Tab.1 Statistical analysis of different algorithms for pose parameter correction (°)

分析表1得知，在靜態(tài)直航段，3種算法的性能相當，幾乎沒有軌跡跟蹤誤差和漂移，都具有較好的控制性能.在大迎角飛行階段，由于存在不確定性擾動影響，穩(wěn)定性控制難度較高，本方法能通過擴展Kalman濾波和模糊神經網絡控制，使飛行姿態(tài)角變得相當穩(wěn)定，能在短時間內進行位姿調整，提高定姿的精度.位姿校正確保了飛行的穩(wěn)定性，仿真結果驗證了本方法的有效性和優(yōu)越性.

4 結語

本研究提出了一種基于擴展Kalman濾波的位姿校正方法，進行無人機制導系統(tǒng)穩(wěn)定性控制律設計，建立了飛行運動學數(shù)學模型，構建被控對象并進行控制約束參量分析，采用擴展Kalman濾波算法進行位姿參量融合校正，并通過模糊神經網絡構建優(yōu)化的控制律.研究得知，本設計能提高飛行姿態(tài)角的穩(wěn)定性且定姿精度較高，特別是在大迎角飛行中，飛行穩(wěn)定性控制得到了很好的展現(xiàn).

[1] 劉煒,陸興華.飛行失衡條件下的無人機慣導魯棒性控制研究[J].計算機與數(shù)字工程,2016,44(12):2380-2385.

[2] 潘加亮,熊智,王麗娜,等.一種簡化的發(fā)射系下SINS/GPS/CNS組合導航系統(tǒng)無跡Kalman濾波算法[J].兵工學報,2015,36(3):484-491.

[3] 王勛,張代兵,沈林成.一種基于虛擬力的無人機路徑跟蹤控制方法[J].機器人,2016,38(3):329-336.

[4] 邸斌,周銳,董卓寧.考慮信息成功傳遞概率的多無人機協(xié)同目標最優(yōu)觀測與跟蹤[J].控制與決策,2016,31(4):616-622.

[5] 陸興華.姿態(tài)融合濾波的無人機抗干擾控制算法[J].傳感器與微系統(tǒng),2016,35(7):116-119.

[6] 劉凱,朱紀洪,余波.推力矢量飛機縱向魯棒動態(tài)逆控制[J].控制與決策,2013,28(7):1113-1116.

[7] LEE W,BANG H,LEEGHIM H.Cooperative localization between small UAVs using a combination of heterogeneous sensors[J].Aerospace Science and Technology,2013,27(1):105-111.

[8] EVANGELIO R H,PATZOLD M,KELLER I.Adaptively splitted GMM with feedback improvement for the task of background subtraction[J].IEEE Transactions on Information Forensics and Security,2014,9(5):863-874.

[9] ST-CHARLES P,BILODEAU G,BERGEVIN R.SuBSENSE:a universal change detection method with local adaptive sensitivity[J].IEEE Transactions on Image Processing,2015,24(1):359-373.

[10]HELMY A,HEDAYAT A,AL-DHAHIR N.Robust weighted sum-rate maximization for the multi-stream MIMO interference channel with sparse equalization[J].IEEE Transactions on Communications,2015,60(10):3645-3659.

[11]譚中權.離散與連續(xù)時間強相依高斯過程最大值與和的漸近關系[J].應用數(shù)學學報,2015,38(1):27-36.

ResearchontheattitudecorrectionofunmannedaerialvehiclebasedonextendedKalmanfilter

LIKe

(EngineeringTrainingCenter,HenanUniversityofEngineering,Zhengzhou451191,China)

UAV attitude control is the key technology for UAV flight stability. Considering the lower accuracy of the guidance controller attitude positioning of UAV in the posture parameters under uncertainty problems, an UAV attitude correction method is proposed based on extended Kalman filter, and the navigation system stability control law design method is presented. The establishment of UAV flight dynamics model is taken, the flight trajectory equation is constructed, the UAV guidance system with constraint parameter object is analyzed, the accelerometer, gyroscope and magnetometer are used to obtain the position parameter measurement considering the pose parameter uncertainty, the qualitative treatment of extended Kalman filter algorithm of attitude parameters is conducted, realizing the angle correction. The position and orientation parameters are input into the fuzzy neural network to realize the optimal design of the guidance control law. The simulation results show that the proposed method can be used in the flight guidance control of UAV, and the attitude accuracy is higher.

unmanned aerial vehicle; guidance system; stability control; extended Kalman filter

TP273

A

1674-330X(2017)03-0044-06

2017-02-09

河南工程學院校級質量工程項目(508905)

李可(1978-)，男，河南鄭州人，實驗師，主要從事自動化控制研究.