JP2011200331A - Position measurement system, position measurement apparatus, and position measurement program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a position measurement system, a position measurement apparatus and a position measurement program which calculate the position of a grip part even when an arm part equipped with a label device and a hand part having the grip part for gripping an object to be gripped are bent.SOLUTION: The position measurement system 100 comprises: the label device 1 having first to fourth LEDs 11-14 which is attached to a forearm part 95 connected so as to be bent freely to a hand part 96 having a grip part 96a for gripping one of first to third assembly parts 41-43; an imaging device 2 for imaging the label device 1; a first calculating means 312 for calculating the position and the orientation of the forearm part 95 on the basis of positional relationship between the first to fourth LEDs 11-14 in an image taken by the imaging device 2; and a second calculating means 313 for calculating the position of the grip part 96a on the basis of the position and the orientation of the forearm part 95 calculated by the first calculating means 312 while taking into account bending of the forearm part 95 and the hand part 96 according to the orientation of the forearm part 95.

Description

  The present invention relates to a position measurement system, a position measurement apparatus, and a position measurement program.

  2. Description of the Related Art Conventionally, a system is known in which a marker is placed at a predetermined part of an operator and an operator's movement is observed based on an image obtained by photographing the marker (see, for example, Patent Document 1).

  In the work instruction system described in Patent Document 1, a marker is placed on a part of the operator's arm, shoulder, head, etc., and the behavior of the worker is grasped and grasped based on a video recording obtained by photographing the marker with a video camera. The information on the behavior of the worker is compared with the reference information, and the worker is instructed based on the comparison result.

JP 2006-239014 A

  The object of the present invention is to calculate the position of the gripping part in consideration of the bending even when the arm part to which the marker device is attached and the hand part having the gripping part for gripping the gripping object are bent. A position measurement system, a position measurement device, and a position measurement program are provided.

In order to achieve the above object, one aspect of the present invention provides the following position measurement system, position measurement apparatus, and position measurement program.
[1] A sign device having at least three position markers attached to an arm portion that is flexibly connected to a hand portion having a grip portion for gripping an object to be grasped, a photographing device for photographing the sign device, A first computing means for computing the position and orientation of the arm portion based on the positional relationship of the at least three position markers in an image photographed by the photographing device; and the position of the arm portion computed by the first computing means. And a second measuring means for calculating the position of the gripping part in consideration of bending of the arm part and the hand part according to the direction of the arm part based on the direction of the arm part.

[2] When the second computing means has no bending between the arm and the hand when the orientation of the arm computed by the first computing means is upward from a set angle When the position of the gripper is corrected to be lower than the position of the gripper, and when the orientation of the arm calculated by the first calculation means is downward than a set angle, The position measurement system according to [1], wherein the correction is performed so that the position of the grip portion becomes higher than the position of the grip portion when there is no bending with the hand portion.

[3] The position measurement system according to [1] or [2], wherein the second calculation means calculates the position of the grip portion in consideration of an inclination of a support member on which the grip target is placed. .

[4] An image acquisition means for acquiring an image obtained by photographing a marker device having at least three position markers attached to an arm part to which a hand part having a gripping part for gripping a gripping object is flexibly connected; A first computing means for computing the position and orientation of the arm part based on the positional relationship of the at least three position markers in the image obtained by the image obtaining means; and the arm part computed by the first computing means. A position measuring device comprising: a second computing unit that computes the position of the gripping part in consideration of bending of the arm part and the hand part according to the orientation of the arm part based on the position and orientation.

[5] An image acquisition step of acquiring an image obtained by photographing a marker device having at least three position markers attached to an arm part to which a hand part having a gripping part for gripping a gripping object is flexibly connected; A first calculation step for calculating the position and orientation of the arm portion based on the positional relationship of the at least three position markers in the image acquired by the image acquisition means; and the arm portion calculated by the first calculation means. A position measurement program comprising: a second calculation step for calculating the position of the gripping part in consideration of bending of the arm part and the hand part according to the direction of the arm part based on the position and orientation.

  According to the first, fourth, and fifth aspects of the present invention, even when the arm portion to which the marker device is attached and the grip portion that grips the gripping object are bent, the position of the grip portion is determined in consideration of the bending. It can be calculated.

  According to invention of Claim 2, the position of a holding part can be correct | amended according to direction of an arm part.

  According to the third aspect of the present invention, even when the support member on which the object to be grasped is inclined, the position of the grasping portion can be calculated according to the inclination.

FIG. 1 is an overall view showing a position measurement system according to a first embodiment of the present invention. FIG. 2 is a plan view showing the marker device viewed from the photographing apparatus according to the first embodiment of the present invention. FIG. 3 is a configuration diagram illustrating a schematic configuration example of the position measurement apparatus according to the first embodiment of the present invention. FIG. 4 is an explanatory diagram for explaining a calculation method of the position and orientation of the marker device according to the first embodiment of the present invention. FIG. 5 is a graph illustrating an example of a correction amount when the second calculation unit according to the first embodiment of the present invention corrects the coordinates of the position of the grip portion 96a. FIG. 6 is a flowchart showing an operation procedure of the position measurement system according to the first embodiment of the present invention. FIG. 7 is an overall view showing a position measurement system according to the second embodiment of the present invention. FIG. 8 is a plan view showing the marker device as seen from the photographing apparatus according to the third embodiment of the present invention.

[First Embodiment]
FIG. 1 is a diagram showing a position measurement system according to a first embodiment of the present invention.

  The position measurement system 100 is based on the sign device 1 attached to the forearm portion 95 of the worker 90, the photographing device 2 that photographs the sign device 1 from above the worker 90, and the image photographed by the photographing device 2. And a position measuring device 3 for performing calculation. In the following description, a case where the movement of the human worker 90 is observed with the position measurement system 100 will be described. However, a robot imitating a human form may be the observation target.

  The worker 90 puts any one of a plurality of types (three types in the present embodiment) of first to third assembled components 41, 42, 43 placed on the component shelf 4 while standing on the floor surface 8. It is gripped and taken out from the parts shelf 4 and assembled to an assembly not shown.

  The parts shelf 4 is provided with first to third shelf boards 4a, 4b, 4c as support members for supporting the first to third assembly parts 41, 42, 43, respectively. The 1st-3rd shelf boards 4a, 4b, 4c are provided along the horizontal direction, and the height from the floor surface 8 is different, respectively.

  The first assembly component 41 supported by the first shelf board 4 a is located at a height corresponding to the head 91 of the worker 90. The second assembly component 42 supported by the second shelf 4 b is located at a height corresponding to the chest 92 of the operator 90. The third assembly part 43 supported by the third shelf 4 c is located at a height corresponding to the abdomen 93 of the worker 90. The first to third shelf plates 4a, 4b, and 4c are supplied with the same type of components after the operator 90 takes out the components.

  The upper arm portion 94 of the worker 90 is connected to the chest portion 92 via a shoulder joint 9a as a first connecting portion, and can be bent with respect to the chest portion 92. Further, the forearm portion 95 of the worker 90 is connected to the upper arm portion 94 via an elbow joint 9b as a second connecting portion, and can be bent with respect to the upper arm portion 94. The operator's hand 96 is connected to the forearm 95 through a wrist joint 9c as a third connecting part, and can be bent with respect to the forearm 95.

  The hand part 96 is an example of a grip part that can grip the first to third assembly parts 41, 42, 43. The forearm portion 95 is an example of an arm portion that is connected to the hand portion 96 so as to be bendable.

  The photographing device 2 is realized by, for example, a video camera, and is fixed at a position where the marker device 1 can be photographed above the worker 90. Information on the image photographed by the photographing device 2 is sent to the position measuring device 3.

  In the following description, the vertical direction perpendicular to the floor surface 8 is the Z axis, and the depth direction of the parts shelf 4 is the Y axis. A direction perpendicular to the Y axis and the Z axis (a width direction of the component shelf 4) is taken as an X axis.

  FIG. 2 is a diagram showing the marker device 1 as viewed from the photographing device 2. When the operator 90 grips the first to third assembly parts 41, 42, and 43, the marking device 1 is blocked by the first to third shelves 4 a, 4 b, and 4 c when the image of the labeling device 1 is blocked. The upper arm 94 is mounted on the side of the upper arm 94 with respect to the center position in the longitudinal direction of the forearm 95. The marking device 1 is fixed to the forearm portion 95 by the belt 10, but is not limited thereto, and may be fixed to the clothes of the operator 90 by adhesion or the like.

  In the marker device 1, first to fourth LEDs (Light Emitting Diodes) 11 to 14 as position markers are arranged. The first to fourth LEDs 11 to 14 are configured as light sources that are supplied with power from, for example, an unillustrated battery built in the photographing apparatus 2 and emit light when the position measurement system 100 is used.

  The 1st-4th LED11-14 is arrange | positioned on the same plane, for example, each LED is arrange | positioned at equal intervals so that the vertex of a square may be made. The marking device 1 is attached to the forearm portion 95 so that a plane including the first to fourth LEDs 11 to 14 is parallel to the virtual central axis of the forearm portion 95.

  The area indicated by the broken line in the hand portion 96 in FIG. 2 contacts the upper surface of the first to third assembly parts 41, 42, 43 when gripping the first to third assembly parts 41, 42, 43. It is an area to do. Hereinafter, this area | region is called the holding part 96a.

The distance L ₁ from the center position of the first to fourth LEDs 11 to 14 to the wrist joint 9c along the longitudinal direction of the forearm portion 95 and the distance L ₂ from the wrist joint 9c to the center of the grip portion 96a are measured in advance. It is assumed that

(Configuration of the position measuring device 3)
FIG. 3 is a configuration diagram illustrating a schematic configuration example of the position measuring device 3. The position measuring device 3 includes a control unit 31 configured by electronic components such as a CPU (Central Processing Unit), and a storage unit 32 configured by a magnetic disk, a ROM (Read Only Memory), a RAM (Random Access Memory), and the like. The communication unit 33 communicates with the photographing apparatus 2.

  When the CPU operates in accordance with the position measurement program 320 stored in the storage unit 32, the control unit 31 operates as an image acquisition unit 311, a first calculation unit 312, a second calculation unit 313, a determination unit 314, and a collation unit. It functions as 315 etc.

  The storage unit 32 includes a position measurement program 320, image information 321 that is information of an image captured by the image capturing device 2, and sign device information 322 that indicates a relative positional relationship between the first to fourth LEDs 11 to 14 in the sign device 1. , Worker information 323 which is information relating to the physique of the worker 90, correction amount information 324 for correcting the position of the grip portion 96a, and an assembly part position indicating the positions of the first to third assembly parts 41 to 43 Information 325, assembly component information 326 indicating the type of assembly component to be assembled to the assembly, and the like are stored.

The labeling device information 322 includes information on the mutual distance between the first to fourth LEDs 11 to 14 corresponding to the length of each side of the square formed by the first to fourth LEDs 11 to 14. Further, the worker information 323 includes information of the distance _{L 1} and the distance _{L 2.} The distance L ₁ and the distance L ₂ may be stored as worker information 323 as values set in advance in consideration of the average worker's physique.

  The image acquisition unit 311 acquires information of an image captured by the image capturing device 2 via the communication unit 33 every predetermined time (for example, 0.1 second) and stores the acquired information in the storage unit 32.

  When all of the first to fourth LEDs 11 to 14 are included in the image stored as the image information 321 in the storage unit 32, the first calculation unit 312 includes the first to fourth LEDs 11 to 11 in the image. Based on the positional relationship of 14, the three-dimensional position and orientation of the marker device 1 are calculated.

  The three-dimensional position and orientation of the sign device 1 can be obtained by performing a geometric calculation based on the positional relationship between the first to fourth LEDs 11 to 14 in the image of the image information 321. An example of the calculation method will be described below.

  FIG. 4 is an explanatory diagram shown for explaining a calculation method of the three-dimensional position and orientation of the marker device 1.

  First, the image positions c1, c2, c3 of three (for example, first to third LEDs 11 to 13) of the four LEDs of the labeling device 1 on the pixel surface 21 of the photographing device 2 and the optical center 20 of the photographing device 2 are illustrated. From the positional relationship, direction vectors d1, d2, and d3 of the positions of the first to third LEDs 11 to 13 in the coordinate system with the optical center 20 as a reference are calculated. The direction vectors d1, d2, and d3 are standardized unit vectors, x1, x2, and x3 indicating vectors in the X-axis direction, y1, y2, and y3 indicating vectors in the Y-axis direction, and vectors in the Z-axis direction. Z1, z2 and z3 representing the above.

When the position vectors in the three-dimensional space of the first to third LEDs 11 to 13 are p ₁ , p ₂ , and p ₃ , these exist on the extension lines of the direction vectors d ₁ , d ₂ , and d ₃ , so that the optical center When the distances from 20 are t ₁ , t ₂ , and t ₃ , the position vectors p ₁ , p ₂ , and p ₃ are expressed by the following equations.
(Equation 1)
p ₁ = t ₁ · d ₁
p ₂ = t ₂ · d ₂
p ₃ = t ₃ · d ₃

The actual distance between the first to third LEDs 11 to 13 in the labeling device 1 is stored as the labeling device information 322 in the storage unit 32, the distance between the first LED 11 and the second LED 12, and the second LED 12 When the distance between the third LED 13 and the third LED 13 and the distance between the third LED 13 and the first LED 11 are i ₁ , i ₂ , and i ₃ , the following equations are obtained.
(Equation 2)
(t ₁ · x ₁ -t ₂ · x ₂ ) ² + (t ₁ · y ₁ -t ₂ · y ₂ ) ² + (t ₁ · z ₁ -t ₂ · z ₂ ) ² = i ₁ ²
(t ₂ · x ₂ -t ₃ · x ₃ ) ² + (t ₂ · y ₂ -t ₃ · y ₃ ) ² + (t ₂ · z ₂ -t ₃ · z ₃ ) ² = i ₂ ²
(t ₃ · x ₃ -t ₁ · x ₁ ) ² + (t ₃ · y ₃ -t ₁ · y ₁ ) ² + (t ₃ · z ₃ -t ₁ · z ₁ ) ² = i ₃ ²

By transforming this equation, the following equation is obtained.
(Equation 3)
t ₁ ² −2 · t ₁ · t ₂ · (x ₁ · x ₂ + y ₁ · y ₂ + z ₁ · z ₂ ) + t ₂ ² −i ₁ ² = 0
t ₂ ² -2 · t ₂ · t ₃ · (x ₂ · x ₃ + y ₂ · y ₃ + z ₂ · z ₃ ) + t ₃ ² -i ₂ ² = 0
t ₃ ² −2 · t ₃ · t ₁ · (x ₃ · x ₁ + y ₃ · y ₁ + z ₃ · z ₁ ) + t ₁ ² −i ₃ ² = 0

Based on this equation, the following equation is obtained.
(Equation 4)
t ₁ = A ₁ · t ₂ ± √ ((A ₁ ² -1) · t ₂ ² + i ₁ ² )
t ₂ = A ₂ · t ₃ ± √ ((A ₂ ² -1) · t ₃ ² + i ₂ ² )
t ₃ = A ₃ · t ₁ ± √ ((A ₃ ² -1) · t ₁ ² + i ₃ ² )

However, A ₁ , A ₂ , and A ₃ in the mathematical formula 4 are as follows.
(Equation 5)
A ₁ = x ₁ · x ₂ + y ₁ · y ₂ + z ₁ · z ₂
A ₂ = x ₂ · x ₃ + y ₂ · y ₃ + z ₂ · z ₃
A ₃ = x ₃ · x ₁ + y ₃ · y ₁ + z ₃ · z ₁

Here, since t ₁ , t ₂ , and t ₃ are real numbers, the following expression is obtained as a condition in which the root operator of Expression 4 is positive.
(Equation 6)
t ₁ ≦ √ (i ₃ ² / (1-A ₃ ² )
t ₂ ≦ √ (i ₁ ² / (1-A ₁ ² )
t ₃ ≦ √ (i ₂ ² / (1-A ₂ ² )

Next, t ₁ , t ₂ , and t ₃ satisfying all the mathematical expressions of the above formula 4 are calculated on condition that the mathematical expression of the above mathematical formula 6 is satisfied. The coordinates indicating the three-dimensional positions of the first to third LEDs 11 to 13 are obtained by applying t ₁ , t ₂ , and t ₃ thus obtained to the mathematical formula 1.

  In the above calculation procedure, two solutions are obtained as the coordinates of the first to third LEDs 11 to 13. Therefore, among the first to fourth LEDs 11 to 14, a combination of LEDs different from the LED used in the first calculation described above (for example, the second to fourth LEDs 12 to 14) is used as described above. The second calculation is performed according to the calculation procedure, and a combination of coordinates that coincides with the coordinates obtained by the first calculation is determined as a correct solution. Thereby, the coordinates of the first to fourth LEDs 11 to 14 can be calculated.

  When the number of LEDs of the marking device 1 is three, the first calculation means 312 averages the coordinates of the two solutions obtained by the first calculation as an approximate solution, The three-dimensional coordinates of

  The first calculation means 312 calculates the coordinates of the center point of the coordinates of the first to fourth LEDs 11 to 14 calculated by the above calculation method, and includes the first to fourth LEDs 11 to 14 from the center point. The coordinates of the position moved by a distance corresponding to the radius of the forearm portion 95 in the normal direction of the plane are calculated.

  This movement amount is stored as worker information 323 in the storage unit 32, and may be a value calculated based on, for example, the thickness of the mounting portion of the labeling device 1 of the forearm portion 95 of the worker 90 measured in advance, A fixed value set in advance in consideration of the thickness of the average forearm of the worker may be used. The coordinates thus calculated represent the position of the forearm 95 at the site where the marker device 1 is mounted.

  The first calculation means 312 calculates the direction of the forearm portion 95 based on the coordinates of the first to fourth LEDs 11 to 14. The direction of the forearm portion 95 is expressed by a unit vector from the intermediate position between the first LED 11 and the second LED 12 toward the intermediate position between the third LED 11 and the fourth LED 14, for example.

  Based on the position and orientation of the forearm 95 calculated by the first calculator 312, the second calculator 313 takes into account the bending of the forearm 95 and the hand 96 according to the orientation of the forearm 95. The position of the grip part 96a of the hand part 96 is calculated. An example of this calculation procedure will be described below.

The second calculating means 313 is only a distance L ₁ (see FIG. 2) along the direction of the forearm portion 95 from the position of the portion of the forearm portion 95 calculated by the first calculating means 312 to which the marker device 1 is attached. Calculate the coordinates of the moved position. This coordinate represents the coordinate of the wrist joint 9c.

Further, the second calculation means 313 is further moved from the position of the wrist joint 9c by the distance L ₂ (see FIG. 2) shown in FIG. 2 along the direction of the forearm 95 calculated by the first calculation means 312. Calculate the coordinates of the position. The coordinates indicate provisional coordinates of the center position of the grip portion 96a when the forearm portion 95 and the hand portion 96 are not bent.

  Next, the second calculation means 313 corrects the coordinates of the center position of the grip portion 96 a according to the orientation of the forearm portion 95. This correction is performed by adding or subtracting a correction amount corresponding to the orientation of the forearm portion 95 to the coordinates of the center position of the grip portion 96a.

  FIG. 5 is a graph showing an example of the correction amount when the second calculation means 313 corrects the coordinates of the center position of the grip portion 96a. This graph shows the relationship between the direction of the forearm 95 and the correction amount. The information shown in this graph is an example of the correction amount information 324.

  As shown in this figure, when the direction of the forearm portion 95 is the horizontal direction, the correction amount of the center position of the grip portion 96a is zero, and the negative correction amount increases as the direction of the forearm portion 95 becomes upward. growing. Further, the positive correction amount increases as the direction of the forearm 95 decreases from the horizontal direction to the downward direction. Further, in the region where the orientation of the forearm portion 95 is close to the horizontal direction, the amount of change in the correction value with respect to the orientation of the forearm portion 95 is large, and as the angle of the forearm portion 95 increases upward or downward, the direction of the forearm portion 95 increases. The amount of change in the correction value becomes small.

  This correction amount takes into account the bending angle indicating the degree of bending of the forearm portion 95 and the hand portion 96 when the operator 90 grasps and takes out the first to third assembled components 41 to 43 from the component shelf 4. Is set. This bending angle will be described with reference to FIG.

As indicated by a solid line in FIG. 1, when the operator 90 grips the second assembly component 42 having a height corresponding to the chest 92, the forearm 95 is oriented in a substantially horizontal direction, and the forearm 95 and the hand The virtual center axis 952 of the forearm portion 95 and the virtual central axis 962 of the hand portion 96 are substantially parallel to each other with the portion 96 hardly bent. In this state, the bending angle theta ₂ is the angle of the lower side formed by the center axis 952 and the central axis 962 in the YZ plane is substantially 180 °.

Further, as shown by a broken line in FIG. 1, when the operator 90 grips the first assembly component 41 having a height corresponding to the head 91, the forearm portion 95 is oriented upward from the horizontal direction, part 96 is bent with respect to the forearm 95, the bending angle theta ₁ between imaginary central axis 961 of the virtual central axis 951 and the hand portion 96 of the forearm 95 in this state, for example a 150 °.

Further, as shown by a broken line in FIG. 1, when the operator 90 grips the third assembly component 43 having a height corresponding to the abdomen 93, the forearm portion 95 is directed upward from the horizontal direction, and the hand portion 96 is bent in the opposite direction to the forearm 95, a bending angle theta ₃ is for example 210 ° with imaginary central axis 963 of the virtual central axis 953 and the hand portion 96 of the forearm 95 in this state .

  As described above, the bending angle between the forearm 95 and the hand 96 changes according to the direction of the forearm 95. When the direction of the forearm portion 95 is upward from the horizontal direction, the actual position of the grip portion 96a is lower than the position of the grip portion 96a when the forearm portion 95 and the hand portion 96 are not bent.

  Further, when the forearm 95 is oriented downward from the horizontal direction, the actual position of the grip 96a is higher than the position of the grip 96a when the forearm 95 and the hand 96 are not bent. Become. The correction values shown in FIG. 5 are set based on the relationship between the direction of the forearm portion 95 and the amount of displacement of the grip portion 96a due to the bending angle.

  As shown in the graph of FIG. 5, the second calculation means 313 indicates the Z-axis coordinate value of the temporary coordinate of the center position of the grip portion 96 a when the forearm portion 95 and the hand portion 96 are not bent. Correction is performed by adding a correct correction amount, and coordinates indicating the position of the grip portion 96a are calculated. That is, the second computing means 313 performs correction so that the position of the grip portion 96a is lowered when the orientation of the forearm portion 95 is upward from a set angle corresponding to the horizontal direction, and the orientation of the forearm portion 95 is Correction is performed so that the position of the grip portion 96a becomes higher when the angle is lower than the set angle corresponding to the horizontal direction.

  The relationship between the direction of the forearm portion 95 and the correction amount may be stored as the correction amount information 324 in the form of a correspondence table, for example, and a relational expression indicating the correlation between the direction of the forearm portion 95 and the correction amount is given. The correction amount information 324 may be stored.

  Based on the coordinates indicating the position of the grip portion 96a calculated by the second calculation means 313, the determination means 314 is used by the worker 90 to select any one of the first to third assembly parts 41 to 43. It is determined whether or not the hand is held. The determination unit 314 calculates a relative distance between the arrangement positions of the first to third assembly parts 41 to 43 indicated in the assembly part position information 325 of the storage unit 32 and the coordinates of the grip part 96a, for example. Then, it is determined that the worker 90 has gripped the assembly part with the shortest distance.

  The collating unit 315 collates the assembly part determined by the determination unit 314 with the assembly part to be assembled to the assembly, and determines whether or not the worker 90 has grasped an appropriate assembly part. Information on the assembly parts to be assembled to the assembly is stored in the assembly part information 326 of the storage unit 32. In addition, when it is determined that the worker 90 has gripped an improperly assembled part, the collating unit 315 issues a warning by, for example, a buzzer or a display and notifies the worker 90.

(Operation of the position measurement system 100)
Next, the operation of the position measurement system 100 will be described with reference to FIGS.
FIG. 6 is a flowchart showing an operation procedure of the position measurement system 100.

  The image acquisition unit 311 acquires information of an image captured by the imaging device 2 via the communication unit 33 and stores it as image information 321 in the storage unit 32 (S10).

  Next, the first calculation means 312 determines whether or not all of the first to fourth LEDs 11 to 14 are included in the image of the image information 321 stored in step S10 (S11).

  When all of the first to fourth LEDs 11 to 14 are included in the image of the image information 321 (S11; Yes), the first calculation unit 312 is configured to display the first to fourth LEDs 11 in the image of the image information 321. Based on the positions -14, the position and orientation of the identification device 1 are calculated (S12).

  Next, the first calculation means 312 moves the amount according to the position and orientation of the identification device 1 calculated in step S12 and the thickness of the forearm portion 95 of the worker 90 obtained by referring to the worker information 323. Based on the above, the position of the forearm 95 is calculated (S13).

Next, the second calculation means 313 calculates the distance L ₁ and the distance L obtained by referring to the position of the forearm portion 95 calculated in step S13, the orientation of the identification device 1 calculated in step S12, and the worker information 323. ₂ (see FIG. 2), the position of the grip portion 96a when the forearm portion 95 and the hand portion 96 are not bent is calculated (S14).

  Next, the second calculation means 313 refers to the correction amount information 324 in the storage unit 32 and corrects the position of the grip portion 96a calculated in step S14 (S15).

  Next, based on the coordinates indicating the position of the gripper 96a that has been corrected and calculated in step S15, the determination unit 314 determines which assembly component the worker 90 has among the first to third assembly components 41 to 43. Is determined (S16).

  Next, the collating unit 315 collates the assembled part determined by the determining unit 314 with the assembled part to be assembled to the assembly, and determines whether or not the operator 90 has grasped an appropriate assembled part. (S17).

  If it is determined in step 17 that the proper assembly component is not gripped (S17; No), the collating means 315 notifies the operator 90 that the assembly component gripped by a buzzer or display is not appropriate. (S18).

  The operator 90 confirms the assembly parts by this notification, grips the appropriate assembly parts again, removes them from the parts shelf 4, and assembles them to the assembly.

[Second Embodiment]
Next, a second embodiment of the present invention will be described. In the present embodiment, the position of the grip portion 96a is corrected according to the direction of the forearm portion 95 in the YZ plane when the operator 90 is viewed from the side.

  As shown in FIG. 1, when the worker 90 grips any of the first to third assembly parts 41 to 43, the forearm portion 95 is oriented in the first to third assembly parts 41 to 43. The direction changes depending on the position in the height direction, but the direction of the hand portion 96 is substantially along the horizontal direction. Therefore, the angle formed by the virtual center axis of the forearm portion 95 and the virtual center axis of the hand portion 96 is approximated by the angle formed by the virtual center axis of the forearm portion 95 and the Y axis in the YZ plane. Can be sought.

  Therefore, the second calculation means 313 in the present embodiment forms the virtual central axis of the forearm portion 95 on the YZ plane and the Y axis based on the orientation of the forearm portion 95 calculated by the first calculation means 312. The position of the grip portion 96a is corrected by calculating an angle and performing a geometric calculation using the angle.

More specifically, the second calculation means 313 in the present embodiment is configured such that the angle between the virtual central axis of the forearm portion 95 and the Y axis in the YZ plane is θ, and the grasping portion 96a is moved from the wrist joint 9c. by adding or subtracting the provisional coordinates of the center position of the grip portion 96a of the case where the distance corresponding to the product obtained by multiplying the distance L ₂ of sinθ to the center and that there is no bending of the forearm portion 95 and hand portion 96 Make corrections.

Note that the coordinates of the wrist 9c the first calculating means 312 calculates a distance corresponding to the product obtained by multiplying the distance L ₂ and cosθ also by stretching in the Y-axis direction, it is possible to obtain the same value .

  Further, when the shelf of the component shelf is inclined with respect to the horizontal direction, the correction amount can be changed according to the inclination angle. The calculation method in this case will be described below.

  FIG. 7 is a configuration diagram showing the position measurement system 100 when the first to fourth assembly parts 41 to 43 are taken out from the parts shelf 4A in which the first to third shelf boards 4a, 4b, and 4c are inclined. . As shown in this figure, the angle α formed between the first to third shelf plates 4a, 4b, 4c of the component shelf 4A and the Z axis is smaller than 90 °.

When the shelf board is inclined as described above, the second calculation means 313 calculates the first to third from the angle θ formed between the virtual central axis of the forearm portion 95 and the Y axis in the YZ plane. The inclination angle (90 ° −α) with respect to the horizontal direction of the shelf boards 4a, 4b, 4c is subtracted. Then, using the angle obtained as a result of the subtraction (hereinafter, this angle is referred to as “φ”), a distance corresponding to a product obtained by multiplying the distance L ₂ by sin φ is a distance between the forearm portion 95 and the hand portion 96. Correction is performed by adding or subtracting to the temporary coordinates of the center position of the grip portion 96a in the case where there is not.

  As described above, the angle formed by the virtual central axis of the forearm portion 95 and the virtual central axis of the hand portion 96 is the angle formed by the virtual central axis of the forearm portion 95 and the Y axis in the YZ plane. The correspondence between the direction of the forearm 95 calculated by the first calculation means 312 and the angle formed by the virtual central axis of the forearm 95 and the virtual central axis of the hand 96 is not limited to the calculation method approximated by You may correct | amend the position of the holding part 96a based on the correspondence table | surface or arithmetic expression which shows a relationship.

[Third Embodiment]
Next, a third embodiment of the present invention will be described. In the above-described embodiment, the vertical position (Z-axis direction) of the position of the grip portion 96a is corrected. In the present embodiment, the position of the grip portion 96a in the X-axis direction is also corrected.

  FIG. 8 is a diagram illustrating a state in which the grip portion 96a of the hand portion 96 has moved in the direction of arrow A (the direction along the X axis). As shown in this figure, when gripping any one of a plurality of assembly parts arranged in the horizontal direction, the orientation of the forearm portion 95 varies greatly depending on the position of the grip portion 96a in the arrow A direction. The amount of change in the orientation of the hand portion 96 is smaller than the amount of change in the orientation of the forearm portion 95. In particular, when there is a partition between a plurality of assembly parts, this tendency becomes remarkable. Therefore, the forearm portion 95 and the hand portion 96 are bent.

  The second calculation means 313 according to the present embodiment is configured so that the virtual center axis of the forearm portion 95 and the virtual portion of the hand portion 96 depend on the orientation of the forearm portion 95 on the XY plane calculated by the first calculation means 312. The angle formed with the central axis is determined by calculation or by referring to the correspondence table, and the position of the grip portion 96a is corrected by the same calculation method as in the first or second embodiment according to the angle.

  Then, in accordance with the corrected position of the grip portion 96a, the determination unit 314 determines which of the plurality of types of assembly parts has been gripped, and the verification unit 315 determines whether the assembly part is appropriate. If it is inappropriate, the operator 90 is notified.

DESCRIPTION OF SYMBOLS 1 ... Marking device, 2 ... Imaging device, 3 ... Position measuring device, 4, 4A ... Parts shelf, 4a, 4b, 4c ... 1st-3rd shelf board, 8 ... Floor surface, 9a ... Shoulder joint, 9b ... Elbow joint, 9c ... wrist joint, 10 ... belt, 11-14 ... first to fourth LED, 20 ... optical center, 21 ... pixel surface, 31 ... control unit, 32 ... storage unit, 33 ... communication unit, 41 ˜43 ... first to third assembly parts, 90 ... worker, 91 ... head, 92 ... chest, 93 ... abdomen, 94 ... upper arm, 95 ... forearm, 96 ... hand, 96a ... gripping part , 100 ... Position measurement system, 311 ... Image storage means, 312 ... First calculation means, 313 ... Second calculation means, 314 ... Discrimination means, 315 ... Verification means, 320 ... Position measurement program, 321 ... Image information, 322 ... Marking device information, 323 ... Worker information, 324 ... Correction amount information, 325 ... Assembly parts Location information, 326 ... assembly parts information, 951～953,961～963 ... center axis c1, c2, c3 ... image position, d1, d2, d3 ... direction vector, alpha ... _{angle, θ 1, θ 2, θ} 3 ... Bending angle

Claims (5)

A marking device that is attached to an arm portion that is flexibly connected to a hand portion having a gripping portion that grips a gripping object, and that has at least three position markers;
A photographing device for photographing the marker device;
First computing means for computing the position and orientation of the arm portion based on the positional relationship of the at least three position markers in an image photographed by the photographing device;
Based on the position and orientation of the arm calculated by the first calculating means, the position of the gripper is calculated in consideration of bending of the arm and the hand according to the orientation of the arm. A position measurement system comprising a second calculation means.   The second calculating means holds the grip when the direction of the arm calculated by the first calculating means is upward from a set angle and the arm and the hand are not bent. Correction is performed so that the position of the grip portion is lower than the position of the portion, and when the orientation of the arm calculated by the first calculation means is downward than a set angle, the arm and the hand The position measurement system according to claim 1, wherein correction is performed so that the position of the gripping part is higher than the position of the gripping part when there is no bending.   The position measurement system according to claim 1, wherein the second calculation means calculates the position of the grip portion in consideration of an inclination of a support member on which the grip target is placed. An image acquisition means for acquiring an image obtained by photographing a labeling device having at least three position markers attached to an arm part to which a hand part having a gripping part for gripping a gripping object is foldably connected;
First computing means for computing the position and orientation of the arm portion based on the positional relationship of the at least three position markers in the image obtained by the image obtaining means;
Based on the position and orientation of the arm calculated by the first calculating means, the position of the gripper is calculated in consideration of bending of the arm and the hand according to the orientation of the arm. A position measurement device comprising a second calculation means. An image acquisition step of acquiring an image obtained by photographing a labeling device having at least three position markers attached to an arm portion to which a hand portion having a gripping portion for gripping an object to be bent is connected;
A first calculation step of calculating the position and orientation of the arm portion based on the positional relationship of the at least three position markers in the image acquired by the image acquisition means;
Based on the position and orientation of the arm calculated by the first calculating means, the position of the gripper is calculated in consideration of bending of the arm and the hand according to the orientation of the arm. A position measurement program comprising a second calculation step.

Images