基于改進卡爾曼濾波的伺服跟蹤控制方法

閆巖 李楊

（91245部隊葫蘆島125000）

基于改進卡爾曼濾波的伺服跟蹤控制方法

閆巖 李楊

（91245部隊葫蘆島125000）

同軸跟蹤無線電測控系統中，通常利用濾波外推算法將濾波器估計的目標角位置與天線實時角位置的角誤差作為伺服系統位置環路的輸入以驅動天線跟蹤目標。然而，伴隨目標跟蹤過程中接收機熱噪聲、雜波干擾等不利因素的制約，隨機誤差分量會嚴重影響算法的穩定性，文中對自適應濾波算法進行了改進，引入UD因式分解和野值剔除算法以防止濾波過程的不穩定。仿真結果表明改進算法具有較好的數值穩定性和系統跟蹤精度。

卡爾曼濾波；野值剔除；UD因式分解

Class NumberTP13；TN953

1 引言

無線電自跟蹤系統中，伺服系統通常借鑒同軸跟蹤的復合控制思想來降低信道以及接收機噪聲對跟蹤環路的影響，在不影響跟蹤穩定性的前提下提高系統的無差度，減小滯后誤差，完成對目標的可靠跟蹤［1～2］。該方法的關鍵是借助于實時濾波外推算法，根據目標運動模型對接收到的角誤差數據進行平滑處理，而后再放大作為位置環路校正環節輸入的控制量，驅動天線跟蹤目標，常用的算法有很多，其中卡爾曼濾波算法效果最好。

實際工程應用中，由于缺乏有關目標運動的精確數據，該濾波算法往往存在缺陷，主要表現為［3～5，12］：一是如果目標運動模型是時變的，此算法容易受到初值效應的影響，數值穩定性不強；二是在目標跟蹤過程中，接收機輸出的角誤差信號中通常含有大量的野值序列，會導致濾波過程的不穩定，造成錯誤估計和預測目標的運動狀態。

為有效提高伺服系統跟蹤目標的穩定性并確保其具有較好的精度指標，本文嘗試引入UD因式分解和野值剔除算法［4～6］，利用改進的卡爾曼濾波算法估計目標位置，將估計目標角位置與天線實時角位置之差作為控制量進行放大，以解決系統濾波過程中算法發散引起的目標角位置估計不準確問題。經過實驗仿真，結果表明改進的濾波算法具有較好的穩定性。

2 當前統計模型的自適應跟蹤算法

對于目標沿觀察站方位或俯仰軸向x的運動：

通常采用修正的瑞利分布來描述機動目標加速度的概率密度，當其加速度采用非零均值時間相關模型時，系統的狀態方程可表示為［7］

式中，aˉ為目標角加速度均值，α為目標角加速度相關系數，其值是加速度時間常數的倒數，ω(t)為均值為零，方差為式（3）的白噪聲。

其中δ2α為目標的加速度方差。設T為采樣周期，則式（2）的離散化狀態方程為

式中

其中Xk=[xkkk]T，xk、k、k分別為目標的位置、速度和加速度。Wk是均值為零，方差為Qk的高斯白噪聲序列，并且Qk=σω2·q，其中

雷達和無線電跟蹤系統中常利用跟蹤接收機的角誤差信號Δx和天線基座角度傳感器信號xA重構指令角信號x［8］，可得：

其中v1主要是接收機輸出噪聲，也包含角度傳感器測量誤差和編碼誤差等。同時，與伺服電機相連的測速機輸出y2可作為目標在某支路該軸向運動速度的量測，可得：

其中v2為速度測量誤差，即測速機輸出噪聲，綜合式（6）和式（7）可得量測方程：

設r1和r2分別為零均值的白噪聲，可得

對于式（4）和式（8），可以構造關于狀態向量X的卡爾曼濾波器，其自適應濾波算法的迭代過程為

3 基于UD因式分解和野值剔除的改進算法

3.1UD因式分解

卡爾曼濾波算法能在測量噪聲干擾下對系統狀態進行無偏估計，是一種線性最小方差準則下的最佳估計方法。但在工程應用中，該算法容易發散，基于此本文引入UD因式分解算法，以保證誤差協方差矩陣P的對稱正定性［9］。該算法的核心是對一步預測誤差協方差矩陣Pk|k-1進行因式分解，利用預測更新因子Uk|k-1和Dk|k-1計算濾波增益矩陣Kk和估計協方差矩陣Pk，計算步驟如下：

對于Uk|k-1和Dk|k-1的第3列，通過以下公式可得［9］：

對于矩陣Uk|k-1和Dk|k-1的第j=1，j=2列可通過下式得出：

對于3階矩陣，當j=1，2，3時，3階對角陣Dk|k可通過下式得出：

其中f=UTHT，g=Df，b(0)=R。而對于

k|k-1kk|k-1k

3階矩陣，當i=1，···，j-1時，3階上三角矩陣Uk|k按式（22）計算：

3.2 野值剔除

由于客觀存在目標飛行過程中的環境噪聲及雜波干擾等不確定因素，接收機輸出的角誤差中往往會存在野值信息［5］，這會影響濾波過程，造成濾波器發散，因此在進行濾波以前本文應用文獻［6］的殘差檢驗方法，通過檢驗所構造的n維正態分布隨機向量的均值與方差陣是否與假設相符來實時地確定一組量測值的有效性。可通過下式計算：

其中，dk是為零均值高斯白噪聲的殘差，Sk是方差，式（27）為判別式。式中，目標角Yk是天線實時角位置θ1k與接收機輸出的目標相對天線角誤差Δx的合成信息，可表示為Yk=θk=θ1k+Δx，為達到對野值較好的判斷要求，文中m取5。若式（27）成立，則判斷該次測量值是野值，在利用式（14）計算當前狀態估計矩陣時，設Kk(1)=0，即濾波增益矩陣Kk的第一行為0，否則判斷該次測量值為正常值，不再對濾波增益陣Kk進行處理。

4 仿真實驗與分析

4.1 仿真思路

為有效提高伺服分系統的跟蹤精度，降低目標跟蹤過程中隨機誤差分量的影響，本文借鑒同軸跟蹤控制的思想［2，10～11］，將接收機的角誤差Δx與系統實時角度值θ1送入濾波器，利用誤差電壓對角度值進行修正，修正后的角度值再作為量測值輸入濾波器中進行濾波，外推出目標角位置θest，θest與天線實時角θ1之間的角誤差作為系統位置環路輸入，仿真計算流程圖見圖1。

4.2 仿真實驗與結果

本文利用文獻［2］和［6］中的實時角和角誤差數據，采用文中算法估計目標角位置，其中T=結果如圖2～4所示。

圖2 是采用改進后的濾波算法對目標角度的估計，圖3是采用基于傳統濾波算法的跟蹤方法計算得出的目標角誤差值，其均方差為0.0089°，圖4是采用基于改進濾波算法的跟蹤方法計算得出的目標角誤差值，其均方差為0.0053°。計算結果表明，本文算法具有較好的數值穩定性，能較好地提高系統的跟蹤精度。

5 結語

本文著重針對無線電測控系統中伺服系統的跟蹤算法進行了研究，提出了一種基于卡爾曼濾波算法的跟蹤控制方法。通過引入UD因式分解和野值剔除算法，對傳統算法進行了改進。實驗結果表明，改進的算法可較好提升系統的目標跟蹤能力，減小隨機誤差分量的不利影響，具有較好的數值穩定性，實用性較好。

［1］李邦復.遙測系統［M］.北京：宇航出版社，1992：123-127.

［2］白斌.減小某雷達伺服系統動態滯后的改進方案與實現［D］.成都：電子科技大學，2012：15-18.

［3］王妙，左繼章.Kalman濾波技術在北斗無源動態定位中的應用研究［J］.彈箭與制導學報，2008，28（2）：47-52.

［4］薛弈冰，李建東.基于U-D分解濾波的高性能盲目自適應多用戶檢測算法［J］.電路與系統學報，2004，9（1）：130-134.

［5］盧元磊，何佳洲，安瑾.目標預測中的野值剔除方法研究［J］.計算機與數字工程，2013，41（5）：722-725.

［6］翟茹玲，柴毅，張可.火箭飛行測控數據野值處理及其無跡Kalman濾波［J］.計算機工程與應用，2011，47（3）：233-236.

［7］錢華明，陳亮，滿國晶.基于當前統計模型的機動目標自適應跟蹤算法［J］.系統工程與電子技術，2011，33（10）：2154-2158.

［8］BASSEM R.雷達系統設計MATLAB仿真［M］.北京：電子工業出版社，2009：185-191.

［9］ZHANG Guo-dong，XU Xiao-su.Implementation of Tightly Coupled GPS/INS Navigation Algorit-hm on DSP［C］//2010 International Conference on Computer Design and Applications，2010，V2：130-134.

［10］王德純.精密跟蹤測量雷達技術［M］.北京：電子工業出版社，2006：171-174.

［11］錢平.伺服系統［M］.北京：機械工業出版社，2011：26-27.

［12］王海.基于單脈沖單通道的跟蹤數據處理［D］.成都：電子科技大學，2006：36-39.

A Tracking Control Method for Servo System Based on Improved Kalman Filter Algorithm

YAN YanLI Yang
（No.91245 Troops of PLA，Huludao125000）

Kalman filter algorithm is usually used in on-ax wireless measure and control system in hopes of estimating target's position.In the system，the angle error between real-time angle position of antenna and the estimated angle position is usual used as the input for position closed control loop in servo subsystem which will drive the antenna to track target.However，because there are many disadvantages caused by some random factors such as receiver noise，clutter interference and so on，some random errors will influence the stability of the algorithm seriously.This paper expects to improve the adaptive filter algorithm in order to prevent the instability of the filtering process by using UD factorization algorithm and outlier eliminating algorithm.Through simulation，it shows that the improved algorithm has good numerical stability and tracking accuracy.

Kalman filter，outlier eliminating，UD factorization

TP13；TN953

10.3969/j.issn.1672-9730.2017.06.007

2016年12月7日，

2017年1月19日

閆巖，男，碩士，助理工程師，研究方向：遙測系統。李楊，男，工程師，研究方向：光學測量。