融合激光定位的远距二维码地图创建及定位方法、系统与流程

本发明涉及室内空间建图定位技术，尤其涉及一种融合激光定位的远距二维码地图创建及定位方法、系统。

背景技术：

1、目前主流的移动机器人建图定位方案，通常采用激光建图技术，如机器人使用当前激光点云与点云地图匹配实现激光定位。然而实际实施过程中，由于存在场景变化因素，例如，在堆垛，解垛，窄通道，动态场景下等，容易受到环境及堆放物、移动物体的遮挡及干扰影响，阻挡激光雷达的扫描，加上机器人运动机构时常还存在打滑现象，从而使得采用纯激光定位在长时间的实际运行部署中存在一定的局限。

2、为此，发明人考虑到远距二维码定位的特殊性，可以将远距二维码设置在室内天花板上这种无遮挡、开阔的环境中，此时相机在拍摄远距二维码的过程中，将不易受到地面环境的影响，同时若采用人工标记方式，在场景中容易发生定位丢失的区域内设置远距二维码进行辅助定位，则可减少或避免前述此类问题的发生，以在该些场景下获得定位优势。

技术实现思路

1、为此，本发明的主要目的在于提供一种融合激光定位的远距二维码地图创建及定位方法、系统，以实现背景技术中提出的方案，获得在该些场景下的定位优势。

2、为了实现上述目的，根据本发明的第一个方面，提供了一种融合激光定位的远距二维码地图创建方法，步骤包括：

3、步骤s100将远距二维码设置在建图区域顶部相对于相机的仰望位；建立建图区域的激光地图，并获取激光定位信息；

4、步骤s200经相机获取含远距二维码的图像，以提取远距二维码在相机坐标系下的空间位姿；

5、步骤s300当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中。

6、在可能的优选实施方式中，步骤s300中，计算首次观测到的远距二维码在激光地图中的空间位置的步骤包括：

7、步骤s310通过因子图形式融合所有时刻的激光定位信息及远距二维码在相机坐标系下的空间位姿，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置。

8、在可能的优选实施方式中，步骤s310中，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置的步骤包括：

9、步骤s311设所有远距二维码在激光地图中的位置为，使用测量值约束待优化变量，使用贝叶斯网络建模为，通过最大后验概率求解优化后的状态变量：

10、

11、其中，测量值为一组测量值的集合：

12、表示处于移动机器人位于第个位置时相机对的观测；

13、表示处于移动机器人在激光地图中的第个位置；

14、 表示相机在车体坐标系下的安装位置。

15、在可能的优选实施方式中，所述远距二维码，包括：定位码，信息码，其中定位码由数个方向点围绕中心点在数个非交叉方向布设而成，并在各个相邻方向点的夹角处划分出数个象限区域，所述信息码根据预设码表布设在对应象限区域中，以与定位码组成二维点阵，其中定位码与信息码表面设有反光层。

16、在可能的优选实施方式中，步骤s200中，提取远距二维码在相机坐标系下的空间位姿的步骤包括：

17、步骤s210获取包含远距二维码信息的图像做二值化处理后，进行边缘提取；

18、步骤s220根据提取的边缘信息进行椭圆拟合检测，获取原始 码及其圆心坐标；

19、步骤s230根据定位码的几何关系，定位出原始码中对应各远距二维码的定位码分布位置，以筛选出原始码中的信息码，并根据通码表来获取远距二维码的id信息；

20、步骤s240当判断该id下的远距二维码首次出现时，根据定位码在图像中的圆心坐标，计算对应远距二维码在相机坐标系下的空间位姿并与该id信息绑定。

21、在可能的优选实施方式中，步骤s240中计算远距二维码在相机坐标系下空间位姿的步骤包括：

22、步骤s241设定位码的圆心坐标为，建立齐次矩阵

23、

24、基于svd法求解单应性矩阵h，其中u，v表示定位码圆心的像素坐标、表示远距二维码坐标系下点的坐标，s为等效距离比例因子；

25、步骤s242根据单应性矩阵与远距二维码在相机坐标系下转换矩阵的关系

26、

27、求得旋转和平移矩阵，其中p为相机投影矩阵、e为截断外参矩阵，, 分别为相机方向和方向的焦距，，为相机中心点坐标，为旋转矩阵中的前两列，,, 分别表示在相机坐标系下远距二维码中心的位置，为3×3的单应投影矩阵。

28、在可能的优选实施方式中，步骤s240中还包括：

29、步骤243通过迭代优化方法求解最小化误差函数

30、

31、以优化旋转和平移矩阵，其中r表示旋转，t表示平移

32、

33、

34、表示平移向量中的第i个元素，表示旋转矩阵中的第i列；其中分别表示图像中第个远距二维码的圆心投影到相机归一化坐标系中的坐标；表示所有定位码的的圆心坐标；分别表示由远距二维码中第个定位码的圆心空间坐标，按照旋转平移矩阵转换到相机归一化平面中的点的坐标；表示在远距二维码坐标系中每一个定位码的圆心空间坐标；

35、其中在求解过程中以

36、

37、作为约束条件。

38、在可能的优选实施方式中，步骤s200中，提取远距二维码在相机坐标系下的空间位姿的步骤还包括：

39、步骤s250 按照时间过滤和先验位姿约束，对经步骤s240获得的空间位姿进行约束，其中时间过滤步骤包括：当判断空间位姿结果存在突变时，滤除；先验位姿约束步骤包括：当判断空间位姿结果中远距二维码坐标系和相机坐标系处于非平行时，滤除。

40、为了实现上述目的，对应上述地图创建方法，本发明第二个方面，还提供了一种融合激光定位的远距二维码地图定位方法，其步骤包括：

41、步骤s400当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度，通过各自位置的定位置信度大小进行加权融合获取定位信息：

42、

43、

44、其中为当的位置，表示当前混合定位的置信度，为上一时刻定位位置，为当前时刻使用相机定位的定位位置，为当前时刻使用激光定位的位置；

45、步骤s500当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的id信息，以与地图进行匹配获取定位信息。

46、为了实现上述目的，对应上述地图创建方法及定位方法，本发明第三个方面，还提供了一种融合激光定位的远距二维码地图创建及定位系统，其包括：

47、存储单元，用于存储包括如上任一所述基于远距二维码的地图创建方法步骤的程序及上述融合激光定位的远距二维码地图定位方法步骤的程序，以供控制单元，处理单元，激光定位单元适时调取执行；

48、控制单元，用于控制激光雷达及相机进行时空同步，及控制红外相机拍摄包含远距二维码的待处理图像，控制激光雷达进行建图扫描；

49、激光定位单元，用于根据激光雷达的建图扫描数据，建立建图区域的激光地图，并获取激光定位信息；

50、处理单元，用于以提取远距二维码在相机坐标系下的空间位姿，当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中；当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度，通过各自位置的定位置信度大小进行加权融合获取定位信息；当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的id信息，以与地图进行匹配获取定位信息。

51、通过本发明提供的该融合激光定位的远距二维码地图创建及定位方法、系统，巧妙的利用了远距二维码的建图和定位优势及特性，以对激光建图和定位的短板进行了补充融合，从而解决了传统单一激光建图定位容易发生定位丢失的问题，以在更多场景下获得定位优势。