JP2012040633A - Method for determining redundant degree of freedom of redundant manipulator - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a technology that secures the continuity of an arm angle ψ when changing a hand tip 8 of a redundant manipulator 1 by CP control.SOLUTION: A control device 100 includes: a state acquisition device 20 for acquiring a position and a posture state of the hand tip 8 in each step; an area calculation device 21 for calculating an area in which the arm angle ψ is possible in each step based on the position and the posture state of the hand tip 8 in each step; a combination creation device 22 for creating a combination of areas from a step s0 to a step sn so that areas of adjacent steps are mutually overlapped at least at a part thereof between the adjacent steps; and a redundant degree of a freedom determiner 23 for determining the arm angle ψ in each step of the CP control based on the combination of areas.

Description

  The present invention relates to a redundancy degree determination method, a determination device, and a determination program for each step of a redundant manipulator controlled by CP (Continuous Point).

  In recent years, redundant manipulators having more degrees of freedom than those required for work have become widely used in the robot field. This is because the redundant manipulator can effectively utilize the extra degrees of freedom, for example, to improve the operability of the robot, optimize joint torque, avoid obstacles, and so-called singularities. This is because it can be avoided and joint limits can be avoided.

  Non-Patent Document 1 refers to a 7-DOF redundant manipulator as an example of such a redundant manipulator. In general, the position and posture of the hand of the manipulator in the space can be expressed by six parameters. Therefore, one extra degree of freedom exists in the seven degree of freedom manipulator.

  First, the seven-degree-of-freedom redundant manipulator will be described with reference to FIG. As shown in FIG. 22, the joints of the seven-degree-of-freedom redundant manipulator are all composed of rotary joints. The joint axes of the three shoulder joints (first to third joints, θ1 to θ3) intersect at one point. Similarly, the joint axes of the three joints (fifth to seventh joints, θ5 to θ7) of the wrist also intersect at one point. Moreover, the axes of a pair of adjacent joints are orthogonal to each other. Thus, the three joints of the shoulder and the wrist have a simple structure, and can be regarded as constituting a virtual spherical mechanism. Therefore, this type of manipulator is generally classified as a 3R-1R-3R type or SRS type manipulator.

  Further, the non-patent document 1 employs the arm angle ψ disclosed in the non-patent document 2 as a parameter for expressing the redundancy degree of freedom. The arm angle ψ is defined as an angle formed by the arm plane and the reference plane as shown in FIG. The arm plane is a plane that passes through the three points of the intersection point Ps of the shoulder three joints, the elbow joint position Pe, and the intersection point Pw of the wrist three joints. If the movable range of the joint is not limited, only the arm angle ψ can be freely changed while the position and posture of the hand are fixed. However, in an actual manipulator, the movable range of the joint is limited due to mechanical limitations. As a result, the realizable region of the arm angle ψ is naturally limited. On the other hand, Non-Patent Document 1 proposes a method for algebraically solving a realizable region of the arm angle ψ. See Non-Patent Document 1 for details of this method. In short, according to Non-Patent Document 1, the realizable region of the arm angle ψ can be composed of one or a plurality of closed regions.

Masayuki Shimizu and 4 others, “Analytical Inverse Kinematic Solution of 7 DOF Redundant Manipulator Taking Account of Joint Movement Range”, Journal of the Robotics Society of Japan, The Robotics Society of Japan, 2007, Vol. 25, No. 4 , P.122-133 K. Kreutz-Delgado, M. Long and H. Seraji: "Kinematic Analysis of 7-DOF Manipulators," Int. J. Robotics Research, 1992, vol.11, no.5, pp.469-481.

  As described above, the realizable region of the arm angle ψ is configured by one or a plurality of closed regions. However, there is an unsolved problem between such an area where the arm angle ψ can be realized and CP (Continuous Point) control. Here, as shown in FIG. 24, the CP control is from (1) the first position and posture state (see “s0”) to the second position and posture state (see “s6”). The position and posture state of the hand of the manipulator are finely set, for example, at predetermined time intervals, and (2) the angle of each joint is obtained using inverse kinematics for each hand position, (3) (2) This is the control that causes the robot to reproduce the angles of the joints obtained in step 1. However, since the redundant manipulator has a redundant configuration, the angle of each joint is not uniquely determined in the above (2), and the degree of freedom for the redundancy needs to be determined in advance based on some criterion. Hereinafter, this problem will be described in detail with reference to FIGS. For convenience of explanation, the arm angle ψ is continuously used as a parameter representing the degree of redundancy, but there is no particular circumstance in which the arm angle ψ must be used as a parameter representing the degree of redundancy, and other parameters are used. Note that it may be used instead.

  FIG. 25 shows a feasible region of the arm angle ψ in each step shown in FIG. S0 to s6 arranged on the horizontal axis in FIG. 25 correspond to steps s0 to s6 shown in FIG. The vertical axis in FIG. 25 is the arm angle ψ. In FIG. 25, “ψ” having a superscript and a subscript means a realizable region (range) of the arm angle ψ. The superscript “ψ” means a number for identifying the step to which the area belongs. On the other hand, the subscript “ψ” is an identification number of each area in the step to which the area belongs. To repeat for confirmation, “ψ” means an arm angle, but “ψ” with a superscript and a subscript means a realizable region of the arm angle ψ. In the text, the realizable area of the arm angle ψ is expressed as ψ (superscript, subscript) for convenience.

  Conventionally, there has been no knowledge about the criteria for predetermining the degree of freedom for redundancy. Therefore, there was no knowledge about the criteria for predetermining the arm angle ψ in step 0 as well. So how did the inventors determine the arm angle ψ at step 0? According to an interview with the inventors of the present application, it is as follows. That is, the inventors first compare the magnitudes of ψ (0,1) and ψ (0,2) in step 0, and in the case of FIG. 25, ψ (0,1)> ψ (0,2). Therefore, ψ (0, 1) was selected based on this simple magnitude relationship. The inventors of the present application have adopted the intermediate value of ψ (0,1) as the arm angle ψ in step 0. Further, regarding the arm angle ψ in the next step 1, the inventors of the present invention consider the overlapping relationship between ψ (0,1) and ψ (1,1) in consideration of the temporary continuity of the arm angle ψ. Emphasis was placed on ψ (1,1) among ψ (1,1) and ψ (1,2). The inventors of the present application have adopted the intermediate value of ψ (1,1) as the arm angle ψ in step 1.

  Such ad hoc standards still seem reasonable at first glance. However, according to this standard, a big problem arises when moving from step 4 to step 5. This is because, for ψ (0,1), there is a realizable region of arm angle ψ that overlaps with itself in the next step 1. Similarly, for ψ (1,1), there is a realizable region of the arm angle ψ that overlaps with itself in the next step 2. However, for ψ (4, 1), there is no realizable region of arm angle ψ that overlaps with itself in the next step 5. Therefore, at the expense of the continuity of the arm angle ψ, the arm angle ψ in step 5 has to be selected and adopted from among the only existing ψ (5,1). As a result, the value of the arm angle ψ changes so much that it cannot be overlooked. As a result, the angular velocity of each joint is also finite, and the hand is greatly deviated from the intended trajectory, or the hand is moved to another object. There were various problems such as interference.

  The object of the present invention is to change the position and posture state of the hand of the redundant manipulator from the first position and posture state to the second position and posture state by CP control. The purpose is to provide technology that guarantees continuity. Here, the continuity of the redundancy degrees of freedom means that “the change in the value of the redundancy degrees of freedom is gentle between steps”.

  According to the first aspect of the present invention, the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control. At this time, a method of determining the redundancy degree of freedom of the redundant manipulator in each step of CP control is as follows. The position and posture state of the hand at each step are acquired. Based on the position and posture state of the hand at each step, an area where the redundancy degree of freedom of the redundant manipulator at each step can be realized is calculated. A combination of the regions from the first position and posture state to the second position and posture state is created so that the regions at least partially overlap between adjacent steps. Based on the combination of the regions, the redundancy degree of freedom of the redundant manipulator in each step of the CP control is determined. According to the above method, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

  Preferably, when a plurality of combinations of the areas can be created, the combination having the largest area in the final step is adopted. According to the above method, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

  Preferably, when determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions, the square sum of the difference in the degree of redundancy between the steps is minimized. A redundancy degree of freedom of the redundant manipulator in each step of CP control is determined. According to the above method, the redundant manipulator moves smoothly.

  Preferably, the redundant manipulator has 7 degrees of freedom.

  Preferably, the redundant manipulator is of the SRS type.

  According to the second aspect of the present invention, the position and posture of the hand of the redundant manipulator are changed from the first position and posture to the second position and posture by CP (Continuous Point) control. At this time, an apparatus for determining the redundancy degree of freedom of the redundant manipulator in each step of CP control is as follows. The determination device can realize the redundancy degree of freedom of the redundant manipulator in each step based on the state acquisition means for acquiring the position and posture state of the hand in each step and the position and posture state of the hand in each step. An area calculation means for calculating an area and the area between the first position and posture state to the second position and posture state so that the areas overlap at least partially between adjacent steps. A combination creating means for creating a combination; and a redundancy degree of freedom determining means for determining the degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the areas. According to the above configuration, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

  Preferably, when a plurality of combinations of the areas can be created, the combination creating unit adopts a combination having the largest area in the final step. According to the above configuration, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

  Preferably, the redundancy degree-of-freedom determination means determines the redundancy degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the regions, and the sum of squares of the difference of the redundancy degrees of freedom between the steps. Is determined such that the redundancy degree of freedom of the redundant manipulator in each step of the CP control is minimized. According to the above configuration, the redundant manipulator moves smoothly.

  Preferably, the redundant manipulator has 7 degrees of freedom.

  Preferably, the redundant manipulator is of the SRS type.

  According to the third aspect of the present invention, the position and posture of the hand of the redundant manipulator are changed from the first position and posture to the second position and posture by CP (Continuous Point) control. At this time, the program for determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control includes a computer, a state acquisition means for acquiring the position and posture state of the hand in each step, and the position of the hand in each step. Based on the position and orientation state, the area calculation means for calculating an area where the redundancy degree of freedom of the redundant manipulator in each step can be realized, and the areas are at least partially overlapped between adjacent steps. The set of regions between the first position and posture state and the second position and posture state A combination generating means for generating a combined, based on the combination of the region, the a redundant DOF determination means for determining a redundancy degree of freedom of the redundant manipulator at each step of the CP control, to function as a. According to the above program, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

  Preferably, when a plurality of combinations of the areas can be created, the combination creating unit adopts a combination having the largest area in the final step. According to the above program, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

  Preferably, the redundancy degree-of-freedom determination means determines the redundancy degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the regions, and the sum of squares of the difference of the redundancy degrees of freedom between the steps. Is determined such that the redundancy degree of freedom of the redundant manipulator in each step of the CP control is minimized. According to the above program, the redundant manipulator moves smoothly.

  Preferably, the redundant manipulator has 7 degrees of freedom.

  Preferably, the redundant manipulator is of the SRS type.

  According to the present invention, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, the redundant manipulator The continuity of redundant degrees of freedom can be ensured.

FIG. 1 is an overall schematic diagram of a redundant manipulator (first embodiment). FIG. 2 is a functional block diagram of the redundant manipulator (first embodiment). FIG. 3 is a control flow of the redundant manipulator (first embodiment). FIG. 4 is a control flow of the redundant manipulator (first embodiment). FIG. 5 is a list of regions ψ in which the redundancy degree can be realized (first embodiment). FIG. 6 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (first embodiment). FIG. 7 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (first embodiment). FIG. 8 is an overall schematic diagram of the redundant manipulator (second embodiment). FIG. 9 is a diagram showing a region ψ in which the degree of redundancy can be realized (second embodiment). FIG. 10 is a list of regions ψ in which redundancy degrees of freedom can be realized (second embodiment). FIG. 11 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (second embodiment). FIG. 12 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (second embodiment). FIG. 13 is a diagram showing the degree of redundancy determined in each step (second embodiment). FIG. 14 is a diagram illustrating the redundancy degrees determined in each step (third embodiment). FIG. 15 is a diagram showing a region ψ in which the degree of redundancy can be realized (fourth embodiment). FIG. 16 is a list of regions ψ in which redundancy degrees can be realized (fourth embodiment). FIG. 17 is a control flow of the redundant manipulator (fourth embodiment). FIG. 18 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (fourth embodiment). FIG. 19 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (fourth embodiment). FIG. 20 is a diagram illustrating combinations selected from the plurality of combinations illustrated in FIG. 19 (fourth embodiment). FIG. 21 is a diagram showing the degree of redundancy determined in each step (fourth embodiment). FIG. 22 is an overall schematic diagram of a redundant manipulator (prior art). FIG. 23 is an explanatory diagram of the definition of an arm angle as an example of the redundancy degree (prior art). FIG. 24 is an overall schematic diagram of a redundant manipulator (prior art). FIG. 25 is a diagram showing the redundancy degree of freedom determined in each step (prior art).

(First embodiment)
Hereinafter, the first embodiment of the present invention will be described with reference to FIGS. Hereinafter, the term “step” in the specification may indicate a step of various processes in the control flow of CP control, and may indicate a step of manipulator movement by CP control. Please note that.

  As shown in FIG. 1, in the present embodiment, the redundant manipulator 1 includes a trunk portion 2, a shoulder joint portion 3, an upper arm portion 4, an elbow joint portion 5, a forearm portion 6, a wrist joint portion 7, and a hand tip. 8 and a control device 100 (decision device) as main components. The trunk 2 is fixed to the ground, for example. The ground and the shoulder joint 3 are connected by the trunk 2. The shoulder joint 3 and the elbow joint 5 are connected by the upper arm 4. The elbow joint 5 and the wrist joint 7 are connected by a forearm 6. The hand 8 is supported by the wrist joint 7.

  The shoulder joint 3 is a spherical mechanism composed of first to third joints. The elbow joint 5 is composed of a fourth joint. The wrist joint portion 7 is a spherical mechanism composed of fifth to seventh joints. Therefore, the redundant manipulator 1 is classified as a so-called SRS type manipulator. Further, the redundant manipulator 1 has one extra degree of freedom. Hereinafter, it is assumed that the arm angle ψ disclosed in Non-Patent Document 2 is used as a parameter for expressing the redundancy degree of freedom. Again, the parameter for expressing the redundancy degree of freedom is not limited to the arm angle ψ, but may be, for example, an arbitrary joint angle of one degree of freedom or an arm angle of another expression.

  As shown in FIG. 2, the redundant manipulator 1 further includes a first joint motor (shoulder) 30, a second joint motor (shoulder) 31, a third joint motor (shoulder) 32, and a fourth joint. A motor (elbow part) 33, a fifth joint motor (wrist part) 34, a sixth joint motor (wrist part) 35, and a seventh joint motor (wrist part) 36 are provided. The first joint motor (shoulder portion) 30 is mounted on the first joint constituting the shoulder joint portion 3 and drives the first joint. Similarly, the second joint motor (shoulder portion) 31 is mounted on the second joint constituting the shoulder joint portion 3 and drives the second joint. The third joint motor (shoulder portion) 32 is mounted on the third joint constituting the shoulder joint portion 3 and drives the third joint. The fourth joint motor (elbow part) 33 is mounted on the fourth joint constituting the elbow joint part 5 and drives the fourth joint. The fifth joint motor (wrist part) 34 is mounted on the fifth joint constituting the wrist joint part 7 and drives the fifth joint. The sixth joint motor (wrist part) 35 is mounted on the sixth joint constituting the wrist joint part 7 and drives the sixth joint. The seventh joint motor (wrist part) 36 is mounted on the seventh joint constituting the wrist joint part 7 and drives the seventh joint.

  The control device 100 changes the position and posture state of the hand 8 of the redundant manipulator 1 from the first position and posture state (step s0, see FIG. 1) to the second position and posture state (step sn, When changing to (see FIG. 1), the arm angle ψ of the redundant manipulator 1 in each step of CP control is determined. The control device 100 includes a CPU (Central Processing Unit) 10, a RAM (Random Access Memory) 11, and a ROM (Read Only Memory) 12. The ROM 12 stores a control program (decision program). This control program is read by the CPU 10 and executed on the CPU 10. As a result, the control program causes hardware such as the CPU 10 to function as the state acquisition unit 20, the region calculation unit 21, the combination creation unit 22, the redundancy degree determination unit 23, the joint angle calculation unit 24, and the CP control unit 25. .

  The state acquisition means 20 acquires state information as information relating to the position and posture state of the hand 8 at each step, which is necessary for CP control. The status acquisition means 20 can acquire each status information from the other control device via wire or wireless, input from an operator via an input means such as a keyboard, reading from the ROM 12, etc. Can be considered. Each state information has an X value, a Y value, and a Z value that represent the position of the hand 8 with respect to the space, and a θ1 value, a θ2 value, and a θ3 value that represent the posture of the hand 8 with respect to the space. Composed. The state acquisition unit 20 stores the acquired state information for each step in the RAM 11.

  The area calculation means 21 reads state information for each step from the RAM 11 and calculates area information as information regarding a realizable area of the arm angle ψ of the redundant manipulator 1 for each step based on the state information. . There are a case where only one region information exists in all steps, a case where a plurality of region information exists in all steps, and a case where one or a plurality of region information exists in each step. Refer to Non-Patent Document 1 for the method of calculating each area information by the area calculating means 21. The area calculation means 21 stores the calculated area information for each step in the RAM 11.

  The combination creating unit 22 reads the region information for each step from the RAM 11 and reaches the second position and posture state from the first position and posture state so that the regions at least partially overlap between adjacent steps. One piece of combination information is created as information related to the combination of the region information. The phrase “regions at least partially overlap between adjacent steps” can be rephrased as “includes a redundancy degree of freedom common to regions between adjacent steps”. Note that the area in the first step (step s0) and the area in the last step (step sn) do not necessarily have to overlap. The combination creating unit 22 stores the created combination information in the RAM 11.

  The redundant degree-of-freedom determining means 23 reads combination information from the RAM 11 and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information. The redundant degree-of-freedom determination means 23 stores the determined arm angle ψ for each step in the RAM 11.

  The joint angle calculation means 24 reads the arm angle ψ for each step from the RAM 11 and, for each step, performs inverse kinematics on the basis of the arm angle ψ at that step and the state information at that step. The joint angle of each joint (first to seventh joints) is calculated. The joint angle calculation means 24 stores the calculated joint angle of each joint for each step in the RAM 11.

  The CP control means 25 reads the joint angle of each joint for each step from the RAM 11 and reproduces the motion of the redundant manipulator 1 defined by these joint angles. Specifically, the CP control means 25 has a first joint motor (shoulder) 30 and a second joint motor (shoulder) 31 in order to achieve the joint angle of each joint (first to seventh joints) at each step. , Third joint motor (shoulder) 32, fourth joint motor (elbow) 33, fifth joint motor (wrist part) 34, sixth joint motor (wrist part) 35, seventh joint motor (wrist part) 36 To control.

  Next, the operation of the redundant manipulator 1 will be described with reference to FIGS.

  First, a CP control start signal is transmitted from another control device to the control device 100 via wired or wireless, or an operation personnel instructs the control device 100 to start CP control via an input means such as a keyboard, for example. (S300).

  Then, the state acquisition unit 20 acquires state information for each step, which is necessary for CP control (S302). 5 to 7, the state information for each step is indicated by Tn which is a homogeneous transformation matrix. The subscript n represents the total number of steps of the discretized CP control trajectory. The state acquisition unit 20 stores the acquired state information Tn for each step in the RAM 11.

  Next, the area calculation means 21 reads the state information Tn for each step from the RAM 11, and calculates area information as information on the area where the arm angle ψ can be realized for each step based on each state information Tn. (S304). 5 to 7, the area information for each step is indicated by ψ (i, j). Here, “i” means a superscript, and “j” means a subscript. “I” means a number for identifying a step to which the area belongs. On the other hand, “j” is a number for identifying one or more closed areas calculated in the step to which the area belongs. The subscript “mn” corresponds to the number of area information calculated in that step. The area calculation means 21 stores the calculated area information ψ (i, j) for each step in the RAM 11.

  Next, in all the steps, when the number of the area information ψ (i, j) is 1 or more, the control device 100 advances the process to S308. On the other hand, if the number of the region information ψ (i, j) is not 1 or more in any step, there is no solution, and the process ends (S307).

  In all steps, when the number of the area information ψ (i, j) is 1 or more, the combination creating means 22 reads the area information ψ (i, j) for each step from the RAM 11 and Combination information as information regarding the combination of region information ψ (i, j) from the first position and posture state to the second position and posture state so that the regions at least partially overlap between the matching steps. Create one.

  Specifically, the combination creating unit 22 first creates a continuity graph as shown in FIG. 6 (S400). Specifically, in FIG. 5, if the region information ψ (i, j) and the region information ψ (i−1, k) satisfy the following formula (1) between adjacent steps, at least partly overlaps. The region information ψ (i, j) and the region information ψ (i−1, k) are continuous, and the region information ψ (i, j) and the region information ψ (i−1, k) are expressed by graph theory. They are joined at what they say. For example, in FIG. 5, since the region information ψ (1, 1) and the region information ψ (0, 1) satisfy the following expression (1) between the adjacent steps s0 and s1, the region information ψ (1, 1) and the region information ψ (0, 1) are coupled by an edge as shown by a thick line in FIG. In FIG. 5, m0 pieces of area information ψ (i−1, k) belong to step s0, and m1 pieces of area information ψ (i, j) belong to step s1. Therefore, as a general rule, in order to determine the overlapping relationship between all the area information ψ (i−1, k) belonging to step s0 and all the area information ψ (i, j) belonging to step s1 without omission, It is necessary to consider the following formula (1) m0 × m1 times.

  Next, the combination creating means 22 searches the continuity graph shown in FIG. 6 for a combination of continuous area information ψ (i, j) between step s0 and sn (S402). Since this search algorithm has been publicly known with various proposals in the field of graph theory, its description will be omitted. As a result of this search, as shown in FIG. 7, the combination creating means 22 uses the combination of continuous region information ψ (i, j) between step s0 and sn as shown in FIG. , Ψ (1,1), ψ (2,1), ψ (3,1),..., Ψ (n−1,1), ψ (n, 1) ”. . When a combination of continuous region information ψ (i, j) between step s0 and step sn is found in this way (S404: YES), the combination creating means 22 selects “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1),..., Ψ (n−1,1), ψ (n, 1) ”. Then, the combination creating unit 22 stores the created combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S406). On the other hand, when the combination of the continuous area information ψ (i, j) between step 0 and step n is not found (S404: NO), the process is terminated as no solution (S408).

  Next, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information (S310). For example, the redundancy degree determining means 23 employs the upper limit value, the lower limit value, preferably the intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. The redundant degree-of-freedom determination means 23 stores the determined arm angle ψ for each step in the RAM 11.

  Next, the joint angle calculation means 24 reads the arm angle ψ for each step from the RAM 11, and for each step, based on the arm angle ψ at that step and the state information at that step, by inverse kinematics, The joint angle of each joint (first to seventh joints) in that step is calculated (S312). The joint angle calculation means 24 stores the calculated joint angle of each joint for each step in the RAM 11.

  Then, the CP control means 25 reads the joint angle of each joint for each step from the RAM 11, and reproduces the motion of the redundant manipulator 1 defined by these joint angles (S314).

  The preferred first embodiment of the present invention has been described above. In short, the first embodiment has the following features.

  When changing the position and posture state of the hand 8 of the redundant manipulator 1 from step s0 to step sn in the CP control, the control device 100 that determines the arm angle ψ of the redundant manipulator 1 in each step of the CP control, It is as follows. Based on the state acquisition means 20 for acquiring the position and posture state of the hand 8 at each step, and the position and posture state of the hand 8 at each step, the control device 100 determines a realizable region of the arm angle ψ at each step. Based on the combination of the area calculation means 21, the combination creation means 22 that creates a combination of areas from step s0 to step sn so that the areas at least partially overlap between adjacent steps, and the area combination. Redundant degree-of-freedom determining means 23 for determining the arm angle ψ in each step of CP control. According to the above configuration, when the position and posture state of the hand 8 of the redundant manipulator 1 is changed from step s0 to step sn by CP control, the continuity of the redundant degree of freedom of the redundant manipulator 1 is ensured. Can do.

  As a secondary effect, discontinuity that occurs at the joint angle level is overcome by this, and hand movements that make maximum use of the performance of the mechanism are possible.

(Second Embodiment)
Next, a second embodiment of the present invention will be described with reference to FIGS. Here, the present embodiment will be described with a focus on differences from the first embodiment, and overlapping descriptions will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the respective components of the first embodiment. In the present embodiment, the first embodiment is further embodied.

  As shown in FIG. 8, in this embodiment, the CP control of the redundant manipulator 1 is based on six steps from step s0 to step s5. FIG. 9 shows the relationship between the state information Tn acquired by the state acquisition unit 20 and the region information ψ (i, j) calculated by the region calculation unit 21. Each state information Tn is arranged on the horizontal axis of FIG. The vertical axis in FIG. 9 is the arm angle ψ. Although the origin of the arm angle ψ is not particularly specified, the possible value of the arm angle ψ is from −π to π. FIG. 10 shows a list of the relationship between each state information Tn and each region information ψ (i, j) as shown in FIG. As shown in FIG. 9, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 1). Therefore, the region information ψ (0, 1) and the region information ψ (1, 1) are continuous and are joined by an edge as shown in FIG. Similarly, as shown in FIG. 9, a common arm angle ψ exists between the region information ψ (0,1) and the region information ψ (1,2). Therefore, the region information ψ (0,1) and the region information ψ (1,2) are continuous and are joined at the edge as shown in FIG. The combination creating means 22 advances the processing in this manner, and eventually completes a continuity graph as shown in FIG. 11 (S400).

  Next, the combination creating means 22 searches the continuity graph shown in FIG. 11 for a combination of continuous area information ψ (i, j) between step s0 and step s5 (S402). As a result of this search, the combination creating means 22 obtains “ψ (0,1), ψ as a combination of continuous area information ψ (i, j) between steps s0 and s5 as shown in FIG. (1,1), ψ (2,1), ψ (3,1), ψ (4,1), and ψ (5,1) ”are specified. The combination creating means 22 starts from “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1)”. Create combination information to be configured. Then, the combination creating unit 22 stores the created combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S406).

  Next, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ in each step of CP control based on the combination information as shown in FIG. 13 (S310). The determined arm angle ψ of the redundant manipulator 1 at each step is indicated by a circle in FIG. In the present embodiment, as shown in FIG. 13, the redundancy degree determining means 23 employs an intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. . Then, the redundancy degree of freedom determining means 23 stores the determined arm angle ψ for each step in the RAM 11.

In FIG. 13, Δψ means “Δψ _{p → p + 1} ”, and “Δψ _{p → p + 1} ” means the difference between the arm angle ψ of step p and the arm angle ψ of step p + 1.

(Third embodiment)
Next, a third embodiment of the present invention will be described with reference to FIG. Here, the present embodiment will be described with a focus on differences from the second embodiment, and a duplicate description will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the components of the second embodiment.

  In the second embodiment, the redundancy degree determining means 23 adopts the intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. On the other hand, in this embodiment, as shown in FIG. 14, the redundancy degree determining means 23 determines the arm angle ψ at each step so that the sum of squares of the difference of the arm angle ψ between steps becomes the smallest. To do. Specifically, the redundancy degree determining means 23 determines the arm angle ψ in each step so that the value of the variable Q defined by the following equation (2) is minimized.

  The second embodiment of the present invention has been described above. In short, the second embodiment has the following features.

  When determining the arm angle ψ of the redundant manipulator 1 at each step of the CP control based on the combination information, the redundancy degree of freedom determining means 23 is configured so that the sum of squares of the differences of the arm angles ψ between the steps is minimized. The arm angle ψ in each step of CP control is determined. According to the above configuration, the movement of the redundant manipulator 1 becomes smooth. In addition, there is a merit that energy saving and movement of the hand 8 itself becomes agile.

(Fourth embodiment)
Next, a fourth embodiment of the present invention will be described with reference to FIGS. Here, the present embodiment will be described with a focus on differences from the second embodiment, and a duplicate description will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the components of the second embodiment.

  FIG. 15 shows the relationship between the state information Tn acquired by the state acquisition unit 20 and the region information ψ (i, j) calculated by the region calculation unit 21. FIG. 16 shows a list of the relationship between each state information Tn and each region information ψ (i, j) as shown in FIG. In this embodiment, the control device 100 executes the control flow of FIG. 17 instead of the control flow of FIG. Hereinafter, the operation of the redundant manipulator 1 in the present embodiment will be described along the control flow shown in FIG.

  As shown in FIG. 15, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 1). Therefore, the region information ψ (0, 1) and the region information ψ (1, 1) are continuous and are joined at the edge as shown in FIG. Similarly, as shown in FIG. 15, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 2). Therefore, the region information ψ (0, 1) and the region information ψ (1, 2) are continuous and are joined at the edge as shown in FIG. The combination creating means 22 advances the processing in this manner, and eventually completes a continuity graph as shown in FIG. 18 (S500).

  Next, the combination creating means 22 searches the continuity graph shown in FIG. 18 for a combination of continuous area information ψ (i, j) between step s0 and step s5 (S502). As a result of this search, the combination creating means 22 obtains “ψ (0,1), ψ as a combination of continuous area information ψ (i, j) between steps s0 and s5 as shown in FIG. (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1) ”,“ ψ (0,1), ψ (1 , 2), ψ (2, 2), ψ (3, 3), ψ (4, 3), ψ (5, 3) ”. Thus, when the combination of continuous area | region information (psi) (i, j) between step s0 and step s5 is found, the control apparatus 100 advances a process to S506. On the other hand, when the combination of the continuous area information ψ (i, j) between step s0 and step s5 is not found, the control device 100 ends the process with no solution (S508).

  Then, as shown in FIG. 19, when a plurality of combinations of continuous area information ψ (i, j) between steps s0 and s5 are found (S506: YES), the control device 100 performs the process in S510. Proceed to. On the other hand, when only one combination of continuous area information ψ (i, j) between step s0 and step s5 is found (S506: NO), the control device 100 moves the process to the control flow of FIG. (S512).

  As described above, when a plurality of combinations of continuous area information ψ (i, j) between steps s0 and s5 are found (S506: YES), the combination creating means 22 makes the final step (step s5). The combination having the largest region information ψ (5, j) is selected and adopted (S510). In this embodiment, it consists of “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1)”. The region information ψ (5, j) in the final step (step s5) of the combination is the region information ψ (5,1). In addition, the final combination of “ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4,3), ψ (5,3)” The region information ψ (5, j) in step (step s5) is the region information ψ (5,3). Therefore, the combination creating unit 22 uses the region information ψ (5,1) and the region information ψ () as the region information ψ (5, j) in the final step of the combination of each continuous region information ψ (i, j). 5,3) is compared. In the present embodiment, as shown in FIG. 15, the region information ψ (5,3) includes a wider range with respect to the arm angle ψ than the region information ψ (5,1). 5,3) can be said to be larger than the region information ψ (5,1). Accordingly, as shown in FIG. 20, the combination creating unit 22 “ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4,3) , Ψ (5,3) ”is selected and adopted, and“ ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4 , 3), ψ (5, 3) ”. Then, the combination creating unit 22 stores the created and selected combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S514).

  Next, as shown in FIG. 21, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information (S310). . The determined arm angle ψ of the redundant manipulator 1 at each step is indicated by a circle in FIG. In the present embodiment, as shown in FIG. 21, the redundancy degree determining means 23 employs an intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. . Then, the redundancy degree of freedom determining means 23 stores the determined arm angle ψ for each step in the RAM 11.

  The preferred fourth embodiment of the present invention has been described above. In short, the fourth embodiment has the following features.

  When a plurality of combinations of area information ψ (i, j) can be created, the combination creating unit 22 employs the combination having the largest area information ψ (i, j) in the final step. According to the above configuration, the region information ψ (i, j) is maximized in the final step. Therefore, in step s5, it is possible to greatly change the arm angle ψ while keeping the position and posture state of the hand 8 as they are. This is because, for example, when the position and posture state of the hand 8 is further changed from step s5, the flexibility of selection of the arm angle ψ in the first step of the change is excellent, so at least the following three merits: Occurs. First, the probability that a combination of region information ψ (i, j) can be created increases. Secondly, there is a high probability that the number of combinations that can be created for combinations of region information ψ (i, j) is plural. Thirdly, there is a possibility that the number of combinations that can be created for combinations of the region information ψ (i, j) may be increased.

  The preferred first to fourth embodiments of the present invention have been described above, but these embodiments can be combined as appropriate.

DESCRIPTION OF SYMBOLS 1 Redundant manipulator 20 State acquisition means 21 Area | region calculation means 22 Combination preparation means 23 Redundancy freedom determination means 24 Joint angle calculation means 25 CP control means 100 Control apparatus (determination apparatus)
ψ Arm angle (redundancy freedom)
ψ (i, j) region information
Tn status information

Claims (15)

When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A method for determining the redundancy degree of freedom of
Obtain the position and posture state of the hand in each step,
Based on the position and posture state of the hand in each step, calculate a feasible region of redundancy freedom of the redundant manipulator in each step,
Create a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps,
Determining a redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions;
A method for determining the redundancy degree of freedom of a redundant manipulator. A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 1,
When a plurality of combinations of the areas can be created, the area in the final step adopts the largest combination,
A method for determining the redundancy degree of freedom of a redundant manipulator. A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 1 or 2,
When determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions, the CP control is performed so that the sum of squares of the difference of the redundancy degree of freedom between the steps is minimized. Determining the redundancy degree of freedom of the redundant manipulator at each step;
A method for determining the redundancy degree of freedom of a redundant manipulator. A method for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 1 to 3,
The redundant manipulator has 7 degrees of freedom.
A method for determining the redundancy degree of freedom of a redundant manipulator. A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 4,
The redundant manipulator is of the SRS type,
A method for determining the redundancy degree of freedom of a redundant manipulator. When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A device for determining the degree of redundancy of
State acquisition means for acquiring the position and posture state of the hand in each step;
Based on the position and posture state of the hand in each step, a region calculating means for calculating a feasible region of the redundancy degree of freedom of the redundant manipulator in each step;
A combination creating means for creating a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps;
Redundant degree-of-freedom determining means for determining a redundant degree of freedom of the redundant manipulator in each step of the CP control based on a combination of the regions;
Comprising
A device for determining the degree of redundancy of a redundant manipulator. A redundant manipulator redundancy degree determining apparatus according to claim 6,
The combination creating means adopts the combination having the largest area in the final step when a plurality of combinations of the areas can be created.
A device for determining the degree of redundancy of a redundant manipulator. A redundant manipulator determining apparatus for determining the degree of freedom of redundancy according to claim 6 or 7,
The redundant degree-of-freedom determining means determines the redundant sum of squares of the redundant degrees of freedom between steps when determining the redundant degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions. And determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control.
A device for determining the degree of redundancy of a redundant manipulator. A device for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 6 to 8,
The redundant manipulator has 7 degrees of freedom.
A device for determining the degree of redundancy of a redundant manipulator. A redundant manipulator redundancy degree determining apparatus according to claim 9,
The redundant manipulator is of the SRS type,
A device for determining the degree of redundancy of a redundant manipulator. When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A program for determining the redundancy degree of
Computer
State acquisition means for acquiring the position and posture state of the hand in each step;
Based on the position and posture state of the hand in each step, a region calculating means for calculating a feasible region of the redundancy degree of freedom of the redundant manipulator in each step;
A combination creating means for creating a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps;
Redundant degree-of-freedom determining means for determining a redundant degree of freedom of the redundant manipulator in each step of the CP control based on a combination of the regions;
Function as
A program for determining the degree of redundancy of redundant manipulators. A redundancy manipulator determination program for redundant manipulators according to claim 11,
The combination creating means adopts the combination having the largest area in the final step when a plurality of combinations of the areas can be created.
A program for determining the degree of redundancy of redundant manipulators. A program for determining a redundancy degree of freedom of a redundant manipulator according to claim 11 or 12,
The redundant degree-of-freedom determining means determines the redundant sum of squares of the redundant degrees of freedom between steps when determining the redundant degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions. And determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control.
A program for determining the degree of redundancy of redundant manipulators. A program for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 11 to 13,
The redundant manipulator has 7 degrees of freedom.
A program for determining the degree of redundancy of redundant manipulators. A redundancy manipulator determination program for redundant manipulators according to claim 14,
The redundant manipulator is of the SRS type,
A program for determining the degree of redundancy of redundant manipulators.

Images