[發明專利]一種基于單目ORB-SLAM的自動化地圖恢復方法

有效

• 申請號: 201911325034.7
• 申請日: 2019-12-20
• 公開（公告）號: CN111141295B
• 公開（公告）日: 2023-04-25
• 發明（設計）人: 薛源;孫永榮;趙科東;趙偉;李榮冰
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/32
• 分類號: G01C21/32
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 朱楨榮

1.一種基于單目ORB-SLAM的自動化地圖恢復方法，其特征在于，包括以下步驟：

步驟1、同步采集圖像序列以及實時動態差分RTK數據，進而基于單目ORB-SLAM系統進行地圖跟蹤與構建，恢復地圖信息；

步驟2、若建圖跟蹤失敗，保存當前地圖信息，并記錄關鍵幀與地圖點序號；

步驟3、重置視覺地圖構建系統，利用單目初始化進行地圖重建；

步驟4、基于RTK信息輔助，自動化恢復并融合丟失地圖信息；

步驟5、若視覺跟蹤再次失敗，重復步驟2-4直到地圖構建結束；

步驟4所述的自動化恢復并融合丟失地圖信息是指在新的視覺坐標系下恢復M幀關鍵幀以及相應地圖點信息后，判斷舊地圖是否存在，若舊地圖標志量LastMapFlag為真，自動加載該地圖信息，基于RTK地理信息的約束，將舊地圖關鍵幀和地圖點坐標轉換到當前新視覺坐標系下，并融合到當前地圖構建中；M為14到26之間的整數；

所述將舊地圖關鍵幀和地圖點坐標轉換到當前新視覺坐標系下，并融合到當前地圖構建中；具體包括以下步驟：

4.1、建立視覺地圖從視覺坐標系到真實地理坐標系下的相似變換模型，以獲得視覺信息的縮放、平移以及旋轉關系；

X_g＝s_vgR_vgX_v+t_vg

其中，s_vg為尺度因子且滿足s_vg＞0，R_vg為三維空間內的旋轉矩陣，t_vg＝(t_x,t_y,t_z)^T為平移向量，X_g為地圖在真實地理坐標系下的位置坐標，X_v為地圖在視覺坐標系下的位置坐標，t_x為x軸在視覺坐標系下的平移量，t_y為y軸在視覺坐標系下的平移量，t_z為z軸在視覺坐標系下的平移量；

4.2、建立目標函數，利用對齊RTK數據以及單目視覺恢復的關鍵幀軌跡信息，采用優化最小二乘目標函數的形式求解相似變換參數；

其中，所需估計的關鍵幀集為(c₁,...,c_i,...,c_m)，對齊的RTK數據集為(r₁,...,r_i,...,r_m)，其中，c_i為第i個關鍵幀在視覺坐標系下的坐標，r_i為第i個關鍵幀在地理坐標系下的坐標，R、t、s分別為地圖從視覺坐標系到地理坐標系下變換的旋轉矩陣、平移向量、尺度因子，m為關鍵幀個數，e²(R_vg,t_vg,s_vg)為相似變換后的視覺估計值與對齊RTK兩組數據之間的均方差；

4.3、對舊地圖從原視覺坐標系到新視覺坐標系的坐標變換參數進行求解；

根據步驟4.2的目標函數分別求解新舊地圖的相似變換參數為(R₁,t₁,s₁)、(R₂,t₂,s₂)，基于真實地理信息對兩段地圖信息的約束關系，進而將舊地圖的三維點信息轉換到當前新視覺坐標系下；

X″_vj＝s₂₁R₂₁X′_vj+t₂₁

其中，X′_vj為舊地圖中第j個地圖點在原視覺坐標系下的位置，X″_vj為第j個地圖點在新視覺坐標系下的位置，R₂₁、t₂₁、s₂₁分別為舊地圖從原視覺坐標系到新視覺坐標系下變換的旋轉矩陣、平移向量、尺度因子，R₁、t₁、s₁分別為新地圖從新視覺坐標系到地理坐標系下變換的旋轉矩陣、平移向量、尺度因子，R₂、t₂、s₂分別為舊地圖從原視覺坐標系到地理坐標系下變換的旋轉矩陣、平移向量、尺度因子，上標T為轉置；

4.4、根據相似變換關系，將舊地圖關鍵幀的位姿信息轉換到當前新視覺坐標系下；

第i個舊關鍵幀在原視覺坐標系下的位置坐標為則第i個舊關鍵幀在新視覺坐標系下的位置坐標為其中，為第i個舊關鍵幀在原視覺坐標系下的旋轉矩陣、平移向量，為第i個舊關鍵幀在新視覺坐標系下的旋轉矩陣、平移向量；

4.5、將舊地圖信息添加到當前新地圖變量中，獲得滿足新視覺坐標系下位置分布合理、物理尺度與當前新地圖一致的關鍵幀以及地圖點數據，同時計算舊關鍵幀的視覺詞袋，并將與舊地圖構建相關的索引信息添加到單目ORB-SLAM系統的新地圖構建變量中，使其參與當前系統的地圖測繪，為跟蹤重定位以及回環修正提供更冗余的地圖信息。