[发明专利]一种基于先验视觉标签的无人机定位方法

权利要求书

1.一种基于先验视觉标签的无人机定位方法，其特征在于：包括如下步骤：

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

步骤2、对SLAM系统进行初始化；

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

步骤4、对步骤3求解的相机位姿进行优化，获得最终的相机姿态系统。

2.根据权利要求1所述的一种基于先验视觉标签的无人机定位方法，其特征在于：所述步骤1中，在使用环境中布置一个包括多个二维码的地图，每个二维码具有自己独立的ID和位姿信息，获得的视觉标签地图作为无人机导航的先验知识。

3.根据权利要求1所述的一种基于先验视觉标签的无人机定位方法，其特征在于：所述步骤2中，使用基于先验视觉标签的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿，并判断SLAM系统是否成功求得相对位姿，若成功初始化，则进入特征点三角化环节，若失败，则使用基于特征点的相对位姿求解子方法求解图像第一帧与第二帧之间的相对位姿后再进行三角化；此后将三角化得到的特征点构造为地图点，同时添加关键帧与点以及点与关键帧之间的观测属性，并计算最佳描述子，更新观测方向和距离范围，将新的地图点插入地图，完成初始地图的更新。

4.根据权利要求3所述的一种基于先验视觉标签的无人机定位方法，其特征在于：所述基于先验视觉标签的相对位姿求解子方法的过程为：计算第一帧f₀中视觉标签与第一帧f₀的相对位姿γ₀，第二帧f₁中视觉标签与第二帧f₁的相对位姿γ₁，两帧之间的相对位姿T具体为：

5.根据权利要求3所述的一种基于先验视觉标签的无人机定位方法，其特征在于：所述基于特征点的相对位姿求解子方法的过程为：先计算本质矩阵模型和单应矩阵模型，本质矩阵模型计算方法为：图像的第一帧f₀和第二帧f₁中的一对特征点p₁和p₂是空间中一点P在图片中的投影，一对特征点p₁和p₂可以确定空间中的一点P，在第一帧f₀所设坐标系下点P的空间位置为：

P＝[X,Y,Z]^T；

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

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

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

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

其中，为匹配点对经过本质矩阵模型或单应矩阵模型在当前帧的重投影误差；为匹配点对在经过本质矩阵模型或单应矩阵模型在参考帧的重投影误差；计算判断指标R_H，计算方法为：

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

6.根据权利要求3所述的一种基于先验视觉标签的无人机定位方法，其特征在于：所述特征点三角化环节的具体过程为：空间点P在初始帧第一帧f₀的归一化平面里的位置为：

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

根据位置变化关系有：

其中，T_rw为初始帧的单位矩阵，T_cw为第二帧f₁和第一帧f₀的相对位姿变化；由三维向量叉乘自己为零，得到：

整理后得到：

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