JP6516546B2 - Control method of robot device, robot device, and method of manufacturing article - Google Patents

Description

  The present invention relates to a control method of a robot apparatus provided with an articulated arm having a plurality of joints, and a robot apparatus.

  2. Description of the Related Art Conventionally, a robot apparatus provided with a vertical articulated arm and an end effector (hereinafter referred to as a robot body) and a control device for controlling the robot body has become widespread. For example, when performing an operation of automatically attaching a gripped work to another work with such a robot apparatus, it is desired to work with high accuracy and high speed. For this reason, when the trajectory of the hand as the end effector is, for example, a straight line, a control method of the robot device is known in which the actual movement trajectory is corrected to a straight line in advance before performing the work of assembling the workpieces by the robot device. (See Patent Document 1). In this control method of the robot apparatus, a distance sensor dedicated to correction work is attached to the hand, and while the hand is moved straight along the flat plate, the distance between the flat plate and the distance sensor is measured simultaneously. Then, the error between the actual movement trajectory of the hand and the linear trajectory is calculated from the change in distance between the flat plate and the distance sensor, and the trajectory is corrected based on the error to make the actual movement trajectory linear. It can be corrected.

  However, in the control method of the robot apparatus of Patent Document 1, since the trajectory correction is performed before the hand holds the work, when the work is actually held, the work may be displaced in position and orientation with respect to the hand. The position and orientation of the workpiece can not be controlled with high precision. In this case, it is difficult to assemble the held work on another work with high accuracy. In addition, in this method of controlling the robot apparatus, both the installation of the distance sensor on the hand and the installation of the flat plate along the track must be performed with high accuracy, and each installation operation becomes complicated and the correction operation is long. It takes time.

  On the other hand, there is known a control method of a robot apparatus capable of detecting the position and orientation of a work relative to the hand with high accuracy when holding the work by the hand by providing a distance sensor on the inner surface of the finger of the hand. (See Patent Document 2). According to the control method of the robot apparatus, since the workpiece can be gripped with high precision in the position and orientation relationship with the hand, the position and orientation of the griped workpiece can be controlled with high accuracy, and thus the assembling accuracy to another workpiece can be achieved. Can be improved. Further, since the control is performed every time the work is held by the hand during the work by the robot apparatus, and a dedicated jig is not necessary, it is not necessary to take a long time for the preparation time in advance.

Japanese Patent Application Laid-Open No. 11-165283 JP-A-9-47986

  However, in the control method of the robot apparatus described in Patent Document 2, only the position and orientation of the workpiece with respect to the hand are set with high accuracy, and no consideration is given to the positions and orientations of other workpieces to be assembled. For this reason, even if the workpiece is gripped with a highly accurate position and orientation with respect to the hand, for example, if there is a shift in the position and orientation of another workpiece or an axis offset of the arm, the assembly accuracy may be degraded. was there.

  It is an object of the present invention to provide a control method of a robot apparatus and a robot apparatus capable of correcting a trajectory without using a dedicated tool and assembling a held work with another work with high accuracy. Do.

According to the present invention, there is provided a robot body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and a robot connected to the end effector and having a relative position between a first work and a second work. A detection unit that detects at least one of the amount of change in the force acting on the object and the amount of displacement of the relative position and orientation of the first work and the second work, and a control unit that controls the robot body a method of controlling a robot apparatus, the control section, the first workpiece held by the end effector, the first taught point, through the contact points, Before moving in orbit up to the second teaching point a moving step, the control section, when said at moving step a first workpiece is positioned on the track from the contact point to the second teaching point, the first workpiece and the second Of the variation of the force relative acting between the workpiece, prior to SL and the amount of displacement of the relative position and orientation of the first workpiece and the second workpiece, the first detecting at least one by the detection unit A detection step; a first calculation step of calculating the correction amount of the trajectory based on a change amount of the force detected in the first detection step or a displacement amount of the relative position and posture detected in the first detection step; And a first correction step of correcting a position and a posture of each of the first teaching point and the second teaching point based on the correction amount calculated in the first calculation step. It is characterized by

Further, according to the present invention, there is provided a robot main body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and the robot connected to the end effector, between a first work and a second work. A detection unit that detects at least one of the amount of change in force acting on the relative position and the amount of displacement of the relative position and orientation of the first work and the second work, and a control unit that controls the robot body a robot apparatus comprising, said control unit, said first workpiece held by the end effector, the first taught point, through the contact point is moved in a predetermined orbit to the second teaching point, When the first work is positioned on the track from the contact point to the second teaching point, the amount of change in force acting relative between the first work and the second work and, Serial and displacement of the first work and the relative position and orientation of the second workpiece, the detected at least one by the detection unit, based on the displacement amount of the detected said force variation or the relative position and orientation, the The correction amount of the trajectory is calculated, and each position and posture of the first teaching point and the second teaching point are corrected based on the calculated correction amount.

  According to the present invention, when the control unit moves the first work on the predetermined trajectory and executes the contact operation or the non-contact operation on the second work, the contact between the first work and the second work The amount of change in force of the first work or the amount of displacement of the relative position / posture due to the movement is detected. Then, the control unit calculates the correction amount of the trajectory based on the detected change amount of the force or the displacement amount of the relative position and posture, and corrects the trajectory based on the correction amount. For this reason, it is possible to assemble the held work with another work or the like with high accuracy without correcting the trajectory using a dedicated tool.

It is an explanatory view showing a schematic structure of a robot device concerning a 1st embodiment of the present invention. It is a front view which shows the operation | movement which mounts a 1st workpiece | work to a 2nd workpiece | work by the robot apparatus which concerns on the 1st Embodiment of this invention. (A) is a state in which the first work is positioned at an upper empty point with respect to the second work, (b) is a state in which the first work is positioned at a contact point with respect to the second work, (c) is a state It is a front view in the state where the 1st work was located in a descent point to the 2nd work. It is a graph which shows the relationship between the position of the 1st work in the robot device concerning a 1st embodiment of the present invention, and the reaction force to the 1st work, and (a) is before correction of a track, (b) is At the time of calculation of the virtual reaction force at the upper empty point, (c) is at the time of calculation of the correction amount. It is a flowchart which shows the procedure at the time of starting of the robot apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the procedure which correct | amends the track | orbit of the robot apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the procedure which correct | amends the track | orbit in the modification of the robot apparatus based on the 1st Embodiment of this invention. The robot apparatus which concerns on the 2nd Embodiment of this invention shows the operation | movement which mounts a 3rd workpiece | work to a 4th workpiece | work, (a) is a perspective view at the time of isolation | separation of a 3rd workpiece | work and a 4th workpiece, b) is a front view in the state where the 3rd work is located in the upper empty point to the 4th work. It is a front view which shows the operation | movement which mounts a 3rd workpiece | work to a 4th workpiece | work by the robot apparatus which concerns on the 2nd Embodiment of this invention. (A) is a state in which the third work is positioned at the contact point with respect to the fourth work, (b) is a state in which the third work is positioned at the switching point with respect to the fourth work, (c) is a state It is a front view in the state where the 3rd work was located in a descent point to the 4th work. It is a graph which shows the relationship between the position of the 3rd work in the robot apparatus concerning a 2nd embodiment of the present invention, and the reaction force to the 3rd work, (a) is before amendment of a track, (b) is At the time of calculation of the virtual reaction force at the upper empty point, (c) is at the time of calculation of the correction amount. It is a front view which shows the operation | movement which mounts a 5th workpiece | work to a 6th workpiece | work by the robot apparatus which concerns on the 3rd Embodiment of this invention. (A) is a state where the fifth work is positioned at the upper empty point with respect to the sixth work, (b) is a state where the fifth work is positioned at the contact point with respect to the sixth work, (c) is a state It is a front view in the state where the 5th work rose from the contact point. It is a front view which shows the operation | movement which mounts a 5th workpiece | work to a 6th workpiece | work by the robot apparatus which concerns on the 3rd Embodiment of this invention. (A) is a state in which the engagement portion of the fifth work is moved to the center side, (b) is a state in which the fifth work is lowered to abut on the sixth work, (c) is a fifth work Is a front view in a state where it is located at the lowering point with respect to the sixth work. It is a graph which shows the relationship between the position of the 5th work in the robot apparatus concerning a 3rd embodiment of the present invention, and the reaction force to the 5th work, and (a) is before amendment of a track, (b) is At the time of calculation of the virtual reaction force at the upper empty point, (c) is at the time of calculation of the correction amount. It is a front view which shows the operation | movement which mounts a 7th workpiece | work to an 8th workpiece | work by the robot apparatus which concerns on the 4th Embodiment of this invention. (A) is a front view of a state where the seventh work is positioned at an upper empty point with respect to the eighth work, (b) is a front view of a state where the seventh work is positioned at a contact point with respect to the eighth work . It is a front view which shows the operation | movement which mounts a 7th workpiece | work to an 8th workpiece | work by the robot apparatus based on the 4th Embodiment of this invention, (a) is the state which inclined the 7th workpiece, (b) Is a state in which the seventh work is positioned at the lowering point with respect to the eighth work. It is a graph which shows the relationship between the position of the 7th work in the robot device concerning a 4th embodiment of the present invention, and the reaction force to the 7th work, (a) is before amendment of a track, (b) is a At the time of calculation of the virtual reaction force at the upper empty point, (c) is at the time of calculation of the correction amount.

First Embodiment
Hereinafter, a first embodiment for carrying out the present invention will be described in detail with reference to the drawings. In the present embodiment, the first workpiece W1 held by the hand 21 and the second workpiece W2 for assembling the first workpiece W1 have a cylindrical shape, and the inner diameter of the first workpiece W1 is the second. It is assumed that the outer diameter of the workpiece W2 substantially matches. As a result, by making the central axis W1c of the first work W1 coincide with the central axis W2c of the second work W2, the second work W2 can be detachably assembled on the inner peripheral side of the first work W1. (See FIG. 2 (c)).

  As shown in FIG. 1, the robot apparatus 1 includes a robot body 2 and a control device (control unit) 3 that controls the robot body 2. The robot body 2 includes a 6-axis vertical articulated arm (hereinafter referred to as an arm) 20 having a plurality of joints, and a hand 21 which is an end effector. In the present embodiment, a six-axis vertical articulated arm is applied as the arm 20, but the number of axes may be changed as appropriate depending on the application and purpose.

  The arm 20 includes seven links 61 to 67, and six joints 71 to 76 swingably or rotatably connecting the links 61 to 67. As each link 61-67, what fixed length was employ | adopted. However, for example, a link that can be expanded and contracted by a linear actuator may be adopted. Each of the joints 71 to 76 includes, for example, a joint mechanism and a motor control unit (not shown), and operates according to a command from the control device 3. The base end link 61 is fixed to the base 60, and the base 60 is fastened and fixed to the mount 10 by a bolt or the like. In addition, the workpiece mounting table 11 is installed near the gantry 10, and the second workpiece W2 is mounted on the workpiece mounting table 11. In the present embodiment, the second workpiece W2 is fixed to the workpiece mounting base 11 when in contact with at least the first workpiece W1, and applies a reaction force to the first workpiece W1. It is supposed to be.

  The hand 21 is attached to and supported by the distal end link 67 of the arm 20, and the movement of the arm 20 adjusts at least one degree of position and posture. The hand 21 includes a hand body 24 and a plurality of fingers 23 which are disposed movably with respect to the hand body 24 and can grip the first workpiece W1. A force sensor (detection unit) 25 is provided at a connection portion of the hand main body 24 with the tip link 67. The force sensor 25 can detect an external force acting on the tip link 67 from the hand 21 with forces in three directions (X, Y, Z) and moments in three directions (Mx, My, Mz). The detected value is input to the control device 3 as a reaction force F. That is, the force sensor 25 is configured to detect the amount of change in force acting relative to the first work W1 and the second work W2.

  Here, when the first work W1 performs the contact operation on the second work W2 or the non-contact operation, the relative movement when the first work W1 and the second work W2 contact with each other Power relationships. Here, the contact operation is to change the first work W1 and the second work W2 from the non-contact state to the contact state, and the non-contact operation means the first work W1 and the second work W1. This is to change the work W2 from the contact state to the non-contact state. Further, in the present embodiment, the contact operation is a mounting operation, and the non-contact operation is a detachment operation. Since the second workpiece W2 is fixed to the workpiece mounting table 11, it is relatively fixed to the proximal end link 61 of the arm 20 via the workpiece mounting table 11, the floor, the gantry 10, and the base 60. On the other hand, since the first work W1 is gripped by the hand 21, it is relatively fixed to the tip link 67 of the arm 20. For this reason, the supporting relationship between the first work W1 and the second work W2 constitutes a closed loop, and by bringing the first work W1 into contact with the second work W2 using the robot main body 2, A reaction force is generated relatively between the first work W1 and the second work W2. The reaction force is detected by a force sensor 25 provided at the connecting portion between the arm 20 and the hand 21, and the force sensor 25 acts relatively between the first work W1 and the second work W2. The amount of change in force can be detected.

  The control device 3 is configured by a computer, and can control the robot main body 2 by controlling a motor. The computer constituting the control device 3 includes, for example, a CPU 30, a ROM 31 for storing a program for controlling each part, a RAM 32 for temporarily storing data, and an input / output interface circuit (I / F) 33. There is. Further, a teaching pendant 4 used to teach a teaching point or the like can be connected to the control device 3.

  The control device 3 relatively acts in the contact state of the first work W1 and the second work W2 when moving the first work W1 on the track and performing the mounting operation on the second work W2. The reaction force (amount of change) F is detected by the force sensor 25. Further, the control device 3 calculates the correction amount of the trajectory based on the reaction force F detected by the force sensor 25 and corrects the trajectory based on the correction amount obtained by the calculation.

  Here, using FIG. 2 and FIG. 3, the control device 3 mounts the first work W1 and the second work W2 to detect the reaction force F, calculates the correction amount of the track, and calculates the track The principle of correcting

  As shown in FIG. 2A, the central axis of the first work W1 in a state (non-contact state) in which the first work W1 is appropriately separated by a suitable distance without contacting directly above the second work W2. It arranges so that central axis W2c of W1c and the 2nd work W2 may be in agreement. The position and orientation of the first workpiece W1 at this time is taken as an empty point (first teaching point). At this time, the reaction force F detected by the force sensor 25 is zero.

  Then, while the central axis W1c of the first workpiece W1 and the central axis W2c of the second workpiece W2 are aligned by the robot body 2, the first workpiece W1 is moved downward. As a result, as shown in FIG. 2B, a portion of the lower end surface of the first work W1 contacts a portion of the upper end surface of the second work W2. The position and orientation of the first workpiece W1 at this time is taken as a contact point (third teaching point).

  Furthermore, the first workpiece W1 is moved downward by the robot body 2 while the central axis W1c of the first workpiece W1 and the central axis W2c of the second workpiece W2 coincide with each other. As a result, as shown in FIG. 2C, the first work W1 is fitted to the second work W2 to the end. The position and orientation of the first workpiece W1 at this time is taken as a lowering point (second teaching point). At this time, if the position or angle between the central axis W1c of the first work W1 and the central axis W2c of the second work W2 is deviated, the inner peripheral surface of the first work W1 and the outer circumference of the second work W2 Contact with the surface generates a reaction force F on each other. Further, a straight line connecting the upper empty point and the lower point is a predetermined trajectory.

  The reaction force F is fitted from the state (contact point in FIG. 3 (a)) in which part of the end face shown in FIG. 2 (b) is in contact with each other (the contact point in FIG. 3 (a)). It rises linearly to the state (falling point in FIG. 3 (a)). That is, in the present embodiment, the reaction force F is a change amount in which the first work W1 is displaced by the contact operation of the first work W1 and the second work W2. The state between the state of FIG. 2 (b) and the state of FIG. 2 (c), that is, the state between the contact point and the lowering point is the contact state between the first workpiece W1 and the second workpiece W2. It is.

  For example, as shown in FIG. 3A, the stroke from the upper empty point to the contact point is A2, the stroke from the contact point to the descending point is A1, and the reaction force F detected by the force sensor 25 is from the contact point It is assumed that it linearly rises up to the falling point and becomes F1 at the falling point. In this case, the slope ΔF of the reaction force from the contact point to the falling point is ΔF = F1 / A1.

  Next, as shown in FIG. 3B, the inclination ΔF of the reaction force from the contact point to the falling point is extended to the upper empty point, and the second work W2 is extended to the upper empty point. It calculates taking into consideration the reaction force F2 that the work W1 of 1 receives. The reaction force F2 is F2 = −ΔF × A2. Incidentally, as shown in FIG. 3 (b), the reaction force F2 can also be determined by drawing as an intersection point of a straight line extending the reaction force F1 at the falling point and the reaction force 0 at the contact point. .

  Furthermore, as shown in FIG. 3C, the reaction force F is converted into a correction amount X, which is, for example, the length in the horizontal direction, for the up empty point and the down point. Therefore, assuming that the elastic modulus of the robot body 2 is K, the correction amount X2 at the upper empty point is X2 = F2 / K, and the correction amount X1 at the falling point is X1 = F1 / K. The correction amounts X1 and X2 with respect to the upper empty point and the falling point become correction amounts of the trajectory. Then, the correction amount X2 is added to the current position and orientation of the sky point to correct, and the position and orientation of the current drop point is corrected to add the correction amount X1 to connect the corrected sky point and the falling point. Trajectory correction can be performed by using the straight line as the corrected trajectory. Here, although the elastic modulus K uses the elastic modulus of the robot main body 2, it is not limited to this, for example, the elastic modulus of the finger 23 of the hand 21 may be used. The elastic modulus of which portion is to be used as the elastic modulus K can be appropriately set, for example, by the configuration of the robot device 1 or the operation of the robot body 2 or the like.

  The procedure of the control method of the robot apparatus 1 described above will be described along the flowcharts shown in FIGS. 4 and 5.

  As shown in FIG. 4, when starting up the robot device 1, the user sets a temporary teaching point offline to create a basic operation program (step S 1) and install it on the actual robot device 1. (Step S2). Thereafter, the user debugs the program (step S3), and starts teaching when it is completed (step S4).

  In the teaching operation, the robot device 1 is taught by positioning the first workpiece W1 using the second workpiece W2 or a dedicated tool (not shown) and registering it as a teaching point. In the present embodiment, the position and orientation of the first workpiece W1 shown in FIG. 2A is taught as an air gap, and the position and orientation of the first workpiece W1 shown in FIG. 2C are taught as a descending point. The control device 3 sets a straight line in which the upper empty point and the lower point are directly connected as a track, the operation program is completed, and the teaching work is completed. The trajectory is an open position that is the position and orientation of the first workpiece W1 in a non-contacting state, a descending point that is a position and orientation of the first workpiece W1 in a contacting state, and a position and orientation of the first workpiece W1 in a contacting state And calculated on the basis of a contact point located between the upper empty point and the lower point on the orbit. That is, in the present embodiment, the trajectory at the falling point and the trajectory at the contact point are straight trajectories. After the teaching operation, as shown in FIG. 2, the control device 3 drives the robot body 2 to perform a checking operation (step S5), and shifts to an actual assembling operation (step S6). In the assembly operation, the control device 3 operates the operation program continuously.

  After maintenance of the robot apparatus 1 or after setup change, as shown in FIG. 5, the operation of trajectory correction is performed and then the assembly operation is performed. First, the user activates the robot apparatus 1 (step S11). Then, the control device 3 reads the operation program (step S12) and executes the preparation operation (step S13). In the preparation operation, the robot body 2 is operated at a low speed, and the user checks whether the first workpiece W1 and the robot body 2 do not interfere with other members or the like.

  After confirming the operation, the control device 3 moves the first workpiece W1 from the upper empty point to the descending point at low speed (mounting operation), and the force sensor 25 causes the first workpiece W at least at the contact point and the descending point. A reaction force F acting on W1 is detected (step S14, first detection step). Here, the control device 3 constantly detects the reaction force F by the force sensor 25 and detects the reaction force F over the whole area of the trajectory from the upper empty point to the descending point (FIG. a) see). In the present embodiment, the first workpiece W1 is moved at a low speed from the upper empty point to the descending point. However, the present invention is not limited thereto and may move at an actual speed.

  Then, based on the detected reaction force F, the control device 3 calculates the correction amount X1 of the position in the horizontal direction at the falling point and the correction amount X2 of the position in the horizontal direction at the empty point (step S15) Calculation process) (see FIG. 3 (b) (c)). That is, in the first calculation step, the control device 3 calculates the correction amount of each position and orientation of the up and down points as the correction amount of the trajectory based on the change amount of the force detected in the first detection step. . Further, the control device 3 corrects the horizontal position of the two teaching points of the up and down points based on the position correction amounts X1, X2 (step S16, first correction step). Thereby, the trajectory between the upper empty point and the lower point is corrected. That is, in the first correction step, the control device 3 corrects the positions and orientations of the upward empty point and the downward point based on the correction amounts of the respective positions and orientations of the upward empty point and the downward point calculated in the first calculation step. Correct the trajectory.

  After the first trajectory correction, the control device 3 moves the first workpiece W1 from the upper empty point to the descending point at low speed, and the force sensor 25 again causes the first workpiece at at least the contact point and the descending point. A reaction force F acting on W1 is detected (step S17, second detection step). Similar to the first detection of the reaction force F in step S14, the control device 3 constantly detects the reaction force F by the force sensor 25, and the controller 3 is antipodal over the entire trajectory from the upper empty point to the descending point. Force F is to be detected.

  Then, the control device 3 determines whether or not the detected reaction force F is within the preset specified value (step S18). If the reaction force F is within the specified value, the specified value here is a value that assumes that the linearity of the track is sufficient, and can be set as appropriate. If the control device 3 determines that the detected reaction force F is within the specified value, the process proceeds to assembly work as the trajectory is corrected (step S19).

  When the control device 3 determines that the reaction force F is not within the specified value, the control device 3 corrects the correction amount θ1 of the posture (inclination) at the falling point based on the detected reaction force F, and A correction amount θ2 of posture (tilt) is calculated (step S20, second calculation step). Further, the control device 3 corrects the postures (tilts) of the two teaching points of the up-empty point and the down-point based on the correction amounts θ1 and θ2 (step S21, second correction step). Thereby, the trajectory between the upper empty point and the lower point is corrected again. After the second trajectory correction, the control device 3 executes step S17 and subsequent steps, and repeatedly executes the process until the reaction force F becomes less than the specified value and the trajectory is corrected.

  As described above, according to the robot device 1 of the present embodiment, the control device 3 moves the first workpiece W1 on the track to execute the mounting operation for the second workpiece W2. At that time, the control device 3 detects a reaction force F generated by displacement of the first workpiece W1 due to the contact operation of the first workpiece W1 and the second workpiece W2. Then, the control device 3 calculates the correction amounts X1, X2, θ1, θ2 of the trajectory based on the reaction force F, and corrects the trajectory based on them. For this reason, it is possible to assemble the held first work W1 to the second work W2 with high accuracy without correcting the trajectory using a dedicated tool. Moreover, according to the robot device 1 of the present embodiment, the robot device does not use a singular point and realizes stable operation because the same corrected teaching point is continuously used even after shifting to the assembly operation. Can.

  Further, according to the robot device 1 of the present embodiment, the control device 3 first corrects the position of the first workpiece W1 based on the reaction force F (steps S14 to S16 in FIG. 5). And the control apparatus 3 correct | amends the attitude | position of 1st workpiece | work W1 when the shift | offset | difference of a track | orbit can not be suppressed within the stipulated amount also by this correction | amendment (step S17-S21 of FIG. 5). That is, in order to correct the trajectory, correction is performed in two steps. Here, in general, the error in the position and orientation of the teaching point at the time of maintenance of the arm 20 is more likely to occur at a position than the posture. For this reason, as in the present embodiment, by correcting the position first and then correcting the attitude, there is a high possibility that the error of the trajectory can be suppressed within the regulation amount only by the first correction. Good trajectory correction work can be realized.

  In the embodiment described above, the control device 3 first corrects the position of the first workpiece W1 based on the reaction force F, and the first deviation can not be suppressed within the specified amount. Although the case of correcting the posture of the work W1 has been described, the present invention is not limited thereto. For example, the control device 3 first corrects the posture of the first work W1 based on the reaction force F, and corrects the position of the first work W1 when the deviation of the trajectory can not be suppressed within the prescribed amount. You may do so. As described above, the order of performing the correction of the position and the posture can be set as appropriate depending on, for example, the shape of the workpieces to be assembled, the performance of the robot main body 2, and the like.

  Further, in the above-described embodiment, the control device 3 detects the reaction force F received by the first work W1 from the second work W2 by the force sensor 25 as a detection unit. Although each correction amount is calculated, it is not limited to this. For example, a displacement amount at which the first work W1 displaces the position and orientation due to contact with the second work W2 is detected by a position sensor such as a camera as a detection unit, and each correction amount is calculated based on the displacement amount. You may do so. For example, when using a camera, since the force sensor 25 can be omitted, the weight of the hand 21 can be reduced. Further, whether the reaction force F is detected using the force sensor 25 as the detection unit or the displacement amount of the position and orientation is detected using the camera 5 depends on, for example, the shape of the workpieces to be assembled and the performance of the robot main body 2 It can be set appropriately.

  Here, when the camera 5 (see FIG. 1) is used as the detection unit and the control device 3 first corrects the position of the first workpiece W1 and the deviation of the trajectory can not be suppressed within the prescribed amount, the first The correction of the posture of the workpiece W1 will be described with reference to FIG. The camera 5 is a stereo camera installed on the ceiling, but is not limited thereto. For example, the first work W1 may be a camera capable of photographing from the side, or a single eye depending on the movable direction of the first work W1. It may be a camera of The camera 5 captures the first work W1, but the invention is not limited thereto. For example, the position and orientation of the first work W1 may be detected by capturing the finger 23 or the like of the hand 21. . That is, the camera 5 is configured to detect the amount of displacement of the relative position and orientation of the first workpiece W1 and the second workpiece W2. As the detection unit, the reaction force F acting relatively between the first work W1 and the second work W2 and the displacement amount of the relative position and posture of the first work W1 and the second work W2 What is necessary is just to detect at least one.

  First, the user activates the robot device 1 (step S31). Then, the control device 3 reads the operation program (step S32), and executes the preparation operation (step S33). The operation up to this point is the same as steps S11 to S13 in FIG.

  After confirmation of the operation, the control device 3 moves the first workpiece W1 from the upper empty point to the descending point at low speed, and the camera 5 displaces the position and orientation of the first workpiece W1 at least at the contact point and the descending point. The amount is detected (step S34, first detection step). Here, the amount of displacement of the relative posture of the first workpiece W1 and the second workpiece W2 when the non-contact state and the contact state are switched by the camera 5 is detected. The control device 3 constantly detects the position and orientation of the first workpiece W1 with the camera 5, and detects the entire position of the trajectory from the upper empty point to the lower point.

  Then, the control device 3 calculates the correction amount θ1 of the posture (tilt) at the falling point and the posture (tilt) θ2 at the upper empty point based on the detected displacement amount of the position and posture (step S35, first calculation) Process). Further, the control device 3 corrects the postures of the two teaching points of the upper empty point and the falling point based on the correction amounts θ1 and θ2 of the posture (step S36, first correction step). Thereby, the trajectory between the upper empty point and the lower point is corrected.

  After the first trajectory correction, the control device 3 moves the first workpiece W1 from the upper empty point to the descending point at low speed, and the camera 5 again performs the first work W1 at the contact point and the descending point. The displacement amount of the position and orientation is detected (step S37, second detection step). Here, the amount of displacement of the relative position of the first workpiece W1 and the second workpiece W2 when the non-contacting state and the contacting state are switched by the camera 5 is detected. Similar to the first detection of the amount of displacement of the position and orientation in step S34, the control device 3 always detects the amount of displacement using the camera 5, and the amount of displacement over the entire trajectory from the upper empty point to the descending point Is supposed to be detected.

  Then, the control device 3 determines whether or not the detected displacement amount is within the preset specified value (step S38). The specified value here is a value that assumes that the linearity of the track is sufficient if it is a displacement amount within the specified value, and can be set as appropriate. If the control device 3 determines that the detected displacement amount is within the specified value, the process proceeds to assembly work as the trajectory is corrected (step S39).

  When the control device 3 determines that the displacement amount is not within the specified value, the control device 3 corrects the horizontal position correction amount X1 at the falling point and the horizontal direction at the upper empty point based on the detected displacement amount. The position correction amount X2 is calculated (step S40, second calculation step). That is, in the second calculation step, based on the amount of displacement of the relative position and posture detected in the second detection step, the controller 3 corrects the amounts of correction of the positions and postures of the up and down points as trajectory correction amounts. Calculate Furthermore, the control device 3 corrects the positions of the two teaching points of the up-empty point and the falling point based on the correction amounts X1 and X2 (step S41, second correction step). Thereby, the trajectory between the upper empty point and the lower point is corrected again. That is, in the second correction step, the control device 3 corrects the positions and orientations of the upward empty point and the downward point based on the correction amounts of the respective positions and orientations of the upward empty point and the downward point calculated in the second calculation step. Correct the trajectory. After the second trajectory correction, the control device 3 executes step S37 and subsequent steps, and repeatedly executes the process until the displacement amount becomes less than the specified value and the trajectory is corrected.

  In the embodiment described above, for example, as in step S14 of FIG. 5, the reaction force F is detected using the force sensor 25 in the mounting operation for moving the first workpiece W1 from the upper empty point to the lower point Although it explained about, it is not restricted to this. For example, in a separation operation in which the first work W1 is gripped by the hand 21 in a state in which the first work W1 and the second work W2 are mounted, and the first work W1 is pulled up from the lowering point to the upper empty point 25 may be used to detect the reaction force F.

  Moreover, although the case where the reaction force F was detected by two points of a contact point and a fall point was demonstrated in embodiment mentioned above, it is not restricted to this. For example, if the first work W1 and the second work W2 are not in contact with each other, the reaction force F is 0, so that the reaction force F in the contact state is detected by detecting only the reaction force F at the descending point. The trajectory may be corrected by calculating the slope ΔF of Alternatively, the reaction force F may be detected at three or more points between the contact point and the descending point, and the inclination ΔF of the reaction force F may be calculated by, for example, the least squares method.

  Further, in the embodiment described above, the case where the correction of the trajectory is performed at the time of maintenance or the like has been described, but the present invention is not limited thereto. For example, it is performed at the time of confirmation operation of step S5 in FIG. May be

  Moreover, although the case where the track | orbit was linear was demonstrated in embodiment mentioned above, it is not restricted to this. For example, the track may be in the form of a broken line or in the form of a curved line such as an arc or a spiral. In any case, high-precision assembly is realized by detecting the reaction force F and the displacement amount of the position and orientation in the contact state between the first work W1 and the second work W2 and correcting the trajectory based thereon. can do.

Second Embodiment
Next, a second embodiment for carrying out the present invention will be described in detail with reference to the drawings. In the present embodiment, the shapes of the third workpiece (first workpiece) W3 and the fourth workpiece (second workpiece) W4 to be assembled are the same as those of the workpieces W1 and W2 of the first embodiment. It is different from the shape. The configuration of the robot apparatus 1 is the same as that of the first embodiment, so the reference numerals are the same and the detailed description is omitted.

  In the present embodiment, as shown in FIG. 7A, the third work W3 held by the hand 21 and the fourth work W4 for assembling the third work W3 have a cylindrical shape, and the third work W3 is a third work W3. The inner diameter of the workpiece W3 substantially matches the outer diameter of the fourth workpiece W4. The third work W3 has a shape protruding toward the center on the inner circumferential surface, and has projections W3a provided at three locations at equal intervals in the circumferential direction.

  The fourth work W4 has a substantially L-shaped engagement groove W4a provided in three places spaced at equal intervals in the circumferential direction. The engagement groove W4a is parallel to the axial direction of the fourth work W4 and has a parallel portion W4b extended from the edge of the fourth work W4 to the axial central portion, and the axial central portion side of the parallel portion W4b And an orthogonal portion W4d which is orthogonal to and continuous with the end portion of The parallel portions W4b of the three engagement grooves W4a are open to one side of the fourth work W4 in the axial direction. The orthogonal portions W4d of the three engaging grooves W4a have the same circumferential direction orthogonal to the parallel portions W4b.

  Each protrusion W3a of the third work W3 is insertable into the engagement groove W4a of the fourth work W4. The protrusion W3a of the third work W3 is inserted into and engaged with the engagement groove W4a of the fourth work W4, whereby the third work W3 is attached to the fourth work W4. That is, when the third work W3 is attached to the fourth work W4, the projection W3a of the third work W3 is inserted from one axial end of the fourth work W4 into the parallel part W4b (see FIG. a) see). Then, after the protrusion W3a of the third work W3 abuts on the deepest part of the parallel part W4b of the fourth work W4 (see FIG. 8B), the third work W3 is rotated, The protrusion W3a of the third work W3 is inserted into the orthogonal portion W4d of the fourth work W4. Furthermore, when the projection W3a of the third work W3 abuts on the deepest part of the orthogonal part W4d of the fourth work W4 (refer to FIG. 8C), the third work W3 works against the fourth work W4. Be worn. On the other hand, when removing the third work W3 from the fourth work W4, the operation reverse to the above is performed. As described above, in the present embodiment, the third work W3 is a snap ring that receives the force in the thrust direction, and the fourth work W4 is a unit to which the snap ring is detachably attached.

  Here, the control device 3 mounts the third work W3 and the fourth work W4 using FIG. 7 to FIG. 9, detects the reaction force F, calculates the correction amount of the track, and calculates the track The principle of correcting In the present embodiment, the reaction force F is a force in the Z direction, ie, an axial direction of the tip link 67 among external forces acting on the tip link 67 detected by the force sensor 25 from the hand 21. Power. Further, as the reaction force F in this case, among external forces acting from the hand 21 on the tip link 67 detected by the force sensor 25, forces in the X and Y directions other than the force in the Z direction and three directions. The moments (Mx, My, Mz) are not included. Further, the coordinate system of the force sensor 25 is integrated with the coordinate system of the hand 21, and the coordinate system of the force sensor 25 also moves and rotates with the change of the position and orientation of the hand 21.

  As shown in FIG. 7 (b), the central axis of the third work W3 in a state (non-contact state) where the third work W3 is appropriately separated by a suitable distance without contacting immediately above the fourth work W4. It arranges so that W3c and central axis W4c of the 4th work W4 may be in agreement. The position and orientation of the third workpiece W3 at this time is set as an empty point (first teaching point). At this time, the reaction force F detected by the force sensor 25 is zero.

  Then, the third work W3 is moved downward while the central axis W3c of the third work W3 and the central axis W4c of the fourth work W4 coincide with each other by the robot body 2. Thereby, as shown to Fig.8 (a), a part of lower end surface of 3rd workpiece | work W3 and a part of upper end surface of 4th workpiece | work W4 contact. The position and orientation of the third workpiece W3 at this time is taken as a contact point (third teaching point).

  Further, the third work W3 is moved downward while the central axis W3c of the third work W3 and the central axis W4c of the fourth work W4 coincide with each other by the robot body 2. As a result, as shown in FIG. 8B, the projection W3a of the third work W3 abuts on the lowermost portion of the parallel part W4b of the fourth work W4. The position and orientation of the third workpiece W3 at this time is taken as a switching point.

  Then, while the central axis W3c of the third workpiece W3 and the central axis W4c of the fourth workpiece W4 are aligned by the robot body 2, the third workpiece W3 is centered on the Z-axis (central axes W3c and W4c) Rotate in the R direction. As a result, as shown in FIG. 8C, the projection W3a of the third work W3 is fitted to the deepest part of the orthogonal part W4d of the fourth work W4. The position and orientation of the third workpiece W3 at this time is taken as a falling point (second teaching point). Note that, with the rotation of the third workpiece W3 about the Z axis, the coordinate system of the hand 21 also rotates, and the directions of the X axis and the Y axis change. At this time, since the third work W3 rotates about the Z axis, the position in the Z axis direction does not change. Further, a straight line connecting the upper empty point and the lower point is a predetermined trajectory. That is, in the present embodiment, the trajectory at the contact point is a linear trajectory, and the trajectory at the falling point is a trajectory in the rotational direction about an axis in the same direction as the straight line at the contact point.

  The reaction force F is fitted from the state (contact point in FIG. 9 (a)) in which part of the end face shown in FIG. 7 (b) is in contact (the contact point in FIG. 9 (a)) to the end shown in FIG. It rises linearly to the state (falling point (switching point) in FIG. 9A). That is, in the present embodiment, the reaction force F is a change amount in which the third work W3 is displaced by the contact operation of the third work W3 and the fourth work W4. In the present embodiment, the state of FIG. 8A and the state of FIG. 8C, that is, the states of the contact point and the falling point are the third work W3 and the fourth work W4, respectively. It is in a contact state.

  Here, for example, as shown in FIG. 9A, the stroke in the Z direction from the upper empty point to the contact point is A2, and the stroke in the Z direction from the contact point to the falling point (switching point) is A1. It is assumed that the reaction force F in the Z direction detected by the force sensor 25 linearly rises from the contact point to the falling point and becomes F1 at the falling point (switching point). In addition, the reference waveform in the case where there is no displacement of the relative position and orientation measured in advance is acquired as a theoretical value, and the reaction force F in the Z direction detected by the force sensor 25 at that time is broken linewise from the contact point to the falling point Suppose that it rises and it becomes F0 at the falling point. In this case, the slope ΔF of the difference between the reaction force F from the contact point to the falling point with and without the relative position / posture displacement is ΔF = (F1−F0) / A1.

  Next, as shown in FIG. 9B, the slope ΔF of the difference of the reaction force F from the contact point to the falling point is extended to the upper empty point, and the fourth work W4 is extended to the upper empty point. In this case, it is calculated on the assumption of a reaction force F2 that the third work W3 will receive. The reaction force F2 is F2 = ΔF × A2. As shown in FIG. 9 (b), the reaction force F2 is the point of intersection of the falling point (reaction force F1) and the upper empty point of the straight line connecting the contact point (reaction force 0) and the falling point (reaction force F0). And it can obtain | require also by drawing as a difference of and the intersection in the upper empty point of the straight line which connected the contact point (reaction force 0).

  Furthermore, as shown in FIG. 9C, the reaction force F is converted into a correction amount Z, which is the length in the Z direction, for the up empty point and the down point. Therefore, assuming that the elastic modulus of the robot body 2 is K, the correction amount Z2 at the upper empty point is Z2 = F2 / K, and the correction amount Z1 at the falling point is Z1 = F1 / K. The correction amounts Z1 and Z2 with respect to the upper empty point and the falling point become correction amounts of the trajectory. Then, the correction amount Z2 is added to the Z direction of the position and orientation of the current sky point to correct it, and the correction amount Z1 is added to correct the Z direction of the position and posture of the current drop point to correct the sky space after correction. Trajectory correction can be performed by using the straight line connecting the falling point as the corrected trajectory.

  The procedure of the control method of the robot apparatus 1 described above is the same as that of the first embodiment (FIG. 4 and FIG. 5), and thus the detailed description will be omitted. In addition, the order of the correction of the position and the correction of the posture with respect to the third workpiece W3 can be appropriately set as in the first embodiment.

  As described above, also in the robot apparatus 1 according to the present embodiment, the control device 3 moves the third work W3 on the track to execute the mounting operation on the fourth work W4. At that time, the control device 3 detects a reaction force F generated by displacement of the third work W3 due to the contact operation of the third work W3 and the fourth work W4. Then, the control device 3 calculates the correction amounts Z1 and Z2 in the Z direction of the trajectory based on the reaction force F, and corrects the trajectory based on them. For this reason, it is possible to assemble the held third work W3 to the fourth work W4 with high accuracy, without correcting the trajectory using a dedicated tool.

  Also in the present embodiment, similarly to the first embodiment, the displacement amount at which the third work W3 is displaced in position and orientation due to the contact with the fourth work W4 without using the force sensor 25 is obtained. Each correction amount may be calculated based on the amount of displacement detected by the camera 5.

Third Embodiment
Next, a third embodiment for carrying out the present invention will be described in detail with reference to the drawings. In the present embodiment, the shapes of the fifth work (first work) W5 to be assembled and the sixth work (second work) W6 are the same as those of the works W1 and W2 of the first embodiment. It is different from the shape. The configuration of the robot apparatus 1 is the same as that of the first embodiment, so the reference numerals are the same and the detailed description is omitted.

  In the present embodiment, for example, as shown in FIGS. 11A to 11C, the fifth work W5 held by the hand 21 has a disk shape, and the sixth work W6 for assembling the fifth work W5 has a cylindrical shape. It is. The fifth work W5 protrudes from one side surface along the central axis W5c, and has a claw-shaped engaging portion W5a whose tip is bent to the outer peripheral side. The sixth work W6 has an edge W6a shaped like a flange that protrudes inward from one end in the axial direction.

  When the engaging portion W5a of the fifth work W5 is engaged with the edge W6a of the sixth work W6, the fifth work W5 and the sixth work W6 coincide with each other in central axes W5c and W6c. It forms so that (refer FIG.11 (c)). Then, the engagement portion W5a of the fifth work W5 engages with the edge W6a of the sixth work W6, whereby the fifth work W5 is attached to the sixth work W6. That is, when attaching the fifth work W5 to the sixth work W6, the fifth work W5 is made to approach from the edge W6a side of the sixth work W6 (see FIG. 11A); The workpiece W5 is in surface contact with the edge W6a of the sixth workpiece W6 (see FIG. 11B). Then, the fifth work W5 is slid vertically in the axial direction, and the engagement portion W5a of the fifth work W5 is engaged with the edge W6a of the sixth work W6 (see FIG. 11C). As a result, the fifth work W5 is detachably attached to the sixth work W6. On the other hand, when removing the fifth work W5 from the sixth work W6, the operation reverse to the above is performed.

  Here, the control device 3 mounts the fifth work W5 and the sixth work W6 using FIG. 10 to FIG. 12, detects the reaction force F, calculates the correction amount of the track, and calculates the track The principle of correcting In the present embodiment, the reaction force F is a force in the X direction among external forces acting on the tip link 67 detected by the force sensor 25 from the hand 21. That is, the reaction force F is orthogonal to the axial direction (Z direction) of the tip link 67, and is an X direction in which the engaging portion W5a of the fifth work W5 is engaged with or released from the edge W6a of the sixth work W6. (See Fig. 11 (c) etc.). Further, as the reaction force F in this case, among external forces acting from the hand 21 on the tip link 67 detected by the force sensor 25, forces in the Y and Z directions other than the force in the X direction and in three directions. The moments (Mx, My, Mz) are not included.

  As shown in FIG. 10A, the fifth work W5 is disposed in a state (non-contact state) appropriately separated by a distance without contacting the sixth work W6. At this time, the fifth work W5 is horizontal, and the sixth work W6 does not exist immediately below the engagement portion W5a, and the lowermost portion of the engagement portion W5a is lower than the uppermost surface of the sixth work W6. It is made to hold | maintain by the hand 21 in the positioned state. The position and orientation of the fifth work W5 at this time is set as an empty point (first teaching point). At this time, the reaction force F detected by the force sensor 25 is zero.

  Then, the fifth work W5 is moved by the robot body 2 in the direction (X direction) in which the central axis W5c of the fifth work W5 and the central axis W6c of the sixth work W6 coincide. As a result, as shown in FIG. 10B, the engaging portion W5a of the fifth work W5 abuts on the side surface of the sixth work W6. The position and orientation of the fifth work W5 at this time is taken as a contact point (third teaching point).

  Further, the fifth work W5 is moved upward by the robot body 2. As a result, as shown in FIG. 10C, the engaging portion W5a of the fifth work W5 is not located on the side of the sixth work W6. Then, the fifth work W5 is moved in the X direction by the robot body 2 until the engagement portion W5a of the fifth work W5 exceeds the edge W6a of the sixth work W6. As a result, as shown in FIG. 11A, the engaging portion W5a of the fifth work W5 is located closer to the central axis W6c than the edge W6a of the sixth work W6.

  Further, the fifth work W5 is moved downward by the robot body 2. Thereby, as shown in FIG. 11B, the fifth work W5 abuts on the top surface of the sixth work W6. At this time, the engaging portion W5a of the fifth work W5 enters the inside of the edge W6a of the sixth work W6, and the engaging portion W5a does not interfere with the edge W6a.

  Then, the robot main body 2 makes the fifth work W5 c of the fifth work W5 coincide with the center axis W6 c of the sixth work W6, that is, the engagement portion W5 a is engaged with the edge W6 a. The workpiece W5 is moved in the X direction. As a result, as shown in FIG. 11C, the engaging portion W5a of the fifth work W5 engages with the edge W6a of the sixth work W6. The position and orientation of the fifth work W5 at this time is taken as a lowering point (second teaching point). At this time, when the position or angle between the central axis W5c of the fifth work W5 and the central axis W6c of the sixth work W6 is shifted, the engagement portion W5a of the fifth work W5 and the sixth work W6 are They contact with each other at places other than the edge W6a, and an excessive reaction force F is generated. Further, a straight line connecting the upper empty point and the lower point is a predetermined trajectory. That is, in the present embodiment, the trajectory at the falling point and the trajectory at the contact point are linear trajectories in the same direction, and the fifth workpiece W5 and the sixth workpiece W6 are not between each trajectory. It is a touch.

  This reaction force F is shown in FIG. 11B from the state (the contact point in FIG. 12A) in which the engaging portion W5a of the fifth workpiece W5 shown in FIG. 10B contacts the side portion of the sixth workpiece W6. It moves up and down in the form of a broken line to the state (falling point in FIG. 12 (a)) which is fitted up to the end shown in (c). That is, in the present embodiment, the reaction force F is a change amount in which the fifth work W5 is displaced by the contact operation of the fifth work W5 and the sixth work W6. The state of FIG. 10 (b) and the state of FIG. 11 (c), that is, each state of the contact point and the lowering point is a contact state of the fifth work W5 and the sixth work W6.

  Here, for example, as shown in FIG. 12A, the stroke in the X direction from the upper empty point to the contact point is A2, and the stroke in the X direction from the contact point to the falling point is A1. It is assumed that the reaction force F in the X direction detected by the force sensor 25 ascends and descends from the contact point to the falling point, and becomes f1 at the contact point and F1 at the falling point. In addition, the reference waveform in the case where there is no displacement of the relative position and orientation measured in advance is acquired as a theoretical value, and the reaction force F in the X direction detected by the force sensor 25 at that time is broken linewise from the contact point to the falling point It raises and lowers, and it will be assumed that it became f0 in a contact point, and became F0 in a fall point. In this case, the slope ΔF of the difference between the reaction force F from the contact point to the falling point with and without displacement of the relative position and posture is ΔF = ((F1−F0) − (f1−f0)) / A1. become.

  Next, as shown in FIG. 12B, the slope ΔF of the difference of the reaction force F from the contact point to the falling point is extended to the upper empty point, and the sixth work W6 is extended to the upper empty point. In this case, it is calculated on the assumption of the reaction force F2 that the fifth work W5 will receive. The reaction force F2 is F2 = .DELTA.F.times.A2 + (f1-f0). As shown in the figure, the reaction force F2 is the point of intersection of the falling point (reaction force F1) and the contact point (reaction force f1) on the straight line connecting the depression point (reaction force F0) and the contact point It can also be determined by drawing as the difference between the point of intersection of the upper empty point of the straight line connecting the (reaction force f0).

  Further, as shown in FIG. 12C, the reaction force F is converted into a correction amount X, which is the length in the X direction, for the up empty point and the down point. Therefore, assuming that the elastic modulus of the robot body 2 is K, the correction amount X2 at the upper empty point is X2 = F2 / K, and the correction amount X1 at the falling point is X1 = F1 / K. The correction amounts X1 and X2 with respect to the upper empty point and the falling point become correction amounts of the trajectory. Then, the correction amount X2 is added to the X direction of the position and orientation of the current sky point to correct it, and the correction amount X1 is added to correct the X direction of the position and posture of the current drop point to correct the sky space after correction. Trajectory correction can be performed by using the straight line connecting the falling point as the corrected trajectory.

  The procedure of the control method of the robot apparatus 1 described above is the same as that of the first embodiment (FIG. 4 and FIG. 5), and thus the detailed description will be omitted. In addition, the order of the correction of the position and the correction of the posture with respect to the fifth workpiece W5 can be set as appropriate as in the first embodiment.

  As described above, also in the robot apparatus 1 according to the present embodiment, the control device 3 moves the fifth work W5 on the track to execute the mounting operation on the sixth work W6. At that time, the control device 3 detects a reaction force F generated by displacement of the fifth work W5 due to the contact operation of the fifth work W5 and the sixth work W6. Then, the control device 3 calculates the correction amounts X1 and X2 in the X direction of the trajectory based on the reaction force F, and corrects the trajectory based on them. For this reason, it becomes possible to assemble the held fifth work W5 with high precision to the sixth work W6 without correcting the trajectory using a dedicated tool.

  Further, also in the present embodiment, similarly to the first embodiment, the displacement amount at which the fifth work W5 is displaced in position and orientation due to the contact with the sixth work W6 without using the force sensor 25 is obtained. Each correction amount may be calculated based on the amount of displacement detected by the camera 5.

Fourth Embodiment
Next, a fourth embodiment for carrying out the present invention will be described in detail with reference to the drawings. In the present embodiment, the shapes of the seventh work (first work) W7 and the eighth work (second work) W8 to be assembled are the same as those of the works W1 and W2 of the first embodiment. It is different from the shape. The configuration of the robot apparatus 1 is the same as that of the first embodiment, so the reference numerals are the same and the detailed description is omitted.

  In the present embodiment, for example, as shown in FIGS. 13A to 14B, the seventh work W7 held by the hand 21 has a disk shape, and the eighth work W8 for assembling the seventh work W7 is It has a cylindrical shape. The seventh work W7 has a protrusion W7a having a shape protruding along the central axis W7c from one side, a claw-shaped engagement portion W7b bent to the inner peripheral side from the tip of the protrusion W7a, and the protrusion W7a And a claw portion W7d having a shape protruding to the inner peripheral side from the root portion. The eighth work W8 has an edge W8a shaped like a flange that protrudes inward from one end in the axial direction.

  In the seventh work W7 and the eighth work W8, the seventh work W7 is inclined 90 ° with respect to the eighth work W8, and the protrusion W7a, the engagement part W7b, and the claws of the seventh work W7 The portion W7d is mounted by being engaged with the edge W8a of the eighth work W8 (see FIG. 14B). That is, in the seventh work W7 and the eighth work W8, when the protrusion W7a, the engagement part W7b, and the claw W7d of the seventh work W7 engage with the edge W8a of the eighth work W8. The respective central axes W7c and W8c are formed to be mounted at right angles to each other.

  When the seventh work W7 is attached to the eighth work W8, the protrusion W7a of the seventh work W7 is the eighth work W8 with the central axes W7c and W8c of each other being parallel to the Z-axis direction. The edge W8a of the frame is approached in the X direction from the inner circumferential side (see FIG. 13A). Then, the protrusion W7a of the seventh work W7 is brought into contact with the edge W8a of the eighth work W8 (see FIG. 13B). The seventh work W7 is rotated about the Y axis at the contact point between the protrusion W7a and the edge W8a (see FIG. 14A). Then, the central axis W7c of the seventh work W7 is rotated by 90 ° with respect to the central axis W8c of the eighth work W8, and the protrusion W7a, the engagement part W7b, and the claw W7d of the seventh work W7 are It engages with the edge W8a of the eight workpieces W8 (see FIG. 14B). Thus, the seventh work W7 is detachably attached to the eighth work W8. On the other hand, when removing the seventh work W7 from the eighth work W8, the operation reverse to the above is performed.

  Here, the control device 3 mounts the seventh work W7 and the eighth work W8 using FIG. 13 to FIG. 15, detects the reaction force F, calculates the correction amount of the track, and calculates the track The principle of correcting In the present embodiment, the reaction force F is a force in the X direction among external forces acting on the tip link 67 detected by the force sensor 25 from the hand 21. That is, the reaction force F is orthogonal to the axial direction (Z direction) of the tip link 67, and is a direction in which the protrusion W7a of the seventh work W7 is engaged with or released from the edge W8a of the eighth work W8. Force (see Fig. 14 (b) etc.). Further, as the reaction force F in this case, among external forces acting from the hand 21 on the tip link 67 detected by the force sensor 25, forces in the Y and Z directions other than the force in the X direction and in three directions. The moments (Mx, My, Mz) are not included. Further, the coordinate system of the force sensor 25 is integrated with the coordinate system of the hand 21, and the coordinate system of the force sensor 25 also moves and rotates with the change of the position and orientation of the hand 21.

  As shown in FIG. 13A, the seventh work W7 is disposed in a state (non-contact state) separated by an appropriate distance without contacting the eighth work W8. At this time, with the seventh work W7 having the central axes W7c and W8c parallel to each other in the Z-axis direction, the protrusion W7a of the seventh work W7 is in the inner circumference to the edge W8a of the eighth work W8. Position on the side. The position and orientation of the seventh workpiece W7 at this time is set as an empty point (first teaching point). At this time, the reaction force F detected by the force sensor 25 is zero.

  Then, the protrusion W7a of the seventh work W7 is made to approach the edge W8a of the eighth work W8 by the robot body 2. As a result, as shown in FIG. 13B, the protrusion W7a of the seventh work W7 abuts on the edge W8a of the eighth work W8. The position and orientation of the seventh workpiece W7 at this time is taken as a contact point (third teaching point).

  Further, the robot body 2 rotates the seventh work W7 at the contact point between the protrusion W7a of the seventh work W7 and the edge W8a of the eighth work W8 about the Y axis. As a result, as shown in FIG. 14A, the posture of the seventh workpiece W7 is inclined while the protrusion W7a and the edge W8a are in contact with each other.

  Then, the central axis W7c of the seventh work W7 is rotated by 90 ° with respect to the central axis W8c of the eighth work W8 by the robot body 2. As a result, as shown in FIG. 14B, the protrusion W7a, the engaging portion W7b, and the claw W7d of the seventh work W7 engage with the edge W8a of the eighth work W8. The position and orientation of the seventh workpiece W7 at this time is taken as a lowering point (second teaching point). At this time, if the position or angle between the central axis W7c of the seventh work W7 and the central axis W8c of the eighth work W8 is deviated, the seventh work W7 and the eighth work W8 contact each other, An extra reaction force F is generated. Further, a straight line connecting the upper empty point and the lower point is a predetermined trajectory. That is, in the present embodiment, the trajectory at the falling point and the trajectory at the contact point are relatively inclined linear trajectories.

  This reaction force F is shown in FIG. 14 from the state where the projection W7a of the seventh work W7 shown in FIG. 13B is in contact with the edge W8a of the eighth work W8 (contact point in FIG. 15A). It moves up and down in the form of a broken line to the state (falling point in FIG. 15 (a)) which has been fitted up to the end shown in (b). That is, in the present embodiment, the reaction force F is an amount of change in which the seventh work W7 is displaced by the contact operation of the seventh work W7 and the eighth work W8. In the present embodiment, the state of FIG. 13 (b) and the state of FIG. 14 (b), that is, the states of the contact point and the lowering point are respectively the seventh work W7 and the eighth work W8. It is in a contact state.

  Here, for example, as shown in FIG. 15A, the stroke in the X direction from the upper empty point to the contact point is A2, and the stroke in the X direction from the contact point to the falling point is A1. It is assumed that the reaction force F in the X direction detected by the force sensor 25 ascends and descends from the contact point to the falling point, and becomes f1 at the contact point and F1 at the falling point. In addition, the reference waveform in the case where there is no displacement of the relative position and orientation measured in advance is acquired as a theoretical value, and the reaction force F in the X direction detected by the force sensor 25 at that time is broken linewise from the contact point to the falling point It raises and lowers, and it will be assumed that it became f0 in a contact point, and became F0 in a fall point. In this case, the slope ΔF of the difference between the reaction force F from the contact point to the falling point with and without displacement of the relative position and posture is ΔF = ((F1−F0) − (f1−f0)) / A1. become.

  Next, as shown in FIG. 15B, the slope ΔF of the difference of the reaction force F from the contact point to the descending point is extended to the upper empty point, and the eighth work W8 is extended to the upper empty point. In this case, it is calculated on the assumption of the reaction force F2 that the seventh work W7 will receive. The reaction force F2 is F2 = .DELTA.F.times.A2 + (f1-f0). As shown in the figure, the reaction force F2 is the point of intersection of the falling point (reaction force F1) and the contact point (reaction force f1) on the straight line connecting the depression point (reaction force F0) and the contact point It can also be determined by drawing as the difference between the point of intersection of the upper empty point of the straight line connecting the (reaction force f0).

  Further, as shown in FIG. 15C, the reaction force F is converted into a correction amount X which is a length in the X direction with respect to the up empty point and the down point. Therefore, assuming that the elastic modulus of the robot body 2 is K, the correction amount X2 at the upper empty point is X2 = F2 / K, and the correction amount X1 at the falling point is X1 = F1 / K. The correction amounts X1 and X2 in the X direction with respect to the upper empty point and the lower point are correction amounts of the trajectory. Then, the correction amount X2 is added to the X direction of the position and orientation of the current sky point to correct it, and the correction amount X1 is added to correct the X direction of the position and posture of the current drop point to correct the sky space after correction. Trajectory correction can be performed by using the straight line connecting the falling point as the corrected trajectory.

  The procedure of the control method of the robot apparatus 1 described above is the same as that of the first embodiment (FIG. 4 and FIG. 5), and thus the detailed description will be omitted. In addition, the order of the correction of the position and the correction of the posture with respect to the seventh work W7 can be appropriately set as in the first embodiment.

  As described above, also in the robot apparatus 1 of the present embodiment, the control device 3 moves the seventh work W7 on the track to execute the mounting operation on the eighth work W8. At that time, the control device 3 detects a reaction force F generated by displacement of the seventh work W7 due to the contact operation of the seventh work W7 and the eighth work W8. Then, the control device 3 calculates the correction amounts X1 and X2 in the X direction of the trajectory based on the reaction force F, and corrects the trajectory based on them. For this reason, it is possible to assemble the held seventh work W7 with high precision to the eighth work W8 without correcting the trajectory using a dedicated tool.

  Further, also in the present embodiment, similarly to the first embodiment, the displacement amount at which the seventh work W7 is displaced in position and orientation due to the contact with the eighth work W8 without using the force sensor 25 is determined. Each correction amount may be calculated based on the amount of displacement detected by the camera 5.

  Moreover, in the first to fourth embodiments described above, although the case where the contact operation is a mounting operation and the non-contact operation is a detachment operation has been described, the present invention is not limited thereto. For example, as the contact operation, when the first work is used as a tool and the second work is processed, or when the first work is used as a label and attached to the second work, contact other than attachment It can contain actions.

  Specifically, each processing operation of the first to fourth embodiments described above is executed by the CPU 30. Therefore, a recording medium in which a robot control program of software that realizes the above-described function is recorded may be supplied to the CPU 30, and the CPU 30 may be achieved by reading and executing the program stored in the recording medium. In this case, the program itself read from the recording medium implements the functions of the above-described embodiments, and the robot control program itself and the recording medium recording the program constitute the present invention.

  In the first to fourth embodiments, the case where the computer readable recording medium is the ROM 31 and the program is stored in the ROM 31 has been described. However, the present invention is not limited to this. The program may be recorded on any recording medium as long as it is a computer readable recording medium. For example, as a recording medium for supplying the program, an HDD, an external storage device, a recording disk or the like may be used.

DESCRIPTION OF SYMBOLS 1 ... Robot apparatus, 2 ... Robot main body, 3 ... Control apparatus (control part), 5 ... Camera (detection part), 20 ... Multi-joint arm, 21 ... Hand (end effector), 25 ... Force sensor (detection part) , 30: CPU, 71 to 76: multiple joints, W1: first work, W2: second work, W3: third work (first work), W4: fourth work (second Work), W5: fifth work (first work), W6: sixth work (second work), W7: seventh work (first work), W8: eighth work (third) 2 work)

Claims (18)

A robot body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and is connected to the end effector and acts relative to a first work and a second work Control of a robot apparatus comprising: a detection unit that detects at least one of a change amount of force and a displacement amount of a relative position and orientation of the first work and the second work; and a control unit that controls the robot body In the method
Wherein the control unit, the first workpiece held by the end effector, the first taught point, through the contact point, the moving step Before moving in orbit up to the second teaching point,
When the control unit is positioned on the track from the contact point to the second teaching point in the movement step, the control unit is between the first work and the second work. and variation of the force relative effect, a first detection step of the displacement amount before SL relative position and orientation of the first workpiece and the second workpiece, the at least one detected by the detecting unit,
A first calculation step of calculating the correction amount of the trajectory based on the change amount of the force detected in the first detection step or the displacement amount of the relative position and posture in the first detection step;
A first correction step of correcting the positions and attitudes of the first teaching point and the second teaching point based on the correction amount calculated in the first calculation step; Prepare,
And controlling the robot apparatus. Said track includes a case where the first workpiece and the second workpiece is in a state come in contact, in the case in a non-contact state, a straight line,
A control method of a robot apparatus according to claim 1, characterized in that. The trajectory is based on the first teaching point is the position and orientation of the first workpiece in a non-contact state, and the second teaching point is the position and orientation of the first workpiece in contact touch state, Is calculated,
In the first calculation step, the first teaching is performed as the correction amount of the trajectory based on the change amount of the force detected in the first detection step or the displacement amount of the relative position and posture in the first calculation step. Calculate the correction amount of each position and orientation of the point and the second teaching point,
In the first correction step, the control unit performs the first teaching based on correction amounts of respective positions and orientations of the first teaching point and the second teaching point calculated in the first calculation step. We correct the points and the position and orientation of the second teaching point,
The control method of the robot apparatus according to claim 1 or 2, characterized in that It said track includes a first teaching point is the position and orientation of the first workpiece in a non-contact state, and the second teaching point is the position and orientation of the first workpiece in contact touch state, the contact the position and orientation of the first workpiece in the state, a third teaching point located between said first taught point and the second teaching point in the orbit, is calculated based on the,
In the first calculation step, the first teaching is performed as the correction amount of the trajectory based on the change amount of the force detected in the first detection step or the displacement amount of the relative position and posture in the first calculation step. Calculate the correction amount of each position and orientation of the point and the second teaching point,
In the first correction step, the control unit performs the first teaching based on correction amounts of respective positions and orientations of the first teaching point and the second teaching point calculated in the first calculation step. We correct the points and the position and orientation of the second teaching point,
A control method of a robot apparatus according to claim 1, characterized in that. The trajectory at the second teaching point and the trajectory at the third teaching point are straight trajectories,
A control method of a robot apparatus according to claim 4, characterized in that. The trajectory at the second teaching point and the trajectory at the third teaching point are linear trajectories in the same direction, and between the respective trajectories, the first work and the second work are Contactless,
A control method of a robot apparatus according to claim 4, characterized in that. The trajectory at the second teaching point and the trajectory at the third teaching point are relatively inclined linear trajectories,
A control method of a robot apparatus according to claim 4, characterized in that. The trajectory at the third teaching point is a linear trajectory, and the trajectory at the second teaching point is a trajectory in a rotational direction about an axis in the same direction as the straight line.
A control method of a robot apparatus according to claim 4, characterized in that. In the first detection step, the change amount is a change amount of a force acting relative to the direction of the straight line between the first work and the second work, or the displacement amount is A displacement amount of a relative position of the first work and the second work in the direction of the straight line,
A control method of a robot apparatus according to any one of claims 5 to 8, characterized in that. In the first detection step, when the first work is positioned on the track from the contact point to the second teaching point in the movement step, the control unit performs the first work and the first work. and variation of the force relative acting between said second workpiece, detects a front Symbol displacement of the relative position of the first workpiece and the second workpiece, the at least one by the detection unit,
In the first operation step, the control section, based on the displacement amount of the first detection amount of change in the force detected by the step or the relative position location, as the correction amount of the track, the first teaching Calculate the correction amount of each position of the point and the second teaching point,
In the first correction process, the control section, based on the correction amount calculated by said first calculating step, correct for each position of the first taught point and the second teaching point,
When the control unit is positioned on the track from the contact point to the second teaching point in the movement step after the control unit corrects in the first correction step, of the work and force variation which relatively acts between the second workpiece, the pre-Symbol the detecting unit and the displacement amount of at least one of the relative orientation of the first workpiece and the second workpiece A second detection step of detecting;
Wherein the control unit is based on the displacement amount of the second variation of the force detected by the detection process or the phase Taisugata bias, as the correction amount of the track, said first taught point and said second teaching A second operation step of calculating the correction amount of each posture of the point;
Wherein the control unit, based on the correction amount calculated by the second calculating step, and a second correction step you correct each attitude of the first taught point and the second teaching point,
The control method of the robot apparatus according to any one of claims 3 to 9, characterized in that: In the first detection step, when the first work is positioned on the track from the contact point to the second teaching point in the movement step, the control unit performs the first work and the first work. and variation of the force relative acting between said second workpiece is detected by the front Symbol the detecting unit and the displacement amount of at least one of the relative orientation of the first workpiece and the second workpiece,
In the first operation step, the control section, based on the displacement amount of the first variation of the force detected by the detection process or the phase Taisugata bias, as the correction amount of the track, the first Calculate the correction amount of each posture of the teaching point and the second teaching point,
In the first correction process, the control section, based on the correction amount calculated by said first calculating step, correct for the orientation of the first taught point and the second teaching point,
When the control unit is positioned on the track from the contact point to the second teaching point in the movement step after the control unit corrects in the first correction step, and the work and the variation of the force relative acting between said second workpiece, and before Symbol displacement of the relative position of the first workpiece and the second workpiece, by the detecting unit at least one of A second detection step of detecting;
Wherein the control unit is based on the displacement amount of the second detection amount of change in the force detected by the step or the relative position location, as the correction amount of the track, the first teaching point and the second teaching point A second operation step of calculating the correction amount of each position of
Wherein the control unit, based on the correction amount calculated by the second calculating step, and a second correction step you correct the respective positions of the first taught point and the second teaching point,
The control method of the robot apparatus according to any one of claims 3 to 9, characterized in that:   The robot control program for making a computer perform each process of the control method of the robot apparatus of any one of Claims 1-11.   A computer readable recording medium on which the robot control program according to claim 12 is recorded. A robot body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and is connected to the end effector and acts relative to a first work and a second work A robot apparatus comprising: a detection unit that detects at least one of a force change amount and a displacement amount of a relative position and orientation of the first work and the second work; and a control unit that controls the robot body.
Wherein the control unit, the first workpiece held by the end effector, the first taught point, via a contact point, to a second teaching point is moved in a predetermined orbit, said first workpiece when located on the track from the contact point to the second teaching point, the change amount of the force relative acting between said first workpiece and said second workpiece, prior Symbol first Correction of the trajectory based on the detected amount of change in the force or the detected relative position and posture detected by the detection unit at least one of the relative position and posture displacement of the workpiece and the second workpiece Calculating an amount, and correcting each position and orientation of the first teaching point and the second teaching point based on the calculated correction amount;
A robot apparatus characterized by   A robot apparatus controlled by the control method of a robot apparatus according to any one of claims 1 to 11, or an article is manufactured using the robot apparatus according to claim 14.
  A method of producing an article characterized by   A robot main body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and a change amount of a force acting on the work relative to the end effector and the work A control unit that detects at least one of a relative position and a displacement amount of the relative position and a control unit that controls the robot body,
  Moving the end effector in a trajectory from the first teaching point to the second teaching point via the contact point;
  When the control unit is positioned on the track from the contact point to the second teaching point in the movement step, the control unit relatively moves the force between the end effector and the work. A first detection step of detecting at least one of a change amount and a displacement amount of a relative position / posture of the end effector and the work by the detection unit;
  A first calculation step of calculating the correction amount of the trajectory based on the change amount of the force detected in the first detection step or the displacement amount of the relative position and posture in the first detection step;
  A first correction step of correcting the positions and attitudes of the first teaching point and the second teaching point based on the correction amount calculated in the first calculation step; Prepare,
  And controlling the robot apparatus.   A robot main body having an articulated arm having a plurality of joints and an end effector supported by the articulated arm, and a change amount of a force acting on the work relative to the end effector and the work A robot unit comprising: a detection unit that detects at least one of a relative position and a displacement amount of the relative position posture; and a control unit that controls the robot body,
  The control unit moves the end effector in a trajectory from a first teaching point to a second teaching point via a contact point, and the end effector moves from the contact point to the second teaching point. The detection of at least one of the amount of change in force acting relative between the end effector and the work when positioned on the track, and the amount of displacement of the relative position and orientation of the end effector and the work The correction amount of the trajectory is calculated based on the detected change amount of the force or the displacement amount of the relative position and posture detected by the unit, and the first teaching point and the second teaching point are calculated based on the calculated correction amount. Correct each position and posture with the teaching point of
  A robot apparatus characterized by   A robot apparatus controlled by the control method of a robot apparatus according to claim 16 or an article is manufactured using the robot apparatus according to claim 17.
  A method of producing an article characterized by

Images