基于迭代无迹卡尔曼滤波的小型无人机目标定位方法

唐大全,柳向阳,邓伟栋,丁鹏程

(海军航空大学,山东 烟台 264001)

在现代军用领域中,无人机常用于侦察监视、目标打击、作战效能评估等[1],对地面目标定位是侦察监视的重要需求之一,其目的为求取目标在大地坐标系下的三维坐标[2]。目前,高精度的无人机目标定位已成为国内外科研工作者的研究热点。

基于测距信息等有源定位方式,需要无人机有较大的载荷功率发射信息信号,小型无人机受功率载荷的限制,主要采用无源的定位方法,主要采集目标的图像,通过摄像机的内外参数及共线方程,计算目标的位置信息[3-4]。探测角度的唯方位测量方法是这类定位的常用方法,侧向传感器是获取这一信息的重要渠道。解决这类问题一般用非线性的滤波方法。

扩展卡尔曼滤波(Extended Kalman Filter, EKF)是解决非线性问题的方法之一,核心思想是将非线性方程在预测处进行一阶泰勒展开并取近似项,线性化之后再进行卡尔曼滤波,这样不可避免地存在偏差,当系统非线性较强和误差较大时,滤波可能发散[5]。无迹卡尔曼滤波[6-7](Unscented Kalman Filer, UKF)是基于无迹变换(Unscented Transformation, UT),使用变换后的统计量来估计状态的均值及方差,能够避免线性化带来的误差,由于无迹变化对于协方差估计的不足,测量误差较大时,存在收敛速度慢和收敛易发散的问题[8]。针对以上问题,本文在UKF算法基础上,提出了一种迭代UKF算法,不同于基于sigama采样点和基于高斯牛顿法的迭代法的UKF[9-10]估计目标状态,该方法通过极大似然函数确定迭代条件,对目标定位的量测环节进行迭代更新,缩小了量测误差,提高了迭代UKF算法的收敛速度和跟踪精度。

1 移动目标定位的原理

小型无人机的目标定位系统一般包括GPS定位系统、惯性测量单元和光电成像平台等。光电成像平台上一般有稳定伺服机构、摄像机云台和摄像机等部件。由于小型无人机功率载荷有限,激光测距仪等部件一般不能安装在无人机上。无人机定位时,将目标锁定在成像平面的视场中心,通过测量光心相对于目标的高低角和方位角,结合无人机的位姿信息,解算出目标位置。能够精准地对无人机定位和测量无人机与目标的相对角度是解决这类问题的关键。

1.1 视线方位角的计算

无人机目标定位就是基于摄影测量、图像处理和信息处理等技术,通过对无人机侦察以及与图像处理相关的遥测数据的处理与分析,提取目标精确三维坐标的过程[11]。在目标定位过程中,首先图像传感器采集图像,光电成像平台稳定伺服系统驱动CCD摄像机光轴对准目标,使目标位于成像靶面中心。定义视线方位角为在无人机地理坐标系中,采用东北天坐标系,视线高低角α为视线与无人机Z轴正向(向上)之间的夹角,视线水平角β为视线在水平面的投影与x轴正向(东向)之间的夹角,如图1所示,利用测角传感器测量这些角度值。在无人机的定位过程中,摄像机的光轴指向与无人机的姿态角共同决定了目标视线方位角。

视场中心点在摄像机坐标系下为tp(0,0,f),f为摄像机焦距。设无人机的偏航角ψ、俯仰角θ、横滚角γ和摄像机坐标系下光轴的方位ρ和高低角φ,可以得到无人机地理坐标系下的坐标为

(1)

(2)

图1 目标定位示意图

1.2 状态方程的建立

无人机对目标进行定位,需要围绕目标飞行一段距离,测量各时刻目标相对于无人机的角度信息,根据角度信息建立相应的量测方程,估计目标的位置。

xk+1=Φk+1,kxk+wk

(3)

zk=h(xk)+vk

(4)

wk～N(0,Qk)

上式中,在系统方程(3)和量测方程(4)中,wk,vk同时满足:

Qk,Rk分别为系统噪声序列的方差阵和量测噪声序列的方差阵,假定都是正定矩阵。量测方程为

(5)

2 目标定位的迭代无迹卡尔曼滤波算法

标准的UKF算法受数值计算舍入误差、可观测性和观测噪声大等因素的影响产生不稳定、收敛速度慢的问题。迭代无迹卡尔曼滤波是对无迹卡尔曼滤波的改进,能够克服无迹卡尔曼滤波收敛速度慢和收敛精度不高的缺点[12-13]。

2.1 迭代UKF的原理步骤

1)滤波初始化

2)计算sigama样本点集

(6)

3)时间更新

(7)

(8)

4)量测更新

产生新的采样点:

(9)

量测更新:

(10)

(11)

(12)

(13)

(14)

(15)

(16)

Pk,j=Pk,j-1-Kk,jPzz,k,jKk,j

(17)

5)确定迭代条件

对于滤波估计问题,测量更新的目的就是利用已有的测量信息估计到准确的状态量和协方差矩阵。考虑到独立的随机向量,服从正态分布,即:

(18)

式(19)可以构成一个新的变量:

(19)

由式(19)和式(20)可得

(20)

那么y的似然函数为

(21)

n和m分别为状态向量和量测向量的维数,那么对状态y的极大似然估计为

y*=arg max[L(y)]

(22)

比较式(22),也就是求

y*=arg min[q(x)]

(23)

其中

(24)

当

q(xj+1)

(25)

成立时,可以看到q(xj)时呈收敛趋势,也就是q(xj+1)比q(xj)更加接近最优解。

由式(24)、(25)可得

(26)

于是可得

(27)

其中:

(28)

重新进入步骤4),完成量测更新。当j不满足这个条件时,进入下一步。

2.2 目标状态估计仿真流程

目标状态估计就是利用无人机自身的状态信息,融合:测向传感器的测量信息,对地面目标状态的估计。具体流程如下:

Step1:锁定目标图像,测量位于光轴中心位置的目标的测量角(ρ,φ),结合无人机位置,利用公式(5)得到量测量z(k-1)。

Step3:从Step1开始,开始下一次估计。

3 仿真结果及分析

为了验证算法的性能,进行N次蒙特卡罗实验。采用均方误差(Root Mean Square Error,RMSE)来表示 目标定位与跟踪的精度,定位为

(29)

仿真实验1:无人机做盘旋上升飞行,目标静止。设无人机飞行半径100 m,圆心坐标为(500,600,400),初始航高z=400 m,盘旋上升飞行速度vz=1 m/s,运动角速度为0.02 rad/s,无人机自身定位服从均值为零,方差为10的正态分布;目标真实坐标为(350,450)。此时目标初始状态x0=[330,480,0,0],静止目标Q为零矩阵,无人机视轴角误差为1°,记为精度1,目标定位的均方误差如图2a所示,当无人机视轴角误差为0.1°,记为精度2,目标定位的均方误差如图2b所示。

图2 仿真情景1的目标定位均方误差图

图3 仿真情景2的目标定位均方误差图

视轴观测角误差是影响目标定位精度的主要因素,当观测误差较小,观测精度较高时,各算法都有较好的定位精度;观测精度较小误差比较大时,可以看到UKF和EKF定位精度相当,IUKF的收敛速度和定位精度略微优于前两者。在非线性较弱的条件下,UKF和IUKF的优势不明显。

仿真实验2目标做匀速直线运动,无人机对目标的仿真情况。设无人机飞行半径100 m,圆心坐标为(500,600,400),初始航高z=400 m,盘旋上升飞行速度vz=1 m/s,运动角速度为0.09 rad/s,无人机自身定位服从均值为零,方差为10的正态分布;目标初始起点为(350,450),以v=[3.9, 3.9]的速度运动,即x0=[330, 480, 0, 3.9, 3.9, 0]。无人机视轴角误差为1°,记为精度1,目标定位的均方误差如图3a所示,当无人机视轴角误差为0.1°,记为精度2,目标定位的均方误差如图3b所示。

从目标运动的跟踪定位图中可以看出,各种算法都可以很好地跟踪运动目标;在观测精度高和观测精度低的情况下,IUKF都能很好地跟踪运动目标,并且算法的收敛速度和收敛精度都明显好于前两种算法,能够达到快速稳定准确跟踪运动目标的效果。

4 结束语

本文提出了一种基于无迹卡尔曼滤波的无人机目标定位方法。利用小型无人机的光电成像平台将目标锁定在成像平面中心,鉴于小型无人机载荷能力有限的情况,测量无人机与目标的视轴偏角,以此构建目标定位的模型。无迹卡尔曼滤波是解决非线性问题的常用方法,由于计算误差和系统参数分配出现滤波发散和收敛速度慢等问题,迭代无迹卡尔曼滤波能够克服系统收敛速度慢和收敛发散的问题,提高收敛的效率。仿真实验表明,迭代无迹卡尔曼滤波能够提高滤波收敛速度,抑制滤波发散,具有一定的实用价值。