[发明专利]一种导航定位误差测量方法

权利要求书

1.一种导航定位误差测量方法，包括以下步骤：

在站点放置两个10mm*10mm的二维码，两个二维码横向排布；

建立世界坐标系Xw—Yw—Zw、机器人坐标系Xr—Yr—Zr、机械臂末端坐标系Xt—Yt—Zt、相机坐标系Xc—Yc—Zc和特征点所在面的像素坐标系U—V，二维码坐标系XP—YP，相机装在机械臂的末端，二维码坐标系在世界坐标系中是固定不变的；

机械臂示教，当AGV在目标停靠站点位置时，获得移动机械臂在该站点停靠时的机械臂拍照姿态，以及获得机械臂的姿态参数，即机械臂末端坐标系相对于机器人坐标系的位置关系，相机拍摄后两个二维码位于图像中心且图像边缘具有预定空间；

视觉标定，提取出在图像中两个二维码中心点a、b所在的像素位置，利用坐标转换获得中心点a、b在机器人坐标系下的坐标，然后获得AGV在目标站点时二维码坐标系与机器人坐标系的相对关系；

开启导航系统，AGV每次停靠站点后都以同样的机械臂姿态拍照进行视觉标定，并获得在AGV当前位置时二维码坐标系与机器人坐标系的相对关系；

最终由于二维码坐标系在世界坐标系中是固定不变的，所以根据AGV在不同位置时二维码坐标系与机器人坐标系的相对关系，利用坐标转换得到AGV实际停靠位置相对目标停靠位置的误差，坐标转换是指通过二维码坐标系与机器人坐标系的相对关系，以及机械臂末端坐标系与机器人坐标系的相对关系进行的坐标转换；

将中心点a,b在像素坐标系下的坐标转换为相机坐标系下的坐标，中心点a,b统称为参考点P，具体为：

采用张正友标定方法获得内参矩阵，获得空间中某个参考点P像素坐标与相机坐标下坐标的转换关系，利用公式(2)进行坐标转换：

其中[X_c,Y_c,Z_c]^T为二维码上的参考点P在相机坐标系下的坐标，[u,v]^T为参考点P在像素坐标系下的坐标，并记[u₀,v₀]^T为相机的光轴中心线与成像平面交点的像素坐标，f_x和f_y是相机坐标系下X和Y方向的等效焦距，ρ为比例因子，通过测量得到相机与站点平面距离Z_c。

2.根据权利要求1所述的导航定位误差测量方法，其特征在于，AGV在目标停靠位置时，使得该AGV停靠在站点的设定距离范围内，机械臂带动相机移动时相机坐标系与机器人坐标系保持在水平状态，机械臂带动相机移动至两个二维码的上方，使两个二维码的位置位于相机的拍摄中心。

3.根据权利要求2所述的导航定位误差测量方法，其特征在于，所述视觉标定过程中，获得AGV在目标停靠站点位置时中心点a、b在像素坐标系下的坐标具体为：

采用基于二维码的模板匹配方法，获得中心点a、b在像素坐标系下的坐标，相机拍摄得到被搜索图像，令二维码模板T的大小为K*L,被搜索图像的大小为M*N，将二维码模板在被搜索图像中遍历平移，二维码模板重合的区域记为S_(i,j)，(i,j)为该区域左上角像素在被搜索图像中的坐标，因此1≤i≤M-K+1,1≤j≤N-L+1,T与S_(i,j)进行匹配，若完成匹配，则T与S_(i,j)的差等于零，采用公式(1)归一化积相关函数来衡量T与S_(i,j)的相似度：

由公式(1)可知：0≤R(i,j)≤1，当R(i,j)＝1时，T与S_(i,j)完全匹配，匹配到两个二维码位置后，求得中心点a、b在像素坐标系下的坐标。

4.根据权利要求3所述的导航定位误差测量方法，其特征在于，将参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标具体为：

利用机械臂示教时的机械臂姿态参数和相机在机械臂上的安装位置，得到一个机器人坐标系转换到相机坐标系的齐次变换矩阵R，利用公式(3)完成参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标，

从而根据公式(4)为了得到参考点P的像素坐标和其在移动机械臂坐标系下坐标的转换关系，从而获得了参考点P，即中心点a,b在机器人坐标系统中的坐标，

5.根据权利要求4所述的导航定位误差测量方法，其特征在于，根据公式(4)可获得的AGV在目标停靠位置时中心点a、b在机器人坐标系下的坐标和AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标，T是AGV在目标停靠位置时二维码坐标系相对机器人坐标系的相对关系，可由在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标求得C是AGV在实际停靠位置时二维码坐标系相对机器人坐标系的相对关系，可由AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标求得，如公式(5)(6)所示：

其中，(a_xt，a_yt)和(b_xt，b_yt)是在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标，(a_xa，a_ya)和(b_xa，b_ya)是AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标；

最终利用坐标转换即可得到AGV实际停靠位置相对目标停靠位置的误差[x_e,y_e,θ_e]^T，坐标转换通过公式(7)(8)求解：

T＝EC (7)

其中，E是机械臂实际停靠位置相对目标停靠位置的齐次变换矩阵。