[發明專利]一種多傳感器融合的SLAM方法和裝置

在審

• 申請號: 202110541906.4
• 申請日: 2021-05-18
• 公開（公告）號: CN113516692A
• 公開（公告）日: 2021-10-19
• 發明（設計）人: 劉燕霖;陳敏鶴;金忠孝
• 申請（專利權）人: 上海汽車集團股份有限公司;上海汽車工業(集團)總公司
• 主分類號: G06T7/292
• 分類號: G06T7/292;G06T7/246;G06T7/73;G06T7/77;G06T7/80;G06F9/50;G06K9/62;G01C21/32
• 代理公司: 北京信遠達知識產權代理有限公司 11304
• 代理人: 王會會

1.一種多傳感器融合的SLAM方法，其特征在于，包括：

標定SLAM系統中相機的內參；

標定SLAM系統中相機、輪速計和IMU的外參；

基于所述相機的內參以及相機、輪速計和IMU的外參的標定結果給定相機投影模型；

控制所述SLAM系統中多攝像機和輪速計松耦合初始化；

采用追蹤線程獲取初始化后的地圖點和位姿，基于獲取到的圖像數據、IMU數據和輪速計數據、基于所述圖像數據插入關鍵幀并對所述位姿和地圖點進行修正，將所述關鍵幀發送至局部建圖線程；

采用局部建圖線程獲取追蹤線程發送的關鍵幀，基于所述關鍵幀三角化生成新的地圖點并融合地圖點，篩選并剔除冗余關鍵幀和冗余地圖點；

采用回環檢測線程對所述局部建圖線程篩選后的關鍵幀進行回環檢測，根據回環幀調整對篩選后的關鍵幀位姿和地圖點位置進行修正。

2.根據權利要求1所述的多傳感器融合的SLAM方法，其特征在于，當所述SLAM系統中相機為透視相機時，所述標定的SLAM系統中相機的內參，包括但不限于焦距、透視圖像中心坐標、畸變系數中的一項或多項組合；當所述SLAM系統中相機為魚眼相機時，所述標定的SLAM系統中相機的內參，包括但不限于正向映射系數，逆向映射系數，魚眼圖像中心坐標，仿射變換系數中的一項或多項的組合。

3.根據權利要求1所述的多傳感器融合的SLAM方法，其特征在于，基于所述相機的內參以及相機、輪速計和IMU的外參的標定結果給定相機投影模型，包括：

基于所述內參以及外參，計算得到世界坐標系到本體坐標系的姿態變換矩陣、本體坐標系到相機坐標系的姿態變換矩陣、魚眼相機的相機坐標系到第j個虛擬透視相機的相機坐標系的變換矩陣；

將公式作為相機投影模型，其中，所述p為世界坐標系下的地圖點P(X，Y，Z)投影到相機c_i的像素坐標系下得到的像素點p(x，y)，所述K_j為第j個虛擬透視相機的透視變換矩陣，所述為從魚眼相機c_i的相機坐標系到第j個虛擬透視相機的相機坐標系的變換矩陣，所述為本體坐標系到第i個相機c_i相機坐標系的位姿變換矩陣，所述T_bw為從世界坐標系到本體坐標系的位姿變換矩陣，P為世界坐標系下的地圖點P(X，Y，Z)。

4.根據權利要求1所述的多傳感器融合的SLAM方法，其特征在于，所述控制所述SLAM系統中多攝像機和輪速計松耦合初始化，包括：

通過對極約束分別計算每路相機的本質矩陣，選擇內點最多的矩陣對應的相機作為初始化相機；

進行視覺初始化得到所述初始化相機的相機光心的平移距離，記為第一平移距離；

通過輪速計預積分得到相鄰兩幀之間的本體坐標系坐標點的平移距離；

基于所述本體坐標系坐標點的平移距離計算得到相鄰兩幀之間所述初始化相機的相機光心的平移距離，記為第二平移距離；

基于所述第一平移距離和第二平移距離計算尺度因子；

基于所述尺度因子恢復姿態和地圖點的絕對尺度，并對所述IMU輪速計進行初始化。

5.根據權利要求1所述的多傳感器融合的SLAM方法，其特征在于，采用回環檢測線程對所述局部建圖線程篩選后的關鍵幀進行回環檢測，根據回環幀調整對篩選后的關鍵幀位姿和地圖點位置進行修正，包括：

將與當前關鍵幀相同的單詞數超過閾值、相似度檢測大于相鄰關鍵幀最低分數的關鍵幀作為候選幀，再從所述候選幀中選出具有連續性的關鍵幀作為回環候選幀，所述相鄰關鍵幀最低分數指的是：所述當前關鍵幀的相鄰關鍵幀與所述前關鍵幀的最低相似度；

通過詞袋向量加速描述子的匹配，利用RANSAC計算當前關鍵幀和閉環候選幀的Sim3變換，并根據初步求得的Sim3變換對地圖點投影得到更多匹配的地圖點；

將兩個關鍵幀對應的Sim3變換和地圖點作為頂點，遍歷所有地圖點的觀測，添加正向和逆向投影的二元邊，固定地圖點的位姿，通過最小化重投影誤差來優化兩幀的Sim3變換；

根據優化后的Sim3，將閉環關鍵幀及其相連關鍵幀的地圖點都投影到當前關鍵幀進行匹配，以得到更多的匹配點；

通過對優化后的Sim3求解，基于求解的Sim3，調整關鍵幀位姿和地圖點位置。