JP2010172969A - Robot system and method of controlling robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To enable the instruction and regeneration of holding action including measurement without caring about a sensor coordinate system and without requiring an input of a dimension of a workpiece. <P>SOLUTION: In a robot control device 102, a gripper 107 is moved to a holding position by manual operation and a holding position is recorded as an instruction position, a preliminary determined area with the instruction position as a center is determined as a measurement range, and a workpiece shape obtained by measuring the measurement range by the shape measurement means of the workpiece is recorded as master data in relation to the instruction position. The master data are compared with measurement data. The three-dimensional position/attitude of the measurement data with respect to the master data is calculated and made to be a modified amount. The holding position is modified according to the modified amount, and an instruction for moving the gripper 107 to the holding position is generated. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

The present invention relates to a robot system and a control method used for assembling components.

The part assembly process performed at a car or home appliance manufacturing factory includes a work for fitting workpieces. When an assembly process including such a fitting operation is automated and labor-saving using a robot, it is necessary to teach the fitting operation to the robot by some method. This teaching can be divided into online teaching and offline teaching.
As an online teaching method, for example, there is a method in which an operator guides a robot to create a position and orientation and teaches an actual operation or procedure. Further, as an off-line teaching method, there is a method in which a work program is created in advance by a personal computer, and the generated work program is transferred to a robot controller for reproduction operation.
However, in the online teaching, there is a possibility that the actually taught position and the working position during the regenerative operation may be shifted due to the workpiece positioning error. In off-line teaching, the teaching data origin may deviate from the actual robot origin, which makes it difficult to correct the teaching point position.

Therefore, in order to solve such problems, there have been the following conventional techniques.
In Patent Document 1, as a method for teaching a teaching position with high accuracy by online teaching, a position detection unit for detecting a position of a workpiece is provided on a robot end effector, and the position detection unit irradiates a position detection region with measurement light, A technique for acquiring position information of a workpiece based on irregularly reflected light or regular reflected light returning from a position detection region is disclosed.
In Patent Document 2, as a method of correcting work movement, a robot is moved by a measurement operation file, a reference workpiece is sensed by a distance sensor, a measurement distance is stored, and an actual workpiece is sensed and measured. A technique for obtaining a distance and correcting a work operation from a difference from a measurement distance of a reference workpiece is disclosed.

On the other hand, in recent years, there has been an active movement in the manufacturing industry to utilize a double-arm robot for replacing work by humans.
Patent Document 3 discloses a dual arm robot in which one arm of a dual arm robot is preparing one component while the other arm is preparing to assemble another component, or two arms are combined with both arms. Have been disclosed that operate in an alternating or simultaneous manner.
Patent Document 4 discloses a technique in which two robot arms are operated in a coordinated manner, and one object is held and moved by the two arms.

JP-A-5-309590 (page 10, FIG. 1) Japanese Patent Laid-Open No. 6-210580 (page 4, FIG. 1, page 5, FIG. 4) JP 2006-35346 A (page 9, FIG. 6) Japanese Unexamined Patent Publication No. 2003-159683 (page 8, FIG. 1)

  However, in Patent Documents 1 and 2, it is impossible to measure the three-dimensional posture deviation of the workpiece by the method of scanning the workpiece in the cross direction or only in one direction. In addition, it is troublesome and time-consuming to teach the operator how to scan the workpiece for each workpiece. In addition, an efficient three-dimensional position / orientation measurement operation itself is not clear, and an operator needs trial and error for each workpiece.

In addition, Patent Document 3 does not fully describe the case where one work is handled with two arms, and the work when one end and the other end of one work is a flexible object is used as a double arm. The method of gripping with is not clear.
In addition, in Patent Document 4, if the position where both arms grip the workpiece cannot be fit properly, both arms must accurately grip the exact position of the workpiece. Since the slave arm supports only the gravitational direction, it cannot maintain an accurate gripping position.
The present invention has been made in view of such problems, and it is not necessary for the operator to teach the position and orientation of the measurement operation for each workpiece, so that the three-dimensional position and orientation can be obtained with high accuracy and the position of the workpiece can be obtained. The purpose is to correct the deviation of the posture and the posture, and further to a method that can be applied to the assembly work by the double-arm robot.

In order to solve the above problem, the present invention is configured as follows.
According to a first aspect of the present invention, in a robot control device including an end effector for gripping a workpiece and a shape measurement sensor for the workpiece, the end effector is moved to a gripping position and recorded as a teaching position. A teaching step in which a predetermined range centering on the teaching position is set as a measurement range, and a workpiece shape measured by the workpiece shape measuring means is recorded as master data in association with the teaching position, and the teaching position is The measurement range is a predetermined range as the center, the workpiece shape measured by the workpiece shape measuring means is the measurement data, the master data is compared with the measurement data, and the measurement data for the master data is compared. A three-dimensional position / orientation is calculated as a correction amount, and the correction amount is Correct the lifting position, is characterized in that the end effector including a reproducing step of generating a command to move to the modified gripping position.
The invention according to claim 2 is characterized in that the reproduction step is executed by one instruction of a robot operation program.
The invention according to claim 3 is characterized in that the measurement range is determined by a scan angle, a scan distance, and a scan speed.
The invention described in claim 4 is characterized in that feature data extracted from the measured workpiece shape is recorded in a memory as the master data.
According to a fifth aspect of the present invention, in the robot control apparatus including an end effector that grips a workpiece and a shape measuring unit of the workpiece, the end effector is moved manually to a target position that moves after gripping the workpiece. And recorded as a first teaching position, a predetermined range centered on the first teaching position is determined as a measurement range, and the workpiece shape measured by the workpiece shape measuring means is used as first master data. Recording in association with the first teaching position, moving the end effector to a gripping position and recording it as a second teaching position, determining a predetermined area around the second teaching position as a measurement range, The workpiece shape measured by the workpiece shape measuring means is associated with the second teaching position as second master data. A teaching step for recording, a predetermined range centered on the first teaching position is determined as a measurement range, a workpiece shape measured by the workpiece shape measuring means is defined as first measurement data, and the first The master data and the first measurement data are compared, the three-dimensional position and orientation of the first measurement data with respect to the first master data is calculated as a first correction amount, and a predetermined range centering on the second teaching position Is determined as a measurement range, the workpiece shape measured by the workpiece shape measuring means is used as second measurement data, the second master data is compared with the second measurement data, and the second master data is compared with the second master data. The three-dimensional position and orientation of the second measurement data is calculated as the second correction amount, the gripping position is corrected according to the second correction amount, and the end effect is obtained. A command to move the slider to the gripping position and grip the workpiece, correct the target position to move after gripping according to the first correction amount, and command to move the end effector that grips the workpiece to the corrected target position. And a reproduction step to be generated.
According to a sixth aspect of the present invention, there is provided a control device for a dual-arm robot including an end effector for gripping a workpiece and a shape measuring unit for the workpiece. The end effector of the other arm provided with the workpiece shape measuring means is moved to the workpiece measurement position and recorded as a teaching position, a predetermined range centered on the teaching position is determined as a measurement range, and the measurement is performed. A teaching step for recording a workpiece shape measured by a workpiece shape measuring means in association with the teaching position as master data, a range determined in advance with the teaching position as a center is determined as a measuring range, and the measuring range is determined. Using the workpiece shape measured by the workpiece shape measuring means as measurement data, the master data and the measurement data The arm that holds the workpiece in order to move the workpiece to the position of the master data according to the correction amount. And a regenerating step for generating a command to move by correcting the position.
According to a seventh aspect of the present invention, in the reproduction step, a predetermined range centered on the teaching position is determined as a first measurement range, and the first measurement range is determined by a workpiece shape measuring means for a first time. Measurement is performed, the measured workpiece shape is used as the first measurement data, the master data is compared with the first measurement data, and there is enough data to match the first measurement data with the master data. If not, a second measurement range in which the posture is changed with respect to the workpiece is generated from the first measurement range, and the second measurement range is subjected to a second measurement by the workpiece shape measurement means, and the first measurement range is executed. The measurement data of the second time and the measurement data of the second time are overlapped to obtain measurement data, and the correction amount is calculated by calculating the three-dimensional position and orientation of the measurement data with respect to the master data. Fixed grip position Therefore, is characterized in that said end effector is a step of generating a command to move to the gripping position the modified.
The invention according to claim 8 is a dual-arm robot having an end effector for gripping a workpiece on an arm, the dual-arm robot having a sensor for measuring the shape of the workpiece on the arm, and the dual-arm robot. A control device for controlling an arm robot, a memory storing an operation program describing the operation of the robot, an analysis unit for analyzing the operation program, and teaching data to be written in the operation program using the current position as a teaching position A storage unit, a measurement unit for obtaining measurement data of the workpiece shape from the sensor, a master data storage unit for storing the measurement data obtained at the time of teaching in a memory as master data, and comparing the master data and the measurement data A comparison unit that outputs the amount of change as a comparison result as a correction amount, and a predetermined scan condition and a correction amount based on the correction amount. A control device comprising: a command generation unit that generates an arm trajectory; a servo control unit that drives a motor according to the generated trajectory; and a teaching device connected to the control device, wherein the double-arm robot And a teaching device that teaches the operation command.
The invention according to claim 9 is characterized in that the master data is measurement data of a workpiece shape measured by the workpiece shape measuring means along a trajectory of a helical scan.
The invention described in claim 10 is characterized in that, in the reproducing step, the workpiece shape measuring means measures the workpiece shape along a trajectory of a helical scan.

According to the first aspect of the present invention, it seems to the operator that the measurement range is determined around the teaching position of an end effector such as a gripper that grips a workpiece. Since it does not care about the position where the laser beam hits, and it is not necessary to input the workpiece dimensions, it is possible to easily teach the gripping operation including the measurement operation. When a single workpiece is gripped by a double-arm robot, after accurately gripping one end of the workpiece with the left arm, the other end of the workpiece is measured and gripped with the right arm. Work by grasping the position.
According to the second aspect of the present invention, at the time of motion reproduction, the measurement operation is not performed separately from the gripping operation, but is automatically executed as an operation necessary for the work of gripping. Becomes easier to understand.
According to the invention described in claim 3, it is not necessary to teach a plurality of measurement positions, and the measurement range can be determined around the teach position without inputting the workpiece dimensions, and the measurement is automatically performed. Since the motion can be generated, the teaching time is shortened.
In addition, according to the invention described in claim 4, it is possible to store the measurement data with a smaller data amount than to store all of the measurement data as master data.
Further, according to the invention described in claim 5, since not only the gripping position but also the target position such as the fitting position after gripping can be corrected, it is possible to correct the deviation of the work operation from gripping to fitting. it can.
According to the invention described in claim 6, in the double-arm robot, the operation of the arm on the side holding the workpiece can be corrected.
According to the seventh aspect of the present invention, since necessary data can be actively measured and acquired, a reproduction operation can be executed while allowing a large deviation without increasing the teaching position.
According to the eighth aspect of the present invention, in the dual-arm robot, it is possible to configure a control system for the dual-arm robot for gripping a workpiece at a position shifted from the teaching position.
According to the ninth aspect of the present invention, the measurement data viewed from various directions can be used as master data, and even when the deviation of the posture of the workpiece from the teaching position is large during the reproduction operation, it matches the measurement data. Since the portion is included, the correction amount can be calculated.
According to the tenth aspect of the present invention, measurement data viewed from various directions can be obtained during the reproduction operation, and it is not necessary to re-measure by changing the posture, so that the measurement time can be shortened.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a configuration diagram of a robot system according to the present invention. This robot system includes a double-arm robot 101 and a control device 102 that controls its operation.
First, the dual arm robot 101 will be described.
In the figure, the left arm 104 and the right arm 105 of the robot are schematically shown including joints. Each joint is driven by a motor (not shown).
The left gripper (end effector) 106 is disposed at the tip of the left arm.
The right gripper (end effector) 107 is disposed at the tip of the right arm.
A workpiece 109 is a gripping object of the double arm robot 101. Although the work 109 is a cable with a connector in this embodiment, the details will be described later.
The workpiece shape measurement sensor 108 is a two-dimensional laser displacement sensor. This sensor is mounted on the right gripper 107. The workpiece shape measurement sensor 108 can continuously acquire two-dimensional measurement data of the workpiece 109 by acquiring the data of the workpiece 109 while the right arm 105 moves. By superimposing the acquired data, three-dimensional measurement data (work shape) is obtained. Note that this sensor may be mounted not on the right gripper 107 but on the left gripper 106. Moreover, you may mount in the arm instead of a gripper. The sensor may be any sensor as long as it can measure the three-dimensional shape of the workpiece. For example, this sensor may be a vision sensor with a plurality of cameras.

Next, the control device 102 will be described.
The operation program analysis unit 110 analyzes an operation program (JOB) 118 in which the operation of the robot is described and calls a necessary process.
The teaching data storage unit 113 writes the current position 122 as the teaching position 123 in the operation program 118. The teaching position 123 is written in the operation program 118 when the operator inputs a predetermined key of the teaching device 103 during teaching work. The teaching data storage unit 113 writes the number of master data 121 described later as an object number 125 in the operation program 118. Here, the object number 125 is a number assigned to specify the master data when a plurality of master data 121 are stored. The teaching data storage unit 113 writes the teaching position 123 and the object number 125 as a set to the operation program 118. This operation program 118 is stored in the nonvolatile memory 117.
The measurement unit 114 obtains workpiece measurement data 120 using the workpiece shape measurement sensor 108.
The master data storage unit 115 numbers the measurement data 120 as the master data 121 and stores the master data 121 in the nonvolatile memory 117.
The position / orientation comparison unit 116 compares the master data 121 and the measurement data 120 to calculate a three-dimensional position / orientation of the measurement data 120 with respect to the master data 121. The position / orientation comparison unit 116 outputs the change amount of the position / orientation as a correction amount 124 to the command generation unit 111.
The command generation unit 111 generates a trajectory according to a manual operation from the teaching device 103. Further, the command generation unit 111 generates a trajectory of an arm that performs a gripping operation or a measurement operation based on the operation program 118, the scan condition 119 stored as a parameter of the control device, and the correction amount 124.
The servo control unit 112 drives the motor according to the generated trajectory.
The teaching device 103 is connected to the control device 102 and is a device for teaching and operating the dual-arm robot 101. On the teaching device 103, a key for inputting commands and data to the robot and a display for displaying information are provided.

Next, the operation of the robot system according to the present invention will be described. In this embodiment, the work 109 handled by the double-arm robot is a cable having connectors at both ends of a plurality of lead wires shown in FIG. FIG. 4A shows a state in which the left gripper 106 is holding the connector 404 by holding the workpiece connector 404 whose position is fixed by a jig on the table or a supply device as previously taught. Show. The position of the connector 403 is not fixed. Since the lead wire 405 between the connector 404 and the connector 403 is flexible, the position of the connector 403 is not fixed.

The operation of the robot system according to the present invention is a teaching operation (steps S201 to S206) shown in FIG. 2 and a reproduction operation (steps S301 to S305) shown in FIG. Hereinafter, each operation will be described.

I. Teaching Operation First, the teaching operation (steps S201 to S206) will be described with reference to FIG.
(S201)
An operator operates a key of the teaching device 103. The operator operates the key so as to move the right gripper 107 to the gripping position of the workpiece 109. The command generator 111 generates a trajectory along which the robot moves according to this key operation.

(S202)
The operator operates the teaching device 103 to register a scan command (MOVSCAN) in the operation program. This scan command (MOVSCAN) is a command for causing the workpiece shape measurement sensor 108 to perform a scanning operation for obtaining the workpiece measurement data 120. This operation program is stored in the nonvolatile memory 117.
A specific example will be described with reference to FIG. FIG. 5 shows an operation program display screen 501 of the teaching device. In the operation program display screen 501, a teaching position 503 displays the teaching position of the robot. Reference numeral 502 denotes an operation program that is currently being edited.
The operator selects a scan command (MOVSCAN) from a menu command list screen (not shown). When the operator presses the Enter key on the teaching device 103 after selection, the scan command 504 is registered in the operation program 502 as shown in the figure. At the same time, the current position of the robot is saved as teaching position data 503. This teaching position data is stored as teaching position number 505 (C00001).
The scan command 504 includes (i) a teaching position number 505 (C00001), (ii) a position variable 506 (P012) to which the teaching position corrected in step S304 is substituted, and (iii) a moving speed 507 (V = 10.0) and (iv) is recorded in the operation program in association with the object number 125 (OBJ # (1)). After recording, (iii) the moving speed 507 of the scanning operation and (iv) the object number 125 set by default can be changed.

(S203)
An operator operates the teaching device 103 to register a movement command (MOVL) in the operation program. This movement command 601 is a command for moving the gripper to the gripping position.
A specific example will be described with reference to FIG. FIG. 6 shows an operation program display screen 501 of the teaching device. The scan command executed in step S202 is already displayed.
The movement command (MOVL) 601 is registered in the operation program in association with the position variable 506 (P012) and the movement speed 602 (V = 10.0). The position variable 506 is a variable designated by the scan command in step S202.

(S204)
The operator aligns the cursor displayed on the screen of the teaching apparatus 103 with the scan command line and presses the scan execution button 508 on the screen (FIG. 6). When the scan execution button 508 is pressed, the command generation unit 111 measures a predetermined measurement range based on a predetermined scan condition 119 on the basis of the teaching position (the teaching position stored in C00001) of the operation program. Generate a trajectory for Here, the predetermined scan condition 119 is stored in advance in the parameters of the control device as the scan angle 406, the scan distance 407, and the scan speed. Here, the scan angle is an angle at which the workpiece shape measuring sensor 108 is tilted about an axis in the front direction of the object (FIG. 4C). The scan distance is the distance that the workpiece shape measurement sensor 108 moves (FIG. 4C). The scan speed is the speed at which the workpiece shape measurement sensor 108 moves.
The servo control unit 112 drives the motor to move the robot according to the generated trajectory. As the robot moves, the measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S205)
The obtained measurement data is displayed on the display of the teaching device. The measurement data is displayed as a three-dimensional image 702 as shown in FIG. In the figure, reference numeral 125 denotes an object number, and 703 denotes an object name representing a connector type name or common name. This object name may be an arbitrary name. Reference numeral 704 denotes a registration button.

(S206)
An operator performs a master registration operation. This master registration operation is an operation in which the operator inputs the object number 125 using the teaching device and presses the registration button 704. When the operator performs this master registration operation, the measurement data is stored in the nonvolatile memory 117 as master data 121.

As described above, the operation program 118 and the master data 121 are created by the teaching operation (steps S201 to S206). The operation program 118 and master data 121 are stored in the nonvolatile memory 117.

FIG. 4 shows the movement of the workpiece shape measuring sensor 108 during the teaching operation. In FIG. 4A, a gripper coordinate system 401 is set at the tip of the right gripper 107. A sensor coordinate system 402 is set at the sensor origin of the workpiece shape measurement sensor 108. Here, the object to be gripped is the connector 403 that the left gripper 106 does not grip.
FIG. 4B shows a state when the steps S201 and S202 are executed. The robot is moved by manual operation from the teaching device 103, and the right gripper 107 is positioned at the gripping position of the connector to obtain the teaching position.
FIG. 4C shows a state when step S204 is executed. The control device 102 automatically adjusts the origin of the sensor coordinate system 402 to the origin of the gripper coordinate system 401 at the teaching position. Thereafter, the control device 102 rotates the sensor coordinate system 402 by the scan angle 406 around the front direction of the connector 403 and moves the workpiece shape measurement sensor 108 by the scan distance 407. At this time, by starting measurement from a position moved in the negative direction of the movement direction by a half of the scan distance 407, scanning is performed so that the teaching position becomes the center of the measurement range as shown in FIG. . From the position before execution of the scan command to the measurement start position, it moves at the movement speed specified in the scan command. The speed during the measurement is not the movement speed designated in the scan command in step S202, but moves at the scan speed designated as the scan condition.

II. Reproduction Operation Next, the reproduction operation (steps S301 to S305) will be described with reference to FIG.
This reproduction operation is an operation for executing a scan command written in the operation program created by the teaching operation (steps S301 to S304) and an operation for executing a movement command (step 305).

(S301)
The operation program analysis unit 110 calls the operation program 118 and the scan condition 119 from the nonvolatile memory 117. The operation program analysis unit 110 instructs the command generation unit 111 to execute the scan command written in the operation program 118. When commanded by the operation program analysis unit 110, the command generation unit 111 measures a predetermined measurement range according to the scan condition 119 with reference to the teaching position registered with the scan command (the teaching position stored in C00001). Generate a trajectory. The servo control unit 112 drives the motor to move the arm according to the generated trajectory. As the robot operates, the measurement unit 114 obtains measurement data of the three-dimensional position and orientation of the connector 403 from the workpiece shape measurement sensor 108.

(S302)
The position / orientation comparison unit 116 reads the master data 121 corresponding to the object number 125 registered in step S202 from the nonvolatile memory 117. Next, the position / orientation comparison unit 116 calculates how much the three-dimensional position / orientation of the connector 403 obtained in step S 301 has changed with respect to the master data 121. The position and orientation comparison unit 116 sets the changed position and orientation amount as the correction amount 124.

Here, the calculation of the correction amount 124 will be described below. In FIG. 8, 801 is a three-dimensional image of the master data 121, and 802 is a three-dimensional image of the measurement data 120.
First, feature points are extracted. When edge points are extracted from the three-dimensional image of FIG. 8, a point group shown in FIG. 9A is obtained. In this figure, reference numeral 901 denotes an edge point extracted from the master data, and reference numeral 902 denotes an edge point extracted from the measurement data. As a method for extracting the edge point, a known method or the like using a point at which a difference from an adjacent point becomes a certain threshold value or more as an edge point is used.
When a straight line is extracted from the edge point data, a straight line connecting the edge points is obtained as shown in FIG. In this figure, 903 is a straight line of master data, and 904 is a straight line of measurement data. As a method of extracting the straight line, a known method by Hough transform is used.
The feature point to be obtained is the intersection of the extracted straight lines. A sign is attached to this intersection. At this time, the first feature point located in the scanning direction is set to the first. Each feature point has position components X, Y, and Z. In FIG. 9C, four points 905 are feature points of master data, and four points 906 are feature points of measurement data. Characteristic points are attached with p1 to p4 for master data and q1 to q4 for measurement data.

The correction amount 124 is obtained as a change amount of the position and orientation of the measurement data with respect to the master data 121.
First, when the master data shown in FIG. 10 (a1) and the measurement data shown in FIG. 10 (a2) are viewed from the same sensor coordinate system 402, they are as shown in FIG. 10 (b). However, since the sensor coordinate system moves parallel to the scan direction at the time of measurement, for example, the measurement data is overlapped with the position advanced by a half of the scan distance from the scan start point as the origin of the sensor coordinate system.
Next, the entire master data is moved so that the feature points p1 and q1 coincide. When the sensor coordinate system is moved in parallel by q1 viewed from p1, the result is as shown in FIG. In FIG. 10C, the moved p1, p2, p3, and p4 are represented by p1b, p2b, p3b, and p4b. The distance between p1b and q1 is zero. A coordinate system having the same posture as the sensor coordinate system S1 is set at the position p1b, and is defined as a coordinate system S2. That is, S2 is a coordinate system obtained by translating S1 by q1 viewed from S1.
Next, S2 is rotated by ΔTr, and at the same time, p2b to p4b are also rotated on the basis of S2 to be p2c, p3c, and p4c, respectively. That is, the entire quadrangle having ap1b, p2b, p3b, and p4b as vertices is rotated together with S2. ΔTr is a 4 × 4 homogeneous transformation matrix representing rotation. After the rotation, J in Equation 1 obtained by calculating and adding the distance between points having the same number in the code is calculated as an evaluation function.

Here, i is a subscript indicating the number of the point from 1 to n, ^S2 q _i is the coordinate of the point q _i viewed from the coordinate system S2, and ^S2 _pic is the coordinate of the point p _ic viewed from the coordinate system S2. .
ΔTr is obtained as in the following (1) to (5).
(1) Let ΔTr be a rotation matrix calculated from Roll, Pitch, and Yaw. The Pitch angle and Yaw angle are fixed, and the Roll angle is changed by 1 degree from −45 degrees to 45 degrees to obtain the Roll angle that minimizes the evaluation function J.
(2) Change ΔTr to the Roll angle obtained in (1), and further change the Pitch angle by 1 degree from −45 degrees to 45 degrees to obtain the Pitch angle that minimizes the evaluation function J.
(3) ΔTr is set to the Roll angle obtained in (1), the Pitch angle is changed to the Pitch angle obtained in (2), and the Yaw angle is changed by 1 degree from −45 degrees to 45 degrees. The Yaw angle that minimizes the evaluation function J is obtained.
(4) Let ΔTr be the Pitch angle obtained in (2) for the Pitch angle, and the Yaw angle obtained in (3) for the Yaw angle.
(5) The transformation matrix ΔTr that minimizes the evaluation function J is obtained by repeating the above (1) to (4) several times. Using the obtained ΔTr, the correction amount is expressed by a matrix ^S1 T _SF of the following equation 2.

Here, Trans () is a transformation matrix representing parallel movement. The correction amount ^G1 T _GF expressed in the gripper coordinate system is expressed by Equation 3.

Here, ^G R _S represents a 3 × 3 rotation matrix representing the orientation of the sensor coordinate system as viewed from the gripper coordinate system. ^S _RG represents a 3 × 3 rotation matrix representing the posture of the gripper coordinate system as viewed from the sensor coordinate system.

(S303)
The command generation unit 111 corrects the teaching position according to the correction amount 124. A corrected teaching position is obtained by converting the teaching position into the form of a homogeneous transformation matrix and multiplying the matrix of Expression 3 from the right. That is, if the teaching position viewed from the robot coordinate system is ^R _Tt , the corrected teaching position ^R _Tt2 is expressed by Equation 4.

(S304)
The corrected teaching position is substituted into the position variable 506 (P012) designated as the argument of the scan command. However, the values of X, Y, Z, Roll, Pitch, and Yaw calculated from the matrix of the corrected teaching position ^R _Tt2 are substituted into the position variable 506.

(S305)
The operation program analysis unit 110 instructs the command generation unit 111 to execute the movement command written in the operation program 118. When commanded by the motion program analysis unit 110, the command generation unit 111 generates a trajectory to the position of the position variable specified in the movement command of the motion program 118. The servo controller 112 drives the motor to move the arm along the generated trajectory.

FIG. 11 shows the movement of the workpiece shape measuring sensor 108 during the reproducing operation. In FIG. 11A, scanning is performed according to a scan command to obtain measurement data. FIG. 11B shows a state in which the movement command is used to move to the corrected teaching position, which is assigned to the position variable as a result of the scan command.

In this way, the operator only needs to teach one teaching position in step S202, so there is no need to teach the robot measuring operation itself using a plurality of teaching points. As a result, the operator can perform teaching operation by manual positioning while viewing the gripper and three-dimensional data display. That is, teaching operation including measurement of the position and orientation of the workpiece is simplified. During the measurement operation, since the workpiece shape measurement sensor 108 moves within a predetermined measurement range centered on the teaching position, the operator teaches without worrying about the position of the sensor coordinate system and without needing to input the dimensions of the object. be able to.
Note that the scan command and the movement command are independent commands, but they may be a gripping movement command (MOVCATCH) in which these are combined.

FIG. 13 is a configuration diagram of a robot system according to the second embodiment. The robot system according to the present invention includes a double-arm robot 101 and a control device 102 that controls its operation.
The double-arm robot 101 is the same as that shown in FIG.
Hereinafter, the control apparatus 102 will be described.
The scan condition input unit 1301 uses the scan angle (FIG. 4C), the scan distance (FIG. 4C), the scan speed, and the like input from the teaching device 103 during the teaching operation as a scan condition 119 as a nonvolatile memory. Save to 117.
Since other parts are the same as those in the first embodiment, description thereof is omitted.

The operation of the robot system according to the present embodiment is a teaching operation (steps S1401 to S1406) as shown in FIG. 14 and a reproduction operation (steps S1501 to S1504) as shown in FIG. Hereinafter, each operation will be described.

I. Teaching Operation First, the teaching operation (steps S1401 to S1406) will be described with reference to FIG.
(S1401)
An operator operates a key of the teaching device 103. The operator operates the key so as to move the right gripper 107 to the gripping position of the workpiece 109. The command generator 111 generates a trajectory along which the robot moves according to this key operation.

(S1402)
The operator operates the teaching device 103 to register the grip movement command (MOVCATCH) in the operation program. This gripping movement command (MOVCATCH) is a multi-command that combines a scan command and a movement command for the workpiece shape measurement sensor 108 to obtain workpiece measurement data 120. This operation program is stored in a nonvolatile memory.
A specific example will be described with reference to FIG. FIG. 12 shows an example of the operation program display screen 501 of the teaching device.
The operator selects a grip movement command (MOVCATCH) from a menu command list screen (not shown). After the selection, when the operator presses the Enter key on the teaching device 103, a grip movement command 1201 is registered in the operation program 502 as shown in the figure. At the same time, the current position of the robot is saved as teaching position data 503. This teaching position data is stored as teaching position number 505 (C00001).
The grip movement command 1201 is associated with (i) a teaching position number 505 (C00001), (ii) a movement speed 507 (V = 10.0) for scanning operation, and (iii) an object number 507 (OBJ # (1)). And recorded in the operation program. After recording, the moving speed 507 and the object number 507 set by default can be changed.

(S1403)
The scan angle, scan distance, and scan speed input from the teaching device are stored in the nonvolatile memory as scan conditions. FIG. 16 shows a scan condition input screen 1601 of the teaching apparatus. The operator can set scan conditions by inputting values of a scan angle 1602, a scan distance 1603, and a scan speed 1604 and pressing a setting button (SET) 1605.

(S1404)
When the operator presses the scan execution button 508 of the teaching apparatus, the command generation unit 111 measures the measurement range according to the scan conditions set with reference to the teaching position of the operation program (the teaching position stored in C00001). Is generated. The servo control unit 112 drives the motor to move the robot. As the robot moves, the measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S1405)
The obtained measurement data is displayed on the display of the teaching device 103. The measurement data is displayed as a three-dimensional image 702 as shown in FIG. In the figure, 125 is an object number, 703 is an object name, and 704 is a registration button.

(S1406)
Next, the operator performs a master registration operation. When the operator performs this master registration operation, the measurement data is stored in the nonvolatile memory 117 as master data 121.

As described above, the scan conditions, the operation program, and the master data are created by the teaching operation (S1401 to S1406). The scan condition, operation program, and master data are stored in the nonvolatile memory 117.

II. Reproduction Operation Next, the reproduction operation (steps S1501 to S1504) will be described with reference to FIG. This reproduction operation is an operation for executing a gripping movement command written in the operation program.

(S1501)
The operation program analysis unit 110 calls the operation program 118 and the scan condition 119 from the nonvolatile memory 117. The motion program analysis unit 110 instructs the command generation unit 111 to execute the grip movement command written in the motion program 118. When commanded by the motion program analysis unit 110, the command generation unit 111 performs predetermined measurement according to the scan condition 119 with reference to the teaching position (teaching position stored in C00001) registered together with the grip movement command of the motion program 118. Generate a trajectory to measure the range. The servo control unit 112 drives the motor to move the arm according to the generated trajectory. As the robot operates, the measurement unit 114 obtains measurement data of the three-dimensional position and orientation of the connector 403 from the workpiece shape measurement sensor 108.

(S1502)
The position / orientation comparison unit 116 reads from the nonvolatile memory 117 master data 121 corresponding to the object number 125 registered together with the grip movement command in the operation program 118. Next, the position / orientation comparison unit 116 calculates how much the three-dimensional position / orientation of the connector 403 obtained in step S <b> 1501 has changed with respect to the master data 121. The position and orientation comparison unit 116 sets the changed position and orientation amount as the correction amount 124.
FIG. 8 shows an example of a master data 121 and a three-dimensional image of measurement data.

(S1503)
The command generation unit 111 corrects the teaching position according to the correction amount 124. Since the method for obtaining the correction amount 124 is the same as that in the first embodiment, a description thereof is omitted.

(S1504)
The command generation unit 111 corrects the teaching position according to the correction amount 124 and generates a trajectory to the corrected position. The servo control unit 112 drives the motor to move the arm along the generated trajectory. In the grip movement command (MOVCATCH), the movement from the position before execution of the scan command to the measurement start position and the movement from the measurement end position to the grip position are performed at a movement speed specified in the scan command. The speed during measurement moves at the scan speed specified as the scan condition.

FIG. 11 shows the operation playback operation. In FIG. 11A, scanning is performed in accordance with the grip movement command to obtain measurement data. FIG. 11B shows how the correction amount is calculated from the measurement result and master data, the corrected teaching position is calculated, and the approach position calculated from the teaching position is reached. FIG. 11C shows the corrected teaching position. The state that the gripper is moved is shown.

In this way, the measurement range can be determined around the teaching position according to the scan conditions without inputting the workpiece dimensions. Accordingly, since the measurement operation can be automatically generated, the teaching time is shortened.

When the master data is stored in the flowchart of FIG. 2 (S206), the feature point extraction shown in FIG. 9 is executed, and only the extracted feature point data is stored. Further, at the time of reading the master data in FIG. 3 (S302), the stored feature point data is read and compared with the feature points obtained from the measurement data, the correction amount is calculated as shown in FIG.
In this way, it is possible to save data with a smaller data amount than to save all measurement data as master data. Therefore, the amount of non-volatile memory used for master data can be reduced.

FIG. 17 is a diagram illustrating the operation of the fourth embodiment. The left gripper 106 holds the case 1703. From this case 1703, a ground wire 1702 is provided. In this embodiment, the lower end 1701 of the ground wire 1702 is gripped by the right gripper 107 and inserted into the ground wire insertion tab 1704.
In the present embodiment, the position of the ground wire 1702, which is the gripping position, is uncertain. Therefore, the lower end 1701 of the ground wire 1702 is measured and gripped by the workpiece shape measuring sensor 108. Further, the position of the tab 1704 for inserting the lower end of the ground wire after gripping the lower end 1701 of the ground wire 1702 is also uncertain. Therefore, the tab 1704 is measured by a sensor, the insertion teaching position is corrected, and the insertion operation is performed.

The operation of the robot system according to the present embodiment is a teaching operation (steps S1801 to S1811) as shown in FIG. 18 and a reproduction operation (steps S1901 to S1910) as shown in FIG.

I. Teaching Operation First, the teaching operation (steps S1801 to S1811) will be described with reference to FIG.
(S1801)
In order to teach the operation of measuring the insertion position on the tab, the operator operates the key of the teaching device 103. The operator operates the key so as to move the right gripper 107 to the tab position. The command generator 111 generates a trajectory along which the robot moves according to this key operation.

(S1802)
The operator operates the teaching device 103 to register the scan command (MOVSCAN2) together with the first teaching position and the object number 125 in the operation program. This scan command (MOVSCAN2) is a command for causing the workpiece shape measurement sensor 108 to perform a scan operation for obtaining the workpiece measurement data 120, and MOVSCAN is a point of a value included in a position variable designated as an argument. Is different. This operation program is stored in the nonvolatile memory 117. Here, after the scan command is executed, the value that enters the position variable specified as the argument of the scan command is not the corrected teaching position, but the correction amount, and how much the master data is moved around the robot coordinate system 126 becomes the measurement data. This is a conversion amount indicating whether they match.
A specific example will be described with reference to FIG. FIG. 20 shows an operation program display screen 2001 of the teaching device. The operator selects a scan command from a menu command list screen (not shown). After selection, when the operator presses the Enter key on the teaching device 103, it is registered in the operation program 2002 as indicated by L001. At the same time, the current position of the robot is stored as first teaching position data. The first teaching position data is stored as a teaching position number (C00001).
The scan command (MOVSCAN2) shown in L001 is (i) a teaching position number (C00001), (ii) a position variable (P018) into which correction amount components X, Y, Z, Roll, Pitch, and Yaw as a scan result are substituted. , (Iii) The movement speed (V = 5.0) of the scanning operation and (iv) the object number (OBJ # (3)) are recorded in the operation program in association with each other.

(S1803)
The operator aligns the cursor displayed on the screen of the teaching apparatus 103 with the scan command line and presses the scan execution button (SCAN) 508 on the screen (FIG. 20). When the scan execution button 508 is pressed, the command generation unit 111 measures a predetermined measurement range based on a predetermined scan condition with the first teaching position (the teaching position stored in C00001) of the operation program as a reference. Generate a trajectory. The predetermined scan conditions are stored in advance as parameters of the control device as a scan angle, a scan distance, and a scan speed.
The servo control unit 112 drives the motor and moves the right arm of the robot according to the generated trajectory. As the robot moves, the measurement unit 114 obtains measurement data of the three-dimensional position and orientation of the connector 403 from the workpiece shape measurement sensor 108.

(S1804)
Measurement data obtained on the display of the teaching apparatus 103 is displayed as a three-dimensional image.

(S1805)
An operator performs a master registration operation. This master registration operation is an operation in which the operator inputs the object number 125 using the teaching device 103 and presses the registration button. When the operator performs this master registration operation, the measurement data is stored in the nonvolatile memory 117 as first master data.

(S1806)
In order to teach the operation of gripping the lower end of the ground wire, the operator operates the key of the teaching device 103. The operator operates the key so as to move the right gripper 107 to the gripping position. The command generator 111 generates a trajectory along which the robot moves according to this key input.

(S1807)
The operator operates the teaching device 103 and registers the scan command (MOVSCAN2) together with the second teaching position and the object number 125 in the operation program. This operation program 121 is stored in the nonvolatile memory 117.
In FIG. 20, the operator selects a scan command from a menu command list screen (not shown). After selection, when the operator presses the Enter key, it is registered in the operation program 2002 as L002. At the same time, the current position of the robot is stored as second teaching position data 2003. The second teaching position data is stored as a teaching position number (C00002).
A scan command (MOVSCAN2) shown in L002 is (i) a teaching position number (C00002), (ii) a position variable (P010) into which correction amount components X, Y, Z, Roll, Pitch, and Yaw as a scan result are substituted. , (Iii) The movement speed (V = 5.0) of the scanning operation and (iv) the object number (OBJ # (2)) are recorded in the operation program in association with each other.

(S1808)
The operator places the cursor on the screen of the teaching device 103 on the line of the scan command and presses the scan execution button 508 on the screen of the teaching device (FIG. 20). When the scan execution button 508 is pressed, the command generation unit 111 measures a predetermined measurement range based on a predetermined scan condition based on the second teaching position (the teaching position stored in C00002) of the operation program. Is generated. The predetermined scan conditions are stored in advance as parameters of the control device as a scan angle, a scan distance, and a scan speed.
The servo control unit 112 drives the motor and moves the right arm of the robot according to the generated trajectory. As the robot moves, the measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S1809)
The measurement data is displayed as a three-dimensional image on the display of the teaching device 103.

(S1810)
An operator performs a master registration operation. When the operator performs this master registration operation, the measurement data is stored in the nonvolatile memory 117 as second master data.

(S1811)
A command for operating the teaching device 103 to calculate the teaching position correction by using the correction amount entered in the reproduction step to the position variables (P018, P010) designated by the scan command (MOVSCAN2) by operating the keys of the teaching device 103. Register the movement command in the operation program.
In FIG. 20, L003 is a subroutine call command (CALL) in which a moving program for holding the lower end of the ground wire with the right gripper is described.
L004 is a movement command (MOVL) for moving the ground wire over the tab, and is stored in association with the teaching position (C00003 to C00006) and the moving speed (V = 10.0).
L005 is a command (GETS) for acquiring the current position of the right arm as viewed from the robot coordinate system 126, and is a system variable ($ PX001) that represents the current position and a position variable that substitutes a copy of the contents of the system variable ( P011) and stored.
L006 is an operation command (MULTIMAT) that calculates the product of P018 as a homogeneous transformation matrix and P011 as a homogeneous transformation matrix, and puts the calculation result in P012. In L006, P018 is the tab correction amount obtained in L001, and P011 is the current position.
L007 is a move command (MOVL) to the position of P012, and is stored in association with the position variable (P012) and the moving speed (V = 10.0). The position for insertion into the tab is corrected by L007.
L008 is a relative movement command (IMOV) that moves from the current position by the amount set in P019, and is associated with the position variable (P019), the moving speed (V = 10.0), and the coordinate system designation (RF). Saved. In P019, the amount of movement in the insertion direction is set in advance, and RF means that it moves as a relative amount of movement on the robot coordinate system.

As described above, the operation program, the first master data, and the second master data are created and stored in the nonvolatile memory 117 by the teaching operation.

II. Reproduction Operation Next, the reproduction operation (steps S1901 to S1910) will be described with reference to FIG. This reproduction operation is an operation for executing a command written in the operation program.

(S1901)
The operation program analysis unit 110 calls the operation program and scan conditions from the nonvolatile memory 117. The operation program analysis unit 110 instructs the command generation unit to execute the scan command (MOVSCAN2) written in L001 in the operation program. The command generation unit 111 generates a trajectory for measuring a predetermined measurement range according to the scan condition with reference to the first teaching position (teaching position stored in C00001) registered together with the scan command of the operation program. The servo control unit 112 moves the robot by driving a motor according to the generated trajectory. As the robot operates, the measurement unit 114 obtains first measurement data from the workpiece shape measurement sensor 108. The first measurement data is measurement data of the ground wire insertion tab 1704.

(S1902)
The position / orientation comparison unit 116 reads out from the nonvolatile memory 117 master data 121 corresponding to the object number registered together with the L001 scan command (MOVSCAN2) in the operation program. The position / orientation comparison unit 116 calculates a three-dimensional position / orientation of the first measurement data with respect to the first master data, and sets the change amount as the first correction amount. However, the correction amount is a conversion amount that represents how much the master data is moved around the robot coordinate system 126 to match the measurement data. That is, the transformation matrix T _a is obtained such that the relationship between the position / orientation ^R T _C2 of the object determined from the master data and the position / orientation ^R T _C1 of the object determined from the measurement data becomes the relationship of Equation 5, and is used as the correction amount. .

(S1903)
The operation program analysis unit 110 instructs the command generation unit to execute the scan command (MOVSCAN2) written in L002 in the operation program. The command generation unit 111 generates a trajectory for measuring a predetermined measurement range according to the scan condition with reference to the second teaching position (teaching position stored in C00002) registered together with the scan command of the operation program. The servo control unit 112 moves the robot by driving a motor according to the generated trajectory. As the robot operates, the measurement unit 114 obtains second measurement data from the workpiece shape measurement sensor 108. The second measurement data is measurement data of the ground wire lower end 1701.

(S1904)
The position / orientation comparison unit 116 reads, from the nonvolatile memory 117, master data 121 corresponding to the object number registered together with the L002 scan command (MOVSCAN2) in the operation program. The position / orientation comparison unit 116 calculates the three-dimensional position / orientation of the second measurement data with respect to the second master data, and sets the change amount as the second correction amount.

(S1905)
The operation program analysis unit 110 calls a subroutine (CATCH) written in L003 in the operation program. L003 uses the second correction amount (P010) to correct the second teaching position, which is the grip position of the lower end of the ground wire, and moves to grip the lower end of the ground wire with the right gripper. Corrected teaching position ^R T _G2, using the gripper teaching position ^R T _G1 and correction amount T _a is a ground wire gripping position is calculated as Equation 6.

(S1906)
The move command (MOVL) written in L004 is executed, and the ground wire is moved over the tab.

(S1907)
A command (GETS) for acquiring the current position of the right arm viewed from the robot coordinate system 126 written in L005 is executed, and the current position is substituted into the position variable P011.

(S1908)
The matrix product calculation command (MULMAT) written in L006 is executed, and the result of multiplying the current position by the tab correction amount is substituted into the position variable P012.

(S1909)
A move command (MOVL) to the position of P012 written in L007 is executed, and the current position is corrected and moved to the correct position on the tab. This teaching position is the current position of the tab correction amount as T _a, the movement to a position that fixes using Equation 6.

(S1910)
The move command (IMOV) written in L008 is executed, and the ground wire is inserted into the tab.

Thus, by describing an operation program that combines a scan command, a calculation command, and a movement command, the gripping operation and the insertion operation can be corrected in the same way of teaching the scan.

FIG. 21 is a diagram illustrating the operation of the fifth embodiment. The left gripper 106 holds the connector 404. The connector 404 is connected to a flexible lead wire 405, and the connector 403 is connected to the other end. There is a hole in the upper part of the case 1703. In this embodiment, the operation of passing the connector 403 through the hole of the case 1703 is performed.
Since the position of the connector 403 is indeterminate, measurement is performed by a sensor, and the connector 403 is moved over the hole in the case 1703 by moving the left arm, and the connector is passed through the hole.

The operations are a teaching operation (S2201 to S2206) as shown in FIG. 22 and a reproducing operation (S2301 to S2307) as shown in FIG.

I. Teaching Operation First, the teaching operation (S2201 to S2206) will be described with reference to FIG.
(S2201)
First, in order to teach the operation of measuring the connector shape, the operator operates a key of the teaching device 103. The operator operates the key so as to move the right gripper 107 to the position of the connector 403. The command generator 111 generates a trajectory along which the robot moves according to this key operation.

(S2202)
The operator operates the teaching device 103 and registers the scan command (MOVSCAN2) in the operation program together with the teaching position and the object number. This moving scan command (MOVSCAN2) is a command for causing the workpiece shape measurement sensor 108 to perform a scanning operation for obtaining the workpiece measurement data 120, and is a command for causing the workpiece shape measurement sensor 108 to be specified as an argument. The difference is in the value that enters the position variable. The operation program is stored in a nonvolatile memory. Here, after the scan command is executed, the value that enters the position variable specified as the argument of the scan command is not the corrected teaching position, but the correction amount, and how much the master data is moved around the robot coordinate system 126 becomes the measurement data. This is a conversion amount indicating whether they match.
A specific example will be described with reference to FIG. FIG. 24 shows an operation program display screen 2401 of the teaching device. The operator selects a scan command from a menu command list screen (not shown). After selection, when the operator presses the Enter key on the teaching device 103, it is registered in the operation program 2402 as indicated by L001 in FIG. At the same time, the current position of the robot is stored as teaching position data 2403. The teaching position data 2403 is stored as a teaching position number (C00003).
The scan command (MOVSCAN2) includes (i) a teaching position number (C00003), (ii) a position variable (P010) into which the correction amount components X, Y, Z, Roll, Pitch, and Yaw as a scan result are substituted. iii) It is recorded in the operation program in association with the moving speed (V = 5.0) of the scanning operation and (iv) the object number (OBJ # (1)).

(S2203)
The operator aligns the cursor displayed on the screen of the teaching device 103 with the scan command line and presses the scan execution button (SCAN) 508 on the screen (FIG. 24). When the scan execution button 508 is pressed, the command generation unit 111 uses the teaching position of the operation program (the teaching position stored in C00003) as a reference to determine a trajectory for measuring a predetermined measurement range based on a predetermined scanning condition. Generate.
The servo control unit 112 drives the motor and moves the right arm of the robot according to the generated trajectory. The predetermined scanning conditions are stored in advance as parameters of the control device, for example, as a scanning angle, a scanning distance, and a scanning speed. As the robot moves, the measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S2204)
Next, the obtained measurement data is displayed as a three-dimensional image on the display of the teaching device.

(S2205)
An operator performs a master registration operation. This master registration operation is not described again. When the operator performs this master registration operation, the measurement data is stored in the nonvolatile memory as master data.

(S2206)
Commands and movement commands for calculating teaching position correction by using the correction amount entered in the reproduction step to the position variable (P010) designated by the scan command (MOVSCAN2) by the operator operating the keys of the teaching device 103 Is registered in the operation program.
L002 is a command (INVMAT) for calculating an inverse matrix using the correction amount as a result of the scan command as a homogeneous transformation matrix and substituting the result into P011. The position variable (P010) of the conversion source is stored in association with the variable (P011) to which the calculated inverse matrix is substituted.
L003 is a command (GETS) for acquiring the current position of the left arm as viewed from the robot coordinate system 126, and is a system variable ($ PX001) that represents the current position and a position variable that substitutes a copy of the contents of the system variable ( P012) and stored.
L004 is an operation command (MULTIMAT) that calculates the product of P011 as a homogeneous transformation matrix and P012 as a homogeneous transformation matrix and puts the result in P013. In L004, P011 is the inverse matrix of the connector correction amount, and P012 is the current position.
L005 is a move command (MOVL) to the position of P013, and is stored in association with the position variable (P013) and the moving speed (V = 10.0). By L005, the connector at the shifted position is corrected to the master registered position.
L006 is a relative movement command (IMOV) that moves by the amount set in P020, and is stored in association with the position variable (P020) and the movement speed (V = 10.0). In P020, a vertical movement amount of the connector for passing the hole of the case is set in advance, and RF means that the movement is performed as a relative movement amount on the robot coordinate system.

As described above, the operation program and master data are created and stored in the nonvolatile memory 117 by the teaching operation.

II. Reproduction Operation Next, the reproduction operation (S2301 to S2307) will be described with reference to FIG. This reproduction operation is an operation for executing a command written in the operation program.

(S2301)
The operation program analysis unit 110 calls the operation program and scan conditions from the nonvolatile memory. The operation program analysis unit 110 instructs the command generation unit to execute the scan command (MOVSCAN2) written in L001 in the operation program. The command generation unit 111 generates a trajectory for measuring a predetermined measurement range according to the scan condition with reference to the teaching position (P010) registered together with the scan command of the operation program. The servo control unit 112 moves the robot by driving a motor according to the generated trajectory. The measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S2302)
The position / orientation comparison unit 116 reads out from the nonvolatile memory 117 master data 121 corresponding to the object number registered together with the L001 scan command (MOVSCAN2) in the operation program. The position / orientation comparison unit 116 calculates a three-dimensional position / orientation of the measurement data with respect to the master data, and sets the change amount as a correction amount. However, the correction amount is a conversion amount that represents how much the master data is moved around the robot coordinate system 126 to match the measurement data. That is, the transformation matrix T _a is obtained such that the relationship between the position / orientation ^R T _C2 of the object determined from the master data and the position / orientation ^R T _C1 of the object determined from the measurement data becomes the relationship of Equation 5, and is used as the correction amount. .

(S2303)
The inverse matrix command (INVMAT) written in L002 is executed to calculate the inverse matrix of the correction amount and substitute it into P011.

(S2304)
A command (GETS) for acquiring the current position of the left arm viewed from the robot coordinate system 126 written in L003 is executed, and the current position is substituted into P012.

(S2305)
The matrix product calculation command (MULMAT) written in L004 is executed, and the result obtained by multiplying the current position of the left arm by the inverse matrix of the correction amount is substituted into P013. This calculation is for correcting the current position of the connector to the ideal position by moving the current position of the left gripper in the reverse direction by the amount of the measured displacement of the connector, and is corrected to the gripper teaching position ^R T _G1. use the amount T _a is computed as equation 7.

(S2306)
A move command (MOVL) to the position of the position variable P013 written in L005 is executed, and the connector at the shifted position is moved to the correct master data position.

(S2307)
The move command (IMOV) written in L006 is executed, and the connector is passed through the hole of the case.

Thus, by writing an operation program that combines a scan command, a calculation command, and a movement command, the position of an indeterminate target can be corrected to a correct position.

In this embodiment, when necessary measurement data is insufficient, re-measurement is automatically performed to actively acquire data, and the teaching position is corrected.
FIG. 25 is a flowchart showing the operation of the sixth embodiment. Since the teaching operation is the same as the procedure shown in FIG. 2, the description thereof is omitted.
Hereinafter, the reproduction operation will be described. The reproduction operation (S2501 to S2507) in FIG. 25 is an operation for executing the scan command and the movement command written in the operation program. The operation program display screen is the same as that shown in FIG.

(S2501)
The operation program analysis unit 110 calls the operation program 118 and the scan condition 119 from the nonvolatile memory 117. The operation program analysis unit 110 instructs the command generation unit 111 to execute the scan command written in the operation program. The command generation unit 111 generates a trajectory for measuring a predetermined measurement range according to the scan condition 119 with reference to the teaching position registered together with the scan command of the operation program 118. The servo control unit 112 moves the robot by driving a motor according to the generated trajectory. The measurement unit 114 obtains measurement data from the workpiece shape measurement sensor 108.

(S2502)
The position / orientation comparison unit 116 reads out, from the nonvolatile memory 117, master data corresponding to the object number 125 registered together with the scan command in the operation program. The position / orientation comparison unit 116 calculates a three-dimensional position / orientation of the measurement data with respect to the master data 121.
(S2503)
The position / orientation comparison unit 116 determines that the length of the side of the rectangular parallelepiped formed by connecting the corner points extracted from the master data 121 is determined from the measurement data when there is no portion that matches the master data 121 in the measurement data. If no corner of the rectangular parallelepiped formed by connecting the extracted corner points matches), it is determined that there is no portion that matches the master data 121 in the measurement data. On the other hand, if the position / orientation comparison unit 116 determines that there is a portion that matches the master data 121 in the measurement data, the position / orientation comparison unit 116 calculates a three-dimensional position / orientation of the measurement data with respect to the master data 121 to obtain a correction amount. .
(S2504)
If the position / orientation comparison unit 116 determines that there is no portion that matches the master data 121 in the measurement data, the position / orientation comparison unit 116 instructs the command generation unit to execute a measurement operation with a posture different from the previous measurement posture.
FIG. 26 is an example of a three-dimensional image of master data 801 and measurement data 2601. The measurement data 2601 is for the case where the attitude is greatly deviated from the master data.

In calculating the correction amount 124, the position / orientation comparison unit 116 first extracts feature points. When edge points are extracted from the three-dimensional image of FIG. 26, points as shown in FIG. 27A are obtained.
Reference numeral 2701 denotes an edge point extracted from the master data 801. Reference numeral 2702 denotes an edge point extracted from the measurement data 2601.

When a straight line is extracted from the edge point data, a straight line connecting the edge points is obtained as shown in FIG.
In the figure, 2703 is a straight line of master data, and 2704 is a straight line of measurement data.

The intersection of the straight lines is used as a feature point, and a symbol is attached to the intersection of the straight lines as shown in FIG. At this time, the first feature point located in the scanning direction is set to the first. In FIG. 27C, the master data are denoted by reference numerals p1 to p4. Reference numerals q1 to q4 are attached to the measurement data. Reference numeral 2707 denotes a feature point of the master data. Reference numeral 2708 denotes a feature point of measurement data. Each feature point has position components X, Y, and Z.

However, the master data in FIG. 27C and the measurement data are different in terms of the surface on which the object is viewed, so that the shapes do not match. Therefore, the position / orientation comparison unit 116 determines that there is no portion that matches the master data in the measurement data.

FIG. 28 shows an example of re-measurement execution. The second measurement is performed with a posture different from the first measurement posture. At this time, since it moves to a position different from the teaching position, a range in which no object exists is selected from the first measurement data as candidates for the second measurement posture in order to prevent a collision.

FIG. 29 shows the first measurement data 2601 and the second measurement data 3001 of the same object.
30 extracts edge points from the three-dimensional image of FIG. 29 (FIG. 30A), extracts straight lines (FIG. 30B), and numbers the intersections of the straight lines as feature points (FIG. 30C). )) Shows the procedure. At this time, the measurement data is used by overlapping the first measurement data and the second measurement data. In FIG. 30C, the master data rectangle surrounded by the points p1, p2, p3, and p4 is similar to the measurement data rectangle surrounded by the points q1, q2, q3, and q4, and the lengths of the sides are almost the same. Therefore, the difference between the postures of the two rectangles is used as the correction amount. If there is no portion that matches the master data in the second measurement data, the process returns to step S2502 and is repeated.

(S2505)
When the correction amount is calculated, the command generation unit 111 corrects the teaching position according to the correction amount.
(S2506)
The command generation unit 111 substitutes the corrected teaching position into the position variable specified in the scan command.

(S2507)
The operation program analysis unit instructs the command generation unit to execute the movement command written in the operation program, and the command generation unit 111 generates a trajectory to the position of the position variable specified in the movement command of the operation program.

As described above, when necessary data is insufficient, re-measurement is automatically performed to actively acquire data. Since the correction amount is calculated based on the acquired data, the teaching position can be corrected.

In this embodiment, in the teaching operation, the data measured by performing the helical scan that is measured in the spiral trajectory is registered as master data, and in the reproducing operation, the spiral trajectory is not re-measured multiple times by changing the posture. In this method, the helical scan to be measured is performed once to obtain measurement data.
In the method according to the present invention, the teaching operation is the same as the procedure shown in FIG. 2, and the reproducing operation is the same as the procedure shown in FIG. However, the measurement operation in the teaching operation and the reproduction operation is performed by helical scanning according to the scanning condition with the teaching position as a reference. The scan conditions are stored in advance as parameters of the control device as a scan angle 406, a scan distance 407, and a scan speed.

FIG. 31 shows a spiral trajectory created from the scanning conditions. As shown in FIG. 31A, when a trajectory inclined by the scan angle 406 and moved by the scan distance is rotated around the vertical axis 3202 by a predetermined rotation angle 3203, the spiral trajectory 3101 in FIG. become. At this time, the movement distance 3204 in the vertical direction is calculated from the scan angle and the scan distance (vertical movement distance) = (scan distance) · cos (scan angle).
It is. In FIG. 31 (b), the workpiece shape measuring sensor 108 is rotated around the vertical axis 3202 of the teaching position by a rotation angle 3203 of 180 degrees around the teaching position, and simultaneously moved in the upward direction of the vertical axis. A state in which measurement data is obtained by drawing a spiral trajectory 3101 is shown.
By doing in this way, data measured from a plurality of directions can be obtained by one measurement, so that it is possible to cope with a case where the posture deviation from the teaching position of the workpiece is large. Further, during the reproduction operation, the measurement time is shorter than when the posture is changed and remeasurement is performed a plurality of times.

The block diagram of the robot system containing the robot control apparatus which shows 1st Example of this invention The flowchart which shows teaching operation | movement of the robot control apparatus of 1st Example of this invention. The flowchart which shows reproduction | regeneration operation | movement of the robot control apparatus of 1st Example of this invention. The figure which shows teaching operation | movement of this invention Example of the operation program display screen of the first embodiment of the present invention (part 1) Example of the operation program display screen of the first embodiment of the present invention (part 2) Example of measurement data display screen of the present invention Example of 3D image of master data and measurement data of the present invention Example of feature point extraction of the present invention Example of how to obtain the position and orientation of measurement data viewed from the master data of the present invention The figure which shows reproduction | regeneration operation | movement of this invention Example of operation program display screen of second embodiment of the present invention The block diagram of the robot system containing the robot control apparatus of 2nd Example of this invention The flowchart which shows teaching operation | movement of the robot control apparatus of 2nd Example of this invention. The flowchart which shows reproduction | regeneration operation | movement of the robot control apparatus of 2nd Example of this invention. Example of scan condition input screen of the present invention Relationship diagram of coordinate system in the operation of the fourth embodiment of the present invention The flowchart which shows teaching operation | movement of the robot control apparatus of 4th Example of this invention. The flowchart which shows reproduction | regeneration operation | movement of the robot control apparatus of 4th Example of this invention. Example of operation program display screen of the fourth embodiment of the present invention Relationship diagram of coordinate system in operation of fifth embodiment of the present invention The flowchart which shows teaching operation | movement of the robot control apparatus of 5th Example of this invention. The flowchart which shows reproduction | regeneration operation | movement of the robot control apparatus of 5th Example of this invention. Example of operation program display screen of fifth embodiment of the present invention The flowchart which shows the reproduction | regeneration operation | movement of the robot control apparatus of 6th Example of this invention. Example of three-dimensional image of master data and measurement data according to the sixth embodiment of the present invention Example of first feature point extraction of the sixth embodiment of the present invention The figure which shows the 2nd measurement operation | movement of 6th Example of this invention. Example of a three-dimensional image of the first and second measurement data of the sixth embodiment of the present invention Example of second feature point extraction of the sixth embodiment of the present invention Example of trajectory of helical scan of the seventh embodiment of the present invention

101 Robot 102 Control Device 103 Teaching Device 104 Left Arm 105 Right Arm 106 Left Gripper (End Effector)
107 Right gripper (end effector)
108 Work shape measurement sensor 109 Work 110 Operation program analysis unit 111 Command generation unit 112 Servo control unit 113 Teaching data storage unit 114 Measurement unit 115 Master data storage unit 116 Position / orientation comparison unit 117 Non-volatile memory 118 Operation program (JOB)
119 Scanning condition 120 Measurement data 121 Master data 122 Current position 123 Teaching position 124 Correction amount 125 Object number 401 Gripper coordinate system 402 Sensor coordinate system 403 Connector 404 Connector (Left gripper gripping)
405 Connector lead wire 406 Scan angle 407 Scan distance 501 Operation program display screen 502 Operation program 503 Teaching position data 504 Scan command 505 Teaching position number 506 Position variable 507 Scan operation moving speed 508 Scan execution button 601 Move command 602 Grasping operation Movement speed 701 Measurement data display screen 702 Three-dimensional image 703 Object name 704 Registration button 801 Three-dimensional image 802 of master data Three-dimensional image 901 of measurement data Image 902 of edge point extraction from master data Extraction of edge point from measurement data Image 903 Image of straight line extraction from master data 904 Image of line extraction from measurement data 905 Image of line intersection extraction from master data 906 Measurement data Image of extracting intersection point of straight line 907 Edge point 908 Straight line 1201 Grasping movement command 1301 Scan condition input unit 1601 Scan condition input screen 1602 Scan angle 1603 Scan distance 1604 Scan speed 1605 Scan condition setting button 1701 Earth line lower end 1702 Earth line 1703 Case 1704 Ground wire lower end insertion tab 2001 Operation program display screen 2002 Operation program 2003 Teaching position data 2401 Operation program display screen 2402 Operation program 2403 Teaching position data 2601 First measurement data 2701 Image of edge point extraction from master data 2702 Measurement data Image of edge point extraction from image 2703 Image of line extraction from master data 2704 Image of line extraction from measurement data 2705 Image of extracting intersection point of straight line from master data 2706 Image of extracting intersection point of straight line from measurement data 2707 Edge point 2708 Line 2901 Second measurement data 3001 Image of extracting edge point from master data 3002 From measurement data Image of edge point extraction 3003 Image of line extraction from master data 3004 Image of line extraction from measurement data 3005 Image of line intersection extraction from master data 3006 Image of line intersection extraction from measurement data 3007 Edge point 3008 Line 3101 Spiral orbit 3102 Vertical axis 3103 Angle of rotation about the vertical axis 3104 Vertical movement distance

Claims (10)

In a robot control device comprising an end effector for gripping a workpiece and a shape measurement sensor for the workpiece,
The end effector is moved to a gripping position and recorded as a teaching position, a predetermined range centered on the teaching position is set as a measurement range, and a workpiece shape measured by the workpiece shape measuring means is used as master data. A teaching step for recording in association with the teaching position;
A predetermined range centered on the teaching position is set as a measurement range, a workpiece shape measured by a workpiece shape measuring unit is set as measurement data, the master data is compared with the measurement data, and the master data is compared with the master data. A reproduction step of calculating a three-dimensional position and orientation of the measurement data as a correction amount, correcting the gripping position according to the correction amount, and generating a command to move the end effector to the corrected gripping position; A robot control method comprising: The robot control method according to claim 1, wherein the reproduction step is executed by one instruction of a robot operation program. The robot control method according to claim 1, wherein the measurement range is determined by a scan angle, a scan distance, and a scan speed. The robot control method according to claim 1, wherein feature data extracted from the measured workpiece shape is recorded in a memory as the master data. In a robot control device comprising an end effector for gripping a workpiece and the shape measuring means of the workpiece,
The end effector is moved manually to the target position to be moved after gripping the workpiece and recorded as a first teaching position, a predetermined range around the first teaching position is determined as a measurement range, and the measurement range is determined. A workpiece shape measured by the workpiece shape measuring means is recorded as first master data in association with the first teaching position;
The end effector is moved to the gripping position and recorded as the second teaching position, a predetermined area around the second teaching position is determined as the measurement range, and the measurement range is measured by the workpiece shape measuring means. A teaching step of recording a workpiece shape as second master data in association with the second teaching position;
A predetermined range centered on the first teaching position is determined as a measurement range, a workpiece shape measured by the workpiece shape measuring means is defined as first measurement data, and the first master data and the first The measurement data is compared, the three-dimensional position and orientation of the first measurement data with respect to the first master data is calculated as the first correction amount,
A predetermined range centered on the second teaching position is determined as a measurement range, a workpiece shape measured by the workpiece shape measuring means is set as second measurement data, and the second master data and the second Comparing measurement data, calculating a three-dimensional position and orientation of the second measurement data with respect to the second master data as a second correction amount,
A command for correcting the gripping position according to the second correction amount, moving the end effector to the gripping position and gripping the workpiece,
A robot control method comprising: a regeneration step of correcting a target position that moves after gripping according to the first correction amount and generating a command to move the end effector that grips the workpiece to the corrected target position. In a control apparatus for a double-arm robot comprising an end effector for gripping a workpiece and a shape measuring means for the workpiece,
With respect to the workpiece gripped by the end effector of one arm, the end effector of the other arm provided with the workpiece shape measuring means is moved to the workpiece measurement position and recorded as the teaching position, and the teaching position is set as the center. A teaching step of determining a predetermined range as a measurement range, and recording the workpiece shape measured by the workpiece shape measuring means in association with the teaching position as master data;
A predetermined range centered on the teaching position is determined as a measurement range, a workpiece shape measured by a workpiece shape measuring unit is used as measurement data, the master data is compared with the measurement data, and the master data is compared. The three-dimensional position and orientation of the measurement data with respect to the data is calculated and used as a correction amount. In order to move the workpiece to the master data position according to the correction amount, the position of the arm holding the workpiece is corrected and moved. And a playback step for generating a command to perform the robot control. In the reproduction step, a predetermined range centered on the teaching position is determined as a first measurement range, the first measurement range is measured for the first time by a workpiece shape measuring means, and the measured workpiece shape is determined. As the first measurement data, the master data is compared with the first measurement data, and when there is not enough data to match the first measurement data with the master data, the first measurement is performed. A second measurement range in which the posture is changed with respect to the workpiece is generated from the range, the second measurement range is executed by the workpiece shape measuring means, and the first measurement data and the second measurement range are executed. The measurement data is overlaid as measurement data, the three-dimensional position and orientation of the measurement data with respect to the master data is calculated as a correction amount, and the gripping position is corrected according to the correction amount. Robot control method according to claim 1, characterized in that the step of generating a command to move the end effector to grip position said modified. A dual-arm robot having an end effector for gripping a workpiece on an arm, the dual-arm robot having a sensor for measuring the shape of the workpiece on the arm;
A control device for controlling the dual-arm robot, wherein a memory storing an operation program describing the operation of the robot, an analysis unit for analyzing the operation program, and writing the current position to the operation program as a teaching position A teaching data storage unit, a measuring unit that obtains measurement data of the workpiece shape from the sensor, a master data storage unit that stores the measurement data obtained at the time of teaching in a memory as master data, and the master data and the measurement data In comparison, a comparison unit that outputs the amount of change as a comparison result as a correction amount, a command generation unit that generates a trajectory of the arm based on a predetermined scan condition and the correction amount, and the generated trajectory A servo control unit for driving the motor, and a control device,
A teaching device connected to the control device, comprising: a teaching device that teaches an operation command of the dual-arm robot. 2. The robot control method according to claim 1, wherein the master data is workpiece shape measurement data measured by the workpiece shape measuring means along a helical scan trajectory. 2. The robot control method according to claim 1, wherein, in the reproducing step, the workpiece shape measuring means measures the workpiece shape along a trajectory of a helical scan.