CN101839692B - 单相机测量物体三维位置与姿态的方法 - Google Patents

Abstract

本发明公开一种单相机测量物体三维位置与姿态的方法，利用单相机获得待测目标的图像，通过精确识别待测目标上的标记点，来确定待测目标的实时三维位置与姿态信息；根据检测场景和范围选定合适的相机，并对相机进行标定，获得相机的内外参数；根据待测目标设计目标标记点并将标记点合理布局；然后根据相机捕获的图像进行目标检测和特征点的识别，并将检测到的特征点与标记点进行匹配；最后根据测量点与物体标记点的对应关系来求待测目标的三维位置与姿态信息。对于非刚性物体还能够检测到物体是否有变形。本发明采用单相机来实现三维测量，能够获知目标在三维空间中的空间几何参数、位置与姿态等信息，降低了测量成本和测量系统的体积，并且便于操作。

Description

单相机测量物体三维位置与姿态的方法

技术领域：

本发明属于信息技术中的视觉测量领域，涉及一种利用相机标定与目标检测来确定待测目标的三维位置与姿态的视觉测量方法。

背景技术：

视觉测量方法能够对空间几何尺寸进行精确测量和定位，是目前信息技术中的一个重要的研究领域，已经在产品在线质量监控、微电子器件的自动检测、各种模具三维形状的测量以及机械手的定位与瞄准等领域得到应用。目前一般采用双目相机或者单相机加光投射器的方式来实现三维测量。为了提高测量精度，要求两个相机之间的距离尽可能大，这就使得测量系统的体积增大，重量增加。并且一般情况下，大多数测量物体都是刚性物体，通过视觉测量系统来获得该物体在三维空间中的几何参数和位置姿态等信息。本发明采用单相机来实现三维测量，降低了成本和测量系统的体积与重量，并且便于操作。除了能够测量刚性物体的空间几何参数和位置姿态等信息，对于形变不大的非刚性物体，本发明还能够检测到该物体产生的形变，如检测建筑物发生的形变等。

发明内容：

本发明的目的在于，提供一种成本低，易实现的视觉测量方法以实现对目标的检测并测量其三维位置与姿态，即利用单相机获得待测目标的图像，通过精确识别待测目标上的标记点，来确定待测目标的实时三维位置与姿态信息。

为达到本发明目的，本发明采用视觉测量技术，首先根据检测场景和范围选定合适的相机，并对相机进行标定，获得相机的内外参数；根据待测目标设计目标标记点并将标记点合理布局；然后根据相机捕获的图像进行目标检测和特征点的识别，并将检测到的特征点与标记点进行匹配；最后根据测量点与物体标记点的对应关系来求待测目标的三维位置与姿态信息。对于非刚性物体还能够检测到物体是否有变形。

单相机测量物体三维位置与姿态的方法，其特征在于：包括相机标定，目标标记，特征点的检测，特征点与标记点的匹配和目标三维位置与姿态信息的计算五个步骤；

步骤1：根据工作环境选择合适的相机，考虑焦距、视角、畸变等参数，使得相机在特定的范围内都能够对目标清晰成像，为了建立视觉测量系统中三维世界坐标和二维图像坐标的映射关系，需要对相机进行标定，即确定相机的内外参数，所述的相机的内参数是指相机的焦比、光心及成像坐标系两轴间的夹角；外参数是指旋转矩阵和平移向量；

步骤2：对目标进行标记，即将标记点标记在目标上，以便准确快速的检测和识别；

步骤3：对相机采集的图像进行图像处理，获得图像中的所有特征点，计算各特征点在图像中的位置，所述的图像的特征点是成像后待测目标上的标记点；

步骤4：在检测出的所有特征点中确定用于匹配的特征点，将其与目标标记点进行匹配，获得二维坐标与三维坐标的对应关系，所述的标记点是待测目标上标记点；

步骤5：根据特征点与标记点的对应关系，计算目标的三维位置与姿态信息，所述的三维位置与姿态信息是待测目标在三维世界坐标系中的三维坐标和其相对于坐标系的偏转角度。

实现上述视觉检测测量方法，按以下具体步骤进行：

步骤1.1建立二维坐标和三维坐标之间的映射关系，令图像点p＝(u，v，1)^T是空间一点^CP＝(x，y，z，l)^T在图像平面的齐次坐标，则规一化图像坐标

的齐次坐标为：

$\hat{p}=\left(\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right)=\left(\begin{array}{c}x/z\\ y/z\\ 1\end{array}\right)---\left(1\right)$

物理图像坐标系与归一化图像坐标系之间的关系为：

$p=A\hat{p},$ 其中 $A\stackrel{\mathrm{def}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(2\right)$

(u₀，v₀，α，β，c)统称为相机的内部参数，其中(α，β)为相机的焦比，c反应了u轴和v轴夹角斜度，C₀＝(u₀，v₀)^T是相机的光心。则图像点p在物理图像坐标系中的齐次坐标与空间点^CP在相机坐标系中的齐次坐标之间的关系为：

$p=\frac{1}{2}\left(\begin{array}{cc}A& 0\end{array}\right)\left(P_C\right)---\left(3\right)$

相机坐标系(C)在世界坐标系(W)＝(O_w，X，Y，Z)中的位置记为：

$\left(\begin{array}{c}P_C\\ 1\end{array}\right)=\left(\begin{array}{cc}R_W^C& O_W^C\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}P\\ 1\end{array}\right)$

代入(3)式得

$p=\frac{1}{z}\mathrm{HP},$ 其中 $H\stackrel{\mathrm{def}}{=}A\left(\begin{array}{cc}R& t\end{array}\right)---\left(4\right)$

其中，

是旋转矩阵，t＝^CO_W是平移向量，它们是相机的外参数。

步骤1.2对选定的相机进行标定，即获得相机的内外参数。采用基于平面单应矩阵的标定方法，利用不同视点间的平面匹配来计算相机参数，最少只需3个不同视点的图像即可完成标定。记像平面二维坐标点p＝(u，v)^T，世界坐标P＝(X，Y，Z)^T，由公式(4)可知，从三维到二维的映射可以表示为其中

$p=\frac{1}{z}A\left[\begin{array}{cc}R& t\end{array}\right]P,$ $A\stackrel{\mathrm{def}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(5\right)$

令标板所在的世界坐标Z＝0，则上式可写为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z}A\left[\begin{array}{cccc}{r}_1& r_2& r_3& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\frac{1}{z}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]---\left(6\right)$

将上式重新记为

$p=\frac{1}{z}\mathrm{HP},$ 其中 $h\stackrel{\mathrm{def}}{=}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]---\left(7\right)$

H矩阵即确定了三维空间坐标到二维像平面坐标的映射关系，也即求解出H矩阵就完成了相机的标定。

步骤1.3考虑到测量与计算误差，定义误差函数如下：

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|p_{\mathrm{ij}}-P_{\mathrm{ij}}(A,R,t)\left|\right|}^2---\left(8\right)$

使上式最小即可求得最大似然估计下的最优参数。

步骤2是对目标进行标记，即将标记点标记在目标上，以便准确快速的检测和识别。目标一般分为刚性、非刚性两大类。对于刚性目标，标记点不会发生形变。对于非刚性目标，标记点可能会发生形变。本发明除了能够检测目标的三维位置与姿态，对于非刚性目标还能够检测目标发生的形变。

步骤3.1用相机采集待测目标的图像，并进行图像处理，将其转换成灰度图像；

步骤3.2对灰度图像进行预处理，包括图像去噪，图像增强等方法，使图像中各个图形更为清晰；

步骤3.3利用prewitt算子对图像作边缘检测，找梯度最大的地方，通常找到的是标记点图形所在区域的边缘；

步骤3.4对步骤3.3得到的边缘图进行直方图统计，根据双峰法选取波谷处的灰度值作为阈值，根据该阈值对图像进行二值化，使特征图形所形成的区域更明显；

步骤3.5对于找到的超过阈值的区域S，计算区域的质心作为该图形的中心；

步骤3.6在确定图形的中心后，根据该中心及其4邻域是否为白色来确定该图形是实心还是空心。

步骤4.1检测出的特征点数通常有多个，从中确定3个以上待匹配的图像特征点；

步骤4.2根据4.1确定的多个特征点的位置来确定它们对应于实际目标中的标记点，包括特征点分类、特征点排序、判断特征点组号和点集匹配四个步骤，得到二维坐标与三维坐标的映射关系；

步骤5.1：根据特征点、对应的目标标记点及相机内部参数，采用ICP方法求解旋转矩阵和平移矩阵，得到目标位置与姿态的六个参数；

步骤5.2：要确定空间中相机坐标系下某一点P(X_c，Y_c，Z_c)，根据图像中对应的测量点p(x，y)、二维坐标与三维坐标的关系Z_c 以及相机坐标系与世界坐标系之间的转换关系：

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left[R\right]\begin{array}{c}\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]\end{array}+T$ 或 $\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(9\right)$

可得：

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\α}}_x& 0& u_0& 0\\ 0& {\mathrm{\α}}_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(10\right)$

其中：

$\left[R\right]\≈\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z-\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z\\ \cos {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z-\sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z\\ -\sin {\mathrm{\θ}}_y& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y& \cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\end{array}\right],$

$T=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right].$

至少需要3个标记点来计算M矩阵中的6个参数，考虑到计算精度，本发明采用7个标记点来计算目标的三维位置与姿态。

从上述测量过程可知，本发明采用单相机来实现三维测量，能够获知目标在三维空间中的空间几何参数、位置与姿态等信息，降低了测量成本和测量系统的体积，并且便于操作。

附图说明：

图1：本发明的视觉测量系统总框图；

图2：本发明中相机成像模型及物理成像和归一化成像坐标系；

图3：本发明中相机标定示意图；

图4：本发明中目标标记点的设计和布局示意图；

图5：本发明中图像特征点检测结果；

图6：本发明中确定待匹配的图像特征点流程图；

图7：本发明中图像特征点与标记点的匹配流程图；

图8：本发明中图像特征点排序示意图。

具体实施方式：

以下结合附图对本发明做进一步的详细描述。

本发明包括相机标定、目标标记点的标记、特征点检测、特征点与标记点的匹配、位置与姿态计算五个部分，

如图1所示，该方法按以下步骤进行：

步骤1.1设定整个工作范围为12m×8m，则为了全局监控检测，并对目标进行定位，需要选用视场比较大的镜头。但镜头视场越大其焦距越小，那么成像后，畸变就越严重，不利于目标检测以及位置与姿态计算。综合考虑这些因素，可选取焦距为5mm的镜头(其水平视场角约为60度)进行全局监控。

参见图2所示，建立二维坐标和三维坐标之间的映射关系，令图像点p＝(u，v，l)^T是空间一点^CP＝(x，y，z，l)^T在图像平面的齐次坐标，则规一化图像坐标

的齐次坐标为：

$\hat{p}=\left(\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right)=\left(\begin{array}{c}x/z\\ y/z\\ 1\end{array}\right)---\left(1\right)$

物理图像坐标系与归一化图像坐标系之间的关系为：

$p=A\hat{p},$ 其中 $A\stackrel{\mathrm{def}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(2\right)$

(u₀，v₀，α，β，c)统称为相机的内部参数，其中(α，β)为相机的焦比，c反应了u轴和v轴夹角斜度，C₀＝(u₀，v₀)^T是相机的光心，

图像点p在物理图像坐标系中的齐次坐标与空间点^CP在相机坐标系(C)中的齐次坐标之间的关系为：

$p=\frac{1}{2}\left(\begin{array}{cc}A& 0\end{array}\right)\left(P_C\right)---\left(3\right)$

相机坐标系(C)在世界坐标系(W)＝(O_w，X，Y，Z)中的位置记为：

$\left(\begin{array}{c}P_C\\ 1\end{array}\right)=\left(\begin{array}{cc}R_W^C& O_W^C\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}P\\ 1\end{array}\right)$

代入(3)式得

$p=\frac{1}{z}\mathrm{HP},$ 其中 $H\stackrel{\mathrm{def}}{=}A\left(\begin{array}{cc}R& t\end{array}\right)---\left(4\right)$

其中，

是旋转矩阵，t＝^CO_W是平移向量，它们是相机的外参数；

步骤1.2在相机使用前，对相机进行标定，如图3所示，具体按以下步骤进行：

(1)按如下说明建立目标坐标系，以标板左上第一个特征点为坐标原点，标板所在平面为XY平面，Z轴垂直于XY平面，使其符合右手坐标系。

(2)将相机移至标板正上最佳成像距离(即标板完全可见且清晰成像)处，使标板图像占满图像尺幅1/2～3/4为宜。以此距离为拍摄参考距离。

(3)操纵机械臂沿环形轨道一运动(如图3所示)，在运动过程中，应使相机光轴始终指向标板中心附近。相机在等间距位置上静止采集12幅图像。

说明：轨道一为倒置圆锥体的底面圆周，倒置锥体的顶角60°～80°之间，椎体的高与底面半径可依据实际情况调整，使得标板图像始终完全清晰可见，且不受复杂背景影响。以上对距离和角度的要求均为参考值，在保证图像质量的情况下不做严格要求。

(4)操纵机械臂沿弧形轨道二运动(如图3所示)，在运动过程中，应使相机光轴始终指向标板中心附近。相机在以中心为基准的-40°～40°之间等间距位置上静止采集5幅图像。

说明：轨道二为平行于XZ平面的扇面的弧形边，扇形半径可依据实际情况调整，使得标板图像始终完全清晰可见，且不受复杂背景影响。以上对距离和角度的要求均为参考值，在保证图像质量的情况下不做严格要求。

(5)操纵机械臂沿弧形轨道三运动(如图3所示)，在运动过程中，应使相机光轴始终指向标板中心附近。相机在以中心为基准的-40°～40°之间等间距位置上静止采集5幅图像。

说明：轨道三为平行于YZ平面的扇面的弧形边，扇形半径可依据实际情况调整，使得标板图像始终完全清晰可见，且不受复杂背景影响。以上对距离和角度的要求均为参考值，在保证图像质量的情况下不做严格要求。

(6)以上共采集得到22幅图像。对每幅图像进行特征点检测，对每个棋盘格使用其角点，故一幅图像共存在64个角点，以这64个角点的图像坐标来匹配其空间坐标。

采用基于平面单应矩阵的标定方法，利用不同视点间的平面匹配来计算相机参数，最少需要3个不同视点的图像完成标定；记像平面二维坐标点p＝(u，v)^T，世界坐标P＝(X，Y，Z)^T，由公式(4)可知，从三维到二维的映射可以表示为：

$p=\frac{1}{z}A\left[\begin{array}{cc}R& t\end{array}\right]P,$ 其中 $A\stackrel{\mathrm{def}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(5\right)$

不失一般性，可令标板所在的世界坐标Z＝0，则上式可写为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z}A\left[\begin{array}{cccc}{r}_1& r_2& r_3& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\frac{1}{z}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]---\left(6\right)$

将上式重新记为：

$p=\frac{1}{z}\mathrm{HP},$ 其中 $h\stackrel{\mathrm{def}}{=}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]---\left(7\right)$

H矩阵即确定了三维空间坐标到二维像平面坐标的映射关系，求解出H矩阵就完成了相机的标定；

考虑到测量与计算误差，求出的参数不一定满足相机要求的空间约束。定义误差函数如下：

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|p_{\mathrm{ij}}-P_{\mathrm{ij}}(A,R,t)\left|\right|}^2---\left(8\right)$

使上式最小即可求得最大似然估计下的最优参数。

得到空间坐标与图像坐标的关系后，根据公式(7)求解H矩阵，即相机的内外参数。

步骤2对目标进行标记，以便准确快速的检测和识别。假设目标模型是圆柱体，可把圆柱体表面划分成固定大小的块，标记点有选择地定标在各个块上。具体的，目标模型的标记点的分布如图4所示。本发明不限于该实例。在标记点分布图中，有两类图形，包括空心圆和实心圆，每个图形都可能作为待匹配的一个点，这些标记点的三维坐标信息保存在数据库中。根据不同的物体或者需求，可以人为改变标记点的信息。标记点的布局遵循如下的原则：

(1)同一组的标记点尽量集中，不同组的标记点尽量分开，这样，容易获取一组点。

(2)匹配的标记点不能共面。

(3)同一组标记点的跨度尽可能大，这样可以减小误差。

(4)同一组的7个标记点都能用分成两类，其中一类是3个点，一类是4个点。

(5)每一组的7个点中，至少有3个点是在同一条竖直线上。

(6)各个标记点都是黑色，这样，在白色底的背景下容易检测标记点。

(7)标记点由圆形和圆环组成，主要作用是区分不同组别，而且方便图像特征点与目标标记点的对应。

步骤3.1用相机采集待测目标的图像，并进行图像处理，将其转换成灰度图像；

步骤3.2对灰度图像进行预处理，包括图像去噪，图像增强等方法，使图像中各个图形更为清晰；

步骤3.3利用prewitt算子对图像作边缘检测，找梯度最大的地方，通常找到的是标记点图形所在区域的边缘；

步骤3.4对步骤3.3得到的边缘图进行直方图统计，根据双峰法选取波谷处的灰度值作为阈值，从而根据该阈值对图像进行二值化，使特征图形所形成的区域很明显；

步骤3.5对于找到的超过阈值的区域S，计算区域的质心作为该图形的中心，计算表达式为：

$x_c=\frac{\underset{(x,y)\∈S}{\mathrm{\Σ}}x\·g(x,y)}{\underset{(x,y)\∈S}{\mathrm{\Σ}}g(x,y)},$ $y_c=\frac{\underset{(x,y)\∈S}{\mathrm{\Σ}}y\·g(x,y)}{\underset{(x,y)\∈S}{\mathrm{\Σ}}g(x,y)}---\left(11\right)$

其中S为图形所在的区域；g(x，y)为区域内(x，y)点的灰度，(x_c，y_c)是经过图像分割以后确定特征点所在区域并计算得到质心。具体的检测结果如图5所示。

步骤3.6在确定图形的中心后，根据该中心及其4领域是否为白色来确定该图形是实心还是空心。

步骤4.1检测出的特征点数通常有多个，从中确定7个待匹配的图像特征点，如图6所示，具体步骤如下：

(1)根据所检测到的特征点面积信息判断该点是否为噪声点

a.若面积大于预设阈值，认为识别正确，保留该特征点；

b.若面积小于预设阈值，认为识别有偏差，该特征点为噪声，不予考虑后面的步骤。

(2)特征点个数少于4个或者圆环个数少于3个，则不考虑后面的步骤。

(3)从所有特征点中找出在同一直线上的，且个数不少于3个的圆环。

(4)对于每一组满足步骤B的圆环，做如下操作：

a.从圆环组成的直线一侧找出满足条件的特征点，否则重复(3)。该条件是：特征点所成的直线与该直线平行且离该直线最近。

b.从该直线另一侧同样找出满足条件的特征点，否则重复(3)。

c.如果找到的特征点集所成的三条平行直线满足特征点个数为2，3，2的情况，则该点集为待匹配的特征点集。

(5)如果找到的满足上述条件的点集有两组，则计算每组中点集的平均面积，并取平均面积较小的那组点集作为待匹配的特征点集。

(6)将这7个特征点的数据信息保存。

步骤4.2根据4.1确定的7个特征点的位置来确定它们对应于实际目标中的标记点，特征点与标记点的匹配流程如图7所示。具体的匹配过程主要有以下四个步骤：

(1)特征点分类：设计时已经确定将7个特征点分布成两类，其中一组为4个点，一组为3个点。用K均值聚类方法将该7个点分为两类。确定聚类个数k，初始值k＝2；

a.初始化，将待聚类点的簇标志位flag赋值为0，代表初始同属一簇；

b.选择前k个待聚类点作为k个簇的初始簇中心；

c.计算簇中每个点到k个簇中心的距离，，将每个待聚类点(重新)赋给距离最近的簇，改变簇标志位flag，属于同一簇点的标志位flag相同；

d.计算并更新簇的质心；

e.重复步骤d-e，直到簇的质心不再变化。

(2)特征点排序：根据分类结果和特征点的拓扑结构，对该特征点排列成如下图所示的顺序，并存储下来，特征点排序如图8所示。

排序的过程如下：

a.找出第一类和第二类中距离最远的两个特征点，这两个点中，处于第一类的为特征点1，处于第二类的为特征点7。

b.在第二类点集中，除去特征点7，从剩余的三个点中得到唯一的实心圆形，即特征点4。

c.第二类点集中，剩下特征点5和6，在第一类点集中，找出与点5和6在同一直线上的点，即特征点2。

d.特征点5和6的判断，根据它们与特征点2之间的距离即可区分。离特特征点2近的为5，离特征点2远的为6。

e.第一类中，剩下的点即为特征点3。

这样，就确定了7个特征点的顺序。

(3)判断特征点组号：根据特征点1和特征点3的solidflag属性，确定该特征点簇所对应的目标标记点簇，目标标记点簇的类型编号为GroupNum，具体计算如下公式：

GroupNum＝ImagePnt[1].solidflag×2+ImagePnt[3].solidflag

其中：solidflag是特征点空心/实心标志位。

(4)点集匹配：根据标记点集的类型编号，就可以直接获取与7个特征点相对应的7个目标标记点，这样就得到了二维图像坐标与三维空间坐标的对应关系。

步骤5.1根据7个特征点与其相对应的7个目标标记点以及相机内部参数，采用ICP方法求解旋转矩阵和平移矩阵，从而得到目标位置与姿态的六个参数。要确定空间中相机坐标系下某一点P(X_c，Y_c，Z_c)，根据图像中对应的测量点p(x，y)、二维坐标与三维坐标的关系Z_c 由于相机坐标系与世界坐标系之间的转换关系是：

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left[R\right]\begin{array}{c}\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]\end{array}+T$ 或 $\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(9\right)$

于是可得：

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\α}}_x& 0& u_0& 0\\ 0& {\mathrm{\α}}_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(10\right)$

其中：

$\left[R\right]\≈\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z-\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z\\ \cos {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z-\sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z\\ -\sin {\mathrm{\θ}}_y& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y& \cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\end{array}\right],$

$T=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right].$

给定相对应的目标模型标记点、图像检测特征点以及相机内部参数，求解旋转矩阵和平移矩阵的过程如下：

根据相机模型，即由式10推导，可得：

$\frac{1}{Z_c}A(R{m}_i+t)=p_i---\left(12\right)$

其中，m_i＝R^TZ_cA^-1p_i-R^Tt

这样，我们可以得到在约束

下的目标函数：

$F(s,R_t,t_r)=\min \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(s{R}_t{p}_{\mathrm{ai}}+t_r)-m_i\left|\right|}^2---\left(13\right)$

其中：s＝Z_c，R_t＝R^T，p_ai＝A^-1p_i，t_r＝-R^Tt.

参考ICP方法，我们可以得到的解为：

$\left(1\right)n_i=m_i-\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}{m}_i,q_i=p_{\mathrm{ai}}-\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}{p}_{\mathrm{ai}}$

$\left(2\right)H=\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}{q}_i{n}_i^T=\mathrm{U\ΛV},R_t={\mathrm{VU}}^T$

$\left(3\right)s=\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}{n}_i^T{R}_t{q}_i/\underset{i}{\overset{n}{\mathrm{\Σ}}}{q}_i^T{q}_i$

$\left(4\right)t_t=\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}{m}_i-\frac{1}{n}\underset{i}{\overset{n}{\mathrm{\Σ}}}s{R}_t{p}_{\mathrm{ai}}$

(5)  用Levenberg-Marquardt方法来最优化上述结果。

最终得到的结果就是待测目标的三维位置与姿态。

对于形变不大的非刚性物体，根据计算得到的目标的三维位置与姿态信息，可以跟最初获得的三维位置与姿态信息进行比对，从而可以得知该物体可能发生的形变。例如，可以在建筑物表面进行标记，在初始状态下获得这些标记点的三维位置与姿态，之后对该建筑物进行连续监测，可以再次获得标记点的三维位置与姿态，将其与初始值进行比对，如果发生变化，则表明建筑物可能有形变产生，可根据数据进行计算进一步判断是何种形变。

Claims (3)

1.单相机测量物体三维位置与姿态的方法，其特征在于：包括相机标定，目标标记，特征点的检测，特征点与标记点的匹配和目标三维位置与姿态信息的计算五个步骤；

步骤1：相机标定，标定相机的内外参数，建立视觉测量系统中三维世界坐标和二维图像坐标的映射关系，所述相机的内参数是指相机的焦比、光心及成像坐标系两轴间的夹角；所述相机的外参数是指旋转矩阵和平移向量；

步骤2：目标标记，将标记点标记在目标上，以便准确快速的检测和识别；

步骤3：特征点的检测，对相机采集的图像进行图像处理，获得图像中的所有特征点，计算各特征点在图像中的位置，所述图像的特征点是成像后待测目标上的标记点；

步骤4：特征点与标记点的匹配，在检测出的所有特征点中确定用于匹配的特征点，将其与目标上标记点进行匹配，获得二维坐标与三维坐标的对应关系；

步骤5：目标三维位置与姿态信息的计算，根据特征点与标记点的对应关系，计算目标的三维位置与姿态信息，所述的三维位置与姿态信息是指目标在三维世界坐标系中的三维坐标和其相对于坐标系的偏转角度；

其中所述的步骤1包括以下具体步骤，

步骤11：建立二维坐标和三维坐标之间的映射关系，令图像点p＝(u，v，1)^T是空间一点^CP＝(x，y，z，1)^T在图像平面的齐次坐标，则规一化图像坐标的齐次坐标为：

$\hat{p}=\left(\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right)=\left(\begin{array}{c}x/z\\ y/z\\ 1\end{array}\right)---\left(1\right)$

物理图像坐标系与归一化图像坐标系之间的关系为：

$p=A\hat{p},$ 其中 $A\stackrel{\mathrm{ref}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(2\right)$

(u₀，v₀，α，β，c)统称为相机的内部参数，其中(α，β)为相机的焦比，c反应了u轴和v轴夹角斜度，C₀＝(u₀，v₀)^T是相机的光心，

图像点p在物理图像坐标系中的齐次坐标与空间点^CP在相机坐标系中的齐次坐标之间的关系为：

$p=\frac{1}{z}\left(\begin{array}{cc}A& 0\end{array}\right)\left(P_C\right)---\left(3\right)$

相机坐标系(C)在世界坐标系(W)＝(O_w，X，Y，Z)中的位置记为：

$\left(\begin{array}{c}P_C\\ 1\end{array}\right)=\left(\begin{array}{cc}R_W^C& O_{\text{W}}^C\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}P\\ 1\end{array}\right)$

代入(3)式得

$p=\frac{1}{z}\mathrm{HP},$ 其中 $H\stackrel{\mathrm{ref}}{=}A\left(\begin{array}{cc}R& t\end{array}\right)---\left(4\right)$

其中，

是旋转矩阵，t＝^CO_w是平移向量，它们是相机的外参数；

步骤1.2：对选定的相机进行标定，获得相机的内外参数；采用基于平面单应矩阵的标定方法，利用不同视点间的平面匹配来计算相机参数，最少需要3个不同视点的图像完成标定；记像平面二维坐标点p＝(u，v)^T，世界坐标P＝(X，Y，Z)^T，由公式(4)可知，从三维到二维的映射可以表示为：

$p=\frac{1}{z}A\left[\begin{array}{cc}R& \text{t}\end{array}\right]P,$ 其中 $A\stackrel{\mathrm{ref}}{=}\left(\begin{array}{ccc}\mathrm{\α}& c& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right)---\left(5\right)$

令标板所在的世界坐标Z＝0，则上式可写为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z}A\left[\begin{array}{cccc}{r}_1& r_2& r_3& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\frac{1}{2}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]---\left(6\right)$

将上式重新记为：

$p=\frac{1}{z}\mathrm{HP},$ 其中 $H\stackrel{\mathrm{ref}}{=}A\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]---\left(7\right)$

H矩阵即确定了三维空间坐标到二维像平面坐标的映射关系，求解出H矩阵就完成了相机的标定；

步骤1.3：考虑到测量与计算误差，定义误差函数如下：

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|p_{\mathrm{ij}}-P_{\mathrm{ij}}(A,R,t)\left|\right|}^2---\left(8\right)$

使上式最小即可求得最大似然估计下的最优参数。

2.根据权利要求1所述的单相机测量物体三维位置与姿态的方法，其特征在于：所述的步骤3包括以下具体步骤，

步骤3.1：将相机采集的目标图像进行处理，使其转换成灰度图像；

步骤3.2：对灰度图像进行图像去噪、图像增强处理，使图像中各个图形更为清晰；

步骤3.3：利用prewitt算子对图像作边缘检测，找梯度最大的地方，即是标记点图形所在区域的边缘；

步骤3.4：对步骤3.3得到的边缘图进行直方图统计，选取波谷处的灰度值作为阈值，根据该阈值对图像进行二值化，使特征图形所形成的区域更明显；

步骤3.5：对于找到的超过阈值的区域S，计算区域的质心作为该图形的中心；

步骤3.6：在确定图形的中心后，根据该中心及其4邻域是否为白色来确定该图形是实心还是空心。

3.根据权利要求1所述的单相机测量物体三维位置与姿态的方法，其特征在于：所述的步骤5包括以下具体步骤，

步骤5.1：根据特征点、对应的目标标记点及相机内部参数，采用ICP方法求解旋转矩阵和平移矩阵，得到目标位置与姿态的六个参数；

步骤5.2：要确定空间中相机坐标系下某一点P(X_c，Y_c，Z_c)，根据图像中对应的测量点p(x，y)、二维坐标与三维坐标的关系 $Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$ 以及相机坐标系与世界坐标系之间的转换关系：

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left[R\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+T$ 或 $\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(9\right)$

可得：

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\α}}_x& 0& u_0& 0\\ 0& {\mathrm{\α}}_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(10\right)$

其中：

$\left[R\right]\≈\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_y{\cos \mathrm{\θ}}_z& {\sin \mathrm{\θ}}_x{\sin \mathrm{\θ}}_y{\cos \mathrm{\θ}}_z-\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z+{\sin \mathrm{\θ}}_x{\sin \mathrm{\θ}}_z\\ {\cos \mathrm{\θ}}_y{\sin \mathrm{\θ}}_z& {\sin \mathrm{\θ}}_x{\sin \mathrm{\θ}}_y{\cos \mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z& {\cos \mathrm{\θ}}_x{\sin \mathrm{\θ}}_y{\sin \mathrm{\θ}}_z-\sin {\mathrm{\θ}}_x{\cos \mathrm{\θ}}_z\\ -{\sin \mathrm{\θ}}_y& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y& \cos {\mathrm{\θ}}_x{\cos \mathrm{\θ}}_y\end{array}\right],$

$T=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right].$

Images