[发明专利]视觉定位方法和视觉定位装置

权利要求书

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

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

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

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

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

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

所述多个标识点是设置在所述识别对象上的五个标识点；

其中，所述建立标识点三维坐标的线性方程组，并求解所述多个标识点的三维坐标的步骤具体包括：

令标识点在世界坐标系下的坐标为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]-¹I_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表示标识点在相机坐标系下的坐标为：