CN106991705A - 一种基于p3p算法的位置参数估计方法 - Google Patents

Abstract

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

Description

一种基于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个特征点关于世界坐标和相机坐标的关系式如下，

(C₂-C₁)^T(C₂-C₁)＝(P₂-P₁)^T(P₂-P₁)

(C₃-C₁)^T(C₃-C₁)＝(P₃-P₁)^T(P₃-P₁)

(C₃-C₁)^T(C₂-C₁)＝(P₃-P₁)^T(P₂-P₁)

其中，C₁、C₂、C₃分别为该组P3P特征点中3个特征点的相机坐标，P₁、P₂、P₃分别为该组P3P特征点中3个特征点的世界坐标；

4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中，得到P3P模型的三个约束方程为，

其中，λ₁、λ₂、d₁均为待解未知数，θ₁、θ₂、θ₃分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角；

4-3)利用三角函数换元法，引入中间变量x、y，通过λ₁＝x+cosθ₁和λ₂＝y+cosθ₂来简化约束方程，转化成关于x的一元四次方程，求解出x所有的实数解；

对应的x的多解，求解的相机坐标系下的公共特征点坐标即位置参数存在多解，每一组P3P特征点求解出的公共特征点的位置参数为4个解以下；

4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m，m≤4，其第i个解记为A_i，i∈[1,m]；将另一组P3P特征点的公共特征点的位置参数的解的个数记为n，n≤4，其第j个解记为B_j，j∈[1,n]。

本发明进一步设置为：所述步骤5)从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数，具体为，

5-1)计算两组实数解中的位置参数相似度ρ(i,j)，计算公式为，

其中，分别由A_i、B_j改写成的一维向量；

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)使用已标定好焦距的相机，在相机的当前使用位姿下，对合作目标进行成像获得成像图像；

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

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

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

具体为，

4-1)选取一组P3P特征点，根据相机小孔成像模型、相机内参数模型和外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下，

(C₂-C₁)^T(C₂-C₁)＝(P₂-P₁)^T(P₂-P₁)

(C₃-C₁)^T(C₃-C₁)＝(P₃-P₁)^T(P₃-P₁)

(C₃-C₁)^T(C₂-C₁)＝(P₃-P₁)^T(P₂-P₁)

其中，C₁、C₂、C₃分别为该组P3P特征点中3个特征点的相机坐标，相机坐标根据步骤2)中获得的特征点图像坐标与步骤1)标定得到的相机参数计算得到，P₁、P₂、P₃分别为该组P3P特征点中3个特征点的世界坐标；

4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中，得到P3P模型的三个约束方程为，

其中，λ₁、λ₂、d₁均为未知数，θ₁、θ₂、θ₃分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角，其为图2所示的P3P算法模型示意图中向量与与与之间的夹角；

4-3)利用三角函数换元法，引入中间变量x、y，通过λ₁＝x+cosθ₁和λ₂＝y+cosθ₂来简化约束方程，转化成关于x的一元四次方程，求解出x所有的实数解；

对应的x的多解，求解的相机坐标系下的公共特征点坐标即位置参数存在多解，每一组P3P特征点求解出的公共特征点的位置参数为4个解以下；

4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m，m≤4，其第i个解记为A_i，i∈[1,m]；将另一组P3P特征点的公共特征点的位置参数的解的个数记为n，n≤4，其第j个解记为B_j，j∈[1,n]。

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

具体为，

5-1)计算两组实数解中的位置参数相似度ρ(i,j)，计算公式为，

其中，分别由A_i、B_j改写成的一维向量；

5-2)设置位置参数相似度阈值，根据公共特征点在相机坐标系下的位置参数具有唯一性的特点，选取两组实数解中位置参数相似度最大的两个解，并将该两个解与位置参数相似度阈值进行校验；

若其位置参数相似度大于位置参数相似度阈值，则认定位置参数解算正确，选取两个解的平均值作为公共特征点的位置参数，直接将公共特征点的位置参数用于导航；否则，认定位置参数解算错误，采集下一帧图像重复步骤2)到步骤5)。

本发明的创新点在于，基于P3P算法求解位姿出现多解的情况下，通过确定唯一解并对唯一解进行校验，以提高位置参数解算的效率和准确性。

以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (7)

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个特征点关于世界坐标和相机坐标的关系式如下，

(C₂-C₁)^T(C₂-C₁)＝(P₂-P₁)^T(P₂-P₁)

(C₃-C₁)^T(C₃-C₁)＝(P₃-P₁)^T(P₃-P₁)

(C₃-C₁)^T(C₂-C₁)＝(P₃-P₁)^T(P₂-P₁)

其中，C₁、C₂、C₃分别为该组P3P特征点中3个特征点的相机坐标，P₁、P₂、P₃分别为该组P3P特征点中3个特征点的世界坐标；

4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中，得到P3P模型的三个约束方程为，

d₁ ²(λ₁ ²-2λ₁cosθ₁+1)＝(P₂-P₁)^T(P₂-P₁)

d₁ ²(λ₂ ²-2λ₂cosθ₂+1)＝(P₃-P₁)^T(P₃-P₁)

d₁ ²(λ₁λ₂cosθ₃-λ₁cosθ₁-λ₂cosθ₂+1)＝(P₃-P₁)^T(P₂-P₁)

其中，λ₁、λ₂、d₁均为待解未知数，θ₁、θ₂、θ₃分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角；

4-3)利用三角函数换元法，引入中间变量x、y，通过λ₁＝x+cosθ₁和λ₂＝y+cosθ₂来简化约束方程，转化成关于x的一元四次方程，求解出x所有的实数解；

对应的x的多解，求解的相机坐标系下的公共特征点坐标即位置参数存在多解，每一组P3P特征点求解出的公共特征点的位置参数为4个解以下；

4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m，m≤4，其第i个解记为A_i，i∈[1,m]；将另一组P3P特征点的公共特征点的位置参数的解的个数记为n，n≤4，其第j个解记为B_j，j∈[1,n]。

7.根据权利要求6所述的一种基于P3P算法的位置参数估计方法，其特征在于：所述步骤5)从两组实数解中选取位置参数相似度最大的两个解并进行校验，最终确定出公共特征点的位置参数，具体为，

5-1)计算两组实数解中的位置参数相似度ρ(i,j)，计算公式为，

$\mathrm{\ρ}(i,j)=\frac{{\stackrel{\→}{X}}_i\·{\stackrel{\→}{Y}}_j}{\left|\right|{\stackrel{\→}{X}}_i\left|\right|\·\left|\right|{\stackrel{\→}{Y}}_j\left|\right|}$

其中，分别由A_i、B_j改写成的一维向量；

5-2)设置位置参数相似度阈值，根据公共特征点在相机坐标系下的位置参数具有唯一性的特点，选取两组实数解中位置参数相似度最大的两个解，并将该两个解与位置参数相似度阈值进行校验；

若其位置参数相似度大于位置参数相似度阈值，则认定位置参数解算正确，选取两个解的平均值作为公共特征点的位置参数，直接将公共特征点的位置参数用于导航；否则，认定位置参数解算错误，采集下一帧图像重复步骤2)到步骤5)。