主权项 |
一种基于多个二维码读码器的移动机器人定位方法,其特征在于:包括以下步骤:⑴建立如下三个坐标系:图像坐标系(u、v)、小车坐标系(x小车、y小车)、世界坐标系(x世界、y世界);⑵计算在小车坐标系下移动机器人小车相对于两个二维码中心点连线的偏离角度;⑶计算两个二维码中心点连线相对于世界坐标系的偏离角度;⑷计算移动机器人小车相对于世界坐标系的偏离角度,从而获得移动机器人的精确位置;所述步骤⑵包括以下处理过程:①标定图像坐标系和小车坐标系的关系:P1小车(x,y)=R×P1图像(u1,v1);P1小车(x,y)=R×P2图像(u2,v2);上式中:P1图像(u1,v1)为空间某一点P1在图像坐标系中的坐标;P2图像(u2,v2)为空间某一点P2在图像坐标系中的坐标;P1小车(x,y)为空间某一点P1在小车坐标系下的坐标;P2小车(x,y)为空间某一点P2在小车坐标系下的坐标;R为旋转矩阵值;②将图像坐标系下的四个顶点转换为小车坐标系;第一个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为:P1二维码(x1,y1)=R×P1二维码(u1,v1)P1二维码(x2,y2)=R×P1二维码(u2,v2)P1二维码(x3,y3)=R×P1二维码(u3,v3)P1二维码(x4,y4)=R×P1二维码(u4,v4)第二个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为:P2二维码(x1,y1)=R×P2二维码(u1,v1)P2二维码(x2,y2)=R×P2二维码(u2,v2)P2二维码(x3,y3)=R×P2二维码(u3,v3)P2二维码(x4,y4)=R×P2二维码(u4,v4)③计算小车坐标系下的两个二维码的中心点坐标:第一个二维码的中心点在小车坐标系中的坐标为:X1二维码=(Point11.x+Point12.x+Point13.x+Point14.x)/4;Y1二维码=(Point11.y+Point12.y+Point13.y+Point14.y)/4;上式中,Point11.x、Point12.x、Point13.x、Point14.x为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标;Point11.y、Point12.y、Point13.y、Point14.y为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标;第二个二维码的中心点在小车坐标系中的坐标为:X2二维码=(Point21.x+Point22.x+Point23.x+Point24.x)/4;Y2二维码=(Point21.y+Point22.y+Point23.y+Point24.y)/4;上式中,Point21.x、Point22.x、Point23.x、Point24.x为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标;Point21.y、Point22.y、Point23.y、Point24.y为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标;④计算小车坐标系下两个中心点位置偏差△X小车、△Y小车:△X小车=X2二维码‑X1二维码;△Y小车=Y2二维码‑Y1二维码;⑤计算小车坐标系下的角度偏差tanθ小车_码:tanθ小车_码=△X小车/△Y小车;所述步骤⑶包括以下处理过程:①获得两个二维码在世界坐标系下的坐标:P1(x1世界,y1世界);p2(x2世界,y2世界);②计算得到两个二维码中心点连线相对于世界坐标系的△X世界、△Y世界;△X世界=x2世界‑x1世界;△Y世界=y2世界‑y1世界;③计算得到两个二维码中心点连线相对于世界坐标系的偏离角度;tanθ码_世界=△X世界/△Y世界;所述步骤⑷计算移动机器人小车相对于世界坐标系的偏离角度的公式为:θ车_世界=θ小车_码+θ码_世界上式中,θ车_世界为移动机器人小车相对于世界坐标系的偏离角度;θ小车_码为移动机器人小车相对于二维码的偏离角度;θ码_世界为两个二维码中心点连线相对于世界坐标系的偏离角度。 |