[發明專利]一種融合ORB閉環檢測的激光SLAM方法

在審

• 申請號: 202011361603.6
• 申請日: 2020-11-27
• 公開（公告）號: CN112595322A
• 公開（公告）日: 2021-04-02
• 發明（設計）人: 安康;付衛婷
• 申請（專利權）人: 浙江同善人工智能技術有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 上海伯瑞杰知識產權代理有限公司 31227
• 代理人: 周兵

1.一種融合ORB閉環檢測的激光SLAM方法，其特征在于包括并行運行的激光SLAM過程以及視覺閉環檢測過程；

在激光SLAM過程中，持續檢測位姿信息，當上一個關鍵時間點與當前時間點的位姿信息的變化率大于變化率閾值時，將當前時間點作為關鍵時間點，并觸發一次視覺閉環檢測過程；

在視覺閉環檢測過程中，從環境圖像中提取圖像關鍵幀，提取當前的圖像關鍵幀的ORB特征，用詞袋模型表示當前的圖像關鍵幀，得到當前的圖像關鍵幀的詞向量；根據當前的圖像關鍵幀的詞向量在歷史圖像關鍵幀集合中進行搜索，以檢測視覺閉環，并將當前的圖像關鍵幀的詞向量加入到歷史圖像關鍵幀集合中；

檢測到視覺閉環后，激光SLAM過程進行視覺輔助激光閉環檢測。

2.根據權利要求1所述的一種融合ORB閉環檢測的激光SLAM方法，其特征在于，所述位姿信息為三維向量P(x，y，θ)，其中(x，y)表示機器人在二維空間中的位置，θ表示機器人繞自身的旋轉角；判斷上一個關鍵時間點與當前時間點的位姿信息的變化率是否大于變化率閾值采用的公式為：

P_i-P_i-1＞P_threshold

其中，P_i是本間點的位姿信息，P_i-1是上一關鍵時間點的位姿信息，P_threshold是變化率閾值；上述公式可展開為：

|θ_i-θ_i-1|＞θ_threshold

其中，xy_threshold取0.1m至1m，θ_threshold取為5°～20°；判斷過程中，將當前時間點以及上一個關鍵時間點的位姿信息代入展開后的公式中，滿足至少一個公式時，即可判定當前時間點的位姿信息的變化率大于變化率閾值。

3.根據權利要求1所述的一種融合ORB閉環檢測的激光SLAM方法，其特征在于，用詞袋模型表示當前的圖像關鍵幀，得到圖像關鍵幀的詞向量具體包括以下步驟：

提取圖像關鍵幀的ORB特征，并將ORB特征點周圍的像素使用高斯濾波平滑；

利用平滑后的圖像計算描述子；描述子為一個長度為L_b的二進制向量，該向量的每一位數值由ORB特征點周圍的一對測試點通過計算得出；對于給定的ORB特征點P，它的描述子B(P)由如下公式給出：

其中Bⁱ(P)是描述子的第i位，I(·)是特征點的光照強度，a_i和b_i是測試點對相對于ORB特征點的偏移；a_i和b_i的選取滿足高斯分布和選取L_b＝256以及S_b＝48可以得到描述子；

選取多個圖像關鍵幀，將選取的圖像關鍵幀的特征點的描述子通過kmeans++算法聚類，得到詞袋模型的詞語；利用詞語的譯頻率-逆文檔頻率作為該詞語的權重w_i，并將一幅圖像關鍵幀B表示為一個詞向量v(B)：

其中1·w_i代表圖像關鍵幀B中存在一個描述子可以通過聚類歸為第i類詞語，0·w_j則表示圖像關鍵幀B中不存在與第j類詞語相近的描述子。

4.根據權利要求3所述的一種融合ORB閉環檢測的激光SLAM方法，其特征在于，進行閉環檢測的過程包括以下步驟：

在歷史圖像關鍵幀集合中搜索與當前的圖像關鍵幀的相似度大于相似度閾值的歷史圖像關鍵幀；

根據當前的圖像關鍵幀以及搜索到的歷史圖像關鍵幀進行組匹配、時間一致性檢驗以及幾何一致性檢驗，若通過時間一致性檢驗以及幾何一致性檢驗表示檢測到視覺閉環。