基于迭代无迹卡尔曼滤波的小型无人机目标定位方法
唐大全,柳向阳,邓伟栋,丁鹏程
(海军航空大学,山东 烟台 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算法的收敛速度和跟踪精度。
RS-CPE技术是在传统浊点萃取技术基础上建立的无需水浴加热过程的常温快速CPE技术。实验操作程序如下：准确移取待测元素标准工作液置于10 mL具塞离心管中，然后加入适量络合剂，充分摇荡后调节酸度，加入萃取剂Triton X-114和浊点诱导剂，用纯水定容至10 mL，振摇1 min。充分摇匀后，以3 000 r∕min的转速离心5 min。待水相和有机相完全分离后，弃去水相，用注射器小心抽取有机相10 μL注入非水相CVG反应器进行测定。
1 移动目标定位的原理
小型无人机的目标定位系统一般包括GPS定位系统、惯性测量单元和光电成像平台等。光电成像平台上一般有稳定伺服机构、摄像机云台和摄像机等部件。由于小型无人机功率载荷有限,激光测距仪等部件一般不能安装在无人机上。无人机定位时,将目标锁定在成像平面的视场中心,通过测量光心相对于目标的高低角和方位角,结合无人机的位姿信息,解算出目标位置。能够精准地对无人机定位和测量无人机与目标的相对角度是解决这类问题的关键。
1.1 视线方位角的计算
比较式(22),也就是求
视场中心点在摄像机坐标系下为t p (0,0,f ),f 为摄像机焦距。设无人机的偏航角ψ 、俯仰角θ 、横滚角γ 和摄像机坐标系下光轴的方位ρ 和高低角φ ,可以得到无人机地理坐标系下的坐标为
(1)
为摄像机坐标c 系到载体坐标b 系的旋转矩阵,由光轴的方位ρ 和高低角
为载体坐标系到地理坐标系的旋转矩阵,由无人机的偏航角ψ 、俯仰角θ 、横滚角γ 决定。根据式(1),可得视线方位角(α ,β )为
(2)
图1 目标定位示意图
1.2 状态方程的建立
无人机对目标进行定位,需要围绕目标飞行一段距离,测量各时刻目标相对于无人机的角度信息,根据角度信息建立相应的量测方程,估计目标的位置。
假设无人机绕飞行过程中,对目标进行了M 次观察,第k 次观测目标位置
无人机位置(p x ,p y ,p z ),假设目标做匀速直线运动,那么目标的状态方程可以表示为
x k+1 =Φ k+1,k x k +w k
(3)
为系统转移矩阵,Δt 为采样时间,w k 为系统噪声矩阵,系统的量测方程为
z k =h (x k )+v k
(4)
w k ～N (0,Q k )
上式中,在系统方程(3)和量测方程(4)中,w k ,v k 同时满足:
Q k ,R k 分别为系统噪声序列的方差阵和量测噪声序列的方差阵,假定都是正定矩阵。量测方程为
(5)
2 目标定位的迭代无迹卡尔曼滤波算法
标准的UKF算法受数值计算舍入误差、可观测性和观测噪声大等因素的影响产生不稳定、收敛速度慢的问题。迭代无迹卡尔曼滤波是对无迹卡尔曼滤波的改进,能够克服无迹卡尔曼滤波收敛速度慢和收敛精度不高的缺点[12-13]。
2.1 迭代UKF的原理步骤
1)滤波初始化
给定初始状态、状态协方差阵初始化值
假定系统过程噪声和量测噪声分别为Q k ,R k ,噪声特性如上节所示。
最后，推广先进的栽培技术。针对长期水稻栽培过程中普遍存在无效分蘖量增多、水肥运用不合理、病虫害发病严重和普遍出现倒伏等问题，应该在示范创建过程中，积极推广使用高效的绿色安全生产技术，并进一步对技术路线进行优化，实现水稻生产的全机械化操作，推广应用配方施肥技术和病虫害统防统治技术。进一步加快绿色增产增效生产技术推广应用，不断提高示范区内的种植效益和示范效果。
2)计算sigama样本点集
(6)
3)时间更新
从图5可以看出，高水分小麦干燥过程中含水率变化曲线在干燥初期呈直线下降，在干燥时间为45～135min范围内变化曲线呈匀速下降趋势，而在干燥结束阶段曲线趋于平缓。高水分小麦薄层厚度在3.5～6.5cm范围内，薄层厚度越大，含水率变化曲线的凹凸性越小，其含水率的变化趋势也越趋于平缓。当物料干燥至安全储存含水率时，薄层厚度为3.5，5，6.5cm的物料所需的干燥时间分别为210，270，300min。由此可知当高水分小麦薄层厚度增大，干燥至安全储存含水率的时间也随之增加，但厚度的增加量与干燥时间的变化量之间并不成相同倍数增加的关系。
(7)
由式(19)和式(20)可得
(8)
权重系数
的配置可以参考文献[7]设置。
4)量测更新
ⅡB型RAD-seq技术通过ⅡB型限制内切酶实现基因组DNA的切割,此种酶可在基因组DNA上序列标记位点上端与下端将DNA切开,并得到相同长度的DNA片段.此技术无需了解基因组信息,就可以完成文库建立,对于标记密度同样不难调整,且费用少.Wang等[62]在拟南芥上对此技术进行了检验,结果显示，ⅡB型RAD-seq技术测序准确度高,通过对标记适度微调,可用于连锁图谱和自然群体的遗传变异图谱的建立.
监护室综合征(ICU综合征)是指患者在ICU监护期间出现的以精神障碍为主、兼具其他一系列表现，如:谵妄状态、思维紊乱、情感障碍和行为和动作异常等的一组临床综合症［1］。也称之为ICU精神障碍、ICU谵妄等。目前，国内相关文献报道，其发生率为20% ～40%［2-3］。它的出现，既可能是疾病本身的临床表现，也可能是疾病发展变化的先兆症状，导致延迟康复，使住院天数的延长和治疗费用增加［4］。为防止ICU综合征的发生，国内外学者近年来对其进行了诸多研究。现就ICU综合征的基本概念、临床表现、发生的相关因素及护理进展综述如下。
令
且
迭代次数j =4。
产生新的采样点:
3.猪。表现严重的消化不良（呕吐、腹泻），体重降低，心肌衰弱，呼吸困难，黏膜发绀。步态不稳或跛行，间或出现阵性痉挛发作，可突然死亡。
(9)
目标状态估计就是利用无人机自身的状态信息,融合:测向传感器的测量信息,对地面目标状态的估计。具体流程如下:
(10)
(11)
(12)
(13)
对于滤波估计问题,测量更新的目的就是利用已有的测量信息估计到准确的状态量和协方差矩阵。考虑到独立的随机向量,服从正态分布,即:
(14)
(15)
(16)
P k,j =P k,j -1-K k,j P zz,k ,j K k,j
(17)
5)确定迭代条件
(18)
其中,x 和
分别是j 时刻的估计值和真值,z 是观测值,h (x )是测量的函数。
式(19)可以构成一个新的变量:
(19)
式中,R测试为薄膜实际测得的电阻值,R设计为薄膜的设计电阻值,偏差反应了薄膜实际的电阻值与设计电阻值的差距,偏差值越小说明制备的薄膜电阻的精度越高[6]。NiCr薄膜电阻的设计方阻值为100 Ω/□,测试不同线宽和方阻数的电阻值,并计算得出相应的电阻偏差值,如表1所示。
(20)
那么y 的似然函数为
(21)
n 和m 分别为状态向量和量测向量的维数,那么对状态y 的极大似然估计为
y *=arg max[L (y )]
(22)
无人机目标定位就是基于摄影测量、图像处理和信息处理等技术,通过对无人机侦察以及与图像处理相关的遥测数据的处理与分析,提取目标精确三维坐标的过程[11]。在目标定位过程中,首先图像传感器采集图像,光电成像平台稳定伺服系统驱动CCD摄像机光轴对准目标,使目标位于成像靶面中心。定义视线方位角为在无人机地理坐标系中,采用东北天坐标系,视线高低角α 为视线与无人机Z 轴正向(向上)之间的夹角,视线水平角β 为视线在水平面的投影与x 轴正向(东向)之间的夹角,如图1所示,利用测角传感器测量这些角度值。在无人机的定位过程中,摄像机的光轴指向与无人机的姿态角共同决定了目标视线方位角。
(2)从时间演变来看，成都平原城市群县域经济实力存在排名变化，整体变化相对较小.上升型县域主要分布在成都市区周围，下降型县域主要分布于经济实力相对薄弱的研究区外围，实力相对稳定的县(区)占到研究区的51.7%，空间分布比较分散.
y *=arg min[q (x )]
(23)
其中
研究发现，在低温贮藏条件下，添加甘油处理组的蓝莓失重率增加，硬度降低，不利于蓝莓的保藏；增塑剂的添加对蓝莓中Vc的含量基本上没有影响；与其它组相比，添加山梨醇的HTCC涂膜液可以降低蓝莓的失重率和呼吸强度，减缓花青素和TSS的变化，具有显著性差异（p<0.05）。因此，2%山梨醇可以提高HTCC涂膜液的保鲜效果，为研发蓝莓保鲜涂膜液提供新的依据。
(24)
当
当我信步来到了一栋教学楼前时，透过玻璃门，我看见大厅里一侧的桌子旁坐着一位年轻的保安，当时他手里拿着一本厚厚的文学书籍，正在旁若无人地阅读着，显得温文尔雅。
当前保护环境的一项重要措施就是环境影响评价，但是公众参与程度却不够，主要原因有以下几点：首先，公众对环境影响评价缺少积极性，大多数人认为环境影响评价和自己没有关系，因此不愿意参与，即使有一些人参与也只是形式上的，没有实际作用；其次，公众参与机制却还不够完善。相关部门的关注不足，对公众参与也不重视，没有加强宣传，也没有具体落实公众的知情权，因此，公众的监督作用很难得到发挥；最后，环境影响评价的方式比较单一，采取的形式基本上是选取代表人进行民意调查，这样得出的调查结果并不具有较高的科学价值。
q (x j+1 )<q (x j )
(25)
成立时,可以看到q (x j )时呈收敛趋势,也就是q (x j+1 )比q (x j )更加接近最优解。
由式(24)、(25)可得
[y -h (x j+1 )]TR -1[y -h (x j+1 )]+
[y -h (x j )]TR -1[y -h (x j )]+
(26)
于是可得
(27)
重新进入步骤4),完成量测更新。当j 不满足这个条件时,进入下一步。
(28)
其中:
6)当5)的调节不满足时或者j >N 时,跳出迭代过程,此时可以得到:
完成迭代计算。
2.2 目标状态估计仿真流程
量测更新:
Step1:锁定目标图像,测量位于光轴中心位置的目标的测量角(ρ ,φ ),结合无人机位置,利用公式(5)得到量测量z (k -1)。
Step2:设定目标初始状态x 0,定位的协方差矩阵初始值P 0,利用式(6)、(7)、(8)获得目标的时间预测
和
Step2.1:令
按照量测更新步骤
和P k,j ,其中j 为迭代次数。
Step2.2:按照式(27)、(28)确定迭代条件,当迭代条件不满足或者j 大于给定的迭代条件时,跳出迭代,并令
Step3:从Step1开始,开始下一次估计。
3 仿真结果及分析
为了验证算法的性能,进行N 次蒙特卡罗实验。采用均方误差(Root Mean Square Error,RMSE)来表示 目标定位与跟踪的精度,定位为
(29)
仿真实验1:无人机做盘旋上升飞行,目标静止。设无人机飞行半径100 m,圆心坐标为(500,600,400),初始航高z =400 m,盘旋上升飞行速度v z =1 m/s,运动角速度为0.02 rad/s,无人机自身定位服从均值为零,方差为10的正态分布;目标真实坐标为(350,450)。此时目标初始状态x 0=[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,盘旋上升飞行速度v z =1 m/s,运动角速度为0.09 rad/s,无人机自身定位服从均值为零,方差为10的正态分布;目标初始起点为(350,450),以v =[3.9, 3.9]的速度运动,即x 0=[330, 480, 0, 3.9, 3.9, 0]。无人机视轴角误差为1°,记为精度1,目标定位的均方误差如图3a所示,当无人机视轴角误差为0.1°,记为精度2,目标定位的均方误差如图3b所示。
合作型学习风格者喜欢和同学一起进行小组或全班活动，在共同学习过程中更容易记住所学内容，而小组活动等则对学习者学习和理解新知识起到促进作用。
从目标运动的跟踪定位图中可以看出,各种算法都可以很好地跟踪运动目标;在观测精度高和观测精度低的情况下,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.
[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.
[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.
[7] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3):401-422
[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.
[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.
[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.
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
(Naval Aviation University, Yantai 264001, China)
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
中图分类号 :E911；V279
文献标志码: A
DOI: 10.3969/j.issn.1673-3819.2019.01.021
文章编号 ：1673-3819(2019)01-0104-05
收稿日期 ：2018-06-02
修回日期： 2018-06-28
作者简介 ：
唐大全(1965-),男,贵州都匀人,教授,研究方向为飞行器导航与控制,自动测试与故障诊断。
柳向阳(1989-),男,硕士研究生,助理工程师。