JPH071368A - Position and attitude recognizing method of robot and device thereof - Google Patents

Abstract

PURPOSE:To recognize the position and attitude of a robot without stopping it by detecting an arm angle in the robot, counting this detection signal with a reversible counter, and calculating the travel of an arm from the counted value, while storing this counted value in a register. CONSTITUTION:Each angle of respective arms 3 and 4 in a robot 1 is detected by each of encoders 15 and 16, and each detecting signal is processed by respective detectors 25a and 25b, while relations between each of data of these detectors 25a and 25b and a system of coordinates in the robot 1 are calculated by a central processing unit 21. Incidentally, in these detectors 25a and 25b, a wave detection signal is detected by a phase detector 251, and the wave detection signal is reversibly counted by a reversible counter 252, while when it is produced at camera timing, the countered value of this reversible counter 252 is set through a register 255 by a position recognizing signal. In addition, this register 255 is able to use all angle data at all times in order to keep the angle data at the camera timing.

Description

Detailed Description of the Invention

[0001]

FIELD OF THE INVENTION The present invention relates to a robot. More specifically, the present invention relates to a method and apparatus for recognizing the position / orientation of a robot.

[0002]

2. Description of the Related Art Generally, various types of robots are provided, and it is very important for any type of robot to recognize the position and orientation of the tip of the robot arm in order to drive and control the robot. is important.

Conventionally, as a method of recognizing the position and orientation of a robot, (1) a sensor having neither pattern recognition ability, (2) a sensor having pattern recognition ability, or (3) a sensor and pattern recognition ability They are roughly divided into those with ability. Each method has its own advantages and disadvantages, and they are used properly according to the purpose of their use, but due to the development of computers, the method of (1) above, which can estimate the position relatively inexpensively and reliably, is often adopted. Has been done.

This is a method in which an initial position is given and then the current position is estimated from the number of revolutions of a motor or the like. Specifically, the number of revolutions of the motor is detected by an encoder and taken into the computer. The robot position is estimated by performing a predetermined calculation based on the initial position given by the computer in advance and the detection pulse from the encoder.

[0005]

However, in the above-mentioned conventional position / orientation recognition method for the robot, the position can be recognized only at each calculation cycle of the computer. Therefore, when the position is recognized, the robot arm is used. It is necessary to move the robot at a low speed or to stop the robot arm in some cases, and there is a drawback that recognition takes too long.

An object of the present invention is to solve the above-mentioned drawbacks of the prior art and to provide a robot position / orientation recognition method and device capable of recognizing the position and orientation in a short time.

[0007]

In order to achieve the above object, a robot position / orientation recognition method according to a first aspect of the present invention counts a detection signal from a sensor for detecting an angle of each arm with a reversible counter. Then, by calculating the movement amount of each arm from the count value of these reversible counters, the rough position / orientation of each arm is obtained, and the position recognition signal is sent at the imaging timing of the visual recognition device provided at the tip of the robot arm. The count value of the reversible counter is generated in the register by the position recognition signal at this imaging timing, and the relation between the data obtained from the reversible counter, the value of the register and the imaging data and the coordinate system of the robot is obtained. ing.

According to a second aspect of the present invention, there is provided a position / orientation recognizing device for a robot, wherein a sensor for detecting an angle of an arm and outputting a detection signal, and a phase for detecting a detection signal from the sensor to obtain a detection signal. In a detection circuit for each arm, a detection circuit, a reversible counter for reversibly counting the detection signal from the phase detection circuit, a detection circuit including a register for setting the count value of the reversible counter by a position recognition signal generated at imaging timing, The count value of the reversible counter is read, the movement amount of the arm is calculated from the count value, the value set in the register in the detection circuit for each arm is read, and the data obtained from the set value and imaging data and the coordinates of the robot. An arithmetic processing unit for obtaining the relationship with the system is provided.

Further, a robot position / orientation recognizing device according to a third aspect of the present invention includes a sensor for detecting an angle of an arm and outputting a detection signal, and a detection signal for detecting a phase of the detection signal from the sensor. A phase detection circuit that obtains a reversible counter that reversibly counts the detection signal from the phase detection circuit,
A register that sets the count value of the reversible counter according to a recognition signal generated at the image pickup timing, a detection circuit that includes a determination signal holding unit that is set according to the recognition signal generated at the image pickup timing, and a reversible counter in the detection circuit for each arm. And the amount of movement of the arm is calculated from the count value, and the value set in the register when the determination signal holding unit is in the set state, and the data obtained from the set value and imaging data An arithmetic processing unit for obtaining a relationship with the coordinate system of the robot is provided.

[0010]

According to the first aspect of the invention, the reversible counter counts the detection signals from the sensors that detect the angle of each arm,
By calculating the amount of movement of each arm from the count values of these reversible counters, the rough position / orientation of each arm can be obtained. Further, a rough position / orientation is obtained, and when the value enters a prescribed area, a visual recognition device provided at the tip of the robot arm is imaged. A position recognition signal is generated at this imaging timing. The count value of the reversible counter is stored in the register by the recognition signal at this imaging timing. That is, since the angle of each arm at the image pickup timing is in the reversible counter, these are stored in the register. With this, conventionally, the process of stopping the movement of each arm at the image pickup timing, reading the value of each reversible counter by the arithmetic processing unit, and moving the arm again after the reading is completed, the present invention requires movement. Especially, since the count value of the reversible counter at the imaging timing is stored in the register, it is not necessary to stop each arm. Thereby, the relationship between the data obtained from the reversible counter, the value of the register, and the imaging data and the coordinate system of the robot can be obtained without stopping the robot.

According to a second aspect of the invention, in order to realize the invention of the first aspect, a detection circuit including the phase detection circuit, the reversible counter, and the register is provided for one sensor, Further, a detection circuit is provided for each sensor, and the arithmetic processing unit reads the count value of the reversible counter in each of the detection circuits and calculates the movement amount of the arm from the count value. Further, the arithmetic processing unit can read the value set in the register in each of the detection circuits and obtain the relation between the data obtained from the set value and the image pickup data and the coordinate system of the robot.

According to a third aspect of the present invention, a determination signal holding section is added to the detection circuit according to the second aspect of the invention, and it is set that a recognition signal has been input. This is to notify that the has been input. With this, even with a recognition signal that is not synchronized with the arithmetic processing unit, the relationship between the set value of each register and the data obtained from the imaging data and the coordinate system of the robot can be reliably obtained.

[0013]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The structure of the present invention will be described in detail below with reference to the embodiments shown in the drawings.

FIG. 1 shows an example of an apparatus for realizing an embodiment of the position / orientation recognition method for a robot of the present invention. In the figure, reference numeral 1 is a SCARA robot. The SCARA robot 1 mainly includes a base 2, a first arm 3, a second arm 4, a first motor 5, a first speed reducer 5A, a second motor 6, a second speed reducer 6A, a third motor 7, and a third motor 7. It is provided with four motors 8 and a third speed reducer 8A. Also, the first arm 3 is attached to the base 2.
Is rotatably fixed, and the second arm 4 is rotatably fixed to the tip of the first arm 3. The first motor 5 and the fourth motor 8 are fixed to the base 2. The first motor 5 is connected to the first arm 3 via the speed reducer 5A.
Is designed to rotate. The fourth motor 8 includes a pelt 10a hung over pulleys 9a and 9b and a pulley 9a.
The spline shaft tip grip 11a can be rotated by a desired angle by means of the pelt 10b that is hung on c and 9d. The second motor 6 is the first arm 3
The second motor 4 can be rotated by the second motor 6. A third motor 7 is internally fixed inside the tip of the second motor 6, and the third motor 7 rotates the ball screw 14 by means of the pelt 13 hung on the pulleys 12a and 12b to show the spline shaft 11b. It can move up and down. Further, the first motor 5, the second motor 6, the third motor 7, and the fourth motor 8 have encoders 15, 16, 17, and 1 respectively.
8 are provided for each encoder 15, 1
6, 17 and 18 can detect the number of rotations of each motor 5, 6, 7 and 8. These encoders 1
Reference numerals 5 and 16 are used as sensors that detect the angles of the first arm 3 and the second arm 4. Of course, the encoder 17 is used as a sensor for measuring the vertical movement distance of the spline shaft distal end grip portion 11a, and the encoder 18 is used as a sensor for measuring the angle of the spline shaft distal end grip portion 11a.

The SCARA type robot 1 is electrically connected to a robot controller 20 and is a robot controller 2.
Under control of 0, rotate each motor 5, 6, 7 and 8
Further, the rotation of each motor 5, 6, 7 and 8 is drive-controlled by the detection signal from each encoder 15, 16, 17 and 18, and the tip position of the second arm 4 can be estimated.

The robot controller 20 includes an arithmetic processing unit (CPU) 21 for executing various arithmetic processes and various controls.
, Bus 22, memory 23, and servo motor control interfaces 24a, 24b, 24c, 24
d and detection circuits 25a, 25b, 25c, 25d. The CPU 21 has a memory 23 and a servo motor control interface 2 via a bus 22.
4a, 24b, 24c, 24d are connected, and the detection circuits 25a, 25b, 25c, 25d are connected. Although the servo motor control interfaces 24a, 24b, 24c and 24d are not shown,
The motors 5, 6, 7 and 8 are electrically connected to each other, and the rotation of the motors 5, 6, 7 and 8 is controlled by the CPU 21.
The rotation drive is controlled based on the command. Detection circuit 25a,
Since 25b, 25c, and 25d have the same circuit configuration, only the configuration of the detection circuit 25a will be described. The detection circuit 25a takes in the detection signals SA and SB from the encoder 15 and detects them to detect the detection signals Sa and SB.
A phase detection circuit 251 that can output one of Sb, a reversible counter 252 that subtracts and counts one of the detection signals Sa and Sb from the phase detection circuit 251, and a count value of this reversible counter 252 to the CPU 21. A three-state buffer 253 to be read, a differentiation circuit 254 that forms a differential signal Sc of the position recognition signal SC when the position recognition signal SC is input, and a differentiation circuit 254 when the differential signal Sc is input. A register 255 that sets the count value of the reversible counter 252, a three-state buffer 256 that causes the CPU 21 to read the set value of the register 255, and a register C that is set when the differential signal Sc is input.
It is composed of a determination signal holding unit 257 which is reset by the PU 21.

FIG. 2 shows an example of a specific configuration of the detection circuit 25a of the robot controller 20. This figure shows, in particular, three-state buffers 253, 256 and a differentiation circuit 254.
A configuration example of the determination signal holding unit 257 has been described.

The phase detection circuit 251 takes in the detection signals SA, SB and the clock φ ₀ from the encoder 15,
The detection signal Sa or Sb can be supplied to the reversible counter 252. The reversible counter 252 is a circuit that counts the detection signal Sa or Sb. Reversible counter 25
The output of 2 is supplied to the three-state buffer 253 and the register 255. The three-state buffer 253 operates by a read signal R ₀ given from the CPU 21 via the bus 22, and supplies the count value output of the reversible counter 252 to the data bus of the bus 22. As a result, the CPU 21 can take in the count value of the reversible counter 252.

The position recognition signal SC is input to the differentiating circuit 254. Differentiating circuit 254 includes two D-type flip-flop FF _1, FF _2, and a single NAND circuit NA, enter the position identification signals SC to the D input terminal of the flip-flop FF _1, the flip-flop FF ₁ Output connects the terminal to the D input terminal of the flip-flop FF _2, the clock φ to the clock terminal of the flip-flop FF _1, FF _2
0 is supplied, and the signals from the output terminals of the flip-flops FF ₁ and FF ₂ are supplied to both input terminals of the NAND circuit NA,
The differential signal Sc is obtained from the output terminal of the NAND circuit NA. This differential signal Sc is stored in the register 255.
Is input to the determination signal holding unit 257.

The register 255 sets the count value of the reversible counter 252 at the time when the differential signal Sc is input, and supplies it to the three-state buffer 256. The three-state buffer 256 is connected from the CPU 21 to the bus 2
It operates in response to a read signal R ₁ given via 2 to supply the value of the register 255 to the data bus of the bus 22.

Further, the determination signal holding unit 257 includes a flip-flop FF _3, and an AND circuit AD, a buffer BF, the differential signal Sc is inputted to the flip-flop FF _3, is set flip-flop FF ₃ The output signal from the AND circuit AD is reset. The output terminal of the flip-flop FF ₃ is connected to the data bus of the bus 22 via the buffer BF, and the buffer BF operates by the read signal R ₂ from the CPU 21 to output the output signal of the flip-flop FF ₃ to the bus 22. Can be supplied to the data bus. The output of the buffer BF is supplied to one input terminal of the AND circuit AD, and the write signal W ₀ supplied from the CPU 21 via the bus 22 is supplied to the other input terminal of the AND circuit AD. . The AND circuit AD then outputs the output of the buffer BF and the write signal W
_A clear signal can be supplied to the clear terminal of the flip-flop FF ₃ when both ₀ and ₀ are logically ANDed.

FIG. 3 shows an example of a specific configuration of the phase detection circuit 251. In the figure, the phase detection circuit 251
Are four flip-flops FF ₂₁ , FF ₂₂ , FF ₂₃ ,
An FF ₂₄ , four AND circuits AD ₂₁ , AD ₂₂ , AD ₂₃ , AD ₂₄ having at least one inverting input terminal, and two OR circuits OR ₂₁ , OR ₂₂ are provided.

The signal SA from the encoder is input to the D input terminal of the flip-flop FF _21, connects the output terminal of the flip-flop FF ₂₁ to the D input terminal of the flip-flop FF _22, the flip-flop FF _21, FF ₂₂ A clock φ ₀ can be given to the clock terminal.
The signal from the output terminal of the flip-flop FF ₂₁ is supplied to the input terminals of the AND circuits AD ₂₁ , AD ₂₄ , AD ₂₇ , and AD ₂₈ , and at the same time, the AND circuits AD ₂₂ , AD ₂₃ , AD ₂₅ ,
It can be supplied to the inverting input terminal of AD ₂₆ .
The signal from the output terminal of the flip-flop FF ₂₂ is supplied to the input terminals of the AND circuits AD ₂₁ , AD ₂₂ , AD ₂₅ , AD ₂₇ , and at the same time, the AND circuits AD ₂₃ , AD ₂₄ , AD ₂₆ ,
It can be supplied to the inverting input terminal of AD ₂₇ .
The signal SB from the encoder is the flip-flop FF _23.
Input to the D input terminal of the flip-flop FF ₂₃ , and the output terminal of the flip-flop FF ₂₃ is connected to the D input terminal of the flip-flop FF ₂₄ .
The clock φ ₀ can be applied to the clock terminals of the flip-flops FF ₂₃ and FF ₂₄ . The signal from the output terminal of the flip-flop FF ₂₃ is the AND circuit A
In addition to being supplied to the input terminals of D ₂₃ , AD ₂₄ , AD ₂₅ , and AD ₂₈ , they can be supplied to the inverting input terminals of the AND circuits AD ₂₁ , AD ₂₂ , AD ₂₆ , and AD ₂₇ . The signal from the output terminal of the flip-flop FF ₂₄ is the AND circuit A
In addition to being supplied to the input terminals of D ₂₁ , AD ₂₄ , AD ₂₅ , and AD ₂₆ , they can be supplied to the inverting input terminals of the AND circuits AD ₂₂ , AD ₂₃ , AD ₂₇ , and AD ₂₈ . The output terminals of the AND circuits AD ₂₁ , AD ₂₂ , AD ₂₃ , and AD ₂₄ are connected to the input terminals of the OR circuit OR ₂₁ , and the OR circuit O ₂₁
The detection signal Sa of the R ₂₁ is output from the output terminal when the logical product of the signals input to the respective input terminals is taken. The output terminals of the AND circuits AD ₂₅ , AD ₂₆ , AD ₂₇ , and AD ₂₈ are connected to the input terminals of the OR circuit OR ₂₂ ,
The OR circuit OR ₂₂ outputs the detection signal Sa from the output terminal when the logical product of the signals input to the respective input terminals is obtained.

FIG. 4 shows an explanatory view for explaining the position / orientation recognition method of the robot. Incidentally, FIG. 4 shows the robot 1 as viewed from above. A first arm 3 is rotatably fixed to the base 2, and an X-axis and a Y-axis are set through the origin O ₁ with this fixed point as an origin O ₁ . A second arm 4 is rotatably fixed to the first arm 3, and this fixed point is referred to as a moving point O ₂ . The length of the first arm 3 is set to the length L ₁ between the origin O ₁ and the moving point O _2, and the length of the second arm 4 is set to the moving point O ₂ and the moving point O _2.
3 and the length L ₂ between, and the distance between the camera 30 attached to the tip of the arm 11c fixed to the spline shaft distal clasps 11a to move point O ₂ and L ₂ '. Further, the positions of the workpiece 35 with respect to the X axis and the Y axis are Xt and Yt,
And the camera 30 and the work 35 at the coordinates of the camera 30
The positional relationship with and is Xc and Yc.

The operation of the robot position / orientation recognizing device as described above will be described below with reference to FIGS. 1 to 4 and FIGS. Note that FIG. 5 is a processing flow of the CPU 21. Further, FIG. 6 is a timing chart of the phase detection circuit 251, in which the horizontal axis indicates time and the vertical axis indicates each signal. Further, in this embodiment, the camera 30 is originally rotatable about the moving point O ₃ and is also movable in the orthogonal coordinate system direction with respect to the XY plane. The camera 30 will be described as being fixed to the second arm 4. Therefore, the robot controller 20 processes the detection signals SA and SB from the encoder 15 and the encoder 16.

First, the CPU 21 of the robot controller 20
Based on the command from the servo motor control interfaces 24a and 24b, the first motor 5 and the second motor 5
The motor 6 is driven to rotate. As a result, the first motor 5 rotates and moves the first arm 3 in the direction corresponding to the rotation direction of the first motor 5 via the first speed reducer 5A. In addition, the second motor 6 rotates to move the second arm 4 in the direction corresponding to the rotation direction of the second motor 6 via the second speed reducer 6A. The rotation numbers and rotation directions of the first motor 5 and the second motor 6 are detected by the encoder 15 and the encoder 16. Detection signals SA and SB from the encoder 15
Is input to the detection circuit 25a. Also, encoder 1
The detection signals SA and SB from 6 are input to the detection circuit 25b.

The detection signals SA and SB are detected by the detection circuit 25.
The phase is detected by the phase detection circuit 251 a. That is, as shown in FIG. 6, the clock phi ₀ is the phase detector circuit 2
51 flip-flops FF ₂₁ , FF _22, FF ₂₃ , F
When inputting to the clock terminal of F ₂₄ , the period Tp
At the timings shown in FIG.
Is input, the flip-flops FF ₂₁ , FF _22, F
The detection signal Sa is obtained from the OR circuit OR ₂₁ by the action of F ₂₃ , FF ₂₄ , and AND circuits AD _{21 to} AD ₂₈ , and the detection signal SA at the timing shown in the figure in the period Tm,
When SB is input, flip-flops FF ₂₁ , FF
_The detection signal Sb is obtained from the OR circuit OR ₂₂ by the action of _22, FF ₂₃ , FF ₂₄ , and AND circuits AD _{21 to} AD ₂₈ (FIG. 6).

Such detection signals Sa and Sb are input to the reversible counter 252. In the reversible counter 252,
For example, when the detection signal Sa is input, it is added, and when the detection signal Sb is input, it is subtracted.

Similarly, the detection signal S from the encoder 16
A and SB are the detection circuits 25a in the detection circuit 25b.
Is processed in the same way as.

When the CPU 21 receives the timer interrupt, the CPU 21 proceeds to the process of the flowchart shown in FIG.
The CPU 21 first performs a process of reading the value of the reversible counter 252 of the detection circuits 25a and 25b (step 50).
1). This is because when the CPU 21 first gives a read signal R ₀ to the three-state buffer 253a via the bus 22 with n = 1, the three-state buffer 253a.
Becomes active and the count value Rn of the reversible counter 252a is put on the data bus of the bus 22.
By reading this (count value Rn), the reading process ends. Next, the CPU 21 applies the read signal R ₁ to the three-state buffer (not shown) via the bus 22 with n = 2, the three-state buffer becomes active, and the reversible counter (not shown) is counted. Number R ₂
Is loaded on the data bus of the bus 22, the CPU 21 reads this (count value R ₂ ) to complete the read processing. In this way, when the process of reading the necessary external data is completed, the calculation process is started.

Next, the CPU 21 executes a process of extracting only the change amount of the movement amount of the first arm 3 (step 5).
02). This is the amount of movement of the first arm 3 by subtracting the previous count value Z ₁ of the reversible counter 252 stored in the memory 23 from the count value R ₁ read this time and setting the calculation result to D _1. (Change) is obtained. Similarly, the moving amount (change amount) of the second arm 4 can be obtained by calculating D ₂ = R ₂ −Z ₂ .

Next, the CPU 21 performs a process for obtaining the movement amount (step 503). First, the calculation result D ₁ is added to the data stored in the area P ₁ of the memory 23 (P ₁ = P ₁ + D ₁ ). Similarly, P ₂ = P ₂ + D ₂ is calculated. As a result, P ₁ and P ₂ can be obtained.

Thereafter, the CPU 21 stores the count values R ₁ and R ₂ read this time as Z ₁ and Z ₂ in each area of the memory 23 (step 504).

When this process ends, the CPU 21
Performs a process of calculating the angle theta ₁ and the second arm 4 of the first arm 3 the angle theta ₂ of the first arm 3 (step 505). This is because the angle θ ₁ formed on the X axis of the first arm 3 is obtained by calculating θ ₁ = P ₁ / ja from the reduction ratio ja of the first reduction gear 5A, and the reduction ratio of the second reduction gear 6A is calculated. The angle θ ₁ formed by the second arm 4 with respect to the first arm 3 is obtained by calculating θ ₂ = P ₂ / ja from jb.

After the respective angles θ ₁ and θ ₂ are calculated in this way, the CPU 21 calculates the position of the moving point O ₃ at the tip of the second arm 4 using the following mathematical formula 1 (step 506). ).

[0036]

[Equation 1] After calculating X and Y in this way, the CPU 21
The state of the judgment signal holding unit 257 is read (step 5).
07). This applies the read signal R ₂ from the CPU 21 to the buffer BF via the bus 22 to activate the buffer BF, and the flip-flop FF ₂
The signal at the output terminal of is sent to the CP via the data bus of bus 22.
Obtained by importing into U21. At this point,
Since the value of the flip-flop FF ₃ is a logic "0" (step 507; N), CPU 21 is exits the timer routine proceeds to other processing.

By performing the above-described processing at regular time intervals by a command from the timer, it is possible to obtain the rough position and posture of the first arm 3 and the second arm 4.

Here, when the CPU 21 enters the area in which the rough position and orientation of the first arm 3 and the second arm 4 are defined (the area where the camera 30 can capture an image), the CPU 21 closes the shutter of the camera 30. Command is output. At the same time as the shutter of the camera 30 is closed by this shutter close command, the position recognition signal SC is generated by the shutter close signal.
Is formed. This position recognition signal SC is detected by the detection circuit 25.
a, 25b, 25c, 25d. For example, in the detection circuit 25a, in the differentiating circuit 254, the clock φ ₁ is always supplied to the clock terminals of the two flip-flops FF ₁ and FF ₂ , and the position recognition signal SC is input to the flip-flop FF _1. The rising portions of the position recognition signal SC are differentiated by the action of the two flip-flops FF ₁ and FF ₂ and the NAND circuit NA, and the differentiated signal Sc is output from the output terminal of the NAND circuit NA. When this differential signal Sc is supplied to the register 255, the register 255 sets the count value of the reversible counter 252 at that time. As a result, the register 255 retains the value at that time.

On the other hand, the differential signal Sc is stored in the judgment signal holding section 25.
It is also supplied to 7. In the determination signal holding unit 257, the flip-flop FF ₃ is set when the differential signal Sc is input.

When a timer interrupt occurs in such a state, the CPU 21 processes the flowchart shown in FIG. 5 as described above (steps 501 to 506), and then the output of the flip-flop FF ₃ becomes the logic "1". It is determined (step 507). That is, the CPU 21
Applies a read signal R ₂ to the buffer BF of the determination signal holding unit 257 to activate the buffer BF and read the contents of the flip-flop FF ₃ into the CPU 21 via the bus 22. In this case, since the flip-flop FF ₃ is set, the data at the output terminal of the flip-flop FF ₃ is logical "1" (step 50).
7; Y), the CPU 21 uses the register 2 of the detection circuit 25a.
The value C _{1 of} 55 and the value C ₂ of the register of the detection circuit 25b are read, and the previously processed steps 501 to 5 are performed.
Formulas 2 and 3 are calculated using the data obtained in 06 (step 508).

[0041]

[Equation 2]

[Equation 3] In this way, X ', Y'can be obtained. After that, the CPU 21 sends the determination signal holding unit 2 via the bus 22.
When the runt signal W ₀ is output to the AND circuit 57 of 57,
The AND circuit AD takes the logical product and the flip-flop F
F ₃ is reset (step 508).

Thus, for example, when the position to be gripped by the robot 1 is found in the image coordinate system from the image data, the handshake bit is operated to notify the main routine of the data acquisition (step 509).
Then, the CPU 21 uses Expression 4 to convert the information on the position and orientation of the camera coordinates into the robot coordinates.

[0043]

[Equation 4] As described above, the robot 1
It is possible to calculate the gripping position of. Therefore, in this embodiment, the target position can be calculated from the image pickup only by passing over the image pickup point, so that the work can be speeded up.

In the above embodiment, the position recognition signal SC
In order to reliably detect that the CPU 21 has been input, a determination signal holding unit 257 is provided, the determination signal holding unit 257 is set when the position recognition signal SC is input, and the processing flow of the CPU 21 at the time of timer interruption is described. Although it is determined whether or not the determination signal holding unit 257 is set / reset in the processing, and the subsequent processing is executed, it is known that the CPU 21 side has issued a shutter close command (imaging command). The value of 255 may be read. In the case of such a configuration, the determination signal holding unit 257 becomes unnecessary.

FIG. 7 shows an application example of the present invention. In this application example, the laser sensor 40 is fixed to the tip side of the spline shaft 11b of the robot 1 so that the distance to the work 35 can be measured.
The position recognition signal SC can be output when 1b descends and reaches a certain distance from the work 35.

That is, a sensor processing device 41 is connected to the laser sensor 40. This sensor processing device 41 is
While driving the laser sensor 40, the laser sensor 4
The detection signal from 0 can be processed to output the distance between the laser sensor 40 and the work 35 as digital data or a distance detection voltage signal _VL corresponding to the distance. Distance detection voltage signal V from the sensor processing device 41
_L is input to one input terminal of the comparator 42. The other input terminal of the comparator 42 is connected to the variable terminal of the variable resistor 43 that forms the comparison voltage V _REF . The variable resistor 43 can change the comparison voltage V _REF . Further, the comparator 42 outputs "H" from its output terminal when the distance detection voltage signal V _L larger than the comparison voltage V _REF is input, and when the distance detection voltage signal V _L other than the above is input. "L" is output from the output terminal.

Now, when the laser sensor 40 and the work 35 approach each other by a fixed distance, the sensor processing device 41 outputs a distance detection voltage signal V _L according to the fixed distance.
When the distance detection voltage signal V _L is input to one input terminal of the comparator 42, it becomes larger than the preset comparison voltage V _REF , so that “H” is output from the output terminal of the comparator 42. . "H" output from the output terminal of the comparator 42 is input to the detection circuits 25a, 25b, 25c, 25d as the position recognition signal SC. As a result, the detection circuits 25a, 25b, 25
The values of the reversible counter 252 at that time are set in the registers 255 of c and 25d. In addition, each detection circuit 25
a, 25b, 25c, 25d determination signal holding unit 257
Will be set to "1". Therefore, if the determination signal holding unit 257 is reset before the movement, the accurate position can be recognized during the movement of the robot 1.

If the laser sensor 40 is used,
Is not recognized, the sensor processing device 4
From 1 to the distance detection voltage signal V _L is lower than the comparison voltage V _REF , the output of the comparator 42 (position recognition signal SC)
Remains "L", and the first arm 3 and the second arm 4
The determination signal holding unit 257 remains cleared even after the completion of the movement. Therefore, the first arm 3 and the second arm 4
Of the detection circuits 25a, 25
When the determination signal holding units 257 of b, 25c, and 25d are in the reset state, the CPU 21 can read them to know that the recognition is abnormal. In the case of this application example, in order to hold that the position recognition signal SC has been input, the determination signal holding unit 2
57 must be provided.

[0049]

As is apparent from the above description, according to the invention described in claim 1, the reversible counter counts the detection signal from the sensor for detecting the angle of each arm, and from the count values of these reversible counters. The approximate position / orientation of each arm is calculated by calculating the movement amount of each arm, and the value of the reversible counter at the time when the imaging command is issued to the imaging means is stored in the register to determine the angle of each arm at the time of imaging. Since the relationship between the data obtained from the image pickup data and the coordinate system of the robot is obtained, the position and orientation of the robot can be obtained accurately and at high speed without stopping the robot.

According to the invention described in claim 2, in order to realize the invention described in claim 1, a detection circuit including a phase detection circuit, a reversible counter and a register is provided for one sensor, and each sensor is provided. A detection circuit is provided for each sensor, the arithmetic processing unit reads the count value of the reversible counter in each of the detection circuits, calculates the movement amount of the arm from the count value, and registers the count value of the reversible counter at the imaging timing. Since the relationship between the set value and the data obtained from the imaging data and the coordinate system of the robot is obtained, the position and orientation of the robot can be obtained accurately and at high speed without stopping the robot. A possible robot position / orientation recognition device can be obtained.

Further, in the invention described in claim 3, a judgment signal holding section is added to the detection circuit in the invention described in claim 2, and it is set that the recognition signal is input.
Since the fact that the recognition signal has been input is notified to the arithmetic processing device, the set value of each register can be reliably taken into the arithmetic processing device even with the recognition signal that is not synchronized with the arithmetic processing device.

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of a position / orientation recognition device for a robot that realizes a position / orientation recognition method for a robot according to the present invention.

FIG. 2 is a block diagram showing a specific configuration example of a position / orientation recognition device of the robot.

FIG. 3 is a circuit diagram showing a specific configuration example of the same phase detection circuit.

FIG. 4 is an explanatory diagram for explaining a position / orientation recognition method for the robot.

FIG. 5 is a flowchart showing an operation of the position / orientation recognition device of the robot.

FIG. 6 is a timing chart for explaining the operation of the same phase detection circuit.

FIG. 7 is a block diagram showing the same application example.

[Explanation of symbols]

 1 Robot 2 Base 3 1st Arm 4 2nd Arm 5 1st Motor 6 2nd Motor 7 3rd Motor 8 4th Motor 15 Encoder (Sensor) 16 Encoder (Sensor) 17 Encoder (Sensor) 18 Encoder (Sensor) 20 Robot Control device 21 Arithmetic processing device (CPU) 25a, 25b, 25c, 25d Detection circuit 251 Phase detection circuit 252 Reversible counter 255 Register 257 Judgment signal holding unit

Claims (3)

[Claims] 1. A rough position / orientation of each arm is calculated by counting a detection signal from a sensor that detects the angle of each arm with a reversible counter and calculating the movement amount of each arm from the count value of these reversible counters. A position recognition signal is generated at the imaging timing of the visual recognition device provided at the tip of the robot arm, and the count value of the reversible counter is stored in the register by the position recognition signal at this imaging timing.
A position / orientation recognition method for a robot, characterized in that the relationship between the data obtained from the reversible counter, the value of the register, and the imaging data and the coordinate system of the robot can be obtained. 2. A sensor that detects an arm angle and outputs a detection signal, a phase detection circuit that detects a phase of the detection signal from the sensor to obtain a detection signal, and a detection signal from the phase detection circuit is reversibly counted. A reversible counter, a detection circuit comprising a register for setting the count value of the reversible counter according to a position recognition signal generated at the image pickup timing, and the count value of the reversible counter in the detection circuit for each arm is read and the arm is moved from the count value. An arithmetic processing unit for calculating the amount and for reading the value set in the register in the detection circuit for each arm and for obtaining the relationship between the set value and the data obtained from the imaging data and the coordinate system of the robot is provided. Position / orientation recognition device for robots. 3. A sensor that detects an arm angle and outputs a detection signal, a phase detection circuit that detects a phase of the detection signal from the sensor to obtain a detection signal, and a detection signal from the phase detection circuit is reversibly counted. Reversible counter, a register for setting the count value of the reversible counter according to the recognition signal generated at the image capturing timing, a detection circuit including a determination signal holding unit that is set by the recognition signal generated at the image capturing timing, and detection for each arm The count value of the reversible counter in the circuit is read, the movement amount of the arm is calculated from the count value, and the value set in the register is read when the determination signal holding unit is in the set state. A robot characterized by comprising an arithmetic processing unit for obtaining the relationship between the obtained data and the robot coordinate system. Position / posture recognition device.