一种基于先验视觉标签的无人机定位方法与流程

1.本发明涉及无人机定位技术领域，具体为一种基于先验视觉标签的无人机定位方法。


背景技术：

2.随着无人机技术的不断发展，无人机的应用范围越来越广，比如应用在军事行动、地质勘测、物流运输、农业应用、影视拍摄、消防抗灾以及救援巡查等诸多领域。
3.现有无人机的定位多基于gps定位技术实现，对无人机的应用环境有要求，在煤矿等gps信号微弱或不存在的情况下，无人机无法正常工作，给使用带来不便。


技术实现要素：

4.本发明的目的在于提供一种基于先验视觉标签的无人机定位方法，以解决上述背景技术中提出的问题。
5.为实现上述目的，本发明提供如下技术方案：一种基于先验视觉标签的无人机定位方法，包括如下步骤：
6.步骤1、获得先验视觉标签地图；
7.步骤2、对slam系统进行初始化；
8.步骤3、求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；
9.步骤4、对步骤3求解的相机位姿进行优化，获得最终的相机姿态系统。
10.优选的，所述步骤1中，在使用环境中布置一个包括多个二维码的地图，每个二维码具有自己独立的id和位姿信息，获得的视觉标签地图作为无人机导航的先验知识。
11.优选的，所述步骤2中，使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断slam系统是否成功求得相对位姿，若成功初始化，则进入特征点三角化环节，若失败，则使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿后再进行三角化；此后将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点插入地图，完成初始地图的更新。
12.优选的，所述基于先验视觉标签的相对位姿求解子方法的过程为：计算第一帧f0中视觉标签与第一帧f0的相对位姿γ0，第二帧f1中视觉标签与第二帧f1的相对位姿γ1，两帧之间的相对位姿t具体为：
13.优选的，所述基于特征点的相对位姿求解子方法的过程为：先计算本质矩阵模型和单应矩阵模型，本质矩阵模型计算方法为：图像的第一帧f0和第二帧f1中的一对特征点p1和p2是空间中一点p在图片中的投影，一对特征点p1和p2可以确定空间中的一点p，在第一帧f0所设坐标系下点p的空间位置为：
14.p＝[x,y,z]
t
；
[0015]
影模型中，点p1和p2的像素坐标分别为：
[0016]
x0＝kp
[0017]
x1＝k(rp+t)；
[0018]
其中，k为相机的内参矩阵，根据对极约束可得：
[0019][0020]
令本质矩阵e＝t^r，然后根据点p1和p2的像素位置求解本质矩阵e；
[0021]
单应矩阵模型的计算方法为：使用dlt算法求解单应矩阵h；然后通过计算上述本质矩阵模型和单应矩阵模型各自的得分，分值计算方法为：
[0022][0023][0024]
其中，为匹配点对经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；为匹配点对在经过本质矩阵模型或单应矩阵模型在参考帧的重投影误差；计算判断指标r
h
，计算方法为：
[0025]
若r
h
＞0.45，则选择单应矩阵h恢复位姿(r,t)，否则选择本质矩阵h恢复(r,t)。
[0026]
优选的，所述特征点三角化环节的具体过程为：空间点p在初始帧第一帧f0的归一化平面里的位置为：
[0027][0028]
空间点p在初始帧第二帧f1的归一化平面里的位置为：
[0029][0030]
根据位置变化关系有：
[0031]
z
r
p
r
＝t
rw
p
w
[0032]
z
c
p
c
＝t
cw
p
w
；其中，t
rw
为初始帧的单位矩阵，t
cw
为第二帧f1和第一帧f0的相对位姿变化；由三维向量叉乘自己为零，得到：
[0033]
p
r
×
z
r
p
r
＝p
r
×
t
rw
p
w
＝0
[0034]
p
c
×
z
c
p
c
＝p
c
×
t
cw
p
w
＝0；
[0035]
整理后得到：
[0036]
最终，求解得到特征点的空间坐标p
w
。
[0037]
优选的，所述步骤3中，相机位姿求解方法为第一位姿求解子方法或第二位姿求解子方法；
[0038]
其中第一位姿求解子方法具体为：采用恒速模型，根据上一帧图像的位姿和速度得到当前帧中相机的初始位姿，相机从时刻k到时刻k
‑
1的位姿变化与相机从时刻k
‑
1到时刻k
‑
2的位姿变化相同，即：
[0039]
因此，得到：
[0040]
对上式进行递推以及取逆变换，具体为：
[0041][0042][0043][0044]
相机的运动速度即为：计算当前帧到上一帧的平移向量t，将最后一帧特征点对应的三维点重新投影到当前帧，在重新投影位置寻找特征点，按照最小化重投影误差的方法对当前帧的位姿进行优化；
[0045]
第二位姿求解子方法具体为：获得参考关键帧与当前帧匹配得到的匹配地图点和视觉标签，若匹配到的视觉标签个数大于两个，则根据视觉标签的世界位姿和视觉标签的局部坐标系与当前帧坐标系的相对位姿关系得到当前帧的初始位姿：
[0046]
然后根据最小化重投影误差的方法对当前帧的位姿进行优化，重投影误差的计算方法为：
[0047][0048]
其中，(r,t)为视觉标签角点从视觉标签局部坐标系转换到相机坐标系的变换关系，x
i
为视觉标签的角点在图像中的像素坐标，p
i
为视觉标签的角点在视觉标签局部坐标系中的坐标，π
m
为相机投影模型。
[0049]
优选的，所述步骤3中，更新地图的具体过程为：对于当前帧，若满足：至少含有一个原地图中，不存在的新的视觉标签或在当前帧中跟踪到的特征点数大于50或当前帧跟踪到的特征点与参考关键帧的重复率小于90％，则将当前帧作为关键帧添加到地图中，同时将当前帧三角化得到的地图点也添加到地图中；去除地图中的冗余关键帧的具体过程为：若地图中存在三个关键帧能够观测到本关键帧中大于90％的地图点和所有的视觉标签，则将此关键帧判断为冗余关键帧，同时从地图中去除；去除冗余地图点的具体过程为：将地图点投影至共视关键帧，提取共视关键帧投影区域所在栅格中的特征点，若特征点描述子的
汉明距离小于30，则使用共视关键帧中的相应地图点取代当前地图点。
[0050]
优选的，所述步骤4中，相机姿态系统包括相机位姿、视觉标签位姿和地图点空间位置。
[0051]
优选的，所述步骤4中，具体过程为：首先在地图中寻找与当前帧有大于50个共视地图点或大于2个共视视觉标签的关键帧以及对应的地图点和视觉标签，然后使用l
‑
m方法对在跟踪过程中具有共视关系的相机位姿、视觉标签位姿及地图点空间位置进行优化，优化方法具体为：
[0052][0053][0054]
其中，w1和w2分别表示地图点和视觉标签在优化中所占比重，p
ip
表示空间中的地图点，x
ip
表示地图点对应的像素点，p
im
表示视觉标签角点在视觉标签局部坐标系中的坐标，x
im
表示视觉标签角点对应的像素点，π
m
表示相机的投影方程，w1和w2的计算方法为：
[0055]
w1＝1
[0056][0057]
其中，m为当前帧中观测到的地图点数量，n为当前帧中观测到的先验数据标签数量。
[0058]
与现有技术相比，本发明的有益效果是：本发明所提供的定位方法，最终能够获得经过优化后的相机姿态系统，即相机位姿、视觉标签位姿及地图点空间位置，从而完成煤棚内无人机的精确定位；能够适应无人机在没有gps信号的环境中使用，能够提供稳定、安全的导航信息，保障无人机的正常运转。
附图说明
[0059]
图1为本发明的流程图；
[0060]
图2为本发明的视觉标签地图设计图；
[0061]
图3为本发明的视觉标签地图布置图。
具体实施方式
[0062]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0063]
请参阅图1
‑
3，本发明提供一种技术方案：一种基于先验视觉标签的无人机定位方法，包括如下步骤：
[0064]
步骤1、获得先验视觉标签地图；
[0065]
步骤2、对slam系统进行初始化；
[0066]
步骤3、求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；
[0067]
步骤4、对步骤3求解的相机位姿进行优化，获得最终的相机姿态系统。
[0068]
进一步的，所述步骤1中，在使用环境中布置一个包括多个二维码的地图，每个二维码具有自己独立的id和位姿信息，获得的视觉标签地图作为无人机导航的先验知识。
[0069]
进一步的，所述步骤2中，使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断slam系统是否成功求得相对位姿，若成功初始化，则进入特征点三角化环节，若失败，则使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿后再进行三角化；此后将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点插入地图，完成初始地图的更新。
[0070]
进一步的，所述基于先验视觉标签的相对位姿求解子方法的过程为：计算第一帧f0中视觉标签与第一帧f0的相对位姿γ0，第二帧f1中视觉标签与第二帧f1的相对位姿γ1，两帧之间的相对位姿t具体为：
[0071]
进一步的，所述基于特征点的相对位姿求解子方法的过程为：先计算本质矩阵模型和单应矩阵模型，本质矩阵模型计算方法为：图像的第一帧f0和第二帧f1中的一对特征点p1和p2是空间中一点p在图片中的投影，一对特征点p1和p2可以确定空间中的一点p，在第一帧f0所设坐标系下点p的空间位置为：
[0072]
p＝[x,y,z]
t
；
[0073]
影模型中，点p1和p2的像素坐标分别为：
[0074]
x0＝kp
[0075]
x1＝k(rp+t)；
[0076]
其中，k为相机的内参矩阵，根据对极约束可得：
[0077][0078]
令本质矩阵e＝t^r，然后根据点p1和p2的像素位置求解本质矩阵e；
[0079]
单应矩阵模型的计算方法为：使用dlt算法求解单应矩阵h；然后通过计算上述本质矩阵模型和单应矩阵模型各自的得分，分值计算方法为：
[0080][0081]
其中，为匹配点对经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；为匹配点对在经过本质矩阵模型或单应矩阵模型在参
考帧的重投影误差；计算判断指标r
h
，计算方法为：
[0082]
若r
h
＞0.45，则选择单应矩阵h恢复位姿(r,t)，否则选择本质矩阵h恢复(r,t)。
[0083]
进一步的，所述特征点三角化环节的具体过程为：空间点p在初始帧第一帧f0的归一化平面里的位置为：
[0084][0085]
空间点p在初始帧第二帧f1的归一化平面里的位置为：
[0086][0087]
根据位置变化关系有：
[0088]
其中，t
rw
为初始帧的单位矩阵，t
cw
为第二帧f1和第一帧f0的相对位姿变化；由三维向量叉乘自己为零，得到：
[0089]
p
r
×
z
r
p
r
＝p
r
×
t
rw
p
w
＝0
[0090]
p
c
×
z
c
p
c
＝p
c
×
t
cw
p
w
＝0；
[0091]
整理后得到：
[0092]
最终，求解得到特征点的空间坐标p
w
。
[0093]
进一步的，所述步骤3中，相机位姿求解方法为第一位姿求解子方法或第二位姿求解子方法；
[0094]
其中第一位姿求解子方法具体为：采用恒速模型，根据上一帧图像的位姿和速度得到当前帧中相机的初始位姿，相机从时刻k到时刻k
‑
1的位姿变化与相机从时刻k
‑
1到时刻k
‑
2的位姿变化相同，即：
[0095]
因此，得到：
[0096]
对上式进行递推以及取逆变换，具体为：
[0097][0098][0099][0100]
相机的运动速度即为：计算当前帧到上一帧的平移向量t，将最后一帧特征点对应的三维点重新投影到当前帧，在重新投影位置寻找特征点，按照最小化重投
影误差的方法对当前帧的位姿进行优化；
[0101]
第二位姿求解子方法具体为：获得参考关键帧与当前帧匹配得到的匹配地图点和视觉标签，若匹配到的视觉标签个数大于两个，则根据视觉标签的世界位姿和视觉标签的局部坐标系与当前帧坐标系的相对位姿关系得到当前帧的初始位姿：
[0102]
然后根据最小化重投影误差的方法对当前帧的位姿进行优化，重投影误差的计算方法为：
[0103][0104]
其中，(r,t)为视觉标签角点从视觉标签局部坐标系转换到相机坐标系的变换关系，x
i
为视觉标签的角点在图像中的像素坐标，p
i
为视觉标签的角点在视觉标签局部坐标系中的坐标，π
m
为相机投影模型。
[0105]
进一步的，所述步骤3中，更新地图的具体过程为：对于当前帧，若满足：至少含有一个原地图中，不存在的新的视觉标签或在当前帧中跟踪到的特征点数大于50或当前帧跟踪到的特征点与参考关键帧的重复率小于90％，则将当前帧作为关键帧添加到地图中，同时将当前帧三角化得到的地图点也添加到地图中；去除地图中的冗余关键帧的具体过程为：若地图中存在三个关键帧能够观测到本关键帧中大于90％的地图点和所有的视觉标签，则将此关键帧判断为冗余关键帧，同时从地图中去除；去除冗余地图点的具体过程为：将地图点投影至共视关键帧，提取共视关键帧投影区域所在栅格中的特征点，若特征点描述子的汉明距离小于30，则使用共视关键帧中的相应地图点取代当前地图点。
[0106]
进一步的，所述步骤4中，相机姿态系统包括相机位姿、视觉标签位姿和地图点空间位置。
[0107]
进一步的，所述步骤4中，具体过程为：首先在地图中寻找与当前帧有大于50个共视地图点或大于2个共视视觉标签的关键帧以及对应的地图点和视觉标签，然后使用l
‑
m方法对在跟踪过程中具有共视关系的相机位姿、视觉标签位姿及地图点空间位置进行优化，优化方法具体为：
[0108][0109][0110]
其中，w1和w2分别表示地图点和视觉标签在优化中所占比重，p
ip
表示空间中的地图点，x
ip
表示地图点对应的像素点，p
im
表示视觉标签角点在视觉标签局部坐标系中的坐
标，x
im
表示视觉标签角点对应的像素点，π
m
表示相机的投影方程，w1和w2的计算方法为：
[0111]
w1＝1
[0112][0113]
其中，m为当前帧中观测到的地图点数量，n为当前帧中观测到的先验数据标签数量。
[0114]
工作原理：步骤1中对先验视觉标签地图进行布置：和人类绘制地图一样，机器人描述环境、认识环境的过程主要就是依靠地图，通过环境地图来描述其当前环境信息。如图2所述，设计视觉标签地图，如图3所示，在环境内布置一个包括多个二维码的地图，每个二维码具有自己独立的id和位姿信息，此视觉标签地图作为无人机导航的先验知识。单目相机有尺度问题，所以需要初始化步骤，进行步骤2。由于存在已知尺寸的先验视觉标签地图，当初使图像中检测到二维码时，可将此时相机坐标系下的二维码与先验地图中的对应二维码对齐，实现参考坐标系与地图坐标系的对齐并恢复出真实尺度，将三角化得到的自然特征点包装成地图点(构造地图点，添加关键帧和点、点和关键帧之间的观测属性，计算最佳描述子，更新观测方向和距离范围，插入地图)，更新初始地图。之后步骤3中求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点。步骤4对步骤3求解的相机位姿进行优化，获得最终的相机姿态系统，相机姿态系统包括相机位姿、视觉标签位姿和地图点空间位置。
[0115]
最终获得经过优化后的相机姿态系统，即相机位姿、视觉标签位姿及地图点空间位置，从而完成煤棚内无人机的精确定位。
[0116]
同时在进行上述步骤时即时对slam地图进行维护更新，地图保存就是保存地图“map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的数据自然也就是需要保存的对象。地图主要包含关键帧、自然特征地图点、二维码、bow向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3d地图点、共视关系等元素，参考帧模型需要用到关键帧的bow向量，重定位需要用到bow向量、3d点、二维码等，上述基本元素都需要保存。
[0117]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。