[發明專利]一種基于正十二面體的單目6D位姿估計方法

有效

• 申請號: 202110022822.X
• 申請日: 2021-01-08
• 公開（公告）號: CN112734843B
• 公開（公告）日: 2023-03-21
• 發明（設計）人: 孫昊;譚英倫;段倫輝;崔睿;吳夢坤
• 申請（專利權）人: 河北工業大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/80
• 代理公司: 天津翰林知識產權代理事務所(普通合伙) 12210
• 代理人: 王瑞

1.一種基于正十二面體的單目6D位姿估計方法，其特征在于，該方法包括以下步驟：

第一步、使用ArUco碼對正十二面體進行標記，得到ArUco碼標記的正十二面體，且ArUco碼的幾何中心與其貼附的面的幾何中心重合；標定單目相機，再通過標定后的單目相機觀測ArUco碼標記的正十二面體；

第二步，分別對當前幀和初始幀中所有觀測到的若干個ArUco碼進行定位，再分別獲得當前幀和初始幀中所有觀測到的ArUco碼的編碼id_i,i＝1,2,3,...,n和ArUco碼中心點的空間坐標A_i(x_i,y_i,z_i),i＝1,2,3,...,n，n表示某一幀觀測到的ArUco碼的數量；

第三步、對第二步得到的當前幀ArUco碼中心點的空間坐標進行多元非線性擬合計算出當前幀正十二面體內切球球心坐標作為當前幀正十二面體體心的空間坐標t₀，對初始幀ArUco碼中心點的空間坐標進行多元非線性擬合計算出初始幀正十二面體內切球球心坐標作為初始幀正十二面體體心的空間坐標t_O；再通過公式t_rel＝t₀-t_O求得當前幀正十二面體體心相對于初始幀的相對空間坐標t_rel；

在相機坐標系原點處構建一個虛擬正十二面體，虛擬正十二面體體心與相機坐標系原點重合，再使用ArUco碼對虛擬正十二面體進行標記并得到虛擬正十二面體所有ArUco碼的編碼及其中心點的空間坐標A_Wi(x_Wi,y_Wi,z_Wi,id_Wi),i＝1,2,3,...,12；再將第二步得到的當前幀ArUco碼中心點的空間坐標及其編碼與A_Wi(x_Wi,y_Wi,z_Wi,id_Wi),i＝1,2,3,...,12通過迭代最近點方法計算出當前幀正十二面體的旋轉矩陣R，將初始幀ArUco碼中心點的空間坐標及其編碼與A_Wi(x_Wi,y_Wi,z_Wi,id_Wi),i＝1,2,3,...,12通過迭代最近點方法計算出初始幀正十二面體的旋轉矩陣R_O，再通過公式計算得到當前幀正十二面體相對于初始幀的相對旋轉矩陣R_rel；

再根據相對旋轉矩陣R_rel和相對空間坐標t_rel，得到當前幀正十二面體相對于初始幀正十二面體的位姿矩陣T_rel：

式(2)中，SE(3)表示矩陣的屬性；

當前幀正十二面體體心的空間坐標的獲得：將第二步得到的當前幀觀測到的正十二面體部分面上的ArUco碼中心點的空間坐標視為當前幀正十二面體內切球球面點(x_i,y_i,z_i)的稀疏點云坐標；當單目相機獲得了大于三個ArUco碼中心點的空間坐標后，將當前幀正十二面體內切球球面點(x_i,y_i,z_i)與內切球半徑視為參數，則內切球的標準方程為：

(x_i-x₀)²+(y_i-y₀)²+(z_i-z₀)²-R_內²＝0 (3)

式(3)中，R_內為正十二面體內切球半徑，a為正十二面體的棱長；

利用非線性最小二乘，對式(3)進行該球面多元非線性函數的球心擬合，構建損失函數如下：

由于多元非線性擬合對初始點要求較高，極容易陷入局部極小值中，因此利用已知球面點坐標(x_i,y_i,z_i)對內切球球心坐標生成一組多元非線性擬合的初始值R_初：

再將R_初代入式(4)中獲得損失函數J的一個值，隨后不斷更改R_初的值使得式(4)中損失函數J的值不斷減小，經過多次迭代求解后可以獲得一組使得損失函數J最小的(x₀,y₀,z₀)即為當前幀內切球球心坐標；

當前幀正十二面體的旋轉矩陣的獲得：

搜索A_Wi中與第二步得到的當前幀實際正十二面體的ArUco碼的編碼對應的虛擬正十二面體的ArUco碼的編碼，將編碼相同的進行匹配，進而獲得空間中的一組匹配點：

(A_Wi,A_i),i＝1,2,3,...,n (6)

該組匹配點由實際正十二面體上觀測到的ArUco碼中心點的空間坐標與虛擬正十二面體上與之對應的ArUco碼中心點的空間坐標組成，通過式(7)分別將其去質心化：

得到的q_i為實際正十二面體ArUco碼中心點的空間坐標的去質心點，q_i'為虛擬正十二面體ArUco碼中心點的空間坐標的去質心點，進而得到q_i和q_i'的關系如下：

定義誤差項：

根據式(9)求取使得誤差項E最小的當前幀正十二面體的旋轉矩陣R，式(9)中第一項與優化目標無關，第二項中R^TR＝I亦與優化目標無關，則誤差項變為：

式(10)中，tr表示矩陣的跡；

為求解式(10)中的優化目標R，定義矩陣：

對式(11)中的W進行奇異值分解，得到：

W＝UΣV^T (12)

式(12)中，Σ為W的特征值矩陣，U與V為對角矩陣，則當前幀正十二面體的旋轉矩陣R為：

R＝UV^T (13)；

第四步、首先構建被測物體坐標系，將正十二面體固定于被測物體上，并計算正十二面體相對于被測物體的相對空間坐標t_ref和相對旋轉矩陣R_ref，得到正十二面體相對于被測物體的位姿矩陣T_ref：

則被測物體的相對位姿矩陣T：

得到當前幀被測物體相對于初始幀的空間姿態變化。