JPH05111897A - Finding method of relative position relationship between plurality of robots - Google Patents

Abstract

PURPOSE:To determine relative positions of a plurality of robots by making a needle tool of one robot into contact with a facial tool of the other robot at no less than three points by fixed level of force, to determine force component, torque component, contact point location and so on. CONSTITUTION:An inner force sensor 3 and facial tool 4 are mounted on a robot A, while a needle tool 5 is mounted on a robot B. The needle tool 5 is connected statically by a fixed force to the facial tool 4, and a force component, a torque component are determined by the sensor 3, while a base coordinate system position of the robot A of a power point is determined based on the position of the robot A. The front end position of the needle tool on the base coordinate system of the robot B is also determined. The same process is carried out at three point that are not in the same line. A 6-parameter of a homogeneous transformation matrix of the base coordinate system of the robot B displayed by the base coordinate system of the robot A is determined at the three points, to obtain position relationship between the respective robots A and B. When the process accuracy of the facial tool 4 or the mounting accuracy of the tools 4, 5 are good, the position, relationship can be easily found.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of acquiring a relative positional relationship between robots when a plurality of robots work in cooperation with each other.

[0002]

2. Description of the Related Art For handling a heavy object that cannot be carried by one robot with a plurality of robots, or when handling a flexible object such as a wire with a plurality of robots, one work target In some cases, multiple robots work together. In such a case, since each robot has to cooperate with other robots, it is necessary to know the positional relationship between the robots in order to avoid interference and collaborate. Therefore, conventionally, the positional relationship of each robot is known by the following method. (1) Each robot is accurately positioned and arranged at a predetermined position. (2) A jig is fixed in the middle of each robot, and the robot is moved so that several points on the jig whose positions are known are touched by one point on the tool attached to each robot. The positional relationship of each robot is obtained from the robot position / orientation via the jig coordinate system.

[0003]

However, in the above method (1), it is a very difficult task to accurately position and arrange each robot at a predetermined position. Further, in the above method (2), it is very difficult to measure the position of the jig and position the tool mounted on the robot at a predetermined position on the jig, which requires a complicated operation. .. Therefore, an object of the present invention is to provide a relative positional relationship acquisition method capable of easily obtaining the relative positional relationship between a plurality of robots.

[0004]

According to the present invention, a force sensor and a planar tool having a surface whose shape is known are attached to the tip of one robot arm, and a needle-like tool is attached to the tip of the other robot arm. At three or more points that are not on the line, the tip of the needle-shaped tool is brought into point contact with the surface of the planar tool with a predetermined force, and the force component and torque component detected by the force sensor at each contact and one robot at that time The position of each contact point on the base coordinate system of the one robot is obtained from the posture of the robot, and the position of the tip end point of the needle-shaped tool of the other robot at the time of each contact is obtained on the robot base coordinate system. From the positions of the contact points and the tip of the needle-shaped tool, the relative positional relationship between the plurality of robots is obtained, which is the relative positional relationship between the robots.

[0005]

When the tip of the needle-shaped tool is brought into contact with the planar tool with a predetermined force, the position of the contact point on the surface of the planar tool can be known from the force component and torque component detected by the force sensor. And from the posture of one robot at that time,
The position of the contact point on the base coordinate system of the robot is obtained. On the other hand, the needle-shaped tool tip position of the other robot on the base coordinate system of the robot is obtained from the robot posture, and the position of the contact point of the planar tool and the tip position of the needle-shaped tool are the same in space. , The position of the contact point on the base coordinate system of one of the robots is obtained by multiplying the tip position of the needle tool by the homogenous transformation matrix that transforms the base coordinate system of the other robot to the base coordinate system of the one robot. Becomes However, this homogeneous transformation matrix has six unknown parameters, which are the rotation of the coordinate system and the amount of translation of the origin. Therefore, a total of 9 or more equations can be obtained with 3 points per point by touching 3 or more points that are not on the same straight line, and 6 of these can be selected to solve simultaneous equations, or 7 The above six parameters can be obtained by solving the above equation by the method of least squares. As a result, the homogeneous transformation matrix is obtained, and the positional relationship between the two robots is obtained. In this way, the positional relationship between the plurality of robots is sequentially obtained.

[0006]

1 is a schematic view of one embodiment of the present invention. In this embodiment, two robots (robots with 6 degrees of freedom) A and B are assumed to perform joint work, and these two robots A and B are used.
The relative position relation acquisition method of is described. First, a force sensor (six-axis sensor) 3 that can detect forces in the X, Y, and Z directions orthogonal to each other and forces around these axes is attached to the arm tip of one robot A, and the force sensor is also attached. Three
A tool (hereinafter referred to as a planar tool) 4 having a surface whose shape is known is attached to the top. In this embodiment, the known surface of the above surface tool is a plane. A needle-shaped tool 5 capable of making point contact with the plane of the planar tool 4 is attached to the arm tip of the other robot B. Then, the positional relationship between the robots A and B is determined based on the postures of the robots and the measurement values measured by the force sensor 3 when the needle-shaped tool 5 is brought into contact with the planar tool 4 and the force at the contact point is constant and is in a stationary state. Is to seek. In the figure, 1a is a control device for the robot A, 1b is a control device for the robot B, and 2 is each control device 1.
It is a host computer to which a and 1b are connected.

Therefore, the base coordinate system of the robot A is Σ _A , the base coordinate system of the robot B is Σ _B , the face plate coordinate system of the tool mounting surface of the robot A is Σ _FA , and the face plate coordinate of the tool mounting surface of the robot B is Σ _FA . Let the system be Σ _FB , the force sensor coordinate system be Σ _S , the vector representing the joint displacement of the robot A be θ _A , and the vector representing the joint displacement of the robot B be θ _B. Since the robot has six joints in the present embodiment, each vector θ _A , θ _B is represented by a matrix of 6 rows and 1 column of each joint displacement θ1 to θ6. The robot A of the joint displacement of the robot A when the position of the vector theta _A face plate coordinate system sigma _FA to ^A T _FA of the homogeneous transformation matrix of 4 rows and 4 columns expressed in the base coordinate system sigma _A robot A (Θ _A ). Similarly, the face plate coordinate system sigma _FB the base coordinate system sigma ^B T four rows and four columns of the transformation matrix expressed in _B Robot B Robot B when the joint displacement position of the vector theta _B of the robot B _FB (θ _B ) Furthermore, the homogenous transformation matrix of 4 rows and 4 columns in which the force sensor coordinate system Σ _S is represented by the face plate coordinate system Σ _FA of the robot A is ^FA T _S.

The subscript added to the upper left of the matrix means the coordinate system in which the variable is represented. That is, for example, the homogenous conversion matrix ^FA T _S means a matrix for converting the position represented by the force sensor coordinate system Σ _S into the face plate coordinate system Σ _FA .

Therefore, the equation of the plane of the planar tool 4 represented by the force sensor coordinate system Σ _S is defined as the following first equation. That is, when the planar tool 4 is created, the planar tool 4 is created so that the following first expression is satisfied. ^{S n T (S x- S x} 0) = 0 ... (1) However, ^S n is represented by a second equation follows by the normal vector of the plane. ^S n = [n _x , n _y , n _z ] ^T (2) Note that n _x , n _y , and n _z are X of the normal vector ^S n,
These are Y and Z axis components. ^S x ₀ is an arbitrary point on the plane and is represented by the following third equation.

^S x ₀ = [x0, y0, z0] ^T (3) where x0, y0, z0 are coordinate positions of points on a plane in the force sensor coordinate system Σ _S. The subscript T at the upper right of the matrix or vector represents transposition.

Next, the two robots A and B bring the tip of the needle-shaped tool 5 into point contact with the surface of the surface-shaped tool 4, and a force sensation when a static point contact state is established with a constant force. Sensor 3
If the force component measured in ^S is ^S f (this force component is also a vector) and the torque component is ^S τ (this torque component is also a vector), no torque is generated at the force point in the point contact state. , And the torque component ^S τ is expressed by the following fourth equation, where ^S P (vector) is the force point expressed in the force sensor coordinate system Σ _S.

^S τ = ^S P × ^S f (4) Note that the right side of the above fourth equation is an outer product.

When ^S P in the above-mentioned fourth equation is replaced with ^S x, the following fifth equation is obtained, and this fifth equation means a straight line equation passing through the force point.

^S τ = ^S x × ^S f (5) Therefore, if the above equations 5 and 1 are simultaneously solved to solve, the force point ^S
P (position on the force sensor coordinate system Σ _S ) is obtained. As a result, if the vector representing this force point in the base coordinate system Σ _A of the robot A is ^A P, the vector ^A P can be obtained by calculating the following sixth equation.

^A P = ^A T _FA (θ _A ) ^FA T _S ^S P (6) On the other hand, in the robot B having the needle-shaped tool 5, the tool tip is represented by Σ _FB in the face plate coordinate system of the robot B. When the position (vector) is ^FB r, the position (vector) ^B P in which the position is represented by Σ _B in the base coordinate system of the robot B is obtained by the following seventh formula. When the product of the simultaneous transformation matrix of 4 rows and 4 columns and the position vector is obtained, “1” is added to the position vector as a fourth element to form a column vector of 4 rows and 1 column.

^B P = ^B T _FB (θ _B ) ^FB r (7) The force point position ^A P obtained by the robot A and the needle tool tip position ^B P obtained by the robot B are at the same position in space. Therefore, if the homogeneous transformation matrix of the base coordinate system Σ _B of the robot B represented by the base coordinate system Σ _A of the robot ^{A is} AT _B , the following Eq. 8 is established.

[0017] _{^{A P = A T B B P}} ... (8) base coordinate system sigma _B of the robot B to the base coordinate system sigma _A robot A is the Z axis phi (roll angle), coordinates the rotation Around the Y axis of the system, θ (pitch angle), φ,
It is rotated by ψ (yaw angle) around the X axis of the coordinate system rotated by θ, and the origin translation amount is (xo, yo, zo)
When, the homogeneous transformation matrix ^A T _B is represented by Equation 1, six parameters representing the position and orientation (variable)
It is a non-linear expression having φ, θ, ψ, xo, yo and zo.

[0018]

[Equation 1] In the above homogeneous transformation matrix, Cr = cosφ, Sr = sinφCp = cosθ, Sp = sinθCy = cosφ, Sy = sinφ.

Therefore, the contact points of the planar tool 4 and the needle-shaped tool 3 are moved so that they are not collinear (m ≧ 3).
When the force point ^A P and the needle-shaped tool tip point ^B P are obtained respectively, the following 9th equation is established from the 8th equation at each point i (i = 1, 2, ... On the other hand, three equations are obtained, and a total of 3m equations are obtained.

[0020] _{^{A Pi = A T B B Pi}} ... (9) Thus, ,, roll angle is solved properly select the system of equations six equations than 3m number of equations φ, pitch angle θ, the yaw angle ψ, parallel movement the amount of (xo, yo, zo) can be obtained is Motomari the homogeneous transformation matrix ^a T _B, determined relative positional relationship between the robot a and the robot B is.

When seven or more equations are used, the optimum solution is numerically obtained by repeatedly calculating the least squares method for the nonlinear equation.

2 and 4 are flowcharts of the processing of the host computer for obtaining the relative positional relationship between the robots A and B. FIG. 3 is a flowchart of the process of the control device 1a of the robot A. First, in the host computer 2,
The equation shown in the first equation of the plane of the planar tool 4, the robot A
Of the face plate coordinate system sigma _FA a homogeneous transformation matrix ^A T _FA expressed in the base coordinate system sigma _A robot A as a function of the joints positions θA of the robot A (theta _A), the face plate coordinate system of the robot B sigma _FB Is a function of each joint position θ _B of the robot B in the base coordinate system Σ _B of the robot B, the homogeneous transformation matrix ^B T _FB (θ _B ), and the force sensor coordinate system Σ _S is the face plate coordinate system of the robot A. Homogeneous transformation matrix ^FA T _S represented by Σ _FA , and the position of the tip of needle-shaped tool 5 in robot B face plate coordinate system Σ _FB (vector) ^FB
r is set in advance.

Then, the robot A equipped with the force sensor 3 and the planar tool 4 and the robot B equipped with the needle-shaped tool 5 are manually moved to position the needle-shaped tool 5 near the center position of the planar tool 4. .. When a position acquisition command is input to the host computer 2, the host computer 2
First starts the process shown in the flowchart of FIG.
A position acquisition command is output to the robots A and B (step 101), and the posture θ _B (each joint position θ1 to θ6) of the robot sent from the controller 1b of the robot B is read and the address indicated by the index i is read. It is stored in i (step 102). The index i is initially set to "0" in the initial setting when the power is turned on. Then, together with the position acquisition completion signal from the control device 1a of the robot A, the posture A of the robot _A (each joint position θ1 to θ6) and the force sensor 4 are detected.
It waits for the force component f and the torque component τ detected in step 3 to be sent (step 103).

On the other hand, when the controller 1a of the robot A receives the position acquisition command, it executes the process shown in the flowchart of FIG. 3, and first, the robot A is moved in the normal direction of the planar tool 4.
Is moved (step 201), it is determined whether the force component f detected by the force sensor 3 is equal to or more than a set value,
If it is equal to or more than the set value, the movement of the robot A is stopped, the timer is reset and started (step 2).
02-204). When the timer measures the set time (step 205), it is determined whether the force component f detected by the force sensor 3 is equal to or more than the set value (step 206).
That is, it is determined in the processing of steps 202 to 206 whether or not the robots A and B are stationary and are in point contact with a predetermined force. If no force equal to or greater than the set value is detected in step 206, the process returns to step 201 and the above process is repeated.

Then, when it is detected in step 206 that the planar tool 4 and the needle-shaped tool 5 are statically in point contact with each other with a force equal to or greater than the set value, the position acquisition completion signal and the robot A at that time are detected. Posture θ _A (θ1 to θ6), force sensor 3
The force component f and the torque component τ measured in 3 are transmitted to the host computer 2 (step 207). Figure 2 again
When the host computer 2 receives the position acquisition completion signal and the posture θ _A (θ1 to θ6) of the robot A, the force component f and the torque component τ (step 103), the data is stored in the address i indicated by the index i. [theta] _A , f, [tau] are stored (step 104), the index i is incremented by "1" (step 105), and i position acquisition completion is displayed on the display device (step 106).

In this way, when the point contact position between the planar tool 4 and the needle-shaped tool 5 is detected, the robots A and B are manually moved, and as described above, the needle-shaped tool 4 is placed near the center position of the planar tool 4. Position the tool 5. Then, the position acquisition command is input to the host computer 2 again, and the host computer 2 and the control device 1a of the Rhobotot A are caused to execute the processing of FIGS.
The data θ _B , θ _A , f, τ are stored in 1). When the position acquisition is completed in this way, the robots A and B are manually moved again to position the needle-shaped tool 5 in the vicinity of the central position of the planar tool 4, as described above. At this time, the robots A and B are positioned so that the contact points between the planar tool 4 and the needle tool 5 are not on the same line in space. Then, the position acquisition command is input and the processing of FIGS. 2 and 3 is executed.

When it is displayed that the index i has reached "3" or more and three contact points have been detected (in this embodiment, the relative positional relationship is determined by finding three points). ,
The operator inputs a relative position acquisition command to the host computer 2 to start the processing shown in FIG. The host computer 2 first sets the index i to "0" (step 301), and calculates the above-mentioned equation 7 from the posture θ _B i of the robot B stored at the address i indicated by the index i, The position ^B Pi of the tip of the needle-shaped tool is obtained (step 302). Further, the force component f and the torque component τ stored in the address i are read out, and the set equation of the plane of the planar tool, that is, the simultaneous equations of the first and fifth equations are solved, and the i-th force point position ^S Pi is determined. Ask (Step 30)
3). Next, the position ^A Pi of the force point (contact point) in the base coordinate system of the robot A is calculated by performing the operation of the sixth equation from the obtained force point ^S Pi and the posture θ _A i of the robot (step 304). After that, the index i is incremented by “1”, and steps 302 to 3 are performed until the index i reaches “3”.
The processing of 06 is repeated, and the needle-shaped tool tip position ^B P in the base coordinate shape of the robot B at the three force points (contact points)
0 to ^B P2 and positions ^A P0 to ^A P2 of the robot A on the base coordinate system are obtained. Next, the relational expression of the ninth equation is obtained from the corresponding three determined positions ^A P0 to ^A P2 and ^B P0 to ^B P2, and six are selected from the obtained nine equations to select the transformation matrix ^A T _B. Parameters φ, θ, ψ, xo
, Yo, it obtains a transformation matrix ^A T _B seek zo (step S307), and ends the relative position acquisition process.

[0028] In the above embodiment, the relative positional relationship between the host computer 2, that is, was to obtain the homogeneous transformation matrix ^A T _B, may be determined by the robot controller. In this case, by connecting the robots A and B and inputting a position acquisition command to the robot A, the processor of the control device 1a of the robot A performs the processes of step 102 and steps 201 to 207, The posture θ _{B of B} , the posture θ _{A of} robot A, the force component f, and the torque component τ are obtained and stored respectively, and three contact points are obtained, and then the processes of steps 301 to 307 may be executed. ..

Further, even when three or more robots perform joint work, the relative positional relationship between the robots is obtained by the above-mentioned method.

[0030]

According to the present invention, the positional relationship between robots can be acquired by a relatively simple operation as long as the machining accuracy of the planar tool and the accuracy of mounting the tool and the force sensor are improved.

[Brief description of drawings]

FIG. 1 is a schematic diagram of an embodiment of the present invention.

FIG. 2 is a flowchart of a position acquisition process of the host computer in the embodiment.

FIG. 3 is a flowchart of a position acquisition process executed by the control device of the robot A in the same embodiment.

FIG. 4 is a flowchart of a positional relationship acquisition process of the host computer in the embodiment.

[Explanation of symbols]

 A, B robot 2 host computer 3 force sensor 4 planar tool 5 needle tool 1a, 1b control device

Claims (2)

[Claims] 1. A force sensor and a planar tool having a surface whose shape is known are attached to the tip of one robot arm, and a needle-like tool is attached to the tip of the other robot arm, and at three or more points that are not on the same line. The tip of the needle-shaped tool is brought into point contact with the surface of the planar tool with a predetermined force, and the force component and torque component detected by the force sensor at each contact and the posture of one robot at that time The position of each contact point on the coordinate system is obtained, and the position of the tip of the needle-shaped tool of the other robot at the time of each contact is obtained on the robot base coordinate system. A relative positional relationship acquisition method for multiple robots that determines the relative positional relationship between robots from the position of the tip. 2. The relative positional relationship acquisition method for a plurality of robots according to claim 1, wherein the known surface of the on-surface tool is a flat surface.