本发明涉及一种移动机器人导航领域，特别是涉及一种同步定位与地图构建方法。
背景技术：
自主移动机器人的同步定位与地图构建(simultaneouslocalizationandmapping，slam)问题可描述为：在未知环境中，移动机器人通过机载环境感测传感器(如里程计、视觉环境感测传感器、超声波及激光等)来感知环境信息，逐步构建周围环境地图，同时运用此地图对其位置和姿态进行估计。该问题一直是移动机器人研究领域的热点和难点，被认为是能否真正实现机器人自主导航的关键问题，具有广阔的应用前景。
现有的地图构建方法主要是基于栅格的地图,这种方法也称作占有格（证据格）地图。对于栅格地图,整个环境被分割成一定大小的栅格,每一个栅格赋以一个值,表示这个单元格被占的概率。每个单元格代表一个正方形的区块,用一个在（0,1）范围内的值来指示这个区块被占有的概率,表明它所对应的物理位置是否有障碍物存在。占有格地图清楚地表明某个区块是障碍区块,还是自由空间。被占的栅格赋值,自由空间赋值。对于的栅格地图,每一个栅格赋以一个值,表示这个单元格的高度信息。
对于大规模环境,占有格地图表示方法的一个最大难点就是在单元格分辨率与计算复杂度之间找到平衡。理想情况下,为了得到更精确的环境信息,以得到更精确的位置估计,就要求单元格的尺寸尽可能小。这样,从计算可行性考虑,空间复杂度和时间复杂度随着单元格的增加而增加，故其主要缺点是用于定位时的高计算量和存储空间需求。
技术实现要素：
本发明旨在提供一种简化计算量和减少存储空间的地图构建方法和设备，旨在提高同步定位与地图构建便捷性。
本发明的一种同步定位与地图构建方法，用于机器人，其特征在于，所述地图构建方法包括步骤：加载预先建立的机器人运动学模型；通过环境感测传感器扫描环境而在预设坐标系中建立初始全局地图，所述初始全局地图采用若干几何原形表示，所述几何原形按同一顺序存储，并标注存储序号；在移动至当前位置时，通过环境感测传感器扫描获得当前局部地图，当前局部地图采用若干几何原形表示，所述几何原形按与初始全局地图一致的同一顺序存储，并标注存储序号，根据所述当前局部地图和初始全局地图几何原形存储序号的比对进行融合得到更新全局地图；根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中所述通过环境感测传感器扫描环境而在预设坐标系中建立初始全局地图包括：
a）加载预设建立的机器人运动学模型；
b）通过环境感测传感器在第一位置扫描机器人所在工作环境，采集工作环境中环境感测传感器与所所在工作环境中各障碍物的距离数据帧，生成点云数据，计算机接收环境感测传感器反馈的点云数据，根据最小二乘法拟合出所在工作环境中各障碍物的几何原形，并按同一顺序标注存储序号，生成第一局部地图；
c)移动至第二位置时，通过环境感测传感器扫描并获得第二局部地图，根据所述第二局部地图和第一局部地图融合得到第一局部融合地图；
d）依次多次重复步骤b)、c)，由若干局部地图和前一局部融合地图不断融合得到所在工作环境的初始全局地图。
进一步地，本发明的同步定位与地图构建方法，其中从距离数据帧中提取几何原形区块,并用几何原形对所在工作环境中各障碍物点云数据进行连续的分隔融合处理,拟合生成包含若干特征点和/或特征线段的几何原形，将所述几何原形在环境地图中按照同一方向顺序存储，并标注存储序号。
进一步地，本发明的同步定位与地图构建方法，其中所述几何原形包括线段、椭圆、圆、断点和角点，在初始全局地图中按照几何位置关系逆时针顺序存储，并标注存储序号，所述存储序号为数字、字母或数字与字母的组合。
进一步地，本发明的同步定位与地图构建方法，其中对第一局部地图的每个第一几何原形标注数字表示该第一几何原形在地图中的存储序号；对第二局部地图的每个第二几何原形标注数字表示该第二几何原形在地图中的存储序号，如果第二局部地图没有与第一局部地图相同的存储序号，则按照顺序把第二几何原形插入到第一局部地图中的适当位置形成第一融合局部地图，如果第二局部地图有与第一局部地图相同的存储序号，则将具有存储序号相同的第二几何原形与第一几何原形重叠，融合第二局部地图和第一局部地图形成第一融合局部地图；如果第一局部地图为逆时针顺序地图,则第一融合局部地图也为逆时针顺序地图。
进一步地，本发明的同步定位与地图构建方法，其中在移动至当前位置时，通过环境感测传感器预估所述机器人的当前估计位姿包括：在移动至当前位置时，根据所述机器人运动距离和运动方向，计算获得所述机器人当前位姿，并且将所述机器人当前位姿作为所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中当环境感测传感器扫描到环境中出现新障碍物时：当新障碍物为静态，则将更新全局地图存储；当新障碍物为动态，则不将更新全局地图存储；根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中还包括设置在下部的障碍物识别模块，所述环境感测传感器为激光扫描仪，所述障碍物识别模块为激光扫描仪、声纳环境感测传感器或超声波环境感测传感器。
本发明的另一目的在于提供一种同步定位与地图构建设备，采用上述同步定位与地图构建方法实现。
有益效果：本发明同步定位与地图构建方法，可实现机器人的精准定位，同时地图构建采用几何原形，并按同一顺序存储，并标注存储序号，大大简化计算量和减少存储和调取时间，提高同步定位与地图构建便捷性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1、本发明同步定位与地图构建设备示意图。
图2、本发明数字标记几何原形的地图融合示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1、图2所示，本发明的一种同步定位与地图构建方法，用于机器人，其特征在于，所述地图构建方法包括步骤：加载预先建立的机器人运动学模型；通过环境感测传感器扫描环境而在预设坐标系中建立初始全局地图，所述初始全局地图采用若干几何原形表示，所述几何原形按同一顺序存储，并标注存储序号；在移动至当前位置时，通过环境感测传感器扫描获得当前局部地图，当前局部地图采用若干几何原形表示，所述几何原形按与初始全局地图一致的同一顺序存储，并标注存储序号，根据所述当前局部地图和初始全局地图几何原形存储序号的比对进行融合得到更新全局地图；根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中所述通过环境感测传感器扫描环境而在预设坐标系中建立初始全局地图包括：
a）加载预设建立的机器人运动学模型；
b）通过环境感测传感器在第一位置扫描机器人所在工作环境，采集工作环境中环境感测传感器与所所在工作环境中各障碍物的距离数据帧，生成点云数据，计算机接收环境感测传感器反馈的点云数据，根据最小二乘法拟合出所在工作环境中各障碍物的几何原形，并按同一顺序标注存储序号，生成第一局部地图；
c)移动至第二位置时，通过环境感测传感器扫描并获得第二局部地图，根据所述第二局部地图和第一局部地图融合得到第一局部融合地图；
d）依次多次重复步骤b)、c)，由若干局部地图和前一局部融合地图不断融合得到所在工作环境的初始全局地图。
进一步地，本发明的同步定位与地图构建方法，其中从距离数据帧中提取几何原形区块,并用几何原形对所在工作环境中各障碍物点云数据进行连续的分隔融合处理,拟合生成包含若干特征点和/或特征线段的几何原形，将所述几何原形在环境地图中按照同一方向顺序存储，并标注存储序号。
进一步地，本发明的同步定位与地图构建方法，其中所述几何原形包括线段、椭圆、圆、断点和角点，在初始全局地图中按照几何位置关系逆时针顺序存储，并标注存储序号，所述存储序号为数字、字母或数字与字母的组合。
进一步地，本发明的同步定位与地图构建方法，其中对第一局部地图（a）的每个第一几何原形标注数字表示该第一几何原形在地图中的存储序号；对第二局部地图（b）的每个第二几何原形标注数字表示该第二几何原形在地图中的存储序号，如果第二局部地图没有与第一局部地图相同的存储序号，则按照顺序把第二几何原形插入到第一局部地图中的适当位置形成第一融合局部地图（c），如果第二局部地图有与第一局部地图相同的存储序号，则将具有存储序号相同的第二几何原形与第一几何原形重叠，融合第二局部地图和第一局部地图形成第一融合局部地图；如果第一局部地图为逆时针顺序地图,则第一融合局部地图也为逆时针顺序地图。
进一步地，本发明的同步定位与地图构建方法，其中在移动至当前位置时，通过环境感测传感器预估所述机器人的当前估计位姿包括：在移动至当前位置时，根据所述机器人运动距离和运动方向，计算获得所述机器人当前位姿，并且将所述机器人当前位姿作为所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中当环境感测传感器扫描到环境中出现新障碍物时：当新障碍物为静态，则将更新全局地图存储；当新障碍物为动态，则不将更新全局地图存储；根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建方法，其中还包括设置在下部的障碍物识别模块，所述环境感测传感器为激光扫描仪，所述障碍物识别模块为激光扫描仪、声纳环境感测传感器或超声波环境感测传感器。
本发明实现上述同步定位与地图构建方法的一种同步定位与地图构建设备，用于机器人，其特征在于，包括：
运动模型构建单元，预先建立的机器人运动学模型；
环境感测传感器，通过环境感测传感器在当前位置扫描机器人所在工作环境，采集工作环境中环境感测传感器与所在工作环境中各障碍物的距离数据帧，生成点云数据；
地图构建单元，接收环境感测传感器反馈的点云数据，根据最小二乘法拟合出所在工作环境中各障碍物的几何原形，并按同一顺序存储并标注存储序号，生成当前位置局部地图；
存储单元，存储有事先通过环境感测传感器扫描环境而在预设坐标系中构建的初始全局地图；
融合单元，在移动至当前位置时，将通过环境感测传感器扫描获得当前位置局部地图和初始全局地图融合得到更新全局地图；
位姿估计单元，根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建设备，其中初始全局地图的地图构建包含：
运动模型构建单元，预先建立的机器人运动学模型；
环境感测传感器，通过环境感测传感器在当前位置扫描机器人所在工作环境，采集工作环境中环境感测传感器与所在工作环境中各障碍物的距离数据帧，生成点云数据；
地图构建模块，接收环境感测传感器反馈的当前位置的点云数据，根据最小二乘法拟合出所在工作环境中各障碍物的几何原形，并按同一顺序存储并标注存储序号，生成当前位置局部地图；
融合单元，如果当前局部地图没有与上一局部地图相同的存储序号，则按照同一顺序将当前局部地图中的几何原形插入到上一局部地图中的适当位置形成第一融合局部地图，如果当前局部地图有与上一局部地图相同的存储序号，则按照同一顺序将具有存储序号相同的几何原形重叠，融合第二局部地图和第一局部地图形成第一融合局部地图；
判断单元，判断所述地图是否出现闭环，当所述地图出现闭环时，完成构建闭环的地图。
进一步地，本发明的同步定位与地图构建设备，其中融合单元，从距离数据帧中提取点云数据,并对所在工作环境中各障碍物行连续的分隔融合处理,拟合生成包含若干特征点和/或特征线段的几何原形，将所述几何原形在环境地图中按照同一方向顺序存储。
进一步地，本发明的同步定位与地图构建设备，其中地图构建模块中，所述几何原形包括线段、椭圆、圆、断点和角点，按照几何位置关系以逆时针顺序存储，并标注存储序号。
进一步地，本发明的同步定位与地图构建设备，其中其中地图构建模块中，存储序号包括数字和/或字母。
进一步地，本发明的同步定位与地图构建设备，其中还包括计算单元，在移动至当前位置时，根据所述机器人运动距离和运动方向，计算获得所述机器人当前位姿，并且将所述机器人当前位姿作为所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建设备，其中当环境感测传感器扫描到环境中出现新障碍物时：
当新障碍物为静态，则将更新全局地图存储；
当新障碍物为动态，则不将更新全局地图存储；
根据所述当前局部地图、初始全局地图和更新全局地图对所述机器人的当前估计位姿。
进一步地，本发明的同步定位与地图构建设备，其中还包括设置在下部的障碍物识别模块，所述环境感测传感器为激光扫描仪，所述障碍物识别模块为激光扫描仪、声纳环境感测传感器或超声波环境感测传感器。
有益效果：本发明同步定位与地图构建方法，可实现机器人的精准定位，同时地图构建采用几何原形，并按同一顺序存储，并标注存储序号，大大简化计算量和减少存储和调取时间，提高同步定位与地图构建便捷性。
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。