[發(fā)明專利]視覺定位方法和視覺定位裝置

有效

• 申請?zhí)枺?/td>: 201611069552.3
• 申請日: 2016-11-28
• 公開（公告）號: CN106780609B
• 公開（公告）日: 2019-06-11
• 發(fā)明（設(shè)計）人: 孫鵬飛
• 申請（專利權(quán)）人: 中國電子科技集團公司第三研究所
• 主分類號: G06T7/73
• 分類號: G06T7/73
• 代理公司: 北京天盾知識產(chǎn)權(quán)代理有限公司 11421
• 代理人: 林曉宏

1.一種視覺定位方法，包括：

在識別對象上設(shè)置多個標識點；

根據(jù)所述多個標識點之間的位置關(guān)系，對所述多個標識點進行拓撲區(qū)分；

根據(jù)所述多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解所述多個標識點的三維坐標；

根據(jù)標識點的三維坐標，建立標識點坐標系；和

求解所述標識點坐標系的旋轉(zhuǎn)矩陣，以得到所述識別對象的姿態(tài)信息；

所述多個標識點是設(shè)置在所述識別對象上的五個標識點；

其中，所述建立標識點三維坐標的線性方程組，并求解所述多個標識點的三維坐標的步驟具體包括：

令標識點在世界坐標系下的坐標為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)的關(guān)系表示為以下公式(2)：

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

令5個標識點中P_w2P_w3//P_w0P_w4，設(shè)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]，根據(jù)公式(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)

根據(jù)公式(6)得到公式(10)：

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

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

根據(jù)以下公式(12)由λ_i表示標識點在相機坐標系下的坐標為：