JP2009056513A - Gripping position and attitude determination system and method of determining gripping position gripping attitude - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To maximally prevent a gripping position and a gripping attitude from coming out of the movable ranges of an arm and a hand. <P>SOLUTION: This method of determining a gripping position and a gripping attitude comprises the steps of: determining whether the gripping position and the gripping attitude of an object to be gripped come out of the movable range of the arm having at least two joints and a gripping mechanism attached to the end of the arm according to the position and the attitude of the object to be gripped, measured by a position and attitude measuring sensor, and to object shape information stored in a database; calculating whether there is a gripping position and a gripping attitude corrected within the movable ranges of the arm and the gripping mechanism by correcting the gripping position and the gripping attitude when the gripping position and the gripping attitude come out of the movable ranges of the arm and the gripping mechanism; and controlling the arm and the gripping mechanism so that the gripping position and the gripping attitude are brought into the corrected gripping position and the gripping attitude when there is the corrected gripping position and corrected gripping attitude. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a gripping position / posture determination system and a gripping position / posture determination method.

2. Description of the Related Art Conventionally, an operation has been performed in which a robot grips and transports gripping objects of various shapes arranged at arbitrary positions and orientations. One method for easily calculating the gripping position and orientation of a gripping object having various shapes is to apply the gripping position and orientation defined for each basic solid to the gripping object by applying the gripping object to the basic solid. A method is known (see, for example, Patent Document 1). In the technique described in Patent Document 1, the relative position and orientation between the grasped object and the robot hand are calculated using the size of the basic solid, and the joint angle of the robot arm for realizing the relative position and orientation. Is calculated using the relative position and orientation of the hand and the object to be grasped. If the joint angle that realizes a certain gripping position / posture is outside the movable range of the arm, one within the movable range is selected from several calculated relative positions / postures of the gripping object and the hand. Several types of relative position and orientation were obtained by rotating the initially calculated relative gripping position and orientation around the symmetry axis of the basic solid.
JP 2005-169564 A

  However, Patent Document 1 does not describe a countermeasure when all of the calculated several types of gripping position / postures are outside the movable range of the arm and the hand.

  The present invention has been made in consideration of the above circumstances, and a gripping position / posture determination system and a gripping position / posture determination that can avoid the gripping position / posture from being outside the movable range of the arm and the hand as much as possible. It aims to provide a method.

  A gripping position / posture determination system according to an aspect of the present invention measures an arm having at least two joints, a gripping mechanism provided at a tip of the arm for gripping a gripping target, and the position / posture of the gripping target A position and orientation measurement sensor to be detected, a database in which shape information of the gripping object is stored, an output of the position and orientation measurement sensor and shape information stored in the database, It is determined whether or not the arm and the gripping mechanism are out of the movable range, and when the arm and the gripping mechanism are out of the movable range, the arm and the gripping mechanism are movable by correcting the gripping position and posture. It is calculated whether or not there is a corrected gripping position / orientation within the range, and if it exists, the corrected gripping position / orientation is obtained. Characterized in that and a calculation control unit for controlling the arm and the gripping mechanism.

  The gripping position / orientation determination method according to another aspect of the present invention is based on the position / orientation of the gripping object measured by the position / orientation measurement sensor and the shape information of the gripping object stored in the database. Determining whether or not the gripping position / posture of the object is outside the movable range of the arm having at least two joints and the gripping mechanism provided at the tip of the arm; and out of the movable range of the arm and the gripping mechanism If the gripping position / orientation is corrected, the step of calculating whether there is a corrected gripping position / orientation within the movable range of the arm and the gripping mechanism by correcting the gripping position / orientation is corrected. And a step of controlling the arm and the gripping mechanism so as to be in a gripping position and posture.

  According to the present invention, it is possible to avoid the gripping position / posture from being outside the movable range of the arm and the hand as much as possible.

  Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

  A gripping position / posture determination system for a gripping object according to an embodiment of the present invention is applied to a robot shown in FIG. The robot 100 includes a main body 110, an arm 1 having one end attached to the main body 110, a hand 2 provided at the other end of the arm 1, a position / orientation measurement sensor 3, a force sensor 4, and a database 5. And an arithmetic control device 6. The arm 1 is provided with at least two joints and has a mechanism capable of moving the hand 2 to the position and orientation of the grasped object. This mechanism has 6 or more degrees of freedom. The hand 2 has a flange 2a connected to the arm 1 and at least two fingers 2b provided on the flange 2a. The at least two fingers 2b have a mechanism for sandwiching or wrapping the object to be grasped. This mechanism corresponds to, for example, a well-known parallel two-fingered hand or an articulated hand.

  The position / orientation measurement sensor 3 is a sensor that can measure the three-dimensional position / orientation of the object to be grasped, and refers to, for example, a stereo camera or a laser range sensor. The force sensor 4 is used for measuring a gripping force. The site where the force sensor 4 is mounted is provided at a site where the gripping force can be detected, such as the belly of each finger 2 b of the hand 2. As the force sensor 4, for example, a sheet-type pressure sensor or the like is used. The database 5 stores shape information of the gripping object. The stored information will be described later. The arithmetic control device 6 communicates with the arm 1, the hand 2, the position / orientation measurement sensor 3, the force sensor 4, and the database 5 to control the arm 1 and the hand 2.

  In such a robot 100, when the rectangular parallelepiped 60 shown in FIG. 3 is gripped by using a conventional method (for example, the method described in Patent Document 1), the holding surface of the hand 2 and the predetermined rectangular parallelepiped 60 are fixed. The hand 2 is moved and gripped so that the surface and the opposing surface are in a parallel gripping position / posture. When the wrist joint is out of the movable range in the gripping position / posture P1 from the side of the rectangular parallelepiped 60, the gripping position / posture P2 rotated 90 degrees from the gripping position / posture is attempted. In this case as well, if the wrist joint is out of the movable range, an attempt is made to grip with the gripping position / posture P3 rotated 90 degrees from the gripping position / posture P2. In this case as well, if the wrist joint is out of the movable range, an attempt is made to grip with the gripping position / posture P4 rotated 90 degrees from the gripping position / posture P3.

  However, when this conventional method is used, as shown in FIGS. 4 (a) and 4 (b), the wrist joint is out of the movable range in the gripping position / posture P1, P2, and in the gripping position / posture P3, P4 You may not be able to reach your hands at all. As described above, there are cases where all the gripping positions and postures are out of the movable range of the arm 1 and the hand 2. Hereinafter, the arm 1 and the hand 2 are also referred to as a manipulator.

  Therefore, for example, as shown in FIG. 5, the present inventors change the gripping position / posture of the hand 2 from the predetermined surface of the rectangular parallelepiped 60 and the opposing surface thereof from the holding position / posture parallel to the predetermined surface of the rectangular parallelepiped 60. I noticed that if I shifted it to the surface, it would fall within the range of motion. That is, in this embodiment, when the grip position / posture taught or calculated in advance is outside the movable range of the arm 1 and the hand 2, the grip position / posture is shifted (corrected) to hold the grip within the movable range. An arithmetic control device 6 that calculates whether or not a position and orientation exist and controls the arm 1 and the hand 2 is provided.

Next, the definition of the coordinate system will be described with reference to FIG. The robot 100 grasping position and orientation determining system of the present embodiment is applied, the arm coordinate system sigma _a related arm 1, there is a hand coordinate system sigma _h. Arm coordinate system sigma _a specifies the origin to the pedestal of the arm 1. The direction of the coordinate axes is arbitrary. The hand coordinate system Σ _h draws a normal line from the contact surface between each finger of the hand 2 and the object to be grasped, and takes the intersection of the normal lines as the origin point. Take the midpoint of the contact point of the object as the origin. Axes orientation, the z-axis the vector directed from the flange surface 2a of the hand 2 to the origin of the hand coordinate system sigma _h, the vertical vector to the finger 2b drawn from the origin and the x-axis, the z-axis and x-axis and right-handed Let the vector that is formed be the y-axis. In the present specification, the gripping position and orientation refers to the position and orientation of the hand coordinate system sigma _h as viewed from the arm coordinate system sigma _a.

  Next, the configuration of the arithmetic and control unit 6 according to the present embodiment is shown in FIG. The arithmetic control device 6 includes a position / orientation processing unit 61, a gripping position / orientation calculation unit 62, an arm control unit 63, and a hand control unit 64. The position / orientation processing unit 61 processes image data and position / orientation data sent from the position / orientation measurement sensor 3. The gripping position / posture calculation unit 62 calculates the gripping position / posture based on the data processed by the position / posture processing unit 61 and the shape information of the gripping object stored in the database 5. The arm control unit 63 controls the motor 12 via the motor driver 11 of the arm 1 based on the grip position / posture calculated by the grip position / posture calculation unit 62. The hand controller 64 controls the motor 22 via the motor driver 21 of the hand 2 based on the grip position / posture calculated by the grip position / posture calculator 62.

Next, input / output of the gripping position / orientation calculation unit 62 will be described with reference to FIG. The input of the gripping position / orientation calculation unit 62 has three types: sensor information D1, prior knowledge D2 held by the gripping position / orientation calculation unit 62, and knowledge D3 stored in the database 5 by a human before work. The sensor information D1 has a position / orientation D11 of the grasped object. This refers to the three-dimensional position and orientation of the arm coordinate system sigma _a seen from the grasped object. The position and orientation processing unit 61 generates this position and orientation.

  Prior knowledge D2 possessed by the gripping position / orientation calculation unit 62 includes basic solid (to be described later) representation D21, basic solid gripping position / orientation calculation method D22, movable range D23 of arm 1, shape D24 of hand 2, and hand 2 It has a movable range D25.

  The knowledge D3 stored in the database 5 includes a basic solid D31 to which the gripping target object belongs, an approximate size D32 of the basic solid (D31), and a gripping position / posture D33 of the basic solid (D31).

  Next, the prior knowledge D2 possessed by the gripping position / orientation calculation unit 62 will be described in detail.

  The basic solid representation D21 includes the type of basic solid, the definition of parameters of the basic solid, and the symmetry axis / plane / point of the basic solid.

Types of Basic Solids In this embodiment, basic solids are defined as five types: pillars, cones, spheres, rotating bodies, and extruded solids, as shown in FIG. The columns include a cylinder, an elliptical column, a triangular column, a quadrangular column, a pentagonal column, a hexagonal column, a heptagonal column, and an octagonal column. In addition, a nine-pole prism or more is treated as a cylinder. The weight includes a cone, an elliptical weight, a triangular weight, a quadrangular weight, a pentagonal weight, a hexagonal weight, a heptagonal weight, and an octagonal weight. In addition, a nine-sided pyramid or more is treated as a cone. The sphere includes a true sphere (all the values of the main axes are the same value) and an elliptic sphere (at least one of the three main axis values is different). As shown in FIG. 10, the rotating body includes a circular rotating body formed by rotating 360 degrees and a rotating body of n (0 <n <360) degrees. As shown in FIG. 11, the extruded shape includes vertical extrusion and orbital extrusion.

  Columns, cones, and rotating bodies are further divided into convex solids, concave solids, and hollow solids (see FIG. 9). For example, as shown in FIG. 12, the cylinder is divided into a convex cylinder, a concave cylinder (for example, a cup or the like), and a hollow cylinder (for example, a pipe or the like). As shown in FIG. 13, the cone is a convex cone. It is divided into a concave cone and a hollow cone. In the present embodiment, the convex cone includes a solid in which the top of the cone is cut. As can be seen from the above example, in the present embodiment, the concave solid refers to a solid hollowed out to have a certain thickness. Solids that are hollowed out so as to have different thicknesses are considered convex solids, ignoring the hollowed-out parts. The convex solid, the concave solid, and the hollow solid are necessary for determining the direction to shift the gripping position and posture (details will be described later). Although there are concave solid and hollow solid even in the extruded solid, as will be described later in step S6 of FIG. Since the calculation of the contact area is difficult by manual calculation, it will not be handled in this embodiment. Incidentally, a polyhedron approximates a pillar (hexahedron), a cone (tetrahedron), and a sphere (octahedron or more).

  The shape of many objects can be expressed by a simple solid or a combination thereof. Taking tableware as an example as shown in FIG. 14, for example, the bowl is represented by a combination of a concave rotating body, the mug is represented by a combination of a cylinder and an orbital extrusion, and the square dish is represented by a combination of a concave 90 degree rotating body and a concave quadrangular prism, A fork is represented by a combination of two rectangular parallelepipeds.

Basic Solid Parameters FIG. 15 shows some basic solid parameters. For example, a convex quadrangular prism has parameters such as width, depth, and height, a concave pentagonal prism has side length, thickness, and height as parameters, and a true sphere has a parameter as a diameter. For the combination of the hollow cylinder and the concave cylinder, the respective height and diameter are parameters. The extruded body has parameters of track and cross-sectional shape. As the method for specifying the trajectory of the extruded body, a point sequence of the trajectory of the trajectory, a curve / straight line equation, or the like can be considered. If the cross-sectional shape of the extruded body can be expressed as a parameter, the cross-sectional shape is also described. The parameter may be changed in accordance with the accuracy of approximation of the shape of the grasped object. As shown in FIG. 16, if the shape is approximated with high accuracy, the calculated grip position / posture also follows the shape, and if the shape is approximated with low accuracy, the calculated grip position / posture becomes simple. .

FIG. 15 also shows the definition of the basic solid coordinate system _Σb0 . In the basic three-dimensional coordinate system Σ _b0 , the center of gravity overlaps in the + direction of the z axis, and an object having a symmetric axis such as a cylinder overlaps the symmetric axis and the z axis. The x and y axes of the basic solid coordinate system _Σb0 are defined so as to include the bottom surface of the object in the case of a basic solid with a bottom surface. Further, in the case of a concave object, it is defined that the concave portion exists in the z-axis direction. Also, when expressing an object in a plurality of primitive defines the object coordinate system, the position and orientation of the primitive coordinate system sigma _b0 viewed from the object coordinate system is defined.

Symmetry axis / symmetry plane / symmetry point of basic solid FIG. 17 shows several examples of symmetry axis / symmetry plane / symmetry point of basic solid. In the case of a prism, a pyramid, and an n-degree rotating body, the symmetry is made every n degrees around the symmetry axis (in the case of a hexagonal cylinder, every 60 degrees). For example, in the case of a concave rotating body, the rotation axis is the axis of symmetry. The convex quadrangular column has three symmetry planes, the concave quadrangular column has two symmetry planes, and the center of the true sphere is a symmetric point. Since the extruded body has different symmetry axes / surfaces / points depending on the shape and trajectory of the extruded surface, a specific symmetry axis / surface / point is designated for each extruded body.

  The gripping position / orientation calculation method D22 shown in FIG. 8 may be any method. Since the gripping position / orientation determination system according to the present embodiment uses basic solid information, a method using the geometric characteristics of the basic solid is also suitable for the gripping position / orientation determination system according to the present embodiment. The manipulator movable range D23 specifies a minimum angle and a maximum angle for each joint axis. The shape D24 of the hand 2 is used to calculate the interference between the hand 2 and the grasped object. The movable range D25 of the hand 2 is used to calculate a range in which the gripping position / posture can be shifted. For parallel two-fingered hands, specify the maximum and minimum grip width.

  The information creator can arbitrarily select the basic solid to which the gripping object belongs. One or a plurality of basic solids can be designated, but it is noted that in this embodiment, one basic solid is gripped at that time. For example, if it is desired to grip between the handle of the fork and the tip (part divided into three parts), the handle and the tip are defined as one basic solid. Also, if you want to grip all the tips of the fork, define the three tips of the fork as one rectangular parallelepiped. The numerical value of the parameter of each basic solid is input to the approximate size of the grasped object. The gripping position / posture is used when the above-described gripping position / posture calculation method is not installed.

  Next, the determination process of the gripping position / orientation determination system of this embodiment will be described with reference to FIG.

  First, the gripping object is applied to the basic solid (step S1). The fitting is performed by the position / posture processing unit 61 processing the position / posture of the grasped object obtained from the position / posture measurement sensor 3. Subsequently, the gripping position / posture calculation is performed by the gripping position / posture calculation unit 62 (step S2). Thereafter, it is determined whether the joint angle of the arm 1 that realizes the gripping position and orientation calculated in step S2 is within the movable range or out of the movable range (step S3). This determination is performed by the gripping position / orientation calculation unit 62. When it is determined that it is within the movable range, the process proceeds to step S4, and when it is determined that it is outside the movable range, the process proceeds to step S6.

  If it is determined to be within the movable range, the arm 1 is controlled via the arm control unit 63 so that the gripping position / posture is within the movable range (step S4). Thereafter, the process proceeds to step S5, and the hand 2 is controlled via the hand control unit 64 so that the grip width calculated by the grip position / orientation calculation unit 62 is obtained. At this time, all the fingers start to move simultaneously. In order to cope with the case where the gripping position / posture is deviated from the position / posture with zero error or the gripping width is deviated from the zero-error width after moving to the gripping width, the force sensor 4 is used for gripping. Make fine adjustments to the width. In this way, the object to be grasped is grasped.

  When it is determined that it is out of the movable range, the steps after step S6 are performed. In these steps, the position and orientation within the movable range are calculated by shifting the gripping position and orientation calculated in step S2. Shifting the gripping position / posture refers to changing the gripping position / posture so that the shift amount (x, y, z, rx, ry, rz) is based on the current gripping position / posture. Here, x, y, and z indicate shift amounts of the reference coordinate system in the x-axis direction, y-axis direction, and z-axis direction, respectively. Rx, ry, and rz are rotations around the x-axis of the reference coordinate system, This refers to rotation around the y-axis of the reference coordinate system and rotation around the z-axis of the reference coordinate system. Here, the reference coordinate system means a coordinate system before rotation, and is not a coordinate system after rotation such as a known roll, pitch, and yaw rotation.

  Since there are six types (six components) of freedom of shift amount, the types of shifted gripping position / posture become enormous. In this embodiment, the search space with a large gripping position / posture is divided and searched. For this reason, in this embodiment, the calculation amount of search can be reduced. In this embodiment, instead of shifting all six components at once, the search is performed by shifting several components. In step S6 of the present embodiment, the six components are divided into two sets of three components. The method of dividing will be described later. Subsequently, only one set of three components out of the two divided sets of the six components is shifted (step S7). Thereafter, in step S8, it is determined whether or not the shifted gripping position / posture is a new gripping position / posture. If it is determined as a new gripping position / posture, the process proceeds to step S10 described later. If it is determined as a new gripping position / posture, the process proceeds to step S9, and it is determined whether the manipulator is within the movable range or out of the movable range.

  If it is determined in step S9 that it is the movable range, the process proceeds to step S4 and the above-described processing is performed. If the grip position / posture within the movable range of the manipulator cannot be found, the process proceeds to step S10. In step S10, the remaining three components are shifted. In step S11, it is determined whether or not the shifted gripping position / posture is a new gripping position / posture. If it is determined in step S11 that the gripping position / orientation is a new gripping position, the process proceeds to step S12. In step S12, it is determined whether or not the shifted gripping position / orientation is within the movable range of the manipulator. If it is determined in step S12 that it is within the movable range, the process proceeds to step S4, and the processing described above is performed. If it is determined in step S12 that it is out of the movable range, the process returns to step S10 and the above-described processing is performed. If it is determined in step S11 that the gripping position / posture is not a new gripping position / posture, the process proceeds to step S13, and the gripping position / posture determination process is terminated because the gripping target object cannot be gripped.

  Next, the process of each step shown in FIG. 1 will be specifically described.

ここで、Ｒは可動範囲外の関節の集合とする。 Calculation of gripping position and orientation (step S2)
The gripping position / orientation is calculated by an arbitrary method. If the joint angle when the arm 1 realizes the calculated gripping position and orientation is outside the movable range of the arm 1, the symmetry axis / axis of the basic solid to which the part where the hand 2 and the gripping object are in contact belongs. Move the gripping position / posture around the symmetry plane / symmetry point and check if it is within the movable range. If all the gripping position / postures are out of the movable range, the gripping position / posture with the smallest sum θ _over the difference between the joint angle at the movable limit and the joint angle realizing the gripping position / posture is selected. This sum θ _over is expressed by the following equation.

Here, R is a set of joints outside the movable range.

Divide 6 position and orientation components into 2 sets (step S6)
As an index used for division, a contact area between the hand 2 and the grasped object is used. In the present embodiment, it is defined that the higher the contact area between the hand 2 and the object to be grasped, the more stable it can be grasped (without tilting the object during grasping and without dropping the object during conveyance). The definition of the contact area is defined as the contact area when the hand 2 and the object to be gripped start to contact when the hand 2 is closed to the grip width calculated by the grip position / posture calculator 62. Examples of contact areas are shown in FIGS. 18 (a) to 18 (c). In FIG. 18 (a) to FIG. 18 (c), the finger 2b of the hand 2 before the gripping operation is indicated by hatching. In FIG. 18A, since the positional deviation between the contact surface of the two fingers 2b of the hand 2 during the grip operation in the ideal state is zero, the hand 2 and the object to be gripped contact each other during the grip operation. Therefore, the contact area is larger than those shown in FIGS. 18B and 18C described later. On the other hand, in FIG. 18B, the hand 2 is inclined as compared with the case of FIG. Therefore, the contact area is small compared to the case of FIG. In FIG. 18C, since the position of the hand 2 is shifted, one finger 2b of the hand 2 and the grasped object come into contact with each other during the grasping operation. Therefore, the contact area is small compared to the case of FIG.

FIG most contact area is large gripping position and orientation calculated 1 in step S2 shown in, and the origin of the hand coordinate system sigma _h Assuming the possible matches with the origin of the primitive coordinate system sigma _b0, be described as follows .

The procedure for dividing the six components of the shift amount into two has the following three steps.
1. The position and orientation of the hand 2 are shifted in the ± direction of a certain component with reference to the gripping position and orientation calculated in step S2.
2. A change in the contact area between the hand 2 and the object to be grasped before and after the shift is checked.
3. Divide into three components from components with little reduction in contact area.

  Next, a detailed procedure of division will be described with reference to FIGS.

In order to determine the direction to shift, a new coordinate system Σ _sft is set in step S601 shown in FIG. An example of a method for setting the coordinate system Σ _sft is shown in FIG. First, define the hand coordinate system sigma _h and primitive coordinate system sigma _bo (Fig 20 (a)). Subsequently, the origin of the new coordinate system Σ _sft is set to the current gripping position (FIG. 20B). The direction of each axis of the new coordinate system Σ _sft is matched with the basic solid coordinate system Σ _bo or the hand coordinate system Σ _h (FIG. 20B). Thereafter, when shifting the hand 2 in the ± z-direction of the hand coordinate system sigma _h (FIG. 20 (c)) and, when shifting the hand 2 in the ± z direction of the primitive coordinate system sigma _bo (FIG. 20 ( The change in the contact area is examined for the two types d)), and the z-axis of the coordinate system with a small decrease is adopted as the z-axis of the new coordinate system Σ _sft . The range to be shifted is a range in which the antinode of the hand 2 and the basic solid are in contact with each other and the surface other than the antinode is not in contact with the basic solid.

  In this embodiment, as shown in FIG. 21, the gripping type is (1) when gripping the opposing planes of the convex part (side surfaces of the rotator, top and bottom surfaces of pillars and cones, etc.) (2 ) When gripping the concave portion, it is roughly divided into three types: (3) gripping a plane where the convex portion does not face. Further, the case (1) is divided into two cases: (1-1) gripping the side surfaces and (1-2) gripping the bottom and top surfaces. Incidentally, when there is no plane such as the side surface of the rotating body or the top surface of the cone, the side surfaces are gripped as shown in grip (1-1). When the upper surface and the bottom surface exist, the upper surface and the bottom surface may be gripped as shown in grip (1-2).

  In the present embodiment, the change is classified into the following four stages in order to roughly grasp the change in the contact area between the top surface and the bottom surface. (A) The contact area increases when the direction is changed in the plus direction and the minus direction. (B) The contact area does not change at all. (C) The contact area increases or decreases by a predetermined value or more. (D) The contact area is reduced. (C) can be regarded as the same as (B) when the total increase or decrease of the contact area is less than or equal to a predetermined value. Therefore, in order of decreasing contact area decrease, the first is (A), the second is (B) and (C), and the third is (D).

  Next, the change in the contact area when the column was gripped was examined. As shown in FIG. 22, when gripping (1-1), the hand coordinate system is (B: cylinder) or (C: prism), and the basic solid coordinate system is (B). The amount of reduction is the same for both coordinate systems. When gripping (1-2), the hand coordinate system is (C) and the object coordinate system is (D), and the amount of decrease in the contact area of the hand coordinate system is smaller. When gripping (2), the hand coordinate system is (B) and the object coordinate system is (B), and the contact area reduction amount is the same in both coordinate systems. When gripping (3), the hand coordinate system is (B) and the object coordinate system is (D), and the amount of decrease in the contact area of the hand coordinate system is smaller.

  Therefore, in the case of gripping (1-2) or gripping (3), the z axis of the hand coordinate system is set as the z axis of the new coordinate system. When both the coordinate systems have the same amount of decrease, such as grip (1-1) or grip (2), the z-axis of the basic solid coordinate system is replaced with the z-axis of the new coordinate system regardless of the type of the basic solid. To do. The reason is that if the basic three-dimensional coordinate system is adopted, the balance between the hand 2 and the object to be grasped during transportation can be kept more stable.

Next, the relationship between the coordinate system and the shift amount of the center of gravity will be described with reference to FIGS. Consider a case where a gripping position and orientation as shown in FIG. 23A is calculated in step S2 shown in FIG. 1 because the contact area between the hand 2 and the gripping object is large. At this time, if you set the coordinate system orient the z-axis of the new coordinate system sigma _sft the z-axis of the hand coordinate system sigma _h, as shown in FIG. 23 (b), the hand coordinate system sigma _h When the hand 2 is shifted in the direction of a certain axis (for example, the z-axis), the amount of shift of the center of gravity before and after the shift increases in any translation direction (x, y direction). Therefore, no matter what posture the gripping object is conveyed, the balance between the hand 2 and the gripping object is lost.

On the other hand, as shown in FIG. _23C , when the z axis of the new coordinate system Σ _sft is aligned with the z axis of the basic solid coordinate system Σ _bo , for example, the z axis of Σ _sft of the new coordinate system When the hand 2 is shifted in the direction, the translational direction (x, y direction) in which the shift amount of the center of gravity before and after the shift is not shifted becomes zero. Therefore, when the gripping object is moved while maintaining the posture shown in FIG. 23 (a), the balance between the hand 2 and the gripping object may not be lost during transport, as shown in FIG. 23 (c).

Cone, as shown in variation diagram 24 of the contact area in the case of gripping the rotating body, when the gripping of (1-1), in the case of the hand coordinate system sigma _h (B: conical, circular rotor) or (C: In the case of the basic solid coordinate system Σ _bo (B), the reduction amount of the contact area is the same in both coordinate systems. When gripping (1-2), in the case of the hand coordinate system sigma _h (C), and the case of the primitive coordinate system sigma _bo (D) becomes, the lesser the reduction in the contact area of the hand coordinate system. When gripping of (2), in the case of the hand coordinate system sigma _h (B), and the case of the primitive coordinate system sigma _bo (D) becomes, the lesser the reduction in the contact area of the hand coordinate system. When gripping (3), in the case of the hand coordinate system sigma _h (B), and the case of the primitive coordinate system sigma _bo (D) becomes, the lesser the reduction in the contact area of the hand coordinate system.

  Therefore, when gripping a cone or a rotating body, in the case of gripping (1-2), (2), and (3), the z axis of the hand coordinate system is set as the new coordinate system z axis. When both the coordinate systems have the same amount of decrease as in gripping (1-1), the z-axis of the basic three-dimensional coordinate system is set as the z-axis of the new coordinate system.

As shown in variation diagram 25 of the contact area in the case of gripping the ball, when the gripping of (1-1), in the case of the hand coordinate system sigma _h (B), and the case of the primitive coordinate system sigma _bo (B ), And the contact area reduction amount is the same in both coordinate systems. When gripping (1-2), in the case of the hand coordinate system sigma _h (C), and the case of the primitive coordinate system sigma _bo (D) becomes, the lesser the reduction in the contact area of the hand coordinate system. The grip (2) is not considered because it does not assume a concave solid body of a sphere. When gripping (3), in the case of the hand coordinate system sigma _h (B), and the case of the primitive coordinate system sigma _bo (D) becomes, the lesser the reduction in the contact area of the hand coordinate system.

  Therefore, when gripping the sphere, in the case of gripping (1-2) or gripping (3), the z-axis of the hand coordinate system is set as the z-axis of the new coordinate system. When both coordinate systems have the same amount of decrease as in gripping (1-1), the z-axis of the basic three-dimensional coordinate system is used as the z-axis of the new coordinate system in the case of a column or weight. However, in the case of a sphere, the shift range in the z-axis direction of the basic solid coordinate system is significantly smaller than the shift range of the hand coordinate system. When this is shifted in the z-axis direction of the basic three-dimensional coordinate system, the hand 2 is gripped by the edge of the abdominal surface and the upper surface or the bottom surface immediately after the shift. Edge-only contact is not included in the shift range. For this reason, as a special case, the hand coordinate system is the z-axis of a new coordinate system. Incidentally, the shape of the hand 2 assumes that the finger length is longer than the finger width.

Change in contact area when holding an extruded solid
(A) In the case of a vertically extruded solid As shown in FIG. 26, the positions and postures in which the gripping position and posture can be shifted among the gripping positions and postures calculated in step S2 of FIG. It is limited to two types: a gripping position / posture approached by 2 and (ii) a gripping position / posture having a top surface and a bottom surface. The reason is that the contact area varies depending on the cross-sectional shape in the case of the gripping position and posture approached from the upper surface of the vertical extrusion solid. In detail, the shape of the cross section of a vertically extruded object is complicated (an extruded solid with a simple cross section is regarded as a column), so the shape of the side surface becomes complicated, and when it is approached from the top surface, it has which part of the side surface This is because the contact areas are different and it is difficult to separate the three components. In addition, since the extruded solid does not consider a concave solid, gripping in which the contact relationship between the hand 2 and the gripping object is (2) is not considered. Therefore, only the grip (1-1), the grip (1-2), and the grip (3) are considered in the contact relationship between the hand 2 and the gripping object. When the shape of the cross section is not specified and the grip (1-1) or the grip (3) is not known, it is regarded as the grip (3).

  Next, the change in the contact area between the vertically extruded solid and the hand 2 when the hand 2 is shifted in the z-axis direction of the hand coordinate system and the z-axis direction of the vertical extruded solid coordinate system will be described with reference to FIG. I will explain. When gripping (1-1), that is, gripping (i), the increase / decrease of the contact area is (C) when shifted in the z-axis direction of the hand coordinate system, and shifted in the z-axis direction of the vertical extrusion solid coordinate system When it is made, it becomes (B). Since the amount of contact area reduction is the same in the z-axis direction of the hand coordinate system and the z-axis direction of the vertical extrusion solid coordinate system, the z-axis of the vertical extrusion solid coordinate system is changed to The z axis is assumed.

  When gripping (1-2), that is, gripping (ii), the increase or decrease of the contact area is (C) when shifted in the z-axis direction of the hand coordinate system, and shifted in the z-axis direction of the vertical extrusion three-dimensional coordinate system. (D) when Since the reduction amount of the contact area is smaller when shifted in the ± z-axis direction of the hand coordinate system, the z-axis of the hand coordinate system is set as the z-axis of the new coordinate system.

  When gripping (3), that is, gripping (i), the amount of decrease in contact area is (B) when shifted in the z-axis direction of the hand coordinate system, and shifted in the z-axis direction of the vertical extrusion three-dimensional coordinate system. In case, it becomes (B). Since the reduction amount of the contact area is the same in the hand coordinate system and the vertical extrusion solid coordinate system, the z axis of the vertical extrusion solid coordinate system is taken as the z axis of the new coordinate system.

(B) In the case of an orbital extrusion solid In the present embodiment, the orbital extrusion solid is defined as a set of minute vertical extrusion solids. Next, among the gripping positions and postures calculated in step S2 shown in FIG. . As shown in FIG. 28, the gripping position / posture at which the hand 2 comes into contact with a plurality of vertical extrusion solids as in the case of having the left end and the right end of the handle of the umbrella is not a shift target.

  Since the top and bottom surfaces of the micro-extruded object cannot be gripped, the grip (1-2) indicating the contact relationship between the hand 2 and the gripping object is not considered. In addition, since the extruded solid does not consider a concave solid, gripping (2) is not considered. Therefore, only the grip (1-1) and the grip (3) are considered in the contact relationship between the hand 2 and the gripping object. If the shape of the cross section is not specified and gripping (1-1) or gripping (3) is unknown, it is regarded as gripping (3). Then, the change in the contact area between the vertical extruded solid and the belly of the hand 2 when the hand 2 is shifted in the z-axis direction of the hand coordinate system and the z-axis direction of the vertical extruded solid coordinate system is examined.

  When gripping (1-1), the change in the contact area is the same as that of gripping (1-1) of the vertical extruded body. Therefore, the z axis of the vertical extrusion solid coordinate system is set as the z axis of the new coordinate system.

  When gripping (3), the change in contact area is the same as in (3) of the vertical extruded body. Therefore, the z axis of the vertical extrusion solid coordinate system is set as the z axis of the new coordinate system.

Next, the setting of the x and y axis directions of the new coordinate system will be described with reference to FIGS. 29 (a) to 29 (c). First, as shown in FIG. 29 (a), defined a hand coordinate system sigma _h, the primitive coordinate system sigma _b0. As shown in FIG. 29 (b), if the combined z-axis of the new coordinate system sigma _sft the z-axis of the hand coordinate system sigma _h, x-axis in the new coordinate system sigma _sft, y-axis direction is a hand coordinate system x-axis of Σ _h, the same as the direction of the y-axis. As shown in FIG. 29 (c), if the combined z-axis of the new coordinate system sigma _sft the z-axis of the reference three-dimensional coordinate system sigma _b0, the direction of the x-axis of the new coordinate system sigma _sft, hand coordinate system sigma The x-axis of _h is matched with the direction of the vector projected onto the xy plane of the reference solid coordinate system. The y-axis is set so that the new coordinate system is a right-handed system.

  As described above, the coordinate system is set in step S601 shown in FIG.

Next, using the set coordinate system, the six components of the gripping position and orientation calculated in step S2 in FIG. 1 are divided into two sets of three components. At the time of division, the above-mentioned contact area (between the gripping object and the hand 2) is checked. The check method is shown below. The contact area when one component of the six components (x, y, z, rx, ry, rz) of the new coordinate system Σ _sft is changed in the plus direction and the minus direction is the contact area in the gripping position / posture before the shift. Compare whether it has increased or decreased. The range to be shifted is a range in which the antinode of the hand 2 and the basic solid are in contact with each other and the surface other than the antinode is not in contact with the basic solid. Increase / decrease in contact area when changing in the positive and negative directions is classified into the following four stages. (A) The contact area increases. (B) The contact area does not change by a predetermined value or more. (C) The contact area increases or decreases by a predetermined value or more. (D) The contact area is reduced. In this comparison, when (a) x component is changed, (b) y component is changed, (c) z ..., (d) rx, (e) ry ..., (f) rz component is changed. This is performed six times (steps S602, S603, S604, S605, and S606 in FIG. 19).

  Based on this comparison result, the cases (a) to (f) are rearranged in the order of (A), (B), (C), (D), that is, in the order of decreasing contact area (in FIG. 19). Step S607). Thereafter, the upper three components are set as the first group, and the lower three components are set as the second group (step S608 in FIG. 19). If there are things of the same order and they cannot be divided into three components, the three components are appropriately selected from those of the same order of four or more components. Moreover, you may make another division | segmentation of 2 components x3.

  By applying the above dividing method, the contact area between the hand 2 and the object to be gripped is large (in other words, the gripping stability is high and can be gripped stably), and the contact area is small (in other words, stable). In the shift direction). As described above, in this embodiment, by searching for a solution by shifting the three components in the direction in which the contact area is large, the search can be performed with priority from the gripping position and posture that can be stably gripped in the next step. That is, in the present embodiment, first, the first three components are shifted to find the grip position / posture within the movable range (steps S7 to S9).

  FIG. 30 shows a state where the three components are shifted to search for the gripping position and posture within the movable range.

The gripping position / posture is shifted starting from the gripping position / posture with the smallest sum θ _over calculated in step S2. The shift direction is the direction of the coordinate system determined in step S6 (step of dividing the six position / orientation components into two). The range to be shifted is a range in which the antinode of the hand 2 and the basic solid are in contact with each other and the surface other than the antinode is not in contact with the basic solid. In the case where the object to be grasped is composed of a plurality of basic solids, it is desirable to check the contact with a basic solid other than the currently touching basic solid and reflect it in the shift range.

  As a search method, a depth-first search (vertical search) considering cost is adopted. This method is a method of selecting a direction with low cost (in this embodiment, a direction closer to the movable range) to advance the search, and retreating one search point when the dead end is reached. The search procedure of this search method will be described with reference to FIG.

  Steps S7101 to S7105 in FIG. 31 are phases for checking whether the current search point is within the movable range, and steps S7201 to S7406 are phases for determining the next search point (in other words, determining the search direction). Steps S7201 to S7209 check a cell adjacent to the current search point. Steps S7301 to S7305, which are executed when the search has reached a dead end, move the search point backward. Steps S7401 to S7406 executed when the search point is moved backward when no unsearched cell exists causes the search point to jump to an unsearched point that is not adjacent.

First, let the search point be sp _i . At the start of the search, the search start point sp ₀ is substituted into the search point variable sp _i (step S7101). Next, the sum θ _over (referred to as θ _{over_i} ) of the absolute value of the joint angle at the current search point sp _i and the joint angle at the limit of movement and the joint angle that realizes the gripping position and posture is calculated (step S7103). _For the calculation of the sum θ _{over_i} , the joint angle of the arm 1 that realizes the gripping position and orientation indicated by the current search point sp _i is used. If the sum θ over — _i is 0 (all joint angles are within the movable range), the gripping position / orientation indicated by the search point sp _i is taken as a solution, and the search is terminated (steps S7104 to S7105).

If the sum θ _{over — i} is not 0 in step S 7104, search points sp _ij (up to 6 points) adjacent to the search point sp _i are defined (step S 7201). Then, an unsearched one of the adjacent search points sp _ij is set as sp _ijy (step S7202). If the number of elements of the unsearched point sp _ijy is not 0, the sum θ _over (θ _{over_ijy} ) at the unsearched point sp _ijy is calculated (step S7204). If the number of elements is 0, the search point is moved backward by one (step S7301). Then, the minimum value of the sum θ _{over_ijy at} the unsearched point sp _ijy is set as θ _{over_ijy_min} (step S7205). If less than the sum theta _{Over_i} at the minimum value theta _{Over_ijy_min} is currently search point sp _i of the sum (step S7206), unsearched point sp _Ijy corresponding to the minimum value theta _{Over_ijy_min} the sum solutions than the current search point sp _i It is determined that the position is close to (gripping position / posture within the movable range). The next search point is set as an unsearched point sp _ijy corresponding to the sum minimum value θ _{over_ijy_min} (steps S7208 and S7209), and the process returns to step S7102.

If the minimum value theta _{Over_ijy_min} is theta _{Over_i} greater than the sum, or equal, it is determined that all of the search points adjacent to the current search point sp _i Do away from the solution, or unchanged. The unsearched point sp _ijy has been searched (step S7207), and the search point is moved backward by one (step S7301). A result of the retraction of the search point, if it becomes i <0 in step S7303, all of the search points adjacent to the search starting point sp ₀ from the current search point sp _i is in the search already. Therefore, the search point jumps to a point that is not adjacent to these search points. The jump method is shown below.

First, an unsearched point sp _min having the closest distance (grid distance) from the search start point sp ₀ is selected (step S7401). When the number of elements of the unsearched point sp _min is one, the unsearched point sp _min is set as a new search point (steps S7403 and S7404). If there are two or more elements of the unsearched point sp _min (step S7402), the unsearched point sp _min that minimizes the sum θ _{over_i} is adopted (steps S7405 and 7406).

If no search point with θ _over = 0 is found even after searching all search points in the search space, no solution is set (step S7305), and the search is terminated.

  If the search procedure described above is followed, search points close to the movable range can be intensively searched, so that the gripping position and posture within the movable range can be found with low search costs.

When shifting in the rx direction (rotating around the x axis of Σ _sft ), if the translation component is not 0, the translation component is shifted as shown in FIG. In order to prevent this, when the rotation components (rx, ry, rz) are shifted, the temporarily set coordinate system Σ _sft is moved to the gripping center and rotated around the coordinate system.

If the gripping position / posture cannot be found within the movable range even if the first three components are shifted by the above search method, the process proceeds to step S10 in FIG. 1, the second three components are shifted, A grip position / posture within the movable range is found (steps S10 to S12). The starting point of the search at this time is set to the gripping position / posture P ₁ ^* closest to the movable range, which is found in the search by shifting the first three components (steps S7 to S9). For example, when the first three components are the rx direction, the z direction, and the y direction, the position and orientation closest to the movable range is P ₁ ^* = (0, y ^* , z ^* , rx, 0, 0). Similar to the search for the first three components, the second three components are searched. If a search point with θ _over = 0 is found, it is determined as a solution and the search is terminated.

If no search point with θ _over = 0 is found even after searching all the search points, the search start point is changed. The change point is the search for the first three components, and the grip position / posture p ₂ ^* closest to the movable range is set. Search again after changing. If a search point with θ _over = 0 is not found even when the gripping position / posture P ₂ ^* is the starting point, the starting point is changed to gripping position / posture P ₃ ^* , P ₄ ^* ,. If no search point with θ _over = 0 is found even when all gripping position / postures P _i ^* are set to start points (i = 1, 2,...), No solution is found and the search is terminated.

  As described above, according to the present embodiment, there is a high possibility that an alternative gripping position / posture can be determined even when the calculated gripping position / posture is outside the movable range of the manipulator. For this reason, it is possible to avoid the gripping position / posture from being outside the movable range of the arm and the hand as much as possible. In particular, in the case of a fixed manipulator, since the movable range is narrower than that of a moving manipulator, the advantage of being able to calculate an alternative gripping position and orientation is great.

  In the case of many moving manipulators, when the manipulator is out of the movable range, the grip position / posture calculated by moving the carriage is attempted. However, since most of the cases where the carriage is moved include a movement error, it is necessary to remeasure the relative position and orientation between the object and the hand. Here, if this embodiment is used, the movement of the carriage and the re-measurement of the position and orientation can be avoided as much as possible, so that the time required for these can be saved.

  Furthermore, when a robot performs a handling operation such as cleaning up a desk in a home or office environment, the user of the robot is not a robot expert. Many people who are not robot specialists expect robots to make rational movements equivalent to humans, so rational movements of robots working at home or office are important. On the other hand, when a human is not able to take an optimal gripping position and posture when gripping an object to be gripped, the arm is often moved to a sub-optimal gripping position and posture without moving the foot. In many cases, the carriage is moved to achieve only the optimum gripping position and posture. Here, when this embodiment is used, the robot can execute a movement similar to a human movement. Therefore, the gripping position / posture determination system according to this embodiment is useful for a robot working in a home or office.

The flowchart which shows the determination process of the holding | grip position / posture determination system by one Embodiment of this invention. 1 is a schematic diagram showing a robot to which a gripping position / posture determination system according to an embodiment is applied. FIG. The figure explaining the holding method of a prior art. The figure explaining the problem of a prior art. The figure explaining the shift of the grip position posture of the grip position posture determination system of one embodiment. The figure explaining the coordinate system of an arm and a hand. The block diagram which shows the control system used for the determination of the holding position attitude | position of one Embodiment. The figure explaining the input-output to the holding | grip position and orientation calculation part which concerns on one Embodiment. The figure which shows the example of the basic solid used for one Embodiment. The figure explaining the example of a rotary body. The figure which shows an example of an extrusion body. The figure which shows the example of a convex cylinder, a concave cylinder, and a hollow cylinder. The figure which shows the example of a convex cone, a concave cone, and a hollow cone. The figure which shows an example of the expression of the tableware using a basic solid. The figure which shows the example of the parameter of a basic solid. The figure explaining the variation of the parameter of a basic solid. The figure explaining the example of the symmetry axis / symmetry surface / symmetry point of a basic solid. The figure explaining the difference in the contact area by the positional relationship (gripping state) of a hand and a holding | grip target object. The flowchart which shows the procedure which divides six position and orientation components into two. The figure explaining the setting method of new coordinate system (SIGMA) _sft . The figure explaining the kind of holding | grip. The figure explaining the change of the contact area of a rectangular parallelepiped by the difference in holding | grip. The figure explaining the relationship between the definition of a new coordinate system, and the deviation | shift amount of a gravity center. The figure explaining the change of the contact area of the rotary body by the difference in holding | grip. The figure explaining the change of the contact area of the ball | bowl by the difference in a holding | grip. The figure explaining the holding position attitude | position handled with a vertical extrusion object. The figure explaining the change of the contact area of the vertical extrusion object by the difference in a holding | grip. The figure explaining the gripping position attitude | position handled with a track | orbit extrusion object. The figure explaining the definition of the direction of the axis of a new coordinate system. The figure explaining the outline | summary of a shift. The flowchart which shows the search procedure of a holding | grip position / posture. The figure explaining the necessity to move temporarily of new coordinate system (SIGMA) _sft .

Explanation of symbols

1 Arm 2 Hand 2a Flange 2b Finger 3 Position / Orientation Measurement Sensor 5 Database 6 Arithmetic Control Device 100 Robot 110 Main Body

Claims (7)

An arm having at least two joints;
A gripping mechanism provided at the tip of the arm for gripping a gripping object;
A position and orientation measurement sensor for measuring the position and orientation of the gripping object;
A database storing shape information of the gripping object;
Based on the output of the position and orientation measurement sensor and the shape information stored in the database, it is determined whether or not the grip position / posture regarding the grip target is outside the movable range of the arm and the grip mechanism, If it is outside the movable range of the gripping mechanism, it is calculated by correcting the gripping position / posture to calculate whether there is a corrected gripping position / posture within the movable range of the arm and the gripping mechanism. An arithmetic and control unit for controlling the arm and the gripping mechanism so that the corrected gripping position and posture are
A gripping position / posture determination system comprising: The database stores a plurality of types of basic solids, a symmetry plane / symmetric axis / symmetry point of the basic solids, a movable range of the arm, a shape and a movable range of the gripping mechanism,
The arithmetic and control unit regards the grasped object as a single basic solid selected from a plurality of the basic solids or a virtual solid combining a plurality of basic solids based on the output of the position and orientation measurement sensor, The gripping position / posture determination system according to claim 1, wherein the corrected gripping position / posture is calculated based on a basic solid or the virtual solid.   The gripping position / posture determination system according to claim 1 or 2, wherein the correction of the gripping position / posture is performed based on a size of a contact area between the gripping mechanism and the gripping object.   The arithmetic and control unit sets a new coordinate system whose origin is a gripping point when the gripping mechanism grips the gripping object, and the three position components of the gripping position and orientation in the new coordinate system and 3 The gripping position / posture when the two posture components are corrected are ranked in descending order of the contact area between the gripping mechanism and the gripping target, and the three position components and the three posture components of the gripping position / posture are classified according to the ranking. 4. The gripping position / posture determination system according to claim 3, wherein the system determines whether the corrected gripping position / posture is within a movable range.   The arithmetic and control unit does not correct the three position components and the three posture components of the gripping position and posture at the same time, but divides them into several components according to the size of the contact area. The gripping position / posture determination system according to claim 4.   The correction of the grip position / posture is the sum of the absolute value of the difference between the joint angle at the limit of movement of the joint of the arm and the joint angle of the arm that realizes the grip position / posture at the time of correction with respect to the joint. The gripping position / posture determination system according to claim 4, wherein the gripping position / posture determination system is performed in such a manner as to minimize. Based on the position and orientation of the gripping object measured by the position and orientation measurement sensor and the shape information of the gripping object stored in the database, the gripping position and orientation related to the gripping object has at least two joints and the arm Determining whether it is outside the movable range of the gripping mechanism provided at the tip of the arm;
When the arm and the gripping mechanism are out of the movable range, the gripping position and posture are corrected to calculate whether or not the corrected gripping position and posture exist within the movable range of the arm and the gripping mechanism. Steps,
Controlling the arm and the gripping mechanism to be in the corrected gripping position and posture when present;
A gripping position / posture determination method characterized by comprising:

Images