[發明專利]單目視覺姿態確定方法及系統

有效

• 申請號: 201811102499.1
• 申請日: 2018-09-20
• 公開（公告）號: CN109448055B
• 公開（公告）日: 2021-04-16
• 發明（設計）人: 勞達寶;張慧娟;周維虎;熊芝;崔成君;王國名;李洋;張滋黎;董登峰;紀榮祎
• 申請（專利權）人: 中國科學院光電研究院
• 主分類號: G06T7/73
• 分類號: G06T7/73
• 代理公司: 北京路浩知識產權代理有限公司 11002
• 代理人: 王瑩;吳歡燕

1.一種單目視覺姿態確定方法，其特征在于，包括：

獲取設置在被測目標上的合作立體靶標上每個特征單元的世界坐標，以及每個特征單元在預設相機上成像的像素坐標；

根據每個特征單元的世界坐標和每個特征單元在預設相機上成像的像素坐標，通過EPNP算法確定所述合作立體靶標的初始位姿信息；

將所述合作立體靶標的初始位姿信息作為SoftPOSIT算法的迭代初值，通過SoftPOSIT算法確定所述合作立體靶標的實時姿態信息；

其中，所述合作立體靶標上的每個特征單元均與所述預設相機相對設置，且所述合作立體靶標在所述預設相機的光軸上；

所述合作立體靶標為兩級臺階的棱臺，兩個第一級臺階位于第二級臺階的兩側，且兩個第一級臺階位于同一平面內，第二級臺階位于另一平面內，兩個平面相互平行；

所述將所述合作立體靶標的初始位姿信息作為SoftPOSIT算法的迭代初值，通過SoftPOSIT算法確定所述合作立體靶標的實時姿態信息，具體包括：

基于SoftAssign算法和所述迭代初值，確定所述合作立體靶標上每個特征單元在預設相機上成像中的像素單元的對應關系；

基于POSIT算法、所述對應關系以及所述被測目標運動后所述合作立體靶標上每個特征單元的世界坐標，以及每個特征單元在預設相機上成像的像素坐標，更新所述對應關系以及所述合作立體靶標的位姿信息；

重復執行上述更新過程，直至更新后的所述對應關系和更新后的所述合作立體靶標的位姿信息使全局目標函數取值最小，則將更新后的所述合作立體靶標的位姿信息中的姿態信息作為所述實時姿態信息；

所述合作立體靶標的初始位姿信息包括：初始姿態信息和初始位置信息，所述初始姿態信息包括旋轉矩陣和平移矩陣；

所述基于POSIT算法、所述對應關系以及所述被測目標運動后所述合作立體靶標上每個特征單元的世界坐標，以及每個特征單元在預設相機上成像的像素坐標，更新所述對應關系以及所述合作立體靶標的位姿信息，具體包括：

基于所述對應關系，利用如下公式確定更新后的所述合作立體靶標的位姿信息，并基于更新后的所述合作立體靶標的位姿信息確定更新后的對應關系：

其中，R₁^T,R₂^T,R₃^T分別為旋轉矩陣R的行向量，T＝(T_x T_y T_z)為平移矩陣，f為預設相機的焦距，w為常數，P₀為靶標坐標系的中心，P₀P＝(X_i,Y_i,Z_i,1)，(x_i,y_i)為空間點P在預設相機上的圖像坐標。

2.根據權利要求1所述的單目視覺姿態確定方法，其特征在于，所述合作立體靶標上設置有預設數量個安裝孔，在若干個安裝孔中每個安裝孔內均固定有一圓柱體，在每個圓柱體的頂端均設置有一特征單元。

3.根據權利要求2所述的單目視覺姿態確定方法，其特征在于，每個圓柱體在安裝孔外的高度可調。

4.根據權利要求1所述的單目視覺姿態確定方法，其特征在于，所述根據每個特征單元的世界坐標和每個特征單元在預設相機上成像的像素坐標，通過EPNP算法確定所述合作立體靶標的初始位姿信息，具體包括：

將每一特征單元通過四個虛擬控制點表示，并分別獲取每個虛擬控制點的世界坐標；

基于每個虛擬控制點的世界坐標以及每一特征單元在預設相機上成像的像素坐標，確定每一特征單元在相機坐標系下的三維坐標；

基于每一特征單元中四個虛擬控制點的世界坐標，以及每一特征單元在相機坐標系下的三維坐標，確定所述合作立體靶標的初始位姿信息。

5.根據權利要求1-4中任一項所述的單目視覺姿態確定方法，其特征在于，所述特征單元具體為LED燈或反光片。