CN116423526A - An automatic calibration method, system, and storage medium for tool coordinates of a mechanical arm - Google Patents

Abstract

The invention provides an automatic calibration method and system for mechanical arm tool coordinates, and a storage medium, wherein the method comprises the following steps: step S100, calibrating a tool coordinate system by taking a first mechanical arm as a reference; step S200, when a first tool at the control tail end of the calibrated first mechanical arm can clamp a calibrated object, recognizing Mark arranged beside the calibrated object through a camera calibrated by a hand and an eye on the first mechanical arm, so as to convert the capturing pose at the moment into a Mark coordinate system to obtain an initial teaching point; step S300 is to send an initial teaching point position to the second mechanical arm, and when the pose difference is judged to exceed the threshold value, the teaching is to adjust the second mechanical arm to compensate, so that the calibration of a tool coordinate system is completed. Therefore, the teaching points between the mechanical arms can be multiplexed, and the calibration process of the tool coordinates of the plurality of mechanical arms with the same configuration is accelerated.

Description

A method, system, and storage medium for automatic calibration of tool coordinates of a mechanical arm

technical field

The invention relates to robot calibration technology, in particular to an automatic calibration method, system and storage medium for tool coordinates of a mechanical arm.

Background technique

With the continuous improvement of the demand for industrial flexible production, robotic arms are widely used in flexible production lines. In order to meet the needs of production and processing technology, the robotic arm needs to be equipped with corresponding processing tools at the end. However, when it is necessary to set up When a flexible production line consists of multiple robotic arms of the same style and the same processing tools, it is necessary to assemble the robotic arms and processing tools separately and teach them separately, because each robotic arm and processing tool may be installed. There are machining or installation errors.

Therefore, even though the tool is the same and the robotic arm is the same, the difference between the center of the tool and the end of the robotic arm will also be different, resulting in that the teaching points between the robotic arms cannot be reused. The calibration of the tool coordinate system is time-consuming and labor-intensive, and is not economical.

Contents of the invention

The main purpose of the present invention is to provide an automatic calibration method, system, and storage medium for the tool coordinates of a manipulator, so as to realize the reuse of the points taught between the manipulators and accelerate the calibration process of the tool coordinates of multiple manipulators with the same configuration.

In order to achieve the above object, according to one aspect of the present invention, an automatic calibration method for tool coordinates of a mechanical arm is provided, the steps of which include:

Step S100 uses the first mechanical arm as a reference to calibrate the tool coordinate system;

Step S200: When the first tool at the control end of the calibrated first robotic arm can grasp the calibration object, use the hand-eye calibrated camera on the first robotic arm to identify the Mark set next to the calibration object, so as to capture the position at this time. Convert the pose to the Mark coordinate system to obtain the initial teaching point;

Step S300 Send the initial teaching point to the second robotic arm, and when it is judged that the pose difference exceeds the threshold, teach and adjust the second robotic arm for compensation, so as to complete the calibration of its tool coordinate system.

In a possible preferred implementation manner, wherein the step of calibrating the tool coordinate system in step S100 includes:

，其中/>

表示第一工具到第一机械臂末端的齐次矩阵，/>

表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵；Step S110 controls the first robot arm to hold the calibration needle to touch the calibration point in several different postures, so as to establish a homogeneous matrix from the first tool to the base coordinate system of the robot arm

, where />

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator;

Step S120 expands the matrix in step S110, according to the simultaneous equations of each attitude point, to calculate according to the least square method:

;

;

表示当前第一工具在第一机械臂基坐标系下的位置向量，/>

表示当前第一机械臂末端在基坐标系下的位置向量，

表示当前第一工具到第一机械臂末端的位置向量，/>

表示当前第一机械臂末端到基坐标系的旋转矩阵；Obtain the position difference from the first tool to the end of the first mechanical arm, where

Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system,

Indicates the position vector from the current first tool to the end of the first robotic arm, />

Indicates the rotation matrix from the end of the first robotic arm to the base coordinate system;

Step S130 takes a posture point in step S110 as a starting point, and adjusts the coordinate offset of the first mechanical arm in the X and Y axis directions to calculate the X, Y, and Z direction vectors from the first tool to the end of the first mechanical arm, In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system.

In a possible preferred implementation manner, wherein the step of calibrating the tool coordinate system in step S100 includes:

；Step S140 controls the first mechanical arm to clamp the calibration needle to touch the calibration point, and denote as

;

；Step S150 controls the end of the first mechanical arm to move a certain distance along the Z axis to mark a point

;

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

is the center of the circle, and r is the radius to establish a calibration ring, divide the calibration ring into several quadrants, and rotate the preset angle to obtain several point positions on the ring where each quadrant is located and calculate the corresponding attitude points respectively;

，其中/>

表示第一工具到第一机械臂末端的齐次矩阵，/>

表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵；Step S170 controls the first robot arm to hold the calibration needle to touch the calibration point at the attitude point obtained in step S160, so as to establish a homogeneous matrix from the first tool to the base coordinate system of the first robot arm

, where />

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator;

Step S180 calculates according to the method of least squares according to the simultaneous equations of each attitude point:

;

;

表示当前第一机械臂末端在基坐标系下的位置向量，

表示当前第一工具到第一机械臂末端的位置向量，/>

表示当前第一机械臂末端到基坐标系的旋转矩阵；Obtain the position difference from the first tool to the end of the first mechanical arm, where Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system,

Indicates the position vector from the current first tool to the end of the first robotic arm, />

Indicates the rotation matrix from the end of the first robotic arm to the base coordinate system;

Step S190 takes a posture point in step S160 as a starting point, and adjusts the coordinate offset of the first mechanical arm in the X and Y axis directions to calculate the X, Y, and Z direction vectors from the first tool to the end of the first mechanical arm, In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system.

In a possible preferred implementation manner, the step of teaching and adjusting the second mechanical arm for compensation in step S300 includes:

，以获取第二机械臂的第二工具到末端的齐次矩阵/>

：Step S310 teaches the second robotic arm to move to a pose that meets the grasping accuracy requirements, and inversely multiplies it back to the visual recognition parameters

, to get the homogeneous matrix from the second tool to the end of the second robot arm />

:

为第一机械臂抓取姿态到第一机械臂基座标系下的齐次矩阵，

为第一机械臂中抓取位姿到Mark的齐次逆矩阵，/>为Mark到相机坐标系下的齐次逆矩阵，/>

为相机坐标系到第一机械臂第一工具的齐次逆矩阵；in,

is the homogeneous matrix from the grasping pose of the first manipulator to the base frame of the first manipulator,

It is the homogeneous inverse matrix from the grab pose to Mark in the first robotic arm, /> is the homogeneous inverse matrix from Mark to camera coordinate system, />

is the homogeneous inverse matrix from the camera coordinate system to the first tool of the first robotic arm;

：Step S320 Use the joint angle to find the pose of the base coordinate system from the end of the second manipulator to the base coordinate system of the second manipulator at this time, and calculate the homogeneous matrix from the second tool to the end of the second manipulator

:

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

is the homogeneous inverse matrix from the end of the first manipulator to the base coordinate system of the first manipulator.

In a possible preferred implementation manner, the relative positional relationship between the Mark and the calibration object is fixed.

In order to achieve the above object, corresponding to the above method, the second aspect of the present invention also provides an automatic calibration system for the tool coordinates of the mechanical arm, which includes:

The storage unit is used to store the program including the steps of the automatic calibration method of the tool coordinates of the robotic arm as described above, for the control unit, the transmission unit, and the processing unit to call and execute in due course;

a control unit, configured to control the first mechanical arm and its first tool for teaching movement;

The processing unit is used to record the teaching parameters of the first mechanical arm, so as to calculate the deviation relationship between the first tool and the end of the first mechanical arm, so as to complete the calibration of the tool coordinate system;

The control unit is also used to control the calibrated first mechanical arm to control the first tool at the end to grab the calibration object, and at the same time control the camera on the first mechanical arm to recognize the Mark set next to the calibration object, so that the processing unit can grasp the calibration object at this time. Take the pose and convert it to the Mark coordinate system to obtain the initial teaching point;

The transmission unit is used to send the initial teaching point to the second robot arm, so that after the control unit controls the robot arm to move to the corresponding point, the processing unit judges whether the pose difference exceeds the threshold. When the threshold is exceeded, the processing unit The second mechanical arm performs compensation calculation to complete the calibration of its tool coordinate system.

In a possible preferred implementation, the step of calculating the compensation of the second mechanical arm includes:

a control unit, configured to control the movement of the second mechanical arm to a pose that meets the grasping accuracy requirements;

The processing unit is used to inversely multiply the parameters of the visual recognition in this pose to obtain the homogeneous matrix from the second tool to the end of the second robotic arm; From the end to the pose in the base coordinate system of the second manipulator, calculate the homogeneous matrix from the second tool to the end of the second manipulator.

In order to achieve the above object, corresponding to the above method, the third aspect of the present invention also provides a computer-readable storage medium on which a computer program is stored, wherein when the computer program is executed by a processor, any of the above items can be realized. The steps of the method for automatically calibrating the tool coordinates of the mechanical arm.

Through the automatic calibration method, system and storage medium of the tool coordinates of the manipulator provided by the present invention, after the calibration of the tool coordinate system, the visual recognition technology can be used to record the grasping pose once, and the subsequent grasping pose can be passed according to the grasping pose. Compensation by way of teaching can complete the calibration of the tool coordinates of multiple manipulators with the same configuration, so as to realize the reuse of the teaching points between the manipulators, speed up the calibration process of the tool coordinates of multiple manipulators with the same configuration, and improve the overall Calibration efficiency, and significantly reduce the difficulty of calibration operations for on-site personnel.

Description of drawings

The accompanying drawings constituting a part of this application are used to provide further understanding of the present invention, and the schematic embodiments and descriptions of the present invention are used to explain the present invention, and do not constitute an improper limitation of the present invention. In the attached picture:

Fig. 1 is a schematic diagram of the steps of the automatic calibration method of the mechanical arm tool coordinates of the present invention;

Fig. 2 is the logical flowchart of the automatic calibration method of the tool coordinates of the mechanical arm of the present invention;

3 is a schematic diagram of a mechanical arm, an end tool, a calibration object, a camera and a Mark in the automatic calibration method of the mechanical arm tool coordinates of the present invention;

Fig. 4 is a schematic diagram of the state when the second mechanical arm adopts the calibration of the first mechanical arm tool coordinate system to grasp the calibration object in the automatic calibration method of the mechanical arm tool coordinates of the present invention;

5-6 are schematic diagrams of establishing a calibration ring to obtain attitude points in the automatic calibration method of the robot arm tool coordinates of the present invention;

Fig. 7 is a schematic structural diagram of the automatic calibration system for tool coordinates of the manipulator of the present invention.

Explanation of reference signs

Mechanical arm 1, gripper tool 2, camera 3, calibration object 4, Mark (mark) 5, calibration pin 6.

Detailed ways

In order to enable those skilled in the art to better understand the technical solutions of the present invention, the specific technical solutions of the present invention will be clearly and completely described below in conjunction with examples, so as to help those skilled in the art further understand the present invention. Apparently, the embodiments described in this case are only some embodiments of the present invention, not all embodiments. It should be noted that, for those skilled in the art, the embodiments in the present application and the features in the embodiments can be combined without departing from the concept of the present invention and without conflicting with each other. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without creative efforts shall belong to the disclosure and protection scope of the present invention.

In addition, the terms "first", "second", "S100", "S200" and the like in the description and claims of the present invention and the accompanying drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or priority. It should be understood that the data so used may be interchanged under appropriate circumstances such that the embodiments of the invention described herein can be practiced in sequences other than those described herein. Meanwhile, the terms "including" and "having" and any variations thereof in the present invention are intended to cover non-exclusive inclusion. Unless otherwise clearly specified and limited, the terms "arrangement", "arrangement", "installation", "connection" and "connection" should be interpreted in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integrated Ground connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediary, and it can be the internal communication of two components. Those of ordinary skill in the art can understand the specific meanings of the above terms in this case according to specific situations and in combination with the prior art.

Please refer to Fig. 1 to Fig. 6, in order to realize the reusability of the teaching points between the manipulators, and accelerate the calibration process of the coordinates of the tool coordinates of the plural manipulators with the same configuration, the automatic calibration method of the tool coordinates of the manipulators provided by the present invention , whose steps include:

Step S100 uses the first robotic arm as a reference to calibrate the tool coordinate system.

Specifically, in an example, the calibration step of the tool coordinate system includes:

Step S110 controls the first mechanical arm to hold the calibration needle to touch the same calibration point in space with four different attitude points. At the same time, in order to ensure the accuracy of the touch point, the calibration point can be regarded as the needle tip of a needle-shaped object , as shown in Figure 5, the specific formula for a single point can be as follows:

,

,

表示第一工具到第一机械臂末端的齐次矩阵， />

表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵。In this way, the homogeneous matrix from the first tool to the base coordinate system of the manipulator is established, where

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator.

Step S120 After expanding the matrix in step S110, the following mathematical relationship can be obtained:

,

,

表示当前第一工具在第一机械臂基坐标系下的位置向量， />

表示当前第一机械臂末端在基坐标系下的位置向量， />

表示当前第一工具到第一机械臂末端的位置向量， />

表示当前第一机械臂末端到基坐标系的旋转矩阵。in

Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system, />

Indicates the current position vector from the first tool to the end of the first robot arm, />

Indicates the rotation matrix of the current end of the first robotic arm to the base coordinate system.

Further, the expression form of the four points can finally be displayed as follows:

,

,

,

,

,

,

。

.

可表达为：Among them: because the postures of the four points are different, but the final positions are the same. Therefore, the left side of the equation is equal, and the right side is different. Therefore, the calculation relationship can be obtained by the simultaneous equations, and the final

Can be expressed as:

。

.

Since the coordinates of the equation are in the brackets of a 9*3 matrix, and the brackets on the right side of the equation are a 9*1 matrix, the coefficient matrix is not a square matrix and cannot be directly inverted. At this time, the first tool can be calculated according to the least square method to The position difference of the end of the first robot arm.

Then you can start to calculate the attitude difference from the tool to the end of the robot arm. At this time, two points are needed, one point is used to determine the X axis and the other is used to determine the Y axis.

Step S130 takes a posture point in step S110 as the starting point, for example, taking the last point among the four points as an example, and manipulates the first mechanical arm to move along the X-axis direction in a manner that does not change the posture, so as to obtain a point A, and at the same time The principle is to manipulate the first robotic arm to move along the Y-axis direction in a manner that does not change the posture, and obtain a point B.

At this time, the X direction, Y direction, and Z direction vectors can be expressed as:

,

,

，

,

，

,

In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system.

In addition, in order to verify the rationality of the data, as shown in Figure 4, the gripper of the robotic arm can be manipulated to grab the calibration object. If the grabbing requirements are met, it is considered reasonable, and the first robotic arm grabbing at this time is recorded. Attitude and offset values. If it is not satisfied, the operations of steps S100-S130 need to be performed again.

On the other hand, in a preferred embodiment, when the Z-axis direction of the first tool is consistent with the Z-axis direction of the end of the first mechanical arm, in order to reduce the amount of teaching operation during TCP calibration in the early stage, the following operations can be automatically generated point, the steps include:

。Step S140 controls the first mechanical arm to move above the needle tip of the calibration point, so that the first mechanical arm holds the calibration needle to touch the calibration point, and records the point at this time as

.

。Step S150 controls the end of the first mechanical arm to move a certain distance d along the Z axis to mark a point

.

为圆心，r为半径建立标定环，将标定环均分为数个象限，均转动预设角度，以在各象限所处环上获取数个点位位置并分别计算出对应姿态点位。Step S160 is shown in Figure 6, with

is the center of the circle, and r is the radius to establish a calibration ring, divide the calibration ring into several quadrants, and rotate the preset angles to obtain several point positions on the ring where each quadrant is located and calculate the corresponding attitude points.

点的X或Y值做增减r的操作。这样的目的是仅改变Y或者Y的值，从机械臂末端逆解到关节值，在轨迹规划的时候相对稳定。Specifically, for example, the circular surface is divided into four quadrants at this time, and four angles are respectively taken, which are 0°, 90°, 180°, and 270°. understandable only for

The X or Y value of the point is increased or decreased by r. The purpose of this is to only change the value of Y or Y, from the end of the manipulator to the joint value, which is relatively stable during trajectory planning.

The first of which automatically generates points:

,

,

,Right now:

,

分别为第一个生成点的X、Y、Z坐标值， />

,/>

,/>

分别为圆心 />

的X、Y、Z坐标值。in

are the X, Y, and Z coordinate values of the first generated point respectively, />

,/>

,/>

respectively the center of the circle />

The X, Y, Z coordinate values of .

Second point:

。

.

The third point:

。

.

Fourth point:

。

.

Further, at this time, it is necessary to calculate the attitude value of the generated point from the generated point position value.

The unified specific steps can be entered as follows:

,

,

,

,

,

,

分别为： />

、/>

、/>

、/>

，可分别带入式中。 />

，/>

，/>

分别为新生成点位的的rx，ry，rz姿态值。in

They are: />

, />

, />

, />

, which can be brought into the formula respectively. />

, />

, />

They are the rx, ry, and rz attitude values of the newly generated points respectively.

=/>

*

，其中 />

表示第一工具到第一机械臂末端的齐次矩阵， />

表示第一机械臂末端到第一机械臂基坐标系的齐次矩阵；In step S170, when there are four attitude points, control the first mechanical arm to clamp the calibration needle to touch the calibration points with the attitude points obtained in step S160, so as to establish alignment between the first tool and the base coordinate system of the first mechanical arm. sub-matrix

=/>

*

, where />

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator;

Step S180 calculates according to the method of least squares according to the simultaneous equations of each attitude point:

,

,

表示当前第一工具在第一机械臂基坐标系下的位置向量， />

表示当前第一机械臂末端在基坐标系下的位置向量， />

表示当前第一工具到第一机械臂末端的位置向量， />

表示当前第一机械臂末端到基坐标系的旋转矩阵；Obtain the position difference from the first tool to the end of the first mechanical arm, where

Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system, />

Indicates the current position vector from the first tool to the end of the first robot arm, />

Indicates the rotation matrix from the end of the first robotic arm to the base coordinate system;

Step S190 takes a posture point in step S160 as a starting point, and adjusts the coordinate offset of the first mechanical arm in the X and Y axis directions to calculate the X, Y, and Z direction vectors from the first tool to the end of the first mechanical arm, In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system.

It can be seen that, through the method of the above example, the required points can be automatically generated through the mathematical model, effectively reducing the teaching points.

Furthermore, whether it is automatic point generation or manual teaching, for other subsequent robotic arms and their tools with the same configuration, it is necessary to touch four points from the tip of the needle for TCP, and two for the calibration direction. This poses a great challenge to the operational requirements of on-site operators, and the implementation among multiple devices is too cumbersome. Therefore, in order to solve this problem, in this example, the method of teaching 6 points or 3 points is changed to teaching 1 points (one-step teaching). In addition, high-precision point-to-point forms are no longer required.

For this reason, visual calibration technology is introduced in this case, a camera is installed on the end of the robotic arm, and a recognition mark is placed next to the calibration object to be grasped. The purpose of introducing vision is to have a common reference, that is, Mark's coordinate system. In addition, the relative positional relationship between the Mark and the grasped object will not change.

Step S200: When the first tool at the control end of the calibrated first robotic arm can grasp the calibration object, use the hand-eye calibrated camera on the first robotic arm to identify the Mark set next to the calibration object, so as to capture the position at this time. Transform the pose to the Mark coordinate system to obtain the initial teaching point.

Specifically, the process of camera hand-eye calibration includes: first obtaining the relationship between the camera on the end of the mechanical arm and the end of the mechanical arm. It can be calibrated in the form of teaching ten points based on the least square method and shooting the marker Mark at the same time. Specifically, it can be shown in the following formula:

,

,

...

,

,

表示机械臂末端到基座标系下的齐次矩阵， />

表示相机到机械臂末端的齐次矩阵， />

表示标志物Mark到机械臂基座标系的齐次矩阵。in:

Indicates the homogeneous matrix under the coordinate system from the end of the manipulator to the base, />

represents the homogeneous matrix from the camera to the end of the arm, />

Indicates the homogeneous matrix of the coordinate system from the marker Mark to the base of the manipulator.

By combining 10 sets of equations, the hand-eye matrix can be obtained for subsequent visual tool coordinate system compensation.

After that, the pose will be grasped for a coordinate system transformation operation. Make it transfer to the Mark coordinate system. The specific expression can be as follows:

。

.

表示第一机械臂抓取姿态到第一机械臂基座标系下的齐次矩阵，

表示第一机械臂抓取姿态到Mark标志物座标系下的齐次矩阵， />

表示Mark标志物座标系到相机坐标系下的齐次矩阵， />

表示相机座标系到第一机械臂末端第一工具坐标系下的齐次矩阵， />

表示第一机械臂末端工具坐标系到第一机械臂末端坐标系到下的齐次矩阵。in

Indicates the homogeneous matrix from the grasping attitude of the first manipulator to the base frame of the first manipulator,

Indicates the homogeneous matrix from the grasping posture of the first robotic arm to the Mark marker coordinate system, />

Represents the homogeneous matrix from the Mark marker coordinate system to the camera coordinate system, />

Indicates the homogeneous matrix from the camera coordinate system to the first tool coordinate system at the end of the first robot arm, />

Represents the homogeneous matrix from the tool coordinate system at the end of the first manipulator to the coordinate system at the end of the first manipulator.

Step S300 Send the initial teaching point to the second robotic arm, and when it is judged that the pose difference exceeds the threshold, teach and adjust the second robotic arm for compensation, so as to complete the calibration of its tool coordinate system.

Specifically, due to the influence of errors in the processing or assembly of different robotic arms and their tools, the initial teaching point moved to by the second robotic arm may not meet the requirements for grasping accuracy. Of course, if it can be satisfied, the calibration of the second robotic arm is considered to be completed.

Therefore, in order to judge whether the difference threshold is met, if the grasping accuracy is not satisfied, at this time, the second robotic arm can be manually taught to make it run to a pose that meets the grasping accuracy requirements, and then compare The difference D between the poses of the two robotic arms. The specific mathematical expression can be as follows:

。

.

表示机第一械臂抓取姿态到第一机械臂基座标系下的齐次矩阵，为第一台机械臂的抓取位姿。 />

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

Denotes the homogeneous matrix from the grabbing pose of the first robotic arm to the base frame of the first robotic arm, which is the grabbing pose of the first robotic arm. />

Denotes the homogeneous matrix from the grabbing pose of the second robotic arm to the base frame of the second robotic arm, which is the grabbing pose of the second robotic arm.

At this time, if it is judged that D exceeds the threshold, the second mechanical arm needs to be compensated, and the steps include:

，以获取第二机械臂的第二工具到末端的齐次矩阵 />

：Step S310 teaches the second robotic arm to move to a pose that meets the grasping accuracy requirements, and inversely multiplies it back to the visual recognition parameters

, to get the homogeneous matrix from the second tool to the end of the second robot arm />

:

,

,

为第一机械臂抓取姿态到第一机械臂基座标系下的齐次矩阵，

为第一机械臂中抓取位姿到Mark的齐次逆矩阵， />

为Mark到相机坐标系下的齐次逆矩阵， />

为相机坐标系到第一机械臂第一工具的齐次逆矩阵；in,

is the homogeneous matrix from the grasping pose of the first manipulator to the base frame of the first manipulator,

is the homogeneous inverse matrix from the grasp pose to Mark in the first robotic arm, />

is the homogeneous inverse matrix from Mark to camera coordinate system, />

is the homogeneous inverse matrix from the camera coordinate system to the first tool of the first robotic arm;

：Step S320 At this time, since the pose information of the manipulator has been changed as described above, the old pose information of the manipulator can no longer be multiplied inversely. At this time, the positive solution is obtained by using the joint angle to obtain the position from the end of the second manipulator to the second end of the manipulator. The pose in the base coordinate system of the manipulator, calculate the homogeneous matrix from the second tool to the end of the second manipulator

:

,

,

为第一机械臂末端到第一机械臂基坐标系的齐次逆矩阵。最终得到的 />

即为第二机械臂的工具坐标系。in:

is the homogeneous inverse matrix from the end of the first manipulator to the base coordinate system of the first manipulator. end up with />

It is the tool coordinate system of the second robotic arm.

By repeating the above example steps, the tool coordinate system can be calibrated for other robotic arms and their tools with the same configuration.

Corresponding to the above method, please refer to Figure 7, the present invention also provides an automatic calibration system for the tool coordinates of the mechanical arm, which includes:

The storage unit is used to store the program including the steps of the automatic calibration method of the tool coordinates of the robotic arm as described above, for the control unit, the transmission unit, and the processing unit to call and execute in due course;

a control unit, configured to control the first mechanical arm and its first tool for teaching movement;

The processing unit is used to record the teaching parameters of the first mechanical arm, so as to calculate the deviation relationship between the first tool and the end of the first mechanical arm, so as to complete the calibration of the tool coordinate system;

The control unit is also used to control the calibrated first mechanical arm to control the first tool at the end to grab the calibration object, and at the same time control the camera on the first mechanical arm to recognize the Mark set next to the calibration object, so that the processing unit can grasp the calibration object at this time. Take the pose and convert it to the Mark coordinate system to obtain the initial teaching point;

The transmission unit is used to send the initial teaching point to the second robot arm, so that after the control unit controls the robot arm to move to the corresponding point, the processing unit judges whether the pose difference exceeds the threshold. When the threshold is exceeded, the processing unit The second mechanical arm performs compensation calculation to complete the calibration of its tool coordinate system.

Further, the step of calculating the compensation of the second mechanical arm includes:

a control unit, configured to control the movement of the second mechanical arm to a pose that meets the grasping accuracy requirements;

The processing unit is used to inversely multiply the parameters of the visual recognition in this pose to obtain the homogeneous matrix from the second tool to the end of the second robotic arm; From the end to the pose in the base coordinate system of the second manipulator, calculate the homogeneous matrix from the second tool to the end of the second manipulator.

On the other hand, corresponding to the above method, the present invention also provides a computer-readable storage medium, on which a computer program is stored, wherein when the computer program is executed by a processor, the robot arm tool coordinates described in any one of the above items are realized. The steps of the automatic calibration method.

To sum up, through the automatic calibration method, system and storage medium of the tool coordinates of the manipulator provided by the present invention, after the tool coordinate system is calibrated once, the grasping pose can be recorded once through the technology of visual recognition. The grasping pose is compensated by teaching, and the calibration of the tool coordinates of the plurality of identically configured manipulators can be completed, so as to realize the reuse of the teaching points between the manipulators and accelerate the tool coordinates of the plurality of identically configured manipulators. The calibration process improves the calibration efficiency as a whole, and significantly reduces the difficulty of calibration operations for on-site personnel.

The preferred embodiments of the invention disclosed above are only to help illustrate the invention. The preferred embodiments are not exhaustive in all detail, nor are the inventions limited to specific embodiments described. Obviously, many modifications and variations can be made based on the contents of this specification. This description selects and specifically describes these embodiments in order to better explain the principle and practical application of the present invention, so that those skilled in the art can well understand and utilize the present invention. The present invention is only limited by the claims and their full scope and equivalents. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention. Inside.

Those skilled in the art can understand that, in addition to realizing the system, device and each module provided by the present invention in a purely computer-readable program code mode, the system, device and each module provided by the present invention can be completely implemented by logically programming the method steps. Modules implement the same program in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, and embedded microcontrollers, among others. Therefore, the system, device and each module provided by the present invention can be regarded as a hardware component, and the modules included in it for realizing various programs can also be regarded as the structure in the hardware component; A module for realizing various functions can be regarded as either a software program realizing a method or a structure within a hardware component.

In addition, all or part of the steps in the method of the above-mentioned embodiments can be completed by instructing related hardware through a program. The program is stored in a storage medium and includes several instructions to make the single-chip microcomputer, chip or processor (processor) execute the present invention. Apply for all or part of the steps of the method described in each embodiment. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-OnlyMemory), random access memory (RAM, Random AccessMemory), magnetic disk or optical disk, and other media capable of storing program codes.

In addition, various implementations of the embodiments of the present invention can also be combined arbitrarily, as long as they do not violate the idea of the embodiments of the present invention, they should also be regarded as the content disclosed in the embodiments of the present invention.

Claims (8)

1. An automatic calibration method of mechanical arm tool coordinates, the steps comprising: Step S100 uses the first mechanical arm as a reference to calibrate the tool coordinate system; Step S200: When the first tool at the control end of the calibrated first robotic arm can grasp the calibration object, use the hand-eye calibrated camera on the first robotic arm to identify the Mark set next to the calibration object, so as to capture the position at this time. Convert the pose to the Mark coordinate system to obtain the initial teaching point; Step S300 Send the initial teaching point to the second robotic arm, and when it is judged that the pose difference exceeds the threshold, teach and adjust the second robotic arm for compensation, so as to complete the calibration of its tool coordinate system. 2. The automatic calibration method of the mechanical arm tool coordinates according to claim 1, wherein the calibration step of the tool coordinate system described in step S100 comprises: Step S110 controls the first robot arm to hold the calibration needle to touch the calibration point in several different postures, so as to establish a homogeneous matrix from the first tool to the base coordinate system of the robot arm

, where />

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator; Step S120 expands the matrix in step S110, according to the simultaneous equations of each attitude point, to calculate according to the least square method:

; Obtain the position difference from the first tool to the end of the first mechanical arm, where

Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system, />

Indicates the position vector from the current first tool to the end of the first robotic arm, />

Indicates the rotation matrix from the end of the first robotic arm to the base coordinate system; Step S130 takes a posture point in step S110 as a starting point, and adjusts the coordinate offset of the first mechanical arm in the X and Y axis directions to calculate the X, Y, and Z direction vectors from the first tool to the end of the first mechanical arm, In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system. 3. The automatic calibration method of the mechanical arm tool coordinates according to claim 1, wherein the calibration step of the tool coordinate system described in step S100 comprises: Step S140 controls the first mechanical arm to clamp the calibration needle to touch the calibration point, and denote as

; Step S150 controls the end of the first mechanical arm to move a certain distance along the Z axis to mark a point

; Step S160 with

is the center of the circle, and r is the radius to establish a calibration ring, divide the calibration ring into several quadrants, and rotate the preset angle to obtain several point positions on the ring where each quadrant is located and calculate the corresponding attitude points respectively; Step S170 controls the first robot arm to hold the calibration needle to touch the calibration point at the attitude point obtained in step S160, so as to establish a homogeneous matrix from the first tool to the base coordinate system of the first robot arm

, where />

represents the homogeneous matrix from the first tool to the end of the first arm, />

Represents the homogeneous matrix from the end of the first manipulator to the base coordinate system of the first manipulator; Step S180 calculates according to the method of least squares according to the simultaneous equations of each attitude point:

; Obtain the position difference from the first tool to the end of the first mechanical arm, where

Indicates the current position vector of the first tool in the base coordinate system of the first robot arm, />

Indicates the current position vector of the end of the first robotic arm in the base coordinate system, />

Indicates the position vector from the current first tool to the end of the first robotic arm, />

Indicates the rotation matrix from the end of the first robotic arm to the base coordinate system; Step S190 takes a posture point in step S160 as a starting point, and adjusts the coordinate offset of the first mechanical arm in the X and Y axis directions to calculate the X, Y, and Z direction vectors from the first tool to the end of the first mechanical arm, In this way, the deviation relationship between the first tool and the end of the first mechanical arm is obtained, so as to complete the calibration of the tool coordinate system. 4. The automatic calibration method for tool coordinates of a robotic arm according to claim 1, wherein the step of teaching and adjusting the second robotic arm for compensation in step S300 comprises: Step S310 teaches the second robotic arm to move to a pose that meets the grasping accuracy requirements, and inversely multiplies it back to the visual recognition parameters

, to get the homogeneous matrix from the second tool to the end of the second robot arm />

:

; in,

is the homogeneous matrix from the grasping pose of the first manipulator to the base frame of the first manipulator,

It is the homogeneous inverse matrix from the grab pose to Mark in the first robotic arm, />

is the homogeneous inverse matrix from Mark to camera coordinate system, />

is the homogeneous inverse matrix from the camera coordinate system to the first tool of the first robotic arm; Step S320 Use the joint angle to find the pose of the base coordinate system from the end of the second manipulator to the base coordinate system of the second manipulator at this time, and calculate the homogeneous matrix from the second tool to the end of the second manipulator

:

; in:

is the homogeneous inverse matrix from the end of the first manipulator to the base coordinate system of the first manipulator. 5. The automatic calibration method for tool coordinates of a manipulator according to claim 1, wherein the relative positional relationship between the Mark and the calibration object is fixed. 6. An automatic calibration system for tool coordinates of a mechanical arm, comprising: The storage unit is used to store the program including the steps of the automatic calibration method for the tool coordinates of the manipulator according to any one of claims 1 to 5, so that the control unit, the transmission unit, and the processing unit can be retrieved and executed in due course; a control unit, configured to control the first mechanical arm and its first tool for teaching movement; The processing unit is used to record the teaching parameters of the first mechanical arm, so as to calculate the deviation relationship between the first tool and the end of the first mechanical arm, so as to complete the calibration of the tool coordinate system; The control unit is also used to control the calibrated first mechanical arm to control the first tool at the end to grab the calibration object, and at the same time control the camera on the first mechanical arm to recognize the Mark set next to the calibration object, so that the processing unit can grasp the calibration object at this time. Take the pose and convert it to the Mark coordinate system to obtain the initial teaching point; The transmission unit is used to send the initial teaching point to the second robot arm, so that after the control unit controls the robot arm to move to the corresponding point, the processing unit judges whether the pose difference exceeds the threshold. When the threshold is exceeded, the processing unit The second mechanical arm performs compensation calculation to complete the calibration of its tool coordinate system. 7. The automatic calibration system of the tool coordinates of the mechanical arm according to claim 6, wherein the step of calculating the compensation of the second mechanical arm comprises: a control unit, configured to control the movement of the second mechanical arm to a pose that meets the grasping accuracy requirements; The processing unit is used to inversely multiply the parameters of the visual recognition in this pose to obtain the homogeneous matrix from the second tool to the end of the second robotic arm; From the end to the pose in the base coordinate system of the second manipulator, calculate the homogeneous matrix from the second tool to the end of the second manipulator. 8. A computer-readable storage medium, on which a computer program is stored, wherein when the computer program is executed by a processor, the method for automatically calibrating the coordinates of the mechanical arm tool according to any one of claims 1 to 5 is realized step.

Images