CN111862200B - 一种煤棚内无人机定位方法 - Google Patents

Abstract

本发明涉及一种煤棚内无人机定位方法，包括步骤1：获得先验视觉标签地图；步骤2：对SLAM***进行初始化；步骤3：求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；步骤4：对步骤3求解的相机位姿进行优化，获得最终的相机姿态***，所述的相机姿态***包括相机位姿、视觉标签位姿和地图点空间位置。与现有技术相比，本发明具有定位精度高等优点。

Description

一种煤棚内无人机定位方法

技术领域

本发明涉及煤棚内无人机定位技术领域，尤其是涉及一种基于先验视觉标签及自然特征点地图的煤棚内无人机定位方法。

背景技术

在我国的能源结构中，煤占据着主导地位，而火力发电以燃煤为主。近些年来，国内外一些高等院校及科研院所相继推出了不同型号的堆料体积测量***，并且在电力、钢铁、冶金等资源型企业的堆料盘存中得到了广泛的应用，基于室内外有行车或斗轮机的激光盘煤***存在设备布线复杂，测量周期长，维护困难的缺陷，便携式激光盘煤***操作相对简便，性能比固定式盘煤仪有了很大的提高，但是整个测量过程需要运行人员参与，而且测量点的选择直接影响测量结果的准确性。随着无人机技术的发展，基于无人机平台的火电厂盘煤***也有了快速发展。旋翼飞机具有飞行速度低，可以垂直起降，飞行动作精确，操作灵活，构造简单等特点，已经应用于部分火力发电厂的盘煤工作中。

例如中国专利CN209274918U中公开了一种无人机盘煤***，包括无人机、遥控站和地面站，还包括搭载在所述无人机上的RTK定位模块和水样采集装置，其中RTK定位模块用于获取无人机的位置信息，该***具有能够自主采集水样、并获取无人机位置信息的优点。但是该***中的无人机定位模块的定位精度较低，在GNSS信号微弱或者GNSS信号不存在的情况下无法为盘煤***无人机提供稳定、安全的导航信息。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种高定位精度的煤棚内无人机定位方法。

本发明的目的可以通过以下技术方案来实现：

一种煤棚内无人机定位方法，包括：

步骤1：获得先验视觉标签地图；

步骤2：对SLAM***进行初始化；

步骤3：求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；

步骤4：对步骤3求解的相机位姿进行优化，获得最终的相机姿态***，所述的相机姿态***包括相机位姿、视觉标签位姿和地图点空间位置。

优选地，所述的步骤1中先验视觉标签地图具体为：

煤棚内布置的先验视觉标签地图，该地图内包括多个二维码，每个二维码具有独立的ID和位姿信息。

优选地，所述的步骤2具体为：

步骤2-1：使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断SLAM***是否成功求得相对位姿，若是，则执行步骤2-3，否则，执行步骤2-2；

步骤2-2：使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，然后执行步骤2-3：

步骤2-3：对特征点进行三角化；

步骤2-4：将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点***地图，完成初始地图的更新。

更加优选地，所述的基于先验视觉标签相对位姿求解子方法具体为：

计算第一帧f₀中视觉标签与第一帧f₀的相对位姿γ₀，第二帧f₁中视觉标签与第二帧f₁的相对位姿γ₁，两帧之间的相对位姿T具体为：T＝γ₀ ^-1*γ₁。

更加优选地，所述的基于特征点的相对位姿求解子方法具体为：

首先计算本质矩阵模型和单应矩阵模型；

所述的本质矩阵模型计算方法为：图像的第一帧f₀和第二帧f₁中的一对特征点p₁和p₂是空间中一点P在图片中的投影，一对特征点p₁和p₂可以确定空间中的一点P，在第一帧f₀所设坐标系下点P的空间位置为：

P＝[X,Y,Z]^T

在相机投影模型中，点p₁和p₂的像素坐标分别为：

x₀＝KP

x₁＝K(RP+t)

其中，K为相机的内参矩阵，根据对极约束可得：

x₁ ^TK^-Tt^RK^-1x₀＝0

令本质矩阵E＝t^R，然后根据点p₁和p₂的像素位置求解本质矩阵E；

所述的单应矩阵模型的计算方法为：使用DLT算法求解单应矩阵H；

然后通过计算上述本质矩阵模型和单应矩阵模型各自的得分，分值计算方法为：

其中，

为匹配点对

经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；

为匹配点对

在经过本质矩阵模型或单应矩阵模型在参考帧的重投影误差；

计算判断指标R_H，计算方法为：

若R_H＞0.45，则选择单应矩阵H恢复位姿(R,t)，否则选择本质矩阵E恢复(R,t)。

更加优选地，所述的步骤2-3具体为：

空间点P在初始帧第一帧f₀的归一化平面里的位置为：

空间点P在当前帧第二帧f₁的归一化平面中的位置为：

根据位置变化关系有：

z_rp_r＝T_rwP_w

z_cp_c＝T_cwP_w

其中，T_rw为初始帧的单位矩阵，T_cw为第二帧f₁和第一帧f₀的相对位姿变化；

由三维向量叉乘自己为零，得到：

p_r×z_rp_r＝p_r×T_rwP_w＝0

p_c×z_cp_c＝p_c×T_cwP_w＝0

整理后，得到：

最终，求解得到特征点的空间坐标P_w。

更加优选地，所述的步骤3中相机位姿求解方法为第一位姿求解子方法或第二位姿求解子方法；

所述的第一位姿求解子方法具体为：

采用恒速模型，根据上一帧图像的位姿和速度得到当前帧中相机的初始位姿，相机从时刻k到时刻k-1的位姿变化与相机从时刻k-1到时刻k-2的位姿变化相同，即：

因此，得到：

对上式进行递推以及取逆变换，具体为：

相机的运动速度即为：

计算当前帧到上一帧的平移向量t，将最后一帧特征点对应的三维点重新投影到当前帧，在重新投影位置寻找特征点，按照最小化重投影误差的方法对当前帧的位姿进行优化；

所述的第二位姿求解子方法具体为：

获得参考关键帧与当前帧匹配得到的匹配地图点和视觉标签，若匹配到的视觉标签个数大于两个，则根据视觉标签的世界位姿

和视觉标签的局部坐标系与当前帧坐标系的相对位姿关系

得到当前帧的初始位姿：

然后根据最小化重投影误差的方法对当前帧的位姿进行优化；

所述的重投影误差的计算方法为：

其中，(R,t)为视觉标签角点从视觉标签局部坐标系转换到相机坐标系的变换关系；x_i为视觉标签的角点在图像中的像素坐标；p_i为视觉标签的角点在视觉标签局部坐标系中的坐标；π_m为相机投影模型。

优选地，所述的步骤3中更新地图的具体方法为：

对于当前帧，若满足：至少含有一个原地图中不存在的新的视觉标签或在当前帧中跟踪到的特征点数大于50或当前帧跟踪到的特征点与参考关键帧的重复率小于90％，则将当前帧作为关键帧添加到地图中，同时将当前帧三角化得到的地图点也添加到地图中。

优选地，所述的去除地图中的冗余关键帧的方法具体为：

若地图中存在三个关键帧能够观测到本关键帧中大于90％的地图点和所有的视觉标签，则将此关键帧判断为冗余关键帧，同时从地图中去除；

所述的去除冗余地图点的方法具体为：

将地图点投影至共视关键帧，提取共视关键帧投影区域所在栅格中的特征点，若特征点描述子的汉明距离小于30，则使用共视关键帧中的相应地图点取代当前地图点。

优选地，所述的步骤4具体为：

首先在地图中寻找与当前帧有大于50个共视地图点或大于2个共视视觉标签的关键帧以及对应的地图点和视觉标签，然后使用L-M方法对在跟踪过程中具有共视关系的相机位姿、视觉标签位姿及地图点空间位置进行优化，优化方法具体为：

其中，w₁和w₂分别表示地图点和视觉标签在优化中所占比重；p_i ^p表示空间中的地图点；x_i ^p表示地图点对应的像素点；p_i ^m表示视觉标签角点在视觉标签局部坐标系中的坐标；x_i ^m表示视觉标签角点对应的像素点；π_m表示相机的投影方程；

所述的w₁和w₂的计算方法为：

w₁＝1

其中，m为当前帧中观测到的地图点数量；n为当前帧中观测到的先验数据标签数量。

与现有技术相比，本发明具有以下优点：

定位精度更高：本发明中的煤棚内无人机定位方法通过先验视觉标签地图来确定无人机的真实位姿，由于先验视觉标签的尺寸和位姿是已知的，所以通过先验视觉标签确定的无人机位姿的准确度更高，无人机的定位精度也更高。

附图说明

图1为本发明中煤棚内无人机定位方法的流程示意图；

图2为本发明实施例中先验视觉标签的布置图；

图3为本发明实施例中SLAM地图的示意图；

图4为本发明实施例中无人机的路径规划示意图；

图5为本发明实施例中使用本发明中无人机定位方法与使用基于GPS的无人机定位方法在X轴上的轨迹对比图；

图6为本发明实施例中使用本发明中无人机定位方法与使用基于GPS的无人机定位方法在Y轴上的轨迹对比图；

图7为本发明实施例中使用本发明中无人机定位方法与使用基于GPS的无人机定位方法在Z轴上的轨迹对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

一种煤棚内无人机定位方法，其流程如图1所示，包括：

步骤1：获得先验视觉标签地图；

和人类绘制地图一样，机器人描述环境、认识环境的过程主要就是依靠地图，通过环境地图来描述其当前环境信息。如图2所示，在煤棚内布置一个包括多个二维码的地图，每个二维码具有自己独立的ID和位姿信息，此视觉标签地图作为无人机导航的先验知识；

步骤2：对SLAM***进行初始化，具体为：

步骤2-1：使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断SLAM***是否成功求得相对位姿，若是，则执行步骤2-3，否则，执行步骤2-2；

步骤2-2：使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，然后执行步骤2-3：

基于先验视觉标签相对位姿求解子方法具体为：

计算第一帧f₀中视觉标签与第一帧f₀的相对位姿γ₀，第二帧f₁中视觉标签与第二帧f₁的相对位姿γ₁，两帧之间的相对位姿T具体为：T＝γ₀ ^-1*γ₁；

基于特征点的相对位姿求解子方法具体为：

首先计算本质矩阵模型和单应矩阵模型；

所述的本质矩阵模型计算方法为：图像的第一帧f₀和第二帧f₁中的一对特征点p₁和p₂是空间中一点P在图片中的投影，一对特征点p₁和p₂可以确定空间中的一点P，在第一帧f₀所设坐标系下点P的空间位置为：

P＝[X,Y,Z]^T

在相机投影模型中，点p₁和p₂的像素坐标分别为：

x₀＝KP

x₁＝K(RP+t)

其中，K为相机的内参矩阵，根据对极约束可得：

x₁ ^TK^-Tt^RK^-1x₀＝0

令本质矩阵E＝t^R，然后根据点p₁和p₂的像素位置求解本质矩阵E；

所述的单应矩阵模型的计算方法为：使用DLT算法求解单应矩阵H；

然后通过计算上述本质矩阵模型和单应矩阵模型各自的得分，分值计算方法为：

其中，

为匹配点对

经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；

为匹配点对

在经过本质矩阵模型或单应矩阵模型在参考帧的重投影误差；

计算判断指标R_H，计算方法为：

若R_H＞0.45，则选择单应矩阵H恢复位姿(R,t)，否则选择本质矩阵E恢复(R,t)；

步骤2-3：对特征点进行三角化，具体步骤为：

空间点P在初始帧第一帧f₀的归一化平面里的位置为：

空间点P在当前帧第二帧f₁的归一化平面中的位置为：

根据位置变化关系有：

z_rp_r＝T_rwP_w

z_cp_c＝T_cwP_w

其中，T_rw为初始帧的单位矩阵，T_cw为第二帧f₁和第一帧f₀的相对位姿变化；

由三维向量叉乘自己为零，得到：

p_r×z_rp_r＝p_r×T_rwP_w＝0

p_c×z_cp_c＝p_c×T_cwP_w＝0

整理后，得到：

最终，求解得到特征点的空间坐标P_w。

步骤2-4：将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点***地图，完成初始地图的更新。

步骤3：求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；

使用两种方式求解位姿，即使用第一位姿求解子方法或第二位姿求解子方法求解位姿。

第一位姿求解子方法具体为：

采用恒速模型，根据上一帧图像的位姿和速度得到当前帧中相机的初始位姿，相机从时刻k到时刻k-1的位姿变化与相机从时刻k-1到时刻k-2的位姿变化相同，即：

因此，得到：

对上式进行递推以及取逆变换，具体为：

相机的运动速度即为：

计算当前帧到上一帧的平移向量t，将最后一帧特征点对应的三维点重新投影到当前帧，在重新投影位置寻找特征点，按照最小化重投影误差的方法对当前帧的位姿进行优化；

第二位姿求解子方法具体为：

获得参考关键帧与当前帧匹配得到的匹配地图点和视觉标签，若匹配到的视觉标签个数大于两个，则根据视觉标签的世界位姿

和视觉标签的局部坐标系与当前帧坐标系的相对位姿关系

得到当前帧的初始位姿：

然后根据最小化重投影误差的方法对当前帧的位姿进行优化；

所述的重投影误差的计算方法为：

其中，(R,t)为视觉标签角点从视觉标签局部坐标系转换到相机坐标系的变换关系；x_i为视觉标签的角点在图像中的像素坐标；p_i为视觉标签的角点在视觉标签局部坐标系中的坐标；π_m为相机投影模型；

更新地图的具体方法为：

对于当前帧，若满足：

(1)至少含有一个原地图中不存在的新的视觉标签或在当前帧中跟踪到的特征点数大于50；

(2)当前帧跟踪到的特征点与参考关键帧的重复率小于90％；

满足上述两个条件中的任意一个，则将当前帧作为关键帧添加到地图中，同时将当前帧三角化得到的地图点也添加到地图中。

去除地图中的冗余关键帧的方法具体为：

若地图中存在三个关键帧能够观测到本关键帧中大于90％的地图点和所有的视觉标签，则将此关键帧判断为冗余关键帧，同时从地图中去除；

所述的去除冗余地图点的方法具体为：

将地图点投影至共视关键帧，提取共视关键帧投影区域所在栅格中的特征点，若特征点描述子的汉明距离小于30，则使用共视关键帧中的相应地图点取代当前地图点。

步骤4：对步骤3求解的相机位姿进行优化，获得最终的相机姿态***，包括相机位姿、视觉标签位姿和地图点的空间位置，具体为：

首先在地图中寻找与当前帧有大于50个共视地图点或大于2个共视视觉标签的关键帧以及对应的地图点和视觉标签，然后使用L-M方法对在跟踪过程中具有共视关系的相机姿态***，即相机位姿、视觉标签位姿及地图点空间位置进行优化，优化方法具体为：

其中，w₁和w₂分别表示地图点和视觉标签在优化中所占比重；p_i ^p表示空间中的地图点；x_i ^p表示地图点对应的像素点；p_i ^m表示视觉标签角点在视觉标签局部坐标系中的坐标；x_i ^m表示视觉标签角点对应的像素点；π_m表示相机的投影方程；

所述的w₁和w₂的计算方法为：

w₁＝1

其中，m为当前帧中观测到的地图点数量；n为当前帧中观测到的先验数据标签数量。

最终获得经过优化后的相机姿态***，即相机位姿、视觉标签位姿及地图点空间位置，从而完成煤棚内无人机的精确定位。

同时在进行上述步骤时即时对SLAM地图进行维护更新，地图保存就是保存地图“Map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的数据自然也就是需要保存的对象。地图主要包含关键帧、自然特征地图点、二维码、BoW向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3D地图点、共视关系等元素，参考帧模型需要用到关键帧的BoW向量，重定位需要用到BoW向量、3D点、二维码等，上述基本元素都需要保存。

本实施例中无人机构建的SLAM地图如图3所示，地图元素包括视觉标签、三维地图点以及关键帧，随着无人机的运动，地图中不断添加新的元素；同时地图具有保存功能，在相同的场景进行多次运动可使用之前保存的地图作为先验信息；地图中的视觉标签元素可以给无人机的定位带来一个强约束，当在图像中检测到对应的视觉标签时，会同时优化无人机和视觉标签的位姿。无人机根据先验视觉标签规划出的路径如图4所示，可通过视觉标签构建世界坐标系并进行路径规划。同时在室外的草地贫纹理场景对本实施例中的无人机定位方法以及基于GPS的无人机定位方法所确定的无人机运行轨迹进行了对比，示意图如图5、图6和图7所示，由图可以看出。使用本申请中的无人机定位方法输出的无人机轨迹与GPS的输出的无人机轨迹基本吻合，无人机在自主飞行过程中可以实时进行高精度的定位，在x轴、y轴、z轴的平均距离误差为0.072m、0.107m和0.206m，同时尺度信息与真实世界相吻合，并且对于煤棚、草地、工厂等传统视觉SLAM***无法工作的贫纹理场景，本发明仍可实现高精度的定位。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种煤棚内无人机定位方法，其特征在于，包括：

步骤1：获得先验视觉标签地图；

步骤2：对SLAM***进行初始化；

步骤3：求解相机位姿并同时更新地图，然后去除地图中的冗余关键帧和冗余地图点；

步骤4：对步骤3求解的相机位姿进行优化，获得最终的相机姿态***，所述的相机姿态***包括相机位姿、视觉标签位姿和地图点空间位置；

所述的步骤4具体为：

首先在地图中寻找与当前帧有大于50个共视地图点或大于2个共视视觉标签的关键帧以及对应的地图点和视觉标签，然后使用L-M方法对在跟踪过程中具有共视关系的相机位姿、视觉标签位姿及地图点空间位置进行优化，优化方法具体为：

其中，w₁和w₂分别表示地图点和视觉标签在优化中所占比重；p_i ^p表示空间中的地图点；x_i ^p表示地图点对应的像素点；p_i ^m表示视觉标签角点在视觉标签局部坐标系中的坐标；x_i ^m表示视觉标签角点对应的像素点；π_m表示相机的投影方程；

表示视觉标签的世界位姿，

表示当前帧的初始位姿；

所述的w₁和w₂的计算方法为：

w₁＝1

其中，m为当前帧中观测到的地图点数量；n为当前帧中观测到的先验数据标签数量；

所述的步骤2具体为：

步骤2-1：使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断SLAM***是否成功求得相对位姿，若是，则执行步骤2-3，否则，执行步骤2-2；

步骤2-2：使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，然后执行步骤2-3：

步骤2-3：对特征点进行三角化；

步骤2-4：将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点***地图，完成初始地图的更新。

2.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的步骤1中先验视觉标签地图具体为：

煤棚内布置的先验视觉标签地图，该地图内包括多个二维码，每个二维码具有独立的ID和位姿信息。

3.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的基于先验视觉标签相对位姿求解子方法具体为：

计算第一帧f₀中视觉标签与第一帧f₀的相对位姿γ₀，第二帧f₁中视觉标签与第二帧f₁的相对位姿γ₁，两帧之间的相对位姿T具体为：T＝γ₀ ^-1*γ₁。

4.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的基于特征点的相对位姿求解子方法具体为：

首先计算本质矩阵模型和单应矩阵模型；

所述的本质矩阵模型计算方法为：图像的第一帧f₀和第二帧f₁中的一对特征点p₁和p₂是空间中一点P在图片中的投影，一对特征点p₁和p₂可以确定空间中的一点P，在第一帧f₀所设坐标系下点P的空间位置为：

P＝[X,Y,Z]^T

在相机投影模型中，点p₁和p₂的像素坐标分别为：

x₀＝KP

x₁＝K(RP+t)

其中，K为相机的内参矩阵，根据对极约束可得：

x₁ ^TK^-Tt^RK^-1x₀＝0

令本质矩阵E＝t^R，然后根据点p₁和p₂的像素位置求解本质矩阵E；

所述的单应矩阵模型的计算方法为：使用DLT算法求解单应矩阵H；

然后通过计算上述本质矩阵模型和单应矩阵模型各自的得分，分值计算方法为：

其中，

为匹配点对

经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；

为匹配点对

在经过本质矩阵模型或单应矩阵模型在参考帧的重投影误差；

计算判断指标R_H，计算方法为：

若R_H＞0.45，则选择单应矩阵H恢复位姿(R,t)，否则选择本质矩阵E恢复(R,t)。

5.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的步骤2-3具体为：

空间点P在初始帧第一帧f₀的归一化平面里的位置为：

空间点P在当前帧第二帧f₁的归一化平面中的位置为：

根据位置变化关系有：

z_rp_r＝T_rwP_w

z_cp_c＝T_cwP_w

其中，T_rw为初始帧的单位矩阵，T_cw为第二帧f₁和第一帧f₀的相对位姿变化；

由三维向量叉乘自己为零，得到：

p_r×z_rp_r＝p_r×T_rwP_w＝0

p_c×z_cp_c＝p_c×T_cwP_w＝0

整理后，得到：

最终，求解得到特征点的空间坐标P_w。

6.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的步骤3中更新地图的具体方法为：

对于当前帧，若满足：至少含有一个原地图中不存在的新的视觉标签或在当前帧中跟踪到的特征点数大于50或当前帧跟踪到的特征点与参考关键帧的重复率小于90％，则将当前帧作为关键帧添加到地图中，同时将当前帧三角化得到的地图点也添加到地图中。

7.根据权利要求1所述的一种煤棚内无人机定位方法，其特征在于，所述的去除地图中的冗余关键帧的方法具体为：

若地图中存在三个关键帧能够观测到本关键帧中大于90％的地图点和所有的视觉标签，则将此关键帧判断为冗余关键帧，同时从地图中去除；

所述的去除冗余地图点的方法具体为：

将地图点投影至共视关键帧，提取共视关键帧投影区域所在栅格中的特征点，若特征点描述子的汉明距离小于30，则使用共视关键帧中的相应地图点取代当前地图点。

Images