[发明专利]定位方法、装置、设备及可读存储介质

说明书

本发明提供一种定位方法，该方法包括：持续获取激光雷达扫描装置对空间环境进行扫描得到的每帧scan；将获取的每帧scan插入第一submap，且每获取帧scan新增一个submap，将获取的每帧scan插入不完整的submap，其中，完整的submap包括N帧scan，不完整的submap包括的scan不足N帧，N为被2整除的正整数；每得到一个完整的submap，与空间环境的全局地图匹配一次，获取激光雷达扫描装置的定位。通过本发明，将每帧scan插入到submap中，所得到的完整的submap的累计误差比较小，用每个完整的submap与空间环境的全局地图相匹配，得到的定位精度比较高，漂移误差也比较小。

技术领域

本发明涉及测绘领域，尤其涉及一种定位方法、装置、设备及可读存储介质。

背景技术

目前，激光雷达作为一种高精度的传感器，广泛应用在室内定位，自动驾驶和机器人导航等领域。在室内定位领域中，主要用Scan Matching匹配方法进行激光雷达定位研究。

但是，用Scan Matching匹配方法在定位过程中会产生较大的漂移误差，导致定位精度低。

发明内容

本发明的主要目的在于提供一种定位方法、装置、设备及可读存储介质，旨在解决激光雷达定位过程中漂移误差大，导致定位精度低的问题。

第一方面，本发明提供一种定位方法，包括以下步骤：

持续获取激光雷达扫描装置对空间环境进行扫描得到的每帧scan；

将获取的每帧scan插入第一submap，且每获取帧scan新增一个submap，将获取的每帧scan插入不完整的submap，其中，完整的submap包括N帧scan，不完整的submap包括的scan不足N帧，N为被2整除的正整数；

每得到一个完整的submap，与空间环境的全局地图匹配一次，获取激光雷达扫描装置的定位。

可选的，所述将获取的每帧scan插入第一submap的步骤包括：

读取所获取的每帧scan对应的激光雷达扫描装置的位姿ε＝(ε_x，ε_y,ε_θ)，其中，ε_θ表示激光雷达扫描装置在二维平面上的旋转分量，ε_x表示激光雷达扫描装置在x轴方向上的平移分量，ε_y表示激光雷达扫描装置在y轴方向上的平移分量；

将每帧scan对应的激光雷达扫描装置的位姿ε通过公式映射到第一submap坐标系中，所述公式为：其中，p表示每帧scan在scan坐标系中的坐标，T_εp表示将p转化为在submap坐标系中的坐标；

将每帧scan对应的T_εp与scan matcher优化器搜索范围内的各点的坐标相匹配，得到每帧scan在第一submap中的最佳插入位置，基于每帧scan在第一submap中的最佳插入位置将获取的每帧scan插入第一submap中。

可选的，所述将每帧scan对应的T_εp与scan matcher优化器搜索范围内的各点的坐标相匹配，得到每帧scan在第一submap中的最佳插入位置的步骤包括：

使用ceres solver库的scan matcher优化器计算scan matcher优化器搜索范围内的各点的坐标与每帧scan对应的T_εp的匹配概率，匹配概率最大的点的位置，为每帧scan在第一submap中的最佳插入位置。

可选的，所述每得到一个完整的submap，与空间环境的全局地图匹配一次，获取激光雷达扫描装置的定位的步骤包括：