[发明专利]一种同步定位与地图构建方法及设备

权利要求书

1.一种同步定位与地图构建方法，用于机器人，其特征在于，包括以下步骤：

S01：加载预先建立的机器人运动学模型；

S02：通过环境感测传感器扫描环境在预设坐标系中建立初始全局地图，初始全局地图的建立包括以下步骤：

A)通过环境感测传感器在第一位置扫描机器人所在工作环境，采集工作环境中环境感测传感器与所在工作环境中各障碍物的距离数据帧，生成点云数据，对生成的点云数据，根据最小二乘法拟合出所在工作环境中各障碍物的几何原形，并根据几何位置关系按同一顺序标注存储序号，生成第一局部地图；

B)移动至第二位置时，通过环境感测传感器扫描并获得第二局部地图，根据所述第二局部地图和第一局部地图融合得到第一局部融合地图；

C)依次多次重复步骤A)、B)，由若干局部地图和前一局部融合地图融合得到所在工作环境的初始全局地图；

所述局部地图的融合包括以下步骤：

从距离数据帧中提取几何原形区块，并用几何原形区块对所在工作环境中各障碍物点云数据进行连续的分隔融合处理，拟合生成包含若干特征点和/或特征线段的特征几何原形，将所述特征几何原形在环境地图中按照同一方向顺序存储，并标注存储序号；

S03：在移动至当前位置时，通过环境感测传感器扫描获得当前局部地图，当前局部地图采用若干几何原形表示，所述几何原形按与初始全局地图一致的同一顺序存储，并标注存储序号，根据所述当前局部地图和初始全局地图几何原形存储序号的比对进行融合得到更新全局地图；

S04：根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。

2.根据权利要求1所述的同步定位与地图构建方法，其特征在于：所述几何原形包括线段、椭圆、圆、断点和角点，在初始全局地图中按照几何位置关系逆时针顺序存储，并标注存储序号，所述存储序号为数字、字母或数字与字母的组合。

3.根据权利要求2所述的同步定位与地图构建方法，其特征在于：所述局部地图融合还包括以下步骤：

如果第二局部地图没有与第一局部地图相同的存储序号，则按照顺序把第二局部地图的第二几何原形插入到第一局部地图中的适当位置形成第一融合局部地图；

如果第二局部地图有与第一局部地图相同的存储序号，则将具有存储序号相同的第二局部地图的第二几何原形与第一局部地图的第一几何原形重叠，融合第二局部地图和第一局部地图形成第一融合局部地图；

如果第一局部地图为逆时针顺序地图，则第一融合局部地图也为逆时针顺序地图。

4.根据权利要求3所述的同步定位与地图构建方法，其特征在于，在移动至当前位置时，通过环境感测传感器预估所述机器人的当前估计位姿包括：

在移动至当前位置时，根据所述机器人运动距离和运动方向，计算获得所述机器人当前位姿，并且将所述机器人当前位姿作为所述机器人的当前估计位姿。

5.根据权利要求1所述的同步定位与地图构建方法，其特征在于，当环境感测传感器扫描到环境中出现新障碍物时：

当新障碍物为静态，则将更新全局地图存储；

当新障碍物为动态，则不将更新全局地图存储；

根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。

6.根据权利要求1所述的同步定位与地图构建方法，其特征在于，还包括设置在下部的障碍物识别模块，所述环境感测传感器为激光扫描仪，所述障碍物识别模块为激光扫描仪、声纳环境感测传感器或超声波环境感测传感器。