一种适用于无人机空中防撞的入侵机航迹融合跟踪方法与流程

1.本发明涉及无人机飞行技术，尤其涉及一种适用于无人机空中防撞的入侵机航迹融合跟踪方法。


背景技术：

2.近年来，随着国家智能制造的普及，以及人工智能、5g等新技术的不断发展，无人机行业也迎来了新的机遇。由于无人机运行方式与有人机系统不同，传统的空中防撞系统(traffic collision avoidance system，简称tcas)不能直接移植到无人机上，所以极需一套能够在无人机上运行，并兼容现有设备的感知与避撞系统。其中航迹跟踪是该系统的关键基础，能够探测到入侵机并持续有效正确的进行跟踪，获得入侵机的最优状态估计，是无人机进行避撞的重要前提条件。
3.现有的跟踪方法主要是卡尔曼滤波算法，卡尔曼滤波算法应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。
4.然而卡尔曼滤波适用于解决线性的高斯分布问题，而针对入侵机跟踪这类的目标运动方程或观测方程为非线性时，直接使用卡尔曼滤波算法并不能对入侵机的状态进行估计，因此需要一种能够对入侵机进行跟踪及状态估计的方法。


技术实现要素：

5.本发明提供一种适用于无人机空中防撞的入侵机航迹融合跟踪方法，通过确定入侵机的航迹，再通过扩展卡尔曼滤波算法基于入侵机的航迹确定出入侵机的预测测量值的方式，解决了现有技术不能够直接使用扩展卡尔曼滤波算法对入侵机的状态进行估计的问题，本发明的方案具有更高的适用性及跟踪效果，能够有效提高入侵机跟踪的准确性、鲁棒性和实用性。
6.一方面，本发明提供一种适用于无人机空中防撞的入侵机航迹融合跟踪方法，包括：
7.获取入侵机的第一传感器数据，所述第一传感器数据为所述无人机首次探测到的传感器数据；
8.基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头；
9.基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，所述第二传感器数据为所述无人机第二次探测到的传感器数据；
10.通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，所述第三传感器数据为已建立航迹的入侵机的传感器数据，所述预测测量值包括所述入侵机的预测坐标和速度。
11.可选地，所述状态信息包括俯仰角、横滚角、偏航角；
12.所述基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹
头，包括：
13.通过所述俯仰角、所述横滚角以及所述偏航角，确定出第一旋转矩阵；
14.基于所述第一传感器数据，计算出所述入侵机在本机站心坐标系下的第一坐标，所述本机站心坐标系为以无人机为站心的站心坐标系；
15.根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第一坐标；
16.基于所述经纬度所形成的第二旋转矩阵以及所述入侵机在以本机站心坐标系下的第一坐标，计算出在地心坐标系下的本机与所述入侵机之间的第一相对位置；
17.基于本机在地心坐标系下的坐标以及在地心坐标系下的本机与所述入侵机之间的第一相对位置，计算出所述入侵机在地心坐标系下的第一坐标；
18.将所述入侵机在地心坐标系下的第一坐标转换为所述入侵机的大地坐标，所述大地坐标包括wgs84坐标；
19.将所述入侵机的大地坐标确定为所述入侵机的航迹头。
20.可选地，所述基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，包括：
21.根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第二坐标；
22.基于所述本机在地心坐标系下的第二坐标以及所述入侵机在地心坐标系下的第一坐标，计算出上次入侵机在本次的本机站心坐标系下的第一坐标；
23.基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第一坐标，计算出所述入侵机在本机站心坐标系下的第二坐标；
24.基于所述第二传感器数据以及所述入侵机在本机站心坐标系下的第二坐标，确定出所述入侵机在地心坐标系下的第二坐标，并基于所述入侵机在地心坐标系下的第二坐标更新所述入侵机的大地坐标；
25.基于获取的所述第一传感器数据的测量时间与所述第二传感器数据的测量时间之间的测量时间差，计算出所述入侵机在本机站心坐标系下的第一速度；
26.基于所述入侵机在本机站心坐标系下的第一速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
27.可选地，所述基于所述入侵机在本机站心坐标系下的第一速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹，包括：
28.通过获取的入侵机经纬度及本机当前时刻的经纬度形成的第三旋转矩阵，将所述入侵机在本机站心坐标系下的第一速度转换为所述入侵机在入侵机站心坐标系下的速度，所述入侵机站心坐标系为以入侵机为站心的站心坐标系；
29.基于所述入侵机在入侵机站心坐标系下的速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
30.可选地，所述通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，包括：
31.将获取的所述入侵机在本机站心坐标下的预测位置经过第一旋转矩阵处理，得到
所述入侵机的本机机体坐标系下的预测位置；
32.计算在本机站心坐标系下所述入侵机与本机之间的相对速度；
33.基于所述相对速度、所述入侵机在本机站心坐标下的预测位置以及所述入侵机的本机机体坐标系下的预测位置，确定出所述入侵机的预测测量值。
34.可选地，在所述通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值之前，还包括：
35.当获取第三传感器数据时，根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第三坐标；
36.基于所述本机在地心坐标系下的第三坐标以及所述入侵机在地心坐标系下的第二坐标，计算出上次入侵机在本次的本机站心坐标系下的第二坐标；
37.基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第二坐标，计算出所述入侵机在本机站心坐标系下的第三坐标；
38.基于所述第三传感器数据以及所述入侵机在本机站心坐标系下的第三坐标，确定出所述入侵机在地心坐标系下的第三坐标，并基于所述入侵机在地心坐标系下的第三坐标更新所述入侵机的大地坐标；
39.基于所述第三旋转矩阵，将所述入侵机在入侵机站心坐标系下的速度转换为所述入侵机在本机站心坐标系下的第二速度；
40.基于所述入侵机在本机站心坐标系下的第二速度以及获取的所述入侵机在本机站心坐标系下的预测位置，确定出入侵机的预测状态矩阵。
41.可选地，在所述通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值之前，还包括：
42.通过所述第一传感器数据与获取的传感器标准差，确定出所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵；
43.基于所述第一旋转矩阵以及所述第一测量误差矩阵，计算出所述入侵机在本机站心坐标系下的第一测量误差矩阵；
44.根据获取的所述入侵机当前时刻的经纬度以及本机当前时刻的经纬度，将所述入侵机在本机站心坐标系下的第一测量误差矩阵通过预设矩阵进行旋转平移，确定出所述入侵机在本机站心坐标系下的第二测量误差，并基于所述第二测量误差生成状态误差矩阵；
45.基于所述第二传感器数据以及所述测量时间差，更新所述状态误差矩阵；
46.基于获取的所述入侵机上次更新时的经纬度及本机当前时刻的经纬度形成的第四旋转矩阵，将所述更新后的状态误差矩阵经过所述第四旋转矩阵处理，获取到所述入侵机在本机站心坐标下的估计状态矩阵；
47.基于所述估计状态矩阵以及获取的状态方程的误差矩阵，确定出所述入侵机的预测状态误差矩阵。
48.另一方面，本发明提供一种适用于无人机空中防撞的入侵机航迹融合跟踪装置，包括：
49.获取模块，用于获取入侵机的第一传感器数据，所述第一传感器数据为所述无人
机首次探测到的传感器数据；
50.确定模块，用于基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头；
51.建立模块，用于基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，所述第二传感器数据为所述无人机第二次探测到的传感器数据；
52.计算模块，用于通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，所述第三传感器数据为已建立航迹的入侵机的传感器数据，所述预测测量值包括所述入侵机的预测坐标和速度。
53.另一方面，本发明提供一种适用于无人机空中防撞的入侵机航迹融合跟踪设备，包括：存储器，处理器；
54.存储器，用于存储所述处理器可执行指令的存储器；
55.其中，所述处理器被配置为用于实现上述的入侵机跟踪方法。
56.另一方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现上述的入侵机跟踪方法。
57.另一方面，本发明提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述的入侵机跟踪方法。
58.本发明提供的一种适用于无人机空中防撞的入侵机航迹融合跟踪方法，基于本机的状态信息以及多次获取的传感器数据，通过确定入侵机的航迹头及航迹，再通过扩展卡尔曼滤波算法基于入侵机的航迹确定出入侵机的预测测量值，从而解决了现有技术不能够直接使用扩展卡尔曼滤波算法对入侵机的状态进行估计的问题，因此本发明的方案具有更高的适用性及跟踪效果，能够有效提高入侵机跟踪的跟踪效果、准确性、鲁棒性以及实用性。
附图说明
59.此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
60.图1为本发明实施例所提供的一种入侵机跟踪系统的架构示意图；
61.图2为本发明实施例所提供的一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的流程图；
62.图3为本发明实施例所提供的另一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的流程图；
63.图4为本发明实施例所提供的一种不同坐标系的效果示意图；
64.图5为本发明实施例所提供的一种入侵机跟踪装置的结构示意图；
65.图6为本发明实施例所提供的一种入侵机跟踪设备的框图。
66.通过上述附图，已示出本发明明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本发明构思的范围，而是通过参考特定实施例为
本领域技术人员说明本发明的概念。
具体实施方式
67.这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本发明的一些方面相一致的装置和方法的例子。
68.首先对本发明所涉及的名词进行解释：
69.卡尔曼滤波(kalman filtering，简称kf)：是指一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
70.状态估计：是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。
71.扩展卡尔曼滤波(extend kalman filtering，简称ekf)：是由卡尔曼滤波考虑时间非线性的动态系统，常应用于目标跟踪系统。扩展卡尔曼滤波也叫扩展卡尔曼滤波器，一种高效率的递归滤波器。
72.空中防撞系统(traffic collision avoidance system，tcas)：是安装于中、大型飞机的一组电脑系统，用以避免飞机在空中互相冲撞。
73.机体坐标系：是指固定在飞行器或者飞机上的遵循右手法则的三维正交直角坐标系，其原点位于飞行器的质心。机体坐标系的x轴位于飞行器参考平面内平行于机身轴线并指向飞行器前方，y轴垂直于飞行器参考面并指向飞行器右方，z轴在参考面内垂直于x
‑
y平面，指向航空器下方。
74.enu坐标系：也叫做站点坐标系、东
‑
北
‑
天坐标系，主要是用于需了解以观察者为中心的其他物体运动规律。站心坐标系以站心(如gps接收天线中心)为坐标系原点o，z轴与椭球法线重合，向上为正(天向)，y与椭球短半轴重合(北向)，x轴与地球椭球的长半轴重合(东向)所构成的直角坐标系，称为当地东北天坐标系(enu)。
75.ecef坐标系(earth
‑
centered,earth
‑
fixed，ecef)：是一种以地心为原点的地固坐标系(也称地球坐标系)，是一种笛卡儿坐标系。原点o(0,0,0)为地球质心，z轴与地轴平行指向北极点，x轴指向本初子午线与赤道的交点，y轴垂直于xoz平面(即东经90度与赤道的交点)构成右手坐标系。
76.wgs84坐标系(world geodetic system 1984)：是为gps全球定位系统使用而建立的坐标系统。
77.图1为本发明实施例所提供的一种入侵机跟踪系统的架构示意图，所述入侵机跟踪系统包括：采集模块1，建立模块2以及处理模块3。
78.其中，采集模块1用于获取入侵机的传感器数据。本发明实施例中，采集模块1具体用于多次采集入侵机的传感器数据，作为一种可能的实现方式，传感器数据可包括第一传感器数据、第二传感器数据以及第三传感器数据，其中，第一传感器数据为所述无人机首次探测到的传感器数据，所述第二传感器数据为所述无人机第二次探测到的传感器数据，第三传感器数据为已建立航迹的入侵机的传感器数据。需要说明的是，第三传感器数据并非
指的是第三次获取的入侵机的传感器数据，而指的是在建立入侵机航迹之后获取的传感器数据，可以是第三次获取的传感器数据、第四次获取的传感器数据或者第五次获取的传感器数据等等。
79.建立模块2用于基于采集模块1采集的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，所述第二传感器数据为所述无人机第二次探测到的传感器数据。
80.处理模块3用于通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、采集模块1采集的所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，所述预测测量值包括所述入侵机的预测坐标和速度。
81.本方案的入侵机跟踪系统与现有的跟踪系统区别在于：本方案通过设置建立模块2，通过建立模块2基于采集模块1采集的入侵机的第二传感器数据建立入侵机的航迹，再通过扩展卡尔曼滤波算法基于入侵机的航迹估计出入侵机的预测测量值，从而能够解决现有技术不能够直接使用扩展卡尔曼滤波算法对入侵机的状态进行估计的问题，从而有效的提高了入侵机跟踪的准确性、鲁棒性和实用性。
82.需要说明的是，当目标运动方程或观测方程为非线性时，是不能直接使用卡尔曼滤波计算目前的预测位置的，因此本发明采用的是扩展卡尔曼滤波，其中，是由卡尔曼滤波考虑时间非线性的动态系统，常应用于目标跟踪系统。而在扩展卡尔曼滤波对目标物体进行预测时，仍需要知晓物体自身的运动状态方程。然而对于汽车等物体其运动状态方程是已知的，例如以控制汽车为例，我们知道给多少油门就可以知道汽车对应往前移动多少，也就是说，汽车的状态方程是已知的。然而在本发明中，无人机所探测到的入侵机的状态是不可控的，也就是说入侵机的状态方程是未知的，因此为了解决入侵机状态方程未知的问题，本发明需要通过采集多次入侵机的传感器数据，以确定出入侵机的状态，即入侵机的状态方程，从而再通过扩展卡尔曼滤波进行计算，确定出入侵机的预测测量值。
83.下面以具体地实施例对本发明的技术方案以及本发明的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本发明的实施例进行描述。
84.图2为本发明实施例所提供的一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的流程图，如图2所示，该方法应用于无人机；
85.该方法包括：
86.s101、获取入侵机的第一传感器数据。
87.在该步骤中，所述第一传感器数据为所述无人机首次探测到的传感器数据，其中，传感器数据可包括斜距、方位角、仰角，斜距为入侵机与无人机之间的直线距离，方位角，又称地平经度(azimuth angle，简称az)，为入侵机与无人机之间的角度差。仰角为入侵机与无人机所在处的地平线之间的夹角。
88.需要说明的是，传感器数据除了包括斜距、方位角、仰角之外，还可以包括本机测量到入侵机的测量时间点、斜距、斜距变化率、斜距精度、斜距变化率精度、方位角精度、仰角精度等数据，本发明对此不做限定。
89.本发明实施例中，无人机能够通过本机的探测器探测到入侵机的传感器数据。
90.s102、基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹
头。
91.在该步骤中，状态信息包括俯仰角、横滚角、偏航角。
92.其中，偏航角又称真航向角，是飞机的纵轴与地球北极之间的夹角。其中，真航向角是磁航向角和磁偏角的代数和，取值范围是(
‑
π，π]。如果是相对于地面导航台，这个角就是飞机的实际航线，前进的方向和飞机的实际位置到导航台之间的连线这两条射线的夹角，没有参照物的话就是指飞机的前进方向和正北方向之间的夹角。
93.俯仰角为机体坐标系y轴与水平面的夹角。当机体坐标系的y轴在惯性坐标系xoy平面上方时，俯仰角为正，否则为负。俯仰角顾名思义，就是导弹或飞机相对于惯性坐标系的xoy平面“俯仰”的角度。
94.横滚角为机体坐标系xoz面与水平面的垂直平面的夹角。需要说明的是，以上概念中涉及的飞机是一种统称，针对本发明而言，上述的飞机即为本发明的无人机。
95.还需要说明的是，本机的状态信息除了包括俯仰角、横滚角、偏航角之外，还可以包括本机的处理时间点、经度、纬度、地球椭球面上高度、东西向速度、南北向速度、垂直速度、朝向、轴向加速度测量误差、水平加速度测量误差、垂向加速度误差等状态参数，本发明对此不做限定。
96.本发明实施例中，作为一种可能实现的方案，可基于无人机的俯仰角、横滚角、偏航角以及第一传感器数据中的斜距、方位角、仰角，能够确定出入侵机的大地坐标，并将该入侵机的大地坐标作为入侵机航迹的航迹头。其中，入侵机航迹的航迹头可理解为，第一次探测到入侵机时入侵机所在的位置点。此外，还可以根据无人机的其他状态信息以及第一传感器数据的其他信息计算出入侵机的大地坐标，本发明对此不作限定，上述仅为举例说明。
97.需要说明的是，步骤s102可理解为对入侵机航迹的初始化过程，除了初始化航迹之外，还可以包括初始化状态误差矩阵，初始化跟踪时间，初始化航迹号，具体过程可参见下一个实施例的描述。
98.s103、基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹。
99.在该步骤中，所述第二传感器数据为所述无人机第二次探测到的传感器数据。第二传感器数据所包含的数据类型与第一传感器数据相同，但具体数值可能不同。
100.本发明实施例中，由于步骤s102已对航迹进行了初始化处理，因此当无人机再次探测到入侵机的传感器数据时，能够基于该传感器数据(第二传感器数据)所述入侵机的航迹头，建立所述入侵机的航迹。需要说明的是，建立入侵机的航迹，可理解为确定入侵机的状态方程，以使后续步骤根据扩展卡尔曼滤波、该入侵机的状态方程以及获取的第三传感器数据，确定出入侵机的预测测量值。
101.具体地，建立所述入侵机的航迹的过程可包括：基于获取的入侵机的第二传感器数据确定出入侵机的另一个大地坐标，并基于入侵机的两个大地坐标确定出该入侵机的航迹。可理解为，基于该入侵机的两个位置点能够确定出该入侵机的航迹。需要说明的是，刚建立入侵机的航迹时，该入侵机的航迹目前仅包括入侵机的两个位置点，若后续再次探测到入侵机的传感器数据时，会基于新的传感器数据确定新的位置点，并基于新的位置点继续完善入侵机的航迹。
102.还需要说明的是，建立入侵机航迹，还包括更新入侵机在本机站心坐标系下的坐标，初始化入侵机的速度，更新入侵机的大地坐标，更新入侵机的状态误差矩阵以及跟踪时间，具体过程可参见下一个实施例的描述。需要说明的是，当执行建立入侵机航迹时，还需要用上第二传感器数据中的本机测量到入侵机的测量时间点，斜距，斜距变化率，方位角，仰角，斜距精度，斜距变化率精度，方位角精度，仰角精度等数据。例如，以更新跟踪时间为例，则此时第二传感器数据包括本机测量到入侵机的测量时间点，即需要根据本机测量到入侵机的测量时间点更新所述跟踪时间。
103.s104、通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值。
104.在该步骤中，所述第三传感器数据为已建立航迹的入侵机的传感器数据，所述预测测量值包括所述入侵机的预测坐标和速度。
105.本发明实施例中，当收到已建立过航迹的入侵机传感器数据后，通过扩展卡尔曼滤波算法实现入侵机的预测测量值的估计，以便在实际应用中基于入侵机的位置的最优估计确定无人机与入侵机是否会发生碰撞。
106.步骤s104可理解为更新入侵机的航迹的过程，即当无人机再次探测到入侵机的传感器数据时，会基于新的传感器数据确定新的位置点，并基于新的位置点继续完善入侵机的航迹。需要说明的是，除了更新入侵机的航迹之外，还包括计算滤波器增益矩阵，更新入侵机的大地坐标，计算入侵机的速度，更新入侵机的状态误差，更新跟踪时间，具体过程可参见下一个实施例的描述。
107.本发明提供的一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的实施例中，获取入侵机的第一传感器数据，基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头，基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，从而解决了现有技术不能够直接使用扩展卡尔曼滤波算法对入侵机的状态进行估计的问题，因此本发明的方案具有更高的适用性及跟踪效果，能够有效提高入侵机跟踪的跟踪效果、准确性、鲁棒性以及实用性。
108.图3为本发明实施例所提供的另一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的流程图，如图3所示，该方法应用于无人机；
109.该方法包括：
110.s201、获取入侵机的第一传感器数据。
111.s202、基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头。
112.在该步骤中，作为一种可选的方案，所述状态信息包括俯仰角、横滚角、偏航角、本机的处理时间点、经度、纬度、地球椭球面上高度、东西向速度、南北向速度、垂直速度、朝向、轴向加速度测量误差、水平加速度测量误差、垂向加速度误差等状态参数，所述第一传感器数据包括斜距、方位角、仰角、本机测量到入侵机的测量时间点，斜距，斜距变化率，方位角，仰角，斜距精度，斜距变化率精度，方位角精度，仰角精度等数据。此外，状态信息以及传感器数据还可以包括其他信息，本发明对此不做限定。
113.本发明实施例中，步骤s202可具体包括：
114.s2021、通过所述俯仰角、所述横滚角以及所述偏航角，确定出第一旋转矩阵。
115.本发明实施例中，所确定出的第一旋转矩阵如下所示：
[0116][0117]
其中，m(α,γ,η)表示为第一旋转矩阵，α表示为俯仰角，γ表示为横滚角，η表示为偏航角，m
00
＝cosηcosγ，m
01
＝sinηcosα cosηsinγsinα，m
02
＝
‑
sinηsinα cosηsinγcosα，m
10
＝
‑
sinηcosγ，m
11
＝cosηcosα
‑
sinηsinγsinα，m
12
＝
‑
cosηsinα
‑
sinηsinγcosα，m
20
＝
‑
sinγ，m
21
＝cosγsinα，m
22
＝cosγcosα。
[0118]
需要说明的是，建立第一旋转矩阵以及后续的多个旋转矩阵的目的均在于：由于多个不同坐标系之间的坐标轴角度会存在不吻合的情况，经过对应的旋转矩阵进行处理后，使得多个不同坐标系之间的坐标轴角度能够吻合，从而保证确定出的入侵机状态估计的精确性。
[0119]
s2022、基于所述第一传感器数据，计算出所述入侵机在本机站心坐标系下的第一坐标。
[0120]
在该步骤中，第一传感器数据包括斜距、方位角、仰角，其中，所述斜距包括所述无人机与所述入侵机之间的直线斜距。所述本机站心坐标系为以无人机为站心的站心坐标系。
[0121]
本发明实施例中，作为一种可选方案，可通过公式(1)，计算出所述入侵机在本机站心坐标系下的第一坐标，其中，公式(1)如下所示：
[0122][0123]
其中，表示为入侵机在本机站心坐标系下的第一坐标。m(α,γ,η)表示为第一旋转矩阵，ρ表示为斜距，∈表示为仰角，β表示为方位角。
[0124]
需要说明的是，关于公式(1)的建立过程可包括：矩阵：是将第一传感器数据中的斜距、方位角、仰角转换为在本机的机体坐标系下的前进方向、水平方向、垂直方向的三个斜距。例如，ρcos∈sinβ就是斜距乘以仰角的余弦再乘以方位角的正弦，得到本机机体坐标系下前进方向的斜距；ρcos∈cosβ就是斜距乘以仰角的余弦再乘以方位角的余弦，得到本机机体坐标系下水平方向的斜距，ρsin∈就是斜距乘以仰角的正弦，就得到在本机的机体坐标系下垂直方向的斜距。如图4所示，由于本机的机体坐标(坐标系sr，如图4虚线所示的坐标系，坐标系sr包括xr、yr、zr三个方向)是根据本机的偏航角、俯仰角、横滚
角形成的。而本机在本机站心坐标系(坐标系sg，其中坐标系sg包括xg、yg、zg三个方向)，是以本机站心为原点的坐标系，两坐标系与大地坐标系(大地坐标系包括x、y、z三个方向)之间的角度关系，包括角λ和角φ，由于坐标系sr与坐标系sg的坐标轴角度不吻合，就需要通过第一旋转矩阵m进行旋转，使得坐标系sr与坐标系sg的坐标轴角度吻合，从而确定出入侵机在本机站心坐标系下的第一坐标。
[0125]
需要说明的是，入侵机在本机站心坐标系下的第一坐标是根据第一传感器数据以及当前时刻的无人机的状态信息所确定的，后续的其他坐标则是根据其他的传感器数据以及后续其他时刻的无人机的状态信息所确定的。
[0126]
s2023、当获取第一传感器数据时，根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第一坐标。
[0127]
本发明实施例中，作为一种可选方案，可通过公式(2)，计算出本机在地心坐标系下的第一坐标，其中，公式(2)如下所示：
[0128][0129]
其中，表示为本机在地心坐标系下的第一坐标，h
s
表示为无人机的高度数据，φ
s
表示为无人机的纬度，λ
s
表示为无人机的经度，ε表示为地球第一偏心率，数值为0.081819190842622，n
s
表示为某纬度下的主要垂直曲率半径，其中，a表示为地球长半轴，数值为6378137米。
[0130]
s2024、基于所述经纬度所形成的第二旋转矩阵以及所述入侵机在以本机站心坐标系下的第一坐标，计算出在地心坐标系下的本机与所述入侵机之间的第一相对位置。
[0131]
本发明实施例中，作为一种可选方案，经纬度所形成的第二旋转矩阵t如下公式(3)所示：
[0132][0133]
其中，t(φ
s
,λ
s
)表示为第二旋转矩阵，其中，φ
s
表示为无人机的纬度，λ
s
表示为无人机的经度。
[0134]
作为一种可选方案，可通过公式(4)，基于第二旋转矩阵t(φ
s
,λ
s
)以及所述入侵机在以本机站心坐标系下的第一坐标计算出在地心坐标系下的本机与所述入侵机之间的第一相对位置，其中，公式(4)如下所示：
[0135][0136]
其中，表示为在地心坐标系下的本机与所述入侵机之间的第一相对位置，t(φ
s
,λ
s
)表示为第二旋转矩阵，表示为入侵机在以本机站心坐标系下的第一坐标。
[0137]
s2025、基于本机在地心坐标系下的坐标以及在地心坐标系下的本机与所述入侵机之间的第一相对位置，计算出所述入侵机在地心坐标系下的第一坐标。
[0138]
本发明实施例中，作为一种可选方案，可通过公式(5)，计算出所述入侵机在地心坐标系下的第一坐标，其中，公式(5)如下所示：
[0139][0140]
其中，表示为入侵机在地心坐标系下的第一坐标，表示为在地心坐标系下的本机与所述入侵机之间的第一相对位置，表示为本机在地心坐标系下的第一坐标。
[0141]
需要说明的是，入侵机在地心坐标系下的第一坐标是根据第一传感器数据以及当前时刻的无人机的状态信息经过多层处理所得到的，后续的入侵机在地心坐标系下的其他坐标则是根据其他的传感器数据以及后续其他时刻的无人机的状态信息所确定的。
[0142]
s2026、将所述入侵机在地心坐标系下的第一坐标转换为所述入侵机的大地坐标。
[0143]
在该步骤中，所述大地坐标包括wgs84坐标。
[0144]
本发明实施例中，作为一种可选方案，可通过经典转换方式，将入侵机在地心坐标系下的第一坐标转换为入侵机的大地坐标。需要说明的是，上述所提及的一种可选方案，均为本发明的举例说明，除此之外，还可以通过其他方式实现，本发明对此不做限定。
[0145]
s2027、将所述入侵机的大地坐标确定为所述入侵机的航迹头。
[0146]
进一步地，在步骤s202之后，还包括：
[0147]
s203、通过所述第一传感器数据与获取的传感器标准差，确定出所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵。
[0148]
本发明实施例中，作为一种可选方案，可基于公式(6)，确定出所述入侵机在所述
入侵机的本机机体坐标系下的第一测量误差矩阵，公式(6)如下所示：
[0149][0150]
其中，r
frm
表示为所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵，n(ρ,β,∈)表示为第一传感器数据对应的矩阵，其中，ρ表示为斜距，β表示为方位角，∈表示为仰角，n
00
＝cos∈sinβ，n
01
＝ρcos∈cosβ，n
02
＝
‑
ρsin∈sinβ，n
10
＝cos∈cosβ，n
11
＝
‑
ρcos∈sinβ，n
12
＝
‑
ρsin∈cosβ，n
20
＝sin∈，n
21
＝0，n
22
＝ρcos∈，n
′
(ρ,β,∈)表示为n(ρ,β,∈)的转置，表示为斜距的测量标准差的平方，表示为方位角测量标准差的平方，表示为仰角测量标准差的平方。
[0151]
s204、基于所述第一旋转矩阵以及所述第一测量误差矩阵，计算出所述入侵机在本机站心坐标系下的第一测量误差矩阵。
[0152]
本发明实施例中，作为一种可选方案，可基于公式(7)，计算出所述入侵机在本机站心坐标系下的第一测量误差矩阵，公式(7)如下所示：
[0153]
r
enu
＝m(α,γ,η)r
frm
m
′
(α,γ,η)
ꢀꢀꢀ
(7)
[0154]
其中，r
enu
表示为所述入侵机在本机站心坐标系下的第一测量误差矩阵，m(α,γ,η)表示为第一旋转矩阵，r
frm
表示为所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵，m
′
(α,γ,η)表示为m(α,γ,η)的转置，α表示为无人机的俯仰角，γ表示为无人机的横滚角，η表示为无人机的偏航角。
[0155]
需要说明的是，由于本机站心坐标系的坐标轴角度与本机机体坐标系的坐标轴角度不吻合，通过第一旋转矩阵m旋转处理之后，能够使得本机站心坐标系的坐标轴角度与本机机体坐标系的坐标轴角度吻合。
[0156]
s205、根据获取的所述入侵机当前时刻的经纬度以及本机当前时刻的经纬度，将所述入侵机在本机站心坐标系下的第一测量误差矩阵通过预设矩阵进行旋转平移，确定出所述入侵机在本机站心坐标系下的第二测量误差，并基于所述第二测量误差生成状态误差矩阵。
[0157]
本发明实施例中，作为一种可选方案，可基于公式(8)，确定出所述入侵机在本机站心坐标系下的第二测量误差，公式(8)如下所示：
[0158]
r＝l(φ
t
,λ
t
,φ
s
,λ
s
)r
enu
l
′
(φ
t
,λ
t
,φ
s
,λ
s
)
ꢀꢀꢀ
(8)
[0159]
其中，r表示为所述入侵机在本机站心坐标系下的第二测量误差，r
enu
表示为所述入侵机在本机站心坐标系下的第一测量误差矩阵，l(φ
t
,λ
t
,φ
s
,λ
s
)表示为预设矩阵，l
′
(φ
t
,λ
t
,φ
s
,λ
s
)表示为l(φ
t
,λ
t
,φ
s
,λ
s
)的转置，其中，l
00
＝cos(λ
t
‑
λ
s
)，l
01
＝sinφ
s
sin(λ
t
‑
λ
s
)，l
02
＝
‑
cosφ
s
sin(λ
t
‑
λ
s
)，l
10
＝
‑
sinφ
t
sin(λ
t
‑
λ
s
)，l
11
＝sinφ
t
sinφ
s
cos(λ
t
‑
λ
s
) cosφ
t
cosφ
s
，l
12
＝
‑
sinφ
t
cosφ
s
cos(λ
t
‑
λ
s
) cosφ
t
sinφ
s
，l
20
＝cosφ
t
sin(λ
t
‑
λ
s
)，l
21
＝
‑
cosφ
t
sinφ
s
cos(λ
t
‑
λ
s
) sinφ
t
cosφ
s
，l
22
＝cosφ
t
cosφ
s
cos(λ
t
‑
λ
s
) sinφ
t
sinφ
s
，其中，φ
t
表示为入侵机当前时刻的纬度，λ
t
表示为入侵机当前时刻的经度，φ
s
表示为无人机当前时刻的纬度，λ
s
表示为无人机当前时刻的经度。
[0160]
进一步地，可基于公式(9)，基于所述第二测量误差生成状态误差矩阵，其中，公式(9)如下所示：
[0161][0162]
其中，p表示为状态误差矩阵，r表示为第二测量误差。
[0163]
需要说明的是，上述第一旋转矩阵、预设矩阵、所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵、所述入侵机在本机站心坐标系下的第一测量误差矩阵、所述入侵机在本机站心坐标系下的第二测量误差以及所述状态误差矩阵，均是根据当前时刻的无人机的状态信息、经纬度等信息以及当前时刻的入侵机的传感器数据、经纬度等信息综合确定的，其中，这里的当前时刻可理解为建立入侵机的航迹头的时刻，即获取第一传感器数据的测量时刻。
[0164]
进一步地，还包括：初始化更新时间。在该步骤中，第一传感器数据包括本机测量到入侵机的测量时间点，从而根据该本机测量到入侵机的测量时间点设置为更新时间，从而完成初始化更新时间的过程。
[0165]
进一步地，还包括：初始化航迹号，即将航迹记录中的轨迹号被设置为测量中的轨迹号。
[0166]
s206、基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹。
[0167]
本发明实施例中，建立入侵机的航迹过程即首先计算所述入侵机在本机站心坐标系下的第二坐标，再通过所述入侵机本次计算出的大地坐标转换出入侵机本次的地心坐标。
[0168]
需要说明的是，这里的当前时刻可理解为获取第二传感器数据的测量时刻，与上述基于第一传感器数据来计算的公式相同，但是数据不同，本发明实施例所涉及的数据均为获取第二传感器数据的这一测量时刻，所获取的无人机以及入侵机的数据。
[0169]
本发明实施例中，步骤s206的实现过程可包括：
[0170]
s2061、当获取第二传感器数据时，根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第二坐标。
[0171]
本发明实施例中，步骤s2062的具体计算过程可参见上述步骤s2023，其中，本机在地心坐标系下的第二坐标，也就是下文的
[0172]
s2062、基于所述本机在地心坐标系下的第二坐标以及所述入侵机在地心坐标系下的第一坐标，计算出上次入侵机在本次的本机站心坐标系下的第一坐标。
[0173]
本发明实施例中，作为一种可选方案，可基于公式(10)，计算出在地心坐标系下的本机与所述入侵机之间的第一相对位置，其中，公式(10)如下所示：
[0174][0175]
其中，表示为上次入侵机在本次的本机站心坐标系下的第一坐标，表示为所述入侵机在地心坐标系下的第一坐标，表示为本机在地心坐标系下的第二坐标。
[0176]
s2063、基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第一坐标，计算出所述入侵机在本机站心坐标系下的第二坐标。
[0177]
本发明实施例中，作为一种可选方案，基于公式(11)，计算出所述入侵机在地心坐标系下的第二坐标，其中，公式(11)如下所示：
[0178][0179]
其中，表示为所述入侵机在本机站心坐标系下的第二坐标，t
′
(φ
s
,λ
s
)表示为第二旋转矩阵的转置，表示为上次入侵机在本次的本机站心坐标系下的第一坐标。
[0180]
s2064、基于所述第二传感器数据以及所述入侵机在本机站心坐标系下的第二坐标，确定出所述入侵机在地心坐标系下的第二坐标，并基于所述入侵机在地心坐标系下的第二坐标更新所述入侵机的大地坐标。
[0181]
本发明实施例中，基于所述入侵机在本机站心坐标系下的第二坐标，确定出所述入侵机在地心坐标系下的第二坐标可参见上述步骤s2024
‑
s2025的过程。即先基于无人机当前时刻的经纬度所形成的第二旋转矩阵以及所述入侵机在以本机站心坐标系下的第二坐标，计算出在地心坐标系下的本机与所述入侵机之间的第二相对位置，再基于本机在地心坐标系下的坐标以及在地心坐标系下的本机与所述入侵机之间的第二相对位置，计算出所述入侵机在地心坐标系下的第二坐标。进一步地，基于经典转换方式，将入侵机在本机站心坐标系下的第二坐标转换为新的入侵机的大地坐标，并将该新的入侵机的大地坐标替换在步骤s2026中的大地坐标。
[0182]
s2065、基于获取的所述第一传感器数据的测量时间与所述第二传感器数据的测量时间之间的测量时间差，计算出所述入侵机在本机站心坐标系下的第一速度。
[0183]
本发明实施例中，作为一种可选方案，基于公式(12)，计算出所述入侵机在本机站心坐标系下的第一速度，其中，公式(12)如下所示：
[0184][0185]
其中，表示为所述入侵机在本机站心坐标系下的第一速度，表示为所述入侵机在本机站心坐标系下的东西向速度，表示为所述入侵机在本机站心坐标系下的南北向速度，表示为所述入侵机在本机站心坐标系下的垂直速度，表示为入侵机在以本机站心坐标系下的第一坐标，表示为所述入侵机在本机站心坐标系下的第二坐标，
△
t表示为测量时间差。
[0186]
需要说明的是，关于公式(8)的建立过程可包括：首先利用第一传感器数据计算到的入侵机的大地坐标以及本机当前时刻的经纬度和高度数据(建立航迹点时的无人机的经纬度、高度数据)，可以确定出该入侵机在本机站心坐标坐标系下的第一坐标。再基于获取的入侵机的第二传感器数据以及本机当前时刻的经纬度和高度数据(其中，该经纬度和高度数据是最新本机状态信息下的数据，与建立航迹点时的无人机的经纬度、高度数据是不同的)，可以确定出所述入侵机在地心坐标系下的第二坐标。再基于本机当前时刻的经纬度和高度数据以及计算出的入侵机的第一坐标以及入侵机的第二坐标，计算入侵机在本机站心坐标系下的第一速度。这是由于入侵机的坐标的基准是相同的，因此可以利用两次本机站心坐标相减得到斜距差，再除以时间差
△
t，就得到了入侵机的航迹的速度，即入侵机在本机站心坐标系下的第一速度。
[0187]
s2066、基于所述入侵机在本机站心坐标系下的第一速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
[0188]
本发明实施例中，作为一种可选方案，步骤s2066的实现过程可包括：通过获取的入侵机经纬度及本机当前时刻的经纬度形成的第三旋转矩阵，将所述入侵机在本机站心坐标系下的第一速度转换为所述入侵机在入侵机站心坐标系下的速度，所述入侵机站心坐标系为以入侵机为站心的站心坐标系；基于所述入侵机在入侵机站心坐标系下的速度、所述
入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
[0189]
具体地，基于公式(13)，确定出所述入侵机在入侵机站心坐标系下的速度，其中，公式(13)如下所示：
[0190][0191]
其中，表示为入侵机在入侵机站心坐标系下的速度，表示为入侵机在本机站心坐标系下的第一速度，l(φ
t
,λ
t
,φ
s
,λ
s
)表示为第三旋转矩阵，其中，φ
t
表示为入侵机当前时刻的经度，λ
t
表示为入侵机当前时刻的纬度，φ
s
表示为无人机当前时刻的纬度，λ
s
表示为无人机当前时刻的经度。
[0192]
需要说明的是，关于公式(13)的建立过程：由于只获取一次入侵机的传感器数据，并不能计算出入侵机的速度，因此需要通过获取两次入侵机的传感器数据从而计算出入侵机在本机站心坐标系下的第一速度(可理解为入侵机相对于无人机的速度)，再通过该第一速度以及第三旋转矩阵进行旋转得到入侵机速度，也就是创建航迹头时入侵机的初始化速度，即入侵机在入侵机站心坐标系下的速度。
[0193]
s207、基于所述第二传感器数据以及所述测量时间差，更新所述状态误差矩阵。
[0194]
本发明实施例中，基于传感器数据计算出状态误差矩阵的过程可参见上述步骤s203
‑
s205。区别仅在于，上述步骤s203
‑
s205所计算出的状态误差矩阵是基于第一传感器数据、上一个时刻的无人机的经纬度以及入侵机的经纬度等信息计算得到的，而本发明实施例中基于是第二传感器数据以及当前时刻的无人机的经纬度以及入侵机的经纬度等信息，对状态误差矩阵进行更新，得到新的状态误差矩阵。
[0195]
例如，作为一种可选方案，可基于公式(14)，基于所述第二传感器数据以及所述测量时间差，更新所述状态误差矩阵，其中，公式(14)如下所示：
[0196][0197]
其中，p表示为更新后的状态误差矩阵，r表示为更新后的所述入侵机在本机站心坐标系下的第二测量误差，
△
t表示为测量时间差。
[0198]
进一步地，还包括：初始化更新时间，即将航迹更新时间被设置为第二传感器数据的测量时间。
[0199]
s208、当获取第三传感器数据时，根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第三坐标。
[0200]
本发明实施例中，步骤s208的具体计算过程可参见上述步骤s2023。需要说明的是，步骤s208中无人机当前时刻的经纬度和高度数据是无人机根据当前时刻自身的经纬度和高度确定出的，多个旋转矩阵同样也是基于当前时刻无人机自身的状态信息或者经纬
度、高度确定。
[0201]
s209、基于所述本机在地心坐标系下的第三坐标以及所述入侵机在地心坐标系下的第二坐标，计算出上次入侵机在本次的本机站心坐标系下的第二坐标。
[0202]
本发明实施例中，该步骤的执行过程可参见上述步骤s2062，区别在于：步骤s2062是根据所述本机在地心坐标系下的第二坐标以及所述入侵机在地心坐标系下的第一坐标，计算出上次入侵机在本次的本机站心坐标系下的第一坐标，可理解为是上次(例如第一次)入侵机在本次(例如第二次)的本机站心坐标系下的坐标。而本发明所计算出的上次入侵机在本次的本机站心坐标系下的第二坐标，可理解为是是上次(例如第二次)入侵机在本次(例如第三次)的本机站心坐标系下的坐标。需要说明的是，本发明中的“第一坐标”、“第二坐标”仅作为同一情况的坐标系下的区别使用，并不是代表其对应的传感器的获取次数。
[0203]
s210、基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第二坐标，计算出所述入侵机在本机站心坐标系下的第三坐标。
[0204]
本发明实施例中，该步骤的执行过程可参见上述步骤s2063。
[0205]
s211、基于所述第三传感器数据以及所述入侵机在本机站心坐标系下的第三坐标，确定出所述入侵机在地心坐标系下的第三坐标，并基于所述入侵机在地心坐标系下的第三坐标更新所述入侵机的大地坐标。
[0206]
本发明实施例中，该步骤的执行过程可参见上述步骤s2064。
[0207]
s212、基于所述第三旋转矩阵，将所述入侵机在入侵机站心坐标系下的速度转换为所述入侵机在本机站心坐标系下的第二速度。
[0208]
本发明实施例中，作为一种可选方案，可基于公式(15)，计算出入侵机在本机站心坐标系下的第二速度，所述公式(15)如下所示：
[0209][0210]
其中，表示为入侵机在本机站心坐标系下的第二速度，其中，第二速度可理解在基于无人机当前时刻状态信息所建立的本机站心坐标系下，入侵机的速度，而上文的第一速度可理解为在基于无人机上一时刻的状态信息所建立的本机站心坐标系下，入侵机的速度；l
′
(φ
to
,λ
to
,φ
s
,λ
s
)表示为第三旋转矩阵的转置，其中，φ
to
表示为上一次入侵机的纬度，λ
to
表示为上一次入侵机的经度，φ
s
表示为无人机当前时刻的纬度，λ
s
表示为无人机当前时刻的经度；表示为入侵机在入侵机站心坐标系下的速度。
[0211]
s213、基于所述入侵机在本机站心坐标系下的第二速度以及获取的所述入侵机在
本机站心坐标系下的预测位置，确定出入侵机的预测状态矩阵。
[0212]
本发明实施例中，确定出的预测状态矩阵如下公式(16)所示：
[0213][0214]
其中，
[0215]
在公式(16)中，(e
o
,n
o
,u
o
)表示为所述入侵机在本机站心坐标系下的第二坐标，表示为入侵机在本机站心坐标系下的预测位置(其中，预测位置＝所述入侵机在本机站心坐标系下的第二坐标 t*速度)，表示为所述入侵机在本机站心坐标系下的第二速度，表示为入侵机在本机站心坐标系下的预测速度，假设入侵机在极短时间间隔内做匀速运动，速度是不变的，所以预测速度等于入侵机在本机站心坐标系下的第二速度。
[0216]
s214、基于获取的所述入侵机上次更新时的经纬度及本机当前时刻的经纬度形成的第四旋转矩阵，将所述更新后的状态误差矩阵经过所述第四旋转矩阵处理，获取到所述入侵机在本机站心坐标下的估计状态矩阵。
[0217]
在该步骤中，上次更新时的经纬度即为前一次的入侵机的经纬度。例如，当获取第3次的传感器数据(属于第三传感器数据)时，是使用建立航迹时(也就是第2次的传感器数据，属于第二传感器数据)更新的经纬度。但之后的每一次都使用前一次更新的经纬度，例如，第4次的传感器数据(属于第三传感器数据)就会使用第3次的传感器数据(属于第三传感器数据)更新过的经纬度。
[0218]
本发明实施例中，作为一种可选方案，基于公式(17)，获取到所述入侵机在本机站心坐标下的估计状态矩阵，其中，公式(17)如下所示：
[0219]
p
o
＝upu
′ꢀꢀꢀ
(17)
[0220]
其中，p
o
表示为入侵机在本机站心坐标下的估计状态矩阵，p表示为所述更新后的状态误差矩阵，u表示为第四旋转矩阵，其中，l
′
(φ
to
,λ
to
,φ
s
,λ
s
)表示为第三旋转矩阵的转置，其中，φ
to
表示为入侵机上次更新时的纬度，λ
to
表示为入侵机上次更新时的经度，φ
s
表示为无人机当前时刻的纬度，λ
s
表示为无人机当前时刻的经度。
[0221]
s215、基于所述估计状态矩阵以及获取的状态方程的误差矩阵，确定出所述入侵机的预测状态误差矩阵。
[0222]
在该步骤中，所述入侵机的状态方程的误差矩阵可以由本机传感器在本机站心三个方向上的加速度的测量噪声标准差及测量时间差获得。
[0223]
具体地，可基于公式(18)，确定出状态方程的误差矩阵，其中，公式(18)如下所示：
[0224]
q＝gγg
′ꢀꢀꢀ
(18)
[0225]
其中，q表示为状态方程的误差矩阵，g表示为过程噪声非线性预测矩阵，γ表示为
本机传感器在本机站心三个方向上加速度的测量噪声标准差，g
′
表示为g的转置，其中，
△
t表示为测量时间差。
[0226]
进一步地，作为一种可选方案，基于公式(19)，确定出所述入侵机的预测状态误差矩阵，其中，公式(19)如下所示：
[0227][0228]
其中，表示为所述入侵机的预测状态误差矩阵，f表示为运动状态线性外推矩阵，p0表示为入侵机在本机站心坐标下的估计状态矩阵，f
′
表示为f的转置，q表示为状态方程的误差矩阵，其中，
△
t表示为测量时间差。
[0229]
s216、通过扩展卡尔曼滤波算法，基于所述预测状态矩阵、所述预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值。
[0230]
本发明实施例中，步骤s216可具体包括：
[0231]
s2161、将所述入侵机在本机站心坐标下的预测位置经过第一旋转矩阵处理，得到所述入侵机的本机机体坐标系下的预测位置。
[0232]
本发明实施例中，作为一种可选的方案，可基于公式(20)，计算出所述入侵机的本机机体坐标系下的预测位置，其中，公式(20)如下所示：
[0233][0234]
其中，表示为所述入侵机的本机机体坐标系下的预测位置，其中，表示为入侵机在无人机机身横向坐标轴的预测位置坐标，表示为入侵机在无人机机身轴向坐标轴的
预测位置坐标，表示为入侵机在无人机机身垂直方向坐标轴的预测位置坐标，相当于把入侵机在本机站心坐标系下的预测位置旋转为所述入侵机的本机机体坐标系下的预测位置，m
′
(α,γ,η)表示为m的转置，，表示为所述入侵机在本机站心坐标下的预测位置。
[0235]
s2162、计算在本机站心坐标系下所述入侵机与本机之间的相对速度。
[0236]
本发明实施例中，作为一种可选方案，基于公式(21)，可计算出在本机站心坐标系下所述入侵机与本机之间的相对速度，其中，公式(21)如下所示：
[0237][0238]
其中，表示为相对速度，表示为入侵机在本机站心坐标系下的预测速度，表示为本机速度。
[0239]
s2163、基于所述相对速度、所述入侵机在本机站心坐标下的预测位置以及所述入侵机的本机机体坐标系下的预测位置，确定出所述入侵机的预测测量值。
[0240]
本发明实施例中，作为一种可选方案，可通过公式(22)，确定出所述入侵机的预测测量值，其中，公式(22)如下所示：
[0241][0242][0243][0244][0245]
其中，表示为预测测量值，表示为所述入侵机的本机机体坐标系下的预测位
置，表示为所述入侵机在本机站心坐标下的预测位置，表示为相对速度，表示为代表预测的斜距变化率测量值，表示为入侵机在所述入侵机的本机机体坐标系下的预测位置，表示为预测的方位角测量值，表示为预测的仰角测量值。
[0246]
本发明实施例中，进一步地，还包括：
[0247]
s217、基于获取的测量矩阵、信息矩阵以及增益矩阵，计算出所述入侵机的滤波增益矩阵。
[0248]
本发明实施例中，步骤s216可具体包括：
[0249]
s2171、获取测量矩阵。
[0250]
本发明实施例中，作为一种可选方案，利用公式(23)：生成测量矩阵其中，h
10
＝[h
30 h
31 h
32
]，h
11
＝[h
33 h
34 h
35
]，]，表示为关联矩阵，入侵机在所述入侵机的本机机体坐标系下坐标和预测测量值进行关联，表示为入侵机在无人机机身横向坐标轴的预测位置坐标，表示为入侵机在无人机机身轴向坐标轴的预测位置坐标，表示为入侵机在无人机机身垂直方向坐标轴的预测位置坐标，表示为斜距在无人机机体坐标平面的投影长度，其中，根据勾股定理，斜边平方减垂直方向坐标平方，再开方，所以代表斜距在无人机机体坐标平面的投影长度。
[0251]
s2172、基于所述测量矩阵及预测状态误差矩阵生成新息矩阵。本发明实施例中，作为一种可选方案，通过公式(24)：基于测量矩阵h及预测状态误差
[0252]
矩阵生成新息矩阵s，其中，h表示为观测矩阵，h
′
表示为h的转置，表示为所述入
侵机的预测状态误差矩阵，其中，表示为斜距方差，表示为方位角方差，表示为仰角方差，表示为斜距变化率方差。需要说明的是，斜距方差、方位角方差、仰角方差、斜距变化率方差可由本机传感器直接提供。
[0253]
s2173、基于所述新息矩阵以及所述测量矩阵生成增益矩阵。
[0254]
本发明实施例中，通过公式(25)：基于测量矩阵h以及新息矩阵s，生成增益矩阵w，其中，表示为预测状态矩阵的误差矩阵，基于通过公式(20)计算得到，h
′
表示为h的转置，s表示为新息矩阵。需要说明的是，为了将转换为方阵，所以需要前乘h，后乘h
′
。
[0255]
本发明实施例中，进一步地，还包括：更新所述入侵机的状态向量。具体可通过获得新息向量根据新息向量可以将入侵机的状态向量更新为在该步骤中，第三传感器数据还包括斜距、方位角、仰角、斜距变化率，从而在更新所述入侵机的状态向量时，能够使用从第三传感器数据中获取的实际的斜距、方位角、仰角、斜距变化率数据减去预测的斜距、方位角、仰角、斜距变化率数据，即可得到实际值与预测值的偏差。此外，更新的入侵机的状态向量＝状态预估值(即预测位置和预测速度) 增益矩阵w*新息向量(即测量偏差值)。
[0256]
本发明实施例中，进一步地，还包括：更新入侵机的大地坐标。
[0257]
作为一种可选方案，可获取入侵机在地心坐标系中的相对位置及绝对位置并将其转换最新的入侵机的大地坐标，并将入侵机的大地坐标更新为该最新的入侵机的大地坐标。
[0258]
本发明实施例中，进一步地，还包括：更新入侵机在入侵机站心坐标系下的速度。具体过程可参见上述步骤，本步骤对此不再累述。
[0259]
本发明实施例中，进一步地，还包括：更新入侵机的状态误差矩阵。
[0260]
作为一种可选方案，可将入侵机在本机站心坐标系下的状态误差矩阵更新为p＝
vp1v
′
，其中，i6×6表示为6*6的单位阵，w表示为增益矩阵，h表示为测量矩阵，表示为预测状态矩阵的误差矩阵，r表示为本机enu站心坐标系下的测量误差，其中
[0261]
其中，表示为斜距方差，表示为方位角方差，表示为仰角方差，表示为斜距变化率方差。
[0262]
本发明提供的一种适用于无人机空中防撞的入侵机航迹融合跟踪方法的实施例中，获取入侵机的第一传感器数据，基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头，基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，从而解决了现有技术不能够直接使用扩展卡尔曼滤波算法对入侵机的状态进行估计的问题，因此本发明的方案具有更高的适用性及跟踪效果，能够有效提高入侵机跟踪的跟踪效果、准确性、鲁棒性以及实用性。
[0263]
图5为本发明实施例所提供的一种入侵机跟踪装置的结构示意图，如图5所示，该入侵机跟踪装置10包括：
[0264]
获取模块11，用于获取入侵机的第一传感器数据，所述第一传感器数据为所述无人机首次探测到的传感器数据；
[0265]
确定模块12，用于基于本机的状态信息以及所述第一传感器数据，确定出所述入侵机的航迹头；
[0266]
建立模块13，用于基于获取的所述入侵机的第二传感器数据以及所述入侵机的航迹头，建立所述入侵机的航迹，所述第二传感器数据为所述无人机第二次探测到的传感器数据；
[0267]
计算模块14，用于通过扩展卡尔曼滤波算法，基于获取的入侵机的预测状态矩阵、预测状态误差矩阵、所述入侵机的第三传感器数据以及所述入侵机的航迹，计算预测测量值，所述第三传感器数据为已建立航迹的入侵机的传感器数据，所述预测测量值包括所述入侵机的预测坐标和速度。
[0268]
可选地，本发明实施例中，所述状态信息包括俯仰角、横滚角、偏航角，所述传感器数据包括斜距、方位角、仰角，所述斜距包括所述无人机与所述入侵机之间的直线斜距；
[0269]
该装置的确定模块12具体用于：通过所述俯仰角、所述横滚角以及所述偏航角，确定出第一旋转矩阵；基于所述斜距、所述方位角以及所述仰角，计算出所述入侵机在本机站心坐标系下的第一坐标，所述本机站心坐标系为以无人机为站心的站心坐标系；根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第一坐标；基于所述经纬度所形成的第二旋转矩阵以及所述入侵机在以本机站心坐标系下的第一坐标，计算出在地心
坐标系下的本机与所述入侵机之间的第一相对位置；基于本机在地心坐标系下的坐标以及在地心坐标系下的本机与所述入侵机之间的第一相对位置，计算出所述入侵机在地心坐标系下的第一坐标；将所述入侵机在地心坐标系下的第一坐标转换为所述入侵机的大地坐标，所述大地坐标包括wgs84坐标；将所述入侵机的大地坐标确定为所述入侵机的航迹头。
[0270]
可选地，本发明实施例中，该装置的建立模块13具体用于：根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第二坐标；基于所述本机在地心坐标系下的第二坐标以及所述入侵机在地心坐标系下的第一坐标，计算出上次入侵机在本次的本机站心坐标系下的第一坐标；基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第一坐标，计算出所述入侵机在本机站心坐标系下的第二坐标；基于所述第二传感器数据以及所述入侵机在本机站心坐标系下的第二坐标，确定出所述入侵机在地心坐标系下的第二坐标，并基于所述入侵机在地心坐标系下的第二坐标更新所述入侵机的大地坐标；基于获取的所述第一传感器数据的测量时间与所述第二传感器数据的测量时间之间的测量时间差，计算出所述入侵机在本机站心坐标系下的第一速度；基于所述入侵机在本机站心坐标系下的第一速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
[0271]
可选地，本发明实施例中，该装置的建立模块13具体用于：通过获取的入侵机经纬度及本机当前时刻的经纬度形成的第三旋转矩阵，将所述入侵机在本机站心坐标系下的第一速度转换为所述入侵机在入侵机站心坐标系下的速度，所述入侵机站心坐标系为以入侵机为站心的站心坐标系；基于所述入侵机在入侵机站心坐标系下的速度、所述入侵机在地心坐标系下的第二坐标以及所述入侵机的航迹头，建立所述入侵机的航迹。
[0272]
可选地，本发明实施例中，该装置的计算模块14具体用于：将获取的所述入侵机在本机站心坐标下的预测位置经过第一旋转矩阵处理，得到所述入侵机的本机机体坐标系下的预测位置；计算在本机站心坐标系下所述入侵机与本机之间的相对速度；基于所述相对速度、所述入侵机在本机站心坐标下的预测位置以及所述入侵机的本机机体坐标系下的预测位置，确定出所述入侵机的预测测量值。
[0273]
可选地，本发明实施例中，该装置还包括预处理模块15。
[0274]
预处理模块15用于当获取第三传感器数据时，根据本机当前时刻的经纬度和高度数据，计算出本机在地心坐标系下的第三坐标；基于所述本机在地心坐标系下的第三坐标以及所述入侵机在地心坐标系下的第二坐标，计算出上次入侵机在本次的本机站心坐标系下的第二坐标；基于所述第二旋转矩阵、上次入侵机在本次的本机站心坐标系下的第二坐标，计算出所述入侵机在本机站心坐标系下的第三坐标；基于所述第三传感器数据以及所述入侵机在本机站心坐标系下的第三坐标，确定出所述入侵机在地心坐标系下的第三坐标，并基于所述入侵机在地心坐标系下的第三坐标更新所述入侵机的大地坐标；基于所述第三旋转矩阵，将所述入侵机在入侵机站心坐标系下的速度转换为所述入侵机在本机站心坐标系下的第二速度；基于所述入侵机在本机站心坐标系下的第二速度以及获取的所述入侵机在本机站心坐标系下的预测位置，确定出入侵机的预测状态矩阵。
[0275]
可选地，本发明实施例中，该装置的预处理模块15还用于通过所述第一传感器数据与获取的传感器标准差，确定出所述入侵机在所述入侵机的本机机体坐标系下的第一测量误差矩阵；基于所述第一旋转矩阵以及所述第一测量误差矩阵，计算出所述入侵机在本
机站心坐标系下的第一测量误差矩阵；根据获取的所述入侵机当前时刻的经纬度以及本机当前时刻的经纬度，将所述入侵机在本机站心坐标系下的第一测量误差矩阵通过预设矩阵进行旋转平移，确定出所述入侵机在本机站心坐标系下的第二测量误差，并基于所述第二测量误差生成状态误差矩阵；基于所述第二传感器数据以及所述测量时间差，更新所述状态误差矩阵；基于获取的所述入侵机上次更新时的经纬度及本机当前时刻的经纬度形成的第四旋转矩阵，将所述更新后的状态误差矩阵经过所述第四旋转矩阵处理，获取到所述入侵机在本机站心坐标下的估计状态矩阵；基于所述估计状态矩阵以及获取的状态方程的误差矩阵，确定出所述入侵机的预测状态误差矩阵。
[0276]
本实施例提供的装置的具体实现原理和效果均可以参见前述实施例，此处不再赘述。
[0277]
图6为本发明实施例所提供的一种入侵机跟踪设备的框图，如图6所示，该设备800可以包括以下一个或多个组件：处理组件802，存储器804，电源组件806，多媒体组件808，音频组件810，输入/输出(i/o)接口812，传感器组件814，以及通信组件816。
[0278]
处理组件802通常控制装置800的整体操作，诸如与显示，电话呼叫，数据通信，相机操作和记录操作相关联的操作。处理组件802可以包括一个或多个处理器820来执行指令，以完成上述的方法的全部或部分步骤。此外，处理组件802可以包括一个或多个模块，便于处理组件802和其他组件之间的交互。例如，处理组件802可以包括多媒体模块，以方便多媒体组件808和处理组件802之间的交互。
[0279]
存储器804被配置为存储各种类型的数据以支持在装置800的操作。这些数据的示例包括用于在装置800上操作的任何应用程序或方法的指令，联系人数据，电话簿数据，消息，图片，视频等。存储器804可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(sram)，电可擦除可编程只读存储器(eeprom)，可擦除可编程只读存储器(eprom)，可编程只读存储器(prom)，只读存储器(rom)，磁存储器，快闪存储器，磁盘或光盘。
[0280]
电源组件806为装置800的各种组件提供电力。电源组件806可以包括电源管理系统，一个或多个电源，及其他与为装置800生成、管理和分配电力相关联的组件。
[0281]
多媒体组件808包括在所述装置800和用户之间的提供一个输出接口的屏幕。在一些实施例中，屏幕可以包括液晶显示器(lcd)和触摸面板(tp)。如果屏幕包括触摸面板，屏幕可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。在一些实施例中，多媒体组件808包括一个前置摄像头和/或后置摄像头。当装置800处于操作模式，如拍摄模式或视频模式时，前置摄像头和/或后置摄像头可以接收外部的多媒体数据。每个前置摄像头和后置摄像头可以是一个固定的光学透镜系统或具有焦距和光学变焦能力。
[0282]
音频组件810被配置为输出和/或输入音频信号。例如，音频组件810包括一个麦克风(mic)，当装置800处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在存储器804或经由通信组件816发送。在一些实施例中，音频组件810还包括一个扬声器，用于输出音频信号。
[0283]
i/o接口812为处理组件802和外围接口模块之间提供接口，上述外围接口模块可
以是键盘，点击轮，按钮等。这些按钮可包括但不限于：主页按钮、音量按钮、启动按钮和锁定按钮。
[0284]
传感器组件814包括一个或多个传感器，用于为装置800提供各个方面的状态评估。例如，传感器组件814可以检测到装置800的打开/关闭状态，组件的相对定位，例如所述组件为装置800的显示器和小键盘，传感器组件814还可以检测装置800或装置800一个组件的位置改变，用户与装置800接触的存在或不存在，装置800方位或加速/减速和装置800的温度变化。传感器组件814可以包括接近传感器，被配置用来在没有任何的物理接触时检测附近物体的存在。传感器组件814还可以包括光传感器，如cmos或ccd图像传感器，用于在成像应用中使用。在一些实施例中，该传感器组件814还可以包括加速度传感器，陀螺仪传感器，磁传感器，压力传感器或温度传感器。
[0285]
通信组件816被配置为便于装置800和其他设备之间有线或无线方式的通信。装置800可以接入基于通信标准的无线网络，如wifi，2g或3g，或它们的组合。在一个示例性实施例中，通信组件816经由广播信道接收来自外部广播管理系统的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件816还包括近场通信(nfc)模块，以促进短程通信。例如，在nfc模块可基于射频识别(rfid)技术，红外数据协会(irda)技术，超宽带(uwb)技术，蓝牙(bt)技术和其他技术来实现。
[0286]
在示例性实施例中，装置800可以被一个或多个应用专用集成电路(asic)、数字信号处理器(dsp)、数字信号处理设备(dspd)、可编程逻辑器件(pld)、现场可编程门阵列(fpga)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述方法。
[0287]
在示例性实施例中，还提供了一种包括指令的非临时性计算机可读存储介质，例如包括指令的存储器804，上述指令可由装置800的处理器820执行以完成上述方法。例如，所述非临时性计算机可读存储介质可以是rom、随机存取存储器(ram)、cd
‑
rom、磁带、软盘和光数据存储设备等。
[0288]
一种非临时性计算机可读存储介质，当该存储介质中的指令由入侵机跟踪设备的处理器执行时，使得入侵机跟踪设备能够执行上述入侵机跟踪设备的入侵机跟踪方法。
[0289]
本发明实施例还提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现前述任一实施例所述的入侵机跟踪方法。
[0290]
在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
[0291]
本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本发明的其它实施方案。本发明旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未公开的本技术领域中的公知常识或惯用技术手段。
[0292]
应当理解的是，本发明并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。