一種基于自構(gòu)架模糊EKF的目標(biāo)跟蹤方法

劉志勇，王阿利，王小紅

(1．咸陽(yáng)職業(yè)技術(shù)學(xué)院,陜西 咸陽(yáng) 712000；2．陜西省委黨校，陜西 西安 710061)

無人機(jī)(Unmanned Aircral Vehicle，UAV)是一種通過預(yù)編程序操縱的非載人飛行器。它以其獨(dú)特的靈活性，在海陸空天等領(lǐng)域被廣泛應(yīng)用。而對(duì)無人機(jī)航跡的高精度跟蹤是保障無人機(jī)安全、高效地完成飛行任務(wù)的有力措施。為實(shí)現(xiàn)對(duì)UAV系統(tǒng)的高精度跟蹤，全球衛(wèi)星定位系統(tǒng)(Global Positioning System, GPS)是有效措施之一。GPS系統(tǒng)包括了空間衛(wèi)星星座、輔助接收、地面控制中心及用戶設(shè)備。對(duì)于UAV來講，是將GPS信號(hào)以內(nèi)置模塊的形式安裝在機(jī)身上。GPS模塊可以在起飛、懸停和飛行過程中準(zhǔn)確給予無人機(jī)的位置信息及角度信息。在實(shí)際民用領(lǐng)域中，為保證衛(wèi)星發(fā)射信號(hào)的安全性，在GPS導(dǎo)航衛(wèi)星信號(hào)中人為地加入了高頻振蕩信號(hào)，進(jìn)而使GPS接收機(jī)收到的衛(wèi)星信號(hào)均產(chǎn)生了高頻抖動(dòng)，直接影響了對(duì)檢測(cè)對(duì)象位置及速度信息的計(jì)算。為提高對(duì)檢測(cè)對(duì)象的定位精度，就需要利用濾波技術(shù)對(duì)GPS接收機(jī)的衛(wèi)星信號(hào)進(jìn)行濾波處理。在現(xiàn)代控制理論中，EKF技術(shù)是一種基于控制系統(tǒng)狀態(tài)方程的高效率遞歸濾波器，它可以通過觀測(cè)控制系統(tǒng)的輸入輸出數(shù)據(jù)，實(shí)現(xiàn)對(duì)控制系統(tǒng)最優(yōu)狀態(tài)的估計(jì)。付承彪等人基于給定的機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)模型，利用EKF濾波方法進(jìn)行跟蹤實(shí)驗(yàn)，表明利用EKF濾波方法能夠跟蹤真實(shí)軌跡，而且偏差較少。因此，有許多學(xué)者將EKF濾波方法運(yùn)用到衛(wèi)星觀測(cè)數(shù)據(jù)的分析中。

段順利等人基于EKF濾波技術(shù)設(shè)計(jì)了一種自適應(yīng)EKF抗差算法，通過推導(dǎo)出衛(wèi)星緊組合控制系統(tǒng)狀態(tài)方程，并設(shè)計(jì)了狀態(tài)觀測(cè)方程。通過對(duì)車載實(shí)測(cè)數(shù)據(jù)實(shí)驗(yàn)驗(yàn)證了該算法可以有效地削弱粗差觀測(cè)值的影響。但是，該方法會(huì)由于GPS信號(hào)發(fā)生間隔而影響數(shù)據(jù)的分析精度。由于人工神經(jīng)網(wǎng)絡(luò)具有反復(fù)學(xué)習(xí)的能力，而且能夠?qū)p失函數(shù)控制在最小值范圍內(nèi)。因此，有學(xué)者將人工神經(jīng)網(wǎng)絡(luò)與EKF濾波理論相結(jié)合進(jìn)行研究。朱楠等人和 Derradji Nada將神經(jīng)網(wǎng)絡(luò)與EKF濾波技術(shù)相融合，利用人工神經(jīng)網(wǎng)絡(luò)的輸出分量去修正慣性導(dǎo)航系統(tǒng)的輸出偏差，減少了系統(tǒng)不確定性對(duì)測(cè)量數(shù)據(jù)的影響。但在GPS系統(tǒng)中，由于微型電機(jī)系統(tǒng)中傳感器制造工藝的限制，其采集數(shù)據(jù)精度和穩(wěn)定性較差，從本質(zhì)上不能解決該問題的存在。為了提高定位精度，孫之光在低頻磁場(chǎng)和導(dǎo)航中，利用EKF濾波和數(shù)據(jù)融合定位方法，將無人機(jī)的位置和速度誤差限制在系統(tǒng)要求范圍之內(nèi)。因此，要想利用GPS系統(tǒng)實(shí)現(xiàn)對(duì)被檢測(cè)對(duì)象的精確定位，就必須通過濾波算法對(duì)GPS接收器信號(hào)進(jìn)行濾波處理。但對(duì)于傳統(tǒng)的卡爾曼濾波技術(shù)，當(dāng)控制系統(tǒng)存在不確定性擾動(dòng)時(shí)，其濾波效果就不盡人意。而模糊控制理論能夠?qū)Ρ豢叵到y(tǒng)中存在的外界不確定性與系統(tǒng)內(nèi)部干擾進(jìn)行有效逼近，許多研究者也將其應(yīng)用到被控系統(tǒng)的不確定性與擾動(dòng)的估計(jì)中。由于傳統(tǒng)模糊系統(tǒng)結(jié)構(gòu)在系統(tǒng)初始化后，各模糊參數(shù)在系統(tǒng)逼近的過程中是固定不變的，進(jìn)而也影響了對(duì)被控系統(tǒng)不確定性干擾的估計(jì)精度。自構(gòu)架模糊控制理論可以根據(jù)被控系統(tǒng)不確定性的變化，而改變模糊系統(tǒng)參數(shù)，以提高對(duì)不確定性干擾的逼近精度, 劉志勇等人將其應(yīng)用于故障函數(shù)信息的逼近，具有一定的借鑒價(jià)值。本文基于自構(gòu)架模糊理論，提出了一種基于自構(gòu)架模糊EKF濾波方法，通過對(duì)無人機(jī)GPS模塊接收到的位置觀測(cè)信號(hào)進(jìn)行濾波處理，自適應(yīng)地獲得測(cè)量方差，從測(cè)量數(shù)據(jù)中估計(jì)動(dòng)態(tài)無人機(jī)系統(tǒng)的運(yùn)行狀態(tài)。

1 問題提出

在實(shí)際工程中，由于無人機(jī)在飛行中會(huì)受到來自外界強(qiáng)風(fēng)氣流與自身噪聲的干擾，這些都會(huì)影響無人機(jī)的工作品質(zhì)，而且也嚴(yán)重影響了對(duì)無人機(jī)的跟蹤與目標(biāo)定位。因此，有學(xué)者研究了無人機(jī)目標(biāo)跟蹤控制方法，而這些方法只集中在無人機(jī)的飛行姿態(tài)與高度上，對(duì)無人機(jī)的飛行速度、位置、航向以及軌跡跟蹤考慮得較少。因此，本文基于無人機(jī)航行動(dòng)力學(xué)原理，推導(dǎo)出含有強(qiáng)風(fēng)與自身GPS傳感器噪音的干擾的無人機(jī)的控制模型，并將這些干擾一并視為白噪聲。進(jìn)而通過自構(gòu)架模糊EKF濾波器設(shè)計(jì)，對(duì)控制系統(tǒng)中的白噪聲進(jìn)行了濾波處理，獲得了無人機(jī)的準(zhǔn)確狀態(tài)信息及速度信號(hào)。本文只考慮采樣時(shí)間處無人機(jī)的水平位置、水平速度、縱向位置以及縱向速度。假設(shè)以無人機(jī)飛行的出發(fā)點(diǎn)為坐標(biāo)原點(diǎn)，無人機(jī)的水平真實(shí)位置為()，縱向位置為()。則根據(jù)位移與速度之間的關(guān)系，在采樣時(shí)間時(shí)刻可以將無人機(jī)飛行的水平速度和縱向速度分別表示為：

(1)

(2)

無人機(jī)在飛行時(shí)，為了節(jié)省能量，發(fā)動(dòng)機(jī)動(dòng)力除了用于和阻力平衡外，一部分用來提供無人機(jī)勻加速運(yùn)動(dòng)的動(dòng)力。這里加速度可以理解為隨機(jī)加速度和機(jī)動(dòng)加速度之和，則加速度()可以進(jìn)一步表示為：

()=()+()

(3)

其中，()表示為機(jī)動(dòng)加速度，它也可以理解為無人機(jī)動(dòng)力系統(tǒng)的控制輸入信號(hào)；()是表示為隨機(jī)加速度，它是由強(qiáng)風(fēng)氣流及傳感器噪音引起的，且其從數(shù)值上服從高斯分布。因此可以假設(shè)它是零均值、方差為的白噪聲，并且獨(dú)立于外界強(qiáng)風(fēng)氣流及傳感器噪音引起的觀測(cè)噪聲。

根據(jù)物理運(yùn)動(dòng)量位移關(guān)系，可以將無人機(jī)第時(shí)刻與第+1時(shí)刻的位置公式定義如下：

(4)

(5)

根據(jù)上式，可以將無人機(jī)飛行的水平速度和縱向速度可以定義為：

(6)

(7)

將式(1)、式(2)分別帶入式(4)-～式(7)中，則可以將無人機(jī)第時(shí)刻與第+1時(shí)刻的水平位置、縱向位置、水平速度及縱向速度的迭代公式表示為：

(+1)=()++05()

(8)

(9)

(+1)=()+()

(10)

(11)

則根據(jù)式(8)～(11)，可以進(jìn)一步得到以無人機(jī)在時(shí)刻的水平位置、水平速度、縱向位置及縱向速度為狀態(tài)的離散控制狀態(tài)方程為：

(12)

假設(shè)在時(shí)刻無人機(jī)上GPS模塊的觀測(cè)位置用()表示，則可以定義對(duì)無人機(jī)位置的觀測(cè)模型為：

(13)

其中()用來表示外界強(qiáng)風(fēng)氣流及傳感器噪音引起的觀測(cè)噪聲，也可以理解為GPS模塊對(duì)無人機(jī)的定位誤差。在這里設(shè)定()是零均值、方差為的白噪聲。

在式(12)、式(13)中，進(jìn)行如下假設(shè)：

結(jié)合式(12)與觀測(cè)式(13)及式(3)，則可以將無人機(jī)控制系統(tǒng)的狀態(tài)空間模型表示為：

(+1)=()+()+()

(14)

()=()+()

(15)

在上式中，()為無人機(jī)控系統(tǒng)狀態(tài)，()為無人機(jī)動(dòng)力系統(tǒng)的控制輸入信號(hào)；()為控制系統(tǒng)的白噪聲，()為無人機(jī)觀測(cè)位置的觀測(cè)噪聲。、為控制傳遞矩陣，為觀測(cè)矩陣。

EKF濾波算法的主導(dǎo)思想是利用上一時(shí)刻的估計(jì)值和當(dāng)前測(cè)量值之間的最小誤差方法，得到控制系統(tǒng)當(dāng)前最優(yōu)估計(jì)值。但該濾波方法在進(jìn)行測(cè)量較小誤差的情況下，會(huì)出現(xiàn)穩(wěn)態(tài)誤差，進(jìn)而影響了EKF濾波方法的估計(jì)精度。因此本文提出了一種自構(gòu)架模糊EKF濾波方法，根據(jù)模糊輸入量變化，自適應(yīng)地減小模糊EKF系統(tǒng)估計(jì)誤差，提高對(duì)無人機(jī)跟蹤精度。

2 自組織模糊EKF系統(tǒng)設(shè)計(jì)

根據(jù)無人機(jī)控制系統(tǒng)的狀態(tài)空間模型式(14)和式(15)，設(shè)計(jì)如下的EKF濾波器。首先根據(jù)如下公式對(duì)無人機(jī)飛行控制系統(tǒng)狀態(tài)與協(xié)方差矩陣預(yù)測(cè)：

(16)

(17)

根據(jù)下式進(jìn)行控制系統(tǒng)狀態(tài)預(yù)測(cè)

(|)=(|-1)+(()-

(|-1))

(18)

(|)=(|-1)

((|-1)+)-1

(19)

(|)=(-())(|-1)

(20)

其中為測(cè)量數(shù)據(jù)的估計(jì)方差,為卡爾曼濾波增益。

測(cè)量數(shù)據(jù)的準(zhǔn)確性直接取決于方差估計(jì)值的選擇，如果的取值與真實(shí)測(cè)量值差異較大，則估計(jì)誤差也隨之增加。由此，本文通過EKF濾波器的方差信息，設(shè)計(jì)自構(gòu)架模糊推理系統(tǒng)，對(duì)測(cè)量誤差的方差值進(jìn)行模糊調(diào)整與逼近，使其盡可能地接近真實(shí)值，來提高對(duì)無人機(jī)飛行定位的準(zhǔn)確度。為便于自構(gòu)架模糊EKF系統(tǒng)設(shè)計(jì)，根據(jù)系統(tǒng)估計(jì)協(xié)方差式(17)，可以定義測(cè)量估計(jì)誤差式為：

(21)

測(cè)量實(shí)際誤差可以定義為：

(22)

其中，=-+1，=()-(|-1)。

根據(jù)式(21)與(22)，可以進(jìn)一步得到測(cè)量估計(jì)誤差與實(shí)際誤差之間的誤差式為：

(23)

為實(shí)現(xiàn)對(duì)方差估計(jì)值的準(zhǔn)確逼近，將誤差式(23)定義為自構(gòu)架模糊系統(tǒng)的一個(gè)輸入，并設(shè)定當(dāng)時(shí)刻誤差()>10時(shí)，自構(gòu)架模糊系統(tǒng)參數(shù)進(jìn)行調(diào)整，直到誤差()≤10。

在自構(gòu)架模糊系統(tǒng)中，假設(shè)第條模糊規(guī)則可以定義如下：

(24)

(25)

(26)

其中，代表非負(fù)常數(shù)，且∈(0,1)，()表示自構(gòu)架模糊基函數(shù)向量。

假設(shè)在自構(gòu)架模糊EKF系統(tǒng)中，第時(shí)刻，模糊隸屬度函數(shù)有個(gè)，在系統(tǒng)誤差()>10的條件下，對(duì)于系統(tǒng)輸入變量將會(huì)增加一個(gè)隸屬度函數(shù)。此時(shí)，自構(gòu)架模糊系統(tǒng)的隸屬度函數(shù)將增加為+1個(gè)，進(jìn)而自構(gòu)架模糊系統(tǒng)的規(guī)則數(shù)可以表示為：

(27)

根據(jù)式(26)，增加的模糊規(guī)則可以表示為：

(28)

在自構(gòu)架模糊EKF系統(tǒng)中，當(dāng)增加一個(gè)新的模糊規(guī)則，模糊后件參數(shù)矩陣就必須被重新初始化。假設(shè)在時(shí)刻模糊系統(tǒng)具有模糊規(guī)則條，此時(shí)自構(gòu)架模糊EKF系統(tǒng)輸出可以表示為：

(29)

在時(shí)刻，模糊系統(tǒng)增加的模糊規(guī)則為條，此時(shí)自構(gòu)架模糊EKF系統(tǒng)的輸出為：

(30)

其中為增加模糊規(guī)則后新模糊后件參數(shù)矩陣。

根據(jù)式(29)，第+時(shí)刻自構(gòu)架模糊EKF系統(tǒng)的輸出可以表示為：

(31)

聯(lián)合式(30)和(31)，可以得到下式：

(32)

根據(jù)上式，增加模糊規(guī)則后的模糊后件參數(shù)矩陣的初始化式可以表示為下式：

(33)

結(jié)合上式，方差估計(jì)值的最優(yōu)模糊逼近可以進(jìn)一步表示為：

(34)

則將式(34)代入測(cè)量估計(jì)誤差式(21)，可以一步得到自組織模糊EKF無人機(jī)飛行軌跡位置定位測(cè)量估計(jì)誤差式如下：

(35)

至此，利用自構(gòu)架模糊理論，設(shè)計(jì)完成了自構(gòu)架模糊EKF系統(tǒng)，通過將測(cè)量估計(jì)誤差與實(shí)際誤差之間的誤差()作為模糊系統(tǒng)的輸入，通過調(diào)整模糊系統(tǒng)參數(shù)來實(shí)現(xiàn)對(duì)方差估計(jì)值的最優(yōu)逼近，來提高對(duì)無人機(jī)飛行軌跡的跟蹤精度。算法流程如圖1所示。

圖1 自構(gòu)架模糊EKF系統(tǒng)圖

3 自構(gòu)架模糊EKF系統(tǒng)仿真

為進(jìn)一步驗(yàn)證自構(gòu)架EKF系統(tǒng)對(duì)方差估計(jì)值的最優(yōu)逼近能力，在實(shí)驗(yàn)中，假定無人機(jī)飛行的初始狀態(tài)為：

(36)

無人機(jī)GPS模塊接收信號(hào)的掃描周期=1，則可以進(jìn)一步得到無人機(jī)控制系統(tǒng)的狀態(tài)空間模型的控制矩陣為：

(37)

在自構(gòu)架模糊EKF系統(tǒng)中，模糊隸屬度函數(shù)采用三角型隸屬度函數(shù)，模糊推理采用乘法模糊推理。在對(duì)無人機(jī)飛行軌跡觀測(cè)過程中，如果對(duì)于EKF中觀測(cè)噪聲初始值越小，則說明無人機(jī)的運(yùn)行軌跡越近似于直線飛行軌跡，反之，則說明無人機(jī)的運(yùn)行軌跡為不規(guī)則的曲線運(yùn)動(dòng)。假設(shè)自構(gòu)架模糊EKF系統(tǒng)中的觀測(cè)噪聲均值為0，方差為10。利用自組織模糊EKF系統(tǒng)對(duì)無人機(jī)運(yùn)行軌跡進(jìn)行跟蹤，仿真實(shí)驗(yàn)可以得到如圖2所示結(jié)果。

圖2 自構(gòu)架模糊EKF系統(tǒng)無人機(jī)飛行跟蹤曲線

從圖2可以觀察到，在無人機(jī)飛行過程中，無論其軌跡發(fā)生怎么樣的變化，自構(gòu)架模糊EKF系統(tǒng)總能對(duì)其飛行軌跡進(jìn)行逼近跟蹤，達(dá)到比較好的定位效果。

圖3和圖4為自構(gòu)架模糊EKF系統(tǒng)跟蹤無人機(jī)飛行軌跡方向和方向上的誤差曲線。根據(jù)兩圖可以觀察到，由于無人機(jī)在運(yùn)行軌跡上有兩次不同方向上的變動(dòng)，使得方向和方向上的誤差曲線在第349次逼近位置有一個(gè)大的波動(dòng)，而在其他飛行軌跡上，方向上的誤差值在區(qū)間[0,-1.1]上，方向上的誤差值在區(qū)間[0.9,1]上。進(jìn)而說明自構(gòu)架模糊EKF系統(tǒng)能夠?qū)o人機(jī)的飛行軌跡進(jìn)行較好的跟蹤。

圖3 無人機(jī)x方向跟蹤誤差曲線

圖4 無人機(jī)y方向跟蹤誤差曲線

表1為方差估計(jì)值不同水平下的誤差對(duì)比，從表1中的對(duì)比數(shù)值可以觀察到，在小方差估計(jì)值下，自構(gòu)架模糊EKF比傳統(tǒng)的EKF具有良好的跟蹤估計(jì)性能。

表1 方差估計(jì)值R不同水平下的誤差對(duì)比

由于自構(gòu)架模糊系統(tǒng)要實(shí)現(xiàn)對(duì)方差估計(jì)值的準(zhǔn)確逼近，在系統(tǒng)運(yùn)行的過程中，模糊參數(shù)要根據(jù)無人機(jī)的運(yùn)行狀態(tài)進(jìn)行調(diào)整，去尋找合適的方差估計(jì)值。模糊參數(shù)變化如圖5、圖6所示。

圖5 自構(gòu)架模糊EKF系統(tǒng)模糊規(guī)則變化曲線

圖6 自構(gòu)架模糊EKF系統(tǒng)隸屬度函數(shù)變化圖

從上圖可以觀察到，在模糊系統(tǒng)運(yùn)行初期，初始模糊規(guī)則數(shù)目為2條，隨著無人機(jī)系統(tǒng)的位置發(fā)生變化，模糊系統(tǒng)參數(shù)發(fā)生了變化，模糊規(guī)則數(shù)目依次遞增，當(dāng)方差估計(jì)值穩(wěn)定的時(shí)候，系統(tǒng)的模糊規(guī)則數(shù)目為26條。由于系統(tǒng)的模糊規(guī)則數(shù)據(jù)增加，而另一個(gè)參數(shù)模糊隸屬度函數(shù)也會(huì)發(fā)生變化。在圖6中，虛線所示的為初始隸屬度函數(shù)，而實(shí)線所示為增加隸屬度函數(shù)。在自構(gòu)架模糊系統(tǒng)中，隸屬度函數(shù)參數(shù)的變化是隨著方差估計(jì)值的變化進(jìn)行更新。

4 結(jié) 論

基于自構(gòu)架模糊理論，研究了一種自構(gòu)架模糊EKF濾波方法，在該方法中將EKF估計(jì)方差與實(shí)際觀測(cè)方差間的誤差作為自構(gòu)架模糊系統(tǒng)的一個(gè)輸入，通過自構(gòu)架模糊系統(tǒng)辨識(shí)，自適應(yīng)地減小模糊EKF系統(tǒng)估計(jì)誤差，實(shí)現(xiàn)對(duì)方差估計(jì)值的準(zhǔn)確逼近，提高了對(duì)無人機(jī)飛行軌跡的跟蹤精度。