JP2021013983A - Apparatus and method for acquiring deviation of moving locus of moving machine - Google Patents

Abstract

To meet the demand for the technique for acquiring more highly accurately a deviation from a reference of a moving locus of a moving machine.SOLUTION: An apparatus 70 comprises: an optical sensor 40 moved by a moving machine 14, and continuously imaging a visual feature of an object around the moving machine 14; a locus acquisition part 60 for acquiring a movement locus of the moving machine 14 from a time point when first image data is captured by the optical sensor 40 to a time point when second image data is captured by the optical sensor 40 after the capture of the first image data, on the basis of a position of the visual feature in the first image data and a position of the visual feature in the second image data; and a deviation amount acquisition part 62 for acquiring a deviation amount of the movement locus acquired by the locus acquisition part 60 from a reference locus.SELECTED DRAWING: Figure 2

Description

The present invention relates to an apparatus and a method for acquiring a deviation from a reference of a moving locus of a moving machine.

A technique for correcting a moving locus of a moving machine is known (for example, Patent Document 1).

Japanese Unexamined Patent Publication No. 2001-167817

Conventionally, there has been a demand for a technique for acquiring a deviation from a reference of a moving locus of a moving machine with higher accuracy.

In one aspect of the present disclosure, a device that obtains deviations from a reference of a moving locus of a moving machine is moved by the moving machine and of an object around the moving machine while the moving machine is moving according to a movement command. Visual features are continuously imaged or the visual features of the moving machine or an object moved by the moving machine while it is placed away from the moving machine and moving according to a movement command. The position of the optical feature that is continuously imaged, the position of the visual feature in the first image data captured by the optical sensor, and the visual feature in the second image data captured by the optical sensor after the first image data. A movement command of a locus acquisition unit that acquires the movement locus of the moving machine from the time when the first image data is captured to the time when the second image data is captured, and a movement command of the movement locus acquired by the locus acquisition unit based on the position. It is provided with a deviation amount acquisition unit for acquiring the deviation amount from the reference locus of the moving machine defined by.

In another aspect of the present disclosure, a method of obtaining a deviation of a moving locus of a moving machine from a reference is such that the moving machine is moved by an optical sensor moved by the moving machine while the moving machine is moving according to a movement command. By the moving machine or the moving machine while the moving machine is moving according to the movement command by an optical sensor that continuously images the visual features of the surrounding object or is located away from the moving machine. The visual features of the moving object are continuously imaged, the position of the visual features in the first image data captured by the optical sensor, and the second image captured by the optical sensor after the first image data. Based on the position of the visual feature in the data, the movement locus of the moving machine from the time of capturing the first image data to the time of capturing the second image data is acquired, and the acquired movement locus is defined by the movement command. The amount of deviation from the reference trajectory of the moving machine to be performed is acquired.

According to the present disclosure, the amount of deviation of the movement locus of the moving machine from the reference locus can be automatically and highly accurately obtained from the image data captured by the optical sensor.

It is a figure of the mechanical system which concerns on one Embodiment. It is a block diagram of the mechanical system shown in FIG. It is an enlarged view of the optical apparatus shown in FIG. It is a figure for demonstrating the target position and the reference locus of a moving machine. This is an example of image data captured by the optical sensor when the moving machine is placed at the target position. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. It is a figure for demonstrating the deviation from the reference | movement locus of a moving machine. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. It is a figure for demonstrating the deviation from the reference | movement locus of a moving machine. It is a block diagram which shows the other function of the mechanical system shown in FIG. It is a flowchart which shows the operation flow of the mechanical system shown in FIG. It is a figure for demonstrating another example of the deviation amount. It is a block diagram which shows the other function of the mechanical system shown in FIG. It is a flowchart which shows the operation flow of the mechanical system shown in FIG. It is a figure for demonstrating the deviation from the reference | reference of a reference locus. It is a figure for demonstrating the deviation from the reference | reference | reference locus, and the movement locus. It is a figure of the tool which concerns on one Embodiment. It is a figure of the tool which concerns on other embodiment. It is a figure of the tool which concerns on still another embodiment. It is a figure of the mechanical system which concerns on other embodiment. It is a figure which shows the target position and the reference locus of a moving machine in the mechanical system of FIG. This is an example of image data captured by the optical sensor when the moving machine shown in FIG. 23 is placed at the target position. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. 25. This is an example of the image data captured by the optical sensor next to the image data shown in FIG. 26. It is a figure of the mechanical system which concerns on still another embodiment. It is a figure of the mechanical system which concerns on still another embodiment. An example of the pattern of the light emitted by the light projecting portion of the optical sensor is shown. It is a figure of the mechanical system which concerns on still another embodiment. Another example of an object imaged by an optical sensor is shown.

Hereinafter, embodiments of the present disclosure will be described in detail with reference to the drawings. In various embodiments described below, similar elements are designated by the same reference numerals, and duplicate description will be omitted. First, the mechanical system 10 according to the embodiment will be described with reference to FIGS. 1 to 3. The mechanical system 10 includes a control device 12, a mobile machine 14, and an optical device 16.

The control device 12 is a computer having a processor (CPU, GPU, etc.) 18 and a storage unit (ROM, RAM, etc.) 20, and controls the mobile machine 14 and the optical device 16. The processor 18 is communicably connected to each other via the storage unit 20 and the bus 22, and executes various operations while communicating with the storage unit 20.

The mobile machine 14 has a manipulator 24 and a tool 26. In the present embodiment, the manipulator 24 is a vertical articulated robot, and has a base portion 28, a swivel body 30, a lower arm portion 32, an upper arm portion 34, and a wrist portion 36. The base portion 28 is fixed on the floor of the work cell.

The swivel body 30 is fixed to the base portion 28 so as to be rotatable around a vertical axis. The lower arm portion 32 is provided on the swivel cylinder 30 so as to be rotatable around a horizontal axis. The upper arm portion 34 is rotatably provided at the tip end portion of the lower arm portion 32. The wrist portion 36 is rotatably provided at the front end portion of the upper arm portion 34. The wrist portion 36 rotatably supports the tool 26.

The manipulator 24 has a plurality of servomotors 38 (FIG. 2). These servomotors 38 are built in the base portion 28, the swivel body 30, the lower arm portion 32, the upper arm portion 34, and the wrist portion 36, and are movable components (that is, swivel) of the manipulator 24 in response to a command from the control device 12. Each of the body 30, the lower arm 32, the upper arm 34, and the wrist 36) is rotationally driven around the drive shaft.

The tool 26 is, for example, a laser processing head, a spot welding gun, or a robot hand, and performs a predetermined operation (laser processing, welding, work handling, etc.) on the work W. The tool 26 is detachably attached to the wrist portion 36 and moved by the manipulator 24.

The manipulator 24, the manipulator coordinate system C _M is set. Manipulator coordinate system C _M is a coordinate system for automatically controlling the movable components of the manipulator 24. In this embodiment, the origin of the manipulator coordinate system C _M is positioned in the center of the base portion 28, z-axis of the manipulator coordinate system C _M is parallel to the vertical direction in the real space, rotating body 30 is, manipulators as will be rotated about the z-axis of the coordinate system C _M, the manipulator coordinate system C _M is set. Workpiece W has a surface such that a substantially parallel and the x-y plane of the manipulator coordinate system C _M, is disposed at a predetermined position of the manipulator coordinate system C _M.

On the other hand, the tool coordinate system _CT is set in the tool 26. This tool coordinate system _CT is a coordinate system for automatically controlling the position and orientation of the tool 26 in the three-dimensional space. Processor 18, the position and attitude of the tool 26, as to match the position and orientation is defined by the tool coordinate system C _T, each of the movable components of the manipulator 24 operates in the manipulator coordinate system C _M. Thus, the tool 26 is moved by the manipulator 24, it is disposed in an arbitrary position and orientation in the manipulator coordinate system C _M.

As shown in FIG. 3, the optical device 16 has an optical sensor 40 and a distance sensor 42. The optical sensor 40 takes an image of an object and acquires image data of the object. In the present embodiment, the optical sensor 40 has a light emitting unit 40a and a light receiving unit 40b. The light projecting unit 40a emits light (for example, laser light) along the optical axis O and irradiates the imaging target with the light.

The light receiving unit 40b has an image pickup sensor (CCD, CMOS, etc.) and receives the reflected light of the light emitted to the image pickup target by the light projecting unit 40a. The light receiving unit 40b converts the received light into an electric signal and acquires image data to be imaged. For the optical sensor 40, the imaging coordinate system C _I is set. Imaging coordinate system C _I is a coordinate system that defines the coordinates of each pixel of the image data which the optical sensor 40 is captured, each pixel of image data captured by the optical sensor 40, the coordinates in the imaging coordinate system C _I Be transformed.

The distance sensor 42 measures the distance in the direction of the optical axis O from the optical sensor 40 to the image pickup target. Specifically, the distance sensor 42 is a so-called optical distance sensor, and has a light projecting unit 42a, a light receiving unit 42b, and a distance calculation unit 42c. The light projecting unit 42a emits light (for example, laser light) along the optical axis O, and irradiates the image pickup target of the optical sensor 40 with the light.

The light receiving unit 42b receives the reflected light of the light emitted to the image pickup target by the light projecting unit 42a. The distance calculation unit 42c has a processor or the like, and is from the optical sensor 40 to the image pickup target based on the time from when the light projecting unit 42a emits light until the light receiving unit 42b receives the reflected light of the light. Measure the distance of. The distance sensor 42 is not limited to the optical distance sensor, and may be any type of sensor as long as the distance from the optical sensor 40 to the image pickup target can be measured.

In this embodiment, the optical device 16 is fixed to the tool 26 and moved together with the tool 26 by the manipulator 24. The optical device 16 is arranged so as to have a predetermined positional relationship with respect to the tool 26. Specifically, the optical device 16, the optical axis O is parallel to the z axis of the tool coordinate system C _T (or the matching) manner, are arranged with respect to the tool 26. Thus, x-y plane of the image acquisition coordinate system _{C I} is orthogonal to the z axis of the tool coordinate system _{C T.}

Next, the operation of the mechanical system 10 will be described. The processor 18 operates the manipulator 24 to move the tool 26, and the tool 26 performs a predetermined work (laser machining, welding, work handling, etc.) on the work portion on the work W. When performing work on the work W, the processor 18 sequentially arranges the tool 26 at a plurality of target positions P _n (n = 1, 2, 3, ...).

FIG. 4 shows an example of the target position P _n . In the example shown in FIG. 4, five target positions P ₁ to P ₅ are set. For example, each of the five target positions P ₁ to P ₅ is a teaching point for teaching manipulator 24 so as to place the tool 26. Alternatively, of the five target positions P _{1 to} P ₅ , the target positions P ₁ , P ₃ and P ₅ are teaching points, while the target positions P ₂ between the target positions P ₁ and P ₃ and the target position _{P 4} between the target position _{P 3} and _{P 5} are interpolation points. The interpolation point coordinates of the manipulator coordinate system C _M before and after the teaching point, and, in view of the smoothness and the like of the moving locus of the tool 26 between the teaching points after the front, by calculation from the teaching point after front coordinates Desired.

In the following description, the target position P ₅ is assumed to be the working position for working on the working position of the workpiece W by the tool 26. In this case, the processor 18 arranges the tool 26 in the order of the target position P ₁ → P ₂ → P ₃ → P ₄ → P ₅ for the work of the work W. When causing the tool 26 is positioned at the target position _P n (n = _1~5), the processor 18, as the origin of the tool coordinate system _{C T} (so-called, TCP) are arranged in a target position _{P n,} the tool coordinate system setting the C _T.

Then, the processor 18, to match the position and orientation is defined by the tool coordinate system C _T set, moving the tool 26 by the manipulator 24, thereby placing the tool 26 to the target position P _n. The processor 18 executes a series of operations for moving the tool 26 to the target position _Pn according to the work program WP.

The work program WP includes a movement command MC _n for moving the tool 26 from the target position P _n to the target position P _{n + 1} . Movement command MC _n (specifically, coordinates of the manipulator coordinate system _{C M)} target position _{P n} and _{P n + 1} information including. The work program WP is constructed by instructing the manipulator 24 or the like to move the tool 26 to each of the target positions P _n as a teaching point, and is stored in advance in the storage unit 20.

The movement command MC _n included in the work program WP, the manipulator 24 is standard locus _{A n} to move the tool 26 is defined. For example, in the case of the example shown in FIG. 4, reference trajectories A _{1 to} A ₄ are defined. These reference loci _An are control loci defined on the work program WP.

Here, even when the processor 18 transmits a movement command MC _n to the manipulator 24 according to the work program WP and the manipulator 24 moves the tool 26 according to the movement command MC _n , the actual movement locus of the tool 26 is the reference locus A. _It may deviate from _n . Such a deviation of the movement locus is caused by, for example, minute vibration generated in the tool 26 due to acceleration / deceleration when the tool 26 is moved by the manipulator 24, or backlash between the movable components of the manipulator 24.

With such shift of the movement trajectory is generated, the working position the tool 26 (i.e., the target position P ₅₎ when the tasks on the workpiece W by the tool 26 is disposed on the tool 26 against the workpiece W The actual work position may deviate from the work location defined on the work W.

Therefore, in this embodiment, the processor 18 obtains the deviation from the standard locus A _n of the movement trajectory when moving the tool 26 into each of the target position P _n. Hereinafter, the function of acquiring the deviation amount in the mechanical system 10 will be described. The processor 18 transmits a movement command MC _n to the servomotor 38 of the manipulator 24 according to the work program WP, and the manipulator 24 sets the tool 26 to the target positions P ₁ , P ₂ , P ₃ according to the movement command MC _n from the processor 18. , it is moved in order to _{P 4} and _{P 5.}

Here, there is a limit to the imaging range in which the optical sensor 40 can image an object. The imaging range of the optical sensor 40 depends on the specifications of the optical sensor 40 (size of the imaging sensor, focal length, etc.). Hereinafter, when the tool 26 is in position between the target position P ₃ to P _5, it is assumed that the workpiece W enters the imaging range of the optical sensor 40.

The processor 18, when placing the tool 26 at the target position _P 3, _{P 4} and _{P 5,} z-axis of the tool coordinate system _{C T} (or, the optical axis O) is, of the manipulator coordinate system _{C M} x- y plane (or the surface of the workpiece W) so as to be perpendicular to, to set the tool coordinate system _{C T,} while maintaining the posture of the tool 26 to be constant, the tool 26 to the target position _P 3, _{P 4} and _{P 5} It shall be moved.

The processor 18 operates the optical sensor 40 while the tool 26 is moved sequentially to the target position P _3, P ₄ and P _5, continuously imaging the work W as an object around the mobile machine 14. Specifically, processor 18, when the tool 26 has reached the respective target positions _P 3, _{P 4} and _{P 5,} to image the workpiece W.

Processor 18, the position data of the manipulator 24 (e.g., the rotation angle of each servo motor 38) from the tool 26 can detect whether or not the target position has been reached _P 3, _{P 4} and _{P 5} on the control. When the processor 18 detects that the tool 26 has reached the control target positions P ₃ , P _4, and P ₅ , the optical sensor 40 causes the optical sensor 40 to image the work W.

Figure 5 shows an example in which the processor 18 has imaged image data 50 optical sensor 40 images the workpiece W at the time tau _P3 has been detected in that a tool 26 to the target position P _3. In the image data 50 shown in FIG. 5, visual features such as minute irregularities existing on the surface of the work W are reflected so as to form a pattern. The processor 18 performs image analysis on the image data 50 and extracts one visual feature VF (that is, a black dot reflected in the image data 50 of FIG. 5) included in the image data 50.

The processor 18 obtains the position of the visual feature VF extracted from the image data 50 in the image data 50. Specifically, as information indicating the position of the visual feature VF in the image data 50, one pixel (for example, the central pixel of the image region of the visual feature VF) in which the visual feature VF is copied in the image data 50. , and acquires the coordinates _(x 3, _{y 3)} of the imaging coordinate system _{C I.}

Figure 6 shows an example in which the processor 18 has imaged image data 52 optical sensor 40 images the workpiece W at the time tau _P4 was detected in that a tool 26 to the target position P _4. In FIG. 6, for comparison, the position of the visual features VF caught on the image data 50 shown in FIG. 5, illustrated as white dots D _3.

The processor 18 performs image analysis on the image data 52 and extracts a visual feature VF common to the image data 50. As shown in FIG. 6, the position of the visual features VF caught on the image data 52 from the position D ₃ of the visual features VF caught on the image data 50, only the displacement amount E _3, displaced in the imaging coordinate system C _I There is. Processor 18, as the position of the visual features VF in the image data 52 is acquired by the image data 52 of one pixel copy visual characteristics VF, the coordinates _(x 4, _{y 4)} in the imaging coordinate system _{C I.}

Figure 7 shows an example in which the processor 18 has imaged image data 54 optical sensor 40 images the workpiece W at the time tau _P5 it is detected that placing the tool 26 in the target position P _5. In FIG. 7, for comparison, illustrates the position of the visual features VF caught on the image data 50 shown in FIG. 5 as a white spot D _3, the visual features VF caught on the image data 52 shown in FIG. 6 It is shown positioned as white dots D _4.

The processor 18 performs image analysis on the image data 54 and extracts a visual feature VF common to the image data 50 and 52. As shown in FIG. 7, the position of the visual features VF caught on the image data 54 from the position D ₄ of the visual features VF caught on the image data 52, only the displacement amount E _4, displaced in the imaging coordinate system C _I There is.

Processor 18, as the position of the visual features VF in the image data 54 is acquired by the image data 54 of one pixel copy visual characteristics VF, the coordinates _(x 5, _{y 5)} in the imaging coordinate system _{C I.} As described above, the optical sensor 40, a target position on the tool 26 is controlled _P 3, _{P 4} and the time tau _P3 detects the arrival at the _{P 5,} in tau _P4 and tau _P5, visual features of the workpiece W The VF is continuously imaged.

On the other hand, in synchronization with the operation of the optical sensor 40 to image the work W, the processor 18 operates the distance sensor 42 to measure the distance F in the direction of the optical axis O between the optical sensor 40 and the work W. Therefore, the distance sensor 42 is the distances F _P3 , F _P4, and F _{P5 in} the direction of the optical axis O between the optical sensor 40 and the work W at the time points τ _P3 , τ _P4, and τ _P5 when the optical sensor 40 images the work W. Are measured respectively.

Then, the processor 18 is based on the position of the visual feature VF in the image data 50 and the position of the visual feature VF in the image data 52 captured after the image data 50, and the image data 50 is captured at the time of imaging τ _P3. to obtain the movement locus _{K 3} of tool 26 to the imaging time tau _P4 of the image data 52 from.

Specifically, processor 18, a difference between the acquired coordinates as described above _(x 3, _{y 3)} and the coordinates _{(x 4, y} 4), the displacement amount _{E 3} in the imaging coordinate system _{C I} (Fig. 6 ). The displacement amount E ₃ is a vector. As described above, when moving the tool 26 to the target position P ₃ to P _5, so that the optical axis O is orthogonal to the surface of the workpiece W, the tool coordinate system C _T is set (in other words, the tool The posture of 26 is constant). Therefore, when moving the tool 26 to the target position _P 3 to P _5, orientation of the imaging coordinate system _{C I} for the manipulator coordinate system _{C M} (i.e., the direction of each axis) is known.

Processor 18, and orientation information of the imaging coordinate system _{C I} for the manipulator coordinate system _{C M,} the displacement amount _{E 3} in the imaging coordinate system _{C I,} from the distance _{F P3} and _{F P4} Prefecture, from time tau _P3 to time tau _P4 the movement locus K ₃ in the manipulator coordinate system C _M of the tool 26, it can be obtained by calculation. Thus, in the present embodiment, the processor 18, the imaging time tau _P3 of the image data 50, the locus acquiring unit 60 (Fig for acquiring movement locus K ₃ of tool 26 to the imaging time tau _P4 of the image data 52 It functions as 2).

An example of a moving locus K ₃ which thus acquired in FIG. In the example shown in FIG. 8, the movement locus K ₃ obtained from the image data 50 and 52 are shifted by shift amount [delta] ₄ from the reference locus A _3. In the example shown in FIG. 8, when it is detected that the tool 26 has reached the control target position P ₃ (that is, at the time point τ _P3 ), the actual position of the tool 26 is the control target position. It is assumed to be substantially matched to P _3.

Thus, in order to substantially coincide with the target position P ₃ on the control of the actual position of the tool 26 at time tau _P3, for example, the processor 18, the movement of the tool 26 prior to imaging the work W at the time tau _P3 temporarily dampen the vibrations occurring in the tool 26 is stopped, after a predetermined time has elapsed, the movement of the tool 26 toward the target position P ₄ may be resumed.

The processor 18 is acquired from the image data 50 and 52, the movement locus _{K 3} in the manipulator coordinate system _{C M,} included in the work program WP, the movement command MC for moving the tool 26 from the target position _{P 3} to _{P 4 The} deviation amount δ ₄ is obtained by calculation from the reference locus A ₃ defined by ₃ .

Specifically, since the movement locus K ₃ is a vector from the control target position P ₃ to the position J ₄ , and the reference locus A ₃ is a vector from the control target position P ₃ to P ₄ . the difference in the manipulator coordinate system C _M of these two vectors can be computed as the deviation amount [delta] _4. This deviation amount δ ₄ is a vector from the position J ₄ to the target position P ₄ .

Similarly, processor 18, and orientation information of the imaging coordinate system _{C I} for the manipulator coordinate system _{C M,} the displacement amount in the imaging coordinate system _{C I} acquired from the image data 52 and 54 _{E 4} (FIG. 7), the image data 52 and 54 by using the distance _{F P4} and _{F P5} of the imaging time tau _P4 and tau _P5 of, it acquires the movement trajectory _{K 4} in the manipulator coordinate system _{C M} tools 26 from time tau _P4 to time tau _P5. The movement locus K ₄ is a vector from position J ₄ to J ₅ .

The processor 18 includes a movement locus _{K 4} in the manipulator coordinate system _{C M,} the reference trajectory _{A 4} Metropolitan defined by the movement command MC ₄ for moving the tool 26 from the target position _{P 4} to _{P 5,} the movement trajectory the shift amount [delta] ₅ from the reference trajectory _{a 4} of K ₄ is obtained by calculation. This deviation amount δ ₅ is a vector from the position J ₅ to the target position P ₅ . As described above, the processor 18 functions as a deviation amount acquisition unit 62 (FIG. 2) for acquiring the deviation amounts δ ₄ and δ ₅ from the reference loci A ₃ and A ₄ of the movement loci K ₃ and K ₄ .

Next, the processor 18 corrects the target positions P ₄ and P ₅ of the tool 26 based on the acquired deviation amounts δ ₄ and δ ₅ . Specifically, the processor 18, the target position P ₄ contained in the work program WP, at a position displaced by the offset amount [delta] ₄ in the manipulator coordinate system C _M, it is corrected. Further, the processor 18, the target position P ₅ that was included in the work program WP, at a position displaced by the offset amount [delta] ₅ A manipulator coordinate system C _M, it is corrected.

In this manner, processor 18, by correcting the position information of the target position P _4, P _5, which are specified in the work program WP, updates the working program WP. Therefore, the processor 18 functions as a position correction unit 64 (FIG. 2) that corrects the target position of the tool 26.

As described above, in the present embodiment, the optical sensor 40 that continuously images the visual feature VF of the work W while the manipulator 24 is moving the tool 26 according to the movement command MC _n, and the movement locus K ₃ , K ₄ is acquired by the locus acquisition unit 60, and the deviation amounts δ ₄ and δ ₅ are acquired by the deviation amount acquisition unit 62, so that the movement trajectories K ₃ and K ₄ of the tool 26 deviate from the reference A ₃ and A _4. Is getting.

Accordingly, the optical sensor 40, the locus acquiring unit 60 and the shift amount acquisition unit 62, the reference _A 3, to obtain a deviation from _{A 4} device 70 (FIG. 1 movement locus _K 3, _{K 4} of the tool 26, FIG. 2 ). According to this device 70, from the image data 50, 52, 54 captured by the optical sensor 40, the deviation amounts δ ₄ , δ ₅ from the movement loci K ₃ , K ₄ reference loci A ₃ , A _{4 of} the tool 26 are obtained. , Can be obtained automatically and with high accuracy.

Further, in the present embodiment, the distance sensor 42 constitutes the device 70, and when the optical sensor 40 images the visual feature VF, the distance F from the optical sensor 40 to the work W to be imaged is measured. There is. According to this configuration, the processor 18, the movement trajectory K ₃ and K ₄ in the manipulator coordinate system C _M, can be obtained as a three-dimensional trajectory.

Further, in the present embodiment, the position correction unit 64 constitutes the device 70 and corrects the target position of the tool 26 based on the acquired deviation amounts δ ₄ and δ ₅ . According to this configuration, when the processor 18 operates the manipulator 24 according to the updated work program WP to place the tool 26 at the work position and performs work on the work W with the tool 26, the tool 26 works. The position where the actual work is performed with respect to the W can be arranged more precisely at the work location on the work W. As a result, it is possible to reduce work defects with respect to the work W. Further, since the operator can omit the work of manually correcting the target position _Pn in the work program WP, the work required for teaching the manipulator 24 can be reduced.

Also, the time in the present embodiment, the movement command MC includes information of a plurality of target positions P _n, the optical sensor 40, the tool 26 has reached the each of the plurality of target positions P _3, P ₄ and P ₅ The visual feature VF of the work W is imaged by τ _P3 , τ _P4, and τ _P5 . According to this configuration, the processor 18 can acquire the deviation amounts δ ₄ and δ ₅ of the movement loci K ₃ and K ₄ at the target positions P ₄ and P ₅ defined in the work program WP.

Incidentally, the processor 18, while the tool 26 is moved from the target position _{P 3} to _{P 5,} the workpiece W by the optical sensor 40, the time tau _P3 described above, rather than tau _P4 and tau _P5, periodic or random The image may be taken at any time. Hereinafter, other functions of the device 70 will be described with reference to FIGS. 9 to 12.

In the present embodiment, the processor 18, the time tau _P3 and the tool 26 to reach the target position _{P 3,} the time _{τ α (= τ P3 + t} 1) to time _{t 1} from said time point tau _P3 has elapsed, said time points tau time from _alpha time _{t 2} has elapsed _{τ β (= τ α + t} 2), and, when the time _{t 3} from said time point tau _beta has elapsed tau _gamma by _{(= τ β + t 3)} , the workpiece W to the optical sensor 40 Is imaged.

Times t ₁ , t ₂ and t ₃ may be the same as each other. In this case, the optical sensor 40 images the work W at a predetermined cycle from the time point τ _P3 . Alternatively, the times t ₁ , t ₂ and t ₃ may be different from each other. In this case, the optical sensor 40 images the work W at a random timing from the time point τ _P3 .

On the other hand, in synchronization with the operation of the optical sensor 40 to image the work W, the processor 18 operates the distance sensor 42 to measure the distance between the optical sensor 40 and the work W in the direction of the optical axis O on the distance sensor 42. Let me. Therefore, the distance sensor 42 is the distances F _P3 , F _α , F _β, and F _γ between the optical sensor 40 and the work W at the time points τ _P3 , τ _α , τ _β, and τ _γ when the optical sensor 40 images the work W. Are measured respectively.

The processor 18 captures the image data 50 shown in FIG. 5 at the time point τ _P3 . Figure 9 shows the image data 82 to the optical sensor 40 has captured the workpiece W at the time tau _alpha. In FIG. 9, for comparison, the position of the visual features VF caught on the image data 50 shown in FIG. 5, illustrated as white dots D _3.

Further, FIG. 10 shows the image data 84 to the optical sensor 40 has captured the workpiece W at the time tau _beta. In Figure 10, for comparison, it illustrates the position of the visual features VF caught on the image data 50 shown in FIG. 5 as a white spot D _3, the position of the visual features VF caught on the image data 82 shown in FIG. 9 It is shown as a white point D _α .

Further, FIG. 11 shows image data 86 in which the optical sensor 40 images the work W at the time point τ _γ . In Figure 11, for comparison, it illustrates the position of the visual features VF caught on the image data 50 shown in FIG. 5 as a white spot D _3, the position of the visual features VF caught on the image data 82 shown in FIG. 9 It is shown as a white point D _α , and the position of the visual feature VF reflected in the image data 84 shown in FIG. 10 is shown as a white point D _β .

The processor 18 functions as a locus acquisition unit 60, and captures images from the position of the visual feature VF reflected in the image data 50 to the position of the visual feature VF reflected in the image data 82 in the same manner as in the above-described embodiment. obtains the displacement amount E _alpha in the coordinate system _{C I,} by using the displacement amount E _alpha, the distance _{F P3} and F _alpha, movement trajectory in the manipulator coordinate system _{C M} tools 26 from time tau _P3 to time tau _alpha Acquire K _α (Fig. 12). This movement locus K _α is a vector from the control target position P ₃ to the position J _α .

Similarly, the processor 18, from the position of the visual features VF caught on the image data 82, to the position of the visual features VF caught on the image data 84, the displacement amount E _beta in the imaging coordinate system C _I calculated, displacement amount E and _beta, using the distance and F _alpha and F _beta, acquires the movement trajectory K _beta in the manipulator coordinate system C _M tools 26 from time tau _alpha to time tau _beta. This movement locus K _β is a vector from the position J _α to J _β .

Similarly, the processor 18, from the position of the visual features VF caught on the image data 84, to the position of the visual features VF caught on the image data 86, the displacement amount E _gamma in the imaging coordinate system C _I calculated, displacement amount E and _gamma, using the distance and F _beta and F _gamma, acquires the movement trajectory K _gamma in the manipulator coordinate system C _M tools 26 from time tau _beta to time tau _gamma. This movement locus K _γ is a vector from the position J _β to J _γ .

Then, the processor 18, and functions as a shift amount acquisition unit 62 acquires the shift amount from the movement locus K _alpha, standard locus _{A 3} and _{A 4} of the K _beta and K _gamma in the manipulator coordinate system _{C M.} As an example, the processor 18, the target position _{P 3,} the position J _alpha, position J _beta, and obtains the synthesized moving locus _{K C} connecting the position J _gamma. The synthetic movement locus K _C may be obtained as a smooth curve, or may be obtained as a combination of straight lines corresponding to the movement loci K _α , K _β and K _γ .

Then, the processor 18, the distance between the target position P ₄ Synthesis movement locus K _C becomes the shortest, and the point G ₁ on the synthetic moving locus K _C, the amount of deviation [delta] _alpha between the target position P ₄ get. This amount of deviation [delta] _alpha, is a vector from the point G ₁ to the target position P _4. Further, the processor 18 determines the amount of deviation δ _β between the point G ₂ on the synthetic movement locus K _C and the target position P _{5 at} which the distance between the target position P ₅ and the synthetic movement locus K _C is the shortest. get. This amount of deviation δ _β is a vector from the point G ₂ to the target position P ₅ .

Then, the processor 18, and functions as a position correction unit 64, the target position P ₄ contained in the work program WP, at a position displaced by the offset amount [delta] _alpha A manipulator coordinate system C _M, is corrected. Further, the processor 18, the target position P ₅ that was included in the work program WP, at a position displaced by the offset amount [delta] _beta A manipulator coordinate system C _M, is corrected. As described above, according to the present embodiment, from the image data 50, 82, 84, 86 captured by the optical sensor 40, the movement loci K _α , K _β and K _γ (or the composite movement locus K _C ) of the tool 26. the amount of deviation from the reference locus a _3, a _{4 δ α,} the [delta] _beta, can be obtained automatically and accurately.

Next, still other functions of the mechanical system 10 will be described with reference to FIG. In the mechanical system 10 according to the present embodiment, when the processor 18 acquires the deviation amounts δ ₄ and δ ₅ shown in FIG. 8, it determines whether or not the deviation amounts δ ₄ and δ ₅ exceed the threshold value δ _th. When the determination is made and it is determined that the deviation amounts δ ₄ and δ ₅ exceed the threshold value δ _th , a warning signal is generated. Hereinafter, the operation of the mechanical system 10 according to the present embodiment will be described with reference to FIG.

Flow shown in FIG. 14, the processor 18 places the tool 26 at the target position P _3, when the optical sensor 40 has captured the image data 50 shown in FIG. 5, begins. In step S1, the processor 18 sets the number “n” that specifies the target position P _n to which the tool 26 should be placed to “4”. In the flow shown in FIG. 14, the processor 18 repeatedly executes the loops of steps S2 to S9 until it determines YES in step S9 described later.

In step S2, the processor 18 moves the tool 26 to the target position _Pn . If the step S2 of the first loop is executed, the number "n" for specifying the target position _Pn is set to "4". Therefore, in this step S2, the processor 18 is set to the target position. The manipulator 24 is operated according to the movement command MC ₃ for moving from P ₃ to the target position P ₄ , and the tool 26 is moved toward the target position P ₄ .

In step S3, the processor 18 determines whether or not the tool 26 has reached the target position _Pn . As described above, the processor 18 can detect whether or not the tool 26 has reached the control target position _Pn from the position data of the manipulator 24 (for example, the rotation angle of each servomotor 38).

Assuming that executes step S3 of the first round of the loop, in step S3, the processor 18 determines whether the tool 26 has reached the target position P _4. When the processor 18 detects that the tool 26 has reached the target position _Pn , the processor 18 determines YES and proceeds to step S4. On the other hand, the processor 18 determines NO when the tool 26 has not reached the target position _Pn , and loops step S3.

In step S4, the processor 18 operates the distance sensor 42 to measure the distance _FPn between the optical sensor 40 and the work W in synchronization with the operation of the optical sensor 40 to image the work W. If step S4 of the first loop is executed, the processor 18 causes the distance sensor 42 to work with the optical sensor 40 in synchronization with causing the optical sensor 40 to image the image data 52 shown in FIG. The distance F _P4 from W is measured.

In step S5, the processor 18 acquires the movement trajectory _{K n-1} functions as a track acquiring unit 60, the reference trajectory _{A n-1} of the movement locus _{K n-1} functions as a shift amount obtaining unit 62 The amount of deviation δ _n is obtained. If, when performing step S5 in the first round of the loop, the processor 18, in embodiments a manner similar to FIG. 8, obtains the movement trajectory K _3, the reference trajectory A ₃ moving path K ₃ The deviation amount δ _{4 of} is obtained.

Further, when the step S5 of the second loop is executed, as a result of the step S8 described later, the number "n" for specifying the target position _Pn is set to "5", so that the processor 18 is shown in FIG. In the same manner as in the embodiment of the above, the movement locus K ₄ is acquired, and the deviation amount δ ₅ of the movement locus K _{4 from} the reference locus A ₄ is acquired. Alternatively, as shown in FIG. 15, the processor 18 matches the start point of the movement locus K ₄ (that is, the position J ₄ ) with the start point of the reference locus A ₄ (that is, the target position P ₄ ). the difference [delta] _{5 'of} the moving locus K ₄ and the reference locus a ₄ when, may be obtained as a displacement amount.

In step S6, the processor 18 determines whether or not the deviation amount δ _n acquired in the latest step S5 is larger than the predetermined threshold value δ _th . If step S6 of the first loop is executed, the processor 18 determines whether or not the deviation amount δ ₅ is larger than the threshold value δ _th .

Further, when executing step S5 of the second loop, the processor 18 determines whether or not the deviation amount δ ₅ (or δ ₅ ') is larger than the threshold value δ _th (or δ _th '). .. These threshold values δ _th (or δ _th ') are predetermined by the operator and stored in the storage unit 20.

The processor 18 determines YES when the deviation amount δ _n is larger than the threshold value δ _th , and proceeds to step S7. On the other hand, the processor 18 determines NO when the deviation amount δ _n is equal to or less than the threshold value δ _th , and proceeds to step S8. .. As described above, in the present embodiment, the processor 18 functions as a determination unit 92 (FIG. 13) for determining whether or not the deviation amount δ _n is larger than the threshold value δ _th .

In step S7, the processor 18 generates a warning signal. For example, the processor 18 generates a warning signal in the form of voice or image that "the deviation from the reference of the movement locus is excessive", and through a display or a speaker (not shown) provided in the control device 12. , The warning signal is output. In this way, the processor 18 functions as a warning generation unit 94 (FIG. 13) that generates a warning signal when the deviation amount δ _n is larger than the threshold value δ _th .

In step S8, the processor 18 increments the number “n” that identifies the target position P _n by one. If step S8 of the first loop is executed, the processor 18 increments the number "n" from "4" to "5". In step S9, the processor 18 determines whether or not the number “n” that identifies the target position P _n is larger than “5”. The processor 18 determines YES when the number "n" is larger than "5" and proceeds to step S10, while the processor 18 determines NO when the number "n" is "5" or less and returns to step S2. ..

In step S10, the processor 18 functions as a position correction unit 64 to correct the target position of the tool 26 of the mobile machine 14. Specifically, the processor 18 corrects the target position P ₄ in the work program WP to a position displaced by a displacement amount δ _4, and corrects the target position P ₅ to a position displaced by a displacement amount δ _5. Then, update the work program WP.

As described above, in the present embodiment, the optical sensor 40, the locus acquiring unit 60, the deviation amount acquisition unit 62, the position correcting unit 64, the determination unit 92 and a warning generating unit 94, the movement locus _K 3 of tool 26, A device 90 (FIG. 13) for acquiring deviations from the criteria A ₃ and A _{4 of} K ₄ is configured. According to the device 90, the operator can automatically and intuitively recognize that the deviations of the movement loci K ₃ and K _{4 from} the reference A ₃ and A ₄ are excessive.

In the embodiment shown in FIG. 12, when the deviation amount δ _α or δ _β is acquired, the processor 18 functions as the determination unit 92, and whether the deviation amount δ _α or δ _β is larger than the threshold value δ _th2 . It may be determined whether or not. Then, when the deviation amount δ _α or δ _β is larger than the threshold value δ _th2 , the processor 18 may function as a warning generation unit 94 to generate a warning signal.

Next, other functions of the mechanical system 10 will be described with reference to FIGS. 16 and 17. A machine system 10 according to this embodiment, the processor 18 by the manipulator 24 while moving the tool 26 to the target position P ₁ to P _3, from a reference of the reference trajectory calculated from position data of the manipulator 24 Determine if the deviation is excessive.

Hereinafter, the operation of the mechanical system 10 according to the present embodiment will be described with reference to FIG. In the flow shown in FIG. 17, the same process as the flow shown in FIG. 14 is assigned the same step number, and duplicate description will be omitted. Flow shown in FIG. 17, the processor 18 when placing the tool 26 to the target position P _1, starts. Here, the processor 18, when placing the tool 26 to the target position _{P 1,} and functions as a position data acquiring unit 102 to be described later, to acquire the position data PD ₁ of the manipulator 24.

In step S11, the processor 18 sets the number “n” that specifies the target position _{Pn on} which the tool 26 should be placed to “2”. In the flow shown in FIG. 17, the processor 18 repeatedly executes the loops of steps S2 to S16 until it determines YES in step S16 described later.

When it is determined as YES in step S3, in step S12, the processor 18 acquires the position data PD _n of the manipulator 24. Specifically, the processor 18 acquires the rotation angle of each servomotor 38 of the manipulator 24 as the position data PD _n . This rotation angle can be obtained from a rotation detector (encoder, Hall element, etc., not shown) provided on each servomotor 38. In this way, the processor 18 functions as a position data acquisition unit 102 (FIG. 16) that acquires position data PD _n while the manipulator 24 is moving the tool 26.

In step S13, the processor 18 acquires the reference locus H _n-1 and functions as the deviation amount acquisition unit 62 to acquire the deviation amount Δ _n of the reference locus H _{n-1 from} the reference locus A _n-1. To do. Hereinafter, step S13 will be described with reference to FIG. When the tool 26 is positioned at the target position P _n, the position of the calculation in the manipulator coordinate system C _M of the tool 26 (eg, TCP coordinates), be determined by calculation from the position data PD _n of the manipulator 24 it can.

The processor 18, in step S13, the position data PD _n acquired in the immediately preceding step S12, when the tool 26 is positioned at the target position _{P n,} obtains the position _{I n} on the calculation of the tool 26. Together with this, the processor 18, the position data _{PD n-1} which has been acquired before the position data PD _n, the position _{I n} on the calculation of the tool 26 when the tool 26 is positioned at the target position _{P n-1 Get -1} .

If step S13 in the first round loop is executed, the processor 18 uses the position data PD ₂ of the manipulator 24 when the tool 26 is placed at the target position P ₂ to calculate the position I of the tool 26. ₂ (FIG. 18) is acquired and the calculated position of the tool 26 is calculated from the position data PD ₁ of the manipulator 24 when the tool 26 is placed at the target position P ₁ (that is, at the start of the flow of FIG. 17). Get I ₁ .

Then, the processor 18, a movement trajectory in the manipulator coordinate system _{C M} from the acquired position _{I 1} to _{I 2,} obtained by calculation as the reference trajectory _{H 1.} In this way, the processor 18 acquires the reference locus H _n-1 based on the position data PD _n and PD _n-1 . Therefore, the processor 18 functions as a reference locus acquisition unit 104 (FIG. 16) for acquiring the reference locus H _n-1 .

Then, the processor 18 obtains a deviation amount delta _n from the reference trajectory _{A n-1} of the reference trajectory _{H n-1} as the reference displacement amount. Assuming that executes step S13 in the first round of the loop, the processor 18 obtains a deviation amount delta ₂ from the reference trajectory A ₁ of the reference trajectory H ₁ as a reference shift amount. The reference trajectory _{H 1} is the vector from the position _{I 1} to _{I 2,} reference trajectory _{A 1} is Since the vector from the target position _{P 1} to _{P 2,} the difference in the manipulator coordinate system _{C M} of these two vectors and it can be calculated as a deviation amount delta _2. This deviation amount Δ ₂ is a vector from the position I ₂ to the target position P ₂ .

Also, when executing step S13 in the second round of the loop, the result in step S8, since specifying the target position P _n number "n" is set to "3", the processor 18, the reference trajectory H ₂ obtaining a shift amount delta ₃ from the reference trajectory a ₂ as reference deviation amount. The reference locus H ₂ is a vector from positions I ₂ to I ₃ . Alternatively, the processor 18 coincides with the start point of the reference locus H ₂ (ie, position I ₂ ) and the start point of the reference locus A ₂ (ie, the target position P ₂ ), as described with reference to FIG. the difference delta _{3 'of} the reference trajectory H ₂ and the reference locus a ₂ obtained while, may be obtained by reference shift amount.

In step S14, the processor 18 determines whether or not the reference deviation amount Δ _n acquired in the latest step S13 is larger than the predetermined threshold value Δ _th . If step S14 of the first loop is executed, the processor 18 determines whether or not the reference deviation amount Δ ₂ is larger than the threshold value δ _th .

Also, when executing step S14 in the second round of the loop, the processor 18 determines the reference shift amount delta _{3 (or} delta ₃ ') is a threshold delta _{th (or} delta _th' or larger or not than) To do. These threshold values Δ _th (or Δ _th ′) are predetermined by the operator and stored in the storage unit 20.

Processor 18, the reference shift amount delta _n is determined as YES is larger than the threshold _{delta th,} the process proceeds to step S15, determination is NO if the reference shift amount delta _n is equal to or less than the threshold _{delta th,} step S8 Proceed to. As described above, in the present embodiment, the processor 18 functions as a second determination unit 106 (FIG. 13) for determining whether or not the reference deviation amount Δ _n is larger than the threshold value Δ _th .

In step S15, the processor 18 generates a second warning signal. For example, the processor 18 generates and outputs a second warning signal in the form of a voice or an image that "the deviation from the reference locus reference is excessive". Thus, processor 18 serves as a second alert generation unit 108 for generating a second warning signal (16) if the reference shift amount delta _n is greater than the threshold value delta _th.

In step S16, the processor 18 determines whether or not the number “n” that identifies the target position P _n is larger than “3”. The processor 18 determines YES when the number "n" is larger than "3" and proceeds to step S17, while the processor 18 determines NO when the number "n" is "3" or less and returns to step S2. ..

In step S17, the processor 18 functions as a position correction unit 64 to correct the target position of the tool 26 of the mobile machine 14. Specifically, processor 18 corrects the position of displacing the target position P ₂ in the working program WP by reference shift amount delta _2, corrects the position of displacing only the deviation amount of the target position P ₃ delta ₃ By doing so, the work program WP is updated.

As described above, in the present embodiment, the optical sensor 40, the locus acquisition unit 60, the deviation amount acquisition unit 62, the position correction unit 64, the position data acquisition unit 102, the reference locus acquisition unit 104, the second determination unit 106, and second warning generator 108 constitute a device 100 for obtaining a deviation from the reference _{a n} moving path _{H n} of tools 26 (FIG. 16).

According to the apparatus 100, when the workpiece W is tool 26 between the target position P ₁ to P ₃ which is outside the imaging range of the optical sensor 40 is moving, the reference trajectory H _n to the reference trajectory acquisition unit 104 acquires the reference deviation amount delta _n obtained from, when the tool 26 between the target position P ₃ to P ₅ of the workpiece W is within the imaging range of the optical sensor 40 is moved, the locus acquiring unit 60 acquires moving The reference deviation amount δ _n can be obtained from the locus K _n .

Therefore, the range of the target position _P 1 to P _5, the movement trajectory _H n of tools 26, the deviation from the reference _{A n} of _{K n} can be obtained. Also, the second determination unit 106 and the second warning generating unit 108, the operator that the deviation from the reference A _n of the reference trajectory H _n becomes excessively large, automatically and intuitively recognizable.

It should be noted that the functions of the embodiment of FIG. 16 can be combined with the embodiment of FIG. In this case, the device 90 further includes a position data acquisition unit 102, a reference trajectory acquisition unit 104, a second determination unit 106, and a second warning generation unit 108. Hereinafter, an example of the operation of the mechanical system 10 when the functions of the embodiment of FIG. 16 are combined with FIG. 13 will be described.

First, the processor 18 starts the flow of FIG. 17, when it is determined YES executes step S3 of the second round of the loop (that is, upon detecting the placement of the target position P ₃ of the tool 26) , The optical sensor 40 is made to image the work W, whereby the image data 50 shown in FIG. 5 is imaged and the flow shown in FIG. 14 is started. Then, the processor 18 executes the flow shown in FIG. 14 in parallel with the flow shown in FIG.

As an example, when it is determined YES running step S3 flow of the second round of the loop of Figure 17, the processor 18, approximate the reference shift amount delta ₄ attenuates the vibration generated in the tool 26 to zero Therefore, the movement of the tool 26 may be temporarily stopped, and after a predetermined time has elapsed, the optical sensor 40 may image the work W and start the flow shown in FIG.

As another example, when step S3 in the loop of the second round is executed in the flow of FIG. 17 and a determination is made as YES, the optical sensor 40 is made to image the work W while continuing the movement of the tool 26, and FIG. The flow shown in may be started. Then, when executing step S5 in the first loop of the flow shown in FIG. 14, the processor 18 moves starting from the position I ₃ which is the end point of the reference locus H ₂ as shown in FIG. the trajectory K ₃ may be acquired.

As the above-mentioned tool 26, various types can be considered. Hereinafter, a specific example of the tool 26 will be described with reference to FIGS. 20 and 21. The tool 26 shown in FIG. 20 is a laser machining head. Specifically, the tool 26 has a hollow nozzle 110, emits laser light along the optical axis O ₂ from an exit port 110a formed in the nozzle 110, and laser-processes the work W (laser cutting, laser cutting, Laser welding, etc.). In the present embodiment, the tool coordinate system C _T is the z axis so as to coincide with the optical axis O _2, are set for the tool 26.

When working on the work W with the tool 26, for example, the processor 18 arranges the tool 26 in order at the above-mentioned target position P _n (n = 1 to 5), and arranges the tool 26 at the target position P ₅ . when brought into a laser beam emitted along the optical axis O ₂ by the tool 26 is operated, laser machining the working position on the workpiece W. At this time, the optical axis O ₂ is arranged so as to be substantially orthogonal to the surface of the work W and to pass through the work portion.

On the other hand, the optical device 16 is provided at the exit port 110a of the nozzle 110. The optical device 16 is arranged so as to have a predetermined positional relationship with respect to the tool 26. Specifically, the optical device 16 is arranged with respect to the tool 26 so that the optical axis O coincides with (or is parallel to) the optical axis O _{2 of the} laser beam. That is, the optical device 16, x-y plane of the image acquisition coordinate system _{C I} is the optical axis _{O 2} of the laser light (or, z axes of the tool coordinate system _{C T)} so as to be perpendicular to the arrangement with respect to the tool 26 Has been done. The optical device 16 is not limited to the outlet 110a, and may be provided at any position of the nozzle 110.

The tool 26 shown in FIG. 21 is a spot welding gun. Specifically, the tool 26 has a fixed arm 112, a movable arm 114, a fixed electrode 116, and a movable electrode 118. Fixed arm 112, while being fixed to the wrist unit 36, the movable arm 114, the axis O _{3 (so-called} cancer-axis) are provided to be movable along.

The fixed electrode 116 is fixed to the tip of the fixed arm 112, while the movable electrode 118 is fixed to the tip of the movable arm 114 so as to face the fixed electrode 116. Fixed electrode 116 and the movable electrode 118 is arranged so as to be aligned on the axis O _3. As the movable arm 114 is reciprocated along the axis _{O 3,} movable electrode 118, toward and away from the fixed electrode 116. In the present embodiment, the origin of the tool coordinate system C _T is located on the fixed electrode 116 (for example, the center of the upper surface), and the z-axis of the tool coordinate system C _T is the axis O ₃ (or the optical axis O). It is set to match (or be parallel).

When working on the work W with the tool 26, for example, the processor 18 causes the tool 26 to be sequentially arranged at the above-mentioned target positions P _n (n = 1 to 5). When the tool 26 is positioned at the target position P _5, working position on the workpiece W is positioned between the fixed electrode 116 and the movable electrode 118, the axis O _3, as well as substantially perpendicular to the surface of the workpiece W Arranged to pass through the work area.

When the tool 26 is positioned at the target position P _5, the processor 18 moves toward the movable electrode 118 to the fixed electrode, is sandwiched between the working position of the workpiece W between the stationary electrode 116 and movable electrode 118 The fixed electrode 116 and the movable electrode 118 are energized. As a result, the work portion of the work W is spot welded.

On the other hand, the optical device 16 is provided on the movable electrode 118. The optical device 16 is arranged so as to have a predetermined positional relationship with respect to the tool 26. Specifically, the optical device 16, the optical axis O coincides with the axis O ₃ (or, to be parallel) manner, are arranged with respect to the tool 26.

That is, the optical device 16, x-y plane of the image acquisition coordinate system _{C I} is the axis _{O 3} (or, z axes of the tool coordinate system _{C T)} so as to be perpendicular to, and is positioned relative to the tool 26. The optical device 16 is not limited to the movable electrode 118, and may be provided at any position (for example, the fixed electrode 116) of the nozzle 110.

The tool 26 shown in FIG. 22 is a robot hand. Specifically, the tool 26 has a hand base 120 connected to the wrist portion 36 and a plurality of finger portions 122 provided on the hand base 120 so as to be openable and closable. The tool 26 can grip and release a part (for example, a cylindrical cylinder for an automobile) by opening and closing the finger portion 122. In the present embodiment, the origin of the tool coordinate system C _T is arranged at a position (grip position) between the finger portions 122, and the z-axis of the tool coordinate system C _T is orthogonal to the opening / closing direction of the finger portion 122. Is set to.

When working on the work W with the tool 26, for example, after the processor 18 grips the part with the tool 26, the tool 26 is sequentially arranged at the above-mentioned target position _Pn (n = 1 to 5). .. When the tool 26 is positioned at the target position P _5, components that are gripped by the tool 26, the fitting hole of the working position on the work W (not shown), z-axis plus direction of the tool coordinate system C _T It is positioned at a position separated from. In this state, the processor 18, the tool 26, is moved in the z-axis negative direction of the tool coordinate system C _T, a part which is gripped by tool 26, is fitted into the fitting hole of the work W.

The optical device 16 is fixed to the finger portion 122 (for example, gripped by the finger portion 122). The optical device 16 is arranged so as to have a predetermined positional relationship with respect to the tool 26. Specifically, the optical device 16, the optical axis O z axis of the tool coordinate system C _T (or, part orientation to fit in the fitting hole a) coincides with (or is parallel) manner, tool It is arranged for 26. The optical device 16 is not limited to the finger portion 122, and may be provided at any position (for example, the hand base 120) of the tool 26.

In the above-described embodiment, the case where the optical device 16 (optical sensor 40) is fixed to the tool 26 and moved by the manipulator 24 has been described. However, the present invention is not limited to this, and the optical device 16 (optical sensor 40) may be arranged at a distance from the moving machine 14. Such a form will be described with reference to FIG.

In the mechanical system 130 shown in FIG. 23, the optical device 16 is fixed to the work W at a position separated from the moving machine 14. That is, the optical device 16 is not moved by the manipulator 24 and is stationaryly arranged with respect to the work W. Here, the optical device 16, as in the imaging coordinate system C _I is a known positional relationship with respect to the manipulator coordinate system C _M, is arranged in a predetermined positional relationship with respect to the mobile machine 14.

Next, the function of the mechanical system 130 will be described with reference to FIG. 24. Similar to the above embodiment, the processor 18 sends a movement command MC _n to the manipulator 24 according to the work program WP, and arranges the tool 26 in the order of the target positions P ₁ , P ₂ , P ₃ , P ₄ and P ₅ . .. In this embodiment, the optical device 16, to enter the imaging area of the optical sensor 40 when the tool 26 is between the target position P ₃ to P _5, it is arranged.

The processor 18 operates the optical sensor 40 while the tool 26 is moved to the target position P ₃ to P _5, continuously imaging the tool 26. For example, the processor 18, as in the embodiment of FIG. 8, to image the tool 26 the tool 26 at the target position _P 3, _{P 4} and the time tau _P3 reaches the respective _{P 5,} tau _P4 and tau _P5.

Figure 25 shows an example in which the processor 18 has imaged image data 140 in which the optical sensor 40 has captured the tool 26 at the time tau _P3 has been detected in that a tool 26 to the target position P _3. The tool 26 is shown in the image data 140 shown in FIG. 25. In practice, the components of the manipulator 24 (upper arm 34, etc.) are also shown in the image data 140, but for the sake of easy understanding, only the tool 26 is shown in the image data 140.

The processor 18 analyzes the image data 140 and extracts one visual feature VF of the tool 26 (for example, the center, edge, unevenness, etc. of the tool 26) reflected in the image data 140. Then, processor 18 obtains similar to the embodiment described above, as the information indicating the position of the visual features VF in the image data 140, the image acquisition coordinate system _{C I} visual features VF coordinates _(x 3, _{y 3)} To do.

Figure 26 shows an example in which the processor 18 has imaged image data 142 in which the optical sensor 40 has captured the tool 26 at the time tau _P4 was detected in that a tool 26 to the target position P _4. Incidentally, in FIG. 26, for comparison, the position of the visual features VF caught on the image data 140 shown in FIG. 25 are illustrated as white dots D _3.

The processor 18 analyzes the image data 142 and extracts the visual feature VF common to the image data 140. As shown in FIG. 26, the position of the visual features VF caught on the image data 142 from the position _{D 3} of the visual features VF caught on the image data 140, by a displacement amount _{E 3,} displaced in the imaging coordinate system _{C I} There is. Processor 18, as the position of the visual features VF in the image data 142, to obtain the coordinates _(x 4, _{y 4)} in the imaging coordinate system _{C I} visual characteristics VF.

Figure 27 shows an example in which the processor 18 has imaged image data 144 in which the optical sensor 40 has captured the tool 26 at the time tau _P5 it is detected that placing the tool 26 in the target position P _5. Incidentally, in FIG. 27, for comparison, illustrates the position of the visual features VF caught on the image data 140 shown in FIG. 25 as a white spot D _3, the visual features VF caught on the image data 142 shown in FIG. 26 It is shown positioned as white dots D _4.

The processor 18 performs image analysis on the image data 144 and extracts a visual feature VF common to the image data 140 and 142. As shown in FIG. 27, the position of the visual features VF caught on the image data 144 from the position _{D 4} of the visual features VF caught on the image data 142, by a displacement amount _{E 4,} displaced in the imaging coordinate system _{C I} There is. Processor 18, as the position of the visual features VF in the image data 144, to obtain the coordinates _(x 5, _{y 5)} in the imaging coordinate system _{C I} visual characteristics VF.

In synchronization with the operation of the optical sensor 40 to image the tool 26, the processor 18 operates the distance sensor 42 to measure the distance F in the direction of the optical axis O between the optical sensor 40 and the tool 26. Accordingly, the distance sensor 42 at the time tau _P3, tau _P4 and tau _P5, the distance _{F P3, F P4} and _{F P5} between the optical sensor 40 and the tool 26 is measured.

The processor 18 includes a coordinate acquired from the image data _{140 (x 3, y 3)} , the difference between the acquired coordinates _(x 4, _{y 4)} from the image data 142, the displacement amount E in the imaging coordinate system _{C I It} is calculated as ₃ (Fig. 26). As described above, the imaging coordinate system C _I, compared manipulator coordinate system C _M, is arranged in a known positional relationship.

Processor 18, and functions as a track acquiring unit 60, the positional relationship between the imaging coordinate system _{C I} and the manipulator coordinate system _{C M,} the displacement amount _{E 3} in the imaging coordinate system _{C I,} and the distance _{F P3} and _{F P4} used, movement locus _{K 3} in the manipulator coordinate system _{C M} tools 26 from time tau _P3 to time tau _P4 (FIG. 8), obtained by calculation.

Then, the processor 18 functions as the deviation amount acquisition unit 62 as in the above-described embodiment, and acquires the deviation amount δ ₄ (FIG. 8) of the movement locus K _{3 from} the reference locus A ₃ . Similarly, the processor 18 includes a positional relationship between the imaging coordinate system _{C I} and the manipulator coordinate system _{C M,} the displacement amount in the imaging coordinate system _{C I} acquired from the image data 142 and 144 _{E 4} (FIG. 27), the distance F by using the _P4 and _{F P5,} it acquires the movement trajectory _{K 4} in the manipulator coordinate system _{C M} tools 26 from time tau _P4 to time tau _P5. Then, the processor 18 acquires the deviation amount δ ₅ of the movement locus K _{4 from} the reference locus A ₄ .

As described above, according to the device 70 according to the present embodiment, the visual feature VF of the tool 26 is continuously imaged by the optical sensor 40 arranged apart from the mobile machine 14, and the image data 140 captured. From 142 and 144, the deviation amounts δ ₄ and δ ₅ can be obtained in the same manner as in the above-described embodiment.

In the embodiment shown in FIG. 23, the processor 18, as in the embodiment shown in FIG. 12, while the tool 26 is moved from the target position P ₃ to P _5, the workpiece W by the optical sensor 40, the period Imaging may be performed at a target or random timing. In this case, the processor 18 can acquire the amount of deviation of the above-mentioned movement loci K _α , K _β, and K _{γ from} the reference loci A ₃ and A ₄ .

Next, with reference to FIG. 28, the mechanical system 150 according to another embodiment will be described. In the mechanical system 150, the mobile machine 14 has a tool 26A for gripping the work W, while the tool 26B for working on the work W is arranged at a distance from the mobile machine 14.

The tool 26A is, for example, a robot hand and is connected to the wrist portion 36 of the moving machine 14. On the other hand, the tool 26B, for example, a laser machining head is not moved by the mobile machine 14, it is arranged to be stationary with respect to the manipulator coordinate system C _M. Further, in the mechanical system 150, the optical device 16 is fixed to the tool 26A (or the work W).

The processor 18 transmits a movement command MC _n to the manipulator 24 according to the work program WP, and moves the tool 26A holding the work W to a plurality of target positions P _n in order. Then, the processor 18 operates the optical sensor 40 while moving the tool 26A to continuously image the tool 26B as an object around the moving machine 14.

Then, the processor 18 functions as the locus acquisition unit 60, and moves the tool 26A based on the position of the visual feature VF of the tool 26B in the image data by using the same method as the form of FIGS. 25 to 27. Acquire the locus K. Then, the processor 18 functions as a deviation amount acquisition unit 62 to acquire the deviation amount δ of the movement locus K from the reference locus A.

Next, with reference to FIG. 29, the mechanical system 160 according to still another embodiment will be described. The mechanical system 160 is different from the above-mentioned mechanical system 150 in the installation position of the optical device 16. Specifically, in the mechanical system 160, the optical device 16 is fixed to the tool 26B at a position separated from the moving machine 14.

The processor 18 transmits a movement command MC _n to the manipulator 24 according to the work program WP, and moves the tool 26A holding the work W to a plurality of target positions P _n in order. Then, the processor 18 operates the optical sensor 40 while moving the tool 26A to continuously image the visual feature VF on the surface of the work W moved by the manipulator 24.

Then, the processor 18 functions as a locus acquisition unit 60, and moves the tool 26A based on the position of the visual feature VF of the work W in the image data by using the same method as the form of FIGS. 5 to 7. Acquire the locus K. Then, the processor 18 functions as a deviation amount acquisition unit 62 to acquire the deviation amount δ of the movement locus K from the reference locus A.

The light projecting unit 40a of the optical sensor 40 may emit light having a predetermined pattern. FIG. 30 shows an example of the light emitted by the light projecting unit 40a. In the example shown in FIG. 30, the light projecting unit 40a emits light in a grid pattern. According to this configuration, the optical sensor 40 can easily extract the visual feature VF of the imaging symmetry from the image data obtained by imaging the imaging symmetry.

Further, the optical sensor 40 is not limited to the one that receives the reflected light of the light emitted by the light projecting unit 40a by the light receiving unit 40b, and may be a general camera. Further, the optical sensor 40 and the distance sensor 42 can share the light emitting unit 40a and the light receiving unit 40b. In this case, the distance calculation unit 42c of the distance sensor 42 is an image pickup target from the optical sensor 40 based on the time from when the light projecting unit 40a emits light until the light receiving unit 40b receives the reflected light of the light. Measure the distance to.

Further, the distance sensor 42 can be omitted from the above-described embodiment. For example, in the above embodiment, the target position _P 3, _{P 4} and _{P 5,} when the coordinate in the z-axis of the manipulator coordinate system _{C M} is set equal, and the target position _P 3 to P ₅ and the workpiece W The distance is constant. Therefore, in this case, without measuring the distance F from the optical sensor 40 to the workpiece W, from the image data optical sensor 40 is captured, the movement trajectory K of the tool 26 (or 26A), of the manipulator coordinate system C _M x It can be obtained as a two-dimensional locus in a plane parallel to the −y plane.

Further, in the above-described embodiment, the locus acquisition unit 60, the deviation amount acquisition unit 62, the position correction unit 64, the determination unit 92, the warning generation unit 94, the position data acquisition unit 102, the reference locus acquisition unit 104, and the second determination. The case where the functions of the unit 106 and the second warning generation unit 108 are implemented in the control device 12 and the processor 18 executes these functions has been described.

However, the present invention is not limited to this, and the locus acquisition unit 60, the deviation amount acquisition unit 62, the position correction unit 64, the determination unit 92, the warning generation unit 94, the position data acquisition unit 102, the reference locus acquisition unit 104, and the second determination unit 106. , And at least one of the second warning generation units 108 may be provided separately from the control device 12. Such an embodiment is shown in FIG.

The mechanical system 170 shown in FIG. 31 includes a control device 12, a mobile machine 14, and a device 172. The device 172 acquires the deviation of the movement locus K of the tool 26 from the reference A, and includes an optical sensor 40, a locus acquisition unit 60, and a deviation amount acquisition unit 62. The locus acquisition unit 60 and the deviation amount acquisition unit 62 are provided separately from the control device 12. The locus acquisition unit 60 and the deviation amount acquisition unit 62 may be mounted on one computer having a processor and a storage unit, or may be mounted on separate computers.

In the above embodiment, while the tool 26 is moved to the target position P ₃ to P _5, the optical sensor 40, the workpiece W has been captured as an object around the mobile machine 14. However, the present invention is not limited to this, and the optical sensor 40 may image any object around the moving machine 14. Such a form will be described with reference to FIG.

In the form shown in FIG. 32, the object Q is installed on the work W. Object Q is spaced apart from the mobile machine 14 so that the x-z plane of the manipulator coordinate system C _M and substantially parallel and fixed relative to the workpiece W. On the other hand, the optical device 16 with respect to tool 26, the optical axis O is a positional relationship parallel to the y-axis of the tool coordinate system C _T, is arranged.

In this embodiment, while the processor 18 is moving the tool 26 to the target position P _3, P ₄ and P _5, the optical sensor 40 is continuously imaging an object Q. Then, the processor 18 analyzes the captured image data and extracts the visual feature VF of the object Q. Then, the processor 18 functions as a locus acquisition unit 60, and moves the tool 26 based on the position of the visual feature VF of the object Q in the image data by using the same method as the form of FIGS. 5 to 7. Acquire the locus K _n .

Then, the processor 18 can function as shift amount acquisition unit 62 acquires the shift amount [delta] _n from the reference trajectory A _n of the movement locus K _n. A mark (painting, engraving, sticker, etc.) may be provided on the object Q, and the optical sensor 40 may image the mark as a visual feature VF. Similarly, in the above-described embodiment in which the optical sensor 40 images the work W, the optical sensor 40 may image the mark on the work W as a visual feature VF by providing a mark on the surface of the work W.

In addition, in the above-described embodiment, the case where the locus acquisition unit 60 acquires the movement locus K of the tool 26 has been described. However, not limited to this, the locus acquisition unit 60 may be configured to acquire the movement locus of any movable component of the manipulator 24. In this case, the deviation amount acquisition unit 62 acquires the deviation amount from the reference locus of the movement locus of the movable component.

Further, in the above-described embodiment, five target positions P ₁ to P ₅ has dealt with the case where is set, the number of target positions P _n can be any number. Further, each of these target positions _Pn may be a teaching point, or may be set as a teaching point and an interpolation point. Further, any number of interpolation points may be set between the two teaching points.

Further, the manipulator 24 is not limited to the vertical articulated robot, and may be any type of robot such as a horizontal articulated robot or a parallel link robot. Alternatively, the manipulator 24 may be a work table device having a ball screw mechanism in the x-axis direction, a ball screw mechanism in the y-axis direction, and a ball screw mechanism in the z-axis direction.

Although the present disclosure has been described above through the embodiments, the above-described embodiments do not limit the invention according to the claims.

10, 130, 150, 160, 170 Mechanical system 12 Control device 14 Mobile machine 16 Optical device 18 Processor 24 Manipulator 26, 26A, 26B Tool 40 Optical sensor 42 Distance sensor 60 Trajectory acquisition unit 62 Misalignment amount acquisition unit 64 Position correction unit

Claims (11)

It is a device that acquires the deviation from the reference of the movement trajectory of the moving machine.
Moved by the moving machine, the visual features of objects around the moving machine are continuously imaged or placed away from the moving machine while the moving machine is moving according to a movement command. An optical sensor that continuously captures the visual features of the moving machine or an object moved by the moving machine while the moving machine is moving according to a movement command.
Based on the position of the visual feature in the first image data captured by the optical sensor and the position of the visual feature in the second image data captured by the optical sensor after the first image data. A locus acquisition unit that acquires the movement locus of the moving machine from the time of capturing the first image data to the time of capturing the second image data.
An apparatus including a deviation amount acquisition unit that acquires a deviation amount of the movement locus acquired by the locus acquisition unit from a reference trajectory of the moving machine defined by the movement command. The movement command includes information on a plurality of target positions where the moving machine should be placed.
The device according to claim 1, wherein the optical sensor images the visual features when the moving machine reaches each of the plurality of target positions. The optical sensor is
When the moving machine reaches the first target position, the first image data is imaged, and the first image data is captured.
When the moving machine reaches the second target position, the second image data is imaged, and the second image data is captured.
According to claim 2, the deviation amount acquisition unit acquires the deviation amount of the movement locus acquired by the locus acquisition unit from the reference locus from the first target position to the second target position. The device described. Further comprising a distance sensor that measures the distance from the optical sensor to the imaging target when the optical sensor images the visual feature.
The device according to any one of claims 1 to 3, wherein the locus acquisition unit acquires the movement locus based on the distance measured by the distance sensor. The locus acquisition unit
The imaging, which indicates the position of the visual feature in the first image data, the first coordinate of the imaging coordinate system of the optical sensor, and the position of the visual feature in the second image data. Get the second coordinate of the coordinate system and
The apparatus according to any one of claims 1 to 4, wherein the movement locus is acquired by using the difference between the first coordinate and the second coordinate in the imaging coordinate system. The apparatus according to any one of claims 1 to 5, further comprising a position correction unit that corrects the position of the moving machine based on the deviation amount acquired by the deviation amount acquisition unit. A determination unit that determines whether or not the deviation amount acquired by the deviation amount acquisition unit is larger than a predetermined threshold value.
The apparatus according to any one of claims 1 to 6, further comprising a warning generation unit that generates a warning signal when the determination unit determines that the deviation amount is larger than the threshold value. A position data acquisition unit that acquires position data of the moving machine while the moving machine is moving,
The invention according to any one of claims 1 to 7, further comprising a reference locus acquisition unit that acquires the movement locus of the moving machine as a reference locus based on the position data acquired by the position data acquisition unit. apparatus. The deviation amount acquisition unit further acquires the deviation amount of the reference locus acquired by the reference locus acquisition unit from the reference locus as the reference deviation amount.
The device
A second determination unit that determines whether or not the reference deviation amount is larger than a predetermined second threshold value, and
8. The eighth aspect of the present invention further includes a second warning generation unit that generates a second warning signal when the reference deviation amount is determined to be larger than the second threshold value by the second determination unit. The device described. The moving machine has a tool and a manipulator that moves the tool.
The manipulator moves the tool with respect to the work and
The optical sensor is
Placed in a predetermined positional relationship with respect to the tool, moved by the manipulator,
The visual features of the work as the surrounding object are continuously imaged.
The device according to any one of claims 1 to 9, wherein the locus acquisition unit acquires the movement locus of the tool. It is a method to obtain the deviation from the reference of the movement trajectory of the moving machine.
The optical sensor moved by the moving machine continuously images the visual features of objects around the moving machine while the moving machine is moving according to a movement command, or is separated from the moving machine. The optical sensors arranged on the moving machine continuously image the visual features of the moving machine or the object moved by the moving machine while the moving machine is moving according to the movement command.
Based on the position of the visual feature in the first image data captured by the optical sensor and the position of the visual feature in the second image data captured by the optical sensor after the first image data. Then, the movement locus of the moving machine from the time of capturing the first image data to the time of capturing the second image data is acquired.
A method for acquiring the amount of deviation of the acquired movement locus from the reference locus of the moving machine defined by the movement command.

Images