[發明專利]基于3D激光雷達和慣性測量單元的融合的測距和繪圖

在審

• 申請號: 202010104509.6
• 申請日: 2020-02-20
• 公開（公告）號: CN111665512A
• 公開（公告）日: 2020-09-15
• 發明（設計）人: 葉昊陽;陳玉瑩;劉明
• 申請（專利權）人: 香港科技大學
• 主分類號: G01S17/08
• 分類號: G01S17/08;G01S17/894;G01S7/48;G01C9/00
• 代理公司: 北京天昊聯合知識產權代理有限公司 11112
• 代理人: 張娜;李榮勝

1.一種用于姿態追蹤和三維繪圖的系統，包括：

3D激光雷達，其在第一時間和第二時間處獲得激光雷達掃查結果，第一時間在第二時間之前；

慣性測量單元，其與所述3D激光雷達緊密耦合；和

計算裝置，其構造為：

在所述第二時間到來之前，利用在所述第一時間和所述第二時間之間來自所述慣性測量單元的測量數據對所述慣性測量單元在所述第二時間處的狀態進行更新，同時對在所述第一時間和所述第二時間之間來自所述慣性測量單元的測量數據進行預整合；

根據預整合后的所述慣性測量單元的測量數據以及所述慣性測量單元與所述3D激光雷達之間的外部參數來預測所述3D激光雷達的姿態，并將預測的所述3D激光雷達的姿態應用于在所述第二時間處接收的來自所述3D激光雷達的測量數據，以得到所述3D激光雷達的去偏移的測量數據；

提取在所述第二時間處接收的來自所述3D激光雷達的測量數據的特征點；

將在所述第二時間之前的來自所述3D激光雷達的測量數據的特征點融合為局部地圖；

利用預測的3D激光雷達的姿態和所述去偏移的測量數據，得到相對激光雷達測量；

利用所述相對激光雷達測量和所述預整合，進行聯合非線性優化，以得到優化后的3D激光雷達的姿態，進而得到局部時間窗口內的狀態的繪圖估計，并將估計結果用于所述更新中。

2.根據權利要求1所述的系統，其中，所述計算裝置還構造為：

利用3D激光雷達的去偏移的測量數據和優化后的3D激光雷達的姿態來將多幀去偏移的數據融合而得到全局地圖。

3.根據權利要求2所述的系統，其中，所述計算裝置還構造為：

對優化后的3D激光雷達的姿態添加旋轉約束來優化所述全局地圖使得所述全局地圖與所述3D激光雷達的去偏移的測量數據相匹配，以得到具有高一致性的全局地圖。

4.根據權利要求1所述的系統，其中，所述相對激光雷達測量是所述局部地圖與所述3D激光雷達的去偏移的測量數據之間的對應關系。

5.一種用于姿態追蹤和三維繪圖的方法，包括：

將慣性測量單元與所述3D激光雷達緊密耦合；

通過3D激光雷達在第一時間和第二時間處獲得激光雷達掃查結果，第一時間在第二時間之前；

在所述第二時間到來之前，利用在所述第一時間和所述第二時間之間來自所述慣性測量單元的測量數據對所述慣性測量單元在所述第二時間處的狀態進行更新，同時對在所述第一時間和所述第二時間之間來自所述慣性測量單元的測量數據進行預整合；

根據預整合后的所述慣性測量單元的測量數據以及慣性測量單元與激光雷達之間的外部參數來預測所述3D激光雷達的姿態，并將預測的所述3D激光雷達的姿態應用于在所述第二時間處接收的來自所述3D激光雷達的測量數據，以得到所述3D激光雷達的去偏移的測量數據；

提取在所述第二時間處接收的來自所述3D激光雷達的測量數據的特征點；

將在所述第二時間之前的來自所述3D激光雷達的測量數據的特征點融合為局部地圖；

利用預測的激光雷達的姿態和所述去偏移的測量數據，得到相對激光雷達測量；

利用所述相對激光雷達測量和所述預整合，進行聯合非線性優化，以得到優化后的3D激光雷達的姿態，進而得到局部時間窗口內的狀態的繪圖估計，并將估計結果用于所述更新中。

6.根據權利要求5所述的方法，還包括：

利用3D激光雷達的去偏移的測量數據和優化后的3D激光雷達的姿態來將多幀去偏移的數據融合而得到全局地圖。

7.根據權利要求6所述的方法，其中，

對優化后的3D激光雷達的姿態添加旋轉約束來優化所述全局地圖使得所述全局地圖與所述3D激光雷達的去偏移的測量數據相匹配，以得到具有高一致性的全局地圖。

8.根據權利要求5所述的方法，其中，所述相對激光雷達測量是所述局部地圖與所述3D激光雷達的去偏移的測量數據之間的對應關系。