[发明专利]基于恒速模型的自适应3D-LSLAM定位方法、装置和系统

权利要求书

1.一种基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，所述定位方法包括：

S1，初始化用于接收IMU信息和激光点云信息的接收接口；初始化车辆位姿；初始化后验子地图；所述激光点云信息包括激光雷达感知到的车辆前方障碍物的几何位置以及强度信息；

S2，根据接收到的IMU信息实时更新车辆姿态；

S3，接收激光点云信息，将点云坐标从激光雷达坐标系转换到当前后验子地图坐标系，构建点云与当前后验子地图的非线性优化函数，通过LM法得到优化后的点云位姿；基于优化后的点云位姿将当前点云信息通过贝耶斯滤波更新到当前后验子地图中；

S4，根据当前点云位姿与上一帧点云优化后的位姿计算并更新车辆的速度和角速度；

S5，结合更新后的当前后验子地图、全局先验地图和计算得到的车辆的速度和角速度，对后验子地图的位姿进行非线性优化；其中，采用实时点云与先验子地图匹配构建非线性约束；

S6，基于优化后的当前后验子地图，将当前点云的位姿转换到全局先验地图坐标系中；

S7，采用恒速模型，输出当前时刻车辆在全局坐标系下的位姿。

2.如权利要求1所述的基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，步骤S1中，所述初始化车辆位姿的过程包括：

根据接收到的第一帧IMU信息，初始化车辆位姿。

3.如权利要求1所述的基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，步骤S1中，所述初始化后验子地图的过程包括：

结合接收到的第一帧激光点云信息，将点云坐标从激光雷达坐标系转换到车体坐标系，初始化后验子地图。

4.如权利要求1所述的基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，步骤S3中，所述基于优化后的点云位姿将当前点云信息通过贝耶斯滤波更新到当前后验子地图中的过程包括以下步骤：

基于优化后的点云位姿将当前点云信息通过贝耶斯滤波更新到当前后验子地图，如果当前后验子地图的帧数已达到构建子地图帧数设定值的一半，则用当前点云初始化下一个预备后验子地图。

5.如权利要求1所述的基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，所述定位方法还包括：

基于单独的线程队列处理步骤S5至步骤S7。

6.如权利要求1所述的基于恒速模型的自适应3D-LSLAM定位方法，其特征在于，步骤S5中，所述结合更新后的当前后验子地图、全局先验地图和计算得到的车辆的速度和角速度，对后验子地图的位姿进行非线性优化的过程包括以下步骤：

S51，将点云在当前后验子地图中的位姿转换成全局先验地图位姿，遍历先验子地图，寻找与当前点云几何距离在预设距离阈值内的先验子地图集合；

S52，选择先验子地图集合中的其中一个先验子地图，根据更新后的车辆的速度和角速度，采用自适应律得到点云与先验子地图的实时匹配频率；

S53，如果实时匹配频率达到预设匹配频率，采用多分辨率匹配方法对当前点云与先验子地图进行匹配，转入步骤S54，如果实时匹配频率未达到预设匹配频率，直接转入步骤S56；

S54，如果匹配结果达到预设匹配度阈值，将点云位姿及其与先验子地图的约束加入用于优化后验子地图位姿的非线性优化函数中，转入步骤S55，如果匹配结果未达到预设匹配度阈值，直接转入步骤S56；

S55，如果点云帧数达到优化条件，对后验子地图的位姿进行非线性优化，转入步骤S56，如果点云帧数未达到优化条件，直接转入步骤S55；

S56，判断先验子地图集合是否遍历完毕，如果完毕，转入步骤S57，否则，转入步骤S52；

S57，基于优化后的当前后验子地图，将当前点云的位姿转换到全局先验地图坐标系中。