无人机视觉里程计与卫星地图定位松耦合融合定位方法与流程

1.本发明涉及无人机视觉定位领域，更为具体的，涉及一种无人机视觉里程计与卫星地图定位松耦合融合定位方法。


背景技术：

2.目前无人机地定位主要依赖gnss等导航卫星系统，或者地面差分站。在战争场景下，导航卫星系统的信号容易受到干扰，会导致无人机坠毁或被地方捕获，地面差分站则需要提前在地面上部署基站，这在战争场景下是不现实的。


技术实现要素：

3.本发明的目的在于克服现有技术的不足，提供一种无人机视觉里程计与卫星地图定位松耦合融合定位方法，不依赖gnss和地面差分站对无人机进行定位，在可视化操作界面上实时显示无人机的飞行轨迹，并且可以通过可视化操作界面给无人机发送飞行指令，将有效降低无人机的事故率和战时的消耗率等。
4.本发明的目的是通过以下方案实现的：
5.一种无人机视觉里程计与卫星地图定位松耦合融合定位方法，包括以下步骤：
6.s1，通过gnss坐标与视觉里程计的轨迹关联，确定初始位置，完成初始化；
7.s2，利用视觉里程计跟踪每一帧航拍图像，计算无人机的相对位置；
8.s3，通过航拍图像与局部卫星地图的模板匹配，计算无人机的绝对位置；
9.s4，通过窗口轨迹关联将步骤s2中视觉里程计计算的无人机的相对位置与步骤s3中卫星地图定位计算的绝对位置相融合；
10.s5，在可视化界面显示无人机的实时位置。
11.进一步地，在步骤s1中，包括子步骤：
12.s11，获取一段飞行距离的视觉里程计轨迹和对应的gnss的经纬高坐标轨迹，将gnss的经纬高坐标变换到东北天坐标系下；
13.s12，计算步骤s11中两段轨迹之间的相对坐标变换关系，通过所述相对坐标变换关系将视觉里程计轨迹变换到东北天坐标系下，进而变换成经纬高坐标。
14.进一步地，在步骤s3中，包括子步骤：
15.s31，从视觉里程计获取先验位姿，在无人机初始位置附近提取局部卫星地图；
16.s32，用理论观测图像和航拍图像之间的互信息的倒数作为目标函数，
17.在先验位姿附近优化搜索最优位姿。
18.进一步地，在步骤s4中，包括子步骤：
19.s41，当新一帧完成卫星地图定位，首先提取距离该帧最近的若干个旧帧，旧帧同时拥有两个位姿，分别是来自视觉里程计的先验位姿和来自融合后的后验位姿；
20.s42，计算所述若干个旧帧之间的先验位姿轨迹和后验位姿轨迹之间的相对变换；
21.s43，将新一帧的视觉里程计先验位姿变换到后验位姿轨迹坐标系下；
22.s44，计算变换后的位姿和卫星地图定位位姿的加权和，权重取决于最优模板匹配的互信息；
23.s45，将加权结果存储为新一帧的后验位姿。
24.进一步地，在步骤s5中，所述可视化界面设置有显示条框。
25.进一步地，在所述可视化界面中，包括子步骤：用不显色的线条表示不同的飞行轨迹。
26.进一步地，所述飞行轨迹包括gnss轨迹和视觉定位系统估计的轨迹，轨迹被画在无人机飞行位置的局部卫星地图上，无人机的实时航拍图像显示在可视化界面的左下角，飞机的经纬高数据显示在可视化界面左侧的显示条框中，通过鼠标右键点击和选择将导航目标的位置发送给无人机。
27.进一步地，在步骤s41中，所述先验位姿的坐标原点是视觉里程计的初始化点，后验位姿的坐标系是东北天坐标系。
28.进一步地，在步骤s12中，所述相对坐标变换关系包括相对旋转、相对平移和相对尺度。
29.本发明的有益效果包括：
30.本发明实施例提供了一种完整的无人机视觉定位方案，可以在初始化完成后，不依赖gnss和地面差分站对无人机进行定位，在可视化操作界面上实时显示无人机的飞行轨迹，并且可以通过可视化操作界面给无人机发送飞行指令，将有效降低无人机的事故率和战时的消耗率。
31.本发明实施例中，无人机起飞后，当视觉定位系统初始化完成，在无gnss的情况下仍可以实时确定无人机的经纬高坐标，并且估计的飞行轨迹光滑、噪声小。
32.本发明实施例系统的可视化操作界面，可以实时显示无人机的位置，并给无人机发送飞行指令。
附图说明
33.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
34.图1为本发明实施例方法的步骤流程图；
35.图2为本发明实施例中初始化模块流程图；
36.图3为本发明实施例中模板匹配卫星地图定位流程图；
37.图4为本发明实施例中视觉里程计和卫星地图定位的松耦合融合定位流程图；
38.图5为本发明实施例中可视化操作界面。
具体实施方式
39.下面结合附图和实施例对发明进一步说明。本说明书中所有实施例公开的所有特征，或隐含公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合和/或扩展、替换。
40.本发明实施例提供的无人机视觉里程计与卫星地图定位松耦合融合定位方法，发明构思如下：系统初始化之前，需要先对视觉里程计初始化。视觉里程计通过图像特征点跟踪，快速地估计无人机的相对运动。但视觉里程计所计算的无人机飞行轨迹的坐标原点是视觉里程计完成初始化的位置，无法获取飞机的在真实世界坐标系下的绝对位置。因此，本发明实施例视觉定位系统在视觉里程计初始化完成后，设计为通过gnss信号进行初始化，即利用gnss信号计算视觉里程计轨迹对应的经纬高坐标轨迹。初始化完成后，视觉里程计估计的飞行轨迹可以通过旋转、平移和尺度放缩变换到东北天坐标系下，再由东北天坐标变换成经纬高坐标。但视觉里程计会随着时间的推移产生累积误差，本发明实施例系统通过一种基于模板匹配的卫星地图定位方法对视觉里程计估计的轨迹进行修正。卫星地图定位可以直接计算飞机的经纬高坐标，但定位结果噪声较大，直接使用该坐标会导致估计的飞行轨迹存在剧烈的抖动。本发明实施例通过一种松耦合轨迹融合方法对估计的轨迹进行平滑，减小噪声。
41.在具体实施方式中，本发明实施例基于视觉里程计与gnss轨迹关联的初始化包括：初始化模块主要包括两个步骤。第一步，获取一段飞行距离的视觉里程计轨迹和对应的gnss的经纬高坐标轨迹，将gnss的经纬高坐标变换到东北天坐标系下。第二步，计算两段轨迹之间的相对坐标变换关系，包括相对旋转、相对平移和相对尺度。通过相对坐标变换关系就可以将视觉里程计轨迹变换到东北天坐标系下，进而变换成经纬高坐标。
42.在具体实施方式中，本发明实施例基于模板匹配的卫星地图定位包括：本发明实施例使用基于互信息的模板匹配方法进行卫星地图定位，具体计算流程是：首先将视觉里程计估计的位置变换为经纬高坐标，然后在该经纬高坐标对应的卫星地图附近提取局部卫星地图。视觉里程计估计的位置和姿态称为先验位姿，根据先验位姿，从局部卫星地图中提取理论观测图像，这个理论观测图像就是在先验位姿假设下，飞机理论上应该从卫星地图上看到的图像。再利用图像的互信息可以计算实际航拍图像与理论观测图像之间的相似度。将图像相似度的倒数作为目标函数，在先验位姿附近用优化方法搜索相似度最高的位姿，作为卫星地图定位得到的位姿。
43.在具体实施方式中，本发明实施例视觉里程计和卫星地图定位的松耦合融合定位包括：视觉里程计估计的轨迹在短时间内比较精确，但会随着时间推移产生累积误差。卫星地图定位用于直接计算绝对位置，对视觉里程计的累积误差进行修正，但计算量较大，计算结果误差较大。本发明实施例用一种松耦合融合定位方法对视觉里程计和卫星地图定位的结果进行融合，本方法既可以修正视觉里程计的累积误差，又可以不受卫星地图定位误差的影响，且融合算法的处理速度很快。
44.一般的松耦合信息融合算法都是卡尔曼滤波类算法，这类算法的信息融合结果只依赖于上一帧数据。无人机的飞行轨迹一般是比较平滑的，用卡尔曼滤波类算法融合无人机飞行轨迹，会将卫星地图定位的误差也融合到无人机飞行轨迹中，导致飞行轨迹产生抖动。本发明实施例发明了一种新的针对飞机飞行轨迹的松耦合融合定位方法，可以修正累积误差的同时，避免噪声的影响，保证融合轨迹的光滑。
45.在具体实施方式中，当新一帧完成卫星地图定位，首先提取距离该帧最近的若干个旧帧，本系统设置为15帧。这些旧帧同时拥有两个位姿，分别是来自视觉里程计的先验位姿和来自松耦合融合定位的后验位姿，先验位姿的坐标原点是视觉里程计的初始化点，后
验位姿的坐标系是东北天坐标系。这些旧帧的先验位姿和后验位姿分别构成两条轨迹，用和系统初始化时相同的方法可以计算出两条轨迹之间的相对变换。将新一帧的视觉里程计位姿通过该相对变换变换到东北天坐标系下，然后计算新一帧变换所得位姿和卫星地图定位位姿的加权和，加权的权重取决于卫星地图定位的最优匹配所对应的互信息，互信息越大，说明航拍图像和卫星地图理论观测图像的相似度越高，从而说明卫星地图定位结果越可靠，卫星地图定位的位姿所占比重越大，反之说明定位结果误差可能较大。
46.本发明实施例中系统的可视化操作界面如图5所示，可视化操作界面设置有显示条框(图5中未表示出来)，并且用不显色的线条表示不同的飞行轨迹，包括gnss轨迹和视觉定位系统估计的轨迹，轨迹被画在了飞机飞行位置的局部卫星地图上，飞机的实时航拍图像显示在界面的左下角，飞机的经纬高等数据可以显示在显示条框中，显示条框设置在界面左侧，通过鼠标右键点击和选择可以将导航目标的位置发送给飞机。
47.实施例1
48.一种无人机视觉里程计与卫星地图定位松耦合融合定位方法，包括以下步骤：
49.s1，通过gnss坐标与视觉里程计的轨迹关联，确定初始位置，完成初始化；
50.s2，利用视觉里程计跟踪每一帧航拍图像，计算无人机的相对位置；
51.s3，通过航拍图像与局部卫星地图的模板匹配，计算无人机的绝对位置；
52.s4，通过窗口轨迹关联将步骤s2中视觉里程计计算的无人机的相对位置与步骤s3中卫星地图定位计算的绝对位置相融合；
53.s5，在可视化界面显示无人机的实时位置。
54.实施例2
55.在实施例1的基础上，在步骤s1中，包括子步骤：
56.s11，获取一段飞行距离的视觉里程计轨迹和对应的gnss的经纬高坐标轨迹，将gnss的经纬高坐标变换到东北天坐标系下；
57.s12，计算步骤s11中两段轨迹之间的相对坐标变换关系，通过所述相对坐标变换关系将视觉里程计轨迹变换到东北天坐标系下，进而变换成经纬高坐标。
58.实施例3
59.在实施例1的基础上，在步骤s3中，包括子步骤：
60.s31，从视觉里程计获取先验位姿，在无人机初始位置附近提取局部卫星地图；
61.s32，用理论观测图像和航拍图像之间的互信息的倒数作为目标函数，在先验位姿附近优化搜索最优位姿。
62.实施例4
63.在实施例1的基础上，在步骤s4中，包括子步骤：
64.s41，当新一帧完成卫星地图定位，首先提取距离该帧最近的若干个旧帧，旧帧同时拥有两个位姿，分别是来自视觉里程计的先验位姿和来自融合后的后验位姿；
65.s42，计算所述若干个旧帧之间的先验位姿轨迹和后验位姿轨迹之间的相对变换；
66.s43，将新一帧的视觉里程计先验位姿变换到后验位姿轨迹坐标系下；
67.s44，计算变换后的位姿和卫星地图定位位姿的加权和，权重取决于最优模板匹配的互信息；
68.s45，将加权结果存储为新一帧的后验位姿。
69.实施例5
70.在实施例1的基础上，在步骤s5中，所述可视化界面设置有显示条框。
71.实施例6
72.在实施例5的基础上，在所述可视化界面中，包括子步骤：用不显色的线条表示不同的飞行轨迹。
73.实施例7
74.在实施例6的基础上，所述飞行轨迹包括gnss轨迹和视觉定位系统估计的轨迹，轨迹被画在无人机飞行位置的局部卫星地图上，无人机的实时航拍图像显示在可视化界面的左下角，飞机的经纬高数据显示在可视化界面左侧的显示条框中，通过鼠标右键点击和选择将导航目标的位置发送给无人机。
75.实施例8
76.在实施例4的基础上，在步骤s41中，所述先验位姿的坐标原点是视觉里程计的初始化点，后验位姿的坐标系是东北天坐标系。
77.实施例9
78.在实施例2的基础上，在步骤s12中，所述相对坐标变换关系包括相对旋转、相对平移和相对尺度。
79.描述于本发明实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现，所描述的单元也可以设置在处理器中。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定。
80.根据本技术的一个方面，提供了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行上述各种可选实现方式中提供的方法。
81.作为另一方面，本技术还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该电子设备执行时，使得该电子设备实现上述实施例中所述的方法。
82.本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。
83.上述技术方案只是本发明的一种实施方式，对于本领域内的技术人员而言，在本发明公开了应用方法和原理的基础上，很容易做出各种类型的改进或变形，而不仅限于本发明上述具体实施方式所描述的方法，因此前面描述的方式只是优选的，而并不具有限制性的意义。
84.除以上实例以外，本领域技术人员根据上述公开内容获得启示或利用相关领域的知识或技术进行改动获得其他实施例，各个实施例的特征可以互换或替换，本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。