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

权利要求书

1.融合组合导航信息的激光雷达定位方法，其特征在于，包括如下步骤：

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

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

S3.根据当前车辆在世界坐标系的位置与地图坐标系原点在世界坐标系的位置，经过坐标转换可以计算出当前车辆在地图坐标系中的位置，并将此相对位置作为NDT预测值；

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

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

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

S7.若第一定位信息和第二定位信息两者的量化差异大于设定阈值可以认为配准退化，将第一定位信息输出作为最终定位信息；若两者差异小于设定阈值，可以认为激光点云配准准确，将第二定位信息输出作为最终定位信息。

2.根据权利要求1所述的融合组合导航信息的激光雷达定位方法，其特征在于，在步骤S1中，环境地图中包含环境的三维点云信息和该地图坐标系原点在世界坐标系的位置信息。

3.根据权利要求1所述的融合组合导航信息的激光雷达定位方法，其特征在于，根据当前时刻组合导航的输出信息得到世界坐标系下的位置，计算该位置与步骤S1中的环境地图坐标系原点的相对变换，进而得到当前车辆在环境地图坐标系中的初始位置。

4.根据权利要求1所述的融合组合导航信息的激光雷达定位方法，其特征在于，在步骤S3中，如果NTD求解收敛，说明当前时刻点云与环境地图配准成功，完成了定位初始化；如果NTD求解收敛失败，重新输入步骤S3中的数据，进行NDT求解直到收敛成功，完成匹配定位初始化。

5.根据权利要求1所述的融合组合导航信息的激光雷达定位方法，其特征在于，在步骤S4中，由于组合导航频率足够高，两者时间戳上的差异可以忽略不计，可以认为两者时间上是对齐的。

6.根据权利要求1所述的融合组合导航信息的激光雷达定位方法，其特征在于，在步骤S7中，若第一定位信息和第二定位信息两者的量化差异大于设定阈值可以认为配准退化，将第一定位信息输出作为最终定位信息；若两者差异小于设定阈值，可以认为激光点云配准准确，点云配准由于将每个点都和地图中的点配准对齐，具有很高的精度，对作为初始值的第一定位信息起到了一定的修正作用，因此将第二定位信息输出作为最终定位信息。

7.根据权利要求1至6任一所述的融合组合导航信息的激光雷达定位方法，其特征在于，若定位车辆发生定位失败或跟踪丢失现象，重新进行初始化流程。