[發明專利]基于視覺慣性的SLAM方法、裝置、存儲介質和設備

有效

• 申請號: 201910411404.2
• 申請日: 2019-05-16
• 公開（公告）號: CN110118554B
• 公開（公告）日: 2021-07-16
• 發明（設計）人: 高軍強;林義閩;廉士國
• 申請（專利權）人: 達闥機器人有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 北京英創嘉友知識產權代理事務所(普通合伙) 11447
• 代理人: 魏云鹿

1.一種基于視覺慣性的SLAM方法，其特征在于，所述方法包括：

通過前端線程將第一時刻圖像采集單元采集的圖像信息和慣性測量單元IMU采集的運動信息按照擴展卡爾曼濾波算法進行融合，以獲取初始位姿；

通過所述前端線程根據所述圖像信息和所述初始位姿，確定初始局部地圖；

通過后端線程按照預設的非線性優化算法對所述初始位姿和所述初始局部地圖進行優化，以獲取優化位姿和優化局部地圖，所述前端線程與所述后端線程為并行執行的線程；

根據所述優化位姿和所述優化局部地圖，更新目標初始位姿和目標初始局部地圖，所述目標初始位姿為將第二時刻所述圖像采集單元采集的圖像信息和所述IMU采集的運動信息按照所述擴展卡爾曼濾波算法進行融合，獲取的初始位姿，所述目標初始局部地圖為根據所述第二時刻所述圖像采集單元采集的圖像信息和所述目標初始位姿確定的初始局部地圖，所述第二時刻為獲取所述優化位姿和所述優化局部地圖的時刻；

所述通過所述前端線程根據所述圖像信息和所述初始位姿，確定初始局部地圖，包括：

將所述圖像信息中滿足預設條件的像素點確定為特征點；

根據所述特征點和所述初始位姿進行三維重建，以獲取所述初始局部地圖；

所述按照預設的非線性優化算法對所述初始位姿和所述初始局部地圖進行優化，以獲取優化位姿和優化局部地圖，包括：

當所述圖像信息為關鍵幀時，根據所述初始位姿、所述初始局部地圖和所述第一時刻之前的預設數量個關鍵位姿信息建立目標函數，所述目標函數包括所述IMU的測量殘差和所述圖像采集單元的測量殘差，所述關鍵位姿信息為所述第一時刻之前所述圖像采集單元采集的歷史圖像信息為關鍵幀時，所述歷史圖像信息中包括的位姿信息，所述關鍵位姿信息還包括所述第一時刻之前所述圖像采集單元采集的歷史圖像信息為關鍵幀時，所述IMU測量獲得的速度和偏置量；

按照預設算法對所述目標函數進行迭代，以獲取在所述目標函數最小的條件下對應的所述優化位姿和所述優化局部地圖。

2.根據權利要求1所述的方法，其特征在于，所述將第一時刻圖像采集單元采集的圖像信息和慣性測量單元IMU采集的運動信息按照擴展卡爾曼濾波算法進行融合，以獲取初始位姿，包括：

將所述運動信息和歷史位姿作為IMU運動模型的輸入，以獲取所述IMU運動模型輸出的預測位姿，所述歷史位姿為將第三時刻所述圖像采集單元采集的圖像信息和所述IMU采集的運動信息按照所述擴展卡爾曼濾波算法進行融合，獲取的初始位姿，所述第三時刻為所述第一時刻之前的時刻；

按照預設的圖像處理算法處理所述圖像信息，以獲取所述圖像信息中包括的視覺約束關系；

按照所述視覺約束關系對所述預測位姿進行更新，以獲取所述初始位姿，所述視覺約束關系包括：重投影誤差最小，或光度誤差最小。

3.根據權利要求2所述的方法，其特征在于，所述根據所述優化位姿和所述優化局部地圖，更新目標初始位姿和目標初始局部地圖，包括：

將所述目標初始局部地圖更新為所述優化局部地圖；

當所述視覺約束關系為重投影誤差最小時，將所述優化位姿和第二時刻之前的預設數量個關鍵位姿信息按照卡爾曼濾波算法進行融合，以將所述目標初始位姿更新為所述卡爾曼濾波算法輸出的位姿，所述關鍵位姿信息為所述第二時刻之前所述圖像采集單元采集的歷史圖像信息為關鍵幀時，所述歷史圖像信息中包括的位姿信息；

當所述視覺約束關系為光度誤差最小時，獲取所述初始位姿與所述優化位姿之間的偏移量；

根據所述偏移量更新所述目標初始位姿。

4.根據權利要求3所述的方法，其特征在于，所述根據所述優化位姿和所述優化局部地圖，更新目標初始位姿和目標初始局部地圖，還包括：

將更新后的所述目標初始局部地圖存入全局地圖。