一种基于P3P算法的位置参数估计方法技术

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

A method of position parameter estimation based on P3P algorithm

The invention discloses a method for estimating the location parameter based on P3P algorithm comprises the steps of: using the camera calibration parameters of the camera; the camera's current pose, the imaging of the 4 feature points in the world coordinate space and obtain 4 feature points and the image coordinates of 4 feature; points are divided into two groups, with two points of diagonal position as common feature points, 2 points respectively and the remaining public features two groups of P3P feature points; according to the characteristics of each P3P feature points in the world coordinate and image coordinate, P3P algorithm using a set of real public feature points the solutions of parameters, two sets of real numbers to obtain the corresponding feature points of the two groups of P3P solution; from two groups of real solutions selected position parameters of the greatest similarity of the two solution and check, eventually determine the positions of the feature points in public Parameter. By determining the unique solution and checking the unique solution, the efficiency and accuracy of the location parameter calculation are improved.

【技术实现步骤摘要】
一种基于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个特征点；2-2)以4个特征点中的任意一个特征点为原点建立世界坐标系，通过工具测量特征点的世界坐标；2-3)使用已标定好焦距的相机，在相机的当前使用位姿下，对合作目标进行成像获得成像图像；2-4)利用图像处理方法，从相机采集的合作目标的成像图像中提取特征点的图像坐标。本专利技术进一步设置为：所述工具包括卷尺和全站仪。本专利技术进一步设置为：所述步骤4)中的获取两组P3P特征点对应的两组实数解，具体为，4-1)选取一组P3P特征点，根据相机小孔成像模型、相机内参数模型和相机外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下，(C2-C1)T(C2-C1)＝(P2-P1)T(P2-P1)(C3-C1)T(C3-C1)＝(P3-P1)T(P3-P1)(C3-C1)T(C2-C1)＝(P3-P1)T(P2-P1)其中，C1、C2、C3分别为该组P3P特征点中3个特征点的相机坐标，P1、P2、P3分别为该组P3P特征点中3个特征点的世界坐标；4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中，得到P3P模型的三个约束方程为，其中，λ1、λ2、d1均为待解未知数，θ1、θ2、θ3分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角；4-3)利用三角函数换元法，引入中间变量x、y，通过λ1＝x+cosθ1和λ2＝y+cosθ2来简化约束方程，转化成关于x的一元四次方程，求解出x所有的实数解；对应的x的多解，求解的相机坐标系下的公共特征点坐标即位置参数存在多解，每一组P3P特征点求解出的公共特征点的位置参数为4个解以下；4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m，m≤4，其第i个解记为Ai，i∈[1,m]；将另一组P3P特征点的公共特征点的位置参数的解的个数记为n，n≤4，其第j个解记为Bj，j∈[1,n]。本专利技术进一步设置为：所述步骤5)从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数，具体为，5-1)计算两组实数解中的位置参数相似度ρ(i,j)，计算公式为，其中，分别由Ai、Bj改写成的一维向量；5-2)设置位置参数相似度阈值，根据公共特征点在相机坐标系下的位置参数具有唯一性的特点，选取两组实数解中位置参数相似度最大的两个解，并将该两个解与位置参数相似度阈值进行校验；若其位置参数相似度大于位置参数相似度阈值，则认定位置参数解算正确，选取两个解的平均值作为公共特征点的位置参数，直接将公共特征点的位置参数用于导航；否则，认定位置参数解算错误，采集下一帧图像重复步骤2)到步骤5)。与现有技术相比，本专利技术具有的有益效果是：本专利技术提高的一种基于P3P算法的位置参数估计方法，通过设置公共特征点组成两组P3P特征点来确定真值，并通过有效地简化约束方程来提高位置参数估计的实时性和稳定性，克服了P3P算法的多解和计算效率低的问题；同时，在确定唯一解的基础上，通过计算不同解的位置参数相似度对位置参数求解结果加入一个校验的环节，以提高位置参数解算的准确性，使之更具有工程实用性。上述内容仅是本专利技术技术方案的概述，为了更清楚的了解本专利技术的技术手段，下面结合附图对本专利技术作进一步的描述。附图说明图1为本专利技术实施例的4个特征点的位置示意图；图2为本专利技术实施例的P3P算法模型的示意图。具体实施方式下面结合说明书附图，对本专利技术作进一步的说明。本专利技术提供一种基于P3P算法的位置参数估计方法，包括以下步骤：1)对使用的相机进行标定，得到用于计算相机内参数模型的相机内参数，相机内参数包括相机的焦距和光心坐标；其中的标定采用张正友标定法，即张氏标定法。2)在相机的当前使用位姿下，对空间中的4个特征点进行成像获得成像图像，以4个特征点中的任意一个特征点为原点建立世界坐标系，并获取4个特征点的世界坐标及其在成像图像中的图像坐标；其中，4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求，如图1所示。具体为，2-1)在空间中布置分离式的4个合作目标，每个合作目标的中心表示一个特征点，则共有4个特征点；2-2)以4个特征点中的任意一个特征点为原点O建立世界坐标系，通过卷尺和全站仪等工具测量特征点的世界坐标；2-3)使用已标定好焦距的相机，在相机的本文档来自技高网...

【技术保护点】
一种基于P3P算法的位置参数估计方法，其特征在于，包括以下步骤：1)对使用的相机进行标定，得到用于计算相机内参数模型的相机内参数，相机内参数包括相机的焦距和光心坐标；2)在相机的当前使用位姿下，对空间中的4个特征点进行成像获得成像图像，以4个特征点中的任意一个特征点为原点建立世界坐标系，并获取4个特征点的世界坐标及其在成像图像中的图像坐标；3)将4个特征点分成两组，以4个特征点中位于对角位置的两个特征点作为公共特征点，将4个特征点中剩余的2个特征点分别与公共特征点组成均具有3个特征点的两个子集，两个子集即为两组P3P特征点；4)根据每组P3P特征点中特征点的世界坐标和图像坐标，利用P3P算法求解出公共特征点位置参数的一组实数解，获取两组P3P特征点对应的两组实数解；5)从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数。

【技术特征摘要】
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算法的位置参数估计方法，其特征在于：所述工具包括卷尺和全站仪。6.根据权利要求1所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述步骤4)中的获取两组P3P特征点对应的两组实数解，具体为，4-1)选取一组P3P特征点，根据相机小孔成像模型、相机内参数模型和相机外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下，(C2-C1)T(C2-C1)＝(P2-P1)T(P2-P1)(C3-C1)T(C3-C1)＝(P3-P1)T(P3-P1)(C3-C1)T(C2...

【专利技术属性】
技术研发人员：徐贵力，谢瑒，程月华，姜斌，郭瑞鹏，陈茂武，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32