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

李 可

(河南工程學(xué)院 工程訓(xùn)練中心，河南 鄭州 451191)

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

李 可

(河南工程學(xué)院 工程訓(xùn)練中心，河南 鄭州 451191)

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

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

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

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

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

1 飛行動力學(xué)分析與被控對象描述

1.1無人機飛行動力學(xué)方程

(1)

(2)

(3)

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

(4)

1.2被控對象描述

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

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

(5)

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

(6)

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

2 控制律設(shè)計

2.1擴展Kalman濾波算法

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

(7)

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

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

(8)

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

(9)

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

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

(10)

(11)

M=Mn+ΔM,

(12)

(13)

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

(14)

圖2 模糊神經(jīng)網(wǎng)絡(luò)Fig.2 Fuzzy neural network system structure

2.2模糊神經(jīng)網(wǎng)絡(luò)控制律設(shè)計

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

圖2給出的神經(jīng)網(wǎng)絡(luò)為3層網(wǎng)絡(luò)結(jié)構(gòu)，各層權(quán)值的迭代方程為

(15)

輸入層至隱含層的權(quán)重值為

(16)

且學(xué)習(xí)步長η滿足

(17)

(18)

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

(19)

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

(20)

(21)

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

3 仿真測試分析

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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

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

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

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

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

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

[6] 劉凱,朱紀(jì)洪,余波.推力矢量飛機縱向魯棒動態(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]譚中權(quán).離散與連續(xù)時間強相依高斯過程最大值與和的漸近關(guān)系[J].應(yīng)用數(shù)學(xué)學(xué)報,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

河南工程學(xué)院校級質(zhì)量工程項目(508905)

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