[發(fā)明專利]一種VSLAM特征地圖尺度恢復(fù)方法及系統(tǒng)

有效

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

1.一種VSLAM特征地圖尺度恢復(fù)方法，其特征在于，在安裝有攝像頭、激光雷達(dá)和組合慣性導(dǎo)航系統(tǒng)的車輛在指定區(qū)域內(nèi)行駛的情況下，包括步驟：

100：獲取攝像頭采集的圖像數(shù)據(jù)、激光雷達(dá)采集的點云數(shù)據(jù)以及組合慣性導(dǎo)航系統(tǒng)采集的高精度地圖數(shù)據(jù)；

200：對攝像頭、激光雷達(dá)和組合慣性導(dǎo)航系統(tǒng)的相對位置進(jìn)行標(biāo)定，以將所述圖像數(shù)據(jù)、點云數(shù)據(jù)和高精度地圖數(shù)據(jù)的坐標(biāo)統(tǒng)一；

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

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

500：采用絕對尺度定位軌跡T_all對定位軌跡T_vslam進(jìn)行粗標(biāo)定，得到第一縮放系數(shù)S₁與第一變換矩陣RT₁；

600：對經(jīng)過粗標(biāo)定的定位軌跡T_vslam進(jìn)行精標(biāo)定，得到第二縮放系數(shù)S₂與第二變換矩陣RT₂；

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

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

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

4.如權(quán)利要求1所述的VSLAM特征地圖尺度恢復(fù)方法，其特征在于，步驟500進(jìn)一步包括：

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

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

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