閆巖 李楊

（91245部隊(duì)葫蘆島125000）

基于改進(jìn)卡爾曼濾波的伺服跟蹤控制方法

閆巖 李楊

（91245部隊(duì)葫蘆島125000）

同軸跟蹤無(wú)線電測(cè)控系統(tǒng)中，通常利用濾波外推算法將濾波器估計(jì)的目標(biāo)角位置與天線實(shí)時(shí)角位置的角誤差作為伺服系統(tǒng)位置環(huán)路的輸入以驅(qū)動(dòng)天線跟蹤目標(biāo)。然而，伴隨目標(biāo)跟蹤過(guò)程中接收機(jī)熱噪聲、雜波干擾等不利因素的制約，隨機(jī)誤差分量會(huì)嚴(yán)重影響算法的穩(wěn)定性，文中對(duì)自適應(yīng)濾波算法進(jìn)行了改進(jìn)，引入U(xiǎn)D因式分解和野值剔除算法以防止濾波過(guò)程的不穩(wěn)定。仿真結(jié)果表明改進(jìn)算法具有較好的數(shù)值穩(wěn)定性和系統(tǒng)跟蹤精度。

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

Class NumberTP13；TN953

1 引言

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

實(shí)際工程應(yīng)用中，由于缺乏有關(guān)目標(biāo)運(yùn)動(dòng)的精確數(shù)據(jù)，該濾波算法往往存在缺陷，主要表現(xiàn)為［3～5，12］：一是如果目標(biāo)運(yùn)動(dòng)模型是時(shí)變的，此算法容易受到初值效應(yīng)的影響，數(shù)值穩(wěn)定性不強(qiáng)；二是在目標(biāo)跟蹤過(guò)程中，接收機(jī)輸出的角誤差信號(hào)中通常含有大量的野值序列，會(huì)導(dǎo)致濾波過(guò)程的不穩(wěn)定，造成錯(cuò)誤估計(jì)和預(yù)測(cè)目標(biāo)的運(yùn)動(dòng)狀態(tài)。

為有效提高伺服系統(tǒng)跟蹤目標(biāo)的穩(wěn)定性并確保其具有較好的精度指標(biāo)，本文嘗試引入U(xiǎn)D因式分解和野值剔除算法［4～6］，利用改進(jìn)的卡爾曼濾波算法估計(jì)目標(biāo)位置，將估計(jì)目標(biāo)角位置與天線實(shí)時(shí)角位置之差作為控制量進(jìn)行放大，以解決系統(tǒng)濾波過(guò)程中算法發(fā)散引起的目標(biāo)角位置估計(jì)不準(zhǔn)確問(wèn)題。經(jīng)過(guò)實(shí)驗(yàn)仿真，結(jié)果表明改進(jìn)的濾波算法具有較好的穩(wěn)定性。

2 當(dāng)前統(tǒng)計(jì)模型的自適應(yīng)跟蹤算法

對(duì)于目標(biāo)沿觀察站方位或俯仰軸向x的運(yùn)動(dòng)：

通常采用修正的瑞利分布來(lái)描述機(jī)動(dòng)目標(biāo)加速度的概率密度，當(dāng)其加速度采用非零均值時(shí)間相關(guān)模型時(shí)，系統(tǒng)的狀態(tài)方程可表示為［7］

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

其中δ2α為目標(biāo)的加速度方差。設(shè)T為采樣周期，則式（2）的離散化狀態(tài)方程為

式中

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

雷達(dá)和無(wú)線電跟蹤系統(tǒng)中常利用跟蹤接收機(jī)的角誤差信號(hào)Δx和天線基座角度傳感器信號(hào)xA重構(gòu)指令角信號(hào)x［8］，可得：

其中v1主要是接收機(jī)輸出噪聲，也包含角度傳感器測(cè)量誤差和編碼誤差等。同時(shí)，與伺服電機(jī)相連的測(cè)速機(jī)輸出y2可作為目標(biāo)在某支路該軸向運(yùn)動(dòng)速度的量測(cè)，可得：

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

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

對(duì)于式（4）和式（8），可以構(gòu)造關(guān)于狀態(tài)向量X的卡爾曼濾波器，其自適應(yīng)濾波算法的迭代過(guò)程為

3 基于UD因式分解和野值剔除的改進(jìn)算法

3.1UD因式分解

卡爾曼濾波算法能在測(cè)量噪聲干擾下對(duì)系統(tǒng)狀態(tài)進(jìn)行無(wú)偏估計(jì)，是一種線性最小方差準(zhǔn)則下的最佳估計(jì)方法。但在工程應(yīng)用中，該算法容易發(fā)散，基于此本文引入U(xiǎn)D因式分解算法，以保證誤差協(xié)方差矩陣P的對(duì)稱正定性［9］。該算法的核心是對(duì)一步預(yù)測(cè)誤差協(xié)方差矩陣Pk|k-1進(jìn)行因式分解，利用預(yù)測(cè)更新因子Uk|k-1和Dk|k-1計(jì)算濾波增益矩陣Kk和估計(jì)協(xié)方差矩陣Pk，計(jì)算步驟如下：

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

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

對(duì)于3階矩陣，當(dāng)j=1，2，3時(shí)，3階對(duì)角陣Dk|k可通過(guò)下式得出：

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

k|k-1kk|k-1k

3階矩陣，當(dāng)i=1，···，j-1時(shí)，3階上三角矩陣Uk|k按式（22）計(jì)算：

3.2 野值剔除

由于客觀存在目標(biāo)飛行過(guò)程中的環(huán)境噪聲及雜波干擾等不確定因素，接收機(jī)輸出的角誤差中往往會(huì)存在野值信息［5］，這會(huì)影響濾波過(guò)程，造成濾波器發(fā)散，因此在進(jìn)行濾波以前本文應(yīng)用文獻(xiàn)［6］的殘差檢驗(yàn)方法，通過(guò)檢驗(yàn)所構(gòu)造的n維正態(tài)分布隨機(jī)向量的均值與方差陣是否與假設(shè)相符來(lái)實(shí)時(shí)地確定一組量測(cè)值的有效性?？赏ㄟ^(guò)下式計(jì)算：

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

4 仿真實(shí)驗(yàn)與分析

4.1 仿真思路

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

4.2 仿真實(shí)驗(yàn)與結(jié)果

本文利用文獻(xiàn)［2］和［6］中的實(shí)時(shí)角和角誤差數(shù)據(jù)，采用文中算法估計(jì)目標(biāo)角位置，其中T=結(jié)果如圖2～4所示。

圖2 是采用改進(jìn)后的濾波算法對(duì)目標(biāo)角度的估計(jì)，圖3是采用基于傳統(tǒng)濾波算法的跟蹤方法計(jì)算得出的目標(biāo)角誤差值，其均方差為0.0089°，圖4是采用基于改進(jìn)濾波算法的跟蹤方法計(jì)算得出的目標(biāo)角誤差值，其均方差為0.0053°。計(jì)算結(jié)果表明，本文算法具有較好的數(shù)值穩(wěn)定性，能較好地提高系統(tǒng)的跟蹤精度。

5 結(jié)語(yǔ)

本文著重針對(duì)無(wú)線電測(cè)控系統(tǒng)中伺服系統(tǒng)的跟蹤算法進(jìn)行了研究，提出了一種基于卡爾曼濾波算法的跟蹤控制方法。通過(guò)引入U(xiǎn)D因式分解和野值剔除算法，對(duì)傳統(tǒng)算法進(jìn)行了改進(jìn)。實(shí)驗(yàn)結(jié)果表明，改進(jìn)的算法可較好提升系統(tǒng)的目標(biāo)跟蹤能力，減小隨機(jī)誤差分量的不利影響，具有較好的數(shù)值穩(wěn)定性，實(shí)用性較好。

［1］李邦復(fù).遙測(cè)系統(tǒng)［M］.北京：宇航出版社，1992：123-127.

［2］白斌.減小某雷達(dá)伺服系統(tǒng)動(dòng)態(tài)滯后的改進(jìn)方案與實(shí)現(xiàn)［D］.成都：電子科技大學(xué)，2012：15-18.

［3］王妙，左繼章.Kalman濾波技術(shù)在北斗無(wú)源動(dòng)態(tài)定位中的應(yīng)用研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2008，28（2）：47-52.

［4］薛弈冰，李建東.基于U-D分解濾波的高性能盲目自適應(yīng)多用戶檢測(cè)算法［J］.電路與系統(tǒng)學(xué)報(bào)，2004，9（1）：130-134.

［5］盧元磊，何佳洲，安瑾.目標(biāo)預(yù)測(cè)中的野值剔除方法研究［J］.計(jì)算機(jī)與數(shù)字工程，2013，41（5）：722-725.

［6］翟茹玲，柴毅，張可.火箭飛行測(cè)控?cái)?shù)據(jù)野值處理及其無(wú)跡Kalman濾波［J］.計(jì)算機(jī)工程與應(yīng)用，2011，47（3）：233-236.

［7］錢華明，陳亮，滿國(guó)晶.基于當(dāng)前統(tǒng)計(jì)模型的機(jī)動(dòng)目標(biāo)自適應(yīng)跟蹤算法［J］.系統(tǒng)工程與電子技術(shù)，2011，33（10）：2154-2158.

［8］BASSEM R.雷達(dá)系統(tǒng)設(shè)計(jì)MATLAB仿真［M］.北京：電子工業(yè)出版社，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］王德純.精密跟蹤測(cè)量雷達(dá)技術(shù)［M］.北京：電子工業(yè)出版社，2006：171-174.

［11］錢平.伺服系統(tǒng)［M］.北京：機(jī)械工業(yè)出版社，2011：26-27.

［12］王海.基于單脈沖單通道的跟蹤數(shù)據(jù)處理［D］.成都：電子科技大學(xué)，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日

閆巖，男，碩士，助理工程師，研究方向：遙測(cè)系統(tǒng)。李楊，男，工程師，研究方向：光學(xué)測(cè)量。