基于时空信息的工业厂房定位和建图方法及系统

本发明涉及定位领域，具体地，涉及一种基于时空信息的工业厂房定位和建图方法及系统。

背景技术：

1、同时定位与地图构建技术(slam)能够为自主移动机器人提供位置信息和周围环境的信息，是其完成移动任务的关键技术之一。但在加工大型工件的厂房中，其复杂的室内环境会对slam带来以下困难：

2、一、slam的前端配准环节是通过对环境中的特征结构信息进行匹配来实现的。大型厂房地面较为平整光滑，会出现反光现象，如果使用视觉传感器会在图片中产生耀光，无法有效提取场景内的特征点；并且大型厂房较为空旷，激光雷达传感器无法获取有效的点云信息。厂房的这些特点会导致多种传感器无法从地面获取有效的环境特征信息，进而导致slam的前端配准环节难以进行，出现定位不准甚至完全失效等现象。二、大型厂房中通常用流水线处理大型工件，场景变化率高，如果使用传统slam系统所构建的地图作为先验地图进行重定位，厂房中的变动会增大环境与先验地图之间的差异，导致重定位效果的精度降低。

3、专利文献cn115439601a公开了一种基于三维建模的厂房管理系统，主要包括厂房定位模块、厂房平面图获取模块、建模模块、轨迹生成模块和信息处理模块，其中厂房定位模块采用aoa定位算法对设备和/或人员进行实时定位，能够及时准确地反应设备和/或人员的移动情况，算法简单，计算结果精确；建模模块根据厂房cad格式的2d平面图生成厂房的三维建模，轨迹生成模块根据位置信息生成设备和/或人员的三维坐标，并根据实时生成的三维坐标形成设备和/或人员的运动轨迹。但该发明没有利用实例分割网络进行建图。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于时空信息的工业厂房定位和建图方法及系统。

2、根据本发明提供的一种基于时空信息的工业厂房定位和建图方法，包括：

3、步骤s1：将激光雷达获得的激光点云与单目相机获得的图片进行特征深度关联；进行前端点云配准，对单目相机获得的视频流中进行语义实例分割，将环境物体点进行标记，进行因子图优化，根据优化后的轨迹进行全局地图拼接，得到当前厂房环境的全局地图；对全局地图设置加工状态标签；构建和维护工厂时空地图库；

4、步骤s2：将时空信息地图库和工厂生产计划的先验信息进行当前地图预测，结合预测的地图和工厂顶部结构化信息进行扩展卡尔曼滤波，获得基于时空信息的高精度定位信息。

5、优选地，在所述步骤s1中：

6、步骤s1.1：对地面单目相机和地面激光雷达进行外参标定，得到地面单目相机和地面激光雷达之间的变换矩阵；

7、步骤s1.2：根据标定获得的传感器外参，将激光雷达获得的激光点云与单目相机获得的图片进行特征深度关联，恢复图片中的深度信息；

8、步骤s1.3：对激光雷达获取的点云进行前端点云配准，得到激光雷达坐标系下的移动机器人的全局位姿tg，作为激光雷达因子；

9、步骤s1.4：对单目相机获得的视频流中进行语义实例分割，获得厂房处理当前工件时环境物体的语义信息；并根据特征匹配获取当前环境物体的位姿tobj；

10、步骤s1.5：根据相机和激光雷达之间的变化矩阵、全局位姿tg，以及标记语义物体的位姿tobj，利用特征深度关联的方法将激光雷达点云中的环境物体点进行标记，记作pobj；

11、步骤s1.6：视觉里程计对关键帧进行描述子提取，利用词袋模型进行闭环检测，当发现闭环候选帧时，将时间戳发送到激光里程计，激光里程计利用icp进行配准，得到回环后的位姿；

12、步骤s1.7：将视觉因子、激光雷达因子和回环因子加入因子图进行图优化，得到优化后的高精度轨迹；

13、步骤s1.8：根据优化后的轨迹以及传感器外参变换矩阵，进行全局地图拼接，得到当前厂房环境的全局地图；

14、步骤s1.9：根据地图中物体的语义、当日生产计划和人工标注，对全局地图设置加工状态标签；进行保存，构成时空信息地图库。

15、优选地，在步骤s1.2中，为了进行特征深度关联，将激光雷达点云与图片特征根据外参变换矩阵投影到球体上，利用kdtree搜索视觉特征坐标，得到距离此特征最近的三个激光雷达点，此视觉特征的深度为特征点到这三个激光雷达点所组成的平面的距离；

16、在步骤s1.4中，利用yolact edge深度学习网络对厂房环境中有概率变动的物体进行语义实例分割，重点训练能体现出当前工厂处于何种加工状态的物品，语义信息用于判断当前工厂的加工状态；

17、在步骤s1.5中，利用步骤s1.2中特征深度关联的方法，将视觉图片中的语义信息标注到对应的激光雷达点云上；

18、在步骤s1.8中，得到的全局地图为此次slam运行周期内创建的全局地图，其中实例分割出的物体被特殊标注，额外添加了其语义信息；

19、在步骤s1.9中，根据所加工的工件的语义标签、当日的生产计划或采用人工标注方式，对地图设置标签；标签体现当前时空域下的工厂地图处于何种状态、加工何种物体。

20、优选地，在所述步骤s2中：

21、步骤s2.1：利用激光雷达提取厂房天花板桁架和墙面上的结构化特征信息，当发现回环时，将当前帧的激光雷达点云与局部地图进行最近点搜索和正态分布变换方法进行配准，得到回环信息，将定位因子和回环因子加入因子图进行优化，得到空间定位信息ts；

22、步骤s2.2：利用负责获取地面信息的单目相机和激光雷达实现里程计功能，在运行过程中实时进行点云拼接，得到目前运行的局部地图ml；

23、步骤s2.3：将所提供的工厂生产作业计划作为先验信息，将步骤s2.2的局部地图先与时空信息地图库中符合生产计划的地图ms进行icp匹配，将匹配程度超过阈值且最高的地图作为目前的预测地图mp；如果没有匹配程度超过阈值的地图，则取消时间信息定位，执行步骤s2.5；

24、步骤s2.4：将步骤s2.3获得的预测地图mp与目前的局部地图ml进行重定位，得到基于时间信息定位信息tt；

25、步骤s2.5：将时间定位信息tt和空间定位信息ts进行扩展卡尔曼滤波，得到基于时空信息的定位。

26、优选地，在步骤s2.2中，实现激光里程计和局部地图拼接的过程中，将激光里程计的误差和因子图优化的残差保存下来，作为空间定位的置信度；

27、在步骤s2.3中，局部地图ml与地图库中的地图ms的匹配流程如下：首先将地图库中标签符合当日生产计划的地图取出进行icp匹配，保存其匹配程度；如果匹配程度超过阈值，则仅对比符合生产计划的地图，并选取得分最高的地图作为预测地图；如果匹配程度未超过阈值，则对比标签是其他生产状态的地图；保存预测地图的匹配程度作为时间定位的置信度，为后续卡尔曼增益的确定提供数据；如果最后有匹配程度超过阈值的地图，则用其作为预测地图，如果始终没有匹配程度超过阈值的地图，则取消利用时间信息定位，不进行步骤s2.4；

28、在步骤s2.4中，根据局部地图与预测地图的匹配程度，以及预测地图的置信度，决定时间定位的置信度；

29、在步骤s2.5中，根据空间定位的置信度决定空间定位协方差，根据时间定位的置信度决定时间定位协方差；根据二者协方差决定卡尔曼增益，进行卡尔曼滤波，得到最后的基于时空信息的高精度定位。

30、根据本发明提供的一种基于时空信息的工业厂房定位和建图系统，包括：

31、模块m1：将激光雷达获得的激光点云与单目相机获得的图片进行特征深度关联；进行前端点云配准，对单目相机获得的视频流中进行语义实例分割，将环境物体点进行标记，进行因子图优化，根据优化后的轨迹进行全局地图拼接，得到当前厂房环境的全局地图；对全局地图设置加工状态标签；构建和维护工厂时空地图库；

32、模块m2：将时空信息地图库和工厂生产计划的先验信息进行当前地图预测，结合预测的地图和工厂顶部结构化信息进行扩展卡尔曼滤波，获得基于时空信息的高精度定位信息。

33、优选地，在所述模块m1中：

34、模块m1.1：对地面单目相机和地面激光雷达进行外参标定，得到地面单目相机和地面激光雷达之间的变换矩阵；

35、模块m1.2：根据标定获得的传感器外参，将激光雷达获得的激光点云与单目相机获得的图片进行特征深度关联，恢复图片中的深度信息；

36、模块m1.3：对激光雷达获取的点云进行前端点云配准，得到激光雷达坐标系下的移动机器人的全局位姿tg，作为激光雷达因子；

37、模块m1.4：对单目相机获得的视频流中进行语义实例分割，获得厂房处理当前工件时环境物体的语义信息；并根据特征匹配获取当前环境物体的位姿tobj；

38、模块m1.5：根据相机和激光雷达之间的变化矩阵、全局位姿tg，以及标记语义物体的位姿tobj，利用特征深度关联的方法将激光雷达点云中的环境物体点进行标记，记作pobj；

39、模块m1.6：视觉里程计对关键帧进行描述子提取，利用词袋模型进行闭环检测，当发现闭环候选帧时，将时间戳发送到激光里程计，激光里程计利用icp进行配准，得到回环后的位姿；

40、模块m1.7：将视觉因子、激光雷达因子和回环因子加入因子图进行图优化，得到优化后的高精度轨迹；

41、模块m1.8：根据优化后的轨迹以及传感器外参变换矩阵，进行全局地图拼接，得到当前厂房环境的全局地图；

42、模块m1.9：根据地图中物体的语义、当日生产计划和人工标注，对全局地图设置加工状态标签；进行保存，构成时空信息地图库。

43、优选地，在模块m1.2中，为了进行特征深度关联，将激光雷达点云与图片特征根据外参变换矩阵投影到球体上，利用kdtree搜索视觉特征坐标，得到距离此特征最近的三个激光雷达点，此视觉特征的深度为特征点到这三个激光雷达点所组成的平面的距离；

44、在模块m1.4中，利用yolact edge深度学习网络对厂房环境中有概率变动的物体进行语义实例分割，重点训练能体现出当前工厂处于何种加工状态的物品，语义信息用于判断当前工厂的加工状态；

45、在模块m1.5中，利用模块m1.2中特征深度关联的方法，将视觉图片中的语义信息标注到对应的激光雷达点云上；

46、在模块m1.8中，得到的全局地图为此次slam运行周期内创建的全局地图，其中实例分割出的物体被特殊标注，额外添加了其语义信息；

47、在模块m1.9中，根据所加工的工件的语义标签、当日的生产计划或采用人工标注方式，对地图设置标签；标签体现当前时空域下的工厂地图处于何种状态、加工何种物体。

48、优选地，在所述模块m2中：

49、模块m2.1：利用激光雷达提取厂房天花板桁架和墙面上的结构化特征信息，当发现回环时，将当前帧的激光雷达点云与局部地图进行最近点搜索和正态分布变换方法进行配准，得到回环信息，将定位因子和回环因子加入因子图进行优化，得到空间定位信息ts；

50、模块m2.2：利用负责获取地面信息的单目相机和激光雷达实现里程计功能，在运行过程中实时进行点云拼接，得到目前运行的局部地图ml；

51、模块m2.3：将所提供的工厂生产作业计划作为先验信息，将模块m2.2的局部地图先与时空信息地图库中符合生产计划的地图ms进行icp匹配，将匹配程度超过阈值且最高的地图作为目前的预测地图mp；如果没有匹配程度超过阈值的地图，则取消时间信息定位，执行模块m2.5；

52、模块m2.4：将模块m2.3获得的预测地图mp与目前的局部地图ml进行重定位，得到基于时间信息定位信息tt；

53、模块m2.5：将时间定位信息tt和空间定位信息ts进行扩展卡尔曼滤波，得到基于时空信息的定位。

54、优选地，在模块m2.2中，实现激光里程计和局部地图拼接的过程中，将激光里程计的误差和因子图优化的残差保存下来，作为空间定位的置信度；

55、在模块m2.3中，局部地图ml与地图库中的地图ms的匹配流程如下：首先将地图库中标签符合当日生产计划的地图取出进行icp匹配，保存其匹配程度；如果匹配程度超过阈值，则仅对比符合生产计划的地图，并选取得分最高的地图作为预测地图；如果匹配程度未超过阈值，则对比标签是其他生产状态的地图；保存预测地图的匹配程度作为时间定位的置信度，为后续卡尔曼增益的确定提供数据；如果最后有匹配程度超过阈值的地图，则用其作为预测地图，如果始终没有匹配程度超过阈值的地图，则取消利用时间信息定位，不进行模块m2.4；

56、在模块m2.4中，根据局部地图与预测地图的匹配程度，以及预测地图的置信度，决定时间定位的置信度；

57、在模块m2.5中，根据空间定位的置信度决定空间定位协方差，根据时间定位的置信度决定时间定位协方差；根据二者协方差决定卡尔曼增益，进行卡尔曼滤波，得到最后的基于时空信息的高精度定位。

58、与现有技术相比，本发明具有如下的有益效果：

59、1、本发明在多品类工件处理厂房环境变化率高的情况下，利用实例分割网络进行建图；根据地图中的语义信息、当日生产计划和人工标注等，对地图添加标签，以表示建图期间的工厂生产状态，并存入地图库，为以后的定位提供时间信息；

60、2、本发明定位过程中，为避免地面退化场景(地面光滑导致的反光、地面空旷缺少特征信息)和地面环境高变化率(大型工件的变化导致环境变化率高)的影响，利用了厂房天花板、墙面等结构化环境，进行特征提取，作为空间信息提高定位的精度。同时结合工厂生产计划和地图库的时间信息，预测当前运行期间的工厂生产状态，获取其对应的地图作为预测地图进行重定位；

61、3、本发明将基于空间信息和时间信息的定位进行扩展卡尔曼滤波，提高了高变化工厂环境下的定位精度。