JP2002292585A - Control device for multi-joint robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To control a multi-joint robot equipped with several legs having many joints so as not to interfere legs each other. SOLUTION: The control device for the multi-joint robot has the following means. When a target position of top leg Ptd is given it has a knee joint target position setting means that sets up a target position of knee joint Pmd with a specified condition. The present leg top position is denoted by Pt , the displacement of leg top position without rotating other joints except No.i joint rotated by Δθi is denoted by ΔPt (Δθi ), and the displacement of knee joint position without rotating other joints except No.i joint rotated by Δθi is denoted by ΔPm (Δθi ). The evaluation function E is defined by E=(Ptd -Pt -ΔPt (Δθi ))<2> +(Pmd -Pm -ΔPm (Δθi ))<2> . By using this evaluation function E it has a No.i joint rotation computation means that calculates the value of Δθi from the first through Nth joint sequentially. Until the value of evaluation function E becomes less than a specified threshold value, the sequential calculations of rotation angle for each joint are repeated.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

The present invention relates to a control device for an articulated robot, and more particularly to a control device for an articulated robot having a plurality of legs having redundant degrees of freedom.

[0002]

2. Description of the Related Art FIG. 8 shows a four-legged walking robot 10 described in the 4th Annual Meeting of the Robotics Society of Japan (December 1986, pp. 383-386). The 13th Robotics Society of Japan (November 1995, pp899-900)
A two-legged walking robot is described. Furthermore, at the 10th Micromachine Exhibition (October 1999), walking robots having various numbers of legs are known, such as an eight-legged walking robot being exhibited. In general, the walking robot 10 is stable as long as the center of gravity 14 of the robot exists on a polygon 12 connecting the tips of the legs in contact with the moving surface. As the number of legs 16 of the walking robot 10 increases, the center of gravity 14 of the robot becomes closer to the center of the polygon 12.
It is hard to fall down, and stable walking can be performed even on undulating or uneven surfaces. In addition, even with the same number of legs, the more joints provided on the legs, the wider the movable range of the legs, so that it is possible to get over a high step.

On the other hand, Japanese Patent No. 3068577 discloses a control device for an articulated robot. As shown in FIG. 9, the control device includes a main body B, N joints H ₁ , H ₂ ,..., H _N attached to the main body B and N links L ₁ , L ₂ , ..., a plurality of legs having L _N ,
For controlling a robot having FIG.
Indicates a control algorithm of the control device. The control algorithm, X of the target position P _td leg end of the robot, Y, Z coordinates _{(P tXd, P tYd, P} tZd), the coordinates of the current position P _t of the leg tip (P _tX, P _tY, P _tZ ), _rotate the i-th joint by Δθ ゜, and when the other joints are fixed, move the distance of the leg tip in each direction by (ΔP _tX (Δθ _i ), ΔP
The rotation angle of each joint is calculated using the following evaluation function E as _tY (Δθ _i ) and ΔP _tZ (Δθ _i )).

[Number 1] _{E = (P tXd -P tX -ΔP} tX (Δθ i)) 2 + (P tYd -P tY -ΔP tY (Δθ i)) 2 + (P tZd -P tZ -ΔP tZ (Δθ i )) ² (Equation 1)

[0004] by partially differentiating the Equation 1 in [Delta] [theta] _i, determining the [Delta] [theta] _i to the value zero. That is,

## EQU2 ## Δ∂ _i is calculated by the relational expression of (関係 E / (∂Δθ _i )) = 0 (Equation 2). Specifically, FIG.
As shown in (1), in step S101, the X, Y, and Z coordinates of the target position _{Ptd where} the leg tip should be moved are input. Here, the moving distance of each direction of the leg tip, the rotation angle [Delta] [theta] ₁ of each joint, [Delta] [theta] _2, as a function of ·· Δθ _N (ΔP _tX
(Δθ _i ), ΔP _tY (Δθ _i ), ΔP _tZ (Δθ _i )). In step S102, [Delta] [theta] by the rotation angle [Delta] [theta] other than the first joint by a constant, the rotation angle [Delta] [theta] ₁ of the first joint as a variable, by partially differentiating the evaluation function E in [Delta] [theta] _1, which enumeration the value 0 Calculate the value of ₁ . In this calculation, the rotation angles of the joints other than the first joint are fixed, only the first joint is arbitrarily moved, and the rotation angle Δ at which the tip of the leg is closest to the target position ( _PtXd , _PtYd , _PtZd ).
corresponds to obtaining a theta _1. Here, in the following steps, a value obtained by multiplying the calculated Δθ ₁ by a predetermined coefficient between 0 and ₁ is used as the rotation angle Δθ ₁ of the first joint.

Similarly, in step S103, the rotation angle Δθ of the second joint is set as a constant, and the rotation angle Δ
The theta ₂ as variables, evaluated by partially differentiating the function E in [Delta] [theta] _2, the values to calculate the value of [Delta] [theta] ₂ by enumeration and 0, multiplied by a predetermined factor between 0 and 1 to the value Let the value be the rotation angle Δθ ₂ of the second joint. However, at this time, the calculation is performed while fixing the rotation angle Δθ ₁ of the first joint to the value determined in the previous step S102. The same calculation is performed in step S104
Up to the third, fourth,..., Nth joints. Next, in step S105, the rotation angles Δ of the first to Nth joints obtained by the calculation up to step S104
Substituting the values of θ ₁ , Δθ ₂ ,..., Δθ _N into (Equation 1),
The value of the evaluation function E is calculated. If the obtained value of E is equal to or _smaller than the preset threshold value E ₀ , the process is terminated, and the joint is rotated by the calculated rotation angle of each joint. When the value of the evaluation function E is greater than the threshold E _0, the process returns to step S102, the value of the evaluation function E is the same process is repeated until less than the threshold E _0, the rotation of each joint Calculate the angle.

[0006]

However, when the number of legs is increased or the number of joints is increased, interference between the legs, such as contact between the legs, is a problem in the control by the above-described conventional calculation algorithm. Becomes That is, when the entire leg always moves in a vertical plane, such as when the number of joints of each leg is small, control each leg so that the legs do not interfere with each other. However, it is difficult to control a large number of legs having many joints and a wide movable range so as not to interfere with each other. The conventional algorithm shown in FIG. 10 is for moving the leg tip to a predetermined target position. Therefore, since the movement of the intermediate portion between the root and the tip of the leg is not controlled, there is a problem that interference between the legs at the intermediate portion of the leg cannot be prevented.

On the other hand, when a walking robot having legs having a large number of joints walks in a narrow part with a low ceiling,
It is necessary to walk with the robot's overall height lowered by extending the legs almost horizontally. In this case, there is a problem that an excessive load acts on a joint near the base of the leg in order to support the weight of the robot body.

Accordingly, the present invention provides an articulated robot control device capable of controlling an articulated robot having a plurality of legs having a large number of joints so that the legs do not interfere with each other. It is intended to be. In addition, the present invention provides a multi-joint robot that does not apply an excessive load to a leg joint when the multi-joint robot having a plurality of legs having a large number of joints walks in a narrow part with a low ceiling height. The purpose of the present invention is to provide a control device.

[0009]

In order to achieve this object, a control apparatus for an articulated robot according to the present invention comprises a control apparatus for an articulated robot having a plurality of legs having link members connected by N joints. When the control device _receives a target position P _td of the tip of the leg, the knee device provided between the first joint, which is the base joint of the leg, and the N-th joint, which is the tip joint, of the leg. the target position P _md of the M joint is rheumatoid, knee joint target position setting means for setting a predetermined condition, the current leg tip position and P _t, rotate the first joint by [Delta] [theta] _1, the other joint ΔP _t
(Δθ ₁ ), the current knee joint position is P _m , the displacement amount of the knee joint position when the first joint is rotated by Δθ ₁ and the other joint is not rotated is ΔP _m (Δθ ₁ ), and the evaluation is performed. Function E
E = (P _td −P _t −ΔP _t (Δθ ₁ )) ² + (P _md −P _m −
As ΔP _m (Δθ ₁ )) ² , the value of Δθ ₁ that minimizes the value of the evaluation function E is calculated by the least square method, and the value obtained by multiplying Δθ ₁ by a predetermined coefficient is the rotation angle of the first joint. Similarly, the first joint rotation angle calculating means sets the displacement of the leg tip position when the ith joint is rotated by Δθ _i and the other joints are not rotated by ΔP.
_t (Δθ _i ), and the displacement of the knee joint position when the ith joint is rotated by Δθ _i and the other joints are not rotated is ΔP _m
(Δθ _i ), and the evaluation function E is E = (P _td −P _t −ΔP _t (Δθ _i )) ² + (P _md −P _m −
As ΔP _m (Δθ _i )) ² , the value of Δθ _i that minimizes the value of the evaluation function E is calculated by the least square method, and the calculated Δθ _i is multiplied by a predetermined coefficient to obtain a value from the second joint. I-th joint rotation angle calculating means for sequentially calculating the rotation angles of the joints up to N joints, wherein the first joint rotation angle calculating means and the second joint rotation angle calculating means are used until the value of the evaluation function E falls below a predetermined threshold value. The i-joint rotation angle calculating means sequentially and repeatedly calculates the rotation angle of each joint.

In this configuration, the control device of the articulated robot includes an evaluation function E used to calculate the rotation angle Δθ of each leg joint required to move the tip of the articulated robot to the target position P _td. in, and in consideration of the position P _m of the knee joint of the leg middle section. With this configuration, it is possible to control not only the tip of the leg of the articulated robot but also the position of the knee joint at the middle part of the leg, thereby preventing interference between the legs.

Further, the target position P _td is the articulated robot so as to walk in a predetermined direction, the target position P _td, good give a time series for each leg.

The knee joint target position setting means takes the Y coordinate in the traveling direction of the walking of the articulated robot, sets the Y coordinate component of the base position of the leg to _Pr, and _sets the Y coordinate component of the target position of the tip of the leg to _{Yr. PtYd} , K is an arbitrary constant between 0 and 1, and the Y coordinate component _PmYd of the target position of the knee joint is set by the formula of _PmYd = _PrY + K ( _{PtYd- PrY} ). The joint rotation angle calculation means is P _mY
Was a Y coordinate components of P _m, the [Delta] P _mY and Y coordinate components of the [Delta] P _m, the evaluation function _{E, E = (P td -P} t -ΔP t (Δθ 1)) 2 + (P mYd -P mY
The value of Δθ ₁ is calculated as −ΔP _mY (Δθ ₁ )) ² , and the rotation angle of the first joint is obtained by multiplying this Δθ ₁ by a predetermined coefficient. E = (P _td −P _t −ΔP _t (Δθ _i )) ² + (P _mYd −P _mY
It is preferable to calculate the value of Δθ _i as −ΔP _mY (Δθ _i ) ^{2 and} to calculate the rotation angles of the second to N-th joints by multiplying Δθ _i by a predetermined coefficient.

[0013] In this configuration, Y coordinate components P _MYD target position of the knee joint is established between the Y coordinate components P _tyd target position of the root of the Y coordinate component P _rY and leg tips of the legs. Thus, the leg can be moved in a state where the base of the leg, the knee joint, and the tip of the leg are substantially on one plane, and interference between the legs can be effectively prevented.

Further, the articulated robot can walk in a narrow part.
So that the M-th joint touches the walking road surface,
Is lower than the height of the ceiling on the walking road.
Place P _md, P_tdMay be given in chronological order for each leg.
No.

In this configuration, the multi-joint robot walks on a narrow part with a low ceiling height in a state where the M-th joint, which is the knee joint of the leg, is grounded on the walking road surface and the leg tip is lifted off the walking road surface. Thereby, when the articulated robot walks in a narrow portion with a low ceiling height, it is possible to prevent an excessive load from acting on the leg joint.

[0016]

Next, an embodiment of the present invention will be described with reference to the drawings. First, referring to FIGS. 1 to 5,
A first embodiment of the present invention will be described. FIG. 1 shows a control device 1 of the first embodiment, an articulated robot R controlled by the control device 1, and a power supply V for driving the articulated robot R. Here, the multi-joint robot R having eight legs is controlled by the control device 1 of the first embodiment.
Will be described. The control device 1 according to the first embodiment includes a knee joint target position setting unit (not shown) for setting a target position of a knee joint which is a joint of a middle part of a leg.
And joint rotation angle calculation means (not shown) for calculating the rotation angle of each joint of the leg. Articulated robot R
Has a central body 2 and eight legs 4a to 4h extending four each from both sides of the body 2. Here, an axis in the longitudinal direction of the leg 4 is defined as an X axis, an axis orthogonal to the X axis and parallel to the road surface is defined as a Y axis, and a vertical axis orthogonal to the X and Y axes is defined as a Z axis. Each leg 4 is provided with four joints 6c rotatable about the X axis and four joints 6b rotatable about the Y axis, respectively.
Further, it has eight link members 8 for connecting the joints 6. Each of the joints 6b and 6c includes a motor (not shown), and is driven by a power supply V based on a control signal generated by the control device 1. In this specification, the “leg” includes a robot arm having multiple joints, which is used for purposes other than walking the robot and supporting the robot body.

Next, the operation of the control device 1 according to the first embodiment of the present invention will be described with reference to FIGS. FIG.
(A)-(d) is a figure which shows the walking pattern at the time of moving each leg 4 of the articulated robot R shown in FIG. 1 sequentially, and making the robot R walk in a Y direction. First, FIG.
In the state (a), the second leg 4b on the left and the fourth leg 4h on the right are lifted from the walking surface as a free leg, and the tip of the leg is moved in the Y direction by a distance λ. At the same time, the torso 2 is moved in the Y direction by a distance λ / 4 by moving the supporting leg, which is a leg that is in contact with a walking surface other than the free leg. Subsequently, in the state shown in FIG. 2B, the left third leg 4c and the right first leg 4e are moved by a distance λ as free legs, and the trunk 2 is moved by a distance λ by the remaining supporting legs.
/ 4 forward. Similarly, in state (c), the left side is No. 4 and the right side No. 2 is a free leg, and in state (d), the left side No. 1 and the right side No. 3 are free legs, and the body 2 is advanced by a distance λ / 4, respectively.

After the steps shown in FIGS. 2A to 2D, the body 2 advances in the Y direction by a distance λ, and the state of each leg returns to the state shown in FIG. 2A. Furthermore, the articulated robot R can continue moving forward to the target point by repeating the steps of FIGS.

FIG. 3 shows an example of a leg movement pattern when the tip of the leg 4b is moved by a distance λ in the Y direction. In the motion pattern shown in FIG. 3, first, the tip of the leg 4b is raised in the Z direction (trajectory u1), then, the tip is advanced diagonally forward (track u2), and finally, the tip is lowered in the −Z direction. (Trajectory u3).

FIG. 4 is a flowchart summarizing the procedures of the walking patterns shown in FIGS. First, in step S1, the leg tips of the legs 4b and 4h are lifted along the track u1. Next, in step S2, the legs 4b, 4
The leg tip of h is advanced by a distance λ along the trajectory u2. Next, in step S3, the leg tips of the legs 4b and 4h are lowered along the track u3 and grounded.

Further, in step S4, the legs 4c and 4e are advanced in the same procedure as in steps S1 to S3. In step S5, the legs 4d and 4f are advanced. In step S6, the legs 4a and 4h are advanced. As described above, the torso 2 of the robot R advances in the Y direction by the distance λ according to the one-cycle walking pattern. Subsequently, in step S7, it is determined whether or not the body 2 has reached the destination.
Steps 1 to S6 are repeated.

When the leg does not have the redundant joint, the trajectory of the tip of the leg is defined, so that the rotation angle at which each joint should rotate is uniquely determined. However, since the legs 4 have redundant joints, the same leg tip position can be realized with various rotation angles of each joint 6. An example of an algorithm for determining the rotation angle of each joint 6 with respect to the target position of the leg tip is the conventional algorithm shown in FIG. However, since the conventional algorithm controls only the position of the tip of the leg, it is not possible to prevent interference between the legs at an intermediate portion of the leg.

Next, a control algorithm used in the control device 1 of the first embodiment will be described with reference to FIG. Using the algorithm of FIG.
Rotation angle θ of each joint 6 necessary to move the tip of the leg to a predetermined target position for all eight legs 4a to 4h
Calculate _i . In the algorithm shown in FIG. 5, the position of the knee joint is controlled by determining the target position not only for the tip position of the leg 4 but also for the Y coordinate of the knee joint which is the joint 6 between the root and the tip of the leg. , Preventing interference between the legs. As the knee joint whose position is to be controlled, any joint existing in the middle part of the leg can be selected according to the application. Further, in the present embodiment, the control is performed by setting the target value for the value of the Y coordinate of the knee joint because the legs 4 are sequentially sent in the Y direction, so that interference between the legs is likely to occur in this direction. Because.

First, in step S11, the coordinates P of the tip of the leg are calculated from the current rotation angle θ _i (t) of each joint 6.
_{t (P tX, P tY,} P tZ), and calculates the Y coordinate components P _mY knee joint position. Next, in step S12, based on the walking pattern shown in FIGS. 2 and 3, the target position P _td leg tip _{(P tXd, P tYd, P} tZd) setting a. In step S13, a target value _PmYd of the Y coordinate of the knee joint is calculated by (Equation 3).

P _mYd = P _rY + K (P _tYd −P _rY ) (Equation 3) where P _rY is the Y coordinate component at the root of the current leg 4,
K is an arbitrary constant from 0 to 1. The value of the constant K can be arbitrarily and appropriately determined depending on the application.

In step 14, the rotation angle Δθ of each joint 6 is determined in the same procedure as the conventional control algorithm shown in FIG.
Is calculated, but in the present embodiment, the position of the knee joint is also considered. First, an evaluation function E in the case where the first joint is rotated by Δθ ₁ and the other joints are not rotated is defined as (Equation 4).

E = (P _tXd −P _tX −ΔP _tX (Δθ ₁ )) ² + (P _tYd −P _tY −ΔP _tY (Δθ ₁ )) ² + (P _tZd −P _tZ −ΔP _tZ (Δθ ₁₎ )) ² + (P _mYd −P _mY −ΔP _mY (Δθ ₁ )) ² (Equation 4) where ΔP _tX (Δθ ₁ ), ΔP _tY (Δθ ₁ ), ΔP
_tZ (Δθ ₁ ) is X, Y, Z of the movement amount of the leg tip position when the first joint is rotated by Δθ ₁ and the other joints are not rotated.
ΔP _mY (Δθ ₁ ) is a directional component, and is the amount of movement of the knee joint position in the Y direction when the first joint is rotated by Δθ ₁ and the other joints are not rotated.

(Equation 4) is partially differentiated by Δθ ₁ and the result is equalized to 0.

(∂E / (∂Δθ ₁ )) = 0 (Equation 5) is obtained, and Δθ ₁ is calculated from the relational expression of (Equation 5).
Here, in the following steps, a value obtained by multiplying the calculated Δθ ₁ by a predetermined coefficient between 0 and ₁ is used as the rotation angle Δθ ₁ of the first joint. Similarly for the second joint,
(Equation 4) and using the equation obtained by replacing the [Delta] [theta] ₁ (Formula 5) in the [Delta] [theta] ₂ to calculate the [Delta] [theta] _2, can be defined [Delta] [theta] ₂ used in the following step. The same procedure is performed for the third to eighth joints to determine Δθ _{3 to} Δθ ₈ . The predetermined coefficient between 0 and 1 can be arbitrarily and appropriately determined according to the application. Here, multiplying Δθ _i calculated by (Equation 5) by the predetermined coefficient is ΔΔ _i
This is because the value of θ _i is properly converged by iterative calculation.

In step S14, the deviation of the leg tip position from the target value is calculated as X,
Each of the Y and Z direction components is considered, and the deviation of the knee joint position from the target value is considered for the Y direction component. However, depending on the application, a combination of coordinate components to be included in the calculation of the evaluation function E can be arbitrarily selected. In this specification, P _t, as such P _m, the position display symbol coordinate component in the subscript is not specified shall include any and all combinations of coordinate components of the position coordinates. Therefore, an evaluation function obtained by arbitrarily combining each coordinate component of the leg tip position and each coordinate component of the knee joint position is described by a symbol such as (Equation 6).

E = (P _td −P _t −ΔP _t (Δθ ₁ )) ² + (P _md −P _m −ΔP _m (Δθ ₁ )) ² (Equation 6)

[0028] In step S15, when each joint 6 is rotated by a rotation angle [Delta] [theta] ₁ to [Delta] [theta] ₈ obtained in step S14, each joint, checks never exceed the movable range. That is, for the first to eighth joints,

It is checked whether the relationship of | θ _i (t) + Δθ _i | <θ _imax (Formula 7) is satisfied. Where θ
_imax is a predetermined rotation angle allowable by the i-th joint 6. For the joint 6 that satisfies the relationship of (Equation 7), the process proceeds to step S17, and for the joint 6 that is not satisfied, the process of step S16 is performed.

[0029] Step S16 is a process performed when the joint 6 has exceeded the allowable rotation angle, the rotation angle [Delta] [theta] _i,

８ 8 Replace with Δθ _i = 0 (Equation 8) and proceed to step 17. In step S17, each joint 6 calculates the rotation angle Δθ _{i of} each joint 6 calculated in step S16 from the current rotation angle θ _i (t).
After that, the position P _{t of} the tip of the leg after rotation and the Y coordinate component P _mY of the knee joint position are calculated.

[0030] At step S18, P _t calculated in step S17 to calculate the value of _{(P tX, P tY, P} tZ), by substituting P _mY in (Equation 9) the evaluation function E.

E = (P _tXd −P _tX ) ² + (P _tYd −P _tY ) ² + (P _tZd −P _tZ ) ² + (P _mYd −P _mY ) ² (Equation 9) E calculated in S18,
Comparing the predetermined threshold value E ₀ which is set in advance. If the value of E is less than E _0, the process proceeds to step S20. E
Is larger than E ₀ , the process returns to step S14 and the processes of steps S14 to S18 are repeated.

At step S20, the rotation angle θ _i (t +) obtained by adding the current rotation angle θ _i (t) of each joint 6 to the rotation angle Δθ _i of each joint 6 calculated at step S16.
1) is set as the next target rotation angle of each joint, and the process ends. That is,

The next target rotation angle θ _i (t + 1) is calculated by the following equation: θ _i (t + 1) = θ _i (t) + Δθ _i (Equation 10)

A motor (not shown) provided at each joint 6 of the articulated robot R is driven by a power supply V to rotate each joint 6 to a target rotation angle θ _i (t + 1). In this manner, the target positions of the tip of the leg are sequentially given based on the walking pattern of the articulated robot R, the processing of the algorithm in FIG. 5 is performed for each leg, the target rotation angle is calculated, and the articulated robot R is walked accordingly. Let it.

In this embodiment, since the rotation angle of the joint 6 of the leg 4 is calculated in consideration of the position of the knee joint, the articulated robot can walk without interference between the legs. In addition, this effect is achieved by the motion mechanism analysis software Workin
g Confirmed by simulation and experiment using Model 3D (manufactured by MSC).

In the first embodiment, the control is performed in consideration of only the Y-direction component of the knee joint. However, as a modification, the control is performed in consideration of another direction component or two or more direction components. May be. In the present embodiment, the control is performed in consideration of the position of one knee joint for each leg 4.
The control may be performed on the two legs in consideration of the positions of two or more knee joints. Further, in the first embodiment, (Equation 3)
, The target position of the knee joint is set, but the target position of the knee joint may be set by another relational expression or by a preset walking pattern.

Next, a second embodiment of the present invention will be described with reference to FIGS. In the second embodiment, the first
This is a control device for causing the articulated robot R described in the embodiment to walk in a narrow portion. The configuration other than the walking pattern and the control algorithm used by the control device 1 is the same as that of the first embodiment, and the description is omitted.

FIG. 6A shows a conventional control device.
FIG. 7 is a plan view of a walking pattern when the articulated robot R walks a narrow part, and FIG.
As shown in the figure, when the articulated robot R is caused to walk in a narrow portion having a low ceiling height _Hmax by the conventional control device, each leg 4 is extended substantially in the horizontal direction, and the articulated robot R
It is necessary to make the total height of H less than or equal to H _max to allow walking. When the articulated robot R walks in such a posture, the distance W ₁ between the grounding points of the legs 4 increases, and an excessive load acts on the joint 6 near the base of the legs 4 to support the robot R's own weight. .

FIGS. 6C and 6D show walking patterns when the articulated robot R is controlled by the control device according to the second embodiment of the present invention. The movement of each leg viewed from above is the same as the walking pattern in FIG. In this embodiment,
The articulated robot R is contact with the road surface in each knee joint, in order to walk float leg tip upward, short distance W ₂ between the ground point of the legs 4, an excessive load to the joint 6 does not act .

FIG. 7 shows a control algorithm used by the control device of the second embodiment. The difference between the algorithm of FIG. 7 and the algorithm of the first embodiment shown in FIG. 5 is that in the first embodiment, the control is performed in consideration of the X, Y, and Z coordinates of the tip of the leg and the Y coordinate of the knee joint. While
In the second embodiment, the control is performed in consideration of the Y, Z coordinates of the tip of the leg and the X, Y, Z coordinates of the knee joint.

[0039] First, in step S31, the rotation angle θ _i (t) Karaashi tip of Y of each current joint, Z coordinate component P _tY, P _tZ, and the coordinates P _m of the knee joint position (P _mX,
P _mY, calculates the P _{m Z).} Next, in step S32, the knee joint target position P _md (P _mXd , P _mYd , P _mYd ,
P _mZd ). Furthermore, the target value P of the Z coordinate of the tip of the leg
_tZd and set to the leg tip is lower than the ceiling height H _max. In step S33, the target value P of the Y coordinate of the tip of the leg
_{tY d} is calculated by (Equation 11).

P _tYd = P _rY + K (P _mYd −P _rY ) (Equation 11) where P _rY is the Y coordinate component of the current foot base, and K is an arbitrary constant of 0 to 1. The value of the constant K can be arbitrarily and appropriately determined depending on the application.

In step S34, step S1 in FIG.
The rotation angle Δθ of each joint 6 is calculated in the same procedure as in step 4.
In the second embodiment, the evaluation function E corresponding to (Equation 4) is

E = (P _tYd −P _tY −ΔP _tY (Δθ ₁ )) ² + (P _tZd −P _tZ −ΔP _tZ (Δθ ₁ )) ² + (P _mXd −P _mX −ΔP _mX (Δθ ₁₎ )) ² + (P _mYd −P _mY −ΔP _mY (Δθ ₁ )) ² + (P _mZd −P _mZ −ΔP _mZ (Δθ ₁ )) ² (Equation 12) to calculate the rotation angle Δθ of each joint 6. ing.

The processing in steps S35 and S36 is the same as that in FIG.
Since steps S15 and S16 are the same, description thereof will be omitted. Next, in step S37, each joint 6
From the current rotation angle θ _i (t), the Y and Z coordinate components P _tY and P _{tZ of} the tip of the leg after rotation by the rotation angle Δθ _i of each joint 6 calculated in step S36, and the position of the knee joint to calculate the P _m.

[0042] At step S38, P _tY calculated in step S37, to calculate the value of _{P tZ, P m (P mX} , P m Y, P mZ) Evaluation by substituting (Equation 13) function E.

[Number 13] _{E = (P tYd -P tY)} 2 + (P tZd -P tZ) 2 + (P mXd -P mX) 2 + (P mYd -P mY) 2 + (P mZd -P mZ) 2 (Equation 13) The processing in steps S39 and S40 is performed in step S39 in FIG.
Since these are the same as 19 and S20, the description is omitted.

As described above, FIGS. 2 and 6C and 6D
The target positions of the leg tips are sequentially given based on the walking pattern shown in (1), the algorithm of FIG. 7 is applied to each leg to calculate the target rotation angle, and the articulated robot R is caused to walk accordingly. In the present embodiment, since the knee joint walks with the ground touching the road surface, an excessive load does not act on the joint 6 even when the articulated robot R walks in a narrow part.

While the preferred embodiments of the present invention have been described above, various modifications may be made to the disclosed embodiments without departing from the scope or spirit of the present invention and within the technical scope described in the claims. Can be changed. In particular, the present embodiment has eight joints each having eight joints.
Although the control device controls the articulated robot R having the legs, the control device of the present invention can control a robot having an arbitrary number of legs having an arbitrary number of joints. Further, the walking pattern of the robot may be a walking pattern other than the pattern shown in the above embodiment. Furthermore, the control device of the present invention can be configured to control not only walking control of a robot leg having multiple joints, but also a robot arm that performs an arbitrary operation other than walking.

[0045]

According to the articulated robot control apparatus of the present invention, an articulated robot having a plurality of legs having a large number of joints can be controlled so that the legs do not interfere with each other. Further, the control device of the articulated robot of the present invention allows an articulated robot having a plurality of legs having a large number of joints to walk in a narrow part with a low ceiling height without applying an excessive load to the leg joints. can do.

[Brief description of the drawings]

FIG. 1 is a top view illustrating a control device according to a first embodiment of the present invention and an articulated robot controlled by the control device.

FIG. 2 is a schematic top view showing a walking pattern of the articulated robot controlled by the control device of the first embodiment.

FIG. 3 is a side view showing the movement of the legs of the articulated robot controlled by the control device according to the first embodiment.

FIG. 4 is a flowchart showing a walking pattern of the articulated robot controlled by the control device of the first embodiment.

FIG. 5 is a calculation algorithm for calculating a rotation angle of each joint of a leg used in the control device of the first embodiment.

6A and 6B are a top view and a front view of a walking pattern by a conventional control device of an articulated robot, respectively. FIGS. 6A and 6B are top and front views of a walking pattern by a control device according to a second embodiment of the present invention. Are shown in (c) and (d).

FIG. 7 is a calculation algorithm for calculating a rotation angle of each joint of a leg used in the control device of the second embodiment.

FIG. 8 is a top view and a perspective view of a conventional four-legged walking robot.

FIG. 9 is a schematic diagram showing a configuration of a conventional robot leg.

FIG. 10 shows a diagram used in a conventional robot controller.
This is a calculation algorithm for calculating the rotation angle of each joint of the leg.

[Explanation of symbols]

E evaluation function E ₀ evaluation function threshold P _m knee position P _md knee target position P _t leg distal end position P _td leg tip target position [Delta] P _t ([Delta] [theta]) leg tip displacement [Delta] P _m ([Delta] [theta]) knee joint displacement Δθ Rotation angle of each joint R Articulated robot V Power supply 1 Control device of articulated robot 2 Body 4 Legs 6 Joints 8 Link members

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 3C007 CS08 MS03 MS05 WA12 WB02 WB05

Claims (4)

[Claims] 1. A multi-joint robot control device having a plurality of legs in which link members are connected by N joints, the control device being configured to provide a target position P _td of the tip of the leg. A target position P _{md of an} M-th joint which is a knee joint provided between a first joint which is a root joint of the leg and an N-th joint which is a distal joint.
And a knee joint target position setting means for setting a predetermined condition, the current leg tip position and P _t, rotate the first joint by [Delta] [theta] _1, the amount of displacement of the leg tip position of the case of not rotating the other joint the [Delta] P _t ([Delta] [theta] ₁₎ and then, the current of the knee joint position and P _m, to rotate the first joint by [Delta] [theta] _1, [Delta] P the amount of displacement of the knee joint position of the case of not rotating the other joints _m ([Delta] [theta] ₁₎ and then, an evaluation function _{E, E = (P td -P} t -ΔP t (Δθ 1)) 2 + (P md -P m -
As ΔP _m (Δθ ₁ )) ² , the value of Δθ ₁ that minimizes the value of the evaluation function E is calculated by the least square method, and the value obtained by multiplying Δθ ₁ by a predetermined coefficient is the rotation angle of the first joint. Similarly, the displacement amount of the leg tip position when the ith joint is rotated by Δθ _i and the other joints are not rotated is ΔP _t (Δθ _i )
The displacement amount of the knee joint position when the ith joint is rotated by Δθ _i and the other joints are not rotated is ΔP _m (Δθ _i ), and the evaluation function E is E = (P _td −P _t −ΔP) _t (Δθ _i )) ² + (P _md −P _m −
As ΔP _m (Δθ _i )) ² , the value of Δθ _i that minimizes the value of the evaluation function E is calculated by the least squares method, and the calculated Δθ _i is multiplied by a predetermined coefficient to obtain a value from the second joint. And i-th joint rotation angle calculating means for sequentially calculating the rotation angles of the joints up to the N joints.
A control apparatus for a multi-joint robot, wherein the first joint rotation angle calculation means and the i-th joint rotation angle calculation means sequentially and repeatedly calculate the rotation angles of the respective joints. 2. The control apparatus for an articulated robot according to claim 1, wherein the target position P _td is given in a time series for each leg so that the articulated robot walks in a predetermined direction. . 3. The knee joint target position setting means obtains a Y coordinate in a traveling direction of the walking of the articulated robot, sets a Y coordinate component of a base position of the leg to _PrY, and _sets a Y position of a target position of the tip end of the leg.
The coordinate component is P _tYd, and K is an arbitrary constant between 0 and 1, and the Y coordinate component P _mYd of the target position of the knee joint is set by a formula of P _mYd = P _rY + K (P _tYd −P _rY ). The first joint rotation angle calculating means _sets P _mY as the Y coordinate component of P _m , _sets ΔP _mY as the Y coordinate component of ΔP _m , and sets the evaluation function E to E = (P _td −P _t −ΔP _t (Δθ ₁ )) ² + (P _mYd −P _mY
-ΔP _mY (Δθ ₁₎₎ ² the value of [Delta] [theta] ₁ is calculated as to obtain the rotation angle of the first joint by multiplying a predetermined coefficient to the [Delta] [theta] _1, is the i-th joint rotation angle calculating means, the evaluation function the _{E, E = (P td -P} t -ΔP t (Δθ i)) 2 + (P mYd -P mY
3. The method according to claim ² , wherein a value of Δθ _i is calculated as −ΔP _mY (Δθ _i )) ² , and the rotation angles of the second to N-th joints are obtained by multiplying Δθ _i by a predetermined coefficient. Control device for articulated robot. 4. The target position P such that the M-th joint is in contact with a walking road surface so that the articulated robot can walk in a narrow portion, and the tip end position of the leg is lower than the height of the ceiling of the walking road surface. _2. The control device for an articulated robot according to claim 1, wherein _md and _Ptd are given in time series for each leg.