CN106780609A - 视觉定位方法和视觉定位装置 - Google Patents

Abstract

本发明涉及视觉定位方法和视觉定位装置。该视觉定位方法包括：在识别对象上设置多个标识点；根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；根据标识点的三维坐标，建立标识点坐标系；和，求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。通过根据本发明的视觉定位方法和视觉定位装置，可以实现不具有显著的自然特征的识别对象的姿态的实时和高精度定位。

Description

视觉定位方法和视觉定位装置

技术领域

本发明总的来说涉及图像处理领域，具体来说涉及用于基于识别对象的几何约束进行视觉定位的视觉定位方法和视觉定位装置。

背景技术

在自动化设备领域中，越来越依靠使用机器视觉的视觉定位技术。根据目标的不同视觉定位技术按照目标种类进行区分，可以分为基于合作目标的定位技术和基于非合作目标的定位技术。

合作目标可以分为自然特征和人工标识物特征。自然特征主要包括目标自身的夹角、直线、圆、矩形、三角形等规则的几何特征。人工标识物又可分为点特征标识物、线特征标识物以及高级几何特征标识物。

基于标识点的视觉定位技术又可以称为PnP问题，即通过点特征数据获取目标的姿态信息。点特征数据一般由一系列点对组成，点对包括物方空间标识点和其对应的投影点。对PnP问题的各种解决方案，例如EPnP算法、DLS算法、RPnP算法、ASPnP算法和LHM算法等等均可以快速、高精度解决PnP问题。但是，上述算法仅适用于几十个甚至上百个点对的情景，且其测量精度随着点对数量的减少而迅速下降。

因此，需要改进的视觉定位方法和视觉定位装置。

发明内容

本发明的目的在于针对上述现有技术中的缺陷和不足，提供能够基于识别对象的几何约束进行识别对象的姿态的实时和高精度定位的新颖的和改进的视觉定位方法和视觉定位装置。

根据本发明的一方面，提供了一种视觉定位方法，包括：在识别对象上设置多个标识点；根据所述多个标识点之间的位置关系，对所述多个标识点进行拓扑区分；根据所述多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解所述多个标识点的三维坐标；根据标识点的三维坐标，建立标识点坐标系；和，求解所述标识点坐标系的旋转矩阵，以得到所述识别对象的姿态信息。

在上述视觉定位方法中，所述识别对象是视觉定位眼镜，且所述多个标识点是设置在所述视觉定位眼镜的框架上的五个标识点。

在上述视觉定位方法中，所述多个标识点之间的拓扑关系包括所述多个标识点之间的平行和线段比例关系。

在上述视觉定位方法中，所述对多个标识点进行拓扑区分的步骤具体包括：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

在上述视觉定位方法中，所述建立求解标识点三维坐标的线性方程组的步骤具体包括：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

其中，(d_x,d_y)为像元尺寸，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心；

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

在上述视觉定位方法中，所述建立标识点坐标系的步骤具体包括：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

在上述视觉定位方法中，所述求解标识点坐标系的旋转矩阵以得到识别对象的姿态信息的步骤具体包括：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

其中，角度α、β和γ分别为所述识别对象在所述相机坐标系中的方位角、俯仰角和横滚角。

在上述视觉定位方法中，所述五个标识点是安装在所述视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。

根据本发明的另一方面，提供了一种视觉定位装置，包括：设置单元，用于在识别对象上设置多个标识点；拓扑单元，用于根据所述多个标识点之间的位置关系，对所述多个标识点进行拓扑区分；坐标获取单元，用于根据所述多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；坐标系建立单元，用于根据标识点的三维坐标，建立标识点坐标系；和，信息获取单元，用于求解所述标识点坐标系的旋转矩阵，以得到所述识别对象的姿态信息。

在上述视觉定位装置中，所述识别对象是视觉定位眼镜，且所述多个标识点是设置在所述视觉定位眼镜的框架上的五个标识点。

在上述视觉定位装置中，所述多个标识点之间的拓扑关系包括所述多个标识点之间的平行和线段比例关系。

在上述视觉定位装置中，所述拓扑单元具体用于：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

在上述视觉定位装置中，所述方程组建立单元具体用于：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

其中，(d_x,d_y)为像元尺寸，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心；

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

在上述视觉定位装置中，所述坐标系建立单元具体用于：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

在上述视觉定位装置中，所述信息获取单元具体用于：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

其中，角度α、β和γ分别为所述识别对象在所述相机坐标系中的方位角、俯仰角和横滚角。

在上述视觉定位装置中，所述五个标识点是安装在所述视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。

通过根据本发明的视觉定位方法和视觉定位装置，可以实现不具有显著的自然特征的识别对象的姿态的实时和高精度定位。

在根据本发明的视觉定位方法和视觉定位装置中，通过在识别对象上设置标识点来快速实现识别对象的姿态参数的测量。当应用于人体时，根据本发明的视觉定位方法和视觉定位装置适用于室内或封闭空间内，快速实现人体头部姿态参数的测量，具有非接触、不受电磁干扰、工作时间长、稳定性高等优点。

附图说明

图1是根据本发明实施例的视觉定位方法的示意性流程图；

图2是示出根据本发明实施例的视觉定位眼镜上多个标识点之间的拓扑关系的示意图；

图3是示出三点共线性的示意图；

图4是点透视投影成像模型的示意图；

图5是根据本发明实施例的视觉定位装置的示意性框图。

具体实施方式

以下描述用于公开本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。

可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。

以下说明书和权利要求中使用的术语和词不限于字面的含义，而是仅由本发明人使用以使得能够清楚和一致地理解本发明。因此，对本领域技术人员很明显仅为了说明的目的而不是为了如所附权利要求和它们的等效物所定义的限制本发明的目的而提供本发明的各种实施例的以下描述。

虽然比如“第一”、“第二”等的序数将用于描述各种组件，但是在这里不限制那些组件。该术语仅用于区分一个组件与另一组件。例如，第一组件可以被称为第二组件，且同样地，第二组件也可以被称为第一组件，而不脱离发明构思的教导。在此使用的术语“和/或”包括一个或多个关联的列出的项目的任何和全部组合。

在这里使用的术语仅用于描述各种实施例的目的且不意在限制。如在此使用的，单数形式意在也包括复数形式，除非上下文清楚地指示例外。另外将理解术语“包括”和/或“具有”当在该说明书中使用时指定所述的特征、数目、步骤、操作、组件、元件或其组合的存在，而不排除一个或多个其它特征、数目、步骤、操作、组件、元件或其组的存在或者附加。

包括技术和科学术语的在这里使用的术语具有与本领域技术人员通常理解的术语相同的含义，只要不是不同地限定该术语。应当理解在通常使用的词典中限定的术语具有与现有技术中的术语的含义一致的含义。

下面结合附图和具体实施方式对本发明作进一步详细的说明：

如上所述，对于不具有显著的自然特征的识别对象，比如人的头部，常常借助于识别人的头部上佩戴的其他物体，比如眼镜的方式来进行识别。

由于人体工程学原理，眼镜多为不规则的弧线组成，其提供的自然特征也相对有限。因此，在本发明的实施例中，使用了基于合作目标的定位技术，即在眼镜框上布局人工标识物。并且，考虑到眼镜框有限的宽度以及标识物的特征提取，使用了点特征标识物。

并且，考虑到视觉定位眼镜仅能提供有限数量的标识点，本发明不能采用上述PnP问题的各种算法。因此，P3P、P4P、P5P问题是本发明研究的重点。由于P3P问题使用标识点数量较少，其通常含有4组疑似解。通过增加一个标识点，任意三个标识点组成标识点集合，求取标识点集合解的交集即为正确解。P4P问题和P5P问题的解决方案一般使用多个标识点之间的几何约束，即平行、夹角、距离、异面等等，直接或间接解算标识点的三维坐标。Liu.M.L.等人充分利用标识点的几何构型约束，包括夹角、混合积和距离约束等等，使用迭代的方法得到了标识点的精确坐标。Z.Y.Hu等人使用数学方法分析了标识点的几何构型约束问题，量化了姿态解算方程可能解的个数和几何构型的数量之间的关系。Wu PC等人提出了一个解析运动模型用于排除可能解。Yang Guo通过研究透视投影和仿射投影的转换及仿射变换不变形，提出并验证了共面P4P问题的解的上限2个。同时提出了应用于共面P4P问题的奇异值分解算法，但没有进行实验验证。Long Li等人引入了Frobenius范数替代奇异值分解，但是该方法仅提高了算法的运行速度，未能提高算法的精度和抗噪声能力。

因此，根据本发明实施例的一方面，提供了一种视觉定位方法，包括：在识别对象上设置多个标识点；根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；根据标识点的三维坐标，建立标识点坐标系；和，求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。

图1是根据本发明实施例的视觉定位方法的示意性流程图。如图1所示，根据本发明实施例的视觉定位方法包括：S1，在识别对象上设置多个标识点；S2，根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；S3，根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；S4，根据标识点的三维坐标，建立标识点坐标系；和S5，求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。

在上述视觉定位方法中，该识别对象是视觉定位眼镜，且该多个标识点是设置在该视觉定位眼镜的框架上的五个标识点。

这里，本领域技术人员可以理解，应用根据本发明实施例的视觉定位方法的原理，可以对各种识别对象进行视觉定位。即使对于人的头部，也不限于使用视觉定位眼镜来辅助进行定位，还可以使用例如头盔、或者头戴显示器这样的可穿戴设备来辅助进行定位。以下，将以视觉定位眼镜为例对本发明的实施例进行详细描述。

根据眼镜结构，可以在镜框上布局人工标识点。根据标识点数量与定位算法解算复杂程度与精度的关系，在根据本发明实施例的视觉定位方法中，提出了基于5个共面标识点的几何约束姿态解算方法，并设计了相应的视觉定位眼镜。

在上述视觉定位方法中，该多个标识点之间的拓扑关系包括该多个标识点之间的平行和线段比例关系。

图2是示出根据本发明实施例的视觉定位眼镜上多个标识点之间的拓扑关系的示意图。5个共面标识点的位置关系如图3所示，标识点编号为0-4。根据投影不变性对5个标识点进行区分。根据共线性关系可以得到两组具有特殊位置关系的标识点：0号、4号标识点位于直线L₂上，1号、2号、3号标识点位于直线L₁上。由于点的共线性不受透视投影变换的影响，因此使用正弦函数量化表示1号、2号、3号标识点的共线性。图3是示出三点共线性的示意图。如图4所示，利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

其中，是由P₁、P₂、P₃组成的两个矢量。L的取值范围在0和1之间，L越接近1表示两条直线的共线性越低，越接近0则表示两条直线的共线性越高。在视觉测量系统中，共线性会受到图像处理偏差和镜头畸变误差的影响。因此在使用公式(1)判断共线性时，需要添加一个阈值(例如：sin3°)作为判断的准则。

也就是说，在上述视觉定位方法中，对多个标识点进行拓扑区分的步骤具体包括：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

在共线点组P₁、P₂、P₃中，根据其对应图像点x坐标的不同，从右到左可依次区分；同理可以区分共线点组P₀、P₄。至此完成标识点阵列的编号。当然，本领域技术人员可以理解，在上面该中，利用机器视觉完成点的共线性的判断，实际上也可以采用诸如人工标识之类的方法，仅需要对多个标识点完成拓扑区分即可。

图4是点透视投影成像模型的示意图。5个标识点的透视投影成像模型如图5所示。则在根据本发明实施例的视觉定位方法中，建立求解标识点三维坐标的线性方程组的步骤具体包括：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

其中，(d_x,d_y)为像元尺寸，其是相机的制造参数，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心，均为相机内参数，可通过对相机标定获得。

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

在根据本发明实施例的视觉定位方法中，在获得标识点在相机坐标系下的坐标之后，建立标识点坐标系的步骤具体包括：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

在根据本发明实施例的视觉定位方法中，求解标识点坐标系的旋转矩阵以得到识别对象的姿态信息的步骤具体包括：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

其中，角度α、β和γ分别为该识别对象在该相机坐标系中的方位角、俯仰角和横滚角。这样，就得到了识别对象在相机系统中的转动参数，从而实现了识别对象的视觉定位。

在根据本发明实施例的LED视觉定位眼镜中，将5个红外LED灯分别固定安装在特殊设计的眼镜框上的5个沉降孔中，并连接LED灯供电电路。在根据本发明实施例的测量过程中，被测目标带上视觉定位眼镜，点亮LED灯，即可在红外摄像机视场范围内转动。摄像机拍摄视觉定位眼镜图像，并将图像传输至运算处理单元；经过图像处理提取标识点图像坐标，将标识点阵列编号；最后将标识点图像坐标和世界坐标输入标识点三维坐标解算算法，获取标识点在摄像机坐标系下的坐标，由该坐标计算被测目标的姿态信息。

也就是说，在根据本发明实施例的视觉定位方法中，该五个标识点是安装在该视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。

根据本发明实施例的另一方面，提供了一种视觉定位装置，包括：设置单元，用于在识别对象上设置多个标识点；拓扑单元，用于根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；坐标获取单元，用于根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；坐标系建立单元，用于根据标识点的三维坐标，建立标识点坐标系；和，信息获取单元，用于求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。

图5是根据本发明实施例的视觉定位装置的示意性框图。如图5所示，根据本发明实施例的视觉定位装置100包括：设置单元101，用于在识别对象上设置多个标识点；拓扑单元102，用于根据设置单元101所设置的多个标识点之间的位置关系，对该多个标识点进行拓扑区分；坐标获取单元103，用于根据拓扑单元102获得的多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；坐标系建立单元104，用于根据坐标获取单元103所获取的标识点的三维坐标，建立标识点坐标系；和，信息获取单元105，用于求解坐标系建立单元104所建立的标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。

在上述视觉定位装置中，该识别对象是视觉定位眼镜，且该多个标识点是设置在该视觉定位眼镜的框架上的五个标识点。

在上述视觉定位装置中，该多个标识点之间的拓扑关系包括该多个标识点之间的平行和线段比例关系。

在上述视觉定位装置中，该拓扑单元具体用于：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

在上述视觉定位装置中，该方程组建立单元具体用于：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

其中，(d_x,d_y)为像元尺寸，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心；

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

在上述视觉定位装置中，该坐标系建立单元具体用于：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

在上述视觉定位装置中，该信息获取单元具体用于：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

其中，角度α、β和γ分别为该识别对象在该相机坐标系中的方位角、俯仰角和横滚角。

在上述视觉定位装置中，该五个标识点是安装在该视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。

通过根据本发明实施例的视觉定位方法和视觉定位装置，可以实现不具有显著的自然特征的识别对象的姿态的实时和高精度定位。

在根据本发明实施例的视觉定位方法和视觉定位装置中，通过在识别对象上设置标识点来快速实现识别对象的姿态参数的测量，从而实现对象在相机系统中的视觉定位。当应用于人体时，根据本发明的视觉定位方法和视觉定位装置适用于室内或封闭空间内，快速实现人体头部姿态参数的测量，具有非接触、不受电磁干扰、工作时间长、稳定性高等优点。

当然，本发明还可有其它多种实施例，在不背离本发明的精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明做出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (16)

1.一种视觉定位方法，包括：

在识别对象上设置多个标识点；

根据所述多个标识点之间的位置关系，对所述多个标识点进行拓扑区分；

根据所述多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解所述多个标识点的三维坐标；

根据标识点的三维坐标，建立标识点坐标系；和

求解所述标识点坐标系的旋转矩阵，以得到所述识别对象的姿态信息。

2.根据权利要求1所述的视觉定位方法，其特征在于，

所述识别对象是视觉定位眼镜，且所述多个标识点是设置在所述视觉定位眼镜的框架上的五个标识点。

3.根据权利要求1所述的视觉定位方法，其特征在于，

所述多个标识点之间的拓扑关系包括所述多个标识点之间的平行和线段比例关系。

4.根据权利要求2所述的视觉定位方法，其特征在于，所述对多个标识点进行拓扑区分的步骤具体包括：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

$L=sin\mathrm{\θ}=\sqrt{1-{\left(\frac{{\stackrel{\→}{v}}_1\·{\stackrel{\→}{v}}_2}{\left|{\stackrel{\→}{v}}_1\right|\·\left|{\stackrel{\→}{v}}_2\right|}\right)}^2}---\left(1\right)$

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

5.根据权利要求2所述的视觉定位方法，其特征在于，所述建立求解标识点三维坐标的线性方程组的步骤具体包括：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

$\left\{\begin{array}{c}{z}_{ci}{I}_{ui}=\left[\begin{array}{ccc}{s}_{x}f/d_x& 0& u_0\\ 0& s_{x}f/d_y& v_0\\ 0& 0& 1\end{array}\right]P_{ci}=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]P_{ci}={\mathrm{KP}}_{ci}\\ P_{ci}={\mathrm{\λ}}_i{K}^{-1}{I}_{ui}({\mathrm{\λ}}_i=z_{ci})\end{array}\right.---\left(2\right)$

其中，(d_x,d_y)为像元尺寸，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心；

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

$\left\{\begin{array}{c}k=\left|P_{w2}{P}_{w3}\right|/\left|P_{w0}{P}_{w4}\right|\\ P_{w2}=k(P_{w4}-P_{w0})+P_{w3}=\[P_{w0},P_{w3},P_{w4}\]{\[-k,1,k\]}^T\end{array}\right.---\left(3\right)$

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

$\left\{\begin{array}{c}{\mathrm{\λ}}_0=-{\mathrm{\η}}_0{\mathrm{\λ}}_2/k,{\mathrm{\λ}}_2={\mathrm{\η}}_2{\mathrm{\λ}}_2,{\mathrm{\λ}}_3={\mathrm{\η}}_3{\mathrm{\λ}}_2,{\mathrm{\λ}}_4={\mathrm{\η}}_4{\mathrm{\λ}}_2/k({\mathrm{\η}}_2=1)\\ k=\left|P_{w2}{P}_{w3}\right|/\left|P_{w0}{P}_{w4}\right|\\ \left|P_{w0}{P}_{w4}\right|=\left|P_{c0}{P}_{c4}\right|={\mathrm{\λ}}_2|{\mathrm{\η}}_4{K}^{-1}{I}_{u4}/k+{\mathrm{\η}}_0{K}^{-1}{I}_{u0}/k|\\ {\mathrm{\λ}}_2=\left|P_{w2}{P}_{w3}\right|/|{\mathrm{\η}}_4{K}^{-1}{I}_{u4}+{\mathrm{\η}}_0{K}^{-1}{I}_{u0}|\end{array}\right.---\left(11\right)$

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

$\left\{\begin{array}{c}{P}_{c0}=-{\mathrm{\λ}}_2{\mathrm{\η}}_0{K}^{-1}{I}_{u0}/k\\ P_{c2}={\mathrm{\λ}}_2{\mathrm{\η}}_2{K}^{-1}{I}_{u2}\\ P_{c3}={\mathrm{\λ}}_2{\mathrm{\η}}_3{K}^{-1}{I}_{u3}\\ P_{c4}={\mathrm{\λ}}_2{\mathrm{\η}}_4{K}^{-1}{I}_{u4}/k\end{array}\right.---\left(12\right).$

6.根据权利要求2所述的视觉定位方法，其特征在于，所述建立标识点坐标系的步骤具体包括：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

7.根据权利要求2所述的视觉定位方法，其特征在于，所述求解标识点坐标系的旋转矩阵以得到识别对象的姿态信息的步骤具体包括：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

$R=\[{\stackrel{\→}{i}}_{t+1},{\stackrel{\→}{j}}_{t+1},{\stackrel{\→}{k}}_{t+1}\]{\[{\stackrel{\→}{i}}_t,{\stackrel{\→}{j}}_t,{\stackrel{\→}{k}}_t\]}^{-1}---\left(15\right)$

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& \cos \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(16\right)$

其中，角度α、β和γ分别为所述识别对象在所述相机坐标系中的方位角、俯仰角和横滚角。

8.根据权利要求2所述的视觉定位方法，其特征在于，

所述五个标识点是安装在所述视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。

9.一种视觉定位装置，包括：

设置单元，用于在识别对象上设置多个标识点；

拓扑单元，用于根据所述多个标识点之间的位置关系，对所述多个标识点进行拓扑区分；

坐标获取单元，用于根据所述多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解所述多个标识点的三维坐标；

坐标系建立单元，用于根据标识点的三维坐标，建立标识点坐标系；和

信息获取单元，用于求解所述标识点坐标系的旋转矩阵，以得到所述识别对象的姿态信息。

10.根据权利要求9所述的视觉定位装置，其特征在于，

所述识别对象是视觉定位眼镜，且所述多个标识点是设置在所述视觉定位眼镜的框架上的五个标识点。

11.根据权利要求9所述的视觉定位装置，其特征在于，

所述多个标识点之间的拓扑关系包括所述多个标识点之间的平行和线段比例关系。

12.根据权利要求10所述的视觉定位装置，其特征在于，所述拓扑单元具体用于：

利用以下公式(1)判断三个点P₁、P₂、P₃是否位于同一直线上：

$L=sin\mathrm{\θ}=\sqrt{1-{\left(\frac{{\stackrel{\→}{v}}_1\·{\stackrel{\→}{v}}_2}{\left|{\stackrel{\→}{v}}_1\right|\·\left|{\stackrel{\→}{v}}_2\right|}\right)}^2}---\left(1\right)$

其中，是由P₁、P₂、P₃组成的两个矢量，且当L小于预定阈值时，确定三个点P₁、P₂、P₃位于同一直线上。

13.根据权利要求10所述的视觉定位装置，其特征在于，所述方程组建立单元具体用于：

令标识点在世界坐标系下的坐标为P_wi(x_wi,y_wi,z_wi)，在相机坐标系下的坐标为P_ci(x_ci,y_ci,z_ci)，且在成像面上的像素坐标为I_ui(x_ui,y_ui,1)，则P_ci(x_ci,y_ci,z_ci)与I_ui(x_ui,y_ui,1)的关系表示为以下公式(2)：

$\left\{\begin{array}{c}{z}_{ci}{I}_{ui}=\left[\begin{array}{ccc}{s}_{x}f/d_x& 0& u_0\\ 0& s_{x}f/d_y& v_0\\ 0& 0& 1\end{array}\right]P_{ci}=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]P_{ci}={\mathrm{KP}}_{ci}\\ P_{ci}={\mathrm{\λ}}_i{K}^{-1}{I}_{ui}({\mathrm{\λ}}_i=z_{ci})\end{array}\right.---\left(2\right)$

其中，(d_x,d_y)为像元尺寸，s_x为不垂直因子，f为焦距，(u₀,v₀)为图像中心；

令5个标识点中P_w2P_w3//P_w0P_w4，设2号点到3号点的距离|P_w2P_w3|与0号点到4号点的距离|P_w0P_w4|比为k，得到以下公式(3)：

$\left\{\begin{array}{c}k=\left|P_{w2}{P}_{w3}\right|/\left|P_{w0}{P}_{w4}\right|\\ P_{w2}=k(P_{w4}-P_{w0})+P_{w3}=\[P_{w0},P_{w3},P_{w4}\]{\[-k,1,k\]}^T\end{array}\right.---\left(3\right)$

令R、T为标识点从世界坐标系到相机坐标系的变换，得到以下公式(4)：

P_ci＝R·P_wi+T (4)

将公式(4)带入公式(3)，得到公式(5)：

P_c2＝R·P_w2+T

＝R·(k(P_w4-P_w0)+P_w3)+T

＝R·(k(P_w4-P_w0+T-T)+P_w3)+T

＝k(RP_w4-RP_w0+T-T)+RP_w3+T

＝k(P_c4-P_c0)+P_c3 (5)

因此得到公式(6)：

P_c2＝[P_c0,P_c3,P_c4][-k,1,k]^T (6)

令P_c＝[P_c0,P_c3,P_c4]，根据公式(2)和公式(6)得到公式(7)：

P_c＝[P_c0,P_c3,P_c4]＝K^-1diag(λ₀,λ₃,λ₄)[I_u0,I_u3,I_u4] (7)

将公式(7)两边同时去逆并乘以P_c2得到公式(8)：

P_c ^-1P_c2＝diag(1/λ₀,1/λ₃,1/λ₄)[I_u0,I_u3,I_u4]^-1K(λ₂K^-1I_u2)

＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[I_u0,I_u3,I_u4]^-1I_u2 (8)

令[η₀,η₃,η₄]＝[I_u0,I_u3,I_u4]^-1I_u2，带入公式(8)得到公式(9)：

P_c ^-1P_c2＝diag(λ₂/λ₀,λ₂/λ₃,λ₂/λ₄)[η₀,η₃,η₄] (9)

根据公式(6)得到公式(10)：

P_c ^-1P_c2＝[-k,1,k]^T (10)

联立公式(9)和公式(10)求解出λ_i，如以下公式(11)表示：

$\left\{\begin{array}{c}{\mathrm{\λ}}_0=-{\mathrm{\η}}_0{\mathrm{\λ}}_2/k,{\mathrm{\λ}}_2={\mathrm{\η}}_2{\mathrm{\λ}}_2,{\mathrm{\λ}}_3={\mathrm{\η}}_3{\mathrm{\λ}}_2,{\mathrm{\λ}}_4={\mathrm{\η}}_4{\mathrm{\λ}}_2/k({\mathrm{\η}}_2=1)\\ k=\left|P_{w2}{P}_{w3}\right|/\left|P_{w0}{P}_{w4}\right|\\ \left|P_{w0}{P}_{w4}\right|=\left|P_{c0}{P}_{c4}\right|={\mathrm{\λ}}_2|{\mathrm{\η}}_4{K}^{-1}{I}_{u4}/k+{\mathrm{\η}}_0{K}^{-1}{I}_{u0}/k|\\ {\mathrm{\λ}}_2=\left|P_{w2}{P}_{w3}\right|/|{\mathrm{\η}}_4{K}^{-1}{I}_{u4}+{\mathrm{\η}}_0{K}^{-1}{I}_{u0}|\end{array}\right.---\left(11\right)$

根据以下公式(12)由λ_i表示标识点在相机坐标系下的坐标为：

$\left\{\begin{array}{c}{P}_{c0}=-{\mathrm{\λ}}_2{\mathrm{\η}}_0{K}^{-1}{I}_{u0}/k\\ P_{c2}={\mathrm{\λ}}_2{\mathrm{\η}}_2{K}^{-1}{I}_{u2}\\ P_{c3}={\mathrm{\λ}}_2{\mathrm{\η}}_3{K}^{-1}{I}_{u3}\\ P_{c4}={\mathrm{\λ}}_2{\mathrm{\η}}_4{K}^{-1}{I}_{u4}/k\end{array}\right.---\left(12\right).$

14.根据权利要求10所述的视觉定位装置，其特征在于，所述坐标系建立单元具体用于：

根据P_c0、P_c2、P_c3、P_c4四个标识点建立如以下公式(13)表示的标识点坐标系M，坐标系原点为点P_c4：

式中为x轴方向向量，为z轴方向向量，为y轴方向向量。

15.根据权利要求10所述的视觉定位装置，其特征在于，所述信息获取单元具体用于：

标识点群在t时刻的坐标系M_t与t+1时刻的坐标系M_t+1之间的关系由公式(14)表示：

M_t+1＝R·M_t+T (14)

由向量的平移不变性得到以下公式(15)：

$R=\[{\stackrel{\→}{i}}_{t+1},{\stackrel{\→}{j}}_{t+1},{\stackrel{\→}{k}}_{t+1}\]{\[{\stackrel{\→}{i}}_t,{\stackrel{\→}{j}}_t,{\stackrel{\→}{k}}_t\]}^{-1}---\left(15\right)$

以公式(16)将公式(15)得到的旋转矩阵欧拉角化：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& \cos \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\\ \cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(16\right)$

其中，角度α、β和γ分别为所述识别对象在所述相机坐标系中的方位角、俯仰角和横滚角。

16.根据权利要求10所述的视觉定位装置，其特征在于，

所述五个标识点是安装在所述视觉识别眼镜的框架上的五个沉降孔中的五个红外LED灯。