一种导航定位误差测量方法
技术领域
本发明属于导航定位技术领域,具体地说是一种主要针对AGV机器人移动过程中导航定位的误差测量方法。
背景技术
目前很多自动化应用场所,都使用AGV移动机器人进行操作,以提高生产效率。AGV的活动范围较大导致其位置误差的精准测量比较困难,通常采用三角测量、运动捕捉系统、数据集以及特定评估算法等测量或评估位置误差。
三角测量,通过测量设置多个标志点,通过计算当前位置和标志点的几何关系从而估计出当前位置坐标。该方法存在标志点位置确定困难、定位精度不高等问题。
运动捕捉系统是通过多个高性能的高速摄像头实现对移动机器人的空间位置确定,该系统设备十分昂贵。
也有利用算法来评估定位精度的方法,比如利用激光雷达数据的扫描匹配从而获得较高定位结果,以此作为真实位置的估计。
还可以通过算法得出的位置信任分布(belief distributions)的信息熵来度量当前的位置精度。但特定评估算法给出的位置本身就容易受环境变化的影响。
发明内容
为了解决上述的技术问题,本发明提供了一种导航定位误差测量方法,其采取以下技术方案:
一种导航定位误差测量方法,包括以下步骤:
在站点放置两个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实际停靠位置相对目标停靠位置的误差。
所述AGV在目标停靠位置时,使得该AGV停靠在站点的设定距离范围内,机械臂带动相机移动时相机坐标系与机器人坐标系保持在水平状态,机械臂带动相机移动至两个二维码的上方,使两个二维码的位置位于相机的拍摄中心。
所述视觉标定过程中,获得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在像素坐标系下的坐标。
所述将中心点a、b在像素坐标系下的坐标转换为相机坐标系下的坐标,中心点a、b统称为参考点P,具体为:
采用张正友标定方法获得内参矩阵,获得空间中某个参考点P像素坐标与相机坐标下坐标的转换关系,利用公式(2)进行坐标转换:
其中,[Xc,Yc,Zc]T为二维码上的参考点P在相机坐标系下的坐标,[u,v]T为参考点P在像素坐标系下的坐标,并记[u0,v0]T为相机的光轴中心线与成像平面交点的像素坐标,fx和fy是相机坐标系下X和Y方向的等效焦距,ρ为比例因子,通过测量得到相机与站点平面距离Zc。
所述将参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标具体为:
利用机械臂示教时的机械臂姿态参数和相机在机械臂上的安装位置,得到一个机器人坐标系转换到相机坐标系的齐次变换矩阵R,利用公式(3)完成参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标:
从而根据公式(4)可得到参考点P的像素坐标和其在移动机械臂坐标系下坐标的转换关系,从而获得了参考点P,即中心点a、b在机器人坐标系统中的坐标:
根据公式(4)可获得的AGV在目标停靠位置时中心点a、b在机器人坐标系下的坐标和AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标。T是AGV在目标停靠位置时二维码坐标系相对机器人坐标系的相对关系,可由在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标求得C是AGV在实际停靠位置时二维码坐标系相对机器人坐标系的相对关系,可由AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标求得,如公式(5)(6)所示:
其中,(axt,ayt)和(bxt,byt)是在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标,(axa,aya)和(bxa,bya)是AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标
最终利用坐标转换即可得到AGV实际停靠位置相对目标停靠位置的误差[xe,ye,θe]T,坐标转换通过公式(7)(8)求解:
T=EC (7)
其中,E是机械臂实际停靠位置相对目标停靠位置的齐次变换矩阵。
本发明测量精度高,准确的测量出AGV目标停靠位置与实际停靠位置的误差,速度快,提高效率。
附图说明
附图1为本发明测量示意图;
附图2为本发明中心点a、b在二维码上的提取示意图。
具体实施方式
为能进一步了解本发明的特征、技术手段以及所达到的具体目的、功能,下面结合附图与具体实施方式对本发明作进一步详细描述。
如附图1和2所示,AGV上具有机械臂,机械臂末端安装相机,机械臂可进行各个方向的动作,从而带动相机的位置变化。本发明揭示了一种导航定位误差测量方法,包括以下步骤:
搭建一个AGV场景,在站点设两个二维码,该两个二维码的中心点为a、b中心点a和b的连线中点为站点参考点P。
在站点放置两个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实际停靠位置相对目标停靠位置的误差。
所述AGV在目标停靠位置时,使得该AGV停靠在站点的设定距离范围内,机械臂带动相机移动时相机坐标系与机器人坐标系保持在水平状态,机械臂带动相机移动至两个二维码的上方,使两个二维码的位置位于相机的拍摄中心。
所述视觉标定过程中,获得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在像素坐标系下的坐标。
所述将中心点a、b在像素坐标系下的坐标转换为相机坐标系下的坐标,中心点a、b统称为参考点P,具体为:
采用张正友标定方法获得内参矩阵,获得空间中某个参考点P像素坐标与相机坐标下坐标的转换关系,利用公式(2)进行坐标转换:
其中,[Xc,Yc,Zc]T为二维码上的参考点P在相机坐标系下的坐标,[u,v]T为参考点P在像素坐标系下的坐标,并记[u0,v0]T为相机的光轴中心线与成像平面交点的像素坐标,fx和fy是相机坐标系下X和Y方向的等效焦距,ρ为比例因子,通过测量得到相机与站点平面距离Zc。
所述将参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标具体为:
利用机械臂示教时的机械臂姿态参数和相机在机械臂上的安装位置,得到一个机器人坐标系转换到相机坐标系的齐次变换矩阵R,利用公式(3)完成参考点P在相机坐标系下的坐标转换为机器人坐标系下的坐标:
从而根据公式(4)得到参考点P的像素坐标和其在移动机械臂坐标系下坐标的转换关系,从而获得了参考点P,即中心点a、b在机器人坐标系统中的坐标:
根据公式(4)可获得的AGV在目标停靠位置时中心点a、b在机器人坐标系下的坐标和AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标。T是AGV在目标停靠位置时二维码坐标系相对机器人坐标系的相对关系,可由在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标求得C是AGV在实际停靠位置时二维码坐标系相对机器人坐标系的相对关系,可由AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标求得,如公式(5)(6)所示:
其中,(axt,ayt)和(bxt,byt)是在AGV目标停靠位置时中心点a、b在机器人坐标系下的坐标,(axa,aya)和(bxa,bya)是AGV在实际停靠位置时中心点a、b在机器人坐标系下的坐标。
最终利用坐标转换即可得到AGV实际停靠位置相对目标停靠位置的误差[xe,ye,θe]T,坐标转换通过公式(7)(8)求解:
T=EC (7)
其中,E是机械臂实际停靠位置相对目标停靠位置的齐次变换矩阵。
本发明通过在站点上设置两个二维码,通过获取中心点a、b在目标停靠位置时机器人坐标系下的坐标和实际停靠位置时机器人坐标系下的坐标,利用坐标转换即可得到AGV真实停靠位置相对目标停靠位置的误差,速度更快,精度更高。
需要说明的是,以上仅为本发明的优选实施例而已,并不用于限制本发明,尽管参照实施例对本发明进行了详细的说明,对于本领域的技术人员来说,其依然可以对前述实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换,但是凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。