[发明专利]单相机测量物体三维位置与姿态的方法

权利要求书

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

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

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

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

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

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

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

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

p^=u^v^1=x/zy/z1---(1)]]>

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

p=Ap^,]]>其中A=defαcu00βv0001---(2)]]>

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

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

p=1zA0(PC)---(3)]]>

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

PC1=RWCOWC0T1P1]]>

代入(3)式得

p=1zHP,]]>其中H=defARt---(4)]]>

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

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

p=1zARtP,]]>其中A=defαcu00βv0001---(5)]]>

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

uv1=1zAr1r2r3tXY01=1zAr1r2tXY1---(6)]]>

将上式重新记为：

p=1zHp,]]>其中H=defAr1r2t---(7)]]>

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

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

Σi=1nΣj=1m||pij-Pij(A,R,t)||2---(8)]]>

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