[發明專利]一種基于共面目標的單目視覺位姿估計方法

有效

• 申請號: 202110646875.9
• 申請日: 2021-06-10
• 公開（公告）號: CN113379840B
• 公開（公告）日: 2023-02-28
• 發明（設計）人: 胡慶雷;蔣催催;鄭建英;郭雷
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/62;G06F17/15;G06F17/16;G06F17/18
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 安麗;鄧治平

1.一種基于共面目標的單目視覺位姿估計方法，其特征在于，包括如下步驟：

S1：設計單目視覺成像模型對三維空間共面目標的特征點進行成像，得到特征點的像平面像素點，然后以單目視覺原點為圓心確定單位虛擬球面，將像平面像素點投影到單位虛擬球面上，將像平面像素點轉換到單位虛擬球面上特征點；

S2：采用步驟S1中得到的單位虛擬球面上特征點，基于每兩個單位虛擬球面上特征點之間距離平方并構建目標函數用于共面目標位姿估計，利用列文伯格-馬夸爾特非線性優化方法得到目標函數中位姿耦合項第一個局部極小值；

S3：基于步驟S2中得到的位姿耦合項第一個局部極小值，將之轉換至兩個角度參數和一個長度參數，固定其中一個角度參數和一個長度參數，根據共面目標的共面特性確定位姿耦合項第二個局部極小值估計值，以第二個局部極小值估計值為初始值，利用列文伯格-馬夸爾特非線性優化方法得到位姿耦合項第二個局部極小值；

S4：基于步驟S2、S3中得到的兩個位姿耦合項局部極小值，比較兩個位姿耦合項局部極小值對應目標函數大小，選擇誤差更小的局部極小值作為位姿耦合項全局最優值，利用奇異值分解對全局最優值進行解耦，分別得到共面目標姿態和位置。

2.根據權利要求1所述的方法，其特征在于：所述步驟1中，將像平面像素點轉換到單位虛擬球面上特征點為：

其中u_i為像平面坐標點，R為目標姿態，t為目標位置，p_i為共面目標點在目標坐標系的坐標。

3.根據權利要求2所述的方法，其特征在于：所述步驟S2中，得到目標函數中位姿耦合項第一個局部極小值的具體過程為：

每兩個單位虛擬球面上特征點之間距離平方為：

其中位姿耦合項t'＝R^Tt；

目標函數為：

其中然后利用列文伯格-馬夸爾特非線性優化方法迭代求解位姿耦合項第一個局部極小值t′_1-local。

4.根據權利要求3所述的方法，其特征在于：所述步驟S3，得到位姿耦合項第二個局部極小值具體過程如下：

通過位姿耦合項第一個局部極小值t′_1-local確定對應兩個角度參數γ₁,β₁和一個長度參數l₁為：

t′_1-local＝R_z(γ₁)R_y(β₁)[0 0 l₁]^T (4)

其中t_l1＝[0 0 l₁]^T，f_ij(t')的表達形式轉換為：

特征點p_i經過角度γ₁旋轉后的一組特征點并且第i個特征點求解第二個角度為：

位姿耦合項第二個局部極小值估計值為：

其中為n個β_i-2的平均值，并以t'₂為初始值，進行列文伯格-馬夸爾特非線性優化方法，得到位姿耦合項第二個局部極小值t′_2-local。

5.根據權利要求4所述的方法，其特征在于：所述步驟S4中，求解共面目標姿態和位置的具體過程如下：

比較目標函數值E(t′_1-local)和E(t′_2-local)大小，選擇E(t′_1-local)和E(t′_2-local)中較小值對應t'作為位姿耦合項全局最優值t′_global，并定義：

t”_global＝[t'_global(1),t'_global(2),-t'_global(3)]^T (8)

t′_global和t″_global都是目標函數的收斂值，兩組單位虛擬球面上特征點：

結合s_i和奇異值分解方法得到共面目標兩組姿態和位置為：

選擇t'(3),t”(3)＞0的一組姿態和位置作為最終的共面目標姿態和位置。