[發明專利]一種VSLAM特征地圖尺度恢復方法及系統

有效

• 申請號: 202011301722.2
• 申請日: 2020-11-19
• 公開（公告）號: CN112577499B
• 公開（公告）日: 2022-10-11
• 發明（設計）人: 李健聰;姚明江
• 申請（專利權）人: 上汽大眾汽車有限公司
• 主分類號: G01C21/32
• 分類號: G01C21/32;G01C21/16;G01S17/89;G06V20/56;G06V10/80
• 代理公司: 上海東信專利商標事務所(普通合伙) 31228
• 代理人: 楊丹莉;李丹

1.一種VSLAM特征地圖尺度恢復方法，其特征在于，在安裝有攝像頭、激光雷達和組合慣性導航系統的車輛在指定區域內行駛的情況下，包括步驟：

100：獲取攝像頭采集的圖像數據、激光雷達采集的點云數據以及組合慣性導航系統采集的高精度地圖數據；

200：對攝像頭、激光雷達和組合慣性導航系統的相對位置進行標定，以將所述圖像數據、點云數據和高精度地圖數據的坐標統一；

300：基于所述圖像數據采用VSLAM方法生成無尺度的特征地圖M_vslam和定位軌跡T_vslam；基于所述點云數據采用LSLAM方法生成具有相對尺度的特征地圖M_lslam和定位軌跡T_lslam；

400：將定位軌跡T_lslam與高精度地圖數據T_combine融合，得到絕對尺度定位軌跡T_all；其中，對于定位信號好的區域，采用高精度地圖數據T_combine作為具有絕對尺度的第一定位軌跡；對于定位信號不好的區域，將定位軌跡T_lslam中的各軌跡點與高精度地圖數據T_combine中的相應軌跡點進行匹配，得到定位軌跡T_lslam到高精度地圖數據T_combine的轉換矩陣；采用轉換矩陣對定位軌跡T_lslam進行變換，得到具有絕對尺度的第二定位軌跡；將第一定位軌跡和第二定位軌跡組合，得到絕對尺度定位軌跡T_all；

500：采用絕對尺度定位軌跡T_all對定位軌跡T_vslam進行粗標定，得到第一縮放系數S₁與第一變換矩陣RT₁；

600：對經過粗標定的定位軌跡T_vslam進行精標定，得到第二縮放系數S₂與第二變換矩陣RT₂；

700：采用最終縮放系數S和最終變換矩陣RT對無尺度的特征地圖M_vslam進行縮放和轉換，得到具有絕對尺度的VSLAM特征地圖，其中S＝S₁·S₂、RT＝RT₁·RT₂。

2.如權利要求1所述的VSLAM特征地圖尺度恢復方法，其特征在于，所述攝像頭為單目攝像頭或環視攝像頭。

3.如權利要求1所述的VSLAM特征地圖尺度恢復方法，其特征在于，采用迭代最近鄰算法將定位軌跡T_lslam中的各軌跡點與高精度地圖數據T_combine中的相應軌跡點進行匹配。

4.如權利要求1所述的VSLAM特征地圖尺度恢復方法，其特征在于，步驟500進一步包括：

501：將定位軌跡T_vslam與絕對尺度定位軌跡T_all在同一3D坐標系下可視化；

502：將定位軌跡T_vslam縮放至與絕對尺度定位軌跡T_all同一數量級；

503：將縮放后的定位軌跡T_vslam旋轉、平移至與絕對尺度定位軌跡T_all大致重合，得到新定位軌跡T’_vslam以及第一縮放系數S₁與第一變換矩陣RT₁。