基于双目视觉的机器人示教方法

本发明涉及机器人控制领域，具体涉及一种基于双目视觉的机器人示教方法。

背景技术：

1、随着现代计算机和工业机器人制造技术的进步和迅猛发展，机器人被广泛应用于零部件的焊接、喷涂、装配等工业任务中。相对于传统的工人，工业机器人具有可编程、拟人化、通用性等特点以及作业效率高、较好的鲁棒性、重复精度好、适合高危环境作业等优势。这使得工业机器人的发展成为现代工业生产管理过程中不可或缺的一个重要组成部分。

2、目前工业机器人完成大多数任务是依靠示教-再现形式实现，“示教”顾名思义为示范教导的意思，即在机器人正常加工之前，操作者利用某种交互方式示范执行加工任务时所需动作，并且将示教过程记录在存储器中。“再现”即对示教的过程重现，利用存储器中的数据，经过一定的转换，并经由机器人的驱动器、控制器实现机器人对工件的加工动作。示教-再现的工作，一般包括示教轨迹点的位置、姿态以及运动过程中的速度、加速度。

3、传统的机器人示教主要分为两种方式：直接示教和离线示教。直接示教就是指由人直接搬动机器人的手臂对机器人进行示教，如示教盒示教或操作杆示教等。在这种示教中，为了示教方便以及获取信息的快捷而准确，操作者可以选择在不同坐标系下示教，例如，可以选择在关节坐标系、直角坐标系及工具坐标系或用户坐标系下进行示教。直接示教模式存在示教精度低、示教编程繁琐、示教形式单一等技术问题。离线示教与直接示教不同，操作者不对实际作业的机器人直接进行示教，而是脱离实际作业环境生成示教数据，间接地对机器人进行示教。在离线示教中通过使用计算机内存储的机器人cad模型，不要求机器人实际产生运动，便能在示教结果的基础上对机器人运动进行仿真，从而确定示教内容是否恰当及机器人是否按照期望方式运动。但由于离线编程通常需要示教者熟知示教编程模式，对示教者而言，示教门槛较高，且需要花费大量时间完成不同作业环境下的示教操作与调试工作，这往往造成实际生产效率低下。

技术实现思路

1、针对上述提到的技术问题。本技术的实施例的目的在于提出了一种基于双目视觉的机器人示教方法，来解决以上背景技术部分提到的技术问题。通过双目视觉系统跟踪示教笔轨迹的方式进行机器人轨迹编程，在示教过程中示教者用示教笔在每个关键节点摆出机器人末端位姿，经计算后得到的位姿下发到机器人系统，机器人便以与示教笔相同位姿依次通过每个位置。因此用户不需要进行代码操作便能操作机器人，降低了编程门槛，使得机器人轨迹编程得到简化。

2、第一方面，本发明提供了一种基于双目视觉的机器人示教方法，包括以下步骤：

3、s1，利用相机标定原理标定视觉装置的左相机和右相机，得到左相机的内参矩阵、右相机的内参矩阵以及右相机相对于左相机的旋转矩阵和平移向量，视觉装置安装在机器人旁边，采用手眼标定原理标定机器人基坐标系与相机坐标系之间的位置关系，获取机器人基坐标系与相机坐标系之间的转换矩阵；

4、s2，通过左相机和右相机同时拍摄获取到示教笔在初始位姿下的原始图像，示教笔包括杆体、球体和安装部，杆体包括具有不同长度的第一杆、第二杆、第三杆、第四杆和中心杆，球体包括分别位于第一杆、第二杆、第三杆、第四杆和中心杆一端的第一球、第二球、第三球、第四球和中心球，第一杆、第二杆、第三杆、第四杆和中心杆的另一端安装于安装部上，第一杆、第二杆、第三杆和第四杆两两垂直设置并位于同一平面，中心杆垂直于该平面；

5、s3，对原始图像进行预处理，得到球体图像；

6、s4，根据左相机的内参矩阵、右相机的内参矩阵以及右相机相对于左相机的旋转矩阵和平移向量计算球体图像中每个球体对应的投影圆心像素坐标，并求解得到相机坐标系下的每个球体的球心坐标；

7、s5，根据相机坐标系下的每个球体的球心坐标计算得到示教笔在相机坐标系下的位姿，并通过机器人基坐标系与相机坐标系之间的转换矩阵变换得到示教笔在机器人基坐标系下的位姿；

8、s6，改变示教笔的位姿，重复步骤s2-s5，求解得到一系列示教笔在机器人基坐标系下的位姿，并生成机器人可执行程序，通过控制系统控制机器人以与示教笔相同的轨迹进行运动。

9、作为优选，步骤s1中采用手眼标定原理标定机器人基坐标系与相机坐标系之间的位置关系，获取机器人基坐标系与相机坐标系之间的转换矩阵，具体包括：

10、步骤s1中采用手眼标定原理标定机器人基坐标系与相机坐标系之间的位置关系，获取机器人基坐标系与相机坐标系之间的转换矩阵，具体包括：

11、将用于相机标定的棋盘格放置在双目相机视野中，通过相机标定得到相机坐标系相对于棋盘格坐标系的转换矩阵

12、在保持棋盘格位置不变的情况下，利用三点标定法标定机器人工件坐标系，并使得机器人工件坐标系与棋盘格坐标系重合，获得机器人基坐标系相对于工件坐标系的转换矩阵

13、由于机器人工件坐标系与棋盘格坐标系重合，相机坐标系相对于机器人工件坐标系的转换关系表示为：

14、

15、则机器人基坐标系与相机坐标系之间的转换矩阵为：

16、

17、作为优选，球体为白色，示教笔的其他部件均为黑色，步骤s3具体包括：

18、通过图像滤波处理去除原始图像中的噪声，并将原始图像中的示教笔与背景分离，得到滤波处理后的图像；

19、将滤波处理后的图像进行二值化处理，得到球体图像。

20、作为优选，步骤s4具体包括：

21、通过hough变换求解左相机下的球体图像中每个球体对应投影圆的圆心像素坐标pli(uli，vli)以及右相机下的球体图像中每个球体对应投影圆的圆心像素坐标pri(uri，vri)，i＝1,2,3,4,5；

22、根据极线约束求取pli在右相机成像图像中的极线，在极线上搜索距离较近的pri点，将五个圆心一一对应，得到左相机成像图像中每个圆心位置在右相机成像图像上的对应点；极线约束原理为：对于三维空间一点p，在左相机和右相机的成像平面投影点分别为pli和pri，两个平面的极线方程为：

23、l1＝f-1pri；

24、l2＝fpli；

25、其中，f为基本方程，[m]×为m的反对称矩阵，mr1为右相机的内参矩阵，m11为左相机的内参矩阵；由于左相机坐标系与相机测量坐标系重合，左相机坐标系相对于相机测量坐标系的旋转矩阵rl为单位矩阵i，平移向量tl为0，右相机坐标系相对于相机测量坐标系的旋转矩阵rr和平移向量tr为右相机相对于左相机的旋转矩阵r和平移向量t；mr＝tr；ml＝tl；

26、记三维空间中球体i的球心位置为点pi，根据成像模型得到：

27、

28、

29、其中，ml、mr为左相机的投影矩阵和右相机的投影矩阵，表示为：

30、

31、

32、(xi，yi，zi)为pi点坐标，消掉zli、zri后化成矩阵形式：ap＝b；其中：

33、

34、p＝[xi yi zi]t；

35、

36、根据最小二乘法，解得球心pi的坐标为：

37、pi＝(ata)-1atb；

38、分别对五组球体对应投影圆的圆心像素坐标进行求解得到五个球体的球心的三维坐标。

39、作为优选，球体的直径为20mm，第一杆、第二杆、第三杆、第四杆和中心杆的长度分别为50mm、55mm、60mm、65mm和100mm，第一杆、第二杆、第三杆和第四杆的另一端与第一球至第四球连接的十字交叉中心之间的距离为5mm，中心球的球心位置到十字交叉中心的距离为40mm；定义示教笔的第一球至第四球连接的十字交叉中心为坐标原点，中心杆上远离中心球的方向为z轴正方向，采用右手坐标系规则建立一个依附于球体的示教笔坐标系，其中设定x轴正方向指向第一球的方向，则y轴正方向为指向第四球的方向。

40、作为优选，步骤s5具体包括：

41、根据五个球体的球心的三维坐标分别计算出每两个球体的球心之间的距离：

42、

43、其中，djk表示点pj与点pk之间距离，0<j<k，k＝2,3,4,5，由于示教笔的杆体长度不同，根据计算得到每两个球体的球心之间的距离将上述求解得到的五个球体的球心pi坐标与示教笔上的五个球体一一对应位置，记为球体的球心对应点pn，n＝1,2,3,4,5；

44、根据示教笔的第一球、第二球、第三球、第四球的球心对应点pn计算得到当前位置下示教笔坐标系中x轴、y轴和z轴在相机坐标系下的姿态，计算如下：

45、

46、

47、

48、进一步求解得到单位向量：

49、

50、

51、

52、示教笔坐标系的坐标原点位置设为第一球到第四球连接的十字交叉中心位置，设示教笔坐标系的原点坐标为ps，根据五个球体的球心坐标求取ps坐标，ps与五个球体的球心对应点坐标位置关系如下：

53、

54、

55、

56、

57、

58、可得：

59、

60、记为向量ts＝[xs,ys,zs]t；

61、示教笔坐标系与相机坐标系关系为：

62、

63、进一步求解得到示教笔末端坐标pt(xt，yt，zt)：

64、

65、记为向量tt＝[xt,yt,zt]t；

66、根据示教笔末端的位置pt和示教笔的姿态求解机器人末端的位置与姿态，通过手眼标定中求解得到的机器人坐标系相对于相机坐标系的外参平移矩阵和旋转矩阵，计算得到相机坐标系到机器人基坐标系的转换矩阵将相机坐标系下的示教笔末端位姿转换到机器人基坐标系下，得到机器人末端的位姿，求解过程如下：

67、示教笔末端位姿与相机坐标系之间关系为：

68、

69、则示教笔末端在机器人基坐标系下的位姿表示为：

70、

71、其中，为示教笔坐标系的x、y、z轴的方向向量在机器人基坐标系下的坐标，为示教笔末端在机器人基坐标系下的坐标。

72、第二方面，本发明提供了一种基于双目视觉的双臂七关节机器人示教方法，包括以下步骤：

73、t1，利用相机标定原理标定视觉装置的左相机和右相机，得到左相机的内参矩阵、右相机的内参矩阵以及右相机相对于左相机的旋转矩阵和平移向量，视觉装置安装在双臂七关节机器人旁边，双臂七关节机器人设有两个机器人末端，两个机器人末端分别夹持刚体两端进行移动；采用手眼标定原理标定机器人基坐标系与相机坐标系之间的位置关系，获取机器人基坐标系与相机坐标系之间的转换矩阵；

74、t2，通过左相机和右相机同时拍摄获取到示教笔在初始位姿下的原始图像，示教笔包括杆体、球体和安装部，杆体包括具有不同长度的第一杆、第二杆、第三杆、第四杆和中心杆，球体包括分别位于第一杆、第二杆、第三杆、第四杆和中心杆一端的第一球、第二球、第三球、第四球和中心球，第一杆、第二杆、第三杆、第四杆和中心杆的另一端安装于安装部上，第一杆、第二杆、第三杆和第四杆两两垂直设置并位于同一平面，中心杆垂直于该平面；

75、t3，对原始图像进行预处理，得到球体图像；

76、t4，根据左相机的内参矩阵、右相机的内参矩阵以及右相机相对于左相机的旋转矩阵和平移向量计算球体图像中每个球体对应的投影圆心像素坐标，并求解得到相机坐标系下的每个球体的球心坐标；

77、t5，根据相机坐标系下的每个球体的球心坐标计算得到示教笔在相机坐标系下的位姿，并通过机器人基坐标系与相机坐标系之间的转换矩阵变换得到示教笔在机器人基坐标系下的位姿；将刚体坐标系设置为与示教笔坐标系相同，得到刚体坐标系与相机坐标系的关系，并将刚体坐标系转换到机器人末端的两个工具坐标系下，得到两个工具坐标系与刚体坐标系的关系，根据刚体坐标系与相机坐标系的关系以及两个工具坐标系与刚体坐标系的关系得到两个工具坐标系在相机坐标系下的位姿；

78、t6，改变示教笔的位姿，重复步骤t2-t5，求解得到一系列示教笔在机器人基坐标系下的位姿，并生成机器人可执行程序，通过控制系统控制机器人末端分别夹持刚体的两端按照既定轨迹运动。

79、作为优选，步骤t4中将刚体坐标系设置为与示教笔坐标系重合，得到刚体坐标系与相机坐标系的关系，并将刚体坐标系转换到机器人末端的两个工具坐标系下，得到两个工具坐标系与刚体坐标系的关系，根据刚体坐标系与相机坐标系的关系以及两个工具坐标系与刚体坐标系的关系得到两个工具坐标系在相机坐标系下的位姿，具体包括：

80、设定刚体质心点o与示教笔的十字交叉中心重合，即po(xo，yo，zo)＝ps，示教笔坐标系x轴方向对应刚体的长边，y轴方向对应刚体的短边，记为向量to＝[xo,yo,zo]t，则刚体坐标系与相机坐标系的关系为：

81、

82、其中，为示教笔的姿态；

83、在刚体两端矩形中心分别建立两个工具坐标系，两个工具坐标系对应左右机器人末端的位姿，设定两个工具坐标系原点o1、o2的坐标分别为示教笔的十字交叉中心位置分别沿刚体坐标系x轴正负方向偏移距离l/2，l为刚体长度，机器人末端姿态为示教笔坐标系绕y轴分别逆时针和顺时针旋转90°，工具坐标系与刚体坐标系的关系为：

84、

85、

86、则两个工具坐标系在相机坐标系下的位姿为：

87、

88、

89、第二方面，本发明提供了一种验证基于双目视觉的双臂七关节机器人示教精度的方法，包括以下步骤：

90、e1，对当前示教笔坐标系进行校正，得到校正旋转矩阵，根据校正旋转矩阵对当前示教笔坐标系与刚体坐标系之间的关系进行校正，得到校正后的示教笔坐标系与刚体坐标系之间的关系；

91、e2，控制双臂机器人夹持刚体按一定轨迹进行移动，通过左相机和右相机同时拍摄获取到示教笔在当前位置时的原始图像，同时记录下机器人系统获取到的机器人末端的第一位姿；

92、e3，根据校正旋转矩阵和校正后的示教笔坐标系与刚体坐标系之间的关系采用第二方面提出的基于双目视觉的双臂七关节机器人示教方法计算得到机器人末端的第二位姿，计算第二位姿与第一位姿之间的差值，得到示教误差，根据示教误差验证精度是否达到要求。

93、作为优选，步骤e1中具体包括：

94、e11，通过视觉装置的左相机和右相机同时拍摄获取到示教笔在第一位置时的原始图像，并求解出在第一位置时相机坐标系下示教笔的方向向量以及相机坐标系下第一位置时第一球与第四球的圆心三维坐标，分别为p11、p14，其中，示教笔安装在刚体上，示教笔的中心杆穿过刚体的中心，第一球至第四球所形成的平面与刚体的上表面平行，调整示教笔的位置，使第一球与第三球所连直线与刚体长边平行，第二球与第四球所连直线与刚体短边平行；

95、e12，控制双臂机器人同时向刚体坐标系x轴正方向移动至第二位置，通过左相机和右相机同时拍摄获取到示教笔和在第二位置时的原始图像，并求解出相机坐标系下第二位置时第一球的圆心三维坐标p21；

96、e13，控制双臂机器人同时向刚体坐标系y轴正方向移动至第三位置，通过左相机和右相机同时拍摄获取到示教笔在第三位置时的原始图像，并求解出相机坐标系下第三位置时第四球的圆心三维坐标p34；

97、e14，根据相机坐标系下第一位置时第一球与第四球的圆心三维坐标、相机坐标系下第二位置时第一球的圆心三维坐标以及相机坐标系下第三位置时第四球的圆心三维坐标计算得到校正后的示教笔坐标系方向向量，并计算得到校正旋转矩阵：

98、

99、

100、

101、校正后的示教笔坐标系方向向量与校正前的示教笔坐标系方向向量的变换关系为：

102、

103、

104、故求解得到校正旋转矩阵rmx和rmy；

105、采用根据第二方面的基于双目视觉的双臂七关节机器人示教方法计算求得机器人末端的第二位姿，并在计算过程中采用下式将示教笔的姿态替换为

106、

107、

108、并将示教笔坐标系替换为校正后的示教笔坐标系，即相机坐标系与校正后的示教笔坐标系关系为：

109、

110、刚体坐标系相对于示教笔坐标系沿z轴偏移距离d，校正后的示教笔坐标系与刚体坐标系之间的关系为：

111、

112、故校正后的刚体坐标系在相机坐标系下的位姿表示为：

113、

114、相比于现有技术，本发明具有以下有益效果：

115、(1)本发明利用球杆组合的示教笔作为示教工具，通过三维视觉的示教方式，代替传统的示教编程，可以简化机器人轨迹编程步骤，降低机器人操作的门槛，也降低了机器人运行和维护的成本。

116、(2)本发明中双目视觉系统识别特定的示教工具，而不是直接从环境中提取信息，因此可极大降低图像处理的复杂性，也在一定程度上保证了系统的鲁棒性。