[發明專利]一種基于Multi-Camera/Lidar/IMU的多傳感器SLAM方法

有效

• 申請號: 202010860418.5
• 申請日: 2020-08-25
• 公開（公告）號: CN111983639B
• 公開（公告）日: 2023-06-02
• 發明（設計）人: 楊建松;余小歡;陳嵩;白云峰;張合勇
• 申請（專利權）人: 浙江光珀智能科技有限公司
• 主分類號: G01S17/931
• 分類號: G01S17/931;G01S17/894;G01S17/86;G01C21/16;G01C22/00
• 代理公司: 杭州鈐韜知識產權代理事務所(普通合伙) 33329
• 代理人: 唐靈;趙杰香

1.一種基于Multi-Camera/Lidar/IMU的多傳感器SLAM方法，其特征在于：包括以下步驟：

S1：由多目相機獲得的多張圖像數據與IMU慣性測量單元獲得的數據進行緊耦合聯合初始化，獲得系統的初始位姿；

S2:由激光雷達傳感器獲得激光雷達幀的點云數據，對點云數據進行預處理，將點云劃分為強角點、弱角點、強平面點、弱平面點；

S3:通過系統的初始位姿對激光雷達幀位姿進行優化；

S4：通過閉環檢測對激光雷達幀位姿進行進一步優化；

S5：利用優化后的激光雷達幀位姿進行地圖拼接；

其中，步驟S3通過系統的初始位姿對激光雷達幀位姿進行優化步驟包括：

S31：接收優化完成的視覺慣性里程計、關鍵幀的位姿、關鍵幀、預處理的點云數據、IMU數據保存至相應的隊列中；

S32：通過各數據時間戳對齊各數據幀，并由此構建閉環檢測關鍵幀，同時處理分割的點云數據，對齊數據并變化坐標系，由激光雷達坐標系轉化至多目相機的坐標系下，并保存至相應的列隊容器中；同時在激光雷達數據的對列中壓入關鍵幀的索引；

S33：對激光雷達幀數據進行處理，得到優化后的激光雷達幀的位姿；

步驟S33對激光雷達幀數據處理的步驟包括：

S331：取出關鍵幀的索引，對于第一幀關鍵幀，初始化其姿態，即將姿態歐拉角都置零；

S332：當索引大于1時，取出當前幀的前一幀，并在當前幀與前一幀的時間戳之間獲取相應的激光雷達幀數據與IMU幀數據；

S333：獲取前一幀的視覺慣性里程計位姿；

S334：控制當前幀與前一幀的時間戳之間，只有一幀激光雷達數據參與構建云地圖，由于激光雷達幀與當前幀之間存在時間間隔，因此位姿并不相同，利用IMU幀數據進行推位，利用推位得到的較為準確的位姿將點云轉化到世界坐標系下；

S335：對推位得到的激光雷達幀位姿進行進一步的優化。

2.根據權利要求1所述一種基于Multi-Camera/Lidar/IMU的多傳感器SLAM方法，其特征在于：獲得系統的初始化位姿包括如下步驟：

S11：通過樹莓派進行多目相機的硬同步觸發；

S12：從多目相機中獲得多目圖像與IMU數據，通過時間戳將多目圖像數據與IMU數據進行對齊；

S13：多線程進行多目圖像的視覺特征跟蹤，并將跟蹤的結果進行保存；

S14：對多目圖像數據以及IMU數據進行預處理；

S15：對多目圖像數據以及IMU數據進行聯合初始化，將多目相機獲得多張圖像信息分別與IMU數據進行緊耦合處理，將處理得到的數據再一次和IMU數據進行緊耦合處理，得到多目的緊耦合視覺慣性里程計；

S16：對多目的緊耦合視覺慣性里程計進行優化，同時進行邊緣化處理；

S17：得到優化后的多目視覺慣性里程計，獲得系統初始化的位姿，為下一步做準備。

3.根據權利要求2所述一種基于Multi-Camera/Lidar/IMU的多傳感器SLAM方法，其特征在于：多線程進行多目圖像的視覺特征跟蹤，主要使用OpenCV中的光流跟蹤方法。

4.根據權利要求2所述一種基于Multi-Camera/Lidar/IMU的多傳感器SLAM方法，其特征在于：對多目圖像數據以及IMU數據進行預處理包括如下步驟：

S141：首先取出相鄰兩幀間的IMU數據，進行系統初始化位姿估計，將初始的偏航角置零；

S142：對IMU數據預處理主要進行幀間的IMU預積分，同時估計積分后的測量方差與預積分值對狀態變量的雅克比矩陣；

S143：對多目圖像數據預處理：首先要進行關鍵幀的判斷，根據特征點的ID判斷是否將特征壓入特征鏈表中，根據前后幀的平均視差判斷是否將該幀置為關鍵幀，從而確定后端滑動窗口優化的位置。