一种基于语义物体的语义尺寸链的定位方法及其系统

本发明属于机器人平台建图定位导航领域，更具体地，涉及一种基于语义物体的语义尺寸链的定位方法及其系统。

背景技术：

1、基于amcl算法的机器人定位在普遍情况下粒子集能顺利收敛，但是由于算法本身的一些限制，也会有收敛失败的情况，依然存在一些值得改进的不足之处。amcl算法考虑到定位过程中可能会出现粒子更新过快而导致收敛错误，同时也是出于增加粒子观测时数据的多样性，对粒子滤波进行更新的速度进行了限制，防止粒子出现缺失或者退化迅速的不良现象。正是因为如此，机器人由于先验信息的不足，往往需要较多的运动才能较为准确地定位到全局初始位置，这个过程需要不断给机器人发布运动指令使其保持运动直至粒子集收拢汇聚。

2、目前主流重定位算法amcl在定位时由于缺乏环境的语义信息，不仅收敛速度慢，而且在相似环境和动态环境下容易定位错误。

技术实现思路

1、本发明针对现有技术中存在的技术问题，提供一种基于语义物体的语义尺寸链的定位方法及其系统，本发明改进了amcl重定位算法，该算法通过相机获取前方物体类别属性和距离关系，并根据当前帧中物体的类别属性和距离关系判断物体在尺寸链中的对应位置，然后利用当前帧各物体信息反解机器人的位姿，并通过计算机器人位姿估计和粒子状态分布的匹配概率来优化粒子权重更新方式，从而提高重定位成功率和速度。

2、根据本发明的第一方面，提供了一种基于语义物体的语义尺寸链的定位方法，包括以下步骤：

3、步骤1：获取数据集，根据数据集构建语义物体尺寸链，并建立二维语义栅格地图；其中，数据集包括各物体语义信息；

4、步骤2.初始化二维语义栅格地图中随机撒布的粒子集；

5、步骤3.对所述粒子集进行更新。计算出机器人运动量，并对机器人运动量进行坐标变换，对粒子集中各粒子位姿进行预测，得到预测粒子集；

6、步骤4：计算预测粒子集中各粒子的权重值，提取语义物体尺寸链中的环境语义，对环境语义中物体进行识别并进行背景分割以及坐标变换映射，判断物体在语义物体尺寸链中的对应位置，反解机器人的位姿,得到的估计位姿值集合进行权重值二次更新；

7、步骤5：对粒子进行重采样，不断循环更新直至粒子收敛，完成定位。

8、在上述技术方案的基础上，本发明还可以作出如下改进。

9、可选的，在步骤1中，所述根据数据集构建语义物体尺寸链包括：

10、同步定位信息、深度相机彩色图像与深度图像数据，将彩色图像输入到目标检测中，获取图像中的物体检测框与类别，计算彩色图中的物体在深度图中对应位置，将获取到的深度数据转换成点云信息并投影转换至全局地图坐标系下，增量式构建物体语义图，并充分发掘各语义物体之间的关系，构建语义物体尺寸链。

11、可选的，在步骤1中，所述建立二维语义栅格地图包括：

12、采用贝叶斯算法减小误检测和重复检测对物体语义图的影响，将物体语义图和栅格地图进行原点重合以及位置对齐，构建二维语义栅格地图，同时将建立的二维栅格地图和优化后的物体语义图进行坐标转换并合并得到了合并语义地图，使合并语义地图能基本反映室内环境物体语义信息。

13、可选的，在步骤2中，所述初始化二维语义栅格地图中随机撒布的粒子集包括：

14、在地图中随机撒布m个粒子，每个粒子的权重设置相同值，所有粒子权重和为1，在激光地图中初始化粒子集，权重为1/m；在粒子初始化阶段，粒子集为：

15、

16、式中，粒子集大小为χt，粒子数目为m，m大于1，当t＝0时，表示0时刻的初始粒子集。

17、可选的，在步骤3中，所述对粒子集中各粒子位姿进行预测为：根据里程计模型预测粒子集，公式表示为：

18、

19、式中，为机器人在t时刻世界坐标系下的位置，为机器人在t-1时刻世界坐标系下的位置，v表示机器人速度，δt表示时间差。

20、可选的，在步骤4中，所述对环境语义中物体进行识别并进行背景分割以及坐标变换映射包括：

21、当获取到环境语义中物体视角下的彩色图和深度图，利用提前训练好的yolo模型对彩色图进行检测识别，得到检测结果；此时检测框中包含前景和背景，使用分割算法进行前背景去除，利用深度学习模型得到的检测框代替用户绘制目标框，得到视角下彩色图的目标分割图，将目标在图像坐标系中的像素坐标转换到相机坐标系，进行语义映射。

22、可选的，在步骤4中，所述判断物体在语义物体尺寸链中的对应位置包括：

23、依次识别环境中的物体语义信息，将识别到的各物体从左到右依次排序，循环遍历构建的语义物体尺寸链，根据对当前帧识别到物体的类别属性和距离关系，计算各连续物体之间的距离信息判断是否在尺寸链中存在对应位置，直到检索成功。

24、可选的，在步骤4中，所述反解机器人的位姿,得到的估计位姿值集合表示为：

25、xc＝{(x1,y1,θ2),(x2,y2,θ2),...}；

26、式中，xc为机器人估计位置值集合、x1,2...为机器人在世界坐标系的x坐标、y1,2...为机器人在世界坐标系的y坐标，θ1,2...为机器人航向角。

27、可选的，在步骤5中，所述对粒子进行重采样，不断循环更新直至粒子收敛，完成定位包括：

28、重采样器复制高权重粒子，删除低权重粒子，使整体上粒子向二维语义栅格地图中存在检测物体周围且结构环境相似位置处收敛，在多次重采样后，最后收敛至机器人的真实位置处。

29、根据本发明的第二方面，提供一种基于语义物体的语义尺寸链的定位系统，包括：

30、栅格地图构建模块，用于获取数据集和根据数据集构建语义物体尺寸链，并建立二维语义栅格地图；其中，数据集包括各物体语义信息；

31、粒子集预处理模块，用于初始化二维语义栅格地图中随机撒布的粒子集；

32、预测粒子集获取模块，用于对所述粒子集进行更新，计算出的机器人运动量，并对机器人运动量进行坐标变换，对粒子集中各粒子位姿进行预测，得到预测粒子集；

33、估计位姿值集合计算模块，用于计算预测粒子集中各粒子的权重值，提取语义物体尺寸链中的环境语义，对环境语义中物体进行识别并进行背景分割以及坐标变换映射，判断物体在语义物体尺寸链中的对应位置，反解机器人的位姿,得到的估计位姿值集合进行权重值二次更新；

34、粒子收敛模块，用于对粒子进行重采样，不断循环更新直至粒子收敛，完成定位。

35、本发明的技术效果和优点：

36、本发明针对amcl定位算法存在的某些场景下易定位失败的问题，提出结合包含语义物体尺寸链的语义地图进行amcl算法，定位过程中利用深度学习模型yolo进行物体识别检测，将识别到的各物体从左到右依次排序，循环遍历构建的语义物体尺寸链，根据当前帧识别到物体的类别属性和计算的各连续物体之间的距离信息判断是否在尺寸链中存在对应位置,利用构建具有语义物体尺寸链的语义地图先实现视觉预定位，使得机器人可以在少量先验信息和运动的情况下更迅速地实现初定位，结合amcl算法与环境地图匹配，改进了粒子权重更新方式，进行精定位，有效提高了amcl算法的定位效率和准确性。

37、本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所指出的结构来实现和获得。