基于强跟踪滤波器的目标运动参数估计方法研究

《自动化技术与应用》２０１０年第２９卷第４期基于强跟踪滤波器的目标运动参数估计方法研究方晓旻，肖圣龙（中国人民海军驻广船国际公司军事代表室，广东　广州　５１０３８２）

摘　　要：针对船舶动力定位系统中目标跟踪控制需求，提出了一种基于强跟踪滤波器的目标运动参数估计方法，建立了两种目标运动

参考坐标系，给出了坐标系之间转换基本方法；设计了引入渐消因子的强跟踪滤波器进行目标运动状态和参数估计。通过与扩展卡尔曼滤波器的参数估计对比仿真试验，验证了基于强跟踪滤波器的目标运动参数估计方法具有较好的跟踪性能。

关键词：目标跟踪；运动参数估计；强跟踪滤波器；模型

中图分类号：Ｏ２３１．３；ＴＮ７１３　　　　文献标识码：Ａ　　　　文章编号：１００３－７２４１（２０１０）０４－００４１－０４

Research on Technique of Target Movement Parameters

Estimation Based on Strong Tracking Filter

FANG Xiao-min, XIAO Sheng-long

( Military Delegate Section of China People’s Liberation Army Navy stationed in GSI Corporation, Guangzhou 510382 China )

Abstract: Aiming to solve the problem of target tracking problem for ship dynamic positioning system, a method of target move-ment parameters estimation is proposed. Two types of reference coordination and target horizontal plane motion modelare set up. A strong tracking filter by introduced a time-varied eliminated factor is designed. It is suitable for a generalclass nonlinear time-varying systems and can be successfully used to real-time state estimation and parameter identification.Finally, by simulation tests the effectiveness and performance of the proposed strong tracking filter is demonstrated bycompared with extended Kalman filter in estimation of target moving parameters.

Key words: target tracking; target movement parameters estimation; strong tracking filter; model

１　　引言２１世纪是人类广泛应用水下航行器进行大范围、多功能作业的世纪，而水下航行器（例如有缆式遥控潜器ＲＯＶ、无缆式深潜救生艇ＤＳＲＶ、自主式水下航行器ＡＵＶ等）作为一类水下作业目标受其自身能力所限，通常需要水面船舶能够跟随其运动而运动，并对其水下作业过程提供辅助支持和安全保障。由此，水下目标跟踪这一新功能引入水面船舶动力定位系统中，即船舶自动操纵与控制单元通过水声测量单元实时采集水下目标的运动信息，通过自动操控推进器和舵桨等执行机构，使本船保持与水下目标的相对距离和方位不变。目前水下目标跟踪这一项实用的动力定位功能，仅挪威

Ｋｏｎｇｓｂｅｒｇ公司和法国Ａｌｓｔｏｍ公司产品中具备此项功能，国内在水面船舶的自动水下目标跟踪技术研究领域尚属空白。

收稿日期：２０１０－０２－０４

图１　　船舶跟踪水下目标原理示意图

Techniques of Automation & Applications | 41

《自动化技术与应用》２０１０年第２９卷第４期　　通信与信息处理Communication and Information Processing

船舶自动操纵与控制单元进行水下目标跟踪控制

［１］

摇角ϕ，纵摇角θ和首摇角φ，它们定义如下：

首摇角ψ是ox轴在水平面ξEη上的投影与Eξ轴之间的夹角；

纵摇角θ是ox轴与水平面ξEη之间的夹角；横摇角ϕ是xoz平面与通过ox轴的垂直平面xoζ之间的夹角，也即xoz平面的法线oy与xoζ平面的法线之间的夹角（设固定坐标系与运动坐标系的原点重合）。

ξηζ

x=S⋅y　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１）z

，需要首先对目标运动的动态特性进行估计，基于目标

运动特征的时间积累，估计目标的运动参数，从而实现船舶对运动目标的机动跟踪［２］。因此，本文从目标运动估计理论出发，以水下航行器为目标进行某型船操纵控制，使其自动跟踪机动目标［３］～［６］。首先建立机动目标典型运动模型，以Ｋｏｎｇｓｂｅｒｇ公司的Ｈｉｐａｐ　３５０水下定位声纳为目标位置测量单元，以实时采集的目标运动特征信息作为观测量，基于强跟踪滤波算法设计滤波器，对目标的运动参数做出合理估计。船舶跟踪水下目标的原理示意如图１所示。

其中，变换矩阵S满足下式：

cosψcosθcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ+sinψsinϕ

S=sinψcosθsinψsinθsinϕ+cosψcosϕsinψsinθcosϕ−cosψsinϕ（２）

cosθsinϕcosθcosϕ−sinθ

２　　水下目标运动模型的建立本文以水下航行器作为被跟踪目标，为简化目标跟踪问题，本文假设水下目标进行水平面三自由度运动，即仅考虑水下航行器的纵荡、横荡和艏摇方向运动。

首先，建立水下目标的固定坐标系和运动坐标系［７］

［８］

最后，根据滤波器设计需求，选取合适的滤波状态向量，并建立目标运动模型。假设大地坐标系下水下目标航行器沿Ox轴向运动，则利用牛顿动力学定律和运载器运动与建模等相关理论进行目标运动建模。这里建立了加入执行机构故障的水平面三自由度运动方程如式（３）～（５）：

&+vr+wq)=Xuu&+Xvrvr+Xuuuu+(Xnn+bLn)nLnL+(Xnn+bRnnRn)nRnRm(u

　（３）

，如图２所示。固定坐标系E−ξηζ又称大地坐标系，

其坐标原点E选取海面上任何一点，Eζ轴正向指向地心，　Eξ轴与Eη轴在水平面内相互垂直，构成右手直角坐标系。运动坐标系O−xyz的Ox轴取在水下目标的纵中剖面内，指向艏部，平行于水线面；Oy轴与纵中剖面垂直，指向右舷，平行于水平线；垂轴Oz指向底部，与水平面垂直。

&+ur)=Yuvuv+Yurur+Yvv&+Yrr&+Yvvvv+Yδr(δr+br)m(v

　　　　　　　　（４）

&=Nvrvr+Nurur+Nvv&+Nrr&+Nrrrr+Nuvuv+Nvvvv+IZr

Nδr(δr+br)+((Xnn+bLn)nLnL−(Xnn+bRn)nRnR)d

　　（５）

式中，m为航行器质量，d是左、右推进器到航行

Yδq、器纵中线距离；u、w、v、r为速度状态参数；Nδ、

r

r

为舵角系数；XLT

=(Xnn+bLn)nLnL

，XRT=(Xnn+bRn)nRnR

为左右推进器推力输出，δr为方向舵的舵角输出（弧度）；

bRn、br为引入的执行机构偏差量；其余参数为水bLn、

动力系数。

３　　强跟踪滤波器设计本文采用强跟踪卡尔曼滤波方法进行目标运动估

图２　　固定坐标系与运动坐标系

其次，给出两种坐标系下的坐标变换公式。两个坐标系之间通过基于坐标基底的三次绕轴旋转变换完成坐标变换，基本转换公式如式（１）所示。航行器在水下任一点的位置和姿态可以用运动坐标系原点O在固定坐标系上的坐标值(ξo,ηo,ςo)以及运动坐标系相对于固定坐标系的三个姿态角来确定，这三个姿态角分别称为横

计，与扩展卡尔曼滤波方法相比，该滤波方案具有较强的关于模型失配的鲁棒性、较小的关于噪声和初值统计特性的敏感性、极强的关于突变状态的跟踪能力和适中的计算复杂性［９］￣［１３］。设计滤波器时引入了渐消因子［１４］，通过实时调整状态预报误差的协方差阵P(k+1|k)以及相应的增益阵K(k+1)，对过去的数据进行渐消以减弱老数据对当前滤波值的影响。

42 | Techniques of Automation & Applications　　通信与信息处理Communication and Information Processing

《自动化技术与应用》２０１０年第２９卷第４期基于离散非线性运动方程构造强跟踪滤波器的结构如式（６）：

ˆ(k+1|k+1)=xˆ(k+1|k)+K(k+1)γ(k+1)　　　　　　　　　　　　　　（６）x

ˆi(k|k))β:min(∑∑xi(k)−x

β

k=0i=1

Ln

　　　　　　　　　　　　　　　　　　　　　　　　　　（１８）

其中，Ｌ为仿真步数。此准则反映了滤波器的累积误差。

至此，强跟踪滤波器构造完毕，当模型具有足够精度，且滤波器的初值xˆ(0|0)和P(0|0)选取得当时，按照上述方法可得到实际系统比较精确的状态估计值。

其中，状态向量x∈Rn，初始状态x(0)为高斯分布的随机向量，对应为纵向、横向和艏向的速度分量。

ˆ(k+1|k)为状态的一步预报：x

ˆ(k+1|k)=f(k,u(k),xˆ(k|k))　　　　　　　　　　　　　　　　　　（７）x增益阵：

ˆˆK(k+1)=P(k+1|k)HT(k+1,x(k+1|k))[H(k+1),x(k+1|k))ˆ(k+1|k))+R(k+1)]−1 gP(k+1|k)HT(k+1,x

４　　仿真试验与结果分析　（８）

仿真试验时首先设置如下初始值：采样周期：T=0.5s；

（９）　

状态变量初始值：x(0)=[000]，分别对应纵向、横向和艏向速度初始值；

状态变量初始估计值：xˆ(0|0)=[000]；预报协方差阵初始值：

（１０）（１１）

P(0|0)=diag[100.0100.0100.0]；

预报误差协方差阵：

ˆˆP(k+1|k)=λ(k+1)F(k,u(k),x(k|k))P(k|k)FT(k,u(k),x(k|k))+Γ(k)Q(k)ΓT(k)

其中，λ(k+1)≥1为时变的渐消因子。状态估计协方差阵：

ˆP(k+1|k+1)=[I−K(k+1)H(k+1,x(k+1|k))]P(k+1|k)残差序列：

系统噪声：Q=diag[0.0004测量噪声：R=diag[0.0009

0.0004

0.0001]；

ˆˆγ(k+1)=y(k+1)−y(k+1)=y(k+1)−h(k+1,x(k+1|k))

式（８）中

ˆ(k+1|k))=H(k+1,x

∂h(k+1,x(k+1))

|x(k+1)=xˆ(k+1|k)

∂x

0.0009

0.0001]；

假设水下目标在水平面以４ｍ／ｓ的纵向速度直线航

（１２）

行时，在第１００和第２００秒时，系统由于某种外界的强干扰，分别使纵向速度ｕ有±0.3u的突变，在图３和图４中分别给出了采用扩展卡尔曼滤波和强跟踪滤波器进行

ˆ(k|k)x(k)=x

式（９）中

∂[f(k,u(k),x(k))]

ˆ(k|k))=F(k,u(k),x

∂x

　　　　　　　　　　　　（１３）

状态跟踪的仿真试验曲线。

此时输出残差序列的协方差阵为：

V0(k+1)=E[γ(k+1)γT(k+1)]

ˆ(k+1|k)P(k+1|k)HT(k+1,xˆ≈H(k+1,x(k+1|k)+R(k+1)

（１４）

基于正交性原理来确定时变的渐消因子λ(k+1)，进而确定时变增益阵K(k+1)。本文采用近似算法（次优算法）求解渐消因子λ(k+1)。

次优渐消因子λ(k+1)由下式近似得到：

λ0≥λ0 1

λ(k+1)=　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１５）

1 λ0<1

其中：

λ0=

tr[N(k+1)]

tr[M(k+1)]　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１６）

ˆ(k+1|k))Γ(k)Q(k)ΓT(k)N(k+1)=V0(k+1)−H(k+1,x

ˆ(k+1|k))−βR(k+1) gH(k+1,xˆˆM(k+1)=H(k+1,x(k+1|k))F(k,u(k),x(k|k))P(k|k)（１７）ˆˆ gFT(k,u(k),x(k|k))HT(k+1,x(k+1|k))

式中，β≥1为一个选定的弱化因子。引入此弱化因子的目的是使状态估计更加平滑。此数值可以凭经验来选择，也可由仿真得到，其选取可由式（１８）确定：

图３　　扩展卡尔曼滤波器的状态跟踪曲线

在图３（ａ）、图４（ａ）中分别给出了水下目标纵向速度的实际曲线和两种滤波器的状态估计曲线。从图３（ｂ）纵

Techniques of Automation & Applications | 43

《自动化技术与应用》２０１０年第２９卷第４期　　通信与信息处理Communication and Information Processing

向速度残差曲线可以看到，当突变发生时，扩展卡尔曼滤器估计出的纵向速度与实际值有较大差距，误差最大时接近１ｍ／ｓ；相反，从图４（ｂ）纵向速度残差曲线可以看出，强跟踪滤波器在系统达到稳态之后，对系统的突变仍然具有很强的状态跟踪能力，跟踪效果相当理想；图４（ｃ）中给出了次优渐消因子的响应曲线，揭示了次优渐消因子的自动调节机理，也正是由于次优渐消因子的调整，使得滤波器具有较好的跟踪性能。

进一步研究水面船舶基于传感器的目标跟踪控制研究具有重要的应用价值。

参考文献：［１］　周宏仁，敬忠良，王培德．机动目标跟踪［Ｍ］．北京：国防工业出版社，１９９１．

［２］　ＪＯＨＳＯＮ　Ｇ　Ｗ．Ｃｈｏｉｃｅ　ｏｆ　ｃｏｏｒｄｉｎａｔｅｓ　ａｎｄ　ｃｏｍｐｕ－ｔａｔｉｏｎａｌ　ｄｉｆｆｉｃｕｌｔｙ［Ｃ］．ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎ　ｏｎ　Ａｕｔｏｍａｔｉｃ　Ｃｏｎｔｒｏｌ，１９７４，１９（２）：７７－８０．

［３］　王小陵．系统建模与参数估计［Ｍ］．哈尔滨：哈尔滨工程大学，２００２．

［４］　董立志，孙茂相，朱枫．基于实时障碍物预测的机器人运动规划［Ｊ］．机器人，２０００，（１）：１２－１６．

［５］　牛君，李贻斌．一种基于ＢＦＧＳ拟牛顿公式的运动目标跟踪方法［Ｊ］．系统仿真学报，２００７，１９（１）：１３－１５，３４．

［６］　周峰，孟秀云．基于线偏差测量的目标运动参数估计方法及仿真研究［Ｊ］．系统仿真学报，２００８，２０（９）：２４９１－２４９３，２４９７．

［７］　陈涛．ＡＵＶ推进系统故障诊断与容错方法研究［Ｄ］．哈尔滨工程大学硕士学位论文，２００８．

［８］　刘向明，边信黔，施小成．强跟踪滤波器在ＡＵＶ执行机构故障诊断中的应用［Ｊ］．应用科技，２００４，（１１）：３２－３５．

［９］　范文兵，刘春风，张素贞．一种强跟踪扩展卡尔曼滤波器的改进算法［Ｊ］．控制与决策，２００６，（１）：７３－７６．

［１０］　石章松，刘忠．单站纯方位目标多模型卡尔曼滤波跟踪算法的研究［Ｊ］．系统仿真学报，２００６，１８（７）：３９－４１．

［１１］　Ａ．ＹＩＬＭＡＺ．Ｏｂｊｅｃｔ　Ｔｒａｃｋｉｎｇ　ｂｙ　Ａｓｙｍｍｅｔｒｉｃ　ＫｅｒｎｅｌＭｅａｎ　Ｓｈｉｆｔ　ｗｉｔｈ　Ａｕｔｏｍａｔｉｃ　Ｓｃａｌｅ　ａｎｄ　Ｏｒｉｅｎｔａｔｉｏｎ　Ｓｅｌｅｃｔｉｏｎ［Ｃ］．ｌＥＥＥ　Ｃｏｎｆ．ｏｎ　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ，２００７．

［１２］　Ｄ．ＣＯＭＡＮＩＣＩＵ，Ｖ．ＲＡＭＥＳＨ，ａｎｄ　ＥＭＥＥＲ，Ｋｅｒｎｅｌ．Ｂａｓｅｄ　Ｏｂｊｅｃｔ　Ｔｒａｃｋｉｎｇ［Ｃ］，ＩＥＥＥ　Ｔｒａｎｓ．Ｐａｔｔｅｒｎ　Ａｎａｌｙｓｉｓａｎｄ　Ｍａｃｈｉｎｅ　Ｉｎｔｅｌｌｉｇｅｎｃｅ，２００３，２５（５）：５６４－５７５．

图４　　强跟踪滤波器的状态跟踪曲线

［１３］　Ａ．Ｌ　ＯＺＡ，Ｌ．ＭＩＨＡＹＬＯＶＡ，Ｎ．ＣＡＮＡＧＡＲＡＪＡＨ，Ｄ．Ｂｕｌｌ，Ｓｔｒｕｃｔｕｒａｌ　Ｓｉｍｉｌａｒｉｔｙ－Ｂａｓｅｄ　Ｏｂｊｅｃｔ　Ｔｒａｃｋｉｎｇ　ｉｎＭｕｌｔｉｍｏｄａｌｉｔｙ　Ｓｕｒｖｅｉｌｌａｎｃｅ［Ｃ］，Ｍａｃｈｉｎｅ　Ｖｉｓｉｏｎ　ａｎｄ　Ａｐｐｌｉ－ｃａｔｉｏｎｓ　Ｊｏｕｒｎａｌ，Ｓｐｒｉｎｇｅｒ，２００７．

［１４］　刘铭，周东华．残差归一化的强跟踪滤波器及其应用［Ｊ］．中国电机工程学报，２００５，（１）：７２－７５．

５　　结束语水下目标跟踪作为一项重要的船舶动力定位系统功能，目标跟踪控制精度是评价这一功能的重要指标，为确保跟踪控制精度，基于滤波器的目标运动状态估计便显得尢为重要。本文为简化问题研究，建立了水下目标运动参考坐标系和水平面三自由度运动模型，基于扩展卡尔曼滤波理论设计了带渐消次优因子的强跟踪滤波器，用于对目标运动状态与参数进行联合估计，通过与扩展卡尔曼滤波器状态估计方法的对比仿真试验，验证了本文所提出目标运动参数估计方法的有效性，对于

44 | Techniques of Automation & Applications作者简介：方晓旻（１９７１－），工程师，硕士，研究方向：船舶电气控制工程。