CN115774265B - 用于工业机器人的二维码和激光雷达融合定位方法与装置 - Google Patents
用于工业机器人的二维码和激光雷达融合定位方法与装置 Download PDFInfo
- Publication number
- CN115774265B CN115774265B CN202310113218.7A CN202310113218A CN115774265B CN 115774265 B CN115774265 B CN 115774265B CN 202310113218 A CN202310113218 A CN 202310113218A CN 115774265 B CN115774265 B CN 115774265B
- Authority
- CN
- China
- Prior art keywords
- ipm
- point cloud
- point
- coordinate system
- dimensional code
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 230000004927 fusion Effects 0.000 title claims abstract description 29
- 230000009466 transformation Effects 0.000 claims abstract description 68
- 238000013507 mapping Methods 0.000 claims abstract description 11
- 238000006243 chemical reaction Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 38
- 238000013519 translation Methods 0.000 claims description 14
- 239000013598 vector Substances 0.000 claims description 13
- 238000001914 filtration Methods 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 8
- 125000003275 alpha amino acid group Chemical group 0.000 claims description 5
- 150000001875 compounds Chemical class 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000000694 effects Effects 0.000 description 7
- 230000004807 localization Effects 0.000 description 5
- 238000012360 testing method Methods 0.000 description 5
- 230000003190 augmentative effect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000013112 stability test Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种用于工业机器人的二维码和激光雷达融合定位方法与装置，其中方法包括：步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系
和
坐标系
之间的转换关系
，获得世界坐标系到
坐标系的变换关系
；步骤2，融合变换关系
和点云信息，进行全局定位建图，获得机器人位姿
。本发明能够提升系统的鲁棒性。
Description
技术领域
本发明涉及工业机器人定位技术领域，特别是关于一种用于工业机器人的二维码和激光雷达融合定位方法与装置。
背景技术
定位导航精度一直是工业机器人自动化研究的重难点，传统基于磁条或电磁的导航技术限制了机器人的运行路线，其运行路径难以更改、设备难以维护。
例如：基于UWB(英文全称为“Ultra Wide Band”，中文全称为“超带宽”)的定位方式在小范围内部署容易、精度高，然而成本也更高，且难以完成大规模室内掩盖。
还例如：激光SLAM(英文全称为“Simultaneous Localization and Mapping”，中文全称为“同时定位与建图”)技术可有效解决路线变更的难题，然而，面对单一、缺少充足轮廓信息的仓库或工厂环境，其定位易出现退化，且长距离定位存在累计误差，单独依靠激光雷达进行定位难以满足复杂多变的场景需求。
另外，现有还有使用低成本的视觉二维码进行定位的方法，二维码的空间位置可以通过高精度的测绘来获得，布置精度可以达到亚毫米级，工业机器人在检测到二维码时可获得准确的全局位姿，然而二维码不适合在大型室内场景的每一处都进行布置。
发明内容
本发明的目的在于提供一种用于工业机器人的二维码和激光雷达融合定位方法与装置来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种用于工业机器人的二维码和激光雷达融合定位方法，其包括：
步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，获得世界坐标系到IPM坐标系的变换关系TIPM；以及
步骤2，融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T；
其中，步骤1具体包括：
步骤11，获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数；
步骤12，将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i；
步骤13，获取变换关系TIPM：
将二维码定位问题转化为第一优化问题(3)，其中，J表示利用相机IPM坐标系到世界坐标转换构建的约束，‖‖2为最小二乘运算符：
根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵；
式(4)中，
为中心点的世界坐标，
为中心点的IPM坐标，RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵。
进一步地，旋转变换矩阵RIPM的获取方法具体包括：
先用下式(7)计算
和
以及标记点i相对中心点的世界坐标
和像素坐标
再用下式(8)求解RIPM：
式中，pI i为标记点i的IPM坐标，zC i为标记点i在相机坐标系下的z轴坐标，MIPM为地面先验中的IPM投影矩阵，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
进一步地，步骤2具体包括：
步骤21，检测点云信息中当前帧对应的图像帧的二维码；
步骤22，在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿T；
步骤23，在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。
进一步地，步骤23中的关键帧的获取方法包括：
步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
进一步地，步骤23中的“融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T”的方法具体包括：
步骤231c，根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用下式(17)获取激光雷达在世界坐标系下的位姿TL ′，再利用TL ′构建关键帧约束、关键帧局部点云地图或全局点云地图；
步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式(18)描述的点云重投影代价函数，获得机器人位姿T：
其中，
为平面特征点的重投影误差，由下式(19)获得，pi为当前帧平面特征点云中的第i个平面特征点，R为待优化旋转参数，t为待优化平移参数，
为pi在局部点云地图或全局点云地图中的最近点，ni为
所在平面的单位法向量，
为边线特征点的重投影误差，由下式(20)获得，pj为当前帧边线特征点云中的第j个边线特征点，
为pj在局部或全局点云地图中的最近点，
为pj在局部或全局点云地图中与
不在同一线束上的最近点，f(R,t)为待优化成本函数；
式中，‖‖2表示L2范数运算，上标T表示转置。
本发明还提供一种用于工业机器人的二维码和激光雷达融合定位装置，其包括：
二维码信息定位模块，其用于检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，获得世界坐标系到IPM坐标系的变换关系TIPM；以及
位姿获取模块，其用于融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T；
其中，二维码信息定位模块具体包括：
世界坐标获取单元，其用于获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数；
IPM坐标获取单元，其用于将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i；
坐标系变换单元，其用于获取变换关系TIPM：
将二维码定位问题转化为第一优化问题(3)，其中，J表示利用相机IPM坐标系到世界坐标转换构建的约束，‖‖2为最小二乘运算符：
根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵；
式(4)中，
为中心点的世界坐标，
为中心点的IPM坐标，RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵。
进一步地，旋转变换矩阵RIPM的获取方法具体包括：
先用下式(7)计算
和
以及标记点i相对中心点的世界坐标
和像素坐标
再用下式(8)求解RIPM：
式中，pI i为标记点i的IPM坐标，zC i为标记点i在相机坐标系下的z轴坐标，MIPM为地面先验中的IPM投影矩阵，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
进一步地，位姿获取模块具体包括：
二维码检测单元，其用于检测点云信息中当前帧对应的图像帧的二维码；
第一位姿获取单元，其用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系TIPM，获得机器人位姿T；
第二位姿获取单元，其用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。
进一步地，第二位姿获取单元具体包括：
位姿变化量判断子单元，其用于判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
关键帧点云处理子单元，其用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
进一步地，位姿获取模块具体还包括信息融合单元；
信息融合单元具有：
二维码定位子单元，其用于根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用下式(17)获取激光雷达在世界坐标系下的位姿TL ′；
优化子单元，其用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式(18)描述的点云重投影代价函数，获得机器人位姿T：
其中，
为平面特征点的重投影误差，由下式(19)获得，pi为当前帧平面特征点云中的第i个平面特征点，R为待优化旋转参数，t为待优化平移参数，
为pi在局部点云地图或全局点云地图中的最近点，ni为
所在平面的单位法向量，
为边线特征点的重投影误差，由下式(20)获得，pj为当前帧边线特征点云中的第j个边线特征点，
为pj在局部或全局点云地图中的最近点，
为pj在局部或全局点云地图中与
不在同一线束上的最近点，f(R,t)为待优化成本函数；
式中，‖‖2表示L2范数运算，上标T表示转置。
由于本发明通过融合地面模型改进了传统二维码定位方法，通过激光里程计结合二维码定位位姿进行位姿校正，因此能够提升二维码定位精度。还由于本发明引入双地图策略，在普通工况和高动态工况下采取不同的定位策略，例如，为了有效避免累计误差在大多数区域采用全局点云地图以及少量二维码进行定位，而在少数动态加工区域，采用局部点云地图以及较密集的二维码进行定位，因此，本发明能够提升系统的鲁棒性。
附图说明
图1为本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位的框架图。
图2为采用本发明的定位效果的实验说明示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
如图1所示，本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位方法包括：
步骤1，二维码定位：检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，获得世界坐标系到IPM坐标系的变换关系TIPM，即相机位姿。其中，MIPM的标定方法为现有技术，在此不再赘述。
在一个实施例中，步骤1具体包括：
步骤11，二维码检测：获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数。其中，采用基于二进制编码的Aruco(英文全称为“Augmented Reality Library from the University of Cordoba”，中文全称为“科尔多瓦大学提出的一种增强现实库”)码或Apriltag(一种视觉基准系统，广泛用于视觉定位和增强现实)码，获取标记点i的世界坐标pw i，即标记点i的在世界坐标系中的坐标(xw i yw izw i)。本步骤方法可以采用现有方法实现，在此不再展开描述。本实施例选取的标记点是二维码的轮廓外围框的四个角点。当然，本领域技术人员可以根据实际应用场景和需求选取其他点作为标记点。
步骤12，标记点IPM(英文全称为“Inverse Perspective Mapping”，中文全称为“逆透视变换”)变换：将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i，即标记点i的在IPM坐标系中的坐标(xI i yI i zI i)。
例如：将标记点i(第i个标记点)投影到IPM坐标系，满足公式(1)，且标记点i的IPM坐标表示为下式(2)：
(zC i)-1[xI i yI i 1]T～MIPM -1[ui vi 1]T(1)
[xI i yI i 1]T＝TIPM -1[xw i yw i 1]T (2)
式中，zC i为标记点i在相机坐标系下的z轴坐标，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
步骤13，获取变换关系TIPM：
将二维码定位问题转化为第一优化问题(3)，其中，J表示利用“相机IPM坐标系到世界坐标转换”构建的约束，‖‖2为最小二乘运算符：
根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵，为2*1向量；
式(4)中，
为中心点的世界坐标，
为中心点的IPM坐标，RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，为2*2矩阵。
在一个实施例中，本实施例的中心点选取但不限于为标记点i的世界坐标和IPM坐标的质心，如：用下式(5)计算所有检测到的标记点的世界坐标和IPM坐标的质心，即中心点，再用下式(6)计算所有点相对中心点的坐标：
式中，
为标记点i相对于中心点的世界坐标，
为标记点i相对于中心点的IPM坐标。
在一个实施例中，旋转变换矩阵RIPM的获取方法具体包括：
先用下式(7)，结合中心点的世界和IPM坐标以及标记点i相对于中心点的世界和IPM坐标，获得下式(8)：
优选地，考虑到
可以理解为常数项，与i无关，且
有：
那么，第一优化问题(3)可以转化为第二优化问题(9)：
因为式(9)中的两项都不小于0，且第一项与tIPM无关，因此，再用下式(10)求解RIPM：
式中，pI i为标记点i的IPM坐标，zC i为标记点i在相机坐标系下的z轴坐标，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
在一个实施例中，由于式(10)中的第一项旋转矩阵是正交矩阵，第二项的R可以消掉，因此均与RIPM无关，从而可以将求解RIPM的问题可以转化为下式(11)：
将式(11)中
用无物理意义的中间参数替代，如下式(12)所示：
于是，该问题转化为下式(13)：
由于二维码的四个标记点不共线，因此该问题必定满秩。此时，对H进行奇异值分解H＝UΣVτ，可以得到RIPM的最优解为VUτ。
最后，求解得到的RIPM代入式(9)的第二项解得tIPM，如上式(4)所示。
上述实施例提供的方法根据已知标记点的IPM坐标和世界坐标，来恢复相机位姿，可达到厘米级精度定位。
在上述实施例中，在步骤11之前还包括图像预处理：通过灰度化、二值化、滤波等操作对包含有二维码的原始图片进行预处理，减少噪声影响。其中，原始图片可以通过现有摄像机获得。
步骤2，融合定位：融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T。
在一个实施例中，步骤2具体包括：
步骤21，判断是否检测到二维码：检测点云信息中当前帧对应的图像帧的二维码，若判定为是，则进入步骤22，反之，则进入步骤23。
步骤22，二维码定位：在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿T。
步骤23，激光里程定位：在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。本实施例通过融合变换关系TIPM和激光里程计定位信息，可以减少累计位姿误差，提升定位及局部点云地图构建精度，获得连续、稳定且准确的机器人位姿。
在一个实施例中，步骤23中的关键帧的获取方法包括但不限于：
步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，例如：位姿平移阈值0.3m或者位姿旋转阈值超过10°，若是，则为关键帧，否则为非关键帧。
步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，得到更加规整的点云，再存储关键帧位姿及其对应的点云。其中，关键帧位姿表示这一时刻机器人的位姿，其对应的点云指的是关键帧点云。通过点云滤波处理能够获得更加均匀、丰富、准确的点云信息，这样有效提升了点云地图精度。
在一个实施例中，步骤23中，本实施例点云采用深度图格式，通过设定空间区域特征点提取数量，以均匀特征分布，并根据六自由度(沿空间三轴平移和三轴旋转)可视性筛选特征点，“提取当前帧点云中的特征点”的方法具体包括：
步骤231b，根据激光雷达竖直视场角Dver和水平视场角Dhor，选定行分辨率dr和列分辨率dc，例如32线旋转式激光雷达，扫描分辨率为0.2°，则行数选择为32(对应线束数量)，列数选择为1800(360°/0.2°)。再用下式(14)计算深度图的行数量nr和列数量nc。其中，激光雷达竖直视场角Dver指的是点云pi和激光雷达坐标系原点的第一连线与激光雷达坐标系XOY平面(一般指平行于激光雷达上表面的平面)的夹角，水平视场角Dhor指的是第一连线在激光雷达坐标系XOY平面内的投影与X轴方向的夹角。
步骤232b，根据点云pi的三维空间位置(x,y,z)，计算激光雷达竖直视场角Dver和水平视场角Dhor，利用角度信息计算点云pi在深度图中的行元素的数值r和列元素的数值c，并将其储存至深度图对应像素点(r,c)；如下式(15)所示，Vu为激光雷达竖直视场角最小值；Hr为激光雷达水平视场角最小值。
步骤233b，将深度图内的点云进行划分，采用式(16)分别计算深度图内所有点云的曲率pcur，根据曲率pcur的数值对深度图内的点云进行排序，按预先设定的曲率阈值，比如：0.1，其具体数值可以在调试中根据实际效果进行修正，初步筛选候选的边线特征点和待选的平面特征点，m为计算点云曲率时在该点云所在行的左右两侧所需的点云数量，m可以理解为常数，根据实际工况可以选取为2至10之间进行选择。
式中，(xc,yc,zc)为点云pi的三维坐标，(xi,yi,zi)为各相邻点云的三维坐标，c为当前点云pi在深度图中的列坐标。其中的相邻可以理解为点云左侧的m个点云和右侧的m个点云。
步骤234b，对提取的候选的边线特征点
通过聚类的方式剔除树叶等不稳定的特征点云，再计算步骤233b初步筛选得到的各候选的边线特征点与相邻点的水平角度和垂直角度，若两类角度均符合规定阈值，例如：水平阈值1°，垂直阈值3°，则将该候选的边线特征点和相邻点聚类为同一点云，剔除无法形成聚类的候选的边线特征点。其中的相邻可以理解为该点上侧、下侧、左侧和右侧各m个点。
步骤235b，对提取的待选的平面特征点
通过深度图查询其上下两行临近点和左右两列临近点，再利用空间平面方程，对该点及其临近点进行平面拟合，若平面拟合成功则保留该待选的平面特征点及其所在平面单位法向量，若平面拟合失败，则删除该待选的平面特征点。
在一个实施例中，步骤23中的“与局部点云地图相匹配，以获得机器人位姿估计信息”的方法具体包括：
基于ICP或NDT的点云配准算法，进行点云与局部点云地图之间的配准，得到点云之间的转换矩阵，该转换矩阵可以表示为激光雷达自身的运动变化状态。
在一个实施例中，激光SLAM后端维护局部、全局两张点云地图。当工业机器人移动到环境易变的区域时(例如大型加工工件附近、堆放流动货物的货架附近等)，维护局部点云地图进行定位；在其它区域，则定时更新全局点云地图，先基于全局点云地图进行定位，若定位失败再基于局部点云地图进行定位。
步骤23中的“融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T”的方法具体包括：
步骤231c，根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用下式(17)获取激光雷达在世界坐标系下的位姿TL ′，再利用TL ′构建关键帧约束、关键帧局部点云地图或全局点云地图；
步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式(18)描述的点云重投影代价函数，获得机器人位姿T：
其中，
为由步骤235b得到的平面特征点的重投影误差，由下式(19)获得，pi为由步骤235b得到的当前帧平面特征点云中的第i个平面特征点，R为待优化旋转参数，t为待优化平移参数，
为pi在局部点云地图或全局点云地图中的最近点(通过坐标求解地图中每个点到当前点的距离，然后选择最近的那个点)，ni为
所在平面的单位法向量，
为步骤234b得到的边线特征点的重投影误差，由下式(20)获得，pj为步骤234b获得的当前帧边线特征点云中的第j个边线特征点，
为pj在局部或全局点云地图中的最近点，
为pj在局部或全局点云地图中与
不在同一线束上的最近点，f(R,t)为待优化成本函数；
式中，‖‖2表示L2范数运算，上标T表示转置。
通过上述实施例构建的位姿图优化代价函数，迭代优化以减少前端累计位姿估计误差，提高定位精度。
本发明提供的方法将二维码定位和激光雷达定位进行融合，既避免了特殊场景下的激光里程计退化现象，也一定程度上降低了传统定位方式(超带宽、电磁、磁条、大面积部署视觉二维码)的施工成本；另一方面，二维码定位时结合地面模型提升其定位精度，大大提升了系统的定位精度；此外，针对工业机器人工作环境的特殊性，维护全局、局部两张点云地图，在不同的工作区使用不同的地图进行定位，以保证系统的鲁棒性。
本发明实施例提供的用于工业机器人的二维码和激光雷达融合定位装置包括二维码信息定位模块和位姿获取模块，其中：
二维码信息定位模块用于检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，获得世界坐标系到IPM坐标系的变换关系TIPM。
位姿获取模块用于融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T。
其中，二维码信息定位模块具体包括世界坐标获取单元、IPM坐标获取单元和坐标系变换单元，其中：
世界坐标获取单元用于获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数。
IPM坐标获取单元，其用于将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i。
坐标系变换单元用于获取变换关系TIPM：将二维码定位问题转化为第一优化问题(3)，根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵。
在一个实施例中，位姿获取模块具体包括二维码检测单元、第一位姿获取单元和第二位姿获取单元，其中：
二维码检测单元用于检测点云信息中当前帧对应的图像帧的二维码。
第一位姿获取单元用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系TIPM，获得机器人位姿T。
第二位姿获取单元用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。
在一个实施例中，第二位姿获取单元具体包括位姿变化量判断子单元和关键帧点云处理子单元，其中：
位姿变化量判断子单元用于判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧。
关键帧点云处理子单元用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
在一个实施例中，位姿获取模块具体还包括信息融合单元；
信息融合单元具有二维码定位子单元和优化子单元，其中：
二维码定位子单元用于根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用式(17)获取激光雷达在世界坐标系下的位姿TL ′，再利用TL ′构建关键帧约束、关键帧局部点云地图或全局点云地图。
优化子单元用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建式(18)描述的点云重投影代价函数，获得机器人位姿T。
图2为采用本发明的定位效果的实验说明示意图，下表1为采用本发明(方法1)与现有的公开号为CN115014338A的专利技术(方法2)的定位效果进行了实验比对，设置不同的参数(相机到地面的距离d、相机轴线和地面法线的夹角θ、二维码的尺寸s)，将相机向前移动固定距离，通过对比定位结果中x、y方向运动测量误差和横摆角测量误差，来反映两种方法的定位效果。其中，每一个测试都采集了100组数据，并取平均值进行对比。对比如下：
表1
可以看到，本发明的二维码定位精度总体提升了5倍以上。
而且，公开号为CN115014338A的专利技术(方法2)在仅维护局部点云地图的情况下进行定位，需要二维码张贴的更加密集，否则精度较低；在仅维护全局点云地图的情况下进行定位，常出现激光定位丢失的情况。经过系统稳定性测试发现：在加工车间等工况下，本发明(方法1)所提出的双地图维护方案相较于其它方案具有非常明显的鲁棒性。
基于本方案进行室内定位效果测试，结果如图2所示(测试场地约50m，共布置6个二维码，传统定位方法的定位误差约为0.5％，在本测试场地的误差约25cm；图中的横坐标表示测试的序号，纵坐标表示定位误差，单位为m，可以发现定位误差基本在2cm以内)：由此证实本发明大大提升了传统定位方法的精度及鲁棒性。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (8)
1.一种用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，包括：
步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，MIPM为地面先验中的IPM投影矩阵，获得世界坐标系到IPM坐标系的变换关系TIPM；以及
步骤2，融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T；
其中，步骤1具体包括：
步骤11，获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数；
步骤12，将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i；
步骤13，获取变换关系TIPM：
将二维码定位问题转化为第一优化问题(3)，其中，J表示利用相机IPM坐标系到世界坐标转换构建的约束，‖‖2为最小二乘运算符：
根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵；
式(4)中，
为中心点的世界坐标，
为中心点的IPM坐标，
通过式(5)计算得到，RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵；
其中：旋转变换矩阵RIPM的获取方法具体包括：
先用下式(6)和(7)计算
和
以及标记点i相对中心点的世界坐标
和像素坐标
再用下式(8)求解RIPM：
式中，
为标记点i相对于中心点的世界坐标，
为标记点i相对于中心点的IPM坐标，zC i为标记点i在相机坐标系下的z轴坐标，MIPM为地面先验中的IPM投影矩阵，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
2.如权利要求1所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤2具体包括：
步骤21，检测点云信息中当前帧对应的图像帧的二维码；
步骤22，在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿T；
步骤23，在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。
3.如权利要求2所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤23中的关键帧的获取方法包括：
步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
4.如权利要求2所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤23中的“融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T”的方法具体包括：
步骤231c，根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用下式(17)获取激光雷达在世界坐标系下的位姿TL ′，再利用TL ′构建关键帧约束、关键帧局部点云地图或全局点云地图；
步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式(18)描述的点云重投影代价函数，获得机器人位姿T：
其中，
为平面特征点的重投影误差，由下式(19)获得，pi为当前帧平面特征点云中的第i个平面特征点，R为待优化旋转参数，t为待优化平移参数，
为oi在局部点云地图或全局点云地图中的最近点，ni为
所在平面的单位法向量，
为边线特征点的重投影误差，由下式(20)获得，oj为当前帧边线特征点云中的第j个边线特征点，
为oj在局部或全局点云地图中的最近点，
为pj在局部或全局点云地图中与
不在同一线束上的最近点，f(R,t)为待优化成本函数；
式中，‖‖2表示L2范数运算，上标T表示转置。
5.一种用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，包括：
二维码信息定位模块，其用于检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系MIPM，MIPM为地面先验中的IPM投影矩阵，获得世界坐标系到IPM坐标系的变换关系TIPM；以及
位姿获取模块，其用于融合变换关系TIPM和点云信息，进行全局定位建图，获得机器人位姿T；
其中，二维码信息定位模块具体包括：
世界坐标获取单元，其用于获取二维码信息中二维码的标记点i的世界坐标pw i，n为在图像数据上检测到的标记点i的总数；
IPM坐标获取单元，其用于将世界坐标pw i转换到IPM坐标系中，得到标记点i的IPM坐标pI i；
坐标系变换单元，其用于获取变换关系TIPM：
将二维码定位问题转化为第一优化问题(3)，其中，J表示利用相机IPM坐标系到世界坐标转换构建的约束，‖‖2为最小二乘运算符：
根据
RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵，tIPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵；
式(4)中，
为中心点的世界坐标，
为中心点的IPM坐标，
通过式(5)计算得到，RIPM表示世界坐标系到IPM坐标系的旋转变换矩阵；
其中：旋转变换矩阵RIPM的获取方法具体包括：
先用下式(6)和(7)计算
和
以及标记点i相对中心点的世界坐标
和像素坐标
再用下式(8)求解RIPM：
式中，
为标记点i相对于中心点的世界坐标，
为标记点i相对于中心点的IPM坐标，zC i为标记点i在相机坐标系下的z轴坐标，MIPM为地面先验中的IPM投影矩阵，(ui，vi)表示标记点i的像素坐标，<>xy表示取三维向量的x、y二维数据，pw i为标记点i的世界坐标(xw i，yw i)。
6.如权利要求5所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，位姿获取模块具体包括：
二维码检测单元，其用于检测点云信息中当前帧对应的图像帧的二维码；
第一位姿获取单元，其用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系TIPM，获得机器人位姿T；
第二位姿获取单元，其用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系TIPM和激光里程计定位信息，获得机器人位姿T。
7.如权利要求6所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，第二位姿获取单元具体包括：
位姿变化量判断子单元，其用于判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；
关键帧点云处理子单元，其用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。
8.如权利要求6所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，位姿获取模块具体还包括信息融合单元；
信息融合单元具有：
二维码定位子单元，其用于根据变换关系TIPM，结合预先标定的IPM坐标系到激光雷达坐标系的变换矩阵
用下式(17)获取激光雷达在世界坐标系下的位姿TL ′；
优化子单元，其用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式(18)描述的点云重投影代价函数，获得机器人位姿T：
其中，
为平面特征点的重投影误差，由下式(19)获得，pi为当前帧平面特征点云中的第i个平面特征点，R为待优化旋转参数，t为待优化平移参数，
为pi在局部点云地图或全局点云地图中的最近点，ni为
所在平面的单位法向量，
为边线特征点的重投影误差，由下式(20)获得，pj为当前帧边线特征点云中的第j个边线特征点，
为pj在局部或全局点云地图中的最近点，
为pj在局部或全局点云地图中与
不在同一线束上的最近点，f(R,t)为待优化成本函数；
式中，‖‖2表示L2范数运算，上标T表示转置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310113218.7A CN115774265B (zh) | 2023-02-15 | 2023-02-15 | 用于工业机器人的二维码和激光雷达融合定位方法与装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310113218.7A CN115774265B (zh) | 2023-02-15 | 2023-02-15 | 用于工业机器人的二维码和激光雷达融合定位方法与装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115774265A CN115774265A (zh) | 2023-03-10 |
| CN115774265B true CN115774265B (zh) | 2023-05-12 |
Family
ID=85393706
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310113218.7A Active CN115774265B (zh) | 2023-02-15 | 2023-02-15 | 用于工业机器人的二维码和激光雷达融合定位方法与装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115774265B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116228870B (zh) * | 2023-05-05 | 2023-07-28 | 山东省国土测绘院 | 一种基于二维码slam精度控制的建图方法及系统 |
| CN117824667B (zh) * | 2024-03-06 | 2024-05-10 | 成都睿芯行科技有限公司 | 一种基于二维码和激光的融合定位方法及介质 |
| CN117824666B (zh) * | 2024-03-06 | 2024-05-10 | 成都睿芯行科技有限公司 | 融合定位用二维码对、二维码标定方法及融合定位方法 |
| CN117830604B (zh) * | 2024-03-06 | 2024-05-10 | 成都睿芯行科技有限公司 | 一种定位用二维码异常检测方法及介质 |
| CN118031976A (zh) * | 2024-04-15 | 2024-05-14 | 中国科学院国家空间科学中心 | 一种探索未知环境的人机协同系统 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106044645B (zh) * | 2016-08-02 | 2018-07-17 | 诺力机械股份有限公司 | 一种基于二维码定位的货物托盘存取系统、及其存取方法 |
| WO2020230931A1 (ko) * | 2019-05-16 | 2020-11-19 | 엘지전자 주식회사 | 다중 센서 및 인공지능에 기반하여 맵을 생성하고 노드들의 상관 관계를 설정하며 맵을 이용하여 주행하는 로봇 및 맵을 생성하는 방법 |
| CN110345937A (zh) * | 2019-08-09 | 2019-10-18 | 东莞市普灵思智能电子有限公司 | 一种基于二维码的导航定姿定位方法和系统 |
| CN111045017B (zh) * | 2019-12-20 | 2023-03-31 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN112862894B (zh) * | 2021-04-12 | 2022-09-06 | 中国科学技术大学 | 一种机器人三维点云地图构建与扩充方法 |
| CN114648584B (zh) * | 2022-05-23 | 2022-08-30 | 北京理工大学前沿技术研究院 | 一种用于多源融合定位的鲁棒性控制方法和系统 |
-
2023
- 2023-02-15 CN CN202310113218.7A patent/CN115774265B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN115774265A (zh) | 2023-03-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115774265B (zh) | 用于工业机器人的二维码和激光雷达融合定位方法与装置 | |
| CN111775152B (zh) | 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统 | |
| KR102257610B1 (ko) | 자율 주행 시스템을 위한 복수의 3차원 라이다 센서의 외부 파리미터 보정 방법 | |
| CN110116407B (zh) | 柔性机器人位姿测量方法及装置 | |
| CN113409410B (zh) | 一种基于3d激光雷达的多特征融合igv定位与建图方法 | |
| CN112070770B (zh) | 一种高精度三维地图与二维栅格地图同步构建方法 | |
| CN107741234A (zh) | 一种基于视觉的离线地图构建及定位方法 | |
| CN108647646A (zh) | 基于低线束雷达的低矮障碍物的优化检测方法及装置 | |
| CN108994844B (zh) | 一种打磨操作臂手眼关系的标定方法和装置 | |
| CN111046776A (zh) | 基于深度相机的移动机器人行进路径障碍物检测的方法 | |
| CN105045263A (zh) | 一种基于Kinect的机器人自定位方法 | |
| CN112767490A (zh) | 一种基于激光雷达的室外三维同步定位与建图方法 | |
| CN112464812B (zh) | 一种基于车辆的凹陷类障碍物检测方法 | |
| CN112396656B (zh) | 一种视觉与激光雷达融合的室外移动机器人位姿估计方法 | |
| CN113052881A (zh) | 提取极点的室内三维点云自动化配准方法 | |
| CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
| CN112484746B (zh) | 一种基于地平面的单目视觉辅助激光雷达里程计方法 | |
| CN110749895B (zh) | 一种基于激光雷达点云数据的定位方法 | |
| CN111273312A (zh) | 一种智能车辆定位与回环检测方法 | |
| CN112880562A (zh) | 一种机械臂末端位姿误差测量方法及系统 | |
| Olson et al. | Wide-baseline stereo vision for Mars rovers | |
| CN110736456A (zh) | 稀疏环境下基于特征提取的二维激光实时定位方法 | |
| CN113610910B (zh) | 一种移动机器人避障方法 | |
| Ma et al. | RoLM: Radar on LiDAR map localization | |
| Betge-Brezetz et al. | Object-based modelling and localization in natural environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |