[发明专利]基于机器视觉的机器人自动锁付控制系统及方法

权利要求书

1.基于机器视觉的机器人自动锁付控制系统，其特征在于，所述系统包括机器人、控制器、送料单元、相机以及锁付装置；

所述机器人为示教机器人，具有示教盒；

所述锁付装置安装在机器人末端，包括进给气缸、电动螺丝刀、螺栓吸附设备以及力矩传感器，所述螺栓吸附设备安装在所述电动螺丝刀的末端，螺栓吸附设备在设置的定点吸附位置处吸附待使用螺栓；所述进给气缸接收所述控制器发来的位置控制指令，依据所述位置控制指令将所述电动螺丝刀移动至指定位置；所述电动螺丝刀在所述控制器的控制下启动或者停止，所述电动螺丝刀在启动时将所述待使用螺栓拧入待锁付螺纹孔；所述力矩传感器实时获取所述电动螺丝刀的实时力矩，并将其回传至所述控制器；

所述送料单元对螺栓进行送料，将螺栓送至所述设置的定点吸附位置处；

所述相机获取待锁付螺纹孔图像，将所述待锁付螺纹孔图像发送至所述控制器；

所述控制器，建立机器人末端坐标系和机器人基坐标系，获取机器人基坐标系至机器人末端坐标系的转换关系；获取所述锁付装置位置，对所述锁付装置进行工具坐标系标定，获取工具末端位置；获取所述相机的参数，对所述相机进行内参标定，建立相机坐标系，计算所述相机坐标系与机器人基坐标系的转换关系；获取所述相机采集的待锁付螺纹孔图像，计算待锁付螺纹孔图像的像素坐标系下的待锁付螺纹孔中心位置，转换为相机坐标系下待锁付螺纹孔中心位置，利用所述相机坐标系与机器人基坐标系的转换关系，转换获得在机器人基坐标系下待锁付螺纹孔中心位置；

所述控制器，以当前位置为起点，以所述待锁付螺纹孔在机器人基坐标系下的位置为终点，进行路径规划，驱动所述机器人移动至所述待锁付螺纹孔处；所述控制器向所述锁付装置发送位置控制指令，所述位置控制指令的指定位置为所述待锁付螺纹孔位置；所述电动螺丝刀达到所述指定位置后，所述控制器控制所述电动螺丝刀启动，获取实时转动圈数和实时力矩，并设定转动圈数以及力矩设定值，当实时转动圈数达到所述设定转动圈数且实时力矩达到所述力矩设定值，控制所述电动螺丝刀停止；

所述控制器采用如下方法：

步骤1：以所述锁付装置作为工具，建立工具坐标系{T}，所述工具坐标系{T}为：以所述锁付装置的末端点的中心为原点，以所述锁付装置的末端中心轴线为z轴，右手定则设定其他轴；

针对所述机器人建立机器人末端坐标系{E}以及机器人基坐标系{B}；

采用六点标定法对所述工具坐标系{T}进行标定，得到工具位姿；

获取机器人的末端位姿，利用机器人末端位姿与所述工具位姿相乘得到机器人工具末端位姿，从而获取工具末端位置；

所述工具末端位置即为电动螺丝刀末端位置；

在机器人末端坐标系中选取六个标定点，前四个标定点用于标定工具参数的位置向量，前四个标定点之间各差90度且不能在一个平面上；后三个标定点标定工具坐标系的姿态矩阵，保持第4个标定点的姿态不变，采用Z/X方向标定；其中工具中心点位置标定采用线性最小二乘法的矩阵形式进行求解，工具坐标系的姿态采用向量和矩阵的基本运算进行求解；

机器人末端坐标系{E}相对于机器人基坐标系{B}的变换关系为工具坐标系{T}相对于机器人末端坐标系{E}的变换关系为工具坐标系{T}相对于机器人基坐标系{B}的变换关系为三者的转换关系为：

是由机器人的正解方程得到，由旋转矩阵和位置矢量^Bp_E0组成：

式中矢量分别描述了机器人末端坐标下X、Y、Z轴上的单位向量，矢量[n_x,n_y,n_z]^T、[o_x,o_y,o_z]^T、[a_x,a_y,a_z]^T分别描述了机器人末端坐标系下X、Y、Z轴上的单位向量在机器人基坐标系下的坐标表示：

对式(1－1)以各自的分块形式展开，得到式(1－4)：

式中分别为机器人末端坐标系前四个标定点的旋转矩阵，^Bp_iEo分别为机器人末端坐标系前四个标定点的位置矢量；为工具的旋转矩阵，^Ep_t为工具的位置矢量；分别为工具坐标系前四个标定点的旋转矩阵，^Bp_t为工具坐标系在机器人基坐标系下的位置矢量；将式(1－4)进行变形得到计算工具中心点的通式：

式中^Ep_x、^Ep_y、^Ep_z为工具的位置；^Bp_Ex、^Bp_Ey、^Bp_Ez为机器人末端坐标系在机器人基坐标系的位置；^Bp_tx、^Bp_ty、^Bp_tz为工具坐标系在机器人基坐标系下的位置；

因在前四个标定点不同位姿下工具坐标系在机器人基坐标系的位置不变，即^Bp_tx、^Bp_ty和^Bp_tz为定值；结合式(1－5)，将第二点的位姿数据减去第一点的位姿数据有如下关系：

由式(1－6)展开得：

依次类推可得：

计算出工具坐标系的位置，需要进一步标定计算工具末端的姿态；

工具末端的姿态采用Z/X方向标定，过程中保持工具坐标系的姿态不变；所述示教机器人从第四个标定点沿+X方向至少移动250mm后作为第五个标定点；然后回到第四个标定点再将示教机器人沿+Z方向移动至少250mm作为第六个标定点，得到工具坐标系{T}的X轴轴向向量：

同理，得到工具坐标系{T}的Z轴轴向向量：

Y轴轴向向量由右手定则可得：

Y＝Z×X (1－11)

再对Z＝X×Y进行计算，以保证坐标系矢量的正交性；得到每个轴的轴向向量之后，对其进行单位化操作，得到工具坐标系{T}相对于机器人基坐标系{B}的姿态，左乘机器人末端坐标系第4个标定点的旋转矩阵的逆，求出工具坐标系的旋转矩阵，得到工具位姿；根据机器人末端位姿乘以工具位姿得到机器人工具末端位姿，由此可得工具末端的位置；

步骤2：获取所述相机的参数，对所述相机进行内参标定，建立相机坐标系，计算所述相机坐标系与机器人基坐标系的转换关系；

具体为：

通过相机采集标定板；标定过程中，要采集9～16张图片以保证标定板数量；标定板的位置覆盖相机视野中的所有位置，完成后得到焦距、畸变系数、中心点x坐标、中心点y坐标；

得到相机坐标系与机器人基坐标系的关系为：

其中，(O_o,X_o,Y_o,Z_o)为相机坐标系，(O_W,X_W,Y_W,Z_W)为世界坐标系，R为相机坐标系到世界坐标系的旋转矩阵，T为相机坐标系到世界坐标系的平移矩阵；像素坐标系与机器人基坐标系关系为：

其中，(o,u,v)为成像坐标系，f是焦距；(u₀,v₀)为光轴与图像平面的交点；dx、dy分别表示每一个像素在u轴和v轴方向的物理尺寸，Z_c是空间点P在相机坐标系下的Z方向距离；

步骤3：获取所述相机采集的待锁付螺纹孔图像，将所述待锁付螺纹孔图像与预先建立的螺纹孔形状特征模板库中的螺纹孔形状特征模板图像采用高斯图像金字塔分层搜索策略进行匹配，在所述待锁付螺纹孔图像中确定匹配的螺纹孔形状，并计算得到待锁付螺纹孔图像的像素坐标系下的待锁付螺纹孔中心位置；

所述预先建立的螺纹孔形状特征模板库中存有螺纹孔形状特征模板图像，所述螺纹孔形状特征模板图像为不同种类螺纹孔形状特征的模板图像；

步骤4：根据所述相机坐标系与所述机器人基坐标系的转换关系，得到机器人基坐标系下的待锁付螺纹孔中心位置；

步骤5、以当前位置为起点，以所述机器人基坐标系下待锁付螺纹孔中心位置为终点，进行路径规划，驱动所述机器人移动至所述待锁付螺纹孔处；

所述机器人移动至所述待锁付螺纹孔处之后，向所述锁付装置发送位置控制指令，所述位置控制指令的指定位置为所述机器人基坐标系下待锁付螺纹孔位置；

通过控制已吸附待使用螺栓的所述电动螺丝刀，控制所述工具末端位置达到所述指定位置后，控制所述电动螺丝刀启动，获取实时转动圈数和实时力矩，并设定转动圈数以及力矩设定值，当实时转动圈数达到所述设定转动圈数且实时力矩达到所述力矩设定值，控制所述电动螺丝刀停止，发出锁付完成信号。