[发明专利]用于工业机器人的二维码和激光雷达融合定位方法与装置

权利要求书

1.一种用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，包括：

步骤1，检测图像数据中的二维码信息，结合预先标定的相机坐标系C和IPM坐标系I之间的转换关系M_IPM，M_IPM为地面先验中的IPM投影矩阵，获得世界坐标系到IPM坐标系的变换关系T_IPM；以及

步骤2，融合变换关系T_IPM和点云信息，进行全局定位建图，获得机器人位姿T；

其中，步骤1具体包括：

步骤11，获取二维码信息中二维码的标记点i的世界坐标p_wⁱ，n为在图像数据上检测到的标记点i的总数；

步骤12，将世界坐标p_wⁱ转换到IPM坐标系中，得到标记点i的IPM坐标p_Iⁱ；

步骤13，获取变换关系T_IPM：

将二维码定位问题转化为第一优化问题(3)，其中，J表示利用相机IPM坐标系到世界坐标转换构建的约束，‖‖²为最小二乘运算符：

根据R_IPM表示世界坐标系到IPM坐标系的旋转变换矩阵，t_IPM表示下式(4)描述的世界坐标系到IPM坐标系的平移变换矩阵；

式(4)中，为中心点的世界坐标，为中心点的IPM坐标，通过式(5)计算得到，R_IPM表示世界坐标系到IPM坐标系的旋转变换矩阵；

其中：旋转变换矩阵R_IPM的获取方法具体包括：

先用下式(6)和(7)计算和以及标记点i相对中心点的世界坐标和像素坐标再用下式(8)求解R_IPM：

式中，为标记点i相对于中心点的世界坐标，为标记点i相对于中心点的IPM坐标，z_Cⁱ为标记点i在相机坐标系下的z轴坐标，M_IPM为地面先验中的IPM投影矩阵，(uⁱ，vⁱ)表示标记点i的像素坐标，_xy表示取三维向量的x、y二维数据，p_wⁱ为标记点i的世界坐标(x_wⁱ，y_wⁱ)。

2.如权利要求1所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤2具体包括：

步骤21，检测点云信息中当前帧对应的图像帧的二维码；

步骤22，在检测到点云信息中当前帧对应的图像帧有二维码的情形下，结合二维码世界坐标，求解机器人位姿T；

步骤23，在未检测到点云信息中当前帧对应的图像帧有二维码的情形下，利用历史关键帧的位姿信息，构建局部点云地图，提取当前帧点云中的特征点，并与局部点云地图相匹配，以获得机器人位姿估计信息，待重新检测到点云信息中当前帧对应的图像帧有二维码的情形下，融合变换关系T_IPM和激光里程计定位信息，获得机器人位姿T。

3.如权利要求2所述的用于工业机器人的二维码和激光雷达融合定位方法，其特征在于，步骤23中的关键帧的获取方法包括：

步骤231a，判断当前时刻与上一时刻的位姿变化量是否大于预先设定的位姿变化量阈值，若是，则为关键帧，否则为非关键帧；

步骤232a，将非关键帧点云投影至当前关键帧点云，并对投影获得的关键帧点云进行滤波，再存储关键帧位姿及其对应的点云。