基于2D相机的机器人自动加注方法与流程

本发明涉及加注，尤其涉及基于2d相机的机器人自动加注方法。

背景技术：

1、加注是工业现场一种常见的作业需求，例如电池充电，车辆加油，胶桶加注胶水，漆桶加注油漆等等，这一类加注作业的特征可以抽象为同一类动作：准确定位加注口的空间位置，并把枪口准确插入加注口，在机器人普及之前，此类作业往往由人工完成，现在可以由机器视觉自动识别加注口并计算加注口的三维空间位置，并由机器人携带加注枪插入加注口，控制阀门打开并完成加注作业。

2、由于加注作业中，加注口和加注枪往往有一定的配合精度要求，使得加注口和加注枪能够可靠地连接，从而完成作业，因此，在机器人携带加注枪插入加注口时，加注口相对于机器人(更进一步即加注枪)的位姿(位置和姿态)需要精确地计算出来，加注口的位姿计算通常用视觉的方法进行精确定位，视觉定位可以通过3d相机完成，由于这里的定位精度要求高，所以需要高精度的3d相机，而高精度的3d相机通常价格昂贵，往往是普通工业相机(即2d相机)的几十倍上百倍，而且高精度的3d相机往往视野范围有限，故而亟待提出一种基于2d相机的机器人自动加注方法来解决上述问题。

技术实现思路

1、本发明的目的在于：为了解决上述问题，而提出的基于2d相机的机器人自动加注方法。

2、为了实现上述目的，本发明采用了如下技术方案：

3、基于2d相机的机器人自动加注方法，所述基于2d相机的机器人自动加注方法所使用的装置包括有机器人、加注枪、容器与相机，所述加注枪与相机均固定安装在机器人输出端，且加注枪与相机呈竖直分布；

4、所述基于2d相机的机器人自动加注方法包括有以下步骤：

5、s1、相机内参标定，初始状态下，得到相机的内参矩阵k和畸变系数，相机与标靶的相对位置随机，此时计算出相机与标记物的3d姿态，此时3d姿态精度一般；

6、s2、手眼标定，根据第一步的3d姿态调整机器人姿态，使得相机初步垂直与标记物平面且相机到标记物平面的距离为z；

7、s3、精细调整，机器人工具末端姿态，使得加注枪准确对准加注口，此作为校准位置，进一步使得相机初步垂直与标记物平面且相机到标记物平面的距离为z，此时相机与标记物平面基本垂直，且距离z的精度达到0.2mm左右；

8、s4、校准位置，通过识别标记物的特征，分别计算出粗对准和精对准时相机相对于标记物的姿态，在应用辅助工具使得加注枪精确对准加注口，由于此时距离z误差(1mm)相对于距离z本身(300mm)，基本可以忽略不计，通过仿射矩阵计算出加注口xy方向的位置关系，此时xy方向的精度达到几十微米的精度，z方向的精度仍然为第三步z方向的精度；

9、s5、加注作业运行，由机器人带动加注枪插入加注口，开始加注作业。

10、优选地，所述计算相机与标记物的3d姿态的方法包括以下步骤：

11、标记物在世界坐标系下的姿态机器人末端工具世界坐标系下的姿态

12、加注口在标记物坐标系下的姿态加注枪在相机坐标系下的姿态相机在工具坐标系下的姿态相机在标记物坐标系下的姿态加注枪相对于加注口的姿态

13、其中是不变的，因为标记物与加注口是由机械安装固定，相机和加注枪都由机械安装固定在机器人工具末端，因此和也是不变的；

14、由于agv的定位误差，每次agv达到加油位置后，agv相对于加注口都有较大的误差，因此每次

15、调整机器人末端工具的姿态ttool，使得加注枪相对于加注口重合，即即为单位矩阵；

16、此时相机在标记物坐标系中的姿态就可以计算出机器人需要调整的工具末端姿态ttool，使得加注枪相对于加注口的姿态机器人调整之前相机相对于标记物的姿态为

17、

18、机器人调整之后相机和工具末端的姿态都有变化，它们分别记为cam′和tool′

19、相对于标记物的姿态为

20、

21、联立(e.q.1)和(e.q.2)

22、即

23、即

24、由于相机是固定在工具末端的，当机器人工具末端调整时，相机相对于工具末端的姿态并不会发生变化，因此即

25、

26、是在对准位置，机器人工具末端的姿态，控制机器人达到这个姿态后，加注枪即可以对准加注口；

27、当前机器人工具末端的姿态，由机器人读取；

28、相机在机器人工具末端的姿态，即手眼矩阵，事先标定获取；

29、当前标记物相对于相机的姿态，由pnp方法求得；

30、加注枪对准加注口时，相机相对于标记物的姿态，由事先校准过程得到；

31、加注枪对准加注口时，相机相对于机器人工具末端的姿态，即手眼矩阵

32、优选地，所述粗对准时相机相对于标记物的姿态它是三维坐标系下的4*4齐次矩阵，计算的方法为：

33、a).识别标记物的特征点，标记物间隔的物理尺寸为k；

34、b).按照识别的顺序，以标记物平面为z＝0平面，计算特征点的世界坐标为p0，0＝(0，0，0),p0，1＝(0，k*1，0),pi，j＝(k*i，k*j，0),...；

35、c).特征点p0，0，...pi，j在图像中的像素坐标分别为

36、p0，0＝(u0，v0)，p0，1＝(u0，v1)，...，pi，j＝(ui，vj)；

37、d).像素坐标与世界坐标之间的转换关系为

38、

39、

40、其中h＝λk·[r1 r2 t]    (e.q.5)

41、这个h矩阵就是单应矩阵，它描述了特征点的世界坐标与像素坐标之间的关系。

42、根据已知量可以求出单应矩阵h；

43、e).单应矩阵h的求法

44、单应矩阵h由于没有尺度，因此只有8个未知数

45、

46、根据式(e.q.5)，一个pi，j和pi，j的对应关系可以有2个约束，因此最少4个特征点就可以解出来单应矩阵h，通常特征点会多于4个，这时候应用最小二乘法解出h矩阵；

47、根据(e.q.5),由于相机内参矩阵k已知，就可以求到[r1 r2 t]，其中r3＝r1×r2

48、求到r＝[r1 r2 r3]和t，再对r矩阵进行奇异值分解r＝u∑v，其正交投影r＝uv即可保证r为正交矩阵。这样便通过单应矩阵h求到了

49、的初值；

50、再通过非线性优化方法

51、

52、求到更精确的

53、优选地，所述精对准时相机相对于标记物的姿态它的计算方法为：

54、a).识别标记物的特征点，标记物间隔的物理尺寸为k；

55、b).按照识别的顺序，以标记物平面为z＝0平面，计算特征点的平面坐标为p0，0＝(0，0),p0，1＝(0，k*1),pi，j＝(k*i，k*j),...；

56、c).特征点p0，0，...pi，j在图像中的像素坐标分别为

57、p0，0＝(u0，v0)，p0，1＝(u0，v1)，...，pi，j＝(ui，vj)

58、为了减少噪声，我们取所有特征点的均值p作为中心点

59、

60、像素坐标p转化为世界坐标p，

61、精对准时相机相对于标记物的姿态

62、综上所述，由于采用了上述技术方案，本发明的有益效果是：

63、本技术中，初始状态下，相机与标靶的相对位置随机，此时通过pnp方法计算出相机与标记物的3d姿态，此时3d姿态精度一般，根据第一步的3d姿态调整机器人姿态，使得相机初步垂直与标记物平面且相机到标记物平面的距离为z，通过pnp方法计算出相机与标记物的3d姿态，此时3d姿态精度较高，并进一步使得相机初步垂直与标记物平面且相机到标记物平面的距离为z，此时相机与标记物平面基本垂直，且距离z的精度较高，可以达到0.2mm左右，最后由机器人带动加注枪插入加注口，开始加注作业，且这种基于标记的视觉定位系统成本低且性能可靠，在1米～几十米范围内可以提供毫米级到厘米级的定位精度以此在加注过程中，可通过普通相机替代了传统的3d相机，从而有效的降低了设备成本的同时保证定位精度。