[發明專利]基于恒速模型的自適應3D-LSLAM定位方法、裝置和系統

在審

• 申請號: 202011040245.9
• 申請日: 2020-09-28
• 公開（公告）號: CN112180382A
• 公開（公告）日: 2021-01-05
• 發明（設計）人: 張磊
• 申請（專利權）人: 知行汽車科技（蘇州）有限公司
• 主分類號: G01S17/06
• 分類號: G01S17/06;G01S17/86;G01C21/16;G01C21/20;G06T7/73
• 代理公司: 蘇州謹和知識產權代理事務所(特殊普通合伙) 32295
• 代理人: 唐靜芳

本發明公開了一種基于恒速模型的自適應3D?LSLAM定位方法，包括：接收激光點云信息，將點云坐標從激光雷達坐標系轉換到當前后驗子地圖坐標系，構建點云與當前后驗子地圖的非線性優化函數，通過LM法得到優化后的點云位姿；基于優化后的點云位姿將當前點云信息通過貝耶斯濾波更新到當前后驗子地圖中；結合更新后的當前后驗子地圖、全局先驗地圖和計算得到的車輛的速度和角速度，對后驗子地圖的位姿進行非線性優化；基于優化后的當前后驗子地圖，將當前點云的位姿轉換到全局先驗地圖坐標系中。本發明能夠按照任意頻率輸出車輛位姿，對于環境具有強魯棒性；在保證精度的前提下降低了計算量，實現了車輛實時定位輸出；無累計誤差，定位精度高。

技術領域

本發明涉及車輛實時定位領域，屬于一種基于恒速模型的自適應3D-LSLAM定位方法、裝置和系統。

背景技術

目前的車輛位姿獲取主要通過以下兩種方法：第一種方法，基于2D-lslam先驗地圖，采用2D激光點云信息進行粒子濾波得到車輛后驗位姿。第二種方法，采用3D激光點云信息與全局3D-lslam先驗地圖進行配準得到車輛位姿。

然而這兩種獲取車輛位姿的方法均存在一些問題，例如，第一種方法十分依賴里程計，2D激光點云特征少，魯棒性差，不適合室外開闊場景，精度的提升需要消耗計算量為代價；而第二種方法計算量大，實時性難以保證，容易陷入局部最優，存在累計誤差。

發明內容

本發明的目的在于提供一種基于恒速模型的自適應3D-LSLAM定位方法、裝置和系統，能夠按照任意頻率輸出車輛位姿，對于環境具有強魯棒性；在保證精度的前提下降低了計算量，實現了車輛實時定位輸出；無累計誤差，定位精度高。

為達到上述目的，本發明提供如下技術方案：

一種基于恒速模型的自適應3D-LSLAM定位方法，所述定位方法包括：

S1，初始化用于接收IMU信息和激光點云信息的接收接口；初始化車輛位姿；初始化后驗子地圖；所述激光點云信息包括激光雷達感知到的車輛前方障礙物的幾何位置以及強度信息；

S2，根據接收到的IMU信息實時更新車輛姿態；

S3，接收激光點云信息，將點云坐標從激光雷達坐標系轉換到當前后驗子地圖坐標系，構建點云與當前后驗子地圖的非線性優化函數，通過LM法得到優化后的點云位姿；基于優化后的點云位姿將當前點云信息通過貝耶斯濾波更新到當前后驗子地圖中；

S4，根據當前點云位姿與上一幀點云優化后的位姿計算并更新車輛的速度和角速度；

S5，結合更新后的當前后驗子地圖、全局先驗地圖和計算得到的車輛的速度和角速度，對后驗子地圖的位姿進行非線性優化；其中，采用實時點云與先驗子地圖匹配構建非線性約束；

S6，基于優化后的當前后驗子地圖，將當前點云的位姿轉換到全局先驗地圖坐標系中；

S7，采用恒速模型，輸出當前時刻車輛在全局坐標系下的位姿。

進一步地，步驟S1中，所述初始化車輛位姿的過程包括：

根據接收到的第一幀IMU信息，初始化車輛位姿。

進一步地，步驟S1中，所述初始化后驗子地圖的過程包括：

結合接收到的第一幀激光點云信息，將點云坐標從激光雷達坐標系轉換到車體坐標系，初始化后驗子地圖。

進一步地，步驟S3中，所述基于優化后的點云位姿將當前點云信息通過貝耶斯濾波更新到當前后驗子地圖中的過程包括以下步驟：

基于優化后的點云位姿將當前點云信息通過貝耶斯濾波更新到當前后驗子地圖，如果當前后驗子地圖的幀數已達到構建子地圖幀數設定值的一半，則用當前點云初始化下一個預備后驗子地圖。