中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
指挥控制与仿真

指挥控制与仿真 >

2019 , Vol. 41 >Issue 1: 104 - 108

DOI: https://doi.org/10.3969/j.issn.1673-3819.2019.01.021

工程实践

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

  • 唐大全 ,
  • 柳向阳 ,
  • 邓伟栋 ,
  • 丁鹏程
展开
  • 海军航空大学, 山东 烟台 264001

唐大全(1965-),男,贵州都匀人,教授,研究方向为飞行器导航与控制,自动测试与故障诊断。

柳向阳(1989-),男,硕士研究生,助理工程师。

收稿日期: 2018-06-02

  修回日期: 2018-06-28

  网络出版日期: 2022-05-20

收起

Small Unmanned Aerial Vehicle Target Location Method Based on Iterative Unscented Kalman Filtering

  • TANG Da-quan ,
  • LIU Xiang-yang ,
  • DENG Wei-dong ,
  • DING Peng-cheng
Expand
  • Naval Aviation University, Yantai 264001, China

Received date: 2018-06-02

  Revised date: 2018-06-28

  Online published: 2022-05-20

Fold

摘要

为了提高无人机地面目标跟踪定位的准确性,用无人机光电成像平台将目标锁定在视场中心,根据坐标转换将视场角度转换为地理坐标系下的角度,建立目标跟踪的系统方程。无迹卡尔曼滤波用来解决跟踪定位的非线性估计,由于容易出现跟踪速度慢和发散的问题,迭代无无迹卡尔曼滤波由极大似然估计法确定迭代条件,增加滤波的精度和时间。仿真分析表明,迭代无迹卡尔曼滤波能够明显改善滤波准确度和滤波的速度的问题,具有一定的实用价值。

关键词： 小型无人机; 目标定位; 无迹卡尔曼滤波; 极大似然估计

本文引用格式

唐大全 , 柳向阳 , 邓伟栋 , 丁鹏程 . 基于迭代无迹卡尔曼滤波的小型无人机目标定位方法[J]. 指挥控制与仿真, 2019 , 41(1) : 104 -108 . DOI: 10.3969/j.issn.1673-3819.2019.01.021

Abstract

In order to improve the accuracy of UAV ground target tracking and positioning, the UAV optoelectronic imaging platform was used to lock the target in the center of the field of view. According to the coordinate transformation, the angle of view was converted to the angle in the geographic coordinate system, and the target tracking System equations was established. Unscented Kalman filter was used to solve the non-linear estimation of tracking and positioning. Due to the problems of slow tracking speed and divergence, iterative non-unscrambled Kalman filter was used to determine the iteration condition by maximum likelihood estimation method, which increases the accuracy and time of filtering. Simulation analysis shows that iterative unscented Kalman filter can significantly improve the accuracy of filtering and the speed of filtering, which has certain practical value.

Key words： small unmanned aerial vehicle; target location; unscented Kalman filter; maximum likelihood estimation

在现代军用领域中,无人机常用于侦察监视、目标打击、作战效能评估等[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所示,利用测角传感器测量这些角度值。在无人机的定位过程中,摄像机的光轴指向与无人机的姿态角共同决定了目标视线方位角。
图1 目标定位示意图
视场中心点在摄像机坐标系下为tp(0,0,f),f为摄像机焦距。设无人机的偏航角ψ、俯仰角θ、横滚角γ 和摄像机坐标系下光轴的方位ρ和高低角φ,可以得到无人机地理坐标系下的坐标为
tn=(x,y,z)=Ro t b n·Ro t c b·tp
Ro t c b为摄像机坐标c系到载体坐标b系的旋转矩阵,由光轴的方位ρ和高低角φ;Ro t b n为载体坐标系到地理坐标系的旋转矩阵,由无人机的偏航角ψ、俯仰角θ、横滚角γ决定。根据式(1),可得视线方位角(α,β)为
$\beta=\pi+\arctan \frac{\sqrt{x^{2}+y^{2}}}{z}, \beta \in\left(\frac{\pi}{2}, \pi\right] \\ \alpha=\arctan \frac{y}{x}, \alpha \in(0,2 \pi]$

1.2 状态方程的建立

无人机对目标进行定位,需要围绕目标飞行一段距离,测量各时刻目标相对于无人机的角度信息,根据角度信息建立相应的量测方程,估计目标的位置。
假设无人机绕飞行过程中,对目标进行了M次观察,第k次观测目标位置xk=[tx, ty, ty, t ˙ x, t ˙ y, t ˙ z ] T k,无人机位置(px,py,pz),假设目标做匀速直线运动,那么目标的状态方程可以表示为
xk+1=Φk+1,k xk+wk
Φk+1,k= I 3 Δ t I 3 0 3 I 3为系统转移矩阵,Δt为采样时间,wk为系统噪声矩阵,系统的量测方程为
zk=h(xk)+vk
wk ~ N(0, Qk)
上式中,在系统方程(3)和量测方程(4)中,wk,vk同时满足:
E[wk]=0, cov[wk, w k j]=E[ w k k w T k j]=Qk δkj
E[vk]=0, cov[vk,vj]=E[vk v T j]=Rk δkj
cov[wk,vj]=E[wk v T j]=0
Qk,Rk分别为系统噪声序列的方差阵和量测噪声序列的方差阵,假定都是正定矩阵。量测方程为
h(xk)= ρ ω= a r c t a n ( t x - p x ) 2 + ( t y - p y ) 2 t z - p z a r c t a n t y - p y t x - p x

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

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

2.1 迭代UKF的原理步骤

1)滤波初始化
给定初始状态、状态协方差阵初始化值 x ˙ 0=E[x0],P0=E[(x0- x ˙ 0)(x0- x ˙ 0)T];假定系统过程噪声和量测噪声分别为Qk,Rk,噪声特性如上节所示。
2)计算sigama样本点集
χk-1=[ x ˙ k - 1, x ˙ k - 1±( ( n + λ ) P k - 1)i]
3)时间更新
χ k , k - 1 ( i )=Φk,k-1χk-1
P k -= ∑ i = 0 2 n W i ( c ) ( χ k , k - 1 ( i )- x ˙ k -)( χ k , k - 1 ( i )- x ˙ k -)T+Qk
权重系数 W i ( c ), W i ( m )的配置可以参考文献[7]设置。
4)量测更新
令 x ˙ k , 0= x ˙ -,Pk,0= P k -,且 x ˙ k , 1= x ˙ k,Pk,1=Pk。迭代次数j=4。
产生新的采样点:
χ k , j ( i )=[ x ˙ k , j - 1 x ˙ k , j - 1± ( n + λ ) P k , j - 1]
量测更新:
x ˙ k , j -= ∑ i = 0 2 n W i ( m ) χ k , j ( i )
Z k , j ( i )=h( χ k , j ( i ))
Z ˙ k , j -= ∑ i = 0 2 n W i ( m ) Z k , j ( i )
Pzz,k,j= ∑ i = 0 2 n W i ( c )[ Z k , j ( i )- Z ˙ k , j -][ Z k , j ( i )- Z ˙ k , j -]T+Rk
Pxz,k,j= ∑ i = 0 2 n W i ( c )[ χ k , j ( i )- x ˙ k , j -][ Z k , j ( i )- Z ˙ k , j -]T
Kk,j=Pxz,k,j P z z , k , j - 1
x ˙ k , j= x ˙ k , j -+Kk,j(Zk- Z ˙ k , j -)
Pk,j=Pk,j-1-Kk,jPzz,k,jKk,j
5)确定迭代条件
对于滤波估计问题,测量更新的目的就是利用已有的测量信息估计到准确的状态量和协方差矩阵。考虑到独立的随机向量,服从正态分布,即:
x ˙ j~N(x,Pj),z~N(h(x),R)
其中,x和 x ˙分别是j时刻的估计值和真值,z是观测值,h(x)是测量的函数。
式(19)可以构成一个新的变量:
y= z x ˙ j,g(x)= h ( x ) x
由式(19)和式(20)可得
y~N(g(x),S),S= R 0 0 P j
那么y的似然函数为
L(y)= 1 ( 2 π ) ( n + m ) | S |·exp - 1 2 ( y - g ( x ) ) T S - 1 ( y - g ( x ) )
n和m分别为状态向量和量测向量的维数,那么对状态y的极大似然估计为
y*=arg max[L(y)]
比较式(22),也就是求
y*=arg min[q(x)]
其中
q(x)= 1 2(z-g(x))TS-1(z-g(x))
当
q(xj+1)<q(xj)
成立时,可以看到q(xj)时呈收敛趋势,也就是q(xj+1)比q(xj)更加接近最优解。
由式(24)、(25)可得
[y-h(xj+1)]TR-1[y-h(xj+1)]+( x ˙ j-xj+1)T P j - 1 ( x ˙ j-xj+1)<[y-h(xj)]TR-1[y-h(xj)]+( x ˙ j-xj)T P j - 1 ( x ˙ j-xj)
于是可得
x ˙ T k , j P k , j - 1 - 1 x ˙ k , j+ z ˙ T k , j R-1zk,j< z ˙ T k , j - 1 R-1 z ˙ k , j - 1
其中:
z ˙ k , j=h( x ˙ k , j) x ˙= x ˙ k , j- x ˙ k , j - 1 z ˙ k , j=zk- z ˙ k , j
重新进入步骤4),完成量测更新。当j不满足这个条件时,进入下一步。
6)当5)的调节不满足时或者j>N时,跳出迭代过程,此时可以得到: x ˙= x ˙ k , j,Pk=Pk,j,完成迭代计算。

2.2 目标状态估计仿真流程

目标状态估计就是利用无人机自身的状态信息,融合:测向传感器的测量信息,对地面目标状态的估计。具体流程如下:
Step1:锁定目标图像,测量位于光轴中心位置的目标的测量角(ρ,φ),结合无人机位置,利用公式(5)得到量测量z(k-1)。
Step2:设定目标初始状态x0,定位的协方差矩阵初始值P0,利用式(6)、(7)、(8)获得目标的时间预测 x ˙ -和 P k -。
Step2.1:令 x ˙ k , 1= x ˙ k,Pk,1=Pk,按照量测更新步骤 x ˙ k , j和Pk,j,其中j为迭代次数。
Step2.2:按照式(27)、(28)确定迭代条件,当迭代条件不满足或者j大于给定的迭代条件时,跳出迭代,并令 x ˙= x ˙ k , j,Pk=Pk,j。
Step3:从Step1开始,开始下一次估计。

3 仿真结果及分析

为了验证算法的性能,进行N次蒙特卡罗实验。采用均方误差(Root Mean Square Error,RMSE)来表示 目标定位与跟踪的精度,定位为
RMSE= 1 N ∑ i = 1 N [ ( x ˙ k - x ) 2 ]
仿真实验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的目标定位均方误差图
视轴观测角误差是影响目标定位精度的主要因素,当观测误差较小,观测精度较高时,各算法都有较好的定位精度;观测精度较小误差比较大时,可以看到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所示。
图3 仿真情景2的目标定位均方误差图
从目标运动的跟踪定位图中可以看出,各种算法都可以很好地跟踪运动目标;在观测精度高和观测精度低的情况下,IUKF都能很好地跟踪运动目标,并且算法的收敛速度和收敛精度都明显好于前两种算法,能够达到快速稳定准确跟踪运动目标的效果。

4 结束语

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

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
Chen J Y. UAV-guided navigation for ground robot tele-operation in a military reconnaissance environment[J]. Ergonomics, 2010, 53(8):940-50.

DOI

[2]
Tisdale J, Ryan A, Zu K, et al. A multiple UAV system for vision-based search and localization[C]// American Control Conference. IEEE, 2008:1985-1990.

[3]
徐诚, 黄大庆, 孔繁锵. 一种小型无人机无源目标定位方法及精度分析[J]. 仪器仪表学报, 2015, 36(5):1115-1122.

[4]
Lin F, Dong X, Chen B M, et al. A Robust Real-Time Embedded Vision System on an Unmanned Rotorcraft for Ground Target Following[J]. IEEE Transactions on Industrial Electronics, 2013, 59(2):1038-1049.

DOI

[5]
郭福成, 李宗华, 孙仲康. 无源定位跟踪中修正协方差扩展卡尔曼滤波算法[J]. 电子与信息学报, 2004, 26(6):917-922.

[6]
Julier S, Uhlmann J, Durrantwhyte H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Trans Automatic Control, 2001, 45(3):477-482.

DOI

[7]
Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3):401-422

DOI

[8]
Menegaz H M T, Ishihara J Y, Borges G A, et al. A Systematization of the Unscented Kalman Filter Theory[J]. IEEE Transactions on Automatic Control, 2015, 60(10):2583-2598.

DOI

[9]
Sibley G, Sukhatme G, Matthies L. The Iterated Sigma Point Kalman Filter with Applications to Long Range Stereo[J]. Robotics Science & Systems, 2006.

[10]
Bell B M, Cathey F W. The iterated Kalman filter update as a Gauss-Newton method[J]. Automatic Control IEEE Transactions on, 1993, 38(2):294-297.

DOI

[11]
都基焱, 段连飞, 黄国满. 无人机电视侦察目标定位原理[M]. 合肥: 中国科学技术大学出版社, 2013.

[12]
Zhan R, Wan J. Iterated Unscented Kalman Filter for Passive Target Tracking[J]. Aerospace & Electronic Systems IEEE Transactions on, 2007, 43(3):1155-1163.

[13]
Chang G, Xu T, Wang Q. Alternative framework for the iterated unscented Kalman filter[J]. Iet Signal Processing, 2017, 11(3):258-264.

DOI

Options
文章导航

/

〈 〉