JPH05228863A - Manipulator control unit - Google Patents

Abstract

PURPOSE:To facilitate a coordinate transformation finding a desired joint angle out of a desired position and a desired attitude by setting plural pieces of joints in each tip part to a relationship of roll-pitch-roll with one another, while adopting such a means that one intersection is set in each joint shaft of these joints and so on. CONSTITUTION:A control unit 1 of a manipulator with seven joints operates a desired joint angle of each of first to fourth joints at a position arithmetic unit on the basis of a desired position of an intersection being forced by a joint shaft of each of fifth to seventh joints. At this time, not only a first component 5 pertaining to a joint angle of each of first to fourth joints but also a second component 6 pertaining to the joint angle of each of the first to fourth joints for reducing the axial slippage of these first to fourth joints are operated together. In addition, on the basis of a desired attitude of a terminal attached to the seventh joint, the desired joint angle of each of the fifth to seventh joints is operated at an attitude arithmetic unit 3. Furthermore, on the basis of respective operational results of each arithmetic unit 3, the joint angle of each of the first to seventh joints is controlled by a joint angle control unit 4.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a manipulator having seven joints, a first joint to a seventh joint, and more particularly to a fifth joint to a seventh joint at the tip of the manipulator.
The three joints are in a roll-pitch-roll relationship with each other and the joint axes of the fifth joint to the seventh joint have one intersection, and the control device sets the position of the intersection to follow the target position. In addition, the present invention relates to a manipulator control device that controls the posture of the hand attached to the seventh joint so as to follow the target posture.

[0002]

2. Description of the Related Art Manipulators having six joints are mainly used mainly for industrial robots used in factories. In many of these manipulators, the joint axes of the three joints at the tip end intersect at a single point, and these joints have a roll-pitch-roll relationship.

The hand attached to the tip of such a manipulator having six joints has six degrees of freedom because the manipulator has six degrees of freedom, so that the manipulator has arbitrary positions and postures in a three-dimensional space as long as it is within the operation range of the manipulator. Can be realized.

Usually, the manipulator control device performs coordinate conversion of the coordinate values representing the designated target position and target posture into the joint angle values of the joints to obtain the target joint angles, and then realizes these target joint angles. Control each joint axis to do so.

In the case where these manipulators in which the three joints of the tip end are in the roll-pitch-roll relationship to achieve the target position and posture, the following two methods are usually considered.

That is, one is to control the tip position and tip posture of the tip of the manipulator, and the other is to control the position of the intersection of the joint axes of the three joints of the tip and the tip posture of the manipulator. It is what

In the latter case, that is, in the case where the position of the intersection of the joint axes of the three joints at the tip end is set to the target position and the posture of the tip of the manipulator is set to the target posture, the three manipulators at the base end are The target position is defined by the joint,
The target posture is defined by the three joints at the tip. Therefore, since the three joints at the base end and the three joints at the tip can be handled separately, coordinate conversion for obtaining the target joint angle from the target position and the target posture can be easily performed. You can

On the other hand, in recent years, manipulators having seven joints, including those for research and development, have begun to be developed.

In order to be able to control the manipulator to a target position and a target posture which are control targets, it is sufficient for the manipulator to have six joints. Therefore, in the manipulator having seven joints, the joint angles for realizing the target position and the target posture are not uniquely determined. By utilizing this remaining one degree of freedom, it is possible to avoid obstacles, realize a comfortable posture, or avoid a peculiar posture.

The avoidance of an obstacle is to prevent the link of the manipulator from colliding with the obstacle by changing the posture of the elbow while realizing the target position and the target posture.

In order to realize a comfortable posture, an appropriate joint angle is selected from various joint angles capable of realizing the target position and the target posture so that the joint angle of a certain joint does not reach the vicinity of the movable range limit as much as possible. Is to choose.
Generally, if there are six joints, an arbitrary position and posture within the movable range can be realized, but it is assumed that the manipulator has a considerably impossible posture depending on the value of the target position and posture. The human arm usually does not take a very uncomfortable posture. The reason for this lies largely in that the human arm has seven joints. Similarly, also in the manipulator, a comfortable posture can be realized by using seven joints.

A comfortable posture is a posture realized by contributing as many joints as possible. It is limited by the movable range limit of the joint that each joint has a movable range, so that the motion for realizing the control target is biased only to a specific joint.
This is because the movable range of the manipulator becomes narrow.

Further, the avoidance of the peculiar posture means avoidance of a plurality of joint axes being coincident with or parallel to each other. This is because in such a peculiar posture, only one degree of freedom can be realized by a plurality of joints.

As described above, the manipulator having seven joints can utilize one extra joint for various purposes.

[0015]

However, most of the manipulators having seven joints that have been researched and developed so far have been used for avoiding obstacles.
Alternatively, the manipulator's elbow angle was sometimes specified from the outside to achieve the purpose.

Therefore, it has not fully utilized the fact that the manipulator has one extra joint in the absence of special obstacles. Also, no manipulator was found that could achieve a posture that was as comfortable as possible without giving a target value from the outside. That is, there is no manipulator control device that calculates the most reasonable target joint angle among the joint angles that realizes the target position and the target posture, and controls the manipulator so as to follow this target joint angle. Was not fully utilized.

Therefore, an object of the present invention is to solve the above problems of the prior art and to calculate the target joint angle of each joint in which the posture of the manipulator is reasonable within the joint angles that realize the target position and the target posture. It is to provide a manipulator control device.

[0018]

In order to achieve the above object, the present invention is a control device for a manipulator having seven joints, namely, a first joint to a seventh joint, wherein a first end portion of the manipulator is provided. The three joints of the fifth joint to the seventh joint are in a roll-pitch-roll relationship with each other, and the joint axes of the fifth joint to the seventh joint have one intersection, and the control device determines the position of the intersection. According to the target position, the posture of the hand attached to the seventh joint is controlled so as to follow the target posture. The controller controls the target of the fifth joint to the seventh joint based on the value of the target posture of the hand. A posture calculation device for calculating a joint angle, and components of the joint angles of the first to fourth joints, the joint axis direction of the fifth joint, and the seventh joint for realizing the target position from the value of the target position of the intersection. The deviation from the joint axis direction Position calculation device for calculating the joint angle components of the first joint to the fourth joint that act to reduce the size and adding these components to calculate the target joint angles of the first joint to the fourth joint And a joint angle control device that controls the joint angles of the first joint to the seventh joint according to the calculation results of the posture calculation device and the position calculation device, and the position calculation device determines the joint axis direction of the fifth joint. The magnitude of the component of the joint angle of the first joint to the fourth joint, which acts so as to reduce the magnitude of the deviation of the seventh joint from the joint axis direction,
By varying according to the amount of deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint, the magnitude of each deviation of the joint angles of the first to seventh joints from the reference joint angle The target joint angles of the first joint to the fourth joint are calculated so that is within a predetermined range.

[0019]

Since the three joints of the fifth joint to the seventh joint at the tip of the manipulator are in a roll-pitch-roll relationship with each other and the joint axes of the fifth joint to the seventh joint have one intersection, 5th to 7th joints at the tip
The target posture is defined by the individual joints, and the target posture is defined by the four joints of the first to fourth joints at the base end,
Since the three joints at the distal end portion and the four joints at the proximal end portion can be handled separately, coordinate conversion for obtaining the target joint angle from the target position and the target posture can be easily performed.

The position calculation device not only calculates the component of the joint angle of the first joint to the fourth joint for realizing the target position, but also the degree of deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint. The components of the joint angles of the first joint to the fourth joint that act to reduce the height are calculated, and these components are added to calculate the target joint angles of the first joint to the fourth joint.

Further, the position calculation device utilizes the remaining one degree of freedom of the four joints, the first joint to the fourth joint, which defines the target posture, and utilizes the remaining one degree of freedom of the fifth joint and the seventh joint. The magnitude of the component of the joint angle of the first to fourth joints, which acts to reduce the magnitude of deviation from the joint axis direction,
By varying according to the amount of deviation between the joint axis direction of the joint and the joint axis direction of the seventh joint, the magnitude of each deviation of the joint angles of the first joint to the seventh joint from the reference joint angle can be adjusted. The target joint angles of the first to fourth joints are calculated so that they fall within a predetermined range.

[0022]

DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of a manipulator control device according to the present invention will be described below with reference to FIGS.

As shown in FIG. 2, a manipulator 10 having seven joints is attached to a base 20. The seven joints of the manipulator 10 are the first joint 1 at the base end.
It is composed of four joints 1 to 4 and three joints 5 to 15 at the tip. A hand 21 is attached to the tip of the seventh joint 17.

Further, FIG. 2B schematically shows the constitutional relationship of the first joint 11 to the seventh joint 17.

The fifth joint 1 at the tip of the manipulator 10
The five joints, the sixth joint 16 and the seventh joint 17, are in a roll-pitch-roll relationship with each other. That is,
The joint axis direction 17a of the seventh joint 17 faces the tip end direction of the hand 21, and is perpendicular to the joint axis direction 16a of the sixth joint 16. The joint axis direction 15a of the fifth joint 15 is perpendicular to the joint axis direction 16a of the sixth joint 16, and is on the same plane as the joint axis direction 17a of the seventh joint 17. In FIG. 2, the joint axis direction 15 a of the fifth joint 15 and the seventh joint 17
However, generally, the joint axis direction 15a of the fifth joint 15 and the joint axis direction 17a of the seventh joint 17 are deviated from each other.

Further, the joint axes of the three joints of the fifth joint 15, the sixth joint 16 and the seventh joint 17 have one intersection point 22.

The intersection 22 is selected as the control target of the position of the manipulator 10, and the posture of the hand 21 is selected as the control target of the posture of the manipulator. Then, the manipulator 10 calculates and controls the target joint angle of each joint by the manipulator control device 1 so that the position of the intersection 22 follows the target position and the posture of the hand 21 follows the target posture.

Next, FIG. 1 shows an outline of the manipulator control device 1.

In FIG. 1, the manipulator controller 1
Is the position calculation device 2 for calculating the target joint angles of the first joint 11 to the fourth joint 14 from the value of the target position of the intersection point 22, and the fifth joint 15 to the seventh joint 17 from the value of the target posture of the hand 21. And a joint angle control device 4 for controlling the joint angles of the first to seventh joints according to the calculation results of the position calculation device 2 and the posture calculation device 3.

Next, the operation of the manipulator control device 1 will be described with reference to FIG.

Information on the designated target position of the position of the intersection 22 is input to the position calculation device 2 from the outside. Further, information regarding the designated target posture of the posture of the hand 21 is input to the posture calculation device 3 from the outside.

Based on the value of the target position at the intersection 22, the position calculation device 2 calculates not only the first component 5 which is the component of the joint angle of the first joint to the fourth joint for realizing the target position, but also the fifth joint. A second component 6, which is a component of the joint angle of the first joint to the fourth joint, which acts so as to reduce the amount of deviation between the joint axis direction and the joint axis direction of the seventh joint is calculated. The position calculation device 2 calculates the target joint angles of the first to fourth joints by adding the first component and the second component.

Further, the position calculating device 2 changes the magnitude of the second component 6 in accordance with the magnitude of the deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint, whereby the first joint To the fourth joint so that the magnitude of each deviation of the joint angle of the seventh joint from the reference joint angle falls within a predetermined range.
The target joint angle of the joint is calculated.

The calculation results of the position calculation device 2 and the posture calculation device 3 are input to the joint angle control device 4. The joint angle control device 4 operates the manipulator 10 based on the input information.
The joint angles of the first to seventh joints are controlled.

The above contents will be described below using mathematical expressions.

The position vector representing the position of the intersection 22 is x,
When the joint angle vector representing the joint angles of the first joint 11 to the fourth joint 14 is θ, the relationship between the position vector x and the joint angle vector θ can be represented by the function f as in Expression (1).

[0037]

[Equation 1] Here, T represents transposition, and the same applies below.
Further, f1, f2 and f3 each represent a spatial component related to f.

Further, the relationship represented by the equation (2) is established between the minute change amount Δx of the position vector x and the minute change amount Δθ of the joint angle vector θ.

[0039]

[Equation 2] Here, J (θ) is a Jacobian matrix. Also, θj
Is the joint angle of the j-th joint.

Next, the relationship for obtaining Δθ when Δx is given is obtained from the equation (2). This relationship can be expressed as in equation (3).

[0041]

[Equation 3] Next, how to obtain the target joint angle will be described below.

First, the first to fourth joints are calculated using the equation (1).
The current position of the intersection 22 is calculated from the current joint angle of the joint.

Next, after the difference Δx between the target position and the current position of the intersection 22 is calculated, Δθ is calculated according to the equation (3).

By adding Δθ calculated from the equation (3) to the current joint angle, the target joint angles of the first to fourth joints can be obtained.

When the value of Δx becomes large, the equation (3) is not strictly established. However, since the manipulator control device 1 repeatedly performs calculation in a short cycle of several msec to several tens of msec, Δ can be calculated in this short cycle time.
There is no problem because the value of x is sufficiently small.

On the right side of the equation (3), the first term is the intersection point 2
The first component (reference numeral 5 in FIG. 1) which is a component of the joint angle of the first joint 11 to the fourth joint 14 for realizing the target position from the value of the target position of 2. The second term is the fifth
The second component, which is a component of the joint angle of the first joint 11 to the fourth joint 14 that acts to reduce the amount of deviation between the joint axial direction 15a of the joint 15 and the joint axial direction 17a of the seventh joint 17, ( Reference numeral 6) in FIG.

The second component is a component that has no effect on the change in the position of the intersection point 22. That is, no matter how the value of the vector k is set, the position of the intersection 22 does not change.

By utilizing the property of the second component, the value of the vector k is calculated as follows,
In order to minimize the deviation between the joint axis direction 15a of the fifth joint 15 and the joint axis direction 17a of the seventh joint 17,
The target joint angles of the first joint 11 to the fourth joint 14 are calculated.

(1) From the value of the target posture of the hand 21, the unit vector p = (p of the seventh joint 17 in the joint axis direction 17a)
1, p2, p3) are calculated. Where p1, p2, p
3 represents a spatial component of the vector p.

(2) Joint axis direction 15a of the fifth joint 15
Unit vector q = (q1, q2, q3) of the first joint 1
When q = (θ1, θ2, θ3, θ4) is represented by the joint angles θi (i = 1 to 4) of the first to fourth joints 14, the vector k is obtained so as to satisfy the equation (5).

[0051]

[Equation 4] Here, L is the distance between the unit vector p = (p1, p2, p3) and the unit vector q = (q1, q2, q3), and is represented by Expression (5).

[0052]

[Equation 5] The calculation of the equation (4) is performed by the Society of Instrument and Control Engineers “Measurement and Control”, Vol. 25, no. 1, L of the evaluation function p of the second subtask in "Robot Arm Control Method" by Tsuneo Yoshikawa
It is equivalent to performing the calculation of the equation (4.6) in the description of the “robot arm control method” for this L.

However, in this embodiment, unlike the description of the "robot arm control system", it is desired to determine the value of the vector k so that the value of L, which is the evaluation function, is as small as possible. Is marked with a minus sign.

The second component in the equation (3) is obtained by using the vector k calculated by the equation (4). Then, in Expression (3), the sum of the first component and the second component is calculated to obtain Δθ. As described above, the target joint angles of the first joint to the fourth joint are obtained by adding Δθ calculated from the equation (3) to the current joint angle. As a result, the fifth
In order to minimize the deviation between the joint axis direction 15a of the joint 15 and the joint axis direction 17a of the seventh joint 17, the first
The target joint angles of the joints 11 to 14 can be calculated.

A method of setting the magnitude of the vector k will be described below. The magnitude of the vector k is given as the square root of the sum of squares of each component in Expression (4).

The size of the vector k is determined so as to satisfy the following.

(1) As the joint angles of the fifth joint 15 to the seventh joint 17, which are the three joints at the tip of the manipulator 1, deviate greatly from the reference angle which is the reference joint angle, Set the magnitude of the vector k to increase the second component. Accordingly, it is possible to prevent the deviation between the joint axis direction 15a of the fifth joint 15 and the joint axis direction 17a of the seventh joint 17 from becoming extremely large.

(2) As the joint angles of the four joints at the base end of the manipulator 1, that is, the first joint 11 to the fourth joint 14 greatly deviate from the reference angle, the second component in the equation (3) decreases. The magnitude of the vector k is set as follows. As a result, it is possible to prevent the joint angles of the first joint 11 to the fourth joint 14 from being extremely separated from the reference angle.

Next, the setting of the magnitude of the vector k that satisfies the above (1) and (2) will be concretely described by using equations (6) to (10).

Equation (6) shows the deviation vector Δθt from the reference angle of the joint angles of the fifth joint 15 to the seventh joint 17, which are the three joints at the tip.

[0061]

[Equation 6] In addition, the first to eleventh joints, which are four joints at the base end,
Deviation vector Δθ of the joint angle of the joint 14 from the reference angle
Indicates r.

[0062]

[Equation 7] Next, the evaluation function Lt corresponding to the distances of the joint angles of the three joints at the tip from the reference angle is defined by Expression (8).

[0063]

[Equation 8] Further, the evaluation function Lr corresponding to the distances of the joint angles of the four joints at the base end from the reference angle is defined by the equation (9).

[0064]

[Equation 9] In equations (8) and (9), the coefficient ai (1 = 1 to 1
7) is a weighting coefficient as to how much importance is attached to the deviation of the joint angle of the i-th joint from the reference angle, and is a positive constant. In order to reduce the deviation of each joint angle from each reference angle for all joints, for example, the coefficient ai (1 =
All components may be set to 1 as in 1 to 7) = 1.

Next, as shown in equation (10), Lt and L
The ratio λ with r is obtained.

[0066]

[Equation 10] Finally, the magnitude of the vector k is set to be proportional to the ratio λ.

By setting the magnitude of the vector k in this way, it is possible to prevent the deviation between the joint axis direction 15a of the fifth joint 15 and the joint axis direction 17a of the seventh joint 17 from becoming extremely large. In addition, the joint angles of the four joints at the base end can be prevented from being extremely separated from the reference angle.

The deviation of the joint angle from the reference angle due to the roll rotation of the fifth joint 15 around the joint axis and the deviation of the joint angle from the reference angle due to the roll rotation of the seventh joint 17 around the joint axis. It is possible to easily avoid an unreasonable posture by setting the signs to have mutually opposite signs. Further, when only the roll rotation around the joint axis of the seventh joint 17 to which the hand 21 is attached is required, the roll rotation around the joint axis of the fifth joint 15 is stopped and the rotation of the seventh joint 17 is stopped. It does not hurt to take any posture within the allowable range of roll rotation around the joint axis.

According to the configuration of this embodiment, the magnitude of the vector k is set to be proportional to the ratio λ, so that the fifth joint 1
5 joint axis direction 15a and 7th joint 17 joint axis direction 17
It is possible to prevent the deviation from “a” from becoming extremely large, and it is possible to prevent the joint angles of the four joints at the base end from being extremely separated from the reference angle.

As a result, it is possible to select the magnitudes of the respective deviations of the joint angles of the first to seventh joints from the reference joint angle within a reasonable range. Then, it is possible to prevent unnecessarily increasing the joint angle of a specific joint.

Further, this makes it possible to increase the movable range of the entire manipulator 10 without being limited by the movable range limit of the joint angle of a specific joint.

Since the values of the coefficients ai (1 = 1 to 7) in the equations (8) and (9) can be set appropriately, the deviation of the joint angle of each joint from the reference joint angle is calculated. The weighting can be performed according to the allowable range of the deviation.

As a result, in realizing a certain target position and target posture, for example, it is possible to reduce the burden on the joint for which the burden is desired to be reduced.

Although the magnitude of the vector k is assumed to be proportional to the ratio λ, it may be proportional to the ratio λ, for example, the ratio λ.
It may be set so as to be dependent on the amount related to the ratio λ, such as being proportional to the square of.

[0075]

As described above, according to the configuration of the present invention, the position calculation device utilizes the remaining one degree of freedom of the four joints, the first joint to the fourth joint, which define the target posture. , The magnitudes of the components of the joint angles of the first to fourth joints that act to reduce the magnitude of the deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint vary within a predetermined range. Thus, the target joint angles of the first to fourth joints are calculated. As a result, the magnitude of each deviation of the joint angles of the first joint to the seventh joint from the reference joint angle can be set within a predetermined range, and the joint angle that realizes the target position and the target posture of the manipulator can be set. Inside, each joint of the manipulator can take a reasonable joint angle.

[Brief description of drawings]

FIG. 1 is a block diagram of an embodiment of a manipulator control device of the present invention.

FIG. 2 is a perspective view (A) showing a manipulator to be controlled by the manipulator control device of the present invention, and a view (B) schematically showing a structural relationship of joints of the manipulator.

[Explanation of symbols]

 1 Manipulator control device 2 Position calculation device 3 Posture calculation device 4 Joint angle control device 5 1st component 6 2nd component 10 Manipulator 11 1st joint 11a Joint axis direction of 1st joint 12 2nd joint 12a Joint axis of 2nd joint Direction 13 Third joint 13a Joint axis direction of the third joint 14 Fourth joint 14a Joint axis direction of the fourth joint 15 Fifth joint 15a Joint axis direction of the fifth joint 16 Sixth joint 16a Joint axis direction of the sixth joint 17 7th joint 20 Base 21 Minus 22 Intersection

Claims (2)

[Claims] 1. A control device for a manipulator having seven joints of a first joint to a seventh joint, wherein three joints of a fifth joint to a seventh joint at a tip end portion of the manipulator roll / pitch each other.・ Fifth while having a roll relationship
The joint axis of the joint to the seventh joint has one intersection, and the control device controls the position of the intersection to follow the target position and the posture of the hand attached to the seventh joint to follow the target posture. The control device is for realizing a target position from a posture calculation device that calculates a target joint angle of the fifth joint to a seventh joint from the value of the target posture of the hand and a target position value of the intersection. A first joint that acts to reduce the magnitude of the deviation between the joint angle component of the first joint to the fourth joint, the joint axial direction of the fifth joint, and the joint axial direction of the seventh joint
A position calculation device that calculates a component of a joint angle of the joint to the fourth joint and calculates a target joint angle of the first joint to the fourth joint by adding these components, the posture calculation device, and the position calculation device And a joint angle control device that controls the joint angles of the first to seventh joints according to the calculation result of 1., and the position calculation device detects the deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint. The magnitude of the component of the joint angle of the first joint to the fourth joint, which acts to reduce the magnitude, is determined according to the magnitude of the deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint. By varying, the target joint angles of the first joint to the fourth joint are calculated so that the magnitudes of the deviations of the joint angles of the first joint to the seventh joint from the reference joint angle are within a predetermined range. Characteristic manipulator control device. 2. A first unit which acts so as to reduce the amount of deviation between the joint axis direction of the fifth joint and the joint axis direction of the seventh joint.
The magnitudes of the components of the joint angles of the joints to the fourth joint are the distances from the reference joint angle of the joint angles of the fifth joint to the seventh joint and the distances of the joint angles of the first joint to the fourth joint from the reference joint angle. The manipulator control device according to claim 1, wherein the manipulator control device is proportional to a ratio with or a quantity related to this ratio.