JP2005283600A - Position and attitude detection device for movable body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a position and attitude detection device for a movable body, capable of detecting a position of a reference point set in the movable body and an attitude of the movable body with high accuracy. <P>SOLUTION: This position and attitude detection device has: a target unit 11 having three targets not aligned on the same straight line, provided in a tool 30; a plurality of cameras 13 photographing the target unit from a photographing position wherein an absolute position in a robot coordinate system is grasped, and generating image data showing an image of the target unit; an image processing part 14 finding an absolute position of the reference point set in the tool in the robot coordinate system and an absolute attitude of the tool on the basis of the image data from the camera; an acceleration sensor 16 and a gyroscopic sensor 15 provided in the tool, detecting relative slide displacement and relative angle displacement of the tool; and an arithmetic part 18 finding the attitude and the position of the reference point of the tool on the basis of the absolute attitude and the absolute position of the reference point of the tool, and the relative angle displacement and the relative slide displacement of the tool. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a movable body position and posture detection device that detects the position and posture of a tip of a movable body such as a tool connected to the wrist of an industrial robot.

  There are various conventional techniques for detecting and controlling slide displacement and angular displacement of a movable body such as a tool connected to the wrist of an industrial robot. As a conventional technique, there is an attitude detection device in which a gyro is provided at the tip of an arm of an articulated robot. In this posture detection device, the relative angular displacement of the arm tip is detected by a gyro, and the posture of the tool provided at the arm tip is obtained based on the relative angular displacement.

  As another conventional technique, an ultrasonic transmitter for generating an ultrasonic pulse is provided at the tip of a robot arm, and an ultrasonic pulse is received from the ultrasonic transmitter by a receiver fixed at a predetermined position. There is a measuring device that measures the position and posture of a tool provided at the tip of an arm by determining the distance between the tip of the arm and a predetermined position based on the time from when a pulse is generated until it is received.

  As another prior art, when performing teach playback of an articulated robot, a sensor is provided at the tip of the arm of the articulated robot, and a tool provided at the tip of the arm of the articulated robot by the sensor is placed at a predetermined position. There is a method of calibrating each parameter necessary for controlling the robot by measuring the distance from the tip of the calibration jig to be arranged and correcting the change in the length of the arm due to the temperature change.

  Although the industrial robot has a high repeatability when it is repeatedly positioned at a certain position, for example, about 50 micrometers, the absolute accuracy when it is positioned at a certain predetermined position is particularly large, for example, Since it is as low as about 3 to 4 millimeters, it is almost always taught in advance by the teach playback method before using the robot. The reason for the low absolute accuracy is that each encoder provided at each joint of the robot is caused by a geometric manufacturing error of the robot, bending of the arm when holding a heavy object, expansion / contraction of the arm due to temperature, and backlash of the speed reducer. This is because the position and orientation of the reference point set in the tool provided at the tip of the robot arm based on the detected signal has an error with respect to the command value. This is because the computer generates motion data offline, downloads the motion data to the robot, performs work such as welding, or installs a sensor at the tip of the arm and uses the robot as a measurement device, This is an obstacle to the future development of robots.

  For such a problem, a method of continuously measuring the position and orientation using an inertial sensor such as a gyro sensor that detects relative angular displacement and an acceleration sensor that detects relative slide displacement, as in the prior art described above. Is a commonly known method used in inertial navigation. If the position and orientation of the robot arm tip can be obtained using this method, continuous real-time measurement is possible in time, and the robot position and orientation are measured from a position away from the robot. Compared to the above, there is an advantage that even if a blind spot is generated by the position and posture of the robot, the measurement is not affected.

  However, the gyro sensor does not actually detect the relative angular displacement directly, but detects the relative angular displacement by measuring the relative angular velocity and integrating the relative angular velocity with respect to time. Similarly, the acceleration sensor detects the relative slide displacement by measuring the relative acceleration and integrating the relative acceleration twice with respect to time. Therefore, as the displacement amount of the robot arm tip increases or as time elapses, detection errors and drifts accumulate. With such an inertial sensor, the position and orientation of the reference point of the tool provided at the robot arm tip Cannot be measured with high accuracy. For example, commercially available general-purpose optical fiber gyros and ring laser gyros have a relative acceleration accuracy of at most about 0.01 to 0.03 gravitational acceleration. In the worst case, an error of 5 to 15 centimeters is accumulated.

  In addition, as in the above-described prior art, in measuring the position and posture of the tool provided at the arm tip by ultrasonic waves, a part of the ultrasonic pulse from the ultrasonic transmitter is installed around the robot and the robot. The reflected wave is received by a receiver that is reflected by a device or the like and is fixed at a predetermined position, and the distance between the arm tip and the predetermined position cannot be obtained accurately. Further, since the robot operates at a high speed, the distance between the arm tip and the predetermined position cannot be obtained accurately due to the Doppler effect.

  Accordingly, an object of the present invention is to provide a movable body position and posture detection device capable of detecting the position of the reference point set on the movable body and the posture of the movable body with high accuracy.

The present invention according to claim 1 has three targets that are not aligned on the same straight line, and a target unit provided on the movable body,
A plurality of photographing means for photographing a target unit from an imaging position in which an absolute position in a predetermined coordinate system is grasped and generating image data representing an image of the target unit;
First detection means for obtaining an absolute position of a reference point set on the movable body in the coordinate system and an absolute posture of the movable body based on image data from the photographing means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body.

  According to the present invention, there is provided a target unit provided on the movable body, which has three targets that are not arranged on the same straight line from an imaging position in which an absolute position in a predetermined coordinate system is grasped by a plurality of imaging units. Photographed and image data representing an image of the target unit is generated. If there are a plurality of imaging means, for example, two, the target unit can be binocularly viewed. Further, by using three or more photographing means, it is possible to prevent as much as possible each target from entering the blind spot area of the photographing means due to the slide displacement and the angular displacement of the movable body.

  The first detection means obtains the absolute position of the reference point set on the movable body in the coordinate system and the absolute posture of the movable body based on the image data from the photographing means. At this time, in the target unit, since the three targets are not arranged on the same straight line, a plane including these three targets is easily obtained, a normal of the plane is obtained, and a target coordinate system based on the target is obtained. Can be requested. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point set on the movable body and the absolute posture of the movable body in the predetermined coordinate system can be easily and accurately obtained. Furthermore, relative slide displacement and relative angular displacement of the movable body are detected by the second detection means provided on the movable body.

  Based on the absolute position and absolute attitude of the reference point of the movable body from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means, the calculation means calculates the reference point of the movable body. The position and the posture of the movable body are required. Thereby, for example, although the absolute position and absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and relative angular displacement of the movable body detected by the second detection means, If the first detection means cannot continuously obtain the absolute position and absolute posture of the reference point of the movable body, or if the target unit has entered the blind spot area of each imaging means, the calculation means Until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means, the reference point of the movable body is determined based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Can be determined. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

The present invention according to claim 2 includes a target provided on the movable body,
Driving means for driving the target to be angularly displaced about a predetermined axis;
A target distance measuring means for measuring a distance from the distance measuring position to which the absolute position in a predetermined coordinate system is known;
First detection means for determining an absolute position of a reference point and an absolute posture of the movable body set on the movable body in the coordinate system based on the distance measured by the target distance measurement means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body.

  According to the present invention, the distance from the distance measuring position at which the absolute position in the predetermined coordinate system is grasped by the target distance measuring means to the target that is provided on the movable body and is angularly displaced around the predetermined axis by the driving means. Is measured. Based on the distance measured by the target distance measuring unit, the absolute position of the reference point set on the movable body in the coordinate system and the absolute posture of the movable body are obtained by the first detection unit. When the target is angularly displaced, the target distance measuring means can measure the distance from the distance measuring position at which the absolute position in the predetermined coordinate system is known to the target arranged at different angular positions. . At this time, for example, if three angular positions that are not aligned on the same straight line of the target are set, a plane including these three angular positions can be easily obtained, a normal line of the plane is obtained, and a target based on the target is obtained. A coordinate system can be obtained. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point set on the movable body and the absolute posture of the movable body in the predetermined coordinate system can be obtained easily and accurately. Furthermore, relative slide displacement and relative angular displacement of the movable body are detected by the second detection means provided on the movable body.

  Based on the absolute position and absolute attitude of the reference point of the movable body from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means, the calculation means calculates the reference point of the movable body. The position and the posture of the movable body are required. Thereby, for example, although the absolute position and absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and relative angular displacement of the movable body detected by the second detection means, If the first detection means cannot continuously obtain the absolute position and absolute posture of the reference point of the movable body, or if the target has entered the blind spot area of the target distance measurement means, the calculation means Until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means, the reference point of the movable body is determined based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Can be determined. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  The present invention according to claim 3 further includes control means for controlling slide displacement and angular displacement of the movable body based on the position of the reference point of the movable body and the attitude of the movable body from the computing means. .

  According to the present invention, the slide displacement and the angular displacement of the movable body can be controlled with high accuracy based on the position of the reference point of the highly accurate movable body and the posture of the movable body from the calculation means.

  According to the first aspect of the present invention, for example, the absolute position and the absolute posture of the reference point of the movable body determined by the first detection means are the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Although the accuracy is higher than that of the first detection means, the first detection means cannot continuously obtain the absolute position and absolute posture of the reference point of the movable body, or the target unit should enter the blind spot area of each imaging means. In such a case, the calculation means calculates the relative slide displacement and relative angular displacement of the movable body detected by the second detection means until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means. Based on this, the position and orientation of the reference point of the movable body can be obtained. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  According to the second aspect of the present invention, for example, the absolute position and the absolute posture of the reference point of the movable body determined by the first detection means are the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Although the first detection means cannot obtain the absolute position and the absolute posture of the reference point of the movable body continuously, or in the unlikely event that the target enters the blind spot area of the target distance measurement means. In such a case, the calculation means calculates the relative slide displacement and relative angular displacement of the movable body detected by the second detection means until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means. Based on this, the position and orientation of the reference point of the movable body can be obtained. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  According to the third aspect of the present invention, the slide displacement and the angular displacement of the movable body can be controlled with high accuracy based on the position of the reference point of the highly accurate movable body and the posture of the movable body from the calculation means. it can.

  FIG. 1 is a block diagram showing a configuration of a position and orientation detection apparatus 10 according to the first embodiment of the present invention. FIG. 2 is a diagram schematically showing the robot 20 and the plurality of cameras 13A, 13B, and 13C. The position and orientation detection device (hereinafter simply referred to as “position and orientation detection device”) 10 is set to a movable body such as a tool 30 connected to the wrist 24 of the robot 20 such as an industrial robot. The position of the reference point and the posture of the tool 30 are detected, and the slide displacement and angular displacement of the tool 30 are controlled.

  The robot 20 is, for example, a 6-axis vertical articulated robot. The robot 20 has a base portion 21, a lower arm 22, an upper arm 23, and a wrist 24. The base portion 21 can turn around a first axis J1 extending in the vertical direction. The lower arm 22 is connected to the base portion 21 so as to be angularly displaceable about a second axis J2 extending in the horizontal direction. The upper arm 23 is connected to the upper end portion of the lower arm 22 so that the base end portion thereof can be angularly displaced about the third axis J3 extending in the horizontal direction. The wrist 24 is angularly displaceable about a fourth axis J4 extending parallel to the axis of the upper arm 23, and is angularly displaceable about a fifth axis J5 perpendicular to the fourth axis J4. Connected. The wrist 24 has a flange 25 that can be angularly displaced about a sixth axis J6 perpendicular to the fifth axis J5. Tools 30 such as a hand, a welding torch and a sensor are connected to the flange 25 via a tool changer (not shown) provided on the flange 25.

  The position / orientation detection apparatus 10 includes a target unit 11, an LED drive unit 12, a camera 13, an image processing unit 14, a gyro sensor 15, an acceleration sensor 16, a signal processing unit 17, a calculation unit 18, and a robot control unit 19. The

  FIG. 3 is a perspective view showing the target unit 11. The target unit 11 is provided on either the tool changer or the tool 30. Further, the target unit 11 may be provided at the tip of the upper arm 23 of the robot 20 when the error of the wrist portion is small among the errors of the entire robot 20. In the present embodiment, the target unit 11 is provided on the tool 30 and can be slid and angularly displaced together with the tool 30. The target unit 11 has three targets that are not aligned on the same straight line. In the present embodiment, the target unit 11 has four targets, a first target 31, a second target 32, a third target 33, and a fourth target 34.

  The first target 31 to the fourth target 34 are formed in a spherical shape of the same shape, have translucency, and include light emitting diodes (light emitting diodes; abbreviated as LEDs) that emit light in different colors at the center of gravity. The second target 32 is connected to the first target 32 via a cylindrical first link 35. The third target 33 is connected to the first target 32 via a cylindrical second link 36. The fourth target 34 is connected to the first target 32 via a cylindrical third link 37. The first to third links 35 to 37 have exactly the same shape, the distance from the center of gravity of the first target 31 to the center of gravity of the second target 32, and the center of gravity of the first target 31 to the center of gravity of the third target 33. The distance and the distance from the first target 31 to the fourth target 34 are the same. The first link 35, the second link 36, and the third link 37 are arranged such that their axes are orthogonal to each other. The first target 31 of the target unit 11 is arranged so that the center of gravity of the first target 31 becomes a reference point set on the tool 30. Based on the LED control signal from the computing unit 18, the LED drive unit 12 supplies and does not supply drive power necessary for causing the LEDs provided in the targets 31 to 34 of the target unit 11 to emit light.

  The camera 13 as an imaging unit is arranged at an imaging position where an absolute position in a robot coordinate system, which is a predetermined coordinate system, is grasped, and images the target unit 11 from the imaging position to represent an image of the target unit 11. A plurality of image data are generated and provided. In the present embodiment, the robot coordinate system is an orthogonal coordinate composed of an x-axis and a y-axis that extend in parallel to the floor to which the base portion 21 of the robot 20 is fixed, and a z-axis that extends vertically upward. It is a system. In the present embodiment, the camera 13 is realized by a CCD camera including a solid-state imaging device (Charge Coupled Device; abbreviated as CCD). In the present embodiment, the camera 13 is composed of three cameras, a first camera 13A, a second camera 13B, and a third camera 13C. The camera 13 has different imaging positions at which absolute positions in the robot coordinate system are known. Be placed. More specifically, the imaging positions at which the cameras 13A to 13C are arranged are positions that are outside the operation area of the robot 20 and can capture the entire operation area of the robot 20 with all the cameras 13A to 13C. Hereinafter, when the first to third cameras 13 </ b> A to 13 </ b> C are not distinguished from each other, they may be simply referred to as cameras 13.

  The image processing unit 14 serving as the first detection means is realized by, for example, a microcomputer, and when a measurement command signal is given from the calculation unit 18, performs image processing based on image data from the camera 13 in the robot coordinate system. The absolute position of the reference point set in the tool 30 and the absolute posture of the tool 30 are obtained, and an absolute position / orientation detection signal including the absolute position and absolute posture of the obtained reference point of the tool 30 is given to the computing unit 18.

  The gyro sensor 15 and the acceleration sensor 16 as the second detection means are provided in either the tool 30 or the tool changer, and can be slid and angularly displaced together with the tool 30. The gyro sensor 15 detects the relative angular displacement of the tool 30 by detecting the angular velocity of the tool 30, and gives a relative angular displacement detection signal including the relative angular displacement of the tool 30 to the signal processing unit 17. The acceleration sensor 16 detects the relative slide displacement of the tool 30 by detecting the slide acceleration of the tool 30, and gives a relative slide displacement detection signal including the relative slide displacement of the tool 30 to the signal processing unit 17. In the present embodiment, the second detection means includes a gyro sensor 15 and an acceleration sensor 16. The signal processing unit 17 performs signal processing for converting the relative angular displacement detection signal and the relative slide displacement detection signal from the gyro sensor 15 and the acceleration sensor 16 into a signal that can be calculated by the calculation unit 18, and performs relative processing of the tool 30. A relative displacement detection signal including an angular displacement and a relative slide displacement is supplied to the calculation unit 18.

  The arithmetic unit 18 that is an arithmetic means is realized including an arithmetic processing device such as a central processing unit (abbreviation: CPU) and a storage device such as a random access memory, a read-only memory, and a hard disk drive, for example. The position / orientation detection apparatus 10 is comprehensively controlled. The calculation unit 18 provides the LED drive unit 12 with an LED control signal indicating supply or non-supply of drive power to the targets 31 to 34 of the target unit 11. In addition, the calculation unit 18 gives the image processing unit 14 a measurement command signal indicating that the absolute position of the reference point set in the tool 30 in the robot coordinate system and the absolute posture of the tool 30 are obtained. Further, when the command request signal is given from the robot control unit 19, the calculation unit 18 receives the absolute position of the reference point set in the tool 30 in the robot coordinate system included in the absolute position / orientation detection signal from the image processing unit 14 and the tool. The position of the reference point of the tool 30 and the tool 30 based on the absolute posture of the tool 30, the relative angular displacement of the tool 30 included in the relative displacement detection signal from the signal processing unit 17, and the relative slide displacement of the reference point of the tool 30. Is obtained, and a control signal including a corrected control command value based on the obtained position and orientation is given to the robot controller 19.

  A robot control unit (hereinafter, simply referred to as “control unit”) 19 serving as a control means provides a control signal including a control command value set in advance and a correction command value obtained by the calculation unit 18. A command request signal indicating that the request is made is given to the calculation unit 18. Further, the control unit 19 controls the slide displacement and the angular displacement of the tool 30 based on the control command value and the correction control command value included in the control signal given from the calculation unit 18. The lower arm 22, the upper arm 23, the wrist 24, and the flange 25 of the robot 20 are mounted on the robot 20 and are first to first driven by driving forces of a plurality of servo motors (not shown) controlled by the control unit 19. Angular displacement occurs around the six axes J1 to J6. Thereby, the control unit 19 controls the slide displacement and the angular displacement of the tool 30.

FIG. 4 is a diagram for explaining a method of obtaining coordinates (X, Y, Z) of a certain point P in the robot coordinate system using the camera 13. Here, in order to facilitate understanding, a method for obtaining coordinates (X, Y, Z) of a certain point P in the robot coordinate system when the first camera 13A and the second camera 13B are used will be described. When the point P photographed by the first camera 13A is photographed as a point Q1 on the imaging surface A1 of the first camera 13A, the coordinates of the point Q1 on the imaging surface A1 coordinate system of the first camera 13A are (X _C1 , Y _C1 ). When the point P photographed by the second camera 13B is photographed as a point Q2 on the imaging surface A2 of the second camera 13B, the coordinates of the point Q2 in the imaging surface A2 coordinate system of the second camera 13B are expressed as (X _C2 , Y _C2 ). At this time, the relationship between the point P and the point Q1 is expressed by the following equation (1), and the relationship between the point P and the point Q2 is expressed by the following equation (2).

In the previous equation (1), h ₁ is a parameter in the homogeneous coordinate expression, and the left matrix on the right side is a predetermined camera parameter matrix of the first camera 13A. In Equation (2), h ₂ is the parametric in homogeneous coordinates representation, the left side of the matrix on the right side, a camera parameter matrix of the second camera 13B be predetermined.

Based on the image data from the first camera 13A, the image processing unit 14 coordinates the coordinates of the point Q1 (X _C1 , Y _C1 ), and based on the image data from the second camera 13B (X _C2 , Y _C2 ), the previous equations (1) and (2) are combined, the parameters h ₁ and h ₂ are eliminated, and the coordinates (X, Y, Z) of the point P in the robot coordinate system are obtained. ), The following equation (3) is obtained.
B = AV · (3)

  The matrix B on the left side of the previous equation (3) is represented by the following equation (5), the matrix A on the left side on the right side is represented by the following equation (4), and the matrix V on the right side on the right side is represented by the following equation (6). Is done.

Therefore, the coordinates (X, Y, Z) of the point P in the robot coordinate system are obtained as in the following equation (7).
V = (A ^T · A) ⁻¹ · A ^T · B (7)
In the previous equation (7), T indicates transposition of a matrix.

The method of obtaining the coordinates (X, Y, Z) of the point P in the robot coordinate system is the case where the two cameras 13 of the first camera 13A and the second camera 13B are used, but three cameras 13 are used. Even in the case of using the above, the coordinates (X, Y, Z) of the point P in the robot coordinate system can be similarly obtained. In this way, the absolute position of the reference point set on the tool 30 is obtained by obtaining the coordinates (X _L1 , Y _L1 , Z _L1 ) of the center of gravity L1 of the first target 31 of the target unit 11 in the robot coordinate system. Can do.

In this way, the image processing unit 14 can obtain the coordinates of the center of gravity of the first to fourth targets 31 to 34 of the target unit 11 based on the image data from the camera 13. The coordinates (X _L1 , Y _L1 , Z _L1 ) of the center of gravity L1 of the first target 31 in the robot coordinate system, the coordinates (X _L2 , Y _L2 , Z _L2 ) of the center of gravity L2 of the second target 32, and the center of gravity of the third target 33. Assuming that the coordinates of L3 (X _L3 , Y _L3 , Z _L3 ) and the coordinates of the center of gravity L4 of the fourth target 34 (X _L4 , Y _L4 , Z _L4 ), the first base from the first target 31 toward the second target 32 is used. The vector Vx, the second basis vector Vy from the first target 31 to the third target 33, and the third basis vector Vz from the first target 31 to the fourth target 34 are expressed by the following equations (8), (9), and It is represented by Formula (10).

  In this way, a target coordinate system including the first to third base vectors Vx, Vy, and Vz orthogonal to each other with the center of gravity L1 of the first target 31 as the coordinate origin is obtained. As a result, the inclination of the target coordinate system with respect to the robot coordinate system is known, so that the absolute posture of the tool 30 can be obtained.

  Here, even if any one of the second to fourth targets 32 to 34 of the target unit 11 cannot be photographed by the camera 13, the coordinates of the center of gravity of the three remaining targets including the first target 31 are obtained. Thus, a plane including the center of gravity of the three remaining targets is obtained, and the normal line of this plane can be obtained. Thereby, the target coordinate system can be obtained.

  It takes some time to shoot such a target unit 11 with the camera 13, obtain a target coordinate system based on the image data, and obtain the position of the reference point set in the tool 30 and the posture of the tool 30. For example, since it takes about 0.1 second, the position and orientation of the reference point of the tool 30 are obtained by the image processing unit 14, and then the position and orientation of the reference point of the tool 30 are obtained by the image processing unit 14. Until this time, based on the relative angular displacement and the relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16, the calculation unit 18 obtains the position and orientation of the reference point of the tool 30.

  The position and orientation of the reference point of the tool 30 based on the relative angular displacement and the relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16 are compared with the position and orientation of the reference point of the tool 30 obtained by the image processing unit 14. Although the accuracy is low, the position and orientation of the reference point of the tool 30 are newly determined by the image processing unit 14 until the errors of the relative angular displacement and the relative slide displacement are accumulated. The position and orientation of 30 reference points can be determined.

  FIG. 5 is a flowchart showing a procedure for controlling the slide displacement and the angular displacement of the tool 30 by the robot controller 19. FIG. 6 is a diagram for explaining a method of controlling the slide displacement and the angular displacement of the tool 30 by the robot control unit 19. A command cycle that is a time interval at which the robot controller 19 gives a control signal to the robot 20 is t seconds. In step s0, the control procedure is started, and the process proceeds to step s1.

In step s1, the calculation unit 18 measures the actual value R _i indicating the reference point and posture of the tool 30 obtained at a certain time T _i , and the control command indicating the reference point and posture of the tool 30 at the preset time T _i . An error vector Δ _i with respect to the value H _i is obtained, and the process proceeds to step s2. Here, the subscript i is an integer.

In step s2, the arithmetic unit 18, the control command value H _{i + 1} indicating the reference point and posture of the tool 30 of the robot 20 which is set in advance at the time T _{i + 1} after t seconds from the time T _i, the error vector summation ΣΔ a _i-1, by adding the error vector delta _i obtained in step s1, seeking corrective control command value _{N i + 1} at time _{T i + 1,} the process proceeds to step s3. The sum Sigma] [Delta] _i-1 of the error vector, the measured values representing the reference point and posture of the tool 30 determined by the time T _i of t seconds before the time T _i-1 arithmetic unit 18 which is accumulated up to, This is the sum of error vectors with the control command value indicating the reference point and posture of the tool 30 of the robot 20 set in advance.

In step s3, the calculation unit 18 gives a control signal including the correction control command value N _{i + 1} obtained in step s2 to the robot control unit 19 at time T _{i + 1} and proceeds to step s4. In step s4, the subscript i is replaced with i + 1, and the process returns to step s1.

  By performing control in such a procedure, as shown in FIG. 6, the reference point of the tool 30 obtained by the calculation unit 18 and the actually measured value locus L11 indicating the posture are set as the reference point of the tool 30 set in advance. Then, it approaches the locus L10 of the control command value indicating the posture. Therefore, by giving the robot control unit 19 a control signal including a correction control command value based on the position and orientation of the reference point of the tool 30 with high accuracy from the arithmetic unit 18, the slide displacement and angular displacement of the tool 30 can be performed with high accuracy. Can be controlled.

  As described above, according to the position / orientation detection apparatus 10 of the present embodiment, the target unit 11 provided in the tool 30 is detected from the imaging position where the absolute position in the predetermined robot coordinate system is grasped by the plurality of cameras 13. Photographed and image data representing an image of the target unit 11 is generated. If there are a plurality of cameras 13, for example, two, the target unit 11 can be binocularly viewed. Further, by using three or more cameras 13, it is possible to prevent the targets 31 to 34 from entering the blind spot area of each camera 13 as much as possible due to the slide displacement and angular displacement of the tool 30 as much as possible.

  Based on the image data from the camera 13, the image processing unit 14 obtains the absolute position of the reference point set in the tool 30 in the robot coordinate system and the absolute posture of the tool 30. At this time, in the target unit 11, since any three of the four targets 31 to 34 are not aligned on the same straight line, a plane including the three targets is easily obtained, and the normal of the plane is obtained. And a target coordinate system based on the target can be obtained. Therefore, based on the robot coordinate system and the target coordinate system, the absolute position of the reference point set on the tool 30 and the absolute posture of the tool 30 in the robot coordinate system can be obtained easily and accurately. Further, a relative slide displacement and a relative angular displacement of the tool 30 are detected by the gyro sensor 15 and the acceleration sensor 16 provided in the tool 30.

  Based on the absolute position and attitude of the reference point of the tool 30 from the image processing unit 14 and the relative slide displacement and relative angular displacement of the tool 30 from the gyro sensor 15 and acceleration sensor 16 by the calculation unit 18, the tool 30. The position and orientation of the reference point are obtained. Thereby, for example, the absolute position and absolute posture of the reference point of the tool 30 obtained by the image processing unit 14 are more accurate than the relative slide displacement and the relative angular displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. However, if the image processing unit 14 cannot continuously obtain the absolute position and absolute posture of the reference point of the tool 30, or if the target unit 11 has entered the blind spot of each camera 13, The computing unit 18 is based on the relative slide displacement and relative angular displacement of the tool 30 obtained by the gyro sensor 15 and the acceleration sensor 16 until the absolute position and absolute posture of the reference point of the tool 30 are obtained by the image processing unit 14. The position and orientation of the reference point of the tool 30 can be obtained . As described above, the calculation unit 18 has extremely high accuracy based on the absolute position and absolute posture of the reference point of the tool 30 from the image processing unit 14, the gyro sensor 15 and the acceleration sensor 16, and the relative slide displacement and the relative angular displacement. The position and orientation of the reference point of the tool 30 can be obtained continuously.

  Further, according to the position / orientation detection apparatus 10 of the present embodiment, the slide displacement and the angular displacement of the tool 30 are controlled with high accuracy based on the position and orientation of the reference point of the tool 30 with high accuracy from the calculation unit 18. be able to.

  In the position / orientation detection apparatus 10 of the present embodiment, each of the targets 31 to 34 of the target unit 11 is formed in a spherical shape having the same shape and emits light in different colors by the LED. The process part 14 should just be able to distinguish each target 31-34. For example, the targets 31 to 34 may have different shapes, and, as a specific example, spheres having different sizes.

  Further, in the position / orientation detection apparatus 10 of the present embodiment, the target unit 11 is provided on the tool 30, and the first target 31 is set so that the center of gravity of the first target 31 becomes a reference point set in the tool 30. Although it is arranged, it is not limited to this. For example, two target units 11 may be used. In this case, the eight targets emit light in different colors, the first target 31 of each target unit 11 is arranged at a position that is symmetric with respect to the axis of the wrist 24, for example, and the reference point of the tool 30 is set as the tool. As the center of gravity of 30, the positional relationship between each first target 31 and the reference point of the tool 30 is grasped. By using two target units 11, the possibility that both of the two target units 11 enter the blind spot region of all the cameras 13 becomes extremely low, so that the target coordinate system can be obtained almost certainly, and therefore the tool 30. The position and orientation of the reference point can be reliably obtained.

  In addition, each camera 13 has a narrow field of view and is connected to a pan head having a function of automatically tracking the target unit 11 based on the coordinates of the center of gravity of the first target 31 obtained by the image processing unit 14. The camera 13 may be slid and angularly displaced. Thereby, the accuracy of the position and orientation of the reference point of the tool 30 obtained by the image processing unit 14 can be improved.

  FIG. 7 shows a reference point of the tool 30 when the position and orientation of the reference point of the tool 30 are controlled mainly based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. It is a graph which shows the relationship between the difference | error from the true value of the actual value of the position and attitude | position, and time. In the above-described embodiment, the absolute position and the absolute posture of the reference point of the tool 30 obtained by the image processing unit 14 are mainly used, and the relative angular displacement and the relative slide of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 are used. The position and orientation of the reference point of the tool 30 was controlled with the displacement as a sub. On the contrary, the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 are mainly used. The position and orientation of the reference point of the tool 30 may be controlled with the absolute position and absolute orientation of the reference point of the tool 30 obtained by the image processing unit 14 as a sub.

  In this case, the calculation unit 18 gives a control signal including a correction control command value based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 to the robot control unit 19 for a predetermined time. At the times U1, U2, U3,..., Un, which are intervals, a control signal including a correction control command value based on the absolute position and absolute posture of the reference point of the tool 30 obtained by the image processing unit 14 is given to the robot control unit 19. Accordingly, as shown by a broken line L12 in FIG. 7, the error included in the correction control command value is made as small as possible based on the relative angular displacement and the relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16. Can do.

  Further, for example, a plurality of predetermined standby points may be set in the operation area of the robot 20. In this case, when the robot 20 is placed at the standby point, a control signal including a correction control command value based on the absolute position and absolute position of the reference point of the tool 30 obtained by the image processing unit 14 is given to the robot control unit 19. Thus, as indicated by a solid line L13 in FIG. 7, the error included in the correction control command value based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 is made substantially zero. Can do.

  Further, by utilizing the high repeatability of the robot 20, the positions and postures of the reference points of the tool 30 at a plurality of standby points are measured in advance, and when the tool 30 is placed at the standby point, the gyro sensor 15. The position and orientation of the reference point of the tool 30 based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the acceleration sensor 16 may be replaced with numerical values measured in advance.

  Further, in the position / orientation detection apparatus 10 of the present embodiment, the target unit 11 is photographed by the camera 13, and based on the image data, the absolute position and orientation of the reference point of the tool 30 in the robot coordinate system are obtained, and the robot coordinates Although the absolute position and the absolute posture of the reference point of the tool 30 in the system are obtained, the present invention is not limited to this. For example, a distance measurement sensor may be installed in the vicinity of the standby point described above to measure the position and orientation of the reference point of the tool 30.

  Further, when the robot 20 repeatedly performs the work, the position and orientation of the reference point of the tool 30 may be measured during the dry run, and the control command value may be automatically corrected before the work. Further, when the sensor is used as a tool 30 and used as a measuring robot for measuring the shape of the workpiece, the influence on the measurement data due to the error of the reference point and posture of the tool 30 is made as small as possible after the measurement of the workpiece. be able to.

  FIG. 8 is a block diagram showing the configuration of the position and orientation detection device 50 according to the second embodiment of the present invention. FIG. 9 is a diagram schematically showing the robot 20 and the laser interferometer 53. The position and orientation detection device (hereinafter simply referred to as “position and orientation detection device”) 50 is set to a movable body such as a tool 30 connected to the wrist 24 of the robot 20 such as an industrial robot. The position of the reference point and the posture of the tool 30 are detected, and the slide displacement and angular displacement of the tool 30 are controlled. Since the robot 20 is an axially-vertical articulated robot similar to the robot 20 in the first embodiment, the same reference numerals are assigned and detailed description is omitted. In the present embodiment, the robot coordinate system is an orthogonal coordinate composed of an x-axis and a y-axis that extend in parallel to the floor to which the base portion 21 of the robot 20 is fixed, and a z-axis that extends vertically upward. It is a system.

  The position / orientation detection apparatus 50 includes a target 51, a rotation drive unit 52, a laser interferometer 53, a distance measurement processing unit 54, a gyro sensor 15, an acceleration sensor 16, a signal processing unit 17, a calculation unit 58, and a robot control unit 19. Composed.

  The target 51 is provided on any of the tip of the upper arm 23 of the robot 20, the tool changer, and the tool 30. In the present embodiment, the target unit 11 is provided on the tool 30 and can be slid and angularly displaced together with the tool 30. The target 51 is realized by, for example, a corner cube prism, and reflects the laser light incident on the target 51 from the laser interferometer 53 in a direction parallel to the incident direction of the laser light. The rotation drive unit 52 as drive means is realized by, for example, a stepping motor or an induction motor and an encoder, and rotates the target 51 around an additional axis L52 which is a predetermined axis based on a rotation control signal from the calculation unit 58. While driving, the angular position of the target 51 can be detected. At this time, the additional axis L52 is arranged so that the reference point is the rotation center of the target 51. In the present embodiment, the rotation includes an angular displacement of less than 360 degrees and a rotation of 360 degrees or more.

  A laser interferometer 53 as a target distance measuring means is disposed at a distance measurement position where an absolute position in a robot coordinate system which is a predetermined coordinate system is known, emits a laser beam, and enters the laser beam on the target 51. Then, the reflected light and the laser light having the same phase as that of the emitted laser light are caused to interfere with each other, and the number of changes in the maximum value and the minimum value of the interference intensity is counted. Measure the distance. The laser interferometer 53 has a function of tracking the target 51, can always make the laser beam incident on the target 51, and can detect the emission direction of the laser beam.

  The distance measurement processing unit 54 as the first detection means is realized by, for example, a microcomputer, and when a measurement command signal is given from the calculation unit 58, the distance measured by the laser interferometer 53 and the emission of the detected laser light. Based on the direction, the absolute position of the reference point set on the tool 30 and the absolute posture of the tool 30 in the robot coordinate system are obtained, and an absolute position and posture detection signal including the absolute position and absolute posture of the obtained reference point of the tool 30 is obtained. This is given to the calculation unit 58. More specifically, the distance measurement processing unit 54, based on the distance measured by the laser interferometer 53 and the emission direction of the detected laser light, the tool 30 in the interferometer coordinate system that is the coordinate system of the laser interferometer 53. After obtaining the absolute position and absolute posture of the reference point, the absolute position and absolute posture of the reference point of the tool 30 in the robot coordinate system are converted.

  The gyro sensor 15 and the acceleration sensor 16 that are the second detection means are the same as the gyro sensor 15 and the acceleration sensor 16 in the position and orientation detection apparatus 10 of the first embodiment, and thus are denoted by the same reference numerals and described in detail. The detailed explanation is omitted. Since the signal processing unit 17 is the same as the signal processing unit 17 in the position and orientation detection apparatus 10 of the first embodiment, the same reference numerals are assigned and detailed description is omitted.

  The computing unit 58, which is a computing means, is realized including an arithmetic processing unit such as a CPU and a storage device such as a random access memory, a read-only memory, and a hard disk drive, and comprehensively controls the position / orientation detection device 50. The calculation unit 58 provides a rotation control signal indicating rotation drive about the additional axis L52 of the target 51 to the rotation drive unit 52. In addition, the calculation unit 18 calculates the absolute position and the absolute posture of the reference point of the tool 30 set in the tool 30 in the robot coordinate system based on the distance measured by the laser interferometer 53 and the emission direction of the detected laser beam. A distance measurement processing unit 54 is provided with a measurement command signal indicating that it is to be obtained. Further, when the command request signal is given from the robot control unit 19, the calculation unit 58 receives the absolute position and absolute posture of the reference point of the tool 30 in the robot coordinate system included in the absolute position and posture detection signal from the distance measurement processing unit 54. Based on the relative angular displacement and relative slide displacement of the tool 30 included in the relative displacement detection signal from the signal processing unit 17, the position and orientation of the reference point of the tool 30 are obtained, and the correction based on the obtained position and orientation is performed. A control signal including a control command value is given to the robot controller 19.

  Since the robot control unit 19 which is a control means is the same as the robot control unit 19 in the position and orientation detection apparatus 10 of the first embodiment, the same reference numerals are assigned and detailed description is omitted.

  FIG. 10 is a diagram schematically showing an interferometer coordinate system, an additional axis coordinate system, and a robot coordinate system in the laser interferometer 53 and the robot 20. FIG. 11 is a diagram for explaining a method of detecting the position and posture of the reference point of the tool 30 in a stationary state where the tool 30 is not slid and angularly displaced. A method for detecting the position and orientation of the reference point of the tool 30 in a stationary state where the tool 30 is not slid and angularly displaced will be described below.

  The calculation unit 58 gives the rotation drive unit 52 a rotation control signal indicating that the target 51 is disposed at the predetermined first stationary angle position M1. Based on the rotation control signal, the rotation driving unit 52 rotates the target 51 around the additional axis L52 and arranges it at the first stationary angle position M1. At this time, the laser interferometer 53 measures the distance between the target 51 arranged at the first stationary angle position M1 and the distance measurement position, and detects the laser light emission direction. The distance measurement processing unit 54, based on the measured distance and the detected exit direction, coordinates in the interferometer coordinate system of the first stationary position of the interferometer coordinate system of the target 51 arranged at the first stationary angle position M1. Ask for.

  Subsequently, the calculation unit 58 outputs a rotation control signal indicating that the target 51 is disposed at the second stationary angle position M2 that is an angular position that is 90 degrees angularly displaced in the predetermined rotational direction from the first stationary angle position M1. 52. Based on the rotation control signal, the rotation drive unit 52 rotates the target 51 around the additional axis L52 and arranges it at the second stationary angle position M2. At this time, the laser interferometer 53 measures the distance between the target 51 arranged at the second stationary angle position M2 and the distance measurement position, and detects the laser light emission direction. The distance measurement processing unit 54, based on the measured distance and the detected emission direction, coordinates in the interferometer coordinate system of the second stationary position of the interferometer coordinate system of the target 51 arranged at the second stationary angle position M2. Ask for.

  Further, the calculation unit 58 sends a rotation control signal indicating that the target 51 is disposed at the third stationary angle position M3, which is an angular position displaced by 270 degrees in the rotational direction from the first stationary angle position M1, to the rotation driving unit 52. give. Based on the rotation control signal, the rotation driving unit 52 rotates the target 51 around the additional axis L52 and arranges it at the third stationary angle position M3. At this time, the laser interferometer 53 measures the distance between the target 51 arranged at the third stationary angle position M3 and the distance measurement position, and detects the laser light emission direction. The distance measurement processing unit 54 coordinates the third stationary position in the interferometer coordinate system of the interferometer coordinate system of the target 51 arranged at the third stationary angle position M3 based on the measured distance and the detected exit direction. Ask for.

  As described above, since the three stationary positions that are not aligned on the same straight line of the interferometer coordinate system at the three stationary angular positions M1 to M3 of the target 51 are obtained, the distance measurement processing unit 54 includes the three first to third. A plane including a stationary position is obtained, and a rotation center is determined from the three stationary positions and the rotation radius of the target 51, and a first basis vector Vx directed from the rotation center to the first stationary position, and a second from the rotation center. A second basis vector Vy that goes to a stationary position and a third basis vector Vz that goes from the center of rotation to a direction perpendicular to the plane are obtained. In this way, a target coordinate system constituted by the first to third base vectors Vx, Vy, Vz orthogonal to each other with the rotation center, that is, the reference point as the coordinate origin is obtained. Since the inclination of the target coordinate system with respect to the interferometer coordinate system is known, and the relationship between the robot coordinate system and the interferometer coordinate system is also known, the absolute posture of the tool 30 can be obtained.

  Here, even if the laser interferometer 53 cannot make the laser light incident on the target 51 disposed at the first to third angular positions M1 to M3, the first to third angular positions M1. It is possible to determine the first to third positions in the interferometer coordinate system of the target 51 arranged at three angular positions that are not on the same straight line other than .about.M3 and are arranged at these angular positions. Thereby, the distance measurement processing unit 54 obtains a plane including the three positions which are the first to third positions in the interferometer coordinate system, and obtains the rotation center from the three positions and the rotation radius of the target 51, The target coordinate system can be obtained by obtaining the first to third basis vectors Vx, Vy, Vz orthogonal to each other with the rotation center as the coordinate origin.

  FIG. 12 is a diagram for explaining a method of detecting the position and posture of the reference point of the tool 30 in the operation state in which the tool 30 is slid and angularly displaced. A method for detecting the position and orientation of the reference point of the tool 30 in the operation state in which the tool 30 is slid and angularly displaced will be described below.

  The calculation unit 58 gives the rotation drive unit 52 a rotation control signal indicating that the target 51 is rotated at a rotation speed sufficiently faster than the slide speed of the tool 30 of the robot 20. Based on the rotation control signal, the rotation driving unit 52 rotates the target 51 around the additional axis L52 at a rotation speed sufficiently higher than the slide speed of the tool 30. By rotating the target 51 around the additional axis L52 at a rotational speed sufficiently faster than the sliding speed of the tool 30, a plurality of positions of the target 51 at a predetermined time interval, in this embodiment, at three times. Is approximately on a plane perpendicular to the additional axis L52 at the most recent of the three times. In FIG. 12, for ease of understanding, the three angular positions D1 to D3 are shown to exist on different planes (planes perpendicular to the additional axis L52) so that they do not overlap. Actually, there is almost no gap in the direction of the arrow C.

  The laser interferometer 53 measures the distance between the rotating target 51 and the distance measurement position at the predetermined time interval described above and detects the laser light emission direction. Then, the distance measurement processing unit 54 obtains coordinates in the interferometer coordinate system of the target 51 at each measured time based on the measured distance and the detected emission direction.

In order to obtain the absolute position and the absolute posture of the reference point of the tool 30 at a certain measurement time t _i, the coordinates in the interferometer coordinates of the target 51 at three times including the measurement time t _i are necessary. In the present embodiment, the measurement time t _i , the first past time t _i−1 past the predetermined time interval from the measurement time t _i , and the predetermined time from the first past time t _i−1. The coordinates in the interferometer coordinates of the target 51 at the second past time ti _-2 that is past the interval are used.

At the measurement time t _i , the target 51 is assumed to be at the first rotation angle position D1 that is angularly displaced from the predetermined reference angle position W by the first angle θ _i in the rotation direction. In addition, at the first past time t _i−1, it is assumed that the target 51 is at the second rotation angle position D2 that is angularly displaced from the reference angle position W by the second angle θ _i−1 in the rotation direction. Furthermore, at the second past time t _i−2 , the target 51 is assumed to be at the third rotation angle position D3 that is angularly displaced from the reference angle position W by the third angle θ _i−2 in the rotation direction.

The laser interferometer 53 and the distance measurement processing unit 54 obtain the coordinates in the interferometer coordinate system of the respective rotation angle positions D1 to D3 of the target 51 at the respective times t _i−2 , t _i−1 and t _i . As described above, these three coordinates are approximately on a plane perpendicular to the additional axis L52 at the measurement time t _i . Therefore, since these three coordinates are not aligned on the same straight line, the distance measurement processing unit 54 obtains coordinates in the interferometer coordinate system of the center of a circle having a circumference including these three coordinates. The coordinates of the center of this circle are the rotation center of the target 51, that is, the coordinates of the interferometer coordinate system of the reference point of the tool 30. Since the relationship between the robot coordinate system and the interferometer coordinate system is known, the absolute position of the reference point of the tool 30 can be obtained.

The distance measuring section 54, as well as determine the plane containing the three coordinates of the foregoing, the three coordinates, and the rotational speed of the target 51, a predetermined time interval, the first to third angle theta _i through? _{I -2} , the coordinates of the reference angular position W in the interferometer coordinate system are obtained. Further, the distance measurement processing unit 54 sets the first base vector Vx from the rotation center to the reference angular position W, and is parallel to the plane from the rotation center and has an angle of 90 degrees in the rotation direction with respect to the first base vector Vx. A second basis vector Vy formed and a third basis vector Vz extending from the rotation center in a direction perpendicular to the plane are obtained. In this way, a target coordinate system constituted by the first to third base vectors Vx, Vy, Vz orthogonal to each other with the rotation center, that is, the reference point as the coordinate origin is obtained. Since the inclination of the target coordinate system with respect to the interferometer coordinate system is known, and the relationship between the robot coordinate system and the interferometer coordinate system is also known, the absolute posture of the tool 30 can be obtained.

Thus, by rotating the target 51 at a rotational speed sufficiently higher than the sliding speed of the tool 30 of the robot 20, for example, the distance between the target 51 and the rotation center is set to 100 mm, and the tool 30 of the robot 20 is 1 mm. If the target 51 is rotated by an angle of 90 degrees during the sliding displacement, the absolute posture of the tool 30 can be obtained with an accuracy of tan ⁻¹ (1/100) ≈0.5 degrees.

  As described above, according to the position / orientation detection apparatus 50 according to the present embodiment, the laser interferometer 53 is provided on the tool 30 from the distance measurement position at which the absolute position in the robot coordinate system is grasped, and the rotation drive unit 52. Thus, the distance to the target 51 rotating around the additional axis L52 is measured, and the emission direction of the laser beam is detected. The absolute position of the reference point set in the tool 30 in the robot coordinate system and the absolute posture of the tool 30 based on the distance measured by the laser interferometer 53 and the detected emission direction of the laser light by the distance measurement processing unit 54. Is required. When the target 51 is angularly displaced, the laser interferometer 53 measures the distance from the distance measurement position at which the absolute position in the robot coordinate system is known to the target 51 arranged at three different angular positions. The laser beam emission direction can be detected. At this time, for example, when three different positions of the target 51 are not aligned on the same straight line, a plane including these three positions is easily obtained, a normal line of the plane is determined, and the target 51 is used as a reference. A target coordinate system can be determined. Therefore, based on the robot coordinate system, the interferometer coordinate system, and the target coordinate system, the absolute position of the reference point set on the tool 30 and the absolute posture of the tool 30 in the robot coordinate system can be easily and accurately obtained. Further, a relative slide displacement and a relative angular displacement of the tool 30 are detected by the gyro sensor 15 and the acceleration sensor 16 provided in the tool 30.

  Based on the absolute position and absolute posture of the reference point of the tool 30 from the distance measurement processing unit 54 and the relative slide displacement and relative angular displacement of the tool 30 from the gyro sensor 15 and the acceleration sensor 16 by the calculation unit 58. The position and orientation of 30 reference points are determined. Accordingly, for example, the absolute position and absolute posture of the reference point of the tool 30 obtained by the distance measurement processing unit 54 are more accurate than the relative slide displacement and the relative angular displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. However, if the distance measurement processing unit 54 cannot continuously obtain the absolute position and absolute posture of the reference point of the tool 30, or the target 51 has entered the blind spot area of the laser interferometer 53 by any chance. In this case, the calculation unit 58 determines the relative slide displacement and relative of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 until the distance measurement processing unit 54 obtains the absolute position and absolute posture of the reference point of the tool 30. Based on the angular displacement, the position and orientation of the reference point of the tool 30 can be obtained. . As described above, based on the absolute position and attitude of the reference point of the tool 30 from the distance measurement processing unit 54, the gyro sensor 15 and the acceleration sensor 16, and the relative slide displacement and relative angular displacement of the tool 30, an extremely accurate tool. The positions and orientations of 30 reference points can be obtained continuously.

  Further, according to the robot position and orientation detection device 50 of the present embodiment, the slide displacement and the angular displacement of the tool 30 are highly accurate based on the position and orientation of the reference point of the highly accurate tool 30 from the calculation unit 58. Can be controlled.

  In the robot position and orientation detection device 50 according to the present embodiment, when the posture of the tool 30 hardly changes during the operation of the robot 20, a plurality of predetermined standby points are set in the operation region of the robot 20, When the operation of the robot 20 is stopped at the standby point, the target 51 may be rotated to obtain the posture of the tool 30.

  In the robot position and orientation detection device 50 according to the present embodiment, the target distance measuring means is the laser interferometer 51. However, the present invention is not limited to this, and for example, two cameras having CCDs may be used.

The following embodiments are possible for the present invention.
(1) A target unit having three targets that are not arranged on the same straight line and provided on the movable body;
A plurality of photographing means for photographing a target unit from an imaging position in which an absolute position in a predetermined coordinate system is grasped and generating image data representing an image of the target unit;
First detection means for obtaining an absolute position of a reference point set on the movable body in the coordinate system and an absolute posture of the movable body based on image data from the photographing means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body comprising:

  A target unit having three targets that are not aligned on the same straight line from the imaging position in which the absolute position in a predetermined coordinate system is grasped by a plurality of imaging units is imaged, and the target unit Image data representing an image is generated. If there are a plurality of imaging means, for example, two, the target unit can be binocularly viewed. Further, by using three or more photographing means, it is possible to prevent as much as possible each target from entering the blind spot area of the photographing means due to the slide displacement and the angular displacement of the movable body.

  The first detection means obtains the absolute position of the reference point set on the movable body in the coordinate system and the absolute posture of the movable body based on the image data from the photographing means. At this time, in the target unit, since the three targets are not arranged on the same straight line, a plane including these three targets is easily obtained, a normal of the plane is obtained, and a target coordinate system based on the target is obtained. Can be requested. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point set on the movable body and the absolute posture of the movable body in the predetermined coordinate system can be easily and accurately obtained. Furthermore, relative slide displacement and relative angular displacement of the movable body are detected by the second detection means provided on the movable body.

  Based on the absolute position and absolute attitude of the reference point of the movable body from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means, the calculation means calculates the reference point of the movable body. The position and the posture of the movable body are required. Thereby, for example, although the absolute position and absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and relative angular displacement of the movable body detected by the second detection means, If the first detection means cannot continuously obtain the absolute position and absolute posture of the reference point of the movable body, or if the target unit has entered the blind spot area of each imaging means, the calculation means Until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means, the reference point of the movable body is determined based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Can be determined. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  For example, although the absolute position and the absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means, the first detection If the means cannot continuously determine the absolute position and absolute posture of the reference point of the movable body, or if the target unit has entered the blind spot area of each imaging means, the computing means Until the absolute position and the absolute posture of the reference point of the movable body are obtained by the detection means, the position of the reference point of the movable body and the relative angular displacement are detected based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. You can ask for posture. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

(2) a target provided on the movable body;
Driving means for driving the target to be angularly displaced about a predetermined axis;
A target distance measuring means for measuring a distance from the distance measuring position to which the absolute position in a predetermined coordinate system is known;
First detection means for determining an absolute position of a reference point and an absolute posture of the movable body set on the movable body in the coordinate system based on the distance measured by the target distance measurement means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body comprising:

  The target distance measuring means measures the distance from the distance measuring position at which the absolute position in the predetermined coordinate system is grasped to the target provided on the movable body and angularly displaced around the predetermined axis by the driving means. Based on the distance measured by the target distance measuring unit, the absolute position of the reference point set on the movable body in the coordinate system and the absolute posture of the movable body are obtained by the first detection unit. When the target is angularly displaced, the target distance measuring means can measure the distance from the distance measuring position at which the absolute position in the predetermined coordinate system is known to the target arranged at different angular positions. . At this time, for example, if three angular positions that are not aligned on the same straight line of the target are set, a plane including these three angular positions can be easily obtained, a normal line of the plane is obtained, and a target based on the target is obtained. A coordinate system can be obtained. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point set on the movable body and the absolute posture of the movable body in the predetermined coordinate system can be obtained easily and accurately. Furthermore, relative slide displacement and relative angular displacement of the movable body are detected by the second detection means provided on the movable body.

  Based on the absolute position and absolute attitude of the reference point of the movable body from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means, the calculation means calculates the reference point of the movable body. The position and the posture of the movable body are required. Thereby, for example, although the absolute position and absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and relative angular displacement of the movable body detected by the second detection means, If the first detection means cannot continuously obtain the absolute position and absolute posture of the reference point of the movable body, or if the target has entered the blind spot area of the target distance measurement means, the calculation means Until the absolute position and absolute posture of the reference point of the movable body are obtained by the first detection means, the reference point of the movable body is determined based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. Can be determined. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  For example, although the absolute position and the absolute posture of the reference point of the movable body obtained by the first detection means are more accurate than the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means, the first detection If the means cannot continuously determine the absolute position and absolute posture of the reference point of the movable body, or if the target has entered the blind spot area of the target distance measuring means, the computing means Until the absolute position and the absolute posture of the reference point of the movable body are obtained by the detection means, the position of the reference point of the movable body and the relative angular displacement are detected based on the relative slide displacement and the relative angular displacement of the movable body detected by the second detection means. You can ask for posture. In this way, the calculation means can calculate the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the reference point of the movable body from the two detection means and the relative slide displacement and relative angular displacement of the movable body. The position and posture can be obtained continuously.

  (3) The position and orientation of the movable body further comprising control means for controlling slide displacement and angular displacement of the movable body based on the position of the reference point of the movable body and the posture of the movable body from the calculation means. Detection device.

  The slide displacement and angular displacement of the movable body can be controlled with high accuracy based on the position of the reference point of the movable body with high accuracy and the posture of the movable body from the calculation means.

1 is a block diagram illustrating a configuration of a position and orientation detection device 10 according to a first embodiment of the present invention. It is a figure which shows typically the robot 20 and several camera 13A, 13B, 13C. 3 is a perspective view showing a target unit 11. FIG. It is a figure for demonstrating the method of calculating | requiring the coordinate (X, Y, Z) in the robot coordinate system of a certain point P using the camera 13. FIG. 4 is a flowchart showing a procedure for controlling slide displacement and angular displacement of a tool 30 by a robot control unit 19. It is a figure for demonstrating the method of controlling the slide displacement and angular displacement of the tool 30 by the robot control part 19. FIG. The position and orientation of the reference point of the tool 30 when controlling the position and orientation of the reference point of the tool 30 mainly based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. It is a graph which shows the relationship between the difference | error from the true value of actual value, and time. It is a block diagram which shows the structure of the position and attitude | position detection apparatus 50 of the 2nd Embodiment of this invention. It is a figure which shows the robot 20 and the laser interferometer 53 typically. It is a figure which shows typically the interferometer coordinate system in the laser interferometer 53 and the robot 20, an additional axis coordinate system, and a robot coordinate system. It is a figure for demonstrating the method to detect the position and attitude | position of the reference | standard point of the tool 30 in the stationary state where the tool 30 is not a slide displacement and an angular displacement. It is a figure for demonstrating the method to detect the position and attitude | position of the reference | standard point of the tool 30 in the operation state in which the tool 30 is slid and angularly displaced.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10, 50 Position and orientation detection apparatus 11 Target unit 31, 32, 33, 34; 51 Target 13 Camera 14 Image processing part 15 Gyro sensor 16 Acceleration sensor 18, 58 Calculation part 19 Robot control part 30 Tool 52 Rotation drive part 53 Laser interference Total 54 Distance processing section

Claims (3)

A target unit that has three targets that are not aligned on the same straight line and is provided on the movable body;
A plurality of photographing means for photographing a target unit from an imaging position in which an absolute position in a predetermined coordinate system is grasped and generating image data representing an image of the target unit;
First detection means for obtaining an absolute position of a reference point set on the movable body in the coordinate system and an absolute posture of the movable body based on image data from the photographing means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body comprising: A target provided on the movable body;
Driving means for driving the target to be angularly displaced about a predetermined axis;
A target distance measuring means for measuring a distance from the distance measuring position to which the absolute position in a predetermined coordinate system is known;
First detection means for determining an absolute position of a reference point and an absolute posture of the movable body set on the movable body in the coordinate system based on the distance measured by the target distance measurement means;
A second detection means provided on the movable body for detecting relative slide displacement and relative angular displacement of the movable body;
Calculation means for obtaining the position of the reference point and the posture of the movable body based on the absolute position and absolute posture of the reference point from the first detection means and the relative slide displacement and relative angular displacement of the movable body from the second detection means. A position and orientation detection device for a movable body comprising:   3. The movable body according to claim 1, further comprising control means for controlling slide displacement and angular displacement of the movable body based on the position of the reference point of the movable body and the posture of the movable body from the calculating means. Position and orientation detection device.

Images