用于工业机器人的二维码和激光雷达融合定位方法与装置与流程

1.本发明涉及工业机器人定位技术领域，特别是关于一种用于工业机器人的二维码和激光雷达融合定位方法与装置。


背景技术：

2.定位导航精度一直是工业机器人自动化研究的重难点，传统基于磁条或电磁的导航技术限制了机器人的运行路线，其运行路径难以更改、设备难以维护。
3.例如：基于（英文全称为
“”
，中文全称为“超带宽”）的定位方式在小范围内部署容易、精度高，然而成本也更高，且难以完成大规模室内掩盖。
4.还例如：激光（英文全称为“simultaneous localization and mapping”，中文全称为“同时定位与建图”）技术可有效解决路线变更的难题，然而，面对单一、缺少充足轮廓信息的仓库或工厂环境，其定位易出现退化，且长距离定位存在累计误差，单独依靠激光雷达进行定位难以满足复杂多变的场景需求。
5.另外，现有还有使用低成本的视觉二维码进行定位的方法，二维码的空间位置可以通过高精度的测绘来获得，布置精度可以达到亚毫米级，工业机器人在检测到二维码时可获得准确的全局位姿，然而二维码不适合在大型室内场景的每一处都进行布置。


技术实现要素：

6.本发明的目的在于提供一种用于工业机器人的二维码和激光雷达融合定位方法与装置来克服或至少减轻现有技术的上述缺陷中的至少一个。
7.为实现上述目的，本发明提供一种用于工业机器人的二维码和激光雷达融合定位方法，其包括：
8.步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系；以及
9.步骤2，融合变换关系和点云信息，进行全局定位建图，获得机器人位姿；
10.其中，步骤1具体包括：
11.步骤11，获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数；
12.步骤12，将世界坐标转换到坐标系中，得到标记点的坐标；
13.步骤13，获取变换关系：
14.将二维码定位问题转化为第一优化问题（3），其中，表示利用“相机坐标系到世界坐标转换”构建的约束，为最小二乘运算符：
15.（3）
16.根据，表示世界坐标系到坐标系的旋转变换矩阵，表示下式（4）描述的世界坐标系到坐标系的平移变换矩阵；
17.（4）
18.式（4）中，为中心点的世界坐标，为中心点的坐标，表示世界坐标系到坐标系的旋转变换矩阵。
19.进一步地，旋转变换矩阵的获取方法具体包括：
20.先用下式（7）计算和以及标记点相对中心点的世界坐标和像素坐标，再用下式（8）求解：
21.（7）
22.（8）
23.式中，为标记点的坐标，为标记点在相机坐标系下的轴坐标，为地面先验中的投影矩阵，表示标记点的像素坐标，表示取三维向量的、二维数据，
24.为标记点的世界坐标。
25.进一步地，步骤2具体包括：
26.步骤21，检测点云信息中当前帧对应的图像帧的二维码；
27.步骤22，在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿；
28.步骤23，在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。
29.进一步地，步骤23中的关键帧的获取方法包括：
30.步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
31.步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
32.进一步地，步骤23中的“融合变换关系和激光里程计定位信息，获得机器人位姿”的方法具体包括：
33.步骤231c，根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用下式（17）获取激光雷达在世界坐标系下的位姿，再利用构建关键帧约束、关键帧局部点云地图或全局点云地图；
[0034] （17）
[0035]
步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式（18）描述的点云重投影代价函数，获得机器人位姿：
[0036]
ꢀꢀꢀꢀꢀ
（18）
[0037]
其中，为平面特征点的重投影误差，由下式（19）获得，为当前帧平面特征点云中的第个平面特征点，为待优化旋转参数，为待优化平移参数，为在局部点云地图或全局点云地图中的最近点，为所在平面的单位法向量，为边线特征点的重投影误差，由下式（20）获得，为当前帧边线特征点云中的第个边线特征点，为在局部或全局点云地图中的最近点，为在局部或全局点云地图中与不在同一线束上的最近点，为待优化成本函数；
[0038]
（19）
[0039]
ꢀꢀꢀ
（20）
[0040]
式中，表示 范数运算，上标表示转置。
[0041]
本发明还提供一种用于工业机器人的二维码和激光雷达融合定位装置，其包括：
[0042]
二维码信息定位模块，其用于检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系；以及
[0043]
位姿获取模块，其用于融合变换关系和点云信息，进行全局定位建图，获得机器人位姿；
[0044]
其中，二维码信息定位模块具体包括：
[0045]
世界坐标获取单元，其用于获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数；
[0046]
坐标获取单元，其用于将世界坐标转换到坐标系中，得到标记点的
坐标；
[0047]
坐标系变换单元，其用于获取变换关系：
[0048]
将二维码定位问题转化为第一优化问题（3），其中，表示利用“相机坐标系到世界坐标转换”构建的约束，为最小二乘运算符：
[0049]
（3）
[0050]
根据，表示世界坐标系到坐标系的旋转变换矩阵，表示下式（4）描述的世界坐标系到坐标系的平移变换矩阵；
[0051]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（4）
[0052]
式（4）中，为中心点的世界坐标，为中心点的坐标，表示世界坐标系到坐标系的旋转变换矩阵。
[0053]
进一步地，旋转变换矩阵的获取方法具体包括：
[0054]
先用下式（7）计算和以及标记点相对中心点的世界坐标和像素坐标，再用下式（8）求解：
[0055]
（7）
[0056]
（8）
[0057]
式中，为标记点的坐标，为标记点在相机坐标系下的轴坐标，为地面先验中的投影矩阵，表示标记点的像素坐标，表示取三维向量的、二维数据，为标记点的世界坐标。
[0058]
进一步地，位姿获取模块具体包括：
[0059]
二维码检测单元，其用于检测点云信息中当前帧对应的图像帧的二维码；
[0060]
第一位姿获取单元，其用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系，获得机器人位姿；
[0061]
第二位姿获取单元，其用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。
[0062]
进一步地，第二位姿获取单元具体包括：
[0063]
位姿变化量判断子单元，其用于判断当前时刻与上一时刻的位姿变化量是否大于
预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
[0064]
关键帧点云处理子单元，其用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
[0065]
进一步地，位姿获取模块具体还包括信息融合单元；
[0066]
信息融合单元具有：
[0067]
二维码定位子单元，其用于根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用下式（17）获取激光雷达在世界坐标系下的位姿；
[0068]
ꢀꢀ
（17）
[0069]
优化子单元，其用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式（18）描述的点云重投影代价函数，获得机器人位姿：
[0070]
（18）
[0071]
其中，为平面特征点的重投影误差，由下式（19）获得，
[0072]
为当前帧平面特征点云中的第个平面特征点，为待优化旋转参数，为待优化平移参数，为在局部点云地图或全局点云地图中的最近点，为所在平面的单位法向量，为边线特征点的重投影误差，由下式（20）获得，为当前帧边线特征点云中的第个边线特征点，为在局部或全局点云地图中的最近点，为
[0073]
在局部或全局点云地图中与不在同一线束上的最近点，
[0074]
为待优化成本函数；
[0075]
（19）
[0076] （20）
[0077]
式中，表示 范数运算，上标表示转置。
[0078]
由于本发明通过融合地面模型改进了传统二维码定位方法，通过激光里程计结合二维码定位位姿进行位姿校正，因此能够提升二维码定位精度。还由于本发明引入双地图策略，在普通工况和高动态工况下采取不同的定位策略，例如，为了有效避免累计误差在大多数区域采用全局点云地图以及少量二维码进行定位，而在少数动态加工区域，采用局部点云地图以及较密集的二维码进行定位，因此，本发明能够提升系统的鲁棒性。
附图说明
[0079]
图1为本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位的框架图。
[0080]
图2为采用本发明与现有的公开号为cn115014338a的专利技术的定位效果的实验
比对示意图。
具体实施方式
[0081]
下面结合附图和实施例对本发明进行详细的描述。
[0082]
如图1所示，本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位方法包括：
[0083]
步骤1，二维码定位：检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系，即相机位姿。其中，的标定方法为现有技术，在此不再赘述。
[0084]
在一个实施例中，步骤1具体包括：
[0085]
步骤11，二维码检测：获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数。其中，采用基于二进制编码的（英文全称为“augmented reality library from the university of cordoba”，中文全称为“科尔多瓦大学提出的一种增强现实库”）码或（一种视觉基准系统，广泛用于视觉定位和增强现实）码，获取标记点的世界坐标，即标记点的在世界坐标系中的坐标。本步骤方法可以采用新有方法实现，在此不再展开描述。本实施例选取的标记点是二维码的轮廓外围框的四个角点。当然，本领域技术人员可以根据实际应用场景和需求选取其他点作为标记点。
[0086]
步骤12，标记点（英文全称为“inverse perspective mapping”，中文全称为“逆透视变换”）变换：将世界坐标转换到坐标系中，得到标记点的坐标，即标记点的在坐标系中的坐标。
[0087]
例如：将标记点（第个标记点）投影到坐标系，满足公式（1），且标记点的坐标表示为下式（2）：
[0088]
（1）
[0089] （2）
[0090]
式中，为标记点在相机坐标系下的轴坐标，表示标记点的像素坐标，表示取三维向量的、二维数据，为标记点的世界坐标。
[0091]
步骤13，获取变换关系：
[0092]
将二维码定位问题转化为第一优化问题（3），其中，表示利用“相机坐标系到世界坐标转换”构建的约束，为最小二乘运算符：
[0093]
（3）
[0094]
根据，表示世界坐标系到坐标系的旋转变换矩
阵，表示下式（4）描述的世界坐标系到坐标系的平移变换矩阵，为2*1向量；
[0095]
ꢀꢀꢀꢀꢀꢀ
（4）
[0096]
式（4）中，为中心点的世界坐标，为中心点的坐标，表示世界坐标系到坐标系的旋转变换矩阵，为2*2矩阵。
[0097]
在一个实施例中，本实施例的中心点选取但不限于为标记点的世界坐标和坐标的质心，如：用下式（5）计算所有检测到的标记点的世界坐标和坐标的质心，即中心点，再用下式（6）计算所有点相对中心点的坐标：
[0098]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（5）
[0099]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（6）
[0100]
式中，为标记点相对于中心点的世界坐标，为标记点相对于中心点的坐标。
[0101]
在一个实施例中，旋转变换矩阵的获取方法具体包括：
[0102]
先用下式（7），结合中心点的世界和坐标以及标记点相对于中心点的世界和坐标，获得下式（8）：
[0103]
（7）
[0104] （8）优选地，考虑到可以理解为常数项，与无关，且，有：，那么，第一优化问题（3）可以转化为第二优化问题（9）：
[0105]
（9）
[0106]
因为式（9）中的两项都不小于0，且第一项与无关，因此，再用下式（10）求解：
[0107]
（10）
[0108]
式中，为标记点的坐标，为标记点在相机坐标系下的轴坐标，表示标记点的像素坐标，表示取三维向量的、二维数据，为标记点的世界坐标。
[0109]
在一个实施例中，由于式（10）中的第一项旋转矩阵是正交矩阵，第二项的可以消掉，因此均与无关，从而可以将求解的问题可以转化为下式（11）：
[0110]
ꢀꢀ
（11）
[0111]
将式（11）中用无物理意义的中间参数替代，如下式（12）所示：
[0112]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（12）
[0113]
于是，该问题转化为下式（13）：
[0114]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（13）
[0115]
由于二维码的四个标记点不共线，因此该问题必定满秩。此时，对进行奇异值分解，可以得到的最优解为。
[0116]
最后，求解得到的代入式（9）的第二项解得，如上式（4）所示。
[0117]
上述实施例提供的方法根据已知标记点的坐标和世界坐标，来恢复相机位姿，可达到厘米级精度定位。
[0118]
在上述实施例中，在步骤11之前还包括图像预处理：通过灰度化、二值化、滤波等操作对包含有二维码的原始图片进行预处理，减少噪声影响。其中，原始图片可以通过现有摄像机获得。
[0119]
步骤2，融合定位：融合变换关系和点云信息，进行全局定位建图，获得机器人位姿。
[0120]
在一个实施例中，步骤2具体包括：
[0121]
步骤21，判断是否检测到二维码：检测点云信息中当前帧对应的图像帧的二维码，若判定为是，则进入步骤22，反之，则进入步骤23。
[0122]
步骤22，二维码定位：在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿。
[0123]
步骤23，激光里程定位：在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。本实施例通过融合变换关系和激光里程计定位信息，可以减少累计位姿误差，提升定位及局部点云地图构建精度，获得连续、稳定且准确的机器人位姿。
[0124]
在一个实施例中，步骤23中的关键帧的获取方法包括但不限于：
[0125]
步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，例如：位姿平移阈值0.3m或者位姿旋转阈值超过10
°
，若是，则为关键帧，否则为非关键帧。
[0126]
步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，得到更加规整的点云，再存储关键帧位姿及其对应的点云。其中，关键帧位姿表示这一时刻机器人的位姿，其对应的点云指的是关键帧点云。通过点云滤波处理能够获得更加均匀、丰富、准确的点云信息，这样有效提升了点云地图精度。
[0127]
在一个实施例中，步骤23中，本实施例点云采用深度图格式，通过设定空间区域特征点提取数量，以均匀特征分布，并根据六自由度（沿空间三轴平移和三轴旋转）可视性筛选特征点，“提取当前帧点云中的特征点”的方法具体包括：
[0128]
步骤231b，根据激光雷达竖直视场角和水平视场角，选定行分辨率和列分辨率，例如32线旋转式激光雷达，扫描分辨率为0.2
°
，则行数选择为32（对应线束数量），列数选择为1800（360
°
/0.2
°
）。再用下式（14）计算深度图的行数量和列数量。其中，激光雷达竖直视场角指的是点云和激光雷达坐标系原点的第一连线与激光雷达坐标系平面（一般指平行于激光雷达上表面的平面）的夹角，水平视场角指的是第一连线在激光雷达坐标系平面内的投影与轴方向的夹角。
[0129]
ꢀꢀꢀꢀ
（14）
[0130]
步骤232b，根据点云的三维空间位置，计算激光雷达竖直视场角和水平视场角，利用角度信息计算点云在深度图中的行元素的数值和列元素的数值，并将其储存至深度图对应像素点；如下式（15）所示，为激光雷达竖直视场角最小值；为激光雷达水平视场角最小值。
[0131]
（15）
[0132]
步骤233b，将深度图内的点云进行划分，采用式（16）分别计算深度图内所有点云的曲率，根据曲率的数值对深度图内的点云进行排序，按预先设定的曲率阈值，比如：0.1，其具体数值可以在调试中根据实际效果进行修正，初步筛选候选的边线特征点和待选的平面特征点，为计算点云曲率时在该点云所在行的左右两侧所需的点云数量，可以理解为常数，根据实际工况可以选取为2至10之间进行选择。
[0133] （16）
[0134]
式中，为点云的三维坐标，为各相邻点云的三维坐标，
为当前点云在深度图中的列坐标。其中的相邻可以理解为点云左侧的个点云和右侧的个点云。
[0135]
步骤234b，对提取的候选的边线特征点，通过聚类的方式剔除树叶等不稳定的特征点云，再计算步骤233b初步筛选得到的各候选的边线特征点与相邻点的水平角度和垂直角度，若两类角度均符合规定阈值，例如：水平阈值1
°
，垂直阈值3
°
，则将该候选的边线特征点和相邻点聚类为同一点云，剔除无法形成聚类的候选的边线特征点。其中的相邻可以理解为该点上侧、下侧、左侧和右侧各个点。
[0136]
步骤235b，对提取的待选的平面特征点，通过深度图查询其上下两行临近点和左右两列临近点，再利用空间平面方程，对该点及其临近点进行平面拟合，若平面拟合成功则保留该待选的平面特征点及其所在平面单位法向量，若平面拟合失败，则删除该待选的平面特征点。
[0137]
在一个实施例中，步骤23中的“与局部点云地图相匹配，以获得机器人位姿估计信息”的方法具体包括：
[0138]
基于或的点云配准算法，进行点云与局部点云地图之间的配准，得到点云之间的转换矩阵，该转换矩阵可以表示为激光雷达自身的运动变化状态。
[0139]
在一个实施例中，激光后端维护局部、全局两张点云地图。当工业机器人移动到环境易变的区域时（例如大型加工工件附近、堆放流动货物的货架附近等），维护局部点云地图进行定位；在其它区域，则定时更新全局点云地图，先基于全局点云地图进行定位，若定位失败再基于局部点云地图进行定位。
[0140]
步骤23中的“融合变换关系和激光里程计定位信息，获得机器人位姿”的方法具体包括：
[0141]
步骤231c，根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用下式（17）获取激光雷达在世界坐标系下的位姿，再利用构建关键帧约束、关键帧局部点云地图或全局点云地图；
[0142] （17）
[0143]
步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式（18）描述的点云重投影代价函数，获得机器人位姿：
[0144]
（18）
[0145]
其中，为由步骤235b得到的平面特征点的重投影误差，由下式（19）获得，为由步骤235b得到的当前帧平面特征点云中的第个平面特征点，为待优化旋转参数，为待优化平移参数，为在局部点云地图或全局点云地图中的最近点（通过坐标求解地图中每个点到当前点的距离，然后选择最近的那个点），为所在平面的单位法向量，
为步骤234b得到的边线特征点的重投影误差，由下式（20）获得，为步骤234b获得的当前帧边线特征点云中的第个边线特征点，为在局部或全局点云地图中的最近点，为在局部或全局点云地图中与不在同一线束上的最近点，为待优化成本函数；
[0146]
ꢀꢀ
（19）
[0147]
（20）
[0148]
式中，表示 范数运算，上标表示转置。
[0149]
通过上述实施例构建的位姿图优化代价函数，迭代优化以减少前端累计位姿估计误差，提高定位精度。
[0150]
本发明提供的方法将二维码定位和激光雷达定位进行融合，既避免了特殊场景下的激光里程计退化现象，也一定程度上降低了传统定位方式（超带宽、电磁、磁条、大面积部署视觉二维码）的施工成本；另一方面，二维码定位时结合地面模型提升其定位精度，大大提升了系统的定位精度；此外，针对工业机器人工作环境的特殊性，维护全局、局部两张点云地图，在不同的工作区使用不同的地图进行定位，以保证系统的鲁棒性。
[0151]
本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位装置包括二维码信息定位模块和位姿获取模块，其中：
[0152]
二维码信息定位模块用于检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系。
[0153]
位姿获取模块用于融合变换关系和点云信息，进行全局定位建图，获得机器人位姿。
[0154]
其中，二维码信息定位模块具体包括世界坐标获取单元、坐标获取单元和坐标系变换单元，其中：世界坐标获取单元用于获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数。
[0155]
坐标获取单元，其用于将世界坐标转换到坐标系中，得到标记点的坐标。
[0156]
坐标系变换单元用于获取变换关系：将二维码定位问题转化为第一优化问题（3），根据，表示世界坐标系到坐标系的旋转变换矩阵，表示式（4）描述的世界坐标系到坐标系的平移变换矩阵。
[0157]
在一个实施例中，位姿获取模块具体包括二维码检测单元、第一位姿获取单元和第二位姿获取单元，其中：
[0158]
二维码检测单元用于检测点云信息中当前帧对应的图像帧的二维码。
[0159]
第一位姿获取单元用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系，获得机器人位姿。
[0160]
第二位姿获取单元用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。
[0161]
在一个实施例中，第二位姿获取单元具体包括位姿变化量判断子单元和关键帧点云处理子单元，其中：
[0162]
位姿变化量判断子单元用于判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧。
[0163]
关键帧点云处理子单元用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
[0164]
在一个实施例中，位姿获取模块具体还包括信息融合单元；
[0165]
信息融合单元具有二维码定位子单元和优化子单元，其中：
[0166]
二维码定位子单元用于根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用式（17）获取激光雷达在世界坐标系下的位姿，再利用构建关键帧约束、关键帧局部点云地图或全局点云地图。
[0167]
优化子单元用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建式（18）描述的点云重投影代价函数，获得机器人位姿。
[0168]
图2和下表1为采用本发明（方法1）与现有的公开号为cn115014338a的专利技术（方法2）的定位效果进行了实验比对，设置不同的参数（相机到地面的距离、相机轴线和地面法线的夹角、二维码的尺寸），将相机向前移动固定距离，通过对比定位结果中、方向运动测量误差和横摆角测量误差，来反映两种方法的定位效果。其中，每一个测试都采集了100组数据，并取平均值进行对比。对比如下：
[0169][0170]
可以看到，本发明的二维码定位精度总体提升了5倍以上。
[0171]
而且，公开号为cn115014338a的专利技术（方法2）在仅维护局部点云地图的情况下进行定位，需要二维码张贴的更加密集，否则精度较低；在仅维护全局点云地图的情况下进行定位，常出现激光定位丢失的情况。经过系统稳定性测试发现：在加工车间等工况下，本发明（方法1）所提出的双地图维护方案相较于其它方案具有非常明显的鲁棒性。
[0172]
基于本方案进行室内定位效果测试，结果如图2所示（测试场地约50m，共布置6个二维码，传统定位方法的定位误差约为0.5％，在本测试场地的误差约25cm；图中的横坐标表示测试的序号，纵坐标表示定位误差，单位为，可以发现定位误差基本在2cm以内）：由此证实本发明大大提升了传统定位方法的精度及鲁棒性。
[0173]
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。