[發明專利]動態環境下基于幾何與運動約束的機器人RGB-D SLAM方法

有效

• 申請號: 202011381750.X
• 申請日: 2020-12-01
• 公開（公告）號: CN112378409B
• 公開（公告）日: 2022-08-12
• 發明（設計）人: 艾青林;劉剛江
• 申請（專利權）人: 杭州宇芯機器人科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06T7/80
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種動態環境下基于幾何與運動約束的機器人RGB-D SLAM方法，其特征在于，所述方法包括如下步驟：

步驟1：進行相機內參標定；

步驟2：在視頻流中依次對其中的圖像幀進行獲取，首先對獲取的圖像幀建立圖像金字塔，然后分別在圖像金字塔上進行圖像分塊，獲取一定窗口大小的圖像區域，然后利用現有的特征點提取算法在每個分塊里分別進行特征點提取和建立對應特征點的描述子；

步驟3：判斷該幀是否是視頻流里的第一幀，如果是第一幀，則進行初始化操作，否則跳過這個步驟，過程如下：

步驟3.1：將RGB圖像平面的特征點和深度圖像中的像素點相互對應；

步驟3.2：將深度圖像中的像素點轉換為該點離相機平面的距離；

步驟3.3：使用特征點在RGB圖像像素平面的坐標和對應的深度圖像像素值轉換得到的該點距離相機平面的距離，算出該點在相機坐標系下的坐標P,像素坐標(u,v)和深度圖像素值d轉換為坐標P的轉換方式為：

P＝K[u v d]^T

其中K為相機內參；

步驟3.4：將該幀的相機坐系定為世界坐標系，并將步驟3.3獲取的點加入1號地圖，然后跳轉回步驟2；

步驟4，利用特征點描述子的距離匹配圖像中的特征點與1、2號地圖中的3D點，計算相機位姿，將相機坐標系下的坐標P轉換到世界坐標系下的坐標x，并進行關鍵幀選取；過程如下：

步驟4.1，由于該幀非第一幀，即1號地圖中必然存在3D點，且這些點被會認為是靜止的3D點，使用當前幀的ORB特征點的描述子分別與1號地圖以及2號地圖中的3D點的描述子進行匹配，根據配對的特征點對，使用漢明距離，取比例為a的漢明距離最小的點作為正確配對點，0a1，成功配對后可以得到圖像中這部分特征點在世界坐標系下的坐標；

步驟4.2，當前幀特征點中，成功與1號地圖配對上的部分特征點在世界坐標系下的坐標記為x_i，該點所在的圖像金字塔層數為n_i，該點在RGB圖像中的像素坐標和深度圖像對應的像素值組合成Q_i＝[u,v,d]，對于當前幀特征點中，成功與2號地圖配對上的部分特征點在世界坐標系下的坐標記為x_j，該點所在的圖像金字塔層數為n_j，該點在RGB圖像中的像素坐標和深度圖像對應的像素值組合成Q_j＝[u,v,d]，與1號地圖中的點不同的是，2號地圖中的點可能是運動的，因此，需要對2號地圖中的點坐標進行預測；

步驟4.3，構建殘差模型，殘差方程為：

其中，

Σ₁＝n_i×E

Σ₂＝λ×n_j×E

λ表示2號地圖中的點對殘差函數影響的權重，使用g2o對殘差函數進行最小化，得到當前幀相機位姿的最優解T_{cw_k}；

步驟4.4，按照步驟3.1～3.3，獲得該幀中的特征點在相機坐標系下的坐標，根據步驟4.3得到的當前幀相機位姿，將相機坐標系下的點轉換到世界坐標系下；若該幀對應的世界坐標系下的點中，可以被其他幀觀測到的點的比例少于b，0b1，否則判斷為關鍵幀；若距離上一次插入關鍵幀已經過去了較長時間，則判斷為關鍵幀，對于關鍵幀，把所有無法與1號地圖或者2號地圖形成配對的特征點，轉換到世界坐標系下，然后直接加入2號地圖中；

步驟5，當前幀中特征點與2號地圖中的點進行匹配，進行卡爾曼濾波，然后計算場景的動態程度；

步驟6，利用相鄰兩次的速度觀測量對2號地圖中的點進行篩選；

步驟7，根據場景的動態程度，使用多個關鍵幀生成新的1號地圖點；

步驟8，判斷相機運動軌跡是否形成閉環，即相機是否運動到了之前路過的區域，如果發現閉環，則根據1號地圖的點云觀測信息對全局關鍵幀的位姿進行優化，2號地圖點不參與閉環檢測和全局優化，避免動態物體的影響。

2.如權利要求1所述的動態環境下基于幾何與運動約束的機器人RGB-D SLAM方法，其特征在于：所述步驟4中，將地圖點分為1號地圖點與2號地圖點，1號地圖點存放符合對極約束與投影條件的靜態點，2號地圖存放動態地圖點，可疑靜點，狀態未知點。