1 引言
水下运动目标的运动估计技术[1-4]的应用与研究始于上世纪70年代,到90年代成为各国科研机构和公司大力研究的热点,国内外学者提出了很多有效的目标运动估计算法。对于线性系统而言,Kalman滤波是解决估计问题的最优方案[5]。而对于非线性系统而言,并没有一个完全的最优状态估计方案,为此人们提出了很多次优的近似方法,其中应用最广泛的是扩展Kalman滤波(EKF)及其改进方法[6]。EKF算法本身固有的缺点是一阶线性化精度较低,需要计算Jacobian矩阵且非线性函数必须满足可微性,当面对强非线性或非高斯函数时,EKF的估计精度严重降低甚至发散。为了克服上述缺点,Julier和Uhlmann提出了不敏卡尔曼滤波(UKF)[7](unscented KalmanFilter)算法。UKF方法直接使用系统的非线性模型,不像EKF方法那样需要对非线性系统加以线性化,也不需要如二次滤波方法一样计算Jacobin矩阵,且具有和EKF方法相同的算法结构。对于线性系统,UKF和EKF具有同样的估计性能,但对于非线性系统,UKF方法则可以得到更好的估计结果。Wan和Merwe将UKF方法引入到非线性模型的参数估计和双估计中,并提出了UKF方法的方根滤波算法,该算法不仅可以确保滤波的计算稳定性,而且大大减少了实际的计算量。尽管UKF算法能够较好地解决非线性系统的
运动估计问题,但是UKF不能应用于非高斯分布系统,而粒子滤波(PF)[8-11]可以有效地解决这一问题。PF的核心思想是将所需的后验概率密度函数通过一组随机样本表现出来,每一个随机样本都对应一个权值,通过计算这些对应权值的随机样本来得到后验概率密度函数。当样本容量很大时,这种蒙特卡罗的描述就等价于真实的后验概率函数,此时,序贯重要性抽样逼近最佳贝叶斯估计,即可实现基于序贯模拟的统计滤波方法。随着计算存储成本的降低,PF相对于采取解析形式对非线性系统进行近似,得到次优滤波估计值的传统方法来说,极大地提高了算法的精确性、实时性和鲁棒性。估值维数的多少对粒子滤波影响不大,并且该方法适用于任意非线性模型、任意分布的噪声,且具有算法简单、易于编程实现等优点。与扩展卡尔曼滤波方法比较,其滤波效果更好,适用范围更广[12-15]。
本文的主要研究内容为:对水下运动目标的典型运动形式(线性运动,非线性随机运动),分别利用扩展卡尔曼滤波、不敏卡尔曼滤波以及基于重要性采样的粒子滤波进行目标的运动估计,通过三种运动估计效果对比验证UKF和PF在非线性领域相对于EKF的优势。
2 目标运动模型
运动目标的参数估计首先要解决目标的运动模型问题,常用的目标模型有匀速直线运动模型、匀加速直线运动模型以及变加速直线运动模型等。目标运动的动力学模型分为非机动目标模型和机动目标模型两大类。在这里,机动指目标在飞行或航行时突然出现的转向、变速等运动。所以,本文将以上提到的三种单目标运动模型作如下的分类:匀速直线运动模型和匀加速直线运动模型是两种基于直线运动的非机动模型,该模型将目标的运动先验地定义为简单的匀速或匀加速运动,机动可以看作是一种随机的输入,其大小体现在过程噪声的标准差之中;变加速直线运动模型是机动模型,既有加速度的变化又有过程噪声的存在。当目标作匀速直线运动时,匀速直线运动模型包含目标的位置分量、速度分量以及量测噪声;当目标作匀加速或变加速直线运动时,匀加速或变加速直线运动模型中所包含的分量除了有位置分量、速度分量和量测噪声以外,还有加速度分量。
绝大多数的目标运动尤其是目标机动沿各坐标轴的分量是耦合的。为了简单起见,许多已经开发的机动
模型仍然假定坐标耦合很弱,可以忽略不计,由此只须在单一坐标方向上进行运动建模即可。下面的运动模型建模以及仿真实验等均建立于单一坐标方向(X)上。
2.1 目标的线性运动建模
匀速直线运动模型可以表示为式
为测量噪声,服从高斯白噪声分布:均值为零、方差为;
为加速度变化率;
为采样间隔。
以上三种模型是目标运动模型中最为基本的三种模型,其计算量小,能够满足实时运动估计的要求。对于匀速直线运动、匀加速直线运动和变加速直线运动以及近似于匀速、匀加速和变加速的运动,上述模型能够达到相当高的估计精度。
由公式不难看出,当=1时,式(2)和(3)完全相同,即加速度变化率为1,即目标作匀加速直线运动。
2.2 目标的非线性运动建模
考虑系统状态和观测模型如式(4)、式(5):
k=0,1,...,N-1 (4)
k=0,1,...,N (5)
式中,f和h为非线性函数;状态噪声和观测噪
声。当和均为高斯分布时则系统为非线性高斯分布系统,当和均为非高斯分布时则系统为非线性非高斯系统。时刻状态的后验分布如式(6)、式(7)所示:
3 单目标运动估计器设计
3.1 UKF运动估计器
非线性系统的离散时间状态系统方程和量测方程分别为式(8)、式(9):
至此,得到了UKF在时刻的滤波状态和方差。
3.2 重要性采样粒子滤波器
假设一个连续的非线性动态系统的状态空间模型可表示为式(26)、式(27):
其中,,分别表示目标状态向量和观测向量,表示系统状态的非线性转移函数,表示系统状态的非线性观测函数,和相互独立且与系统状态无关,分别表示状态转移噪声和观测噪声,分别表示状态矢量维数和观测矢量维数。
标准的粒子滤波算法(SIR)即为引入了重采样的 SIS 算法,并且选择具有先验性质的系统状态转移概率分布作为参考分布。SIR 算法的具体流程如下:
(1) 初始化从中采样得到,初始化权重均为1/N.
(2) 重要性采样
a.从建议分布中采样得到粒子集合
b.计算 k 时刻粒子的权值,如式(28)所示:
(28)
c.归一化权值,得到式(29):
(29)
(3)重采样
重采样以后,得到新的粒子集,并将其权值归一化为;
(4)状态估计
计算得到估计值,如式(30)。
. (30)
粒子滤波算法的原理图如图1所示:
图1 粒子滤波流程图
4 仿真实验与结果分析
利用前视声纳测得数据在Matlab中进行仿真,在本次仿真实验中分别对线性模型和非线性模型进行仿真实验。其中线性模型选取匀加速直线运动(线性高斯)、非线性模型选取非线性随机运动(非线性高斯)以及非线性非高斯的随机系统的三组数据进行仿真。在此次仿真实验中,三个运动估计器的初始状态均为0.1。
4.1 匀加速直线运动仿真实验
选取式(2.2)的匀加速直线运动模型完成本次仿真实验。
仿真实验中,噪声统计是已知的,实验中设置均值=0.0,方差=10,测量噪声的均值=0.0,方差=1。仿真过程中生成初始位置为0.1m,进行匀加速直线运动估计仿真得到仿真图如图2和图3所示。图2是三种滤波器对匀加速直线运动的运动轨迹估计图,由图可以看出实际运动轨迹与三者的估计轨迹几乎重合,说明三者均能较好进行运动估计,图3是三者的估计误差图,EKF估计误差均方值=0.1973、PF估计误差均方
值=0.19914、UKF估计误差均方值=0.19659。说明当系统为线性系统,且过程和量测噪声为高斯分布时,EKF、UKF、PF均可以获得最优估计。
4.2 非线性(非)高斯随机运动估计仿真实验
为了验证算法和证明其有效性,采用单变量非静态增长模型建立非线性系统模型如式(31)和式(32)。
状态方程为:
图2 匀加速直线运动位置估计仿真图
图3 匀加速直线运动位置估计误差图
针对非线性系统模型开展非线性高斯随机运动估计和非线性非高斯随机运动估计仿真实验。其中,非线性高斯实验中设置 均值=0,方差=10,的均值=0,方差=1;非线性非高斯实验中设置 均值=0.5,方差=10,的均值=0.5,方差=1。
图4 非线性高斯运动位置估计图
图5 非线性高斯运动位置估计误差图
图4和图5是非线性高斯随机运动的仿真图,图4的运动估计曲线表明UKF和PF可以较为准确的跟踪实际轨迹即可以获得最优估计,而EKF的估计曲线和实际运动轨迹有较大的出入,图5中的三者的估计误差图,其中EKF估计误差均方值=18.8126、PF估计误差均方值=4.9016、UKF估计误差均方值=7.9377。可看出UKF和PF的估计值误差较小,EKF的估计值误差较大,说明对于非线性高斯系统EKE的滤波和预测精度很难保证,UKF和PF方法则可以得到更好的估计。
图6和图7是非线性非高斯的随机系统仿真图。从图6可以看出对于非线性非高斯的随机系统PF可以较为准确的跟踪实际轨迹,UKF在部分区域可以较好的跟踪实际轨迹但在有的区域则不能,EKE则完全不能准确的跟踪。图7的估计值误差曲线EKF估计误差均方值=22.0538、PF估计误差均方值=1.3815、UKF估计误差均方值=9.7025。结果表明:对于非线性非高斯的随机系统,PF 的运动估计效果最好,UKF次之,EKF的估计效果最差。
图6 非线性非高斯运动位置估计图
图7 非线性非高斯运动位置估计误差图
5 结束语
综上所述,对于水下自主航行器(AUV)执行水下目标跟踪任务时,当目标做线性运动时,EKF、UKF、PF对于目标的运动估计都能得到较为精确的跟踪轨迹;目标做非线性随机运动时,对于强非线性的高斯系统,UKF和PF能较好地进行运动估计,对EKF而言其滤波和预测精度很难保证;对于非线性非高斯系统,PF可以实现运动目标的运动估计且其精度较高,而UKF的跟踪精度则要差一些。说明在目标运动估计领域,粒子滤波和不敏卡尔曼滤波将有更好的应用效果,其应用范围也将更加广泛。