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

权利要求书

1.一种基于P3P算法的位置参数估计方法，其特征在于，包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述步骤1)中的标定采用张正友标定法。

3.根据权利要求1所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述步骤2)中的4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求。

4.根据权利要求1所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述步骤2)中的获取4个特征点的世界坐标及其在成像图像中的图像坐标，具体为，

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

2-2)以4个特征点中的任意一个特征点为原点建立世界坐标系，通过工具测量特征点的世界坐标；

2-3)使用已标定好焦距的相机，在相机的当前使用位姿下，对合作目标进行成像获得成像图像；

2-4)利用图像处理方法，从相机采集的合作目标的成像图像中提取特征点的图像坐标。

5.根据权利要求4所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述工具包括卷尺和全站仪。