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

技术特征：
1.一种用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，包括：步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系；以及步骤2，融合变换关系和点云信息，进行全局定位建图，获得机器人位姿；其中，步骤1具体包括：步骤11，获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数；步骤12，将世界坐标转换到坐标系中，得到标记点的坐标；步骤13，获取变换关系将二维码定位问题转化为第一优化问题（3），其中，表示利用“相机坐标系到世界坐标转换”构建的约束，为最小二乘运算符：
ꢀꢀꢀꢀꢀꢀꢀꢀ
（3）根据，表示世界坐标系到坐标系的旋转变换矩阵，表示下式（4）描述的世界坐标系到坐标系的平移变换矩阵；
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（4）式（4）中，为中心点的世界坐标，为中心点的坐标，表示世界坐标系到坐标系的旋转变换矩阵。2.如权利要求1所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，旋转变换矩阵的获取方法具体包括：先用下式（7）计算和以及标记点相对中心点的世界坐标和像素坐标，再用下式（8）求解：（7）（8）式中，为标记点的坐标，为标记点在相机坐标系下的轴坐标，为地面先验中的投影矩阵，表示标记点的像素坐标，表示取三维向量的、二维数据，为标记点的世界坐标。3.如权利要求1或2所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤2具体包括：
步骤21，检测点云信息中当前帧对应的图像帧的二维码；步骤22，在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿；步骤23，在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。4.如权利要求3所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤23中的关键帧的获取方法包括：步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。5.如权利要求3所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤23中的“融合变换关系和激光里程计定位信息，获得机器人位姿”的方法具体包括：步骤231c，根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用下式（17）获取激光雷达在世界坐标系下的位姿，再利用构建关键帧约束、关键帧局部点云地图或全局点云地图； （17）步骤232c，通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式（18）描述的点云重投影代价函数，获得机器人位姿：
ꢀꢀꢀ
（18）其中，为平面特征点的重投影误差，由下式（19）获得，为当前帧平面特征点云中的第个平面特征点，为待优化旋转参数，为待优化平移参数，为在局部点云地图或全局点云地图中的最近点，为所在平面的单位法向量，为边线特征点的重投影误差，由下式（20）获得，为当前帧边线特征点云中的第个边线特征点，为在局部或全局点云地图中的最近点，为在局部或全局点云地图中与不在同一线束上的最近点，为待优化成本函数； （19）
（20）式中，表示 范数运算，上标表示转置。6.一种用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，包括：二维码信息定位模块，其用于检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系；以及位姿获取模块，其用于融合变换关系和点云信息，进行全局定位建图，获得机器人位姿；其中，二维码信息定位模块具体包括：世界坐标获取单元，其用于获取二维码信息中二维码的标记点的世界坐标，为在图像数据上检测到的标记点的总数；坐标获取单元，其用于将世界坐标转换到坐标系中，得到标记点的坐标；坐标系变换单元，其用于获取变换关系：将二维码定位问题转化为第一优化问题（3），其中，表示利用“相机坐标系到世界坐标转换”构建的约束，为最小二乘运算符：（3）根据，表示世界坐标系到坐标系的旋转变换矩阵，表示下式（4）描述的世界坐标系到坐标系的平移变换矩阵； （4）式（4）中，为中心点的世界坐标，为中心点的坐标，表示世界坐标系到坐标系的旋转变换矩阵。7.如权利要求6所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，旋转变换矩阵的获取方法具体包括：先用下式（7）计算和以及标记点相对中心点的世界坐标和像素坐标，再用下式（8）求解：
（7）（8）式中，为标记点的坐标，为标记点在相机坐标系下的轴坐标，为地面先验中的投影矩阵，表示标记点的像素坐标，表示取三维向量的、二维数据，为标记点的世界坐标。8.如权利要求6或7所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，位姿获取模块具体包括：二维码检测单元，其用于检测点云信息中当前帧对应的图像帧的二维码；第一位姿获取单元，其用于在检测到点云信息中当前帧对应的图像帧有二维码的情形下，根据变换关系，获得机器人位姿；第二位姿获取单元，其用于在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系和激光里程计定位信息，获得机器人位姿。9.如权利要求8所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，第二位姿获取单元具体包括：位姿变化量判断子单元，其用于判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；关键帧点云处理子单元，其用于将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。10.如权利要求8所述的用于工业机器人的二维码和激光雷达融合定位装置，其特征在于，位姿获取模块具体还包括信息融合单元；信息融合单元具有：二维码定位子单元，其用于根据变换关系，结合预先标定的坐标系到激光雷达坐标系的变换矩阵，用下式（17）获取激光雷达在世界坐标系下的位姿；
ꢀꢀꢀꢀ
（17）优化子单元，其用于通过将当前帧点云中的特征点与局部点云地图或全局点云地图进行匹配，构建下式（18）描述的点云重投影代价函数，获得机器人位姿：（18）
其中，为平面特征点的重投影误差，由下式（19）获得，为当前帧平面特征点云中的第个平面特征点，为待优化旋转参数，为待优化平移参数，为在局部点云地图或全局点云地图中的最近点，为所在平面的单位法向量，为边线特征点的重投影误差，由下式（20）获得，为当前帧边线特征点云中的第个边线特征点，为在局部或全局点云地图中的最近点，为在局部或全局点云地图中与不在同一线束上的最近点，为待优化成本函数；
ꢀꢀꢀꢀꢀꢀꢀ
（19）（20）式中，表示 范数运算，上标表示转置。

技术总结
本发明公开了一种用于工业机器人的二维码和激光雷达融合定位方法与装置，其中方法包括：步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系和坐标系之间的转换关系，获得世界坐标系到坐标系的变换关系；步骤2，融合变换关系和点云信息，进行全局定位建图，获得机器人位姿。本发明能够提升系统的鲁棒性。本发明能够提升系统的鲁棒性。本发明能够提升系统的鲁棒性。


技术研发人员：秦晓辉 周云水 李建中 秦兆博 谢国涛 王晓伟 徐彪
受保护的技术使用者：江苏集萃清联智控科技有限公司
技术研发日：2023.02.15
技术公布日：2023/3/10