一种基于导引头框架信息的地面目标实时定位方法与流程

1.本发明涉及定位领域，尤其是涉及一种基于导引头框架信息的地面目标实时定位方法。


背景技术：

2.地面目标实时定位技术利用未知目标的辐射源信号，采用信号处理技术对信号源进行定位，是导航、制导定位中一项重要技术手段，在无人机侦察与监视方面有广泛的应用前景。在进行地面目标实时定位工作时，导引头首先在每个历元测量地面目标在视线坐标系的方位以及俯仰框架角信息，然后利用上述方位以及俯仰框架角信息，采用几何交会和卡尔曼滤波方法可以实时估计地面目标位置。
3.目前已有的基于导引头框架角信息的地面目标实时定位方法主要有基于扩展卡尔曼滤波的地面目标定位法，但是，上述地面目标定位方法由于未估计测量参数的误差导致目标定位精度难以提高。


技术实现要素：

4.本发明提供了一种基于导引头框架信息的地面目标实时定位方法，以解决拓展卡尔曼滤波中测量方程需要较为精确的初值进行线性化的问题以及框架角测量的系统性误差导致目标定位精度难以提高的问题。
5.根据本发明的第一方面，提供了一种基于导引头框架信息的地面目标实时定位方法，该方法包括：步骤s11，以地面目标位置以及导引头框架角误差信息作为状态参数，建立系统状态模型，并在所述系统状态模型中设定所述状态参数的初值及其方差阵；步骤s12，对所述系统状态模型中的状态参数进行时间更新，并获取到更新后的状态参数及其方差阵；步骤s13，基于导引头的实时测量信息建立测量模型；步骤s14，基于无迹卡尔曼滤波算法、所述测量模型针对所述更新后状态参数及其方差阵进行估计处理，得到估计后的地面目标位置、框架角误差信息及其方差阵；步骤s15，将估计后的地面目标位置、框架角误差信息及其方差阵赋予所述步骤s11中的系统状态模型；循环执行所述步骤s12至步骤s15的方法。
6.进一步地，系统状态模型表示为：
7.x(k)＝x(k-1) ξ(k)；
8.x＝[x y z δφ δθ δψ]；
[0009]
[x y z]为所述地面目标位置；[δφ δθ δψ]为导引头测量俯仰角、滚转角和方位角误差；k为当前时刻；ξ为状态参数的时间更新噪声。
[0010]
进一步地，对所述系统状态模型中的状态参数的方差阵按照如下方式进行时间更新：
[0011]
p(k)＝p(k-1)，其中，
[0012]
按照如下方式在所述系统状态模型中设定所述状态参数的初值及其方差阵：
[0013][0014]
其中，x
t
(0)为地面目标位置初值；xd(0)为框架角测量误差初值；s为导引头最远视距；为导引头的位置；为导引头体坐标系到北东地导航坐标系的转换矩阵；为北东地导航坐标系到地心地固坐标系的转换矩阵；ub为地面目标位置到导引头相对位置矢量在导引头体坐标系的表示；ψ为导引头测量的方位框架角；θ为导引头测量的俯仰框架角。
[0015]
进一步地，按照如下方式建立所述测量模型：
[0016]
步骤s41，按照如下公式得到地面目标位置到导引头相对位置的单位矢量：
[0017]
其中，x
to
为地面目标位置在地心地固坐标系的坐标ue；
[0018]
表示导引头到地面目标相对位置矢量的长度；
[0019]
步骤s42，基于考虑导引头框架角测量误差，按照如下公式将地心地固坐标系下地面目标位置到导引头相对位置的单位矢量转换到导引头坐标系：
[0020]
其中，为地心地固坐标系到北东地导航坐标系的转换矩阵，由导引头的位置确定，由导引头安装的惯导计算姿态矩阵确定；δc为导引头测量框架角误差确定的姿态修正矩阵；
[0021]
步骤s43，按照如下公式，通过地面目标位置到导引头相对位置的单位矢量来表达导引头：
[0022]
其中，u
b1
、u
b2
和u
b3
表示ub的三个分量，其中，所述步骤s41至步骤s43中的公式构成测量模型，所述测量模型中包括地面目标位置以及框架角误差参数。
[0023]
进一步地，按照如下公式将估计后的框架角误差信息及其方差阵赋予所述步骤s11中的系统状态模型：
[0024]
[0025]
进一步地，在执行所述步骤s12至步骤s15之后，所述方法还包括：
[0026]
在估计后的导引头框架角测量信息中的俯仰框架角超过预设俯仰框架角或者所述估计后导引头框架角测量信息中的方位框架角超过预设方位框架角的情况下，将估计后的地面目标位置进行输出。
[0027]
根据本发明的第二方面，提供了一种计算机设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述计算机程序在由所述处理器执行时导致上述任一项所述的方法被执行。
[0028]
根据本发明的第三方面，提供了一种非暂时性计算机可读存储介质，其上存储有计算机程序，所述计算机程序在由处理器执行时导致上述任一项所述的方法被执行。
附图说明
[0029]
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0030]
图1为本发明实施例的基于导引头框架信息的地面目标实时定位方法的流程图；
[0031]
图2为本发明实施例的可选的基于导引头框架信息的地面目标实时定位方法的流程图；
[0032]
图3为根据本发明实施例的效果的示意图；
[0033]
图4为根据本发明实施例的效果的示意图。
具体实施方式
[0034]
为了使本发明的上述以及其他特征和优点更加清楚，下面结合附图进一步描述本发明。应当理解，本文给出的具体实施例是出于向本领域技术人员解释的目的，仅是示例性的，而非限制性的。
[0035]
在以下描述中，阐述了许多具体细节以提供对本发明的透彻理解。然而，对于本领域普通技术人员来说，明显的是，不需要采用具体细节来实践本发明。在其他情况下，未详细描述众所周知的步骤或操作，以避免模糊本发明。
[0036]
实施例一
[0037]
本实施例提供了一种基于导引头框架信息的地面目标实时定位方法，如图1所示，该方法包括：
[0038]
步骤s11，以地面目标位置以及导引头框架角误差信息作为状态参数，建立系统状态模型，并在所述系统状态模型中设定所述状态参数的初值及其方差阵。
[0039]
具体的，在本方案中，可以由上位机、服务器等具有处理数据功能的硬件设备作为本技术方法的执行主体，本方案可以在无人机上搭载导引头，通过无人机与上位机的信息交互，从而实现地面目标的定位。本方案可以针对上述系统状态模型给定状态参数初始值及其方差阵，然后依据建立的系统状态模型进行时间更新，还需要说明的是，由于状态参数是根据时间离散变化的，因此时间更新即为状态更新。
[0040]
步骤s12，对所述系统状态模型中的状态参数进行时间更新，并获取到更新后的状
态参数及其方差阵。
[0041]
步骤s13，基于导引头的实时测量信息建立测量模型。
[0042]
具体的，上述导引头实时测量信息可以为实际测量的俯仰框架角信息、方位框架角信息。本方案可以根据实际测量的俯仰框架角信息、方位框架角信息、导引头体坐标系到北东地坐标系的转换矩阵信息，建立以地面目标位置和框架角测量误差为参数的测量模型，并给定测量角度的方差阵。
[0043]
步骤s14，基于无迹卡尔曼滤波算法、所述测量模型针对所述更新后状态参数及其方差阵进行估计处理，得到估计后的地面目标位置、框架角误差信息及其方差阵。
[0044]
步骤s15，将估计后的地面目标位置、框架角误差信息及其方差阵赋予所述步骤s11中的系统状态模型。
[0045]
步骤s16，循环执行步骤s12至步骤s15。
[0046]
具体的，在步骤s15得到估计后的地面目标位置、框架角误差信息及其方差阵之后，本实施例可以继续循环执行所述的方法。即本方案在估计得到地面目标位置、框架角误差信息及其方差阵之后，将上述地面目标位置、框架角误差信息及其方差阵赋予上述建立的系统状态模型的初值，然后继续循环执行步骤s12至步骤s15，通过上述迭代计算，可以使得估计得到的地面目标位置、框架角误差信息及其方差阵越来越精确。
[0047]
需要说明的是，通过上述多个步骤，本技术提出了一种在系统状态模型中增设框架角误差参数并利用无迹卡尔曼滤波进行目标位置和框架角误差同时估计的方法。首先建立以目标位置和框架角误差为状态的系统模型，然后利用导引头框架角测量信息建立测量模型，采用无迹卡尔曼滤波算法对系统模型和非线性测量模型中的位置、框架角误差参数进行实时估计，从而克服框架角测量系统误差导致目标定位精度不高以及测量方程需要较为精确的初值进行线性化的问题，实现了地面目标的实时高精度定位的技术效果，有效地提高了目标定位精度和实时性。
[0048]
可选的，以地面目标位置和框架角误差为状态参数，考虑到地面目标位置、导引头框架角系统误差不变，离散的系统模型可以表示为：
[0049]
x(k)＝x(k-1) ξ(k)；
[0050]
x＝[x y z δφ δθ δψ]；
[0051]
[x y z]为所述地面目标位置；单位为米，地心地固坐标系，[δφ δθ δψ]为导引头测量俯仰角、滚转角和方位角误差，单位为弧度；k为当前时刻；ξ为状态参数的时间更新噪声，可取零。
[0052]
可选的，对所述系统状态模型中的状态参数的方差阵按照如下方式进行时间更新：
[0053]
p(k)＝p(k-1)其中，
[0054]
按照如下方式在所述系统状态模型中设定所述状态参数的初值及其方差阵：
[0055][0056]
其中，x
t
(0)为地面目标位置初值；xd(0)为框架角测量误差初值；s为导引头最远视距，单位为米；为导引头的位置，根据卫星定位确定；为导引头体坐标系到北东地导航坐标系的转换矩阵，根据导引头安装的冠导确定；为北东地导航坐标系到地心地固坐标系的转换矩阵，根据导引头位置确定；ub为地面目标位置到导引头相对位置矢量在导引头体坐标系的表示，根据导引头测量的框架角确定；ψ为导引头测量的方位框架角，单位为rad；θ为导引头测量的俯仰框架角，单位为rad。
[0057]
可选的，按照如下方式建立所述测量模型：
[0058]
步骤s41，按照如下公式得到地面目标位置到导引头相对位置的单位矢量：
[0059]
其中，x
to
为本方案设的地面目标位置在地心地固坐标系的坐标；
[0060]
表示导引头到地面目标相对位置矢量的长度。
[0061]
步骤s42，基于考虑导引头框架角测量误差，按照如下公式将地心地固坐标系下地面目标位置到导引头相对位置的单位矢量转换到导引头坐标系：
[0062]
其中，为地心地固坐标系到北东地导航坐标系的转换矩阵，由导引头的位置确定，由导引头安装的惯导计算姿态矩阵确定；δc为导引头测量框架角误差确定的姿态修正矩阵。
[0063]
步骤s43，按照如下公式，通过地面目标位置到导引头相对位置的单位矢量来表达框架角：
[0064]
其中，u
b1
、u
b2
和u
b3
表示ub的三个分量，其中，所述步骤s41至步骤s43中的公式构成测量模型，所述测量模型中包括地面目标位置以及框架角误差参数，ψ和θ定义如上。
[0065]
可选的，按照如下公式将估计后的框架角误差信息及其方差阵赋予所述步骤s11
中的系统状态模型：
[0066][0067]
具体的，本方案可以采用标准的无迹卡尔曼滤波算法对系统状态模型和测量模型进行计算，实时得到地面目标位置和框架角误差参数及其方差阵
[0068]
可选的，在执行所述步骤s12至步骤s15之后，所述方法还包括：
[0069]
经过多次的循环计算，在估计后的导引头框架角测量信息中的俯仰框架角超过预设俯仰框架角或者所述估计后导引头框架角测量信息中的方位框架角超过预设方位框架角的情况下，本方案则不再进行循环计算，而是将估计后的地面目标位置进行输出。
[0070]
本技术提供了一种基于导引头框架信息的地面目标实时定位方法，该方法可以应用于地面目标位置的实时定位，下面结合图2，介绍本技术的方法一种可选的实施例：
[0071]
步骤s301，设定地面目标初值、框架角误差参数初值及其方差阵。
[0072]
具体的，以某次导引头搜索确定地面目标试验为例，以泡沫机作为该试验的飞行平台，在飞行原点[0 0 0]处放置一静态目标作为需搜索定位的目标，在飞机距离目标视线距离约1km的位置，锁定目标。根据上述系统状态模型的初始值公式，由飞机当前的位置、姿态与导引头的框架角等状态信息，粗略设定系统状态参数初值为：
[0073]
x(0)＝[202.7 17.2 10 0.05 0.05 0.05]
[0074]
p(0)＝diag[20543.6 1479.2 50 2.5e-3 2.5e-3 2.5e-3
]
[0075]
步骤s302，根据系统模型进行状态参数时间更新。
[0076]
具体的，以同次试验为例，在锁定上目标之后，导引头对目标进行持续跟踪，在时刻t＝17.4s，根据上述离散系统模型公式和系统模型的状态参数对应的方差阵公式，此时状态参数为：
[0077]
x(t)＝[-48.05 8.15
ꢀ‑
0.48 0.003
ꢀ‑
0.005 0.001]
[0078][0079]
步骤s303，根据导引头框架角测量信息建立测量模型。
[0080]
具体的，在该时刻，导引头框架角的测量值(单位为rad)为：[θ(t)ψ(t)]
t
＝[-0.365 0.1]
t
，25根据上述地面目标到导引头相对位置的单位矢量公式，可得目标到导引头相对位置的单位矢量为：
[0081]
ue(t)＝[0.954 0.0524 0.294]
[0082]
根据上述ub为导引头到地面相对位置单位矢量公式以及导引头测量框架角误差确定的姿态修正矩阵公式，可得导引头框架角误差确定的姿态修正矩阵δc(t)为：
[0083][0084]
通过上述公式可以计算出导引头到地面相对位置单位矢量ub(t)为:ub(t)＝[0.926 0.129 0.354]，由上述测量模型的公式，可得出导引头框架角的测量估值(单位为rad)为：
[0085][0086]
步骤s304，根据无迹卡尔曼滤波进行状态参数估计。
[0087]
具体的，由上述所得的t时刻的状态参数，以及导引头框架角的测量值与估计值，根据无迹卡尔曼滤波计算方法，得到该时刻的状态参数估计为：
[0088][0089][0090]
步骤s305，更新地面目标初值及其方差阵。
[0091]
具体的，本方案可以将上述所得到的t时刻的状态参数，作为下一时刻的状态参数初值，继续代入下一时刻计算，该导引头框架角的测量更新周期为0.02s，因此该计算的迭代周期也设为0.02s，直到某一时刻tn达到如下截至条件：
[0092]
θ(tn)＞θ
max
或ψ(tn)＞ψ
max
[0093]
此时，停止计算，也即导引头框架角的测量值，到达了导引头框架的极限，这时输出的也即最终状态量的估值。
[0094]
结合图3至图4介绍本技术的技术效果，图3中，横轴表示北东地坐标，纵轴表示定位误差，单位为m。图4中，横轴表示北东地坐标；纵轴表示定位精度，单位为m。从图3以及图4可以看出，采用本发明方法，在导引头框架角测量25s以后，地面目标定位误差为8.8m，地面目标定位精度为9.7m，需要说明的是，北东地为北东地坐标系“ned(north east down)坐标系”。
[0095]
综上，本技术通过上述方案，至少解决了拓展卡尔曼滤波中测量方程需要较为精确的初值进行线性化的问题以及框架角测量的系统性误差导致目标定位精度难以提高的问题。
[0096]
应理解，本文中前述关于本发明的方法所描述的具体特征、操作和细节也可类似地应用于本发明的装置和系统，或者，反之亦然。另外，上文描述的本发明的方法的每个步骤可由本发明的装置或系统的相应部件或单元执行。
[0097]
应理解，本发明的装置的各个模块/单元可全部或部分地通过软件、硬件、固件或
其组合来实现。各模块/单元各自可以硬件或固件形式内嵌于计算机设备的处理器中或独立于处理器，也可以软件形式存储于计算机设备的存储器中以供处理器调用来执行各模块/单元的操作。各模块/单元各自可以实现为独立的部件或模块，或者两个或更多个模块/单元可实现为单个部件或模块。
[0098]
在一个实施例中，提供了一种计算机设备，其包括存储器和处理器，存储器上存储有可由处理器执行的计算机指令，计算机指令在由处理器执行时指示处理器执行本发明的实施例的方法的各步骤。该计算机设备可以广义地为服务器、终端，或任何其他具有必要的计算和/或处理能力的电子设备。在一个实施例中，该计算机设备可包括通过系统总线连接的处理器、存储器、网络接口、通信接口等。该计算机设备的处理器可用于提供必要的计算、处理和/或控制能力。该计算机设备的存储器可包括非易失性存储介质和内存储器。该非易失性存储介质中或上可存储有操作系统、计算机程序等。该内存储器可为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口和通信接口可用于与外部的设备通过网络连接和通信。该计算机程序被处理器执行时执行本发明的方法的步骤。
[0099]
本发明可以实现为一种计算机可读存储介质，其上存储有计算机程序，计算机程序在由处理器执行时导致本发明实施例的方法的步骤被执行。在一个实施例中，计算机程序被分布在网络耦合的多个计算机设备或处理器上，以使得计算机程序由一个或多个计算机设备或处理器以分布式方式存储、访问和执行。单个方法步骤/操作，或者两个或更多个方法步骤/操作，可以由单个计算机设备或处理器或由两个或更多个计算机设备或处理器执行。一个或多个方法步骤/操作可以由一个或多个计算机设备或处理器执行，并且一个或多个其他方法步骤/操作可以由一个或多个其他计算机设备或处理器执行。一个或多个计算机设备或处理器可以执行单个方法步骤/操作，或执行两个或更多个方法步骤/操作。
[0100]
本领域普通技术人员可以理解，本发明的方法步骤可以通过计算机程序来指示相关的硬件如计算机设备或处理器完成，计算机程序可存储于非暂时性计算机可读存储介质中，该计算机程序被执行时导致本发明的步骤被执行。根据情况，本文中对存储器、存储、数据库或其它介质的任何引用可包括非易失性和/或易失性存储器。非易失性存储器的示例包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)、闪存、磁带、软盘、磁光数据存储装置、光学数据存储装置、硬盘、固态盘等。易失性存储器的示例包括随机存取存储器(ram)、外部高速缓冲存储器等。
[0101]
以上描述的各技术特征可以任意地组合。尽管未对这些技术特征的所有可能组合进行描述，但这些技术特征的任何组合都应当被认为由本说明书涵盖，只要这样的组合不存在矛盾。
[0102]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。