[发明专利]辅助信息与随机抽样一致算法融合的无人机位姿估计方法

权利要求书

1.辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：包括以下步骤：

S1：利用GPS和IMU系统提供的无人机的辅助信息计算出本质矩阵；辅助信息包括位置和姿态信息；

S2：无人机在不同角度对同一目标区域拍摄，利用SURF算法对相邻时刻拍摄的两幅航拍图像进行特征点提取与匹配，同时获得对应的匹配点集D；

S3：先将步骤S1获得的本质矩阵作为辅助信息与随机抽样一致算法相融合，初步剔除匹配点集D中存在的误匹配点；再利用随机抽样一致算法结合五点算法对本质矩阵进行迭代求解，最终得到满足精度要求的本质矩阵；

S4：对步骤S3中获得的本质矩阵进行SVD分解得到各相邻时刻航拍图像间摄像机的平移向量和旋转矩阵，利用旋转矩阵求解无人机的三个姿态角，最终实现无人机的位姿估计。

2.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：步骤S1中所述本质矩阵通过平移向量T和旋转矩阵R求得，平移向量T和旋转矩阵R分别由式(1)、(2)得到：

T＝[x₁-x₀ y₁-y₀ z₁-z₀]^T＝[T_x T_y T_z]^T (1)

式(1)中，(x₀ y₀ z₀)为GPS系统输出的无人机首次拍摄获取图片I₀时对应的无人机位置点坐标，(x₁ y₁ z₁)为GPS系统输出的无人机拍摄获取图片I₁时对应的无人机位置点坐标；T_x表示无人机在拍摄I₀、I₁的两相邻时刻位置点之间沿X轴方向的平移向量，T_y表示无人机在拍摄I₀、I₁的两相邻时刻位置点之间沿Y轴方向的平移向量，T_z表示无人机在拍摄I₀、I₁的两相邻时刻位置点之间沿Z轴方向的平移向量；

式(2)中，R_ψ表示无人机在相邻时刻两位置点间绕X轴旋转的旋转矩阵，R_θ表示无人机在相邻时刻两位置点间绕Y轴旋转的旋转矩阵，表示无人机在相邻时刻两位置点间绕Z轴旋转的旋转矩阵，ψ为偏航角，θ为俯仰角，为横滚角。

3.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S3具体包括以下步骤：

S3.1：对步骤S2获得的匹配点集D中的所有匹配点进行归一化处理；

S3.2：根据式(3)计算出第n次迭代第i对匹配点rⁱ与经本质矩阵E_n变化后的对应的匹配点rⁱ'之间的距离

将与预先设置好的阈值L比较：时，记此点为内点；否则，记此点为外点；从而获得内点集合D_n；若内点数大于前次循环获得的内点数，则更新本质矩阵E；

S3.3：根据式(4)利用随机抽样一致算法计算满足置信率P的本质矩阵的最小迭代次数N：

式(4)中，ε为外点概率，S为每个样本中的匹配点的对数；

S3.4：利用五点算法计算本质矩阵，也即随机抽取S＝5对匹配点计算出本质矩阵E；

S3.5：重复步骤S3.2-S3.4，迭代N次后获得最优本质矩阵。

4.根据权利要求3所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S3.2中，本质矩阵E更新过程中的初值由GPS和IMU系统提供的无人机位姿信息来提供。

5.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S4具体包括以下步骤：

S4.1：对步骤S3中获得的本质矩阵进行SVD分解，得到旋转矩阵R和平移向量T的初步解：R＝UWV^T或者R＝UW^TV^T；T＝u₃或者T＝-u₃；U和V为3×3的正交矩阵；U内部的向量是正交的，称为左奇异向量，U的列为EE^T的正交特征向量；V内部的向量是正交的，称为右奇异向量，V的列为E^TE的正交特征向量；E＝Udiag(σ,σ,0)V^T，σ为本质矩阵的奇异值，u₃为矩阵U的最后一列，

S4.2：对经去除误匹配点之后最终获取的匹配点集中的特征点进行三维重建，通过三维点深度值为正来确定旋转矩阵R和平移向量T的唯一解；

S4.3：利用旋转矩阵R求解出无人机的三个姿态角：偏航角ψ、俯仰角θ、横滚角