JP2011038801A - Position detection device, position detection method, and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To calculate sound speed of a portion to be detected, taking into account the environment around the portion to be detected, measure as accurate as possible sonic speed around a portion to be position-detected, and thereby enhance the detection accuracy of the position. <P>SOLUTION: An ultrasonic-wave transmitting device is arranged at a tip portion 104 of a robot hand 101 and polyhedrons 131 to 133 which can contain the tip portion 104 are arranged, and ultrasonic wave transmitting-and-receiving devices 111 to 126 are arranged at the top points thereof. Upon position detection of the tip portion 104, a top point that is close to the tip portion 104 is selected as a specific point, and sonic speeds from other top points to the specific point are calculated. Based on these sonic speeds, sonic speed along a route from the tip portion 104 to the specific point is calculated. Based on the sonic speed value and propagation time of ultrasonic waves, between the tip portion 104 and the specific point, the distance from the specific point to the tip portion 104 is calculated. Such a distance is determined about a plurality of specific points, to specify the position of the tip portion 104. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a technique for detecting the position or the like of a robot arm tip with ultrasonic waves, and to a technique for reducing detection errors due to the influence of sound speed.

  A technique for detecting the three-dimensional position of an object using ultrasonic waves is known (see, for example, Patent Document 1). In this technique, for example, an ultrasonic generator is arranged in a portion to be detected, and ultrasonic signals generated therefrom are received at three different locations. At this time, the distances r1 to r3 from these three locations to the detection target are determined by measuring the time that the ultrasonic waves travel through the air at the three receiving sides. Then, spheres having radii r1, r2, and r3 centered on these three locations are drawn, and a point where the surfaces of these spheres intersect is specified as a position to be detected.

JP-A-6-102350

  The position specifying technique using ultrasonic waves described above is affected by the speed of sound waves. The speed of sound is affected by the temperature, pressure, humidity, density (atmospheric pressure), and flow (wind) in the air. Therefore, in an environment where the parameter affecting the sound speed is not constant, the accuracy of detecting the position using the sound wave is lowered.

  For example, a technique for detecting the position of the tip of the robot arm is required at a manufacturing technology site using the robot arm. In such an environment, changes in temperature, humidity, air flow, etc. may vary depending on location and time. In this case, in the position detection technique using sound waves, an error occurs in the detected position data due to the above-described change in sound speed.

  In such a background, an object of the present invention is to provide a technique for measuring the speed of sound in the vicinity of a position detection target as accurately as possible and increasing the position detection accuracy using the measurement.

  According to a first aspect of the present invention, there is provided an ultrasonic transmission unit disposed in the detected unit, a plurality of ultrasonic transmission / reception units disposed around the detected unit, and the plurality of ultrasonic transmission / reception units. The sound speed calculating means for calculating the sound speed and calculating the sound speed in the detected part based on the sound speed between the plurality of ultrasonic transmitting / receiving means, and the plurality of ultrasonic transmission means arranged in the detected part. Based on the propagation time of the ultrasonic wave to the specific ultrasonic transmission / reception means included in the ultrasonic transmission / reception means and the sound velocity in the detection target, the specific transmission from the ultrasonic transmission means arranged in the detection target A distance calculation unit that calculates a distance to the ultrasonic transmission / reception unit, wherein there are a plurality of line segments connecting the ultrasonic transmission / reception devices, and the plurality of line segments are used as sound speeds between the ultrasonic transmission / reception units. The speed of sound at In the process of calculating the sound speed in the detected part, the sound speed in the line segment relatively close to the detected part among the sound speeds in the plurality of line segments has a relatively large effect on the sound speed in the detected part. The position detection device is characterized in that a process is performed in which the sound speed in a line segment relatively far from the detected part has a relatively small effect on the sound speed in the detected part.

  In the first aspect of the present invention, first, the speed of sound between a plurality of ultrasonic transmission / reception means arranged around the detected portion is calculated on a plurality of paths. Then, based on a state in which the influence of the sound speed on the plurality of paths is weighted according to the distance between the path and the detected part, the sound speed in the detected part is calculated. Furthermore, the ultrasonic wave transmitted from the detected part is received by a specific ultrasonic transmission / reception means, and the distance r between them is calculated from the sound speed and its propagation time in the detected part. Since the detected part is located on the spherical surface of a sphere having a radius r centering on the specific ultrasonic wave transmitting / receiving means, the position of the detected part is specified based on this.

  The invention according to claim 2 is characterized in that, in the invention according to claim 1, the speed of sound in the line segment relatively close to the detected part is the speed of sound in the line segment closest to the detected part. To do.

  The invention according to claim 3 is the invention according to claim 1 or 2, wherein the plurality of line segments form a triangular pyramid, and the detected portion is included in the triangular pyramid. To do.

According to a fourth aspect of the present invention, in the invention according to any one of the first to third aspects, the calculation of the sound speed in the detected part in the sound speed calculating means is performed by the plurality of line segments from the detected part. L _i (i is a natural number not including 0), L = L ₁ + L ₂ + L ₃ +..., And the speed of sound in each of the plurality of line segments is vi (i is (Natural number not including 0) is performed by the following formula 1.

  According to a fifth aspect of the present invention, in the invention according to any one of the first to fourth aspects, the detected portion is a tip portion of a robot hand.

  According to a sixth aspect of the present invention, the speed of sound between a plurality of ultrasonic transmission / reception means arranged around the detected portion is calculated, and based on the speed of sound between the plurality of ultrasonic transmission / reception means. A step of calculating a sound velocity in the detection unit, an ultrasonic wave propagation time from an ultrasonic transmission unit disposed in the detection unit to a specific ultrasonic transmission / reception unit included in the plurality of ultrasonic transmission / reception units, and the target And calculating the distance from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means based on the sound velocity in the detection part, and connecting the ultrasonic transmission / reception apparatuses There are a plurality of line segments, and sound speeds in the plurality of line segments are calculated as sound speeds between the plurality of ultrasonic transmission / reception means, and in the step of calculating the sound speeds, The speed of sound in the line segment relatively close to the detected part has a relatively large effect on the speed of sound in the detected part, and the speed of sound in the line segment relatively far from the detected part is relative to the sound speed in the detected part. The position detection method is characterized in that processing that has a small effect is performed.

  According to the sixth aspect of the present invention, the computer calculates the sound speed between the plurality of ultrasonic transmission / reception means arranged around the detected portion, and based on the sound speed between the plurality of ultrasonic transmission / reception means. , Processing for calculating the sound velocity in the detected part, and propagation time of ultrasonic waves from the ultrasonic transmitting means arranged in the detected part to specific ultrasonic transmitting / receiving means included in the plurality of ultrasonic transmitting / receiving means, A process of calculating a distance from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means based on a sound velocity in the detected part, and the ultrasonic transmission / reception apparatus There are a plurality of line segments connecting the plurality of line segments. In the process of calculating the sound speed in the plurality of line segments as the sound speed between the plurality of ultrasonic transmission / reception units, Among the sound velocities in the sound, the sound speed in a line segment relatively close to the detected part has a relatively large effect on the sound speed in the detected part, and the sound speed in a line segment relatively far from the detected part is detected. A program is characterized in that a process that has a relatively small effect on the speed of sound in a section is performed.

  According to the first aspect of the present invention, in calculating the sound speed of the detected part after taking in the environment information around the detected part, the degree of influence according to the distance from the detected part is considered. Since the environmental state is taken in, the detection accuracy of the sound velocity near the detected portion is increased. For this reason, the position detection apparatus which can raise the detection accuracy of the position using the sound speed near to-be-detected part is provided.

  According to the second aspect of the present invention, it is possible to detect the position where the environmental information in the portion close to the detected portion is taken in more accurately.

  According to the third aspect of the invention, since the sound speed is calculated in the detected part inside the triangular pyramid based on the sound speed in the hypotenuse of the triangular pyramid, the calculation accuracy of the sound speed in the detected part is further improved. be able to.

  According to the fourth aspect of the present invention, the speed of sound in the detected portion can be obtained by a calculation formula that reflects the difference in the degree of influence depending on the distance.

  According to the invention described in claim 5, it is possible to improve the position detection accuracy of the tip portion of the robot hand.

  According to the sixth aspect of the present invention, in calculating the sound speed of the detected part after taking in the information on the environment around the detected part, the degree of influence according to the distance from the detected part is considered. Since the environmental state is taken in, the detection accuracy of the sound velocity near the detected portion is increased. For this reason, the position detection method which can improve the detection accuracy of the position using the sound speed near to-be-detected part is provided.

  According to the seventh aspect of the present invention, in calculating the sound speed of the detected part after taking in the environment information around the detected part, the degree of influence according to the distance from the detected part is considered. Since the environmental state is taken in, the detection accuracy of the sound velocity near the detected portion is increased. For this reason, the program which can raise the detection accuracy of the position using the sound speed near to-be-detected part is provided.

It is a conceptual diagram of embodiment. It is a block diagram of an embodiment. It is a flowchart which shows an example of the procedure which performs a position detection. It is a principle figure which shows the principle which calculates the sound speed on the line which connects the detection position P and the vertex A1. It is a flowchart which shows an example of control of a robot hand.

(Overview)
In this embodiment, as shown in FIG. 1, an ultrasonic transmission device is disposed at the tip portion 104 of the robot hand 101, and polyhedrons 131 to 133 that can further include the tip portion 104 are set, and the apexes are set at the apexes thereof. The ultrasonic transmission / reception devices 111 to 126 are arranged. In detecting the position of the tip portion 104, a vertex close to the tip portion 104 is selected as a specific point, and the sound speed of a path from another vertex toward the specific point is calculated. Based on this sound velocity, the sound velocity in the path from the tip portion 104 to the specific point is calculated, and based on the value of this sound velocity and the propagation time of the ultrasonic wave between the tip portion 104 and the specific point, the tip portion 104 is calculated. The distance from the specific point is calculated. By obtaining this distance for a plurality of specific points, the position of the tip portion 104 is specified.

(Overall configuration)
FIG. 1 is a conceptual diagram of the embodiment. A robot hand system 100 is shown in FIG. The robot hand system 100 includes a robot hand 101. The robot hand 101 is provided with a plurality of types of screwdrivers at the tip thereof, and has a function of turning a screw of an assembled part. The function of the robot hand 101 is not particularly limited, and it is possible to select a function having an existing function such as parts conveyance, assembly, painting, and welding.

  The position of the robot hand 101 is controlled by the robot controller 102. The robot control apparatus 102 receives position information in the three-dimensional space of the tip portion 104 of the robot hand 101 from the position detection apparatus 103, and performs position control of the robot hand 101 based on the position information. Further, the robot control device 102 outputs position information on the control of the tip portion 104 of the robot hand to the position detection device 103.

  An ultrasonic transmission device is disposed at the distal end portion 104 of the robot hand 101. The operation of this ultrasonic transmission device is controlled by the position detection device 103. The position detection device 103 controls the operations of the 16 ultrasonic transmission / reception devices 111 to 126 and processes output signals from them. The frequency of the ultrasonic wave used here is not particularly limited, but, for example, a frequency of several tens of kHz is used.

  The ultrasonic transmission / reception devices 111 to 126 constitute a rectangular parallelepiped detection space in which three regular hexahedrons 131 to 133 are connected. The regular hexahedrons 131 to 133 function as a unit detection space. Ultrasonic transmission / reception devices 111 to 126 are arranged at the respective apexes of the regular hexahedrons 131 to 133.

  That is, the ultrasonic transmission / reception devices 111, 112, 113, 114, 115, 116, 117 and 118 are arranged at eight vertices of the regular hexahedron 131. The ultrasonic transmission / reception devices 115, 116, 117, 118, 119, 120, 121 and 122 are arranged at eight vertices of the regular hexahedron 132. The ultrasonic transmission / reception devices 119, 120, 121, 122, 123, 124, 125, and 126 are arranged at eight vertices of the regular hexahedron 133.

  In the three-dimensional space constituted by these three regular hexahedrons 131 to 133, the three-dimensional position of the tip 104 of the robot hand 101 is detected. The configuration of the control system for this detection and the detection method will be described later.

(Control system configuration)
FIG. 2 is a block diagram showing an outline of a control system of the robot hand system of FIG. FIG. 2 shows the position detection device 103 of FIG. The position detection device 103 has a computer function and includes a CPU, a memory, and an interface. Further, the position detection device 103 stores a program for executing an operation described later in the memory.

  The position detection device 103 includes an ultrasonic transmission / reception device control unit 201, a sound speed calculation unit 202, a distance calculation unit 203, a position calculation unit 204, and a buffer memory unit 205. The ultrasonic transmission / reception apparatus control unit 201 controls the operations of the ultrasonic transmission / reception apparatuses 111 to 126 when detecting the speed of sound inside the regular hexahedrons 131 to 133.

  Specifically, when the detection of the sound velocity is performed inside the regular hexahedron 131, the control of the ultrasonic transmission / reception devices 111, 112, 113, 114, 115, 116, 117 and 118 is controlled by the ultrasonic transmission / reception device control. This is performed in the unit 201. When the detection of the speed of sound is performed inside the regular hexahedron 132, the ultrasonic transmission / reception devices 115, 116, 117, 118, 119, 120, 121 and 122 are controlled by the ultrasonic transmission / reception device control unit 201. When the detection of the speed of sound is performed inside the regular hexahedron 132, the ultrasonic transmission / reception devices 119, 120, 121, 122, 123, 124, 125, and 126 are controlled by the ultrasonic transmission / reception device control unit 201. In addition, the ultrasonic transmission / reception device control unit 201 controls the operation of the ultrasonic transmission device 206 disposed at the distal end portion 104 of the robot hand 101.

  The sound speed calculation unit 202 calculates the sound speed in the regular hexahedrons 131 to 133 and performs calculations necessary for this calculation. The contents of the processing performed in the sound speed calculation unit 202 will be described later. The distance calculation unit 203, based on the calculation result in the sound speed calculation unit 202, a detected position (tip portion 104) and a specific ultrasonic transmission / reception device (a plurality selected from a plurality of vertices in this example) as a measurement reference. The distance between is calculated.

  The position calculation unit 204 calculates three-dimensional position data (three-dimensional position coordinates) of the detected position based on the calculation result in the distance calculation unit 203. This coordinate data is sent to the robot controller 102 and used for position control of the robot hand. The buffer memory unit 205 functions as a working area for temporarily storing data in calculations performed in the position detection device 103.

(Process for detecting position)
Hereinafter, an example of a detailed procedure of processing for detecting the position of the tip portion 104 of the robot hand 101 will be described. FIG. 3 is a flowchart showing a procedure of processing performed in the position detection device 103 of FIG. A program for executing the processing shown in FIG. 3 is stored in a memory area in the position detection apparatus 103, read out to an appropriate memory area, and executed by the CPU in the position detection apparatus 103. This operation program may be stored in an external storage medium and provided from there.

  When the process for calculating the position of the tip portion 104 is started (step S301), a unit measurement space including the detection position (tip portion 104) is first selected (step S302). This process is performed in the sound speed calculation unit 202. The unit measurement spaces that can be selected here are the three regular hexahedrons 133, 132, and 133 shown in FIG. In step S302, with reference to the positional information of the control tip portion 104 obtained from the robot controller 102, it is searched which regular hexahedron the detection position (tip portion 104) is included. The position data of the tip portion 104 acquired from the robot control device 102 is based on data for performing control to position the tip portion 104 at the position, and includes an error from the actual position.

  Hereinafter, the processing after step S303 will be described based on a specific example. FIG. 4 is a conceptual diagram for explaining an example of processing. FIG. 4 shows a detection position P corresponding to the tip portion 104 of the robot hand 101. 4 shows a regular hexahedron 400 corresponding to any one of the regular hexahedrons 131 to 133 in FIG.

  When the regular hexahedron 400 of FIG. 4 is the regular hexahedron 132 of FIG. 1, for example, the ultrasonic transmission / reception device 116 of FIG. 1 is arranged at the vertex A1, the ultrasonic transmission / reception device 118 is arranged at the vertex A2, and The ultrasonic transmission / reception device 117 is disposed, the ultrasonic transmission / reception device 115 is disposed at the vertex A4, the ultrasonic transmission / reception device 120 is disposed at the vertex A5, the ultrasonic transmission / reception device 122 is disposed at the vertex A6, and the ultrasonic transmission / reception is performed at the vertex A7. The apparatus 121 is disposed, and the ultrasonic transmitting / receiving apparatus 119 is disposed at the vertex A8.

  After step S302, the top three vertices close to the detection position are selected as specific points (step S303). In the example of FIG. 4, vertices A1, A2, and A3 are selected as the top three specific points close to the detection position P. This process is performed in the sound speed calculation unit 202.

  In the process of step S303, when an obstacle such as a work or an arm portion of the robot hand 101 is present between the detection position P and the specific point, the vertex next to the detection position P is selected as the specific point. This is in order to suppress detection errors caused by the propagation of ultrasonic waves being disturbed by obstacles.

After selecting 3 specific points, select the line segment that is close to the detection position from the top 3 lines from the line segment that connects one of them to the other vertex (each vertex of the regular hexahedron selected in step S302). (Step S304). In the example of FIG. 4, line segments C ₁ , C ₂ , and C ₃ are selected for the specific point A1. This process is performed in the sound speed calculation unit 202, and the result of the process is stored in the buffer memory unit 205. This process is performed at all three specific points (A1, A2, A3).

At this time, an obstacle (work or arm part of the robot hand) is positioned between the line segments C ₁ , C ₂ , and C ₃ , and a line segment that does not block the line of sight is selected. When there is a line segment whose line of sight is blocked, the selection of the specific point is performed again, and the line segment close to the detection position P is selected next.

Next, at each specific point, the sound speed in the direction toward the specific point in the three line segments selected in step S304 is calculated (step S305). Using the example of FIG. 4, the speed of sound towards the A1 along the line segment _{C 1,} sound velocity towards the A1 in the line C2, speed of sound towards the A1 in the line C3 is calculated. For example, the sound velocity in the line segment C ₁ is position between the point A1 and the point A3 is known to operate the ultrasonic transmitting and receiving device for a dual point, from the sound wave propagation time in the direction towards the point A1 from the point A3 Calculated. Operation control of the ultrasonic transmission / reception apparatus at this time is performed by the ultrasonic transmission / reception apparatus control unit 201. The processing result is stored in the buffer memory unit 205. Similar processing is performed for the line segments C ₂ and C ₃ . These processes are also performed at specific points A2 and A3.

  Next, the sound speed calculation unit 202 calculates the sound speed in the direction from the detection position toward the three specific points (including the sound speed at the detection position) (step S306). In the example of FIG. 4, the sound velocity in the direction from the detection position P toward the vertex A1 is calculated, the sound velocity in the direction from the detection position P toward the vertex A2 is calculated, and the sound velocity in the direction from the detection position P toward the vertex A3 is calculated. calculate.

At this time, the sound speed in the direction connecting the detection position P and the specific point cannot be directly obtained. This is because the position of the detection position P is an inaccurate position obtained from the control data. Therefore, in the example of FIG. 4, for example, the detection position P in the direction of sound velocity toward the specific point A1 of the line connecting between the specific point A1, the speed of sound towards the A1 on the line segment C _1, on the line C ₂ of speed of sound towards the A1, is calculated as an approximate value from the speed of sound towards the A1 on the line segment C _3.

In this calculation, with respect to speed of sound towards the A1 in the direction connecting the detection position P and the specific point A1, the influence of the speed of sound towards the A1 in the nearest line segment C ₂ at the detection position P (relation) is largest, effect of sound velocity then towards the A1 along the line segment C ₃ closer to the detection position P is in the influence of the speed of sound towards the A1 then the line segment C ₁ is close to the detection position P is estimated to be small, each line segment Formula for weighting the speed of sound toward A1 is established. Thereby, although it is an approximate expression, the sound velocity toward A1 in the direction connecting the detection position P and the specific point A1 is calculated. Similarly, the calculation of the sound velocity in the direction from the detection position P toward the vertex A2 and the calculation of the sound velocity in the direction from the detection position P toward the vertex A3 are also performed.

Hereinafter, a specific calculation formula will be described by taking the case of FIG. 4 as an example. First, in FIG. 4, L ₁ is a perpendicular drawn from the detection position P to the line segment C ₁ . L ₂ is a perpendicular drawn from the detection position P to the line segment C ₂ . L ₃ is a perpendicular drawn from the detection position P to the line segment C ₃ . In the example of FIG. 4, L ₂ <L ₃ <L ₁ is established. In this condition, the speed of sound towards the A1 along the line segment _{C 1} v1, the speed of sound towards the A1 along the line segment _{C 2} v2, When the speed of sound towards the A1 along the line segment _{C 3} v3, toward the apex A1 from the detection position P The sound speed V in the direction is calculated from the following formula 2.

In Equation 2, as the coefficient of v1, v2, v3, it adopts a value corresponding to the distance to the detection position P of the line segment _C 1 -C _3, thereby weighting the impact on v1, v2, v3 of the V It is shaped.

This is based on the following concept. First, the vector of acoustic velocity v2 over the line segment C _2, line C ₂ is closest to the line connecting the P and A1 (angle is the smallest, even the closest position) so close to the vector of the most V . The vector of acoustic velocity v3 on the line segment C ₃ Next, the line segment C ₃ is (small angle is a second position even closer to the second) next closest to the line connecting the P and A1, so 2 It is close to the V vector. Furthermore vector of acoustic velocity v1 on line C _1, line C ₁ is (small third angle formed, located also near the third) 3 close to th line connecting the P and A1, so 3 It is close to the V vector. Therefore, V can be approximately expressed by a linear combination formula of v1, v2, and v3 using a weighting coefficient corresponding to the distance. This is Equation 2 above.

  In step S306, when the sound speed on the line from the detection position to each specific point is calculated, a control signal is output from the ultrasonic transmission / reception apparatus control unit 201, and is transmitted to the tip 104 (detection position) of the robot hand 101 (see FIG. 1). An ultrasonic signal is output from the arranged ultrasonic transmission device 206 and received at three specific points. Then, from the difference between the sound speed calculated in step S305, the transmission time from the ultrasonic transmission device 206, and the reception time at each specific point (that is, the time required for the sound wave to travel), the detected position and the three specific points The distance between them is calculated (step S308). This process is performed in the distance calculation unit 203.

  In the case of FIG. 4, the distance r1 between the detection position P and the specific point A1, the distance r2 between the detection position P and the specific point A2, and the distance r3 between the detection position P and the specific point A3 are as follows. Calculated.

  Next, a sphere having a radius from the three specific points to the detection position is drawn, and the intersection of the surfaces is obtained. Since there are two intersections, the position close to the planned detection position (the position of the tip 104 of the robot hand 101) (or the one in the selected regular hexahedron) is the position of the tip 104 of the robot hand 101. Obtained as data (step S309). This process is performed in the position calculation unit 204. The principle of specifying this position is the same as that of GPS (Global Positioning System).

(Robot hand position control)
FIG. 5 is a flowchart showing a flow of position control of the robot hand 101 performed in the robot control apparatus 102. In FIG. 5, the world coordinate system is a coordinate system (for example, a three-dimensional orthogonal coordinate system or a polar coordinate system) fixed to the environment (robot hand system 100) in which the robot hand 101 is installed. The robot coordinate system is a coordinate system fixed on the robot side used when moving each joint axis.

  In the position control, first, a target position in the world coordinate system is set (S501). This target position is output to step S502 and given to the step of calculating the deviation amount in step S507. In step S502, the amount of deviation between the target position calculated in step 507 and the measured position is given, and the amount of deviation and the data of the target position given in step S501 are given to step S503.

  In step S503, coordinate data taking into account the deviation from the target position is given, and this is converted into a robot coordinate system. Then, based on the converted coordinate data of the robot coordinate system, an operation command is issued to the motor that drives each joint (step S504), and the position control of the robot hand 101 (see FIG. 1) is performed (step S505). ).

  At this time, position information in the world coordinate system of the tip portion 104 of the robot hand 101 is detected by the processing of FIG. 3 in the position detection device 103 (step S506), and based on this, the target position and the actual position of the tip portion 104 are detected. A difference (amount of deviation) from the position is calculated (step S507). This deviation amount is given to step S502 and used for position control of the robot hand 101.

  By this process, position control is performed while correcting the deviation of the position of the tip portion 104 of the robot hand 101 from the target position.

(Superiority)
According to the embodiment described above, the position of the tip portion 104 of the robot hand 101 is detected using ultrasonic waves. At this time, the sound velocity in the propagation path of the sound wave from the tip portion 104 to the specific receiving device is changed to the sound velocity between the ultrasonic transmitting / receiving devices arranged at each vertex of the set unit detection space (in this example, regular hexahedron). Calculated based on

In this example, the sound speeds of the three reference paths close to the path from the tip portion 104 to the specific receiving device are used, and the three reference paths ( The influence of C ₁ , C ₂ , C ₃ ) in FIG. 4 on the path is calculated. Therefore, although approximate, a sound speed close to an actual value affected by parameters such as temperature, humidity, density, and air flow in a path from the detection position P (tip portion 104) to a specific receiving device is obtained. be able to. As a result, the position of the tip portion 104 is detected in a state in which the influence of temporal change and place change of these parameters is reduced. That is, it is possible to suppress the detection error of the position of the tip portion 104 due to the difference in sound speed due to the influence of temperature and wind.

  In the method shown in the embodiment, since the sound speeds of the upper three reference paths close to the tip portion 104 are used, a specific receiving device is detected from the detection position P in a form that more accurately reflects the environmental state of the detection position P. The value of the sound velocity in the direction toward (the sound velocity at the detection position) can be calculated. In addition, since the angle formed between the direction of each reference path and the direction of the path from the detection position to the specific receiving apparatus is 45 degrees or less, even when there is a difference in sound speed due to the difference in direction, the detection position can be accurately detected. The value of the sound speed in the direction toward the specific receiving device (the sound speed at the detection position) can be calculated.

  Further, as shown by regular hexahedrons 131 to 133, a plurality of unit detection spaces are prepared, so that more detailed changes in sound speed can be captured. Thereby, even in an environment where the influence of temperature change or air flow depending on the location is large, the detection accuracy of the position of the tip portion 104 can be kept high.

In the example of FIG. 4, when calculating the speed of sound from the detection position P to the receiving apparatus of the position of the A1, the largest estimate the influence of the speed of sound towards the A1 at the nearest reference path C ₂ to this route, then the second given by weighting according the effect of sound speed towards the A1 in the near reference path C ₃ to difference in the distance, further, according to the influence of the speed of sound towards the A1 in the reference path C ₁ third closest to the difference in the distance The weight is given (see Equation 2). As a result, the sound velocity in the direction of the apex A1 at the detection position P that more accurately reflects the atmosphere (temperature, wind, or the like) near the detection position P can be obtained.

Further, when calculating the sound velocity V from the detection position P to the receiving device at the position A1, a triangular pyramid including the detection position P inside the reference path C ₁ , C ₂ , C ₃ with A1 as a vertex. Since each side is selected, the accuracy of the sound speed V obtained from the sound speed in each reference path can be further increased.

That is, when calculating the sound speed V from the detection position P to the receiving device at the position of A1, the sound speed toward the A1 in the reference paths C ₁ , C ₂ , and C ₃ is close to the direction of the sound speed V. An error from the actual value of the sound speed V calculated from the sound speeds (v1, v2, v3) in the reference paths C ₁ , C ₂ , and C ₃ can be suppressed. In particular, this method can take into account the effect of the direction of air flow (wind effect).

  Further, by selecting A1, A2, and A3 from the top three closest to the detection position P as specific points, an increase in error due to an increase in the propagation distance of the sound wave can be suppressed.

  Further, three specific points A1, A2, and A3 are selected, and the detection position P is obtained from the intersection position of three spheres centered on them, thereby detecting a three-dimensional accurate position.

(Other)
In the above illustration, an example in which a regular hexahedron is adopted as the unit detection space is shown, but a shape other than a regular hexahedron can be used as long as it is a polyhedron. The medium for transmitting sound waves is not limited to air, and may be an inert gas or the like as long as it is a gas. In addition, the present invention can be applied in an atmosphere where fog or smoke exists. The position may be detected not by the tip of the robot hand but by an indirect portion of the robot hand. In addition to the robot hand, the present invention can also be used for the purpose of detecting the three-dimensional position of a workpiece or jig.

  Although FIG. 1 shows a system with one robot hand, the number of robot hands may be plural. Further, the number of unit detection spaces (regular hexahedrons 131 to 133) may be further increased. The number of unit detection spaces (regular hexahedrons 131 to 133) may be one.

  In the example illustrated in FIG. 4, the example in which the position of the detection position P is specified by calculating the distance from the three points of the vertices A1, A2, and A3 to the detection position P has been described. ) May be four or more. Alternatively, only the vertex A1, only the vertices A1 and A2, may be selected, the distance from there to the detection position P may be calculated, and the position of the detection position P may be specified based on the result.

  When four or more reference points for calculating the distance to the detection position are selected, the position calculation accuracy can be further increased. If there are two reference points for calculating the distance to the detection position, the detection position exists on a circle where the spheres centering around each of these two points intersect. In this case, the position detection system can be enhanced by using the calculation data indicating that the detected position is on the circle for correcting the predicted position obtained from other means (for example, control data of the robot arm). .

  When the reference point for calculating the distance to the detection position is one point, the detection position exists on a spherical surface centered on this point. In this case, the position detection system can be enhanced by using the calculation data indicating that the detected position is on the spherical surface in correcting the predicted position obtained from other means (for example, control data of the robot arm). .

In Equation 2, _three line segments C ₁ , C ₂ , and C ₃ are selected, but this number may be one or two, or four or more. The calculation accuracy of the sound speed at the detection position P increases as the number of line segments (for example, C ₁ , C _2, etc.) connecting the ultrasonic transmission / reception devices to be used increases. However, the calculation speed is increased and the calculation speed to the calculation element is increased. In reducing the load, it is advantageous to reduce the number.

  The present invention can be used in a technique for detecting a position using ultrasonic waves. Moreover, it can utilize for the technique which detects the specific position of the robot hand using an ultrasonic wave.

  DESCRIPTION OF SYMBOLS 100 ... Robot hand system, 101 ... Robot hand, 102 ... Robot control apparatus, 103 ... Position detection apparatus, 104 ... Tip part of robot hand, 111-126 ... Ultrasonic transmission / reception apparatus (vertex of regular hexahedron), 131-133 ... Regular hexahedron, 400 ... regular hexahedron.

Claims (7)

Ultrasonic transmission means arranged in the detected part;
A plurality of ultrasonic transmission / reception means arranged around the detected portion;
Calculating a sound speed between the plurality of ultrasonic transmission / reception means, and calculating a sound speed in the detected portion based on a sound speed between the plurality of ultrasonic transmission / reception means;
Based on the propagation time of ultrasonic waves from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means included in the plurality of ultrasonic transmission / reception means, and the sound speed in the detected part, A distance calculation means for calculating a distance from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means, and
There are a plurality of line segments connecting the ultrasonic transmission / reception devices,
As the sound speed between the plurality of ultrasonic transmission / reception means, the sound speed in the plurality of line segments is calculated,
In the process of calculating the sound speed in the detected part,
Of the sound speeds in the plurality of line segments, the sound speed in a line segment relatively close to the detected part has a relatively large effect on the sound speed in the detected part, and in a line segment relatively far from the detected part. A position detection apparatus characterized in that a process is performed in which the sound speed has a relatively small effect on the sound speed in the detected part.   The position detection device according to claim 1, wherein a sound speed in a line segment relatively close to the detected part is a sound speed in a line segment closest to the detected part. The plurality of line segments form a triangular pyramid;
The position detection apparatus according to claim 1, wherein the detected part is included in the triangular pyramid. The calculation of the sound speed in the detected part in the sound speed calculation means is as follows:
L _i (i is a natural number not including 0), the length of a perpendicular line from the detected part to one of the plurality of line segments,
L = L ₁ + L ₂ + L ₃ +...
The speed of sound in each of the plurality of line segments is vi (i is a natural number not including 0).
As

The position detection device according to claim 1, wherein the position detection device is performed by:   The position detection apparatus according to claim 1, wherein the detected portion is a tip portion of a robot hand. Calculating the speed of sound between the plurality of ultrasonic transmission / reception means arranged around the detected part, and calculating the speed of sound in the detected part based on the speed of sound between the plurality of ultrasonic transmission / reception means; ,
Based on the propagation time of ultrasonic waves from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means included in the plurality of ultrasonic transmission / reception means, and the sound speed in the detected part, Calculating a distance from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means, and
There are a plurality of line segments connecting the ultrasonic transmission / reception devices,
As the sound speed between the plurality of ultrasonic transmission / reception means, the sound speed in the plurality of line segments is calculated,
In the step of calculating the sound speed,
Of the sound speeds in the plurality of line segments, the sound speed in a line segment relatively close to the detected part has a relatively large effect on the sound speed in the detected part, and in a line segment relatively far from the detected part. A position detection method characterized in that a process is performed in which the sound speed has a relatively small effect on the sound speed in the detected part. On the computer,
A process for calculating the sound speed between the plurality of ultrasonic transmission / reception means arranged around the detected part, and calculating the sound speed in the detected part based on the sound speed between the plurality of ultrasonic transmission / reception means; ,
Based on the propagation time of ultrasonic waves from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means included in the plurality of ultrasonic transmission / reception means, and the sound speed in the detected part, A process of calculating a distance from the ultrasonic transmission means arranged in the detected part to the specific ultrasonic transmission / reception means, and
There are a plurality of line segments connecting the ultrasonic transmission / reception devices,
As the sound speed between the plurality of ultrasonic transmission / reception means, the sound speed in the plurality of line segments is calculated,
In the process of calculating the sound speed in the detected part,
Of the sound speeds in the plurality of line segments, the sound speed in a line segment relatively close to the detected part has a relatively large effect on the sound speed in the detected part, and in a line segment relatively far from the detected part. A program for performing a process in which a sound speed has a relatively small effect on a sound speed in the detected portion.

Images