李 可

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

基于擴(kuò)展Kalman濾波的無(wú)人機(jī)位姿校正

李 可

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

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

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

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

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

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

1 飛行動(dòng)力學(xué)分析與被控對(duì)象描述

1.1無(wú)人機(jī)飛行動(dòng)力學(xué)方程

(1)

(2)

(3)

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

(4)

1.2被控對(duì)象描述

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

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

(5)

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

(6)

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

2 控制律設(shè)計(jì)

2.1擴(kuò)展Kalman濾波算法

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

(7)

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

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

(8)

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

(9)

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

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

(10)

(11)

M=Mn+ΔM,

(12)

(13)

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

(14)

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

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

將校正后的位姿參數(shù)輸入模糊神經(jīng)網(wǎng)絡(luò)系統(tǒng)，實(shí)現(xiàn)無(wú)人機(jī)制導(dǎo)控制律的優(yōu)化設(shè)計(jì)[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í)步長(zhǎng)η滿足

(17)

(18)

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

(19)

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

(20)

(21)

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

3 仿真測(cè)試分析

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)語(yǔ)

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

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

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

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

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

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

[6] 劉凱,朱紀(jì)洪,余波.推力矢量飛機(jī)縱向魯棒動(dòng)態(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ù)時(shí)間強(qiáng)相依高斯過(guò)程最大值與和的漸近關(guān)系[J].應(yīng)用數(shù)學(xué)學(xué)報(bào),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é)院校級(jí)質(zhì)量工程項(xiàng)目(508905)

李可(1978-)，男，河南鄭州人，實(shí)驗(yàn)師，主要從事自動(dòng)化控制研究.