Kalman濾波算法在遙測伺服系統(tǒng)中的應(yīng)用

李宏偉 陳海建 汪 洋

（1.中國人民解放軍92941部隊，遼寧葫蘆島125000；2.海軍駐北京地區(qū)艦空導(dǎo)彈系統(tǒng)軍事代表室，北京100039）

關(guān)鍵字 Kalman濾波 機動目標(biāo) “當(dāng)前”統(tǒng)計模型 伺服系統(tǒng)

1 引 言

雷達(dá)和無線電角跟蹤系統(tǒng)中，為提高系統(tǒng)無差度、減小滯后誤差，通常引入機動目標(biāo)角速度、角加速度信號，實現(xiàn)前饋復(fù)合控制，而目標(biāo)機動過程中，通常無法準(zhǔn)確預(yù)知目標(biāo)下一時刻運動狀態(tài)，解決這個問題的辦法之一是發(fā)展合適的目標(biāo)運動模型，通過卡爾曼濾波算法對目標(biāo)運動參數(shù)及運動狀態(tài)進(jìn)行估計和預(yù)測［1-3］。

目前常用的目標(biāo)機動模型主要有：勻速模型、勻加速模型、Signer模型和均值自適應(yīng)的“當(dāng)前”統(tǒng)計模型［4，5］。勻速模型和勻加速模型是目標(biāo)運動模型中最基本的兩種模型，Signer模型把機動加速度描述成一個零均值的一階馬爾可夫過程，“當(dāng)前”統(tǒng)計模型則將Signer模型中加速度零均值改進(jìn)為自適應(yīng)的加速度均值，使得跟蹤精度和性能得到較大提高，得到極為廣泛的應(yīng)用［6-9］。

本文論述了一種自適應(yīng)Kalman濾波器，采用機動目標(biāo)均值自適應(yīng)的“當(dāng)前”統(tǒng)計模型，構(gòu)造角位置角速度二階觀測器，實現(xiàn)機動目標(biāo)Kalman濾波算法。

2 基于“當(dāng)前”統(tǒng)計模型的自適應(yīng)Kalman濾波

“當(dāng)前”統(tǒng)計模型的意義是，在每一種具體的戰(zhàn)術(shù)場合，人們所關(guān)心的僅是機動加速度的“當(dāng)前”概率密度，即目標(biāo)機動的當(dāng)前可能性，當(dāng)前目標(biāo)現(xiàn)時正以某一加速度機動時，它在下一瞬時的加速度取值范圍是有限的，而且只能在“當(dāng)前”加速度的領(lǐng)域內(nèi)?？紤]非零加速度均值，一維情況下的狀態(tài)方程為［10，11］：

設(shè)采樣周期為T，通過經(jīng)典的離散處理方法，可以得到下列離散狀態(tài)方程及量測方程：

其中，

式中：Φ（k+1，k）——狀態(tài)轉(zhuǎn)移矩陣；V（k）——角位置傳感器輸出噪聲；Q（k）——模型誤差協(xié)方差陣。

有：

其中，

對于狀態(tài)方程和量測方程，可以構(gòu)造關(guān)于狀態(tài)向量X（k）的卡爾曼濾波估計器如下：

那么，一步預(yù)測方程（17）可以寫為如下形式：

但是要注意的是，一步預(yù)測均方誤差方程（19）中的 Φ（k，k-1）不變。

利用上述關(guān)系，可以得到機動加速度方差算法，當(dāng)“當(dāng)前”加速度為正時，有：

當(dāng)“當(dāng)前”加速度為負(fù)時，有：

遙測伺服系統(tǒng)中，缺乏距離量測量，遙測接收機只能提供目標(biāo)與電軸之間的角度偏差量，無法給出目標(biāo)在遙測天線坐標(biāo)系中的信息，可以通過將天線角位置傳感器測得的天線角位置信息與接收機角誤差相加得到目標(biāo)在遙測天線坐標(biāo)系中的角位置信息，即y=θ+Δθ，式中θ為角位置傳感器量測的角位置信息，Δθ為接收機輸出角誤差信息。

3 提高跟蹤精度

機動目標(biāo)跟蹤過程中，總是希望對機動目標(biāo)角速度、角加速度有較高的估計精度，這就要求Kalman濾波器有良好的動態(tài)性能。在時域內(nèi)考慮連續(xù)系統(tǒng)的卡爾曼濾波問題，目標(biāo)狀態(tài)方程為：

卡爾曼濾波狀態(tài)估計方程可寫為：

式中：K（t）——卡爾曼增益矩陣。

當(dāng)卡爾曼濾波器達(dá)到穩(wěn)定狀態(tài)時，K（t）將趨近于一常數(shù)陣K，進(jìn)行拉普拉斯變換，并設(shè)初始狀態(tài)為零，有：

采用機動目標(biāo)“當(dāng)前”統(tǒng)計模型自適應(yīng)算法，狀態(tài)估計方程為：

其中，

動態(tài)部分傳遞函數(shù)為：

通過方程比較發(fā)現(xiàn)傳遞函數(shù)的分母多項式中，s的冪都是3，但方程分子多項式卻增加了一次冪，這意味著速度量測量的引入相當(dāng)于分別在關(guān)于位置、速度和加速度的傳遞函數(shù)中增加了一個零點，使得跟蹤濾波器的帶寬增大，動態(tài)誤差減小，提高了濾波器跟蹤精度。

4 濾波器的穩(wěn)定性

根據(jù)以上分析，遙測伺服系統(tǒng)中依據(jù)機動目標(biāo)“當(dāng)前”統(tǒng)計模型構(gòu)造二階觀測器設(shè)計的Kalman濾波器，其離散狀態(tài)方程、量測方程為：

其中，Φ（k+1，k）如式（5），U（k）如式（6），H（k）如式（34）。

計算Kalman濾波器離散狀態(tài)方程能控矩陣、能觀矩陣

系統(tǒng)是能控能觀系統(tǒng)，則基于“當(dāng)前”統(tǒng)計模型的卡爾曼濾波算法是一致漸進(jìn)穩(wěn)定的，因此實驗過程中可以首先通過仿真結(jié)果給定目標(biāo)狀態(tài)矩陣和驗后誤差協(xié)方差矩陣初始值X0、P0。

5 實驗結(jié)果

在某項目中進(jìn)行了實驗驗證工作，以遙測伺服系統(tǒng)天控器為平臺，在天控器中，利用嵌入式數(shù)字處理器完成自適應(yīng)Kalman濾波算法，將實驗數(shù)據(jù)從數(shù)字處理器上報至上位機。采樣周期T=0.01s，，狀態(tài)矩陣初始值X0為進(jìn)入Kalman濾波算法時量測角位置和量測角速度值。通過Matlab繪制Kalman濾波算法估計得到的角位置、角速度、角加速度曲線分別如圖1至圖8所示，系統(tǒng)運行過程中Kalman濾波算法穩(wěn)定不發(fā)散，獲得了平穩(wěn)的角位置、角速度及角加速度數(shù)據(jù)。

圖1 目標(biāo)飛行時間20s～100s方位角位置量測與估計曲線Fig.1 Azimuth position measurement and estimation curve of target flight time from 20s to 100s

圖2 目標(biāo)飛行時間39s～43.5s方位角位置量測與估計曲線Fig.2 Azimuth position measurement and estimation curve of target flight time 39s to 43.5s

圖3 目標(biāo)飛行時間20s～100s方位角速度量測與估計曲線Fig.3 Azimuth velocity measurement and estimation curve of target flight time from 20s to 100s

圖4 目標(biāo)飛行時間20s～100s方位角加速度估計曲線Fig.4 Azimuth acceleration estimation curve of target flight time from 20s to 100s

圖5 目標(biāo)飛行時間20s～100s俯仰角位置量測與估計曲線Fig.5 Pitch angle position measurement and estimation curve of target flight time from 20s to 100s

圖6 目標(biāo)飛行時間41.5s～45s俯仰角位置量測與估計曲線Fig.6 Pitch angle position measurement and estimation curve of target flight time from 41.5s to 45s

圖7 目標(biāo)飛行時間20s～100s俯仰角速度量測與估計曲線Fig.7 Pitch angular velocity measurement and estimation curve of target flight time from 20s to 100s

圖8 目標(biāo)飛行時間20s～100s俯仰角加速度估計曲線Fig.8 Pitch angle acceleration estimation curve of target flight time from 20s to 100s

6 結(jié)束語

雷達(dá)和無線電角跟蹤系統(tǒng)目標(biāo)機動過程中，通常無法準(zhǔn)確預(yù)知目標(biāo)下一時刻運動狀態(tài)，本文針對這一問題進(jìn)行了研究，根據(jù)機動目標(biāo)基于“當(dāng)前”統(tǒng)計模型，利用角位置和角速度信息構(gòu)造二階觀測器，實現(xiàn)了機動目標(biāo)Kalman濾波算法，通過理論分析和實驗驗證了本系統(tǒng)中Kalman濾波器的穩(wěn)定性，文章最后給出了在遙測伺服系統(tǒng)中Kalman濾波器的實驗結(jié)果，表明了該濾波器的有效性和可靠性。