[发明专利]融合组合导航信息的激光雷达定位方法

说明书

本发明涉及无人驾驶技术领域，具体涉及一种融合组合导航信息的激光雷达定位方法，当RTK信号不稳定时，组合导航输出的定位信息随着时间会发生漂移甚至跳变，本发明利用组合导航在较短的一段时间内能输出较高精度的里程信息，将其作为地图匹配定位的预测值（也可以说作为NDT迭代优化需要的初始值），通过频繁与地图这种先验信息匹配可以不断修正这个漂移，从而弥补组合导航此时的缺陷。

技术领域

本发明涉及无人驾驶技术领域，具体涉及一种融合组合导航信息的激光雷达定位方法。

背景技术

常见的无人驾驶车辆通过组合导航系统可以在高速等简单场景进行较长时间的位置推算。在一些较为复杂的场景车辆会使用激光雷达配合高精地图进行定位，一般的三维激光雷达定位系统使用各种点云配准算法如NDT等，将当前传感器输入的点云与高精度点云地图进行配准，从而得到当前传感器在地图坐标系中的位置，进而得到车辆的定位信息。

现有技术中存在的问题：1.使用纯组合导航进行定位，当车辆遇到高楼、树木、隧道、高架等RTK信号遮挡的场景，RTK定位结果会发生漂移甚至跳变，此时定位结果只依赖IMU的积分推算，长时间会造成较大的累积漂移；2.使用激光雷达配合地图定位可以满足上述场景的要求，但由于动态物体的干扰，空间结构过于重复，匹配初始值差等因素会造成点云配准结果的退化，造成定位的发散；3.由于高精地图位于世界坐标系下，使用单个激光雷达无法提供经纬度等世界位置的信息，且搭载激光雷达的车辆可能处于地图的任意位置开始定位，因此，在第一个定位时刻无法提供点云与地图配准的初始值，定位系统难以初始化。

发明内容

本申请中为了解决上述技术问题，本发明提供了一种融合组合导航信息的激光雷达定位方法。

本发明提供了如下的技术方案：

融合组合导航信息的激光雷达定位方法，包括如下步骤：

S1.加载即时定位与建图模块构造的环境地图；

S2.使用手眼标定算法，得到激光雷达与组合导航之间的外参信息，并据此对激光点云数据校准，统一坐标系；

S3.输入点云地图作为原始点云，输入当前时刻的激光点云作为目标点云，根据车辆当前世界坐标系位置与地图原点世界位置计算车辆在地图中的相对位置作为NDT预测值，并进行NDT求解；

S4.基于当前激光雷达点云的时间戳信息，获取离此时间最近的组合导航数据帧；

S5.获取上一定位时刻的组合导航里程信息、定位结果以及当前定位时刻的组合导航里程信息，预测的当前第一定位信息；

S6.输入点云地图作为原始点云，输入当前定位时刻的激光点云作为目标点云，输入上述第一定位信息作为NDT预测值，并进行NDT求解输出当前车辆在地图坐标系中的第二定位信息；

S7.比较上述第一定位信息和第二定位信息。

优选的，在步骤S1中，环境地图中包含环境的三维点云信息和该地图坐标系原点在世界坐标系的位置信息。

优选的，根据当前时刻组合导航的输出信息得到世界坐标系下的位置，计算该位置与步骤S1中的环境地图坐标系原点的相对变换，进而得到当前车辆在环境地图坐标系中的初始位置。

优选的，在步骤S3中，如果NTD求解收敛，说明当前时刻点云与环境地图配准成功，完成了定位初始化；如果NTD求解收敛失败，重新输入步骤S3中的数据，进行NDT求解直到收敛成功，完成匹配定位初始化。

优选的，在步骤S4中，由于组合导航频率足够高，两者时间戳上的差异可以忽略不计，可以认为两者时间上是对齐的。