CN108732533A - 单辐射源机动飞行目标自定位及自跟踪方法 - Google Patents

Abstract

本发明提供单辐射源机动飞行目标自定位及自跟踪方法，属于飞行器定位与导航技术领域。本发明首先实时接收从机动飞行目标至单一参考辐射源的往返时间，得到实时的伪距信息；然后，对机动飞行目标的测高信息进行获取，若可获取则采用相对运动时间积累自定位伪测量获取算法求出机动飞行目标的伪测量，若无法获取，则采用两圆面相交自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；再将机动飞行目标的伪测量进行交互式多模型滤波；重复直至跟踪完成，最后形成机动飞行目标的运动航迹。本发明解决了现有技术无法实现单辐射源机动飞行目标自定位自跟踪的问题。本发明可用于飞行器定位与跟踪。

Description

单辐射源机动飞行目标自定位及自跟踪方法

技术领域

本发明涉及机动飞行目标自定位及自跟踪方法，属于飞行器定位与导航技术领域。

背景技术

机动飞行目标的自定位及自跟踪涉及多种作战单位，在国防建设上起着不可替代的作用，同时无人机的发展使得其在民用方面也展现出它的潜在应用与发展。在存在多个参考辐射源的环境下，可采用多辐射源TDOA(Time Difference of Arrival到达时间差)定位算法实现定位，可是，考虑到现实情况如果环境较为恶劣，存在的参考辐射源个数过少，极致情况下只有一个，传统的定位算法就无法实现对机动飞行目标实现自定位功能，同时也无法实现对该目标的后续的自跟踪过程。

利用多辐射源距离信息实现对目标的TDOA定位算法的相关研究在国内外的公开发表的内容有很多，但是现有技术还没有能够实现，在仅有一个参考辐射源的情况下对机动飞行目标自定位和跟踪的方法。

发明内容

本发明为解决现有技术无法实现单辐射源机动飞行目标自定位自跟踪的问题，提供了单辐射源机动飞行目标自定位及自跟踪方法。

本发明所述单辐射源机动飞行目标自定位及自跟踪方法，通过以下技术方案实现：

步骤一、机动飞行目标实时接收从机动飞行目标至单一参考辐射源的往返时间，得到实时的伪距信息；

步骤二、对机动飞行目标的测高信息进行获取，若可获取机动飞行目标的测高信息，则采用相对运动时间积累自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；若机动飞行目标的测高信息无法获取，则采用两圆面相交自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；

所述相对运动时间积累自定位伪测量获取算法的具体过程包括：

步骤A1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系；

步骤A2、降维处理：将机动飞行目标为参考系的三维空间XYZ的伪距和测高信息投影到二维平面XY面上，对应的辐射源到机动飞行目标的径矢也作对应投影；

步骤A3、进行时间积累，选取相隔积累时间t的两个伪距组成伪距对；

步骤A4、针对步骤A3中的伪距对，利用空间三角形余弦定理求出机动飞行目标的水平角信息，如式(1)所示：

其中，θ₁表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与X轴正半轴的夹角，θ₂表示当前时刻辐射源投影到机动飞行目标投影的矢量与X轴正半轴的夹角，θ₃表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与当前时刻辐射源投影到机动飞行目标投影的矢量之间的夹角，D11表示时间积累后的辐射源投影与当前时刻辐射源投影间的距离，R11表示机动飞行目标投影与当前时刻辐射源投影间的距离，R21表示机动飞行目标与时间积累后的辐射源投影间的距离；

步骤A5、根据空间几何关系求出空间内机动飞行目标的伪测量，计算如式(2)所示：

其中，x表示伪测量在三维空间XYZ中的X轴坐标，y表示伪测量在三维空间XYZ中的Y轴坐标，z表示伪测量在三维空间XYZ中的Z轴坐标，h为测高信息；

所述两圆面相交自定位伪测量获取算法具体过程包括：

步骤B1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系；进行两个时间积累，得到当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标；

步骤B2、点距配对；步骤B1得到的当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标中，每两个坐标和其对应伪距信息构成一个点距对，每一个点距对形成一个空间曲线圆，任意选取其中两个空间曲线圆；

步骤B3、如果选取的两空间曲线圆相交则求其交点作为机动飞行目标的伪测量；如果选取的两空间曲线圆不相交，则判断两空间曲线圆所在平面的交线与两空间曲线圆是否相交，若相交则选取交点作为候选的机动飞行目标的伪测量，若不相交，则选取两空间曲线圆上到该交线最近的点作为候选的机动飞行目标的伪测量；并将选择的点放入机动飞行目标的伪测量候选集中；

步骤B4、如果机动飞行目标的伪测量候选集中仅存在一个元素，则该元素为机动飞行目标的伪测量，如果存在多个元素，则求其重心作为该机动飞行目标的伪测量；

步骤三、对步骤二中求得的机动飞行目标的伪测量进行交互式多模型滤波；

步骤四、重复步骤一至步骤三，直至跟踪完成，形成机动飞行目标的运动航迹。

进一步的，步骤三中所述交互式多模型滤波的具体过程包括：

步骤三一、输入交互：

设交互式多模型由n个子模型构成，把各个子模型的初始状态进行交互，得到交互后的状态估计值和交互后的估计协方差：

其中，为机动飞行目标在k时刻第j个模型的状态估计值，为机动飞行目标在k时刻第j个模型的估计协方差，j＝1,2…n；为机动飞行目标在k时刻第j个模型的交互后的状态估计值，为机动飞行目标在k时刻第j个模型的交互后的估计协方差；μ_ij(k+1/k)表示预测模型的转移概率，i＝1,2…n；

步骤三二、状态滤波：将交互后的状态估计值和交互后的估计协方差加入到各个子模型的输入端进行滤波和状态估计，使机动飞行目标的状态估计值和估计协方差得到更新，并得到各个子模型的新息和新息协方差；

步骤三三、模型概率更新：将各个子模型的新息和新息协方差，利用各模型的转移概率进行进一步更新和修正；

步骤三四、状态融合输出：以更新后的模型概率μ_j(k)作为评判该模型与实际运动轨迹吻合度的评判标准，对当前各个时刻子模型的状态估计值和对应的估计协方差进行进一步的加权求和得到融合后的估计状态，完成状态的融合加权输出。

进一步的，步骤三一中所述子模型为卡尔曼滤波模型。

本发明最为突出的特点和显著的有益效果是：

本发明所涉及单辐射源机动飞行目标自定位及自跟踪方法，由相对运动时间积累自定位伪测量获取算法、两圆面相交自定位伪测量获取算法和交互式多模型算法组成。相对运动时间积累自定位伪测量获取算法、两圆面相交自定位伪测量获取算法用于对机动飞行目标的自定位即伪测量的获取，结合交互式多模型算法用于对机动飞行目标实现自跟踪过程，本发明可对仅存在一个参考辐射源环境下的机动飞行目标进行自定位和自跟踪，从而实现对目标的监控和导航。本发明解决了在恶劣环境下对机动目标的自定位和跟踪的问题，保证哪怕在恶劣环境下也能实现对目标的实时监控跟踪。仿真实验中采用本发明方法对机动飞行目标的自定位和自跟踪，误差能够保持在150M以内；提高了对机动飞行目标运动的广泛适应性，进一步提高了跟踪精度。

附图说明

图1是本发明方法流程图；

图2是本发明相对运动时间积累自定位伪测量获取算法三维空间原理图；

图3是本发明相对运动时间积累自定位伪测量获取算法二维空间原理图；

图4是本发明两圆面相交自定位伪测量获取算法原理图；

图5是本发明交互式多模型算法原理图；

图6是本实施例的仿真结果图。

具体实施方式

具体实施方式一：结合图1、图2、图3、图4对本实施方式进行说明，本实施方式给出的单辐射源机动飞行目标自定位及自跟踪方法，具体包括以下步骤：

步骤一、机动飞行目标实时接收从机动飞行目标至单一辐射源的往返时间，从而得到实时的伪距信息；

步骤二、对机动飞行目标的测高信息进行获取，若可获取机动飞行目标的测高信息，则采用相对运动时间积累自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；若机动飞行目标的测高信息无法获取，则采用两圆面相交自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位。

相对运动时间积累自定位伪测量获取算法可在已知机动飞行目标测高信息后对目标实现自定位，由于仅存在一个参考辐射源，若想获取多个伪距信息，需要将目标固定，让参考辐射源作相对运动，为了确定目标位置信息，需要将三维空间伪距信息投影到XY平面上形成二维空间伪距信息，再由时间积累获取两个参考的伪距，利用三角形几何关系求得角度信息，再根据三维空间坐标的求解关系求得机动飞行目标的伪测量，达到自定位目的。

所述相对运动时间积累自定位伪测量获取算法的具体过程包括：

步骤A1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系；相对运动体现在原来的运动是辐射源静止、机动飞行目标运动，变换以后，机动飞行目标静止、辐射源在运动；如图2所示，其中，O为当前时刻辐射源坐标，O1为时间积累后的辐射源坐标，S1为机动飞行目标当前时刻坐标，R1机动飞行目标与当前时刻辐射源间的距离，R2′为机动飞行目标与时间积累后的辐射源间的距离，D1′时间积累后的辐射源与当前时刻辐射源间的距离。

步骤A2、降维处理：将机动飞行目标为参考系的三维空间XYZ的伪距和测高信息投影到二维平面XY面上，对应的辐射源到机动飞行目标的径矢(径向量)也作对应投影，将三维空间问题降到二维空间来处理；如图3所示，其中，O为当前时刻辐射源投影坐标，O11为时间积累后的辐射源投影坐标，S11为机动飞行目标投影坐标。

步骤A3、进行时间积累，选取相隔积累时间t的两个伪距组成伪距对；积累时间t需要适当拉长从而减小伪距误差带来的影响；

步骤A4、针对步骤A3中的伪距对，利用空间三角形余弦定理求出机动飞行目标的水平角信息，如式(1)所示：

其中，θ₁表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与X轴正半轴的夹角，θ₂表示当前时刻辐射源投影到机动飞行目标投影的矢量与X轴正半轴的夹角，θ₃表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与当前时刻辐射源投影到机动飞行目标投影的矢量之间的夹角，D11表示时间积累后的辐射源投影与当前时刻辐射源投影间的距离，R11表示机动飞行目标投影与当前时刻辐射源投影间的距离，R21表示机动飞行目标与时间积累后的辐射源投影间的距离；

步骤A5、根据空间几何关系求出空间内机动飞行目标的伪测量，计算如式(2)所示：

其中，x表示伪测量在三维空间XYZ中的X轴坐标，y表示伪测量在三维空间XYZ中的Y轴坐标，z表示伪测量在三维空间XYZ中的Z轴坐标，h为测高信息。

当目标的测高信息未知时，此时需要引入两圆面相交自定位伪测量获取算法来对已有算法进行改进；

如图4所示，所述两圆面相交自定位伪测量获取算法具体过程包括：

步骤B1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系，相对运动体现在原来的运动是参考辐射源静止、目标运动，变换以后，目标静止、参考辐射源在运动；进行两个时间积累，得到当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标，这三个参考辐射源位置；图4中，O为当前时刻辐射源坐标，O1为时间积累后的辐射源坐标，O2为两个时间积累后的辐射源坐标，S1为机动飞行目标当前时刻坐标，R1机动飞行目标与当前时刻辐射源间的距离，R2′为机动飞行目标与时间积累后的辐射源间的距离，R3′为机动飞行目标与两个时间积累后的辐射源间的距离，D1′时间积累后的辐射源坐标与当前时刻辐射源坐标间的距离，D2′两个时间积累后的辐射源坐标与一个时间积累后的辐射源坐标间的距离。

步骤B2、点距配对：步骤B1得到的当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标中，每两个参考辐射源和其对应伪距信息构成一个点距对，每一个点距对会形成一个空间曲线圆，任意选取其中两个空间曲线圆；

步骤B3、如果选取的两空间曲线圆相交则求其交点作为候选的机动飞行目标的伪测量；如果选取的两空间曲线圆不相交，则判断两空间曲线圆所在平面的交线与两空间曲线圆是否相交，若相交则选取交点作为候选的机动飞行目标的伪测量，若不相交(实际由于噪声的原因，这种情况很少出现)，则选取两空间曲线圆上到该交线最近的点作为候选的机动飞行目标的伪测量；并将选择的点放入机动飞行目标的伪测量候选集中；

步骤B4、如果机动飞行目标的伪测量候选集中仅存在一个元素，则该元素为机动飞行目标的伪测量，如果存在多个元素，则求其重心作为该机动飞行目标的伪测量；

步骤三、对步骤二中求得的机动飞行目标的伪测量Z_k进行交互式多模型滤波；

步骤四、重复步骤一至步骤三，直至跟踪完成，形成机动飞行目标的运动航迹。

其他步骤及参数与具体实施方式一相同。

具体实施方式二：如图5所示，本实施方式与具体实施方式一不同的是，步骤三中所述交互式多模型滤波的具体过程包括：

交互式多模型(Interacting Multiple Model，简称IMM)算法假设多个滤波模型来对机动目标进行全面的描述，输入交互步骤是在滤波过程中通过计算模型概率及模型转移概率来完成，交互后的数据就是IMM的模型输入，通过模型概率估计器来对模型概率进行更新，最后通过交互作用器来对不同的模型进行交互，得到多个模型状态和对应协方差的融合值，估计混合器后将结果输出。

步骤三一、输入交互：

设交互式多模型由n个子模型构成，把各个子模型的初始状态进行交互，从而得到交互后的状态估计值和交互后的估计协方差：

其中，为机动飞行目标在k时刻第j个模型的状态估计值，为机动飞行目标在k时刻第j个模型的估计协方差，j＝1,2…n；为机动飞行目标在k时刻第j个模型的交互后的状态估计值，为机动飞行目标在k时刻第j个模型的交互后的估计协方差；μ_j(k)表征机动飞行目标在k时刻第j个模型的模型概率，如果上一时刻占主导地位的模型为第i个模型，下一时刻占主导地位的模型为第j个模型，那么，我们引入π_ij表示由第i个模型到第j个模型的模型转移概率，我们通常认为模型间的转化符合马尔科夫过程，通过π_ij和Λ_j(k)能够得到μ_ij(k+1/k)，μ_ij(k+1/k)表示预测模型的转移概率，i＝1,2…n；

步骤三二、状态滤波：将交互后的状态估计值和交互后的估计协方差加入到各个子模型的输入端进行滤波和状态估计，使机动飞行目标的状态估计值和估计协方差得到更新，将得到的和Λ_j(k)(k时刻第j个模型的匹配似然函数)；并得到各个子模型的新息和新息协方差；

步骤三三、模型概率更新：将步骤三二获得的各个子模型的新息和新息协方差，利用各模型的转移概率μ_ij(k+1/k)进行进一步更新和修正；

步骤三四、状态融合输出：以更新后的模型概率μ_j(k)作为评判该模型与实际运动轨迹吻合度的评判标准，对当前各个时刻子模型的状态估计值和对应的估计协方差进行进一步的加权求和得到融合后的估计状态，完成状态的融合加权输出。

其他步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式二不同的是，步骤三一中所述子模型为卡尔曼滤波模型。

其他步骤及参数与具体实施方式二相同。

实施例

采用以下实施例验证本发明的有益效果：

本实施例所述单辐射源机动飞行目标自定位及自跟踪方法实验参数设定如下：

采样时间间隔：1s；

采样点数：65个；

机动目标飞行速度：800km/h；

径向距离测量噪声：15m；

高度信息测量噪声：20m；

本实施例按如下步骤进行：

步骤一、机动飞行目标实时接收从机动飞行目标至单一参考辐射源的往返时间，得到实时的伪距信息；

步骤二、对机动飞行目标的测高信息进行获取，若可获取机动飞行目标的测高信息，则采用相对运动时间积累自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；若机动飞行目标的测高信息无法获取，此时需要引入两圆面相交自定位伪测量获取算法来对已有算法进行改进；则采用两圆面相交自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；

步骤三、对步骤二中求得的机动飞行目标的伪测量进行交互式多模型滤波；

步骤四、重复步骤一至步骤三，直至跟踪完成，形成机动飞行目标的运动航迹。

如图6是本实施例的仿真结果图，图中的折线为已知测高信息下采用本发明方法进行的机动飞行目标自定位和自跟踪仿真的误差结果，误差能够保持在150M以内。可以看出，本发明提出的方法可以很好的对机动飞行目标进行自定位和跟踪处理，达到对该目标的实时监控和导航作用。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (3)

1.单辐射源机动飞行目标自定位及自跟踪方法，其特征在于，所述方法具体包括以下步骤：

步骤一、机动飞行目标实时接收从机动飞行目标至单一参考辐射源的往返时间，得到实时的伪距信息；

步骤二、对机动飞行目标的测高信息进行获取，若可获取机动飞行目标的测高信息，则采用相对运动时间积累自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；若机动飞行目标的测高信息无法获取，则采用两圆面相交自定位伪测量获取算法求出机动飞行目标的伪测量，实现机动飞行目标自定位；

所述相对运动时间积累自定位伪测量获取算法的具体过程包括：

步骤A1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系；

步骤A2、降维处理：将机动飞行目标为参考系的三维空间XYZ的伪距和测高信息投影到二维平面XY面上，对应的辐射源到机动飞行目标的径矢也作对应投影；

步骤A3、进行时间积累，选取相隔积累时间t的两个伪距组成伪距对；

步骤A4、针对步骤A3中的伪距对，利用空间三角形余弦定理求出机动飞行目标的水平角信息，如式(1)所示：

其中，θ₁表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与X轴正半轴的夹角，θ₂表示当前时刻辐射源投影到机动飞行目标投影的矢量与X轴正半轴的夹角，θ₃表示当前时刻辐射源投影到时间积累后的辐射源投影的矢量与当前时刻辐射源投影到机动飞行目标投影的矢量之间的夹角，D11表示时间积累后的辐射源投影与当前时刻辐射源投影间的距离，R11表示机动飞行目标投影与当前时刻辐射源投影间的距离，R21表示机动飞行目标与时间积累后的辐射源投影间的距离；

步骤A5、根据空间几何关系求出空间内机动飞行目标的伪测量，计算如式(2)所示：

其中，x表示伪测量在三维空间XYZ中的X轴坐标，y表示伪测量在三维空间XYZ中的Y轴坐标，z表示伪测量在三维空间XYZ中的Z轴坐标，h为测高信息；

所述两圆面相交自定位伪测量获取算法具体过程包括：

步骤B1、进行参考系变换，由以辐射源为参考系变换为以机动飞行目标为参考系；进行两个时间积累，得到当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标；

步骤B2、点距配对；步骤B1得到的当前时刻辐射源坐标、一个时间积累后的辐射源坐标、两个时间积累后的辐射源坐标中，每两个坐标和其对应伪距信息构成一个点距对，每一个点距对形成一个空间曲线圆，任意选取其中两个空间曲线圆；

步骤B3、如果选取的两空间曲线圆相交则求其交点作为机动飞行目标的伪测量；如果选取的两空间曲线圆不相交，则判断两空间曲线圆所在平面的交线与两空间曲线圆是否相交，若相交则选取交点作为候选的机动飞行目标的伪测量，若不相交，则选取两空间曲线圆上到该交线最近的点作为候选的机动飞行目标的伪测量；并将选择的点放入机动飞行目标的伪测量候选集中；

步骤B4、如果机动飞行目标的伪测量候选集中仅存在一个元素，则该元素为机动飞行目标的伪测量，如果存在多个元素，则求其重心作为该机动飞行目标的伪测量；

步骤三、对步骤二中求得的机动飞行目标的伪测量进行交互式多模型滤波；

步骤四、重复步骤一至步骤三，直至跟踪完成，形成机动飞行目标的运动航迹。

2.根据权利要求1所述单辐射源机动飞行目标自定位及自跟踪方法，其特征在于，步骤三中所述交互式多模型滤波的具体过程包括：

步骤三一、输入交互：

设交互式多模型由n个子模型构成，把各个子模型的初始状态进行交互，得到交互后的状态估计值和交互后的估计协方差：

其中，为机动飞行目标在k时刻第j个模型的状态估计值，为机动飞行目标在k时刻第j个模型的估计协方差，j＝1,2…n；为机动飞行目标在k时刻第j个模型的交互后的状态估计值，为机动飞行目标在k时刻第j个模型的交互后的估计协方差；μ_ij(k+1/k)表示预测模型的转移概率，i＝1,2…n；

步骤三二、状态滤波：将交互后的状态估计值和交互后的估计协方差加入到各个子模型的输入端进行滤波和状态估计，使机动飞行目标的状态估计值和估计协方差得到更新，并得到各个子模型的新息和新息协方差；

步骤三三、模型概率更新：将各个子模型的新息和新息协方差，利用各模型的转移概率进行进一步更新和修正；

步骤三四、状态融合输出：以更新后的模型概率μ_j(k)作为评判该模型与实际运动轨迹吻合度的评判标准，对当前各个时刻子模型的状态估计值和对应的估计协方差进行进一步的加权求和得到融合后的估计状态，完成状态的融合加权输出。

3.根据权利要求2所述单辐射源机动飞行目标自定位及自跟踪方法，其特征在于，步骤三一中所述子模型为卡尔曼滤波模型。