[发明专利]一种机械臂工具坐标的自动标定方法及系统、存储介质

权利要求书

1.一种机械臂工具坐标的自动标定方法，其步骤包括：

步骤S100以第一机械臂作为基准，进行工具坐标系的标定；

步骤S200当标定后的第一机械臂操控末端第一工具能夹取到标定物时，通过第一机械臂上手眼标定过的相机识别设置在标定物旁的Mark，以将此时抓取位姿转换至Mark坐标系下获取初始示教点位；

步骤S300 发送初始示教点位至第二机械臂，当判断位姿差异D超出阈值时，示教调整第二机械臂进行补偿，以完成其工具坐标系的标定；其中

D = -

表示第一机械臂抓取姿态到第一机械臂基坐标系下的齐次矩阵，为第一台机械臂的抓取位姿；表示第二机械臂抓取姿态到第二机械臂基坐标系下的齐次矩阵，为第二台机械臂的抓取位姿；

其中示教调整第二机械臂进行补偿的步骤包括：

步骤S310示教第二机械臂移动到符合抓取精度要求的位姿下，并反乘回视觉识别的参数* ，以获取第二机械臂的第二工具到末端的齐次矩阵：

 = **；

其中，表示第二机械臂抓取姿态到第二机械臂基坐标系下的齐次矩阵，为第一机械臂中抓取位姿到Mark的齐次逆矩阵，为Mark到相机坐标系下的齐次逆矩阵，为相机坐标系到第一机械臂第一工具的齐次逆矩阵；

步骤S320通过关节角度，去正解求得此时第二机械臂末端到第二机械臂基坐标系下的位姿，计算第二工具到第二机械臂末端的齐次矩阵：

=

其中：为第一机械臂末端到第一机械臂基坐标系的齐次逆矩阵。

2.根据权利要求1所述的机械臂工具坐标的自动标定方法，其中步骤S100中所述工具坐标系的标定步骤包括：

步骤S110操控第一机械臂夹持标定针以数种不同姿态点位去碰触标定点，以建立第一工具到机械臂基坐标系的齐次矩阵=*，其中表示第一工具到第一机械臂末端的齐次矩阵，表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵；

步骤S120展开步骤S110中的矩阵，依据各姿态点位联立方程组，以根据最小二乘法计算：多次计算旋转

* = ；

获取第一工具到第一机械臂末端的位置差，其中~表示当前第一工具在第一机械臂基坐标系下各姿态点的位置向量，表示当前第一机械臂末端在基坐标系下的位置向量，表示当前第一工具到第一机械臂末端的位置向量，表示当前第一机械臂末端到各位姿点基坐标系的旋转矩阵；

步骤S130以步骤S110中的一个姿态点位为起点，调整第一机械臂在X、Y轴方向进行坐标偏移，以计算第一工具到第一机械臂末端的X、Y、Z方向向量，从而获取第一工具到第一机械臂末端的偏差量关系，以完成工具坐标系的标定。

3.根据权利要求1所述的机械臂工具坐标的自动标定方法，其中步骤S100中所述工具坐标系的标定步骤包括：

步骤S140操控第一机械臂夹持标定针去碰触标定点，并记为；

步骤S150操控第一机械臂末端沿Z轴移动一段距离，以标记出点；

步骤S160以为圆心，r为半径建立标定环，将标定环均分为数个象限，均转动预设角度，以在各象限所处环上获取数个点位位置并分别计算出对应姿态点位；

步骤S170操控第一机械臂夹持标定针以步骤S160所获姿态点位去碰触标定点，以建立第一工具到第一机械臂基坐标系的齐次矩阵=*，其中表示第一工具到第一机械臂末端的齐次矩阵，表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵；

步骤S180根据各姿态点位联立方程组，以根据最小二乘法计算：

* = ；

获取第一工具到第一机械臂末端的位置差，其中~表示当前第一工具在第一机械臂基坐标系下各姿态点的位置向量，表示当前第一机械臂末端在基坐标系下的位置向量，表示当前第一工具到第一机械臂末端的位置向量，表示当前第一机械臂末端到各位姿点基坐标系的旋转矩阵；

步骤S190以步骤S160中的一个姿态点位为起点，调整第一机械臂在X、Y轴方向进行坐标偏移，以计算第一工具到第一机械臂末端的X、Y、Z方向向量，从而获取第一工具到第一机械臂末端的偏差量关系，以完成工具坐标系的标定。

4.根据权利要求1所述的机械臂工具坐标的自动标定方法，其中所述Mark与标定物的相对位置关系固定。