[發明專利]一種激光雷達-相機-慣導聯合標定方法及系統

有效

• 申請號: 202110393764.1
• 申請日: 2021-04-13
• 公開（公告）號: CN113091771B
• 公開（公告）日: 2022-09-23
• 發明（設計）人: 江昆;楊殿閣;張曉龍;劉茂林;陳鑫鑫;楊彥鼎
• 申請（專利權）人: 清華大學;東風汽車集團股份有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01S7/497;G01S19/40
• 代理公司: 北京紀凱知識產權代理有限公司 11245
• 代理人: 孫楠

1.一種激光雷達-相機-慣導聯合標定方法，其特征在于，包括：

步驟1、通過激光雷達、單目相機和GPS/IMU同時進行數據采集，獲取激光雷達點云、單目相機圖像以及GPS/IMU設備位姿信息；

步驟2、根據采集的數據，分別獲得激光雷達位姿估計結果單目相機位姿估計結果以及GPS/IMU設備自身在全局坐標系下的位姿

步驟3、將獲取的三種位姿估計結果進行數據關聯，確定位姿之間的對應關系；

步驟4、將關聯后的三個位姿序列兩兩之間構建手眼標定問題，并將手眼標定問題轉化為優化問題；

步驟5、求解優化問題，將不同傳感器軌跡轉換到同一坐標系下；

所述步驟3中，采用位姿插值的方式進行數據關聯：在三種位姿序列中任選兩種位姿序列進行插值，獲得同一時間戳下兩個位姿序列中對應的位姿；然后在具有同一時間戳的兩個位姿序列中任選一個位姿序列與余下的位姿序列再次進行插值，完成數據關聯；

所述的位姿插值包括以下步驟：

步驟31、輸入兩個位姿序列，并判斷其中第一位姿序列是否遍歷完成，若完成，則輸出與第二位姿序列的插值結果，反之，則進入下一步；

步驟32、取第一位姿序列中的時間戳，判斷其是否落在第二位姿序列中，若落入，則進入下一步，反之，則返回步驟31重新判斷第一位姿序列是否遍歷完成；

步驟33、將第二位姿序列的平移部分和旋轉部分分別進行插值，由此獲得同一時間戳下位姿序列和位姿序列中對應的位姿。

2.如權利要求1所述聯合標定方法，其特征在于，所述步驟2中，采用激光SLAM算法獲得激光雷達位姿估計結果采用視覺SLAM算法獲得單目相機位姿估計結果采用慣性導航設備獲得自身在全局坐標系下的位姿

3.如權利要求2所述聯合標定方法，其特征在于，所述單目相機位姿估計結果

為第i幀單目相機相對于自身起點的位姿；表示視覺里程計估計出的旋轉部分位姿；表示視覺里程計估計出的平移部分位姿，缺乏尺度；C₀表示單目相機起始位置坐標系，C_i表示第i幀單目相機坐標系。

4.如權利要求3所述聯合標定方法，其特征在于，所述視覺里程計估計出的平移部分位姿缺乏尺度，需要通過估計尺度s恢復其實際位姿

5.如權利要求1所述聯合標定方法，其特征在于，所述步驟4中，構建方法包括以下步驟：

步驟41、計算位姿增量；

步驟42、根據位姿增量確定手眼標定問題；

步驟43、將手眼標定問題轉化為優化問題。

6.如權利要求5所述聯合標定方法，其特征在于，所述步驟43中，單目相機-激光雷達之間尺度單目相機-慣性導航設備之間的尺度之間應當滿足如下關系：

引入新的代價函數項：

完成優化問題構建；其中，為激光雷達到相機的外參矩陣，為激光雷達到慣性導航設備的外參矩陣，為相機到慣性導航設備的外參矩陣。

7.如權利要求1所述聯合標定方法，其特征在于，所述步驟5中，通過非線性優化的方式求解優化問題，獲得激光雷達、單目相機、慣性導航設備之間的外參以及單目相機運行中的尺度利用外參和尺度將不同傳感器軌跡轉換到同一坐標系下。

8.一種激光雷達-相機-慣導聯合標定系統，其特征在于，包括：數據采集模塊、位姿估計模塊、數據關聯模塊、優化問題構建模塊和優化求解模塊；

所述數據采集模塊，通過激光雷達、單目相機和GPS/IMU同時進行數據采集，獲取激光雷達點云、單目相機圖像以及GPS/IMU設備位姿信息；

所述位姿估計模塊，根據采集的數據，分別獲得激光雷達位姿估計結果單目相機位姿估計結果以及GPS/IMU設備自身在全局坐標系下的位姿

所述數據關聯模塊，將獲取的三種位姿估計結果進行數據關聯，確定位姿之間的對應關系；

所述優化問題構建模塊，將關聯后的三個位姿序列兩兩之間構建手眼標定問題，并將手眼標定問題轉化為優化問題；

所述優化求解模塊，求解優化問題，將不同傳感器軌跡轉換到同一坐標系下；

所述數據關聯模塊中，采用位姿插值的方式進行數據關聯：在三種位姿序列中任選兩種位姿序列進行插值，獲得同一時間戳下兩個位姿序列中對應的位姿；然后在具有同一時間戳的兩個位姿序列中任選一個位姿序列與余下的位姿序列再次進行插值，完成數據關聯；

所述的位姿插值包括以下步驟：

步驟31、輸入兩個位姿序列，并判斷其中第一位姿序列是否遍歷完成，若完成，則輸出與第二位姿序列的插值結果，反之，則進入下一步；

步驟32、取第一位姿序列中的時間戳，判斷其是否落在第二位姿序列中，若落入，則進入下一步，反之，則返回步驟31重新判斷第一位姿序列是否遍歷完成；

步驟33、將第二位姿序列的平移部分和旋轉部分分別進行插值，由此獲得同一時間戳下位姿序列和位姿序列中對應的位姿。