基于改进卡尔曼滤波的伺服跟踪控制方法

闫岩 李杨

（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日

闫岩，男，硕士，助理工程师，研究方向：遥测系统。李杨，男，工程师，研究方向：光学测量。