基于多导弹协同目标探测的空间配准算法

龙源期刊网基于多导弹协同目标探测的空间配准算法作者：谢宇婷周军卢晓东来源：《航空兵器》2018年第02期摘要：在多导弹协同目标跟踪技术中，通过将多传感器的观测信息进行融合产生目标的全局航迹。理想情况下，各传感器的偏差认为是零，然而实际中由于偏差的存在导致融合效果变差，需要通过空间配准对误差进行估计和补偿。本文主要针对两类误差：传感器量测（系统）误差和姿态（定向）误差，提出一种改进的基于地心地固坐标系的卡尔曼滤波（ECEF-KF）方法。首先建立了系统误差和姿态误差模型；其次将弹上传感器的量测转换至公共坐标系（ECEFsystem），隔绝了导弹自身的运动；然后构造关于状态的线性伪量测并通过卡尔曼滤波（KF）对各偏差进行估计。仿真结果表明，该算法可准确估计各偏差量的大小，并通过误差补偿极大提高目标跟踪的精度。关键词：目标跟踪；空间配准；传感器；误差；卡尔曼滤波；协同制导中图分类号：TJ765文献标识码：A文章编号：1673-5048（2018）02-0009-070引言空间配准是多传感器量测信息融合的前提[1]，指的是通过对公共目标的量测来估计和补偿传感器误差。这一领域中的主流算法是基于合作目标的空间配准。依据系统偏差模型的维数区分，存在基于球（极）投影的二维系统偏差估计算法和基于地心地固坐标（EarthCenteredEarthFixedCoordinate，ECEF）的三维系统偏差估计算法[2]。许多经典的二维算法已经在各类文献中提出，例如，实时质量控制（RTQC）法、最小二乘（LS）法、广义最小二乘（GLS）法、精确极大似然估计（EML）法[3]。与三维算法相比，二维算法的计算量小但精度低。传统的基于ECEF三维算法虽然能够弥补上述方法的不足，但只考虑传感器的量测误差而忽略了其自身的定向偏差和量测噪声。与此同时，一些三维雷达在线配准算法已经被提出。文献[4]提出一种基于卡尔曼滤波的方法，用于估计和校准多传感器的姿态误差，但忽略了量测偏差的影响。文献[5]同时考虑了传感器量测偏差和姿态偏差，但只适用于姿态角很小且传感器彼此相距很近的情况。文献[6]提出一种基于网格搜索偏差估计器的卡尔曼滤波方法。文献[7]展示了一种基于扩展卡尔曼平滑器的期望最大化（EM-EKS）方法。文献[8]展示了一种基于状态值以及无迹卡尔曼滤波（UKF）和标准卡尔曼滤波（KF）构成的联合滤波器的空间偏差的改进算法。上述方法大都是针对静基座传感器，因此，有待于提出一种考虑高速移动导弹上姿态和位置不断变化的传感器的空间配准算法。在多导弹协同目标探测的背景下，应用了一种改进的基于地心地固坐标系的卡尔曼滤波（ECEF-KF）算法。该算法将对公共目标的量测龙源期刊网收稿日期：2018-01-24基金项目：中央高校基本科研业务费专项资金项目（3102016ZB021）作者简介：谢宇婷（1994-），女，陕西西安人，硕士研究生，研究方向为飞行器导航与探测。引用格式：谢宇婷，周军，卢晓东.基于多导弹协同目标探测的空间配准算法[J].航空兵器，2018（2）：9-15.XieYuting，ZhouJun，LuXiaodong.SpatialRegistrationAlgorithmBasedonMultiMissilesCooperativeTargetDetection[J].AeroWeaponry，2018（2）：9-15.（inChinese）转换到ECEF坐标系下，隔绝了弹体的运动，并且利用两弹上传感器的量测差值来同时估计系统（量测）偏差和姿态角误差。算法基于以下假设：（1）系统偏差认为是小量且是常值。（2）姿态偏差是由陀螺误差引起的随时间缓慢变化的小量。（3）忽略各个误差的耦合。（4）不考虑传感器的位置误差。这可以理解为由于弹上其他辅助测量装置（例如GPS）、传感器的位置是精确已知的。1坐标系的定义和转换1.1各个坐标系的定义在多传感器空间配准中，每个传感器都有其自身参考框架或坐标系。其中，传感器的量测在本体系中表述，传感器的姿态角在平台地理坐标系中表述，传感器和目标的位置在ECEF坐标系中表述。这些坐标系的定义如图1所示。图1坐标系Fig.1Coordinatesystem1.1.1ECEF坐标系OXeYeZe龙源期刊网坐标系[9]中所定义的，原点位于地球质心，Ze轴指向协议地球极（ConventionalTerrestrialPole，CTP）的方向，Xe轴指向零子午面与CTP赤道的交点，Ye轴与Xe，Ze垂直，构成右手坐标系。1.1.2平台地理坐标系OXdYdZd原点位于平台（导弹）质心在地球表面的投影点，Xd，Yd和Zd分别指向该点的正东、正北和天向。1.1.3本体坐标系OXbYbZb原点位于平台（导弹）质心。Xb轴与弹体纵轴重合，指向头部为正，Yb轴与弹体竖轴重合，指向天为正，Zb由右手坐标系规则确定。1.2各个参考系间的转换关系航空兵器2018年第2期谢宇婷，等：基于多导弹协同目标探测的空间配准算法1.2.1ECEF坐标系和平台地理坐标系令向量re=xeyezeT，rd=xdydzdT分别表示ECEF和平台地理系下的坐标。后者与前者的转换关系可以表示为re=Cedrd（1）这里Ced是3×3正交矩阵，即Ced=-sinλ-sinφcosλcosφcosλcosλ-sinφsinλcosφsinλ0cosφsinφ（2）式中：λ和φ表示经度和纬度。如果传感器的经度、纬度和高度h已知，则其ECEF坐标可以通过如下公式计算得到：xe=（C+h）cosφcosλye=（C+h）cosφsinλze=（C（1-e2）+h）sinφC=Re/1-e2sin2φ（3）龙源期刊网其中：Re为赤道半径；e表示地球偏心率。1.2.2平台地理坐标系和本体坐标系令向量rb=xbybzbT表示本体系下坐标，同样地，从本体系转换至平台地理系的转换关系可以用矩阵Cdb表示，这一矩阵也叫做导弹的姿态矩阵：Cdb=cosθcosψsinθ-cosθsinψ-sinθcosψcosγ+sinψsinγcosθcosγsinθsinψcosγ+cosψsinγsinθsinγcosψ+cosγsinψ-cosθsinγ-sinθsinψsinγ+cosγcosψ（4）其中：ψ，θ和γ分别表示导弹的偏航、俯仰和滚转角。2数学模型建立状态向量由两弹上传感器的误差项组成：X=ξkδkT（5）其中：k表示第k枚导弹，k=1，2；ξk=ΔrkΔqγkΔqλkT表示第k枚导弹的量测固定偏差，包含斜距、视线高低角和视线偏角误差；δk=ΔψkΔθkΔγkT表示第k枚导弹的姿态偏差，包含天向、北向和东向姿态误差角。最终，所有误差项构成了12维状态向量，状态方程可以表示为X·=FX+W（6）其中：过程噪声W是一个零均值高斯白噪声向量。量测向量Z可以表示为状态量的线性函数：Z=HX+V（7）其中：量测噪声V被近似认为是零均值高斯白噪声向量。下面，首先对各项误差模型进行分析，然后再对状态方程和量测方程的具体形式提出讨论。2.1量测误差和姿态误差模型传感器的量测误差被认为是在理想测量的基础上增加的一个可加性常值：龙源期刊网～=r+Δrq～γ=qγ+Δqγq～λ=qλ+Δqλ（8）其中：上标～表示含有偏差的实际测量。这里暂时忽略量测噪声的影响，在后面构建量测方程时会考虑进来。对目标的量测如图2所示。目标在本体系下的坐标可以表示为xb=rcosqγcosqλyb=rsinqγzb=-rcosqγsinqλ（9）将式（8）代入式（9）：图2对目标的量测Fig.2Measurementoftargetxb=r～-Δrcosq～γ-Δqγcosq～λ-Δqλyb=r～-Δrsinq～γ-Δqγzb=-r～-Δrcosq～γ-Δqγsinq～λ-Δqλ（10）根据假设，量测偏差是小量，式（10）可以展开为关于误差项的一阶线性形式：Xb=xbybzb=X～b+Jξ（11）其中：系数矩阵J是式（10）关于误差项ξ的雅克比矩阵，表示为J=-cosΔqγcosΔqλrsinΔqγcosΔqλrcosΔqγsinΔqλ龙源期刊网λ（12）根据假设，量测误差是固定量，这里有ξ·=0（13）文献[10]给出了标准姿态误差模型，在不考虑速度和位置误差影响以及误差角相互耦合的情况下给出其简单形式：Δψ·=εuΔθ·=εnΔγ·=εe（14）其中：εu，εn，εe表示陀螺天向、北向和东向测量误差，这些误差被认为是标准差为σu，σn，σe的零均值高斯白噪声。根据假设，姿态误差角均为小量，则含有误差的姿态矩阵可以表示为C～db=（I-）Cdb（15）=0-ΔψΔθΔψ0-Δγ-ΔθΔγ0（16）2.2量测和状态方程目标在ECEF坐标系下的坐标可以分别由目标在两导弹的本体坐标系下的坐标转换而来：Xet=Ced2Cd2b2Xb2+Xe2（17）Xet=Ced1Cd1b1Xb1+Xe1（18）其中：Xe1，Xe2分别表示两弹的ECEF坐标。由式（17）～（18）可得Xb1=Cd1b1TCed1TCed2Cd2b2Xb2+ΔXe2-1（19）龙源期刊网由式（15）可知Cd1b1=（I-1）TC～d1b1Cd2b2=（I-2）TC～d2b2（20）将式（20）代入式（19），忽略1和2的乘积：Xb1=C～b1d1（Cd1eCed2-1Cd1eCed2-Cd1eCed2T2）C～d2b2Xb2+C～b1d1（I-1）Cd1eΔXe2-1（21）式（21）两边同乘Ced1C～d1b1，得Ced1C～d1b1Xb1=（Ced2-Ced11Ced1Ced2-Ced2T2）C～b2d2Xb2+Ced1（I-1）Cd1eΔXe2-1（22）将Xb1和Xb2展成式（11）所示形式，因为Δ和Δξ均为小量，所以忽略他们的乘积项，式（22）变成Ced1C～d1b1X～b1-Ced2C～d2b2X～b2+ΔXe1-2=Ced2C～d2b2J2ξ2-Ced1C～d1b1J1ξ1-（Ced11Cd1e+Ced2T2Cd2e）Ced2C～d2b2X～b2（23）式中，等号左边的项即是构造的伪量测，记为Z。令H1=-Ced1C～d1b1J1，H2=Ced2C～d2b2J2，并将等号右边最后一项记为P，式（23）变成Z=H1ξ1+H2ξ2+P（24）将P展开成为姿态误差δk的线性形式，表示为P=-（A1AT+BT2BT）M（25）其中：龙源期刊网=Ced1=a1a2a3B=Ced2=b1b2b3M=BC～d2b2X～b2（26）式中，ai和bi分别是A和B的列向量，这里不加推导地给出如下公式：-A1ATM=-A-a2a10a30-a10-a3a2TMMMΔψ1Δθ1Δγ1（27）同理-BT2BTM=B2BTM=B-b2b10b30-b10-b3b2TMMMΔψ2Δθ2Δγ2（28）在式（27）和（28）中，令龙源期刊网=-A-a2a10a30-a10-a3a2TMMM（29）H4=B-b2b10b30-b10-b3b2TMMM（30）将式（27）～（30）代入式（25），最终将P展开为P=H3Δψ1Δθ1Δγ1+H4Δψ2Δθ2Δγ2=H3δ1+H4δ2（31）将式（31）代入式（24），最终获得量测量和状态量的线性关系：Z=H1H2H3H4X+V（32）其中：X=ξ1ξ2δ1δ2T是一个12维误差向量。考虑到量测噪声的存在，在式（32）