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

说明书

本发明公开了一种基于恒速模型的自适应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中，所述基于优化后的点云位姿将当前点云信息通过贝耶斯滤波更新到当前后验子地图中的过程包括以下步骤：

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