JP5292998B2 - Robot apparatus control method and robot apparatus - Google Patents

Abstract

The invention provides a control method used for a robot device, comprising the steps as follows: when the manipulator of the robot device is controlled, a plurality of benchmark images are not needed to be prepared, rapid control can be carried out owning to light calculation load; furthermore, the position and gesture of manipulator correspondingly to the object can be controlled in three-dimensional space; according to the moving quantity of image of object (101) which leads the direction of the paw of the manipulator (1) to rotate in the image photographed by a camera (6), the direction of the paw of the manipulator (1) is controlled, thus leading the image of the object (101) to be arranged at the center of the visible field; according to the size of the image of the object (101) gained by the camera (6), the distance of the paw and the object (101) of the manipulator (1) is calculated, thus leading the paw of the manipulator (1) to move along the direction of the optical axis of the camera (6) so as to approach the object (101).

Description

  The present invention relates to a robot apparatus having a camera at the tip of a manipulator that operates a manipulator so that a target feature value calculated and stored in advance from a target state matches a feature value obtained from an image acquired by the camera. The present invention relates to an apparatus control method and a robot apparatus.

  Conventionally, a robot apparatus provided with a manipulator has been proposed, and a control method of the robot apparatus for controlling the operation of the manipulator of such a robot apparatus has been proposed. As a control method of the robot apparatus, various control methods for moving the hand of the manipulator with an object to be gripped by the manipulator as a target have been proposed.

  For example, Patent Document 1 prepares a plurality of reference images of an object, acquires image information from a camera attached to the hand of a manipulator, and searches for a reference image close to the shape of the object in the image information. Describes a robot apparatus that calculates the positional relationship between the object and the hand of the manipulator.

  In Patent Document 2, a reference image of an object is prepared, and the manipulator is moved by a certain amount until an image close to the reference image is obtained while taking an image with a camera attached to the hand of the manipulator. A robotic device is described.

JP 2003-231078 A JP 2000-263482 A

  By the way, in the robot apparatus described in Patent Document 1, in order to increase the accuracy of control, it is necessary to prepare a large number of reference images and search all the reference images every time the manipulator operates. The calculation load is high and quick control is not possible.

  In the robot apparatus described in Patent Document 2, only the position and orientation of the manipulator in the two-dimensional plane can be matched, and the position and orientation of the manipulator with respect to the object cannot be controlled in the three-dimensional space. Furthermore, in this robot apparatus, since imaging, measurement, and operation of the manipulator must be repeated, rapid control cannot be performed.

  Therefore, the present invention has been made in view of the above circumstances, and the purpose thereof is to control a manipulator of a robot apparatus without having to prepare a large number of reference images, and to perform a quick control with a light calculation load. It is an object to provide a robot apparatus control method and a robot apparatus that can control the position and orientation of a manipulator with respect to an object in a three-dimensional space.

  In order to solve the above-described problems and achieve the above object, a control method for a robot apparatus according to the present invention has any one of the following configurations.

[Configuration 1]
A robot apparatus control method for controlling a robot apparatus having a manipulator having six degrees of freedom or more and having an image pickup means at a hand position of the manipulator, the image of the object obtained by the image pickup means and the shape of the object in advance The image of the object is positioned at the center of the field of view of the imaging means based on the amount of movement of the image of the object in the imaging means when the direction of the hand of the manipulator is turned. The object obtained by the imaging means by controlling the direction of the hand of the manipulator, and then collating the image of the object obtained by the imaging means with previously stored information about the shape of the object Based on the size of the image of the object, the distance between the hand of the manipulator and the object is calculated, and the hand of the manipulator is moved in the optical axis direction of the lens of the imaging means. Is to approximate該手destination object, then compares the pre-stored information about the shape of the image and the object of the object obtained by the imaging means, the direction of the hand of the manipulator, the object The direction perpendicular to the measurement surface of the object,
Next, as an image of the object within the field of view of the imaging means is a predetermined direction and is characterized that you rotation controlling hand of the manipulator.

[Configuration 2]
In the control method of the robot apparatus having the configuration 1, the object has a feature quantity having a circular shape or a shape approximated to a circular shape, and the upper surface is a measurement surface.

[Configuration 3]
In the control method of the robot apparatus having the configuration 1, the target has an arbitrary shape other than a circle, and has a measurement surface having a feature amount of a circle or a shape approximated to a circle. It is characterized by.

[Configuration 4]
A method of controlling a robot apparatus that have a structure 1, by controlling the direction of the hand of the manipulator, positioning the image of the object in the center of the field of view of the imaging means, the field of view on the image obtained by the imaging means The center C0 of the object and the initial center position C1 of the object on the image are acquired, the selected operation axis is rotated to acquire the center position C2 of the object on the image, and the line C0-C1 and the line C1- The distance and inclination between C2 are obtained, and then the intersection A between the straight line C1-C2 and the straight line passing through the visual field center C0 perpendicular to the straight line C1-C2 is determined, and the distance between the straight line C0-A and the straight line C2-A Determining the amount of rotation of the joint of the manipulator around the selected motion axis, and then rotating the motion axis orthogonal to the selected motion axis to obtain the center position C2 of the object on the image, Between straight line C0-C1 and straight C1-C2 distance and inclination are obtained, and then an intersection A between the straight line C1-C2 and a straight line passing through the visual field center C0 perpendicular to the straight line C1-C2 is determined, and between the straight line C0-A and the straight line C2-A The amount of rotation of the manipulator around the operation axis is determined, the manipulator is controlled based on each determined amount of rotation, and the image of the object is set as the center of the field of view of the imaging means. To do.

  The robot apparatus according to the present invention has any one of the following configurations.

[Configuration 5 ]
A robot apparatus having a manipulator having six or more degrees of freedom and having an image pickup means at a hand position of the manipulator, comprising a control means for controlling the operation of the manipulator, the control means including an image of an object obtained by the image pickup means, The image of the object is imaged based on the amount of movement of the image of the object when the direction of the hand of the manipulator is turned by collating with information stored in advance about the shape of the object Control the direction of the hand of the manipulator so that it is located at the center of the field of view, and then collate the image of the object obtained by the imaging means with pre-stored information about the shape of the object, Based on the size of the image of the object obtained by the imaging means, the distance between the hand of the manipulator and the object is calculated, and the hand of the manipulator is imaged. Is moved in the optical axis direction of the stage of the lens to approximate該手destination object, then compares the pre-stored information about the shape of the image and the object of the object obtained by the image pickup means the direction of the hand of the manipulator, the direction perpendicular to the measurement surface of the object, then, as the image of the object within the field of view of the imaging means is a predetermined direction, to the rotation control of the hand of the manipulator Rukoto It is characterized by.

[Configuration 6 ]
In the robot apparatus having the configuration 5 , the object has a feature amount of a circle or a shape approximated to a circle, and the upper surface is a measurement surface.

[Configuration 7 ]
In the robot apparatus having the configuration 5 , the object has an arbitrary shape other than a circle, and has a measurement surface having a feature amount of a circle or a shape approximated to a circle. To do.

[Configuration 8 ]
In the robot apparatus having the configuration 5 , the control unit controls the direction of the hand of the manipulator so that the image of the object is positioned at the center of the field of view of the imaging unit, the field of view on the image obtained by the imaging unit The center C0 of the object and the initial center position C1 of the object on the image are acquired, the selected operation axis is rotated to acquire the center position C2 of the object on the image, and the line C0-C1 and the line C1- The distance and inclination between C2 are obtained, and then the intersection A between the straight line C1-C2 and the straight line passing through the visual field center C0 perpendicular to the straight line C1-C2 is determined, and the distance between the straight line C0-A and the straight line C2-A Determining the amount of rotation of the joint of the manipulator around the selected motion axis, and then rotating the motion axis orthogonal to the selected motion axis to obtain the center position C2 of the object on the image, Between straight line C0-C1 and The distance and the inclination between the lines C1-C2 are obtained, and then an intersection A between the straight line C1-C2 perpendicular to the straight line C1-C2 and the straight line C1-C2 is determined, and the straight lines C0-A and C2-A are determined. The amount of rotation between the operation axes of the manipulator is determined by determining the distance between the manipulators, the manipulator is controlled based on each determined amount of rotation, and the image of the object is set as the center of the field of view of the imaging means It is what.

In the control method for a plurality of robots according to the present invention, by having the configuration 1, the direction of the hand of the manipulator is controlled based on the amount of movement in the imaging means of the image of the object when the direction of the hand of the manipulator is turned. The image of the object is positioned at the center of the field of view of the imaging means, and then the distance between the hand of the manipulator and the object is calculated based on the size of the image of the object obtained by the imaging means Then, by moving the manipulator hand in the optical axis direction of the lens of the imaging means, the manipulator hand is brought close to the object , and then the object image obtained by the imaging means and the shape of the object in advance The direction of the hand of the manipulator is set to a direction perpendicular to the measurement surface of the object, and then the object in the field of view of the imaging means As the image becomes the predetermined direction, can it to rotate controlling hand of the manipulator.

  In the control method for a plurality of robots according to the present invention, since the object has the feature amount of the circular shape or the shape approximated to the circular shape by having the configuration 2, the upper surface is the measurement surface. It is possible to quickly collate the image of the object obtained by the imaging unit with the information stored in advance about the shape of the object.

  In the method for controlling a plurality of robots according to the present invention, by having the configuration 3, the object has an arbitrary shape other than a circle, and has a feature amount of a circle or a shape approximated to a circle. Since the measurement surface is provided, it is possible to quickly collate the image of the object obtained by the imaging unit with the information stored in advance about the shape of the object.

  That is, in the control method of the robot apparatus according to the present invention, the manipulator and the object can be positioned and aligned in a three-dimensional space, an arbitrary joint is moved by a specified angle, and the imaging means Since the alignment is performed by repeating the operation of a general industrial manipulator such as moving the lens in the optical axis direction, the configuration of the control means can be simplified and the addition of computation can be reduced.

In the plurality of robots according to the present invention, by having the configuration 5 , the control unit is configured to adjust the manipulator's hand based on the amount of movement in the image pickup unit of the image of the object when the direction of the hand of the manipulator is turned. Controlling the direction to position the image of the object in the center of the field of view of the imaging means, and then between the hand of the manipulator and the object based on the size of the image of the object obtained by the imaging means Calculates the distance and moves the manipulator's hand in the optical axis direction of the lens of the imaging means, so that any joint can be moved by a specified angle and moved in the optical axis direction of the lens of the imaging means. manipulator by repeating the operation with the, form of to approximate hand of the manipulator to the object, then the image and the object of the object obtained by the image pickup means The direction of the hand of the manipulator is set to a direction perpendicular to the measurement surface of the object, and then the image of the object in the field of view of the imaging means is a predetermined direction. as will be you to rotate controlling hand of the manipulator.

In the plurality of robots according to the present invention, by having the configuration 6 , the object has a feature quantity of a circle or a shape approximated to a circle, and the upper surface is a measurement surface. In addition, it is possible to quickly collate the image of the object obtained by the imaging unit with the information stored in advance about the shape of the object.

In the plurality of robots according to the present invention, by having the configuration 7 , the object has an arbitrary shape other than a circle, and a measurement surface having a feature amount of a circle or a shape approximated to a circle is provided. Since it has, the control means can perform collation with the information memorize | stored in advance about the image of the target object obtained by the imaging means, and the shape of the target object.

  That is, in the robot apparatus according to the present invention, the manipulator and the object can be aligned and postured in a three-dimensional space, an arbitrary joint can be moved by a specified angle, and the lens of the imaging unit can be moved. Since the alignment is performed by repeating the operation of a general industrial manipulator such as moving in the optical axis direction, the configuration of the control means can be simplified, and calculation addition can be reduced.

  That is, according to the present invention, in controlling the manipulator of the robot apparatus, it is not necessary to prepare many reference images, the calculation load is light, and quick control is possible. It is possible to provide a robot apparatus control method and a robot apparatus which can be controlled in a dimensional space.

  The best mode for carrying out the present invention will be described below with reference to the drawings.

[Robot device configuration]
FIG. 1 is a schematic side view showing a configuration of a robot apparatus according to the present invention.

  As shown in FIG. 1, the robot apparatus according to the present invention includes a manipulator 1, and the manipulator 1 enables the object 101 to be gripped, transported, and assembled to another member.

  The manipulator 1 is composed of a plurality of actuators (driving devices) and links (rigid structures), and has six degrees of freedom. That is, the links are connected via joints 2a, 2b, 2c, 2d, 2e, and 2f that can be rotated (bent) or turned, and are relatively driven by actuators. . Each actuator is controlled by a computer 3 serving as control means.

  In the manipulator 1, the first link (link at the base end) 1 a is installed with the base end connected to the base 4 via the first joint 2 a. The first joint 2a is a joint that can turn around a vertical axis (z-axis). The proximal end side of the second link 1b is connected to the distal end side of the first link 1a via the second joint 2b. The second joint 2b is a joint that allows the second link 1b to rotate around a horizontal axis. The proximal end side of the third link 1c is connected to the distal end side of the second link 1b via the third joint 2c. The third joint 2c is a joint that allows the third link 1c to rotate around a horizontal axis.

  And the base end side of the 4th link 1d is connected to the front end side of the 3rd link 1c via the 4th joint 2d. The fourth joint 2d is a joint that enables the fourth link 1d to turn around the axis of the third link 1c. The proximal end side of the fifth link 1e is connected to the distal end side of the fourth link 1d via the fifth joint 2e. The fifth joint 2e is a joint that allows the fifth link 1e to rotate about an axis orthogonal to the axis of the fourth link 1d. The proximal end side of the sixth link 1f is connected to the distal end side of the fifth link 1e via the sixth joint 2f. The sixth joint 2f is a joint that enables the sixth link 1f to turn around the axis of the fifth link 1e.

  As described above, in the manipulator 1, a total of six joints that can rotate and pivotable joints are alternately provided, so that six degrees of freedom are secured.

  A tip tool mechanism 5 for gripping and processing the object 101 is provided on the tip side of the sixth link 1 f (hereinafter referred to as “hand”). The tip tool mechanism 5 is controlled by the computer 3 or other control device (not shown). In addition, a camera 6 serving as an imaging unit including an imaging lens and a CCD (solid-state imaging device) is attached in the vicinity of the hand.

  The computer 3 has an image feature data table 3a. In the image feature data table 3a, image data and numerical data indicating the shapes of the plurality of objects 101 are stored. The computer 3 has an image processing unit 3b, extracts the shape of the target component 101 from the image signal sent from the camera 6, and stores the shape, image data and numerical values stored in the image feature data table 3a. Match data. The computer 3 has an operation command generation unit 3c. The operation command generation unit 3c generates an operation command signal for each actuator of the manipulator 1 according to the collation result between the image signal in the image processing unit 3b and the data in the image feature data table 3a, and sends the operation command signal to each actuator.

  In the manipulator 1, the position of the second link 1b with respect to the first link 1a, the position of the third link 1c with respect to the second link 1b, the position of the fourth link 1d with respect to the third link 1c, By sequentially controlling the position of the link on the distal end side, the position of the hand tip is controlled, and the work gripped by the tip tool mechanism 5 at this hand tip can be conveyed to a predetermined position.

  In this robot apparatus, the control method of the robot apparatus according to the present invention described below is executed so that the measurement surface of the object 101 is perpendicular to the optical axis of the lens of the camera 6. The manipulator 1 is automatically controlled. In this embodiment, description will be made assuming that the object 101 is a circular component.

  FIG. 2 is a flowchart showing advance preparation (a) and control operation (b) of the control method of the robot apparatus according to the present invention.

  As a preliminary preparation for performing automatic control of the manipulator 1, as shown in FIG. 2A, when the preliminary preparation is started in S1, parameters unique to the manipulator 1 of the robot apparatus to be controlled are set in S2. Calculated and stored as a table in the storage device of the computer 3.

That is, the pixels in the image sensor are based on the number of effective elements (Xpx, Ypx) (resolution (pixel units)) in the vertical and horizontal directions of the image sensor of the camera 6 and the vertical and horizontal sizes (Xln, Yln) (physical length) of the effective pixels. The conversion coefficient between the physical quantity length and the physical quantity length is calculated as follows.
(Kxln, Kyln) = (Xln / Xpx, Yln / Ypx)

  FIG. 4 is a perspective view illustrating a configuration in which the camera 6 is represented as a pinhole camera.

  Further, as shown in FIG. 4, the length in the captured image is based on the distance Dpl (physical length) from the imaging lens 6a of the camera 6 to the imaging element 6b and the distance L from the imaging lens 6a to the object 101. The relationship between PS and actual physical length RS is calculated in advance. Note that the relationship between the distance Dpl from the imaging lens 6a to the imaging element 6b and the distance L from the imaging lens 6a to the object 101 can be calculated based on the focal length of the imaging lens 6a. Therefore, the relationship between the length PS in the captured image and the actual physical length RS can be calculated based on the distance Dpl from the imaging lens 6a to the imaging element 6b and the focal length of the imaging lens 6a.

  Furthermore, as a parameter unique to the robot apparatus, a conversion matrix T from the hand coordinate system to the camera coordinate system is also calculated.

  Next, in S3, parameters unique to the object 101 are calculated and stored in the image feature data table 3a of the computer 3.

  That is, the feature amount of the shape of the target object 101 when the target object 101 is imaged by the camera 6 is stored as the target image feature amount of the target object 101. Specifically, the target position (Px, Py) (center position, center of gravity position, etc.) on the image of the object 101 is stored in units of pixels, and the orientation θ indicating the direction (degree or unit of rad), A size PS (pixel unit) used for distance determination is stored as a table. For example, when the shape of the captured object 101 is an ellipse, the center position (Px, Py), PS (major axis or minor axis), θ (major axis orientation angle), etc. on the image are set. Remember.

  The object 101 is approximated to a circle on the image, for example, covered with a circle, and the center position and the radius are obtained. Note that the area of the object 101 (the area of the upper surface) may be calculated and stored.

  In S4, the preliminary preparation is finished. If the advance preparation has been completed, the control operation can be executed as shown in FIG. That is, when the control operation is started in S5, centering is executed in S6.

  FIG. 3 is a front view showing a captured image corresponding to each stage in a state in which the robot apparatus control method according to the present invention is being executed.

  Centering is control for operating the manipulator 1 so that the image of the object 101 is at the center of the image captured by the camera 6, as shown in phase 1 in FIG.

  FIG. 5 is a front view showing a captured image in a state where the object 101 is centered.

  As shown in FIG. 5, this centering is described with the center position of the image captured by the camera 6 being C0 (X0, Y0) and the initial position of the object 101 on this image being C1 (Px1, Py1). To do.

  FIG. 6 is a flowchart showing the procedure of the centering operation.

  This centering is performed according to the following procedure. That is, as shown in FIG. 6, when centering is started in S11, the center position of the image captured by the camera 6 is acquired in S12, and the initial position C1 of the object 101 on the image is acquired in S12. move on. In S13, the fourth joint 2d is selected, and in S14, it is driven by an appropriate value qt4. The driving amount may be any angle and direction, but is preferably about 3 deg to 5 deg, for example.

  At this time, as shown in FIG. 5, the image is taken again by the camera 6, and the position of the object 101 in the image acquired at this time is C2 (Px2, Py2), and as shown in S14 in FIG. The position C2 is acquired and stored.

Then, the process proceeds to S15, and as shown in FIG. 5, the distance d between the position C0 and the position C2 of the object 101 and the distance d12 between the position C1 and the position C2 on the image are obtained. Since the positions (coordinates on the image) of each point are known, these can be obtained by the following equations.
d = √ {(X0−Px2) ² + (Y0−Py2) ² }
d12 = √ {(Px1−Px2) ² + (Py1−Py2) ² }

Next, the process proceeds to S16 in FIG. 6 to determine the slope k1 of the straight lines C0 to C1, the slope k2 of the straight lines C0 to C2, and the slope k12 of the straight lines C1 to C2. Since these also know the position (coordinate on the image) of each point, they can be obtained by the following equations.
k1 = (Py1-Py0) / (Px1-Px0)
k2 = (Py2-Py0) / (Px2-Px0)
k12 = (Py1-Py2) / (Px1-Px2)

Here, as shown in FIG. 5, a point A is an intersection of a straight line that passes through the position C0 and is perpendicular to the straight lines C1 to C2 and the straight lines C1 to C2. Proceeding to S17 in FIG. 6, the distance d0A between the point A and the position C0 is obtained. Since this is the distance between the straight lines C1 and C2 and the point C0, it can be obtained by the following equation.
d0A = | k12 · X0−Y0 + Py1−k12 · Px1 | / √ (k12 ² +1)

Next, proceeding to S18, a distance d2A from the position C1 to the point A is obtained. This distance d2A is obtained as follows from the relationship of the lengths of the sides of the right triangle C0-A-C2.
d2A = √ (d12 ² −d0A ² )

Proceeding to S19, the fourth joint 2d is rotated by a predetermined value q4. The magnitude of the predetermined value q4 is obtained by the following equation.
q4 = d2A · q4t / d12

  That is, the fourth joint 2d is rotated q4t to move the position on the image by d12, and therefore, what is the rotation angle q4 for moving by d2A is obtained.

  The sign of the predetermined value q4 is determined according to the following rules. That is, if (−1 / k12−k1) and (k2−k1) have the same sign, the direction is the same as q4t, and if (−1 / k12−k1) and (k2−k1) have different signs, The direction is opposite to q4t.

  Next, it progresses to S20 and the same control as S12 to S19 is performed about the 5th joint 2e. If the difference between the center position C0 in the image captured by the camera 6 and the position (Px, Py) of the target object 101 is a certain distance or less, the process proceeds to S21 and the centering operation is terminated. If the difference between the center position C0 of the image and the position (Px, Py) of the object 101 is not below a certain level, the operations from S12 to S20 are performed again.

  When the centering is completed as described above, next, the process proceeds to S7 in (b) in FIG. 2, and the operation of bringing the camera 6 close to the object 101 is performed.

  3, even if the hand is moved in the optical axis direction of the lens of the camera 6 as shown in phase 2 in FIG. 3, the position of the object 101 on the acquired image remains at the center position of the image. Stay stopped. In this state, the hand is moved toward the object 101 in the optical axis direction of the lens of the camera 6.

  In order to move the hand in the optical axis direction of the lens of the camera 6, the positional relationship between the manipulator 1 and the optical axis of the lens of the camera 6 needs to be known. As shown in FIG. 1, the optical axis coordinate system is Tc and the hand coordinate system is T6. If a matrix T for converting this is obtained in advance, the optical axis coordinate system Tc is obtained by T · T6 = Tc. It is only necessary to move the hand in the optical axis direction (zc axis direction) of the lens of the camera 6.

The amount of movement L in the zc-axis direction can be obtained as follows based on the relationship between the length PS and the actual physical length RS in the captured image shown in FIG.
L = Dpl / (Kxpl · PS / cos (θ1)) · RS

However, this is a case where Kxpl and Kypl are equal values. If Kxpl and Kypl are not the same value, the x-axis component and y-axis component of PS are taken out and obtained as PSx and PSy as follows.
L = Dpl / [√ {(Kxpl · PSx) ² + (Kypl · PSy) ² } / cos (θ1)] · RS

Θ1 is an angle indicating how much the hand axis z6 is inclined with respect to the vertical axis, and is obtained by the following equation.
θ1 = acos ((z6 × zc) / (| z6 | · | zc |))

    Here, (z6 × zc) is an inner product of two vectors z6 and zc, and | z6 | and | zc | are norms (lengths) of vectors z6 and zc. acos (a) is θ such that cos (θ) = a, where [0 ≦ θ ≦ 180 deg].

  This is an operation for correcting that the apparent size looks small by looking at the object 101 from an oblique direction. If the shape of the object 101 is a circle or approximates a circle, if the longest diameter is obtained, the influence of viewing from an oblique direction can be reduced, so that cos (θ1 ) Correction is not necessary.

  Based on the distance L thus determined, the camera 6 and the hand are moved toward the object 101 in the direction of the optical axis zc of the lens of the camera 6.

  Next, in (b) of FIG. 2, it progresses to S8 and performs posture alignment. This posture adjustment is an operation for controlling the posture of the hand so that the optical axis of the lens of the camera 6 is perpendicular to the measurement surface (upper surface) of the object 101, as shown in Phase 3 in FIG.

  For example, when the measurement surface is parallel to the floor surface and the manipulator 1 is also placed on a surface parallel to the measurement surface, the hand tip z6 and the optical axis zc of the camera lens are parallel. The posture of the hand of the manipulator 1 is controlled so that the axis z6 is perpendicular to the floor surface, that is, the camera 6 faces directly below.

  Then, in (b) of FIG. 2, the process proceeds to S9 to perform two-dimensional position control and phase alignment.

  FIG. 7 is a flowchart showing a procedure for two-dimensional position control and phase alignment.

  As described above, as a result of the alignment, the image of the target object 101 is deviated from the center of the image acquired by the camera 6 as shown in phase 4 in FIG. However, the optical axis of the lens of the camera 6 is perpendicular to the measurement surface. Therefore, in this state, the position and phase can be corrected by two-dimensional image feedback. Here, the phase is the direction of the image of the object 101 in the field of view of the camera 6 and is corrected by rotating the hand of the manipulator around the optical axis of the lens of the camera 6.

  That is, when two-dimensional position control and phase matching control are started in S22 of FIG. 7, the process proceeds to S23, and the size PS of the object 101 on the image is compared with the size PS1 obtained by image processing. If the size PS1 obtained by image processing is smaller, the hand is too close to the object 101. Therefore, the hand is moved in a direction away from the object 101. If PS1 is larger, the hand is too far from the object 101. The hand is moved in a direction approaching the object 101.

  In S24, it is determined whether or not the deviation between the magnitude PS and the magnitude PS1 is equal to or smaller than a predetermined value. If the deviation is not smaller than the predetermined value, the process returns to S23 to repeat the hand movement operation. If the deviation between the magnitude PS and the magnitude PS1 is less than or equal to a predetermined value, the process proceeds to S25.

In S25, the position (Px, Py) of the object 101 is obtained by image processing, and the movement amount of the hand is determined by the following equation.
[Hand movement amount of camera in x-axis direction] = Kxpl · (Px−X0)
[Amount of movement of the hand in the y-axis direction of the camera] = Kypl · (Py−Y0)

  The actual movement direction is obtained from the relationship between the direction of the axis on the image and the direction of the hand. Then, the process proceeds to S26, where it is determined whether the deviation (Px-X0, Py-Y0) of the center position is equal to or smaller than a predetermined value. If not, the process returns to S25 to move the hand. repeat. If the deviation (Px−X0, Py−Y0) of the center position is not more than a predetermined value, the process proceeds to S27.

  In S27, the sixth joint 2f is rotated by θ−θ1 so that the phase θ1 of the object 101 becomes the target angle θ. Then, the process proceeds to S28, and the two-dimensional position control and the phase matching control are completed. That is, in (b) of FIG. 2, the process proceeds to S10 and the control operation is completed.

  By the way, in above-mentioned embodiment, the shape of the target object 101 is demonstrated as circular. However, the shape of the target object 101 is not limited to a circular shape. It is only necessary that there are some circular feature portions on the surface of the target object 101 and the circular portion can be extracted by image processing. Although it is a polygon, it may have a shape such as a circular hole in it.

It is a typical side view which shows the structure of the robot apparatus which concerns on this invention. It is a flowchart which shows the advance preparation (a) and control action (b) of the control method of the robot apparatus based on this invention. It is a front view which shows the captured image according to each step in the state which is performing the control method of the robot apparatus which concerns on this invention. It is a perspective view which shows the structure which represented the camera in the robot apparatus which concerns on this invention imitating a pinhole camera. It is a front view which shows the captured image in the state which is performing centering in the control method of the robot apparatus which concerns on this invention. It is a flowchart which shows the procedure of the centering operation | movement in the control method of the robot apparatus which concerns on this invention. It is a flowchart which shows the procedure of the two-dimensional position control and phase alignment in the control method of the robot apparatus which concerns on this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Manipulator 1a 1st link 1b 2nd link 1c 3rd link 1d 4th link 1e 5th link 1f 6th link 2a 1st joint 2b 2nd joint 2c 3rd joint 2d 4th Joint 2e 5th joint 2f 6th joint 3 computer 5 tip tool mechanism 6 camera 101 object

Claims (8)

A robot apparatus control method for controlling a robot apparatus having a manipulator having six degrees of freedom or more and having an imaging means at a hand position of the manipulator,
The imaging of the object when the image of the object obtained by the imaging means and information stored in advance on the shape of the object are collated and the hand of the manipulator is turned. Based on the amount of movement in the means, control the direction of the hand of the manipulator so that the image of the object is located at the center of the field of view of the imaging means,
Next, based on the size of the image of the object obtained by the imaging means by comparing the image of the object obtained by the imaging means with information stored in advance about the shape of the object , Calculating the distance between the hand of the manipulator and the object, moving the hand of the manipulator in the optical axis direction of the lens of the imaging means, and bringing the hand closer to the object ;
Next, the image of the object obtained by the imaging unit is compared with information stored in advance about the shape of the object, and the direction of the hand of the manipulator is perpendicular to the measurement surface of the object. Direction,
Next, as an image of the object within the field of view of the imaging means is a predetermined direction, the control method for a robot apparatus, characterized in that you rotate controlling hand of the manipulator. The method for controlling a robot apparatus according to claim 1, wherein the object has a feature amount of a circle or a shape approximated to a circle, and the upper surface is a measurement surface. The robot according to claim 1, wherein the object has an arbitrary shape other than a circle, and has a measurement surface having a feature quantity of a circle or a shape approximated to a circle. Control method of the device.   Controlling the direction of the hand of the manipulator to position the image of the object at the center of the field of view of the imaging means;
  The center C0 of the field of view on the image obtained by the imaging means and the initial center position C1 of the object on the image are acquired, and the object on the image is rotated by rotating the selected operation axis. The center position C2 is obtained, and the distance and inclination between the straight lines C0-C1 and the straight lines C1-C2 are obtained. Next, a straight line passing through the visual field center C0 perpendicular to the straight line C1-C2 and the straight line C1-C2 By determining the intersection point A and determining the distance between the straight line C0-A and the straight line C2-A, the rotation amount of the joint of the manipulator around the selected operation axis is determined,
  Next, the operation axis orthogonal to the selected operation axis is rotated to obtain the center position C2 of the object on the image, and the distance and inclination between the straight lines C0-C1 and between the straight lines C1-C2 are obtained. Next, an intersection A between the straight line C1-C2 and a straight line that is perpendicular to the straight line C1-C2 and passing through the visual field center C0 is determined, and the distance between the straight line C0-A and the straight line C2-A is obtained. Determine the amount of rotation around the motion axis,
  The manipulator is controlled based on the determined rotation amounts, and the image of the object is set as the center of the field of view of the imaging means.
  The method for controlling a robot apparatus according to claim 1. A robot apparatus having a manipulator having 6 degrees of freedom or more and having an imaging means at a hand position of the manipulator,
Control means for controlling the operation of the manipulator,
The control unit compares the image of the object obtained by the imaging unit with information stored in advance about the shape of the object, and rotates the direction of the hand of the manipulator. The direction of the hand of the manipulator is controlled so that the image of the object is positioned at the center of the field of view of the imaging means based on the amount of movement of the image of the imaging means in the imaging means, and then obtained by the imaging means. The manipulator hand and the object based on the size of the image of the object obtained by the imaging means by comparing the image of the object to be obtained and information stored in advance about the shape of the object and calculates the distance between the object, the hand of the manipulator is moved in the optical axis direction of the lens of the imaging unit, to approximate該手destination on the object, then the image pickup means The image of the object thus obtained is compared with information stored in advance about the shape of the object, and the direction of the hand of the manipulator is set to a direction perpendicular to the measurement surface of the object, as the image of the object within the field of view of the imaging means is a predetermined direction, the robot apparatus characterized by you rotate controlling hand of the manipulator. The robot apparatus according to claim 5 , wherein the object has a feature amount of a circle or a shape approximated to a circle, and the upper surface is a measurement surface. The robot according to claim 5 , wherein the object has an arbitrary shape other than a circle, and has a measurement surface having a feature amount of a circle or a shape approximated to a circle. apparatus.   The control means controls the direction of the hand of the manipulator to position the image of the object at the center of the field of view of the imaging means.
  The center C0 of the field of view on the image obtained by the imaging means and the initial center position C1 of the object on the image are acquired, and the object on the image is rotated by rotating the selected operation axis. The center position C2 is obtained, and the distance and inclination between the straight lines C0-C1 and the straight lines C1-C2 are obtained. Next, a straight line perpendicular to the straight line C1-C2 and passing through the visual field center C0 is obtained. By determining the intersection point A and determining the distance between the straight line C0-A and the straight line C2-A, the rotation amount of the joint of the manipulator around the selected operation axis is determined,
  Next, the operation axis orthogonal to the selected operation axis is rotated to obtain the center position C2 of the object on the image, and the distance and inclination between the straight lines C0-C1 and between the straight lines C1-C2 are obtained. Next, an intersection A between the straight line C1-C2 and a straight line that is perpendicular to the straight line C1-C2 and passing through the visual field center C0 is determined, and the distance between the straight line C0-A and the straight line C2-A is obtained. Determine the amount of rotation around the motion axis,
  The manipulator is controlled based on the determined rotation amounts, and the image of the object is set as the center of the field of view of the imaging means.
  The robot apparatus according to claim 5.

Images