[发明专利]一种地图构建方法、装置及终端设备

权利要求书

1.一种地图构建方法，其特征在于，包括：

控制小车在目标范围内运动；

通过设置于所述小车上的激光雷达采集激光雷达数据；

在采集第N帧所述激光雷达数据时创建第M个子图，其中，N和M为正整数，N和M的初始值均为1；

通过相关扫描匹配，将预设帧数的所述激光雷达数据插入所述第M个子图中，获得M个子地图；

在预设帧数的所述激光雷达数据中的每一帧所述激光雷达数据插入第M个所述子图时，进行初值优化；

开启闭环检测，以使预设帧数的所述激光雷达数据插入第M个所述子图时，对第M个所述子图进行闭环检测，消除第M个所述子图与所述激光雷达数据的累积误差；

根据所述M个子地图构建基于所述目标范围的全局地图；

在预设帧数的所述激光雷达数据中的每一帧所述激光雷达数据插入第M个子图时，进行初值优化，包括：

进行第一次相关扫描匹配；

计算每一帧所述激光雷达数据与第M个所述子图匹配的概率，公式为：

；

其中，表示小车当前时刻的位姿，小车上一时刻的位姿为，表示任一帧激光雷达数据，j表示预设帧数，z_j表示预设帧数的激光雷达数据，表示世界模型，且小车当前时刻的位姿通过第一次的相关扫描匹配获取；

获取所述概率的最大值；

选择所述概率最大时的激光雷达数据，并获取梯度下降法的初值；

根据所述初值，通过梯度下降法对所述激光雷达数据进行优化；

进行第二次相关扫描匹配，将优化后的每一帧所述激光雷达数据，插入第M个所述子图。

2.如权利要求1所述的地图构建方法，其特征在于，所述概率最大时，获取梯度下降法的初值，包括：

所述概率最大时的激光雷达数据上均匀划分栅格；

利用高斯分布扩散栅格得分；

在三维空间中迭代循环计算总栅格得分；其中，x表示横坐标，y表示纵坐标，θ表示旋转角度；

所述总栅格得分为所述概率最大时的激光雷达数据的后一帧激光雷达数据，与所述概率最大时的激光雷达数据的前一帧激光雷达数据的栅格得分之和；

输出总栅格得分最高的变换，并以所述总栅格得分最高的变换作为梯度下降法的初值；其中，x_m表示世界模型下的横坐标，y_m表示世界模型下的纵坐标，θ_m表示世界模型下的旋转角度。

3.如权利要求1所述的地图构建方法，其特征在于，所述开启闭环检测，以使预设帧数的所述激光雷达数据插入第M个所述子图时，对第M个所述子图进行闭环检测，消除第M个所述子图与所述激光雷达数据的累积误差，包括：

设置检测周期；

每间隔所述检测周期，在预设帧数的所述激光雷达数据中查找关键帧，并通过关键帧的激光雷达数据进行闭环检测，根据闭环检测结果消除第M个所述子图与所述激光雷达数据的累积误差；

其中，所述检测周期小于所述预设帧数的激光雷达数据插入第M个子图，和所述预设帧数的激光雷达数据插入第M+1个子图的周期。