一种墙角族语义尺寸链构建系统及方法

本发明属于机器人平台建图定位导航，具体涉及一种墙角族语义尺寸链构建系统及方法。

背景技术：

1、随着移动机器人技术的不断发展，其应用范围也越来越广泛，尤其是在室内智能服务、物流仓储等领域。当今室内移动机器人领域最重要的问题是如何准确且快速地实现机器人的定位导航。在实际场景下，移动机器人需要获取周围环境的信息，以实现自主导航、路径规划等功能。而作为定位核心的slam技术取得了良好的效果，但如何提高移动机器人定位的实时性和鲁棒性一直是业界探索的焦点。

技术实现思路

1、为了解决现有技术问题，本发明提出一种墙角族语义尺寸链构建系统及方法。

2、墙角作为室内环境中的稳定特征，能为机器人提供更加准确的定位参考。首先融合机器人多传感器信息并基于gmapping算法构建二维栅格地图，其次基于yolov5的目标检测方法提取室内环境的墙角语义和物体语义信息，结合grabcut算法和贝叶斯方法增量式构建语义映射地图，然后将二维栅格地图和语义映射图合并得到语义地图。根据场景中各物体之间的距离关系构建出墙角族语义尺寸链，并基于墙角族语义尺寸链进行全局预定位。

3、本发明系统的技术方案为一种墙角族语义尺寸链构建系统，包括：

4、机器人平台底盘、主控机、深度相机、激光雷达、电子罗盘；

5、所述的机器人平台底盘装载所述的主控机、深度相机、激光雷达、电子罗盘；

6、所述主控机分别与所述的机器人平台底盘、深度相机、激光雷达、电子罗盘通过有线方式依次连接；

7、本发明方法的技术方案为一种墙角族语义尺寸链构建方法，包括以下步骤：

8、通过深度相机连续采集多个时刻的彩色图像，通过激光雷达连续采集多个时刻的点云数据，通过电子罗盘连续采集多个时刻的航向角；

9、机器人通过yolo目标检测算法获取物体的语义信息、墙角的语义信息，根据对应时刻室内环境中每个物体的位置、类别以及彩色图像的深度信息，通过坐标转换映射到语义映射图上；根据对应时刻室内环境中每个墙角的位置、类别以及彩色图像的深度信息，通过坐标转换映射到语义映射图上；

10、机器人通过gmapping算法构建二维栅格地图；将语义映射图和二维栅格地图原点重合、位置对齐以及坐标转换的操作后，再将语义映射图和二维栅格地图合并得到合并后的语义地图；

11、机器人根据合并语义地图中各墙角之间、墙角与室内物体之间的类别和位置关系，以顺时针为正方向沿墙构建墙角族语义尺寸链，并得到相应的检索表；

12、机器人实时在室内环境中移动，当深度相机识别到的墙角数量大于等于墙角数量阈值时，根据墙角的类别和两墙角之间的距离在检索表中查找，查找成功后进行预定位；当识别到的墙角数量为1时，根据墙角的类别和墙角周围物体的类别在检索表中查找，查找成功后进行预定位。

13、步骤1：机器人选取室内边缘的一处凹墙角为起点，沿着该墙角的一面墙壁出发，通过深度相机连续采集多个时刻的彩色图像，通过激光雷达连续采集多个时刻的点云数据，通过电子罗盘连续采集多个时刻的航向角，遍历整个室内环境，直至回到起点；

14、步骤2：机器人通过yolo目标检测算法获取物体的语义信息、墙角的语义信息，若对应时刻彩色图像存在物体，则标记出对应时刻室内环境中每个物体的位置、类别；若对应时刻彩色图像存在墙角，则标记出对应时刻室内环境中每个墙角的位置、类别，并结合对应时刻的航向角以及激光雷达的点云数据区分对应时刻彩色图像中每个墙角的方向性；根据对应时刻室内环境中每个物体的位置、类别以及彩色图像的深度信息，通过坐标转换映射到语义映射图上；根据对应时刻室内环境中每个墙角的位置、类别以及彩色图像的深度信息，通过坐标转换映射到语义映射图上；

15、步骤3：机器人通过gmapping算法构建二维栅格地图；将语义映射图和二维栅格地图原点重合、位置对齐以及坐标转换的操作后，再将语义映射图和二维栅格地图合并得到合并后的语义地图。

16、步骤4：机器人根据合并语义地图中各墙角之间、墙角与室内物体之间的类别和位置关系，以顺时针为正方向沿墙构建墙角族语义尺寸链，并得到相应的检索表，检索表中存储了每个墙角各自的类别属性，还有上游墙角、下游墙角、上下游墙角的距离信息以及墙角周围物体信息。

17、步骤5：机器人实时在室内环境中移动，通过深度相机采集当前时刻的彩色图像，通过激光雷达采集当前时刻的点云数据，通过电子罗盘获取当前时刻机器人的航向角；当深度相机识别到的墙角数量大于等于墙角数量阈值时，根据墙角的类别和两墙角之间的距离在检索表中查找，查找成功后进行预定位；当识别到的墙角数量为1时，根据墙角的类别和墙角周围物体的类别在检索表中查找，查找成功后进行预定位。

18、作为优选，步骤2所述物体的类别为：门、柜子、垃圾桶、椅子、凳子；

19、步骤2所述墙角的类别为：分为凹墙角和凸墙角；

20、步骤2所述结合对应时刻航向角区分对应时刻彩色图像中每个墙角方向性的算法为：

21、机器人对室内墙面主方向进行判断，以步骤1中所选的起点凹墙角为基准，将机器人视角下左墙壁作为x轴正方向，右墙壁作为y轴正方向，将机器人平行于y轴正方向的航向角置为0°；

22、所述墙面的方向角定义为：机器人平行于墙面延申方向时电子罗盘所示的航向角。当机器人识别到墙角时，将对应时刻电子罗盘所示的航向角记为yawt。将雷达提取到的墙面点云信息进行直线拟合，通过直线的斜率来确定墙面为左墙面还是右墙面；

23、当直线的斜率大于0时，该墙面为左墙壁。当识别到的墙角为凹墙角时，利用下式计算该墙壁的方向角，式中βt，k表示对应时刻识别到第k个墙壁的方向角，θt，k表示对应时刻机器人朝向与第k个墙壁形成的夹角，θt，k的范围在0°-90°，当未识别到墙角时将θt，k置为0：

24、βt，k＝yawt+θt，k+180°

25、当识别到的墙角为凸墙角时，利用下式计算该墙壁的方向角：

26、βt，k＝yawt-θt，k+180°

27、当直线的斜率小于0时，该墙面为右墙壁。当识别到的墙角为凸墙角时，利用下式计算该墙壁的方向角，式中βt，k表示对应时刻识别到第k个墙壁的方向角，θt，k表示对应时刻机器人朝向与第k个墙壁形成的夹角，θt，k的范围在0°-90°，当未识别到墙角时将θt，k置为0：

28、βt，k＝yawt-θt，k+180°

29、当识别到的墙角为凹墙角时，利用下式计算该墙壁的方向角：

30、βt，k＝yawt+θt，k+180°

31、每个墙角的方向角定义为构成墙角的两个墙面方向角之和的一半；将第一类墙角的方向角定义为225°，给正负10°的允许误差，则其方向角的检索范围为215°-235°；将第二类墙角的方向角定义为135°，给正负10°的允许误差，则其方向角的检索范围为125°-145°；将第三类墙角的方向角定义为45°，给正负10°的允许误差，则其方向角的检索范围为35°-55°；将第四类墙角的方向角定义为315°，给正负10°的允许误差，则其方向角的检索范围为305°-325°；

32、作为优选，步骤4所述构建墙角族尺寸链，具体如下：

33、根据环境地图大小设定墙角周围物体范围值rs，rs如下式所示计算：

34、

35、其中，ds代表相机的最佳观测视距，θs代表相机的水平视场角，即当物体和墙角的中心距离小于rs时，该物体属于墙角周围物体。

36、以顺时针为正方向沿墙构建墙角族语义尺寸链，其中第一类凹墙角、第二类凹墙角、第三类凹墙角、第四类凹墙角、第一类凸墙角、第二类凸墙角、第三类凸墙角、第四类凸墙角分别用r1、r2、r3、r4、c1、c2、c3、c4表示，门、柜子、椅子、垃圾桶、凳子到墙角的距离分别用d1、d2、d3、d4、d5表示；

37、墙角族尺寸链检索表中除了存储每个墙角各自的类别属性外，还有上游墙角、下游墙角、上下游墙角的距离信息以及墙角周围物体信息；

38、作为优选，步骤5所述全局预定位的流程为：

39、当识别到的墙角数量大于等于墙角数量阈值时，将识别到的墙角按照从左到右的顺序依次排序，循环遍历构建的尺寸链，根据当前帧识别到各墙角的类别属性初步判断是否在尺寸链中存在对应位置，如果初步判断存在对应位置，则计算两墙角在映射图中的距离d，此时计算距离d和检索表对应标准距离d若满足下式，则检索成功，否则继续往后检索，直到检索成功。

40、(1-a)d<d<(1+a)d

41、式中，a通常取经验值0.1。检索成功后，基于各墙角和机器人之间的深度信息d1和d2推算出机器人的位置。

42、当识别到的墙角数量为1时，首先根据该墙角类别属性筛选出墙角族尺寸链中所有符合该类别属性的墙角，然后依次筛选出墙角族尺寸链中周围物体包含相机识别到的各类物体的墙角，最后将上述筛选后的墙角取交集得到最终墙角。

43、当尺寸链墙角筛选完成后，根据机器人坐标系下的各墙角坐标和各墙角在地图坐标系下的坐标来计算机器人位于世界坐标系下的位姿。首先机器人检测出墙角得到相机坐标系下的墙角坐标，其次通过坐标映射得到墙角坐标位于机器人坐标系下的坐标(xr,yr)，世界坐标系下的点表示为(xw,yw)，然后从语义地图中得到该墙角在地图坐标系下的坐标(xg,yg)，根据变换矩阵得到：

44、

45、

46、

47、式中，用随机变量(xt，yt，θt)来描述t时刻的机器人在世界坐标系下的状态，简称位姿，其中(xt,yt)表示在t时刻机器人在世界坐标系下的位置，θt表示机器人的方向，将地图坐标系下可能的墙角坐标点代入方程中就能计算出机器人的可能位姿。

48、本发明具有如下优点：充分利用了室内墙角天然存在且位置固定这一特点，在基于即时定位与建图算法输出栅格地图、定位信息的基础上，基于yolov5的目标检测方法实现墙角凸凹性和物体识别，结合多传感器数据对墙角进行方向性区分，从而构建具有八类墙角和五类物体的语义地图，为机器人定位提供丰富且稳定的信息，对移动机器人领域具有重要的意义。