[發明專利]一種Slam算法優化基于模型的跟蹤方法

有效

• 申請號: 201711249812.X
• 申請日: 2017-12-01
• 公開（公告）號: CN107958466B
• 公開（公告）日: 2022-03-29
• 發明（設計）人: 張新宇;常亮;沈潤杰;張夢超
• 申請（專利權）人: 大唐國信濱海海上風力發電有限公司;同濟大學
• 主分類號: G06T7/70
• 分類號: G06T7/70;G06T7/30;G06T7/13
• 代理公司: 北京元中知識產權代理有限責任公司 11223
• 代理人: 王明霞

1.一種Slam算法優化基于模型的跟蹤方法，其特征在于，包括以下步驟：

S1、對目標物進行初始化，得到目標物在圖像中的精確位姿，確定目標物的邊沿；

S2、進行邊沿追蹤，確定目標物運動之后的位姿；

S3、使用SLAM算法對目標物所在新的圖像位置進行點云提取；

S4、點云匹配，確定目標物的精確位姿；

在步驟S1中，所述的對目標物進行初始化包括：根據已知目標物的模型，預先建立其點云數據庫，將相機采集的圖像與預先準備好的目標物的圖像做ORB匹配，然后確定目標物在圖像中的位置，提取目標物在圖像中的點云數據，最后將得到的點云數據與所建立的點云數據庫進行點云匹配，得到點云集間的精確變換關系；

所述將相機采集的圖像與預先準備好的目標物的圖像做ORB匹配包括，分別獲取該兩圖像的特征點，得到特征描述子，并判斷兩圖像的特征描述子之間的歐式距離是否小于設定閾值，若判斷結果為是，則判斷兩圖像相匹配，否則判斷兩圖像不匹配；

所述將得到的點云數據與所建立的點云數據庫進行點云匹配，得到點云集間的精確變換關系包括采用點云匹配的算法，得到精確的物體位姿信息，該點云匹配的算法包括以下步驟：

S401、點云特征點選取過程；

S402、計算特征描述子過程；

S403、匹配特征點，對點云進行粗匹配，得到粗匹配的坐標變化T和尺度變換S；

S404、迭代優化過程；

步驟S404包括以下步驟：

S441、設p_i＝(x_i,y_i,z_i),q_j＝(x_j,y_j,z_j)為三維空間中兩個3D點，他們的歐氏距離為：

為求解任意兩個點云集P和Q的旋轉矩陣R和坐標變換T，對于點云P中的任一特征點p_i,和Q中與p_i所對應的特征點q_j，有q_j＝Rp_i+T，利用最小二乘法求最優解得誤差E為：

其中N代表兩個點云中匹配的特征點總量，使用最小二乘法求解使誤差E最小的R與T；

S442、平行移動和旋轉分離：先對坐標變換T進行初始估算，分別得到點集P和Q的中心：

S443、構造點集P和Q的協方差矩陣：

其中，μ_Pμ^T_Q代表點云中心化矩陣,q_i^T為向量的轉置；

S444、由協方差矩陣構造4x4對稱矩陣：

其中，I₃是3×3單位矩陣；

其中，Δ為指代符號，代表Q(∑_P,Q)；

S445、計算Q(∑_P,Q)的特征值和特征向量，其最大特征值對應的特征向量即為最佳旋轉向量q_R＝[q₀q₁q₂q₃]^T；

S446、計算最佳平移向量:

S447、將旋轉矩陣和平移向量疊加在點云Q上，之后再帶入式：

其中，若誤差小于設定閾值，則迭代結束，否則繼續重復以上步驟；迭代結束之后，所得到的旋轉矩陣與平移向量即是目標的初始位置，初始化過程結束。

2.根據權利要求1所述的一種Slam算法優化基于模型的跟蹤方法，其特征在于，在進行目標物初始化后，得到目標物的位姿，并根據已知的目標物三維模型，確定目標物的邊沿在圖像中的坐標，將三維模型與實際圖像擬合。