CN112150546B - 一种基于辅助点几何约束的单目视觉位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于辅助点几何约束的单目视觉位姿估计方法，包括以下步骤：S1.目标点提取：用单反相机对于包含多个目标点的目标对象进行拍摄,得到彩色图片，对图像进行预处理，得到二值化图像；然后在二值化图像上进行八领域边界跟踪，计算轮廓的面积s和周长c，根据目标点尺寸和特性设置约束准则，从而得到目标点轮廓；然后对目标点进行中心定位；S2.位姿解算：该部分采用基于辅助点几何约束的位姿估计算法，最终得到相机坐标系下的目标点坐标P_i ^c；S3.迭代优化：对P_i ^c进行迭代优化，得到优化后的目标点位姿。本发明能够通过对目标点的识别和计算得到准确的目标点位姿，并有效提高了位姿估计的稳定性，减少了优化时间。

Description

一种基于辅助点几何约束的单目视觉位姿估计方法

技术领域

本发明涉及视觉位姿估计，特别是涉及一种基于辅助点几何约束的单目视觉位姿估计方法。

背景技术

视觉位姿测量因其非接触式测量方式、稳定工作时间长、测量速度快等优点成为航空航天、智能机器人、自主导航等领域研究的热点问题之一。视觉位姿测量是通过已知假定的目标点在世界坐标系下的三维坐标值和其成像的二维坐标值得到相机坐标系下的目标点位姿，称为n点透视(PnP)问题。视觉测量可以分为基于单目视觉的方法、基于多目视觉的方法和基于深度视觉的方法。基于单目视觉测量方法结构简单、测量范围大，而且具有现场安装、调试便捷等优点，因此适合于工业制造中的现场测量。

单目视觉位姿测量的核心内容之一是位姿估计算法，从数学角度来看可将其分为解析算法和迭代算法两大类，解析算法的优点是计算效率高，缺点是计算原理复杂且容易受到噪声的影响，迭代算法的优点是计算精度高且噪声影响情况下的稳定性较好，缺点是计算时间消耗太大。在实际应用中发现，位姿估计算法的精度跟目标点分布情况有关，特别当目标点分布为平面、近平面或近线等奇异情况时，位姿估计的精度不高，稳定性不强。当前经典的解析算法有EPnP算法(n≥4)、RPnP算法(n≥4)、OPnP算法等(n≥4)。EPnP算法的核心思想是利用四个虚拟控制点表示目标点，将解表示为零特征值向量的加权和，线性复杂度为n，大大缩短了解析算法的求解时间，尽管EPnP算法针对平面情况运用3个虚拟控制点来提高精度，但其在噪声影响情况下稳定性不强。RPnP算法在角锥体原理的基础上，提出一种高效的解析算法，对目标点位于平面、近平面或近线等各种构型下，都能得到较为准确的位姿解，但在实际测量中，达不到较高的精度。OPnP算法采用非单位四元数对旋转矩阵进行参数化，并利用Grobner基对旋转参数多项式进行求解，解决了矩阵求解中存在的退化问题，但其虽然是全局收敛能达到较高的精度，但是计算过程相对复杂。当前常用的迭代算法有POSIT算法(Pose from Orthography and Scaling with Iterations)和正交迭代算法。POSIT算法(n≥4)基于弱透视投影模型，计算效率相对较高，但其未明确定义用于优化的目标函数，所以算法的收敛性不能被严格地证明。正交迭代算法(OI)(n≥4)采用最小化目标空间共线误差的思想，通过弱透视模型求得位姿初值，SVD方法解决绝对定向问题，可以获得较高的位姿估计精度并具有较强的抗噪能力，但在平面情况下正交迭代算法易收敛到局部最小值，无法得到准确的位姿。

位姿估计过程中，根据相机成像模型需定义4个坐标系，包括世界坐标系(目标点实际所在的三维坐标系、原点自定)、相机坐标系(以相机光心为原点的三维坐标系)、像素坐标系(相机拍摄得到的像片上目标点的二维像素坐标系)、像平面坐标系(由相机内参数矩阵将像素坐标系转换的数值坐标系)，其中像平面坐标系与相机坐标系为透射关系(小孔成像原理，两者间存在缩放因子)，相机坐标系与世界坐标系为旋转和平移关系，存在旋转矩阵和平移向量。求解目标点的位姿，即求解每张图片的相机坐标系与世界坐标系的关系，即旋转矩阵R(3×3)和平移矩阵t(3×1)。

经研究发现，当目标点数n＝3时，PnP问题至多存在4个解，当n≥4时，PnP问题才能确定唯一解。在有解的点数中，当n＝3、4、5时，PnP问题无法直接求解，当n≥6时，PnP问题可依据相机成像原理建立的共线方程直接求解。在实际运用中要求既能高效处理数百个特征点，也能准确处理少数特征点(n＜6)。PnP问题的探索与发展从P3P问题研究开始，逐步扩展至n点问题。大量学者和研究人员对P3P的至多四个解进行分析研究，提出了很多解决方案，也针对n＞6进行了大量研究并提出多种算法，故而当前PnP问题研究的重点在P4P问题和P5P问题。

PST(perspective similar triangle)算法即透射相似三角形法是针对P3P问题的算法，它通过几何约束关系减少需求解的未知参量，对于某些奇异构型分布目标点也能得到较稳定的结果，不同的方法根据几何约束会得到不同的四次方程，方程求解器选择不合适会导致直接解的性能显著退化，PST方法不依赖特定的方程解算器求得直接解，计算效率较高，且在P3P几何奇异性局限的地方也能获得较为稳定的结果，但其联立方程组得到其中一变量的至多4个解后，求解另一变量时存在不确定性，需要额外的步骤进行解的判定，且该算法在噪声影响的情况下稳定性不强。当PST算法用于求解P3P问题时，可直接比较多个解的重投影误差大小来确定最终解，但当目标点数n≥4时，比较次数将会成几何指数上升，造成较大的时间消耗。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于辅助点几何约束的单目视觉位姿估计方法，能够通过对目标点的识别和计算得到准确的目标点位姿，并有效提高了位姿估计的稳定性，减少了优化时间。

本发明的目的是通过以下技术方案来实现的：一种基于辅助点几何约束的单目视觉位姿估计方法，包括以下步骤：

S1.目标点提取：用单反相机对于包含多个目标点的目标对象进行拍摄，得到彩色图片，对图像进行预处理，得到二值化图像；然后在二值化图像上进行八领域边界跟踪，计算轮廓的面积s和周长c，根据目标点尺寸和特性设置约束准则，从而得到目标点轮廓；然后对目标点进行中心定位；

S2.位姿解算：该部分采用基于辅助点几何约束的位姿估计算法，求得最终得到相机坐标系下的目标点坐标P_i ^c；

S3.迭代优化：对P_i ^c进行迭代优化，得到优化后的目标点位姿。

其中，所述步骤S1中对图像的预处理包括：

首先将彩色图像化灰度化，将其变为灰度图像，然后采用高斯滤波的方式滤除灰度图像噪声，再进行灰度拉伸增加目标与背景的对比度得到处理后的灰度图，最后在处理后的灰度图上进行自适应二值化得到二值化图像。

所述步骤S1中的约束准则包括面积和周长的范围约束、轮廓包围盒长短轴长度之比约束，以及轮廓面积与包围盒的面积之比约束：

s₁＜s＜s₂，c₁＜c＜c₂，

其中，s₁，s₂为面积的阀值；c₁，c₂为周长的阀值；a，b为包围盒的长短轴；通过轮廓的行列最大最小值计算得到；T₁，T₂为a，b之比的阀值。

所述步骤S1中，对目标点进行中心定位的过程包括：

选用灰度重心定位法，目标点为全黑或全白，目标点像素区域的灰度值分布均匀，采用灰度重心定位，其公式为：

其中，x＝1...m，y＝1...n是该目标点的轮廓区域内像素坐标，I(x,y)是像素坐标(x,y)处的灰度值；(x₀,y₀)即目标点的中心点坐标。

其中，所述步骤S2包括以下子步骤：

S201.设提取得到目标点共有n个，选定距离最远的两个点p₀'、p₁'为基本点，p_i'为第三点，其中i＝1...n-2，将n个目标点划分为n-2个三点子集；

S202.利用每个三点子集的边角关系建立变量t₁的四次方程，得到n-2个关于t₁的方程，求解方程组后最多可以得到四个解；最后将t₁的解依次带入每个三点子集，求解t_i值，若t_i值的正负无法确定，则需要通过三点子集的位置关系构建辅助点，然后求解t_i值，最终得到相机坐标系下的目标点坐标P_i ^c，包括：

A1、为建立t₁的四次方程，在三点子集中通过两个已知条件建立等式关系：

其一：已知Δp₀'p₁'p_i'与ΔP₀ ^cP₁ ^cP_i ^c相似，它们的对应边成比例关系，即带入变量关系后得到：

A₁t₁ ²-t_i ²+A₂＝0

其中A₁＝D₁ ²/D₂ ²，A₂＝D₁ ²l₀ ²sin²γ₁/D₂ ²-l₀ ²sin²γ₂，l₀＝1为p₀'到光心O的长度，D₁为目标点的世界坐标P₀ ^w至P₁ ^w之间的距离，D₂为P₀ ^w至P_i ^w间的距离，γ₁为p₀'与p₁'间的角度，γ₂为p₀'与p_i'间的角度；

其二：cos∠p₀'p₁'p_i'＝cos∠P₀ ^cP₁ ^cP_i ^c，由几何及向量转换将上述条件表述为：

A₃t₁+A₄t_i+A₅t₁t_i+A₆t₁ ²+A₇＝0

其中A₃＝l_icosγ₃-l₁，A₄＝l₁cosγ₃-l_i，A₅＝cosγ₃，A₆＝(D₃ ²-D₁ ²-D₂ ²)/(2D₁ ²)，A₇＝l₀ ²-l₁ ²-l_i ²+l₁l_icosγ₃+A₆l₀ ²sin²γ₁，l₁表示为p₀'在p₁'方向的投影，l_i表示为p₀'在p_i'方向的投影，D₃为P₁ ^w至P_i ^w间的距离，γ₃为p₁'与p_i'间的角度；

联立A₁t₁ ²-t_i ²+A₂＝0和式A₃t₁+A₄t_i+A₅t₁t_i+A₆t₁ ²+A₇＝0两个方程，得到一个关于t₁的四次方程：

B₄t₁ ⁴+B₃t₁ ³+B₂t₁ ²+B₁t₁ ¹+B₀＝0

其中B₃＝2(A₃A₆-A₁A₄A₅)，/>B₁＝2(A₃A₇-A₂A₄A₅)，/>n-2个三点子集可得到n-2个关于t₁的四次方程，n-2个三点子集可得到n-2个关于t₁的四次方程，通过建立罚函数/>求解方程组，得到至多4个t₁值；

将t₁的每一个解带回原方程，当A₄+A₅t₁≠0时：

t_i＝-(A₃t₁+A₆t₁ ²+A₇)/(A₄+A₅t₁)

当A₄+A₅t₁＝0时，考虑根据三点子集的位置关系构建辅助点，从而增加几何约束，求得t_i：

在时，构建辅助点为Δp₀'p₁'p_i'的重心p'＝[x,y,z]^T：

在时，构建辅助点为/>的中点p'＝[x,y,z]^T：

由余弦定理得：

其中d为p'到光心的距离，d₁＝l₁+t₁，联立两个方程组求得d，再将d作为已知条件求解d_i

联立方程组求解d_i，由d_i＝l_i+t_i求得当前三点子集的t_i；

A2、得到t₁和t_i(i＝2...n-2)之后，通过三角形相似得到比例因子：

最终求得相机坐标系下的目标点坐标：

P_i ^c＝λd_iv_i。

其中，所述步骤S3包括以下子步骤：

S301.采用IEPnP算法的迭代部分对P_i ^c进行优化，优化算法的核心思想是将目标点表示为四个虚拟控制点c₁ ^w＝[1,0,0]^T、c₂ ^w＝[0,1,0]^T、c₃ ^w＝[0,0,1]^T、c₄ ^w＝[0,0,0]^T的线性组合，通过共线方程原理构建方程组:

其中λ_i为像平面与物方空间的比例因子，f_u＝f/d_u，f_v＝f/d_v，f为相机焦距，d_u、d_v为相机传感器参数，为相机坐标系下的虚拟控制点坐标，优化算法中将其作为未知量，写出如下方程组：

其中α_ij表示为虚拟控制点的线性组合系数，(u₀,v₀)表示像素中心点，(u_i,v_i)表示目标点的像素坐标，每个目标点都能得到上述两个方程，每个方程有12个变量，n个目标点则可得到2n个方程，其系数矩阵M为2n×12；

利用X＝(α^Tα)^-1α^TPi^c将位姿解算的P_i ^c带入优化算法，α表示线性组合的矩阵：

β＝(V^TV)^-1V^TX

其中V_i为M^TM的零特征值对应的特征向量，采用控制点间的距离约束对β进行迭代优化：

c_i ^c(i＝1...4)表示相机坐标系下的虚拟控制点，c_i ^w(i＝1...4)表示世界坐标系下的虚拟控制点；优化β后反向推理可得到优化后的P_i ^c；最后采用奇异值分解法，已知P_i ^c和P_i ^w求解旋转矩阵R和平移向量t，即目标点位姿。

本发明的有益效果是：本发明根据三点子集的位置关系构建辅助点，增加PST算法的几何约束，将其从求解P3P问题扩展至求解PnP问题；可以求解目标点分布为普通三维构型、平面构型、近平面或近线构型等情况下的位姿，同时采用IEPnP算法的迭代部分进行位姿优化，相比于传统的高斯牛顿优化算法和LM优化算法，更具有针对性，提高了稳定性并减少了优化时间。

附图说明

图1为本发明的方法流程图；

图2为实施例中重心辅助点示意图；

图3为实施例中中心辅助点示意图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

单目视觉位姿测量方案的核心之一是位姿估计算法。目标点提取准确是进行位姿估计的前提，此时运用不同的位姿估计算法都能完成发明目的。当前算法中能实现在目标点各种分布情况下都能进行位姿计算的算法包括以下两种：

方案一：RPnP算法。该算法的核心思想是将n点问题划分为n-2个三点子集，在PST算法的基础上，每个三点子集都可得到一个一元高次方程，然后以两个基础点的方向为已知方向构建物方坐标系，建立新坐标系与相机坐标系的关系，从而得到最终位姿。这种方式可以快速的求解随机三维目标点分布或奇异构型目标点分布的位姿。但实际运用中其基础点选取为随机距离最大值，稳定性并不强。

方案二：OPnP算法。该算法的核心思想是通过用非单位四元数参数化旋转矩阵，然后将其带入相机成像模型，再构建Grobner基求解器对旋转参数多项式进行求解，解决了矩阵求解中存在的退化问题，可快速高效的求解位姿，对随机分布目标点和奇异构型目标点都能进行有效的位姿计算。虽然该方法是一种全局优化方法，精度很高，但是其内部的原理太过复杂，目前工程实用性不强。本申请中采用基于辅助点几何约束的位姿估计算法，具体地：

如图1所示，一种基于辅助点几何约束的单目视觉位姿估计方法，包括以下步骤：

S1.目标点提取：用单反相机对于包含多个目标点的目标对象进行拍摄，得到彩色图片，在matlab中需要对图像进行预处理，包括灰度化使彩色图像变为灰度图像、高斯滤波滤除图像噪声、灰度拉伸增加目标与背景的对比度、在灰度图上进行自适应二值化(也可以采用工业相机拍摄，如果是工业相机拍摄目标则可省去灰度化步骤。)，然后在二值化图像上进行八领域边界跟踪，计算轮廓的面积s和周长c，根据目标点尺寸和特性设置约束准则，从而得到目标点轮廓。其中包括面积和周长的范围、轮廓包围盒长短轴长度之比、轮廓面积与包围盒的面积之比。

s₁＜s＜s₂，c₁＜c＜c₂，

其中，s₁，s₂为面积的阀值，c₁，c₂为周长的阀值，a，b为包围盒的长短轴，通过轮廓的行列最大最小值计算，T₁，T₂为a，b之比的阀值；然后对目标点进行中心定位，通过文献查阅和方法对比，选用鲁棒性最优的灰度重心定位法，目标点为全黑或全白，目标点像素区域的灰度值分布均匀，采用灰度重心定位，其公式为：

其中，x＝1...m，y＝1...n是该目标点的轮廓区域内像素坐标，I(x,y)是像素坐标(x,y)处的灰度值。(x₀,y₀)即目标点的中心点坐标。

S2.位姿解算：利用角锥体原理，将P3P问题中的三个变量减少至两个变量t₁和t₂。该部分首先需要选定距离最远的两个点p₀'＝[x₀,y₀,z₀]^T、p₁'＝[x₁,y₁,z₁]^T为基本点，p_i'＝[x_i,y_i,z_i]^T为第三点，其中i＝1...n-2，将n个目标点划分为n-2个三点子集，如附图所示；然后利用每个三点子集的边角关系建立变量t₁的四次方程，得到n-2个关于t₁的方程，求解方程组后最多可以得到四个解；最后将t₁的解依次带入每个三点子集，求解t_i值，若t_i值的正负无法确定，则需要通过三点子集的位置关系构建辅助点，然后求解t_i值，最终得到相机坐标系下的目标点坐标P_i ^c。

为建立t₁的四次方程，在三点子集中通过两个已知条件建立等式关系：

其一：已知Δp₀'p₁'p_i'与ΔP₀ ^cP₁ ^cP_i ^c相似，它们的对应边成比例关系，即带入变量关系后得到：

A₁t₁ ²-t_i ²+A₂＝0

其中A₁＝D₁ ²/D₂ ²，A₂＝D₁ ²l₀ ²sin²γ₁/D₂ ²-l₀ ²sin²γ₂，l₀＝1为p₀'到光心O的长度，D₁为目标点的世界坐标P₀ ^w至P₁ ^w之间的距离，D₂为P₀ ^w至P_i ^w间的距离，γ₁为p₀'与p₁'间的角度，γ₂为p₀'与p_i'间的角度；

其二：cos∠p₀'p₁'p_i'＝cos∠P₀ ^cP₁ ^cP_i ^c，由几何及向量转换将上述条件表述为：

A₃t₁+A₄t_i+A₅t₁t_i+A₆t₁ ²+A₇＝0

其中A₃＝l_icosγ₃-l₁，A₄＝l₁cosγ₃-l_i，A₅＝cosγ₃，A₆＝(D₃ ²-D₁ ²-D₂ ²)/(2D₁ ²)，A₇＝l₀ ²-l₁ ²-l_i ²+l₁l_icosγ₃+A₆l₀ ²sin²γ₁，l₁表示为p₀'在p₁'方向的投影，l_i表示为p₀'在p_i'方向的投影，D₃为P₁ ^w至P_i ^w间的距离，γ₃为p₁'与p_i'间的角度；

联立式A₁t₁ ²-t₂ ²+A₂＝0和式A₃t₁+A₄t₂+A₅t₁t₂+A₆t₁ ²+A₇＝0两个方程，得到一个关于t₁的四次方程：

f(t₁)＝B₄t₁ ⁴+B₃t₁ ³+B₂t₁ ²+B₁t₁ ¹+B₀＝0

其中B₃＝2(A₃A₆-A₁A₄A₅)，/>B₁＝2(A₃A₇-A₂A₄A₅)，/>n-2个三点子集可得到n-2个关于t₁的四次方程，通过建立罚函数求解方程组，可得到至多4个t₁值。

将t₁的每一个解带回原方程，当A₄+A₅t₁≠0时：

t_i＝-(A₃t₁+A₆t₁ ²+A₇)/(A₄+A₅t₁)

当A₄+A₅t₁＝0时，考虑根据三点子集的位置关系构建辅助点，从而增加几何约束，求得t_i。

具体地，在时，构建辅助点为Δp₀'p₁'p_i'的重心p'＝[x,y,z]^T，如图2所示：

在时，构建辅助点为/>的中点p'＝[x,y,z]^T，如图3所示：

由余弦定理可得：

其中d为p'到光心的距离，d₁＝l₁+t₁，联立两个方程组可以求得d，再将d作为已知条件求解d_i

联立方程组求解d_i，由d_i＝l_i+t_i求得当前三点子集的t_i。得到t₁和t_i(i＝2...n-2)之后，通过三角形相似得到比例因子：

最终求得相机坐标系下的目标点坐标：

P_i ^c＝λd_iv_i

S3.迭代优化：该部分采用IEPnP算法的迭代部分对P_i ^c进行优化。优化算法的核心思想是将目标点表示为四个虚拟控制点c₁ ^w＝[1,0,0]^T、c₂ ^w＝[0,1,0]^T、c₃ ^w＝[0,0,1]^T、c₄ ^w＝[0,0,0]^T的线性组合，通过共线方程原理构建方程组。

其中λ_i为像平面与物方空间的比例因子，f_u＝f/d_u，f_v＝f/d_v，f为相机焦距，d_u、d_v为相机传感器参数，为相机坐标系下的虚拟控制点坐标，优化算法中将其作为未知量，可写出如下方程组：

其中α_ij表示为虚拟控制点的线性组合系数，(u₀,v₀)表示像素中心点，(u_i,v_i)表示目标点的像素坐标，每个目标点都能得到上述两个方程，每个方程有12个变量，n个目标点则可得到2n个方程，其系数矩阵M为2n×12。

利用X＝(α^Tα)^-1α^TP_i ^c将位姿解算的P_i ^c带入优化算法，α表示线性组合的矩阵：

β＝(V^TV)^-1V^TX

其中V_i为M^TM的零特征值对应的特征向量，采用控制点间的距离约束对β进行迭代优化：

c_i ^c(i＝1...4)表示相机坐标系下的虚拟控制点，c_i ^w(i＝1...4)表示世界坐标系下的虚拟控制点。优化β后反向推理可得到优化后的P_i ^c。最后采用SVD奇异值分解法，已知P_i ^c和P_i ^w求解旋转矩阵R和平移向量t，即目标点位姿。

需要说明的是，以上所述是本发明的优选实施方式，应当理解本发明并非局限于本文所披露的形式，不应该看作是对其他实施例的排除，而可用于其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (4)

1.一种基于辅助点几何约束的单目视觉位姿估计方法，其特征在于：包括以下步骤：

S1.目标点提取：用单反相机对于包含多个目标点的目标对象进行拍摄，得到彩色图片，对图像进行预处理，得到二值化图像；然后在二值化图像上进行八邻域边界跟踪，计算轮廓的面积s和周长c，根据目标点尺寸和特性设置约束准则，从而得到目标点轮廓；然后对目标点进行中心定位；

S2.位姿解算：该部分采用基于辅助点几何约束的位姿估计算法，最终得到相机坐标系下的目标点坐标P_i ^c；

所述步骤S2包括以下子步骤：

S201.设提取得到目标点共有n个，选定距离最远的两个点p₀'、p₁'为基本点，p_i'为第三点，其中i＝1...n-2，将n个目标点划分为n-2个三点子集；

S202.利用每个三点子集的边角关系建立变量t₁的四次方程，得到n-2个关于t₁的方程，求解方程组后最多可以得到四个解；最后将t₁的解依次带入每个三点子集，求解t_i值，若t_i值的正负无法确定，则需要通过三点子集的位置关系构建辅助点，然后求解t_i值，最终得到相机坐标系下的目标点坐标P_i ^c，包括：

A1、为建立t₁的四次方程，在三点子集中通过两个已知条件建立等式关系：

其一：已知Δp₀'p₁'p_i'与ΔP₀ ^cP₁ ^cP_i ^c相似，它们的对应边成比例关系，即带入变量关系后得到：

A₁t₁ ²-t_i ²+A₂＝0

其中A₁＝D₁ ²/D₂ ²，A₂＝D₁ ²l₀ ²sin²γ₁/D₂ ²-l₀ ²sin²γ₂，l₀＝1为p₀'到光心O的长度，D₁为目标点的世界坐标P₀ ^w至P₁ ^w之间的距离，D₂为P₀ ^w至P_i ^w间的距离，γ₁为p₀'与p₁'间的角度，γ₂为p₀'与p_i'间的角度；

其二：cos∠p₀'p₁'p_i'＝cos∠P₀ ^cP₁ ^cP_i ^c，由几何及向量转换将上述条件表述为：

A₃t₁+A₄t_i+A₅t₁t_i+A₆t₁ ²+A₇＝0

其中A₃＝l_icosγ₃-l₁，A₄＝l₁cosγ₃-l_i，A₅＝cosγ₃，A₆＝(D₃ ²-D₁ ²-D₂ ²)/(2D₁ ²)，A₇＝l₀ ²-l₁ ²-l_i ²+l₁l_icosγ₃+A₆l₀ ² sin²γ₁，l₁表示为p₀'在p₁'方向的投影，l_i表示为p₀'在p_i'方向的投影，D₃为P₁ ^w至P_i ^w间的距离，γ₃为p₁'与p_i'间的角度；

联立A₁t₁ ²-t_i ²+A₂＝0和式A₃t₁+A₄t_i+A₅t₁t_i+A₆t₁ ²+A₇＝0两个方程，得到一个关于t₁的四次方程：

B₄t₁ ⁴+B₃t₁ ³+B₂t₁ ²+B₁t₁ ¹+B₀＝0

其中B₃＝2(A₃A₆-A₁A₄A₅)，/>B₁＝2(A₃A₇-A₂A₄A₅)，n-2个三点子集可得到n-2个关于t₁的四次方程，通过建立罚函数/>求解方程组，得到至多4个t₁值；

将t₁的每一个解带回原方程，当A₄+A₅t₁≠0时：

t_i＝-(A₃t₁+A₆t₁ ²+A₇)/(A₄+A₅t₁)

当A₄+A₅t₁＝0时，考虑根据三点子集的位置关系构建辅助点，从而增加几何约束，求得t_i：

在时，构建辅助点为Δp₀'p₁'p_i'的重心p'＝[x,y,z]^T：

(x₀,y₀)即目标点的中心点坐标；

在时，构建辅助点为/>的中点p'＝[x,y,z]^T：

由余弦定理得：

其中d为p'到光心的距离，d₁＝l₁+t₁，联立两个方程组求得d，再将d作为已知条件求解d_i

联立方程组求解d_i，由d_i＝l_i+t_i求得当前三点子集对应的t_i；

A2、得到t₁和t_i,i＝2…n-2之后，通过三角形相似得到比例因子：

最终求得相机坐标系下的目标点坐标：

P_i ^c＝λd_iv_i；

S3.迭代优化：对P_i ^c进行迭代优化，得到优化后的目标点位姿；

所述步骤S3包括以下子步骤：

S301.优化算法的核心思想是将目标点表示为四个虚拟控制点:c₁ ^w＝[1,0,0]^T、c₂ ^w＝[0,1,0]^T、c₃ ^w＝[0,0,1]^T、c₄ ^w＝[0,0,0]^T的线性组合，通过共线方程原理构建方程组:

其中λ_i为像平面与物方空间的比例因子，f_u＝f/d_u，f_v＝f/d_v，f为相机焦距，d_u、d_v为相机传感器参数，为相机坐标系下的虚拟控制点坐标，优化算法中将其作为未知量，写出如下方程组：

其中α_ij表示为虚拟控制点的线性组合系数，(u₀,v₀)表示像素中心点，(u_i,v_i)表示目标点的像素坐标，每个目标点都能得到上述两个方程，每个方程有12个变量，n个目标点则可得到2n个方程，其系数矩阵M为2n×12；

利用X＝(α^Tα)^-1α^TP_i ^c将位姿解算的P_i ^c带入优化算法，α表示线性组合的矩阵：

β＝(V^TV)^-1V^TX

其中V_i为M^TM的零特征值对应的特征向量，采用控制点间的距离约束对β进行迭代优化：

c_i ^c(i＝1...4)表示相机坐标系下的虚拟控制点，c_i ^w,i＝1...4表示世界坐标系下的虚拟控制点；优化β后反向推理可得到优化后的P_i ^c；最后采用SVD奇异值分解法，已知P_i ^c和P_i ^w求解旋转矩阵R和平移向量t，即目标点位姿。

2.根据权利要求1所述的一种基于辅助点几何约束的单目视觉位姿估计方法，其特征在于：所述步骤S1中对图像的预处理包括：

首先将彩色图像灰度化，将其变为灰度图像，然后采用高斯滤波方法滤除灰度图像噪声，再进行灰度拉伸增加目标与背景的对比度得到处理后的灰度图像，最后在处理后的灰度图像上进行自适应二值化得到二值化图像。

3.根据权利要求1所述的一种基于辅助点几何约束的单目视觉位姿估计方法，其特征在于：所述步骤S1中的约束准则包括面积和周长的范围约束、轮廓包围盒长短轴长度之比约束，以及轮廓面积与包围盒的面积之比约束：

s₁＜s＜s₂，c₁＜c＜c₂，

其中，s₁，s₂为面积的阀值；c₁，c₂为周长的阀值；a，b为包围盒的长短轴；通过轮廓的行列最大最小值计算得到；T₁，T₂为a，b之比的阀值。

4.根据权利要求1所述的一种基于辅助点几何约束的单目视觉位姿估计方法，其特征在于：所述步骤S1中，对目标点进行中心定位的过程包括：

选用灰度重心定位法，目标点为全黑或全白，目标点像素区域的灰度值分布均匀，采用灰度重心定位，其公式为：

其中，x＝1...m，y＝1...n是该目标点的轮廓区域内像素坐标，I(x,y)是像素坐标(x,y)处的灰度值；(x₀,y₀)即目标点的中心点坐标。