[發明專利]一種基于場景辨識的多機器人分布式協作視覺建圖方法

在審

• 申請號: 202110318776.8
• 申請日: 2021-03-25
• 公開（公告）號: CN113074737A
• 公開（公告）日: 2021-07-06
• 發明（設計）人: 王東;董晨;連捷;王偉
• 申請（專利權）人: 大連理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/34
• 代理公司: 大連星海專利事務所有限公司 21208
• 代理人: 王樹本;徐雪蓮

1.一種基于場景辨識的多機器人分布式協作視覺建圖方法，其特征在于包括以下步驟：

步驟1、每個機器人進行單機器人視覺建圖，每個機器人在視覺建圖過程中保存自己路過的場景信息，包括該機器人的標號、關鍵幀類中的關鍵幀序號、關鍵幀描述子、關鍵幀的點云信息以及關鍵幀的連接幀的序號和權重，每保存100至300幀就將存儲文件通過主機通信發送給鄰居機器人，實現將自己路過場景通知給鄰居機器人的功能；

步驟2、相似場景辨識，機器人從鄰居機器人收到存儲場景信息的文件后，將文件中每幀的所有二值描述子即所有單詞轉換成詞袋向量，得到的鄰居機器人的每個關鍵幀信息作為相似場景辨識的候選幀按順序存儲到向量容器中，機器人在對當前幀進行閉環檢測的同時，將當前幀與候選幀中的場景進行相似場景辨識，具體包括以下子步驟：

(a)將當前幀與當前幀的連接幀一一進行詞袋相似度計算，得到最大的詞袋相似度乘上一個0.3～0.8的系數作為詞袋相似度閾值；

(b)找出候選幀中和當前幀具有相同單詞的所有候選幀，同時統計候選幀中與當前幀具有相同單詞數最多的候選幀，兩幀得到最大相同單詞的數目乘上一個0.3～0.8的系數作為最小相同單詞數目閾值；

(c)遍歷候選幀，挑選出相同單詞數大于閾值且詞袋相似度大于閾值的候選幀作為一級候選幀，連同詞袋相似度一起保存；

(d)遍歷一級候選幀，每個一級候選幀的前幾連接幀組成一個連接幀組，每組中的每幀與當前幀的詞袋相似度求和作為該組的詞袋相似度和，遍歷結束后，最大的詞袋相似度和乘上一個0.3～0.8的系數作為最小詞袋相似度和的閾值，從所有連接幀組中挑選出詞袋相似度和大于閾值的連接幀組，每組中與當前幀詞袋相似度最大的幀作為二級候選幀，即相似候選幀，由此得到兩個相似場景；

利用詞袋檢測算法進行詞袋相似度計算，該算法使用快速旋轉描述子特征點，即結合了快速特征點和二值描述子的特征點，圖像的特征點簡單的理解為圖像中顯著的點，即輪廓點，暗區域中的亮點，亮區域中的暗點，描述子是用來描述某個點周圍的特征，將一幅圖像中所有特征點的描述子空間離散化，即將該圖像上的描述子轉換為單詞表示，通過式(1)進行描述：

A＝1·w₁+0·w₂+…+1·w_n＝[w₁，w₂，…，w_n][1,0，…，1]^T (1)

式中，w₁…w_n表示n個不同類型的描述子，1表示圖像中包含該類型描述子，0表示圖像中不包含該類型描述子，T表示向量轉置，A表示圖像中包含的所有描述子，由于視覺單詞在向量中的位置是固定的，所以一幅圖像采用包含的描述子類型轉換為詞袋向量表示，通過式(2)進行描述：

v_A＝[1，0，…，1]^T (2)

式中，v_A表示一副圖像的詞袋向量，然后，對比詞袋向量，計算兩幅圖像的相似度，通過式(3)進行描述：

式中，v_B表示另一幅圖像的詞袋向量，||表示1-范數，score(v_A，v_B)表示兩幅圖像的相似度得分；

步驟3、相似場景投票，機器人的當前幀向滿足投票條件的候選相似場景幀投票，當某個候選相似場景幀的票數達到投票閾值，則認為該幀與給其投票的幀為相似場景，將相似場景的點云信息以及該機器人的子點云地圖保存到點云文件中，發送回該相似場景幀存在的鄰居機器人；當前幀向由場景辨識得到的相似候選幀進行投票，當某相似候選幀滿足投票條件時，當前幀向該相似候選幀投一票；投票條件為：若某相似候選幀的最優連接幀也是當前幀最優連接幀的相似候選幀則投一票，當某幀x票數大于閾值，則認為當前機器人給x幀投票的幀是x幀的相似場景；

步驟4、點云配準，當有相似場景的機器人收到從鄰居機器人發來的點云文件后，對點云文件中兩個相似場景對應的點云信息進行配準得到點云間的變換矩陣，采樣一致性初始配準算法進行粗點云配準，正太分布變換算法進行精點云配準，以此來計算相同場景點云之間的變換矩陣，改善了只用一種點云配準算法結果誤差大的問題，具體包括以下子步驟：

(a)采用采樣一致性初始配準算法進行粗點云配準，從步驟1中保存的關鍵幀點云中取出第x幀的點云，將其與給x幀投票的關鍵幀點云信息分別進行配準，點云信息作為輸入，得到的粗變換矩陣作為精匹配的初始變換矩陣；從待配準點云P中選取n個采樣點，為了盡量保證所采樣的點具有不同的快速點特征直方圖特征，采樣點兩兩之間的距離應滿足大于預先給定最小距離閾值d，在目標點云Q中查找與點云P中采樣點具有相似快速點特征直方圖特征的一個或多個點，從這些相似點中隨機選取一個點作為點云P在目標點云Q中的一一對應點，計算對應點之間剛體變換矩陣，通過求解對應點變換后的距離誤差和函數判斷配準變換的性能，此處的距離誤差和函數使用Huber罰函數表示，記為：

其中

式(5)中，a為一預先給定值，l_k為第k組對應點變換之后的距離差，上述配準的最終目的是在所有變換中找到一組最優的變換，使得誤差函數的值最小，此時的變換即為最終的配準變換矩陣；

(b)采用正太分布變換算法進行精點云配準，采樣一致性初始配準算法得到的粗變換矩陣作為正太分布變換算法的初始矩陣，x幀的點云和投票幀的點云作為輸入得到兩片點云之間的精變換矩陣，正態分布變換算法是一種基于概率分布的點云配準算法，其算法原理是將原始點云空間B按一定尺度分成i個立體單元格{b₁,…,b_i}，計算每個單元格中點的概率密度函數，若單元格內的點數量少于三個，則不計算該單元格，概率密度函數的均值μ和協方差∑按正太分布進行計算，通過式(6)、(7)進行描述：

式中，m表示某個單元格b_i中點的數量，y_k表示單元格b_i中第k個點的坐標向量，對待配準點云空間A中的點x_k的坐標按采樣一致性初始配準算法得到的初始變換矩陣N進行仿射變換得到x_k′，計算變換后的待配準點云空間中的每個點落在原始點云空間相應位置的單元格內的概率分布函數，通過式(8)進行描述：

式中，D表示向量維數，x′_k表示仿射變換后的點坐標，∑表示原始點云相應位置單元格內點的概率密度函數的協方差，μ表示原始點云相應位置單元格內點的概率密度函數的均值，構造目標函數通過式(9)進行描述，

式中，T(N，x_k)表示將待配準點云空間A中的點x_k按變換矩陣N進行變換,n表示點云空間A中點的總數量，迭代得到使目標函數最大的變換矩陣N，若給x幀投票的幀數不只1個，會得到多個配準結果，利用正太分布變換算法的誤差計算函數對多個配準結果進行誤差計算，取誤差最小的作為最終的變換矩陣；

步驟5、得到當前時刻的全局點云地圖，通過配準算法得到的變換矩陣將點云文件中子點云地圖旋轉添加到該機器人的子點云地圖中，通過八叉樹地圖包將點云地圖轉換為占據柵格地圖，得到當前時刻的全局點云地圖。