[发明专利]一种基于P3P算法的位置参数估计方法

说明书

本发明公开了一种基于P3P算法的位置参数估计方法，包括步骤：对使用的相机进行标定得到相机参数；在相机的当前使用位姿下，对空间中的4个特征点进行成像并获取4个特征点的世界坐标及其图像坐标；将4个特征点分成两组，以对角位置的两个特征点作为公共特征点，将剩余的2个特征点分别与公共特征点组成两组P3P特征点；根据每组P3P特征点中特征点的世界坐标和图像坐标，利用P3P算法求解出公共特征点位置参数的一组实数解，获取两组P3P特征点对应的两组实数解；从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数。通过确定唯一解并对唯一解进行校验，以提高位置参数解算的效率和准确性。

技术领域

本发明涉及一种位置参数估计方法，特别是涉及一种基于P3P算法的位置参数估计方法，属于计算机视觉技术领域。

背景技术

基于视觉的位姿估计技术是计算机视觉领域中的研究热点之一，是视觉定位及导航技术的重要环节。基于视觉的位姿估计技术，通过相机捕获空间合作目标图像，通过图像处理和位姿估计算法求解相机位姿参数，具有精度适中、成本低、使用独立灵活及抗电磁干扰的特点。

PnP算法作为常用的位姿测量方法，以空间中相对位置已知的n(n≥3)个点作为特征点，由摄像机采集一幅图像，计算摄像机相对于特征点的位姿信息。在实际应用中，特征点数越多，在相机与目标之间发生相对运动时，特征点容易超出成像图像范围，以及特征点匹配出现错误，最终导致解算失败。因此使用的特征点数越少，解算越灵活，一般使用P3P算法和P4P算法。

虽然，P3P算法精度与P4P算法精度相差不大；但是，P3P算法根据3个特征点最多可以解算出四组解，P4P算法根据4个特征点可以解得唯一解。然而，在图像处理阶段如果出现某个特征点提取错误，导致位姿解算错误，则该算法无法进行自我校验。因此，需要找到一种所需特征点数少、可以确定唯一解并且能够对唯一解进行校验的位姿解算算法。

发明内容

本发明的主要目的在于，克服现有技术中的不足，提供一种基于P3P算法的位置参数估计方法，不仅计算效率高，而且可提高位置参数解算的准确性，可应用于采用分离式合作目标引导无人机视觉着陆领域。

为了达到上述目的，本发明所采用的技术方案是：

一种基于P3P算法的位置参数估计方法，包括以下步骤：

1)对使用的相机进行标定，得到用于计算相机内参数模型的相机内参数，相机内参数包括相机的焦距和光心坐标；

2)在相机的当前使用位姿下，对空间中的4个特征点进行成像获得成像图像，以4个特征点中的任意一个特征点为原点建立世界坐标系，并获取4个特征点的世界坐标及其在成像图像中的图像坐标；

3)将4个特征点分成两组，以4个特征点中位于对角位置的两个特征点作为公共特征点，将4个特征点中剩余的2个特征点分别与公共特征点组成均具有3个特征点的两个子集，两个子集即为两组P3P特征点；

4)根据每组P3P特征点中特征点的世界坐标和图像坐标，利用P3P算法求解出公共特征点位置参数的一组实数解，获取两组P3P特征点对应的两组实数解；

5)从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数。

本发明进一步设置为：所述步骤1)中的标定采用张正友标定法。

本发明进一步设置为：所述步骤2)中的4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求。

本发明进一步设置为：所述步骤2)中的获取4个特征点的世界坐标及其在成像图像中的图像坐标，具体为，

2-1)在空间中布置分离式的4个合作目标，每个合作目标的中心表示一个特征点，则共有4个特征点；