JP3068577B2 - Control device for articulated robot - Google Patents

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 calculating a joint angle of an articulated robot having redundant degrees of freedom.

[0002]

2. Description of the Related Art FIG. 1 shows a general articulated robot (articulated manipulator) 1c having redundant redundancy. Here, the terms robot and manipulator are used interchangeably. This multi-joint manipulator 1c includes the joints 1a, 1
1d are connected by a number of links 1b, and each joint 1a, 1p, 1q, 1r... 1d includes a motor (not shown) for rotating the joint. ing. In the multi-joint manipulator 1c, a first joint 1a as a base joint is fixed to a gantry 1e, and each joint 1a, 1p, 1 is controlled by a controller 1g driven by a power supply 1f.
The target position command to q, 1r... 1d is transmitted to and driven by the motor in each joint through the communication interface 1h.

[0003] The articulated manipulator 1 having such a structure.
c, when controlling the position of the tip joint 1d, if the number of joints is 7 or more, the conventional method is described in "Corona Robot Fundamental Theory of Robot Control Tsuneo Yoshikawa, p57-58, p22
2 "," Redundancy Decomposition Control of Articulated Manipulators, Shigeo Hirose, Transactions of the Society of Automatic Measurement Engineers, Vol. 24, No. 9, p.
954-p959, 1988 ", generally, all target joint positions constituting the manipulator 1c are calculated by Equation 1 using a pseudo Jacobian matrix, and the target joint positions obtained by Equation 2 are calculated. Calculates the target joint angle of each joint from. Specifically, it is as follows. q = J _v ⁺ v + (IJ _v ⁺ J _v ) k (q = [q _1, q _2, ... _, q _N ]) (Equation 1) q = ∫q dt (Equation 2) , Q are the moving speed matrices of the first to Nth joints,
_v ⁺ is a pseudo-Jacobi inverse matrix, J _v is a Jacobian inverse matrix, I is an identity matrix, k is an arbitrary constant vector matrix, v is a moving velocity matrix of a tip joint, and q is It is a position matrix of the 1st to Nth joints.

[0004]

However, in the above-mentioned conventional method, the pseudo Jacobi matrix becomes larger as the number of joints of the robot increases, and a high-speed computer is required to calculate in real time. I do. Also,
When the manipulator 2b is inserted into a narrow tube 2a as shown in FIG. 2, the joint 2c at the distal end is moved along the center of the tube 2a. In this case, a conventional method using a pseudo Jacobian matrix is used. Is used, some joints 2d of the manipulator 2b may come into contact with the tube 2a and become unable to be inserted. This is because the insertion range of the manipulator in the insertion into the pipe is limited, so that it is necessary to perform processing such as correcting the angle of the joint in consideration of the contact with the pipe.
However, no proposal has been made so far to add this correction processing to the above-described equation 1, which has been a problem.

SUMMARY OF THE INVENTION The present invention has been made to solve the conventional problems, and can greatly reduce the amount of calculation when calculating the joint angle and can be inserted into a narrow part. An object of the present invention is to provide a control device for a joint robot.

[0006]

In order to achieve the above object, the present invention provides a control device for a multi-joint robot in which N joints each including a motor are connected via a link,
When the control device rotates the first joint, which is the root joint, by a predetermined minute angle Δθ ₁ with the other joints fixed, the target position of the N-th joint, which is the distal joint, is set to D (D _1, D _{2). ,}
D ₃ ), the current position of the N-th joint before rotating the first joint is P (P _1, P _2, P ₃ ), and the change in the position of the N-th joint when the first joint is rotated θ The amount is ΔP (ΔP1 _, ΔP2 _,
ΔP ₃ ), and the square value of the distance between the position of the Nth joint and the target position when the first joint is rotated by θ is E, and ΔP ₁
= F ₁ (θ), ΔP ₂ = f ₂ (θ), ΔP ₃ = f
₃ (θ) and E = (D ₁ −P ₁ −f ₁ (θ)) ² +
(D ₂ −P ₂ −f ₂ (θ)) ² + (D ₃ −P ₃ −f
₃ (theta)) ² the minimum rotation amount theta of the first joint to a minimum 2
A first joint angle computing means to minute angle [Delta] [theta] ₁ and obtained by multiplication in each of the N joint from the second joint adjacent to the first joint, the predetermined minute in a state where the joints were fixed other joints And a second to Nth joint angle calculation means for obtaining the minute angle Δθ by the least square method in the same manner as the first joint rotation angle calculation means. The angle calculation means of the first to Nth joints repeatedly performs the calculation until the value of E becomes equal to or less than a predetermined threshold value, and calculates the rotation angle of each joint. As described above, according to the present invention, the joint angle at which the distal end joint of the robot approaches the target position in a distance is obtained by the least squares method for each axis, and is proportional to the difference between the obtained target angle and the current joint angle. By rotating the joint by one axis at a time, the joint at the distal end finally reaches the target position. Therefore, the multiplication (calculation) amount when calculating the joint angle increases by the square of the number of joints constituting the robot in the conventional method using the inverse Jacobian matrix, whereas in the present invention, the number of joints Since it increases in proportion, the amount of calculation is significantly reduced. As a result, according to the present invention, as the number of joints constituting the robot increases, the amount of calculation of the joint angle decreases more as compared with the conventional method.

Further, in the present invention, when a contact sensor provided on the surface of each joint and the contact sensor detects that a link portion connected to the joint has contacted a surrounding obstacle, It is preferable to have contact avoidance means for driving the rotation angle of the joint by a predetermined minute angle in a direction to avoid contact. This makes it easy to incorporate simple obstacle avoidance logic that turns the joint little by little in the direction to avoid contact if a part of the manipulator's joint comes into contact with surrounding obstacles while driving the robot. As a result, the manipulator can be inserted into a pipe having many obstacles. In contrast, in the conventional Jacobian matrix, the target angles of all the joints are collectively determined so that the tip position of the manipulator approaches the target position. It is difficult to insert a manipulator even in a pipe with many parts.

[0008]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Next, a first embodiment of the present invention will be described with reference to FIGS. FIG. 1 is a schematic diagram showing an N-degree-of-freedom manipulator connecting a large number of general joints to which the present invention can be applied, and FIG. 3 calculates a joint rotation angle according to the first embodiment of the present invention. FIG. 4 is a schematic diagram showing a joint operation state when the first embodiment of the present invention is applied to a manipulator having eight degrees of freedom.

A joint angle calculation method (algorithm) for determining a target angle of a joint according to the first embodiment of the present invention will be described with reference to the control flowchart of FIG. 3 by using an articulated manipulator having N degrees of freedom shown in FIG. This will be explained. FIG.
As shown in the figure, when the target angle of the joint is determined, the operator specifies the target position of the joint 1d at the tip of the manipulator, and then, one joint at a time from the joint 1a at the root to the joint 1d at the tip. The individual joints are rotated little by little so that the position 1d approaches the target position in terms of distance, and the above-described processing is repeated until the error from the target position becomes equal to or less than a certain threshold value. Hereinafter, a specific description will be given.
First, in step S1 of FIG. 3, the operator using the controller 1g, inputs the target position D of the tip joint 1d of the manipulator _{1c (D 1, D 2,} D 3). Here, D ₁ , D ₂ , and D ₃ are x, at the target position D, respectively.
These are y and z direction components.

Next, in step S2, another joint is
While fixed, only the first joint 1a (the joint at the root)
Small angle Δθ₁Just rotate. This small angle Δθ₁of
The calculation is performed as follows. (1) Nth joint when the first joint is rotated by θ (<< 1)
ΔP (ΔP_1,ΔP_2,ΔP_Three).
ΔP is the conventional rotation matrix around the x, y, and z axes (multi-joint ro
Bot's forward kinematics). 1st joint
Is the current position of the Nth joint before rotating_1,P_2,P
_Three). (2) Position and eye of the Nth joint when the first joint is rotated by θ
Assuming that the square value of the distance to the target position is E, E = (D₁−P₁−ΔP₁)^Two+ (D_Two−P_Two−Δ
P_Two)^Two+ (D_Three−P_Three−ΔP_Three)^Two Of the rotation of the first joint that minimizes
You. ΔP in this equation_1,ΔP_2,ΔP_ThreeIs a function of θ,
ΔP₁= F₁(Θ), ΔP_Two= F_Two(Θ), ΔP _Three= F
_Three(Θ). Rewrite the above equation by substituting these
E = (D₁−P₁−f₁(Θ))^Two+ (D_Two−P_Two−f
_Two(Θ))^Two+ (D_Three−P_Three−f_Three(Θ))^TwoIs represented
You. (3) By approximating cos θ = 1 and sin θ = θ,
Purify and obtain θ assuming that a value partially differentiated by θ is 0. First
Joint rotation amount Δθ₁Is a constant to θ obtained in this way.
K (K <1), that is, Δθ₁= Kθ
You. Next, in step S3, another joint was fixed.
While keeping the second joint 1p adjacent to the first joint 1a at a small angle
Δθ_TwoRotate only slightly. Small angle Δθ_TwoHow to calculate
The method is performed in the same manner as in step S2. Since then, the third Seki
The joint 1q, the fourth joint 1r, and the N-th joint 1d, which is the distal joint.
And rotate in the same way. Step S4 is the Nth joint
This is the process for 1d. Further, in step S5
And the distance E between the current position of the tip joint and the target position
Calculate and threshold E₀Judge whether or not it is less than
In this case, the process returns to step S2, and₀Until
Then, the processing from step S2 to step S4 is repeated.
In this way, the final target rotation angle of each joint is calculated
Is done.

FIG. 4 shows a state in which the distal end joint 4h is brought closer to the target position 4i in the state shown in FIG. 4 (4a) for a manipulator composed of all eight pitch axes (rotation axes around the y axis). , The operation state of the manipulator, and it can be seen that as shown in the state (4e), all the joints can be finally used to move to the target position 4i of the distal joint 4h. The joint 4f is the base joint of the manipulator, and is shown in FIG.
By the processing shown in the control flowchart, the joint is repeatedly driven from the joint 4f to the joint 4g to the joint 4h in order.

As will be described later, the present invention is more effective than the conventional method in that the larger the degree of freedom N, the smaller the amount of calculation compared to the conventional method. Hereinafter, the amount of calculation between the present invention and the conventional methods represented by the above-described equations 1 and 2 will be compared. (Prerequisites for Comparison) The amount of calculation of each joint angle when the tip joint of a manipulator having N joints is moved to a target position will be compared. Most of the calculation at the time of calculation is a matrix operation, and the amount of calculation is compared by the total multiplier. The multiplier is calculated by replacing the components of the matrix with one character.

(Comparison of calculation amount between conventional method and the present invention) (1) Conventional method (Corona Corporation, Basic Robot Control Theory, Tsuneo Yoshikawa, pp. 57-58, p222) Calculation (part with multiplication)
Are the processing a (J _v ⁺ v) and the processing b (J _v
⁺ J _v ). Further, the number of multiplications related to the calculation is shown. ^{_{q = J v + v + (}} I- J v + J v) k (q = [q 1, q 2, ······, q N]) ( Equation 1) where, q is the first joint 1a To the moving speed matrix (N × 1 matrix) of the Nth joint 1d, J _v ⁺ is a pseudo Jacobian inverse matrix (3 × N matrix), J _v is a Jacobian inverse matrix (N × 3 matrix), v is a moving speed matrix (3 × 1 matrix) of the distal end joint, and q is a position matrix (N × 1 matrix) of the first joint 1a to the Nth joint 1d. The number of multiplications of the inter-matrix operation in Equation 1 is as follows. Process a: 3N ² times (= N × 1 × 3 × N) Process b: 9N ² times (= 3 × N × N × 3) Total number of matrix portions: 12N ² times

(2) The present invention The operation between the matrices is performed from step S2 to step S4 in the control flowchart shown in FIG. 3, and corresponds to the specific description of item (1) of step S2. The processing in step S2 of FIG. 3 matrix R _N
Is described by the following equation (3), and the multiplication part of the matrix, which is the main part of the calculation, is the processing c (R _N (ω _N ) R _N-1 (ω _{N -1} ) ・ ・ R ₃ (ω ₃ ) R ₂ (ω
₂ ) R ₁ (ω ₁ + Δθ ₁ )) and processing d (R ₁ (ω ₁ + Δθ ₁ )). In process c, a 3 × 3 matrix is created by multiplying the N matrices, and in process d, the 3 × 3 matrix obtained in process c is obtained.
Matrix and is a position matrix P ₁ multiplied process. Further, the number of multiplications in the processing c and the processing d is shown. P _N = R _N (ω _N ) R _N-1 (ω _N-1 ) _··· R ₃ (ω ₃ ) R ₂ (ω ₂ ) R ₁ (ω ₁ + Δθ ₁ ) P ₁ (Equation 3) P ₁ is the current position matrix (3 × 1 matrix) of the first joint 1a, and R _N (ω _N ) is the N-th joint (joint 1 at the tip).
d) is a rotation matrix (3 × 3 matrix), where ω _N is the current joint angle of the Nth joint (joint 1d at the tip), Δ
theta ₁ is a rotation angle of (axis rotating in the process) the first joint 1a. The number of multiplications of the inter-matrix operation in Equation 3 is
It is as follows. Process c: 9N [= (3 × 3) × N] Process d: 9 (= 3 × 3) Total number of matrix portions: 9 (N + 1)

Next, the processing in step S3 in FIG.
Matrix R_NIf it is described by an expression using
The calculation occurs in the underlined processing e (R_N(ω_N) R_N-1(ω
_N-1) ・ ・ R_Three(ω_Three) R_Two(ω_Two+ Δθ_Two)) And processing f (R_Two(ω_Two+
Δθ_Two)). P_N=R _N (ω _N ) R _N-1 (ω _N-1 ) R ₃ (ω ₃ ) R ₂ (ω ₂ + Δθ ₂ )P_Two (Equation 4) where R_N(ω_N) R_N-1(ω_N-1) ・ ・ R_Three(ω_Three) Is the (expression
No new calculation occurs because the calculation result of 3) can be used.
No. Further, the number of multiplications in processing e and processing f is shown.
You. Where P₁Is the current position matrix of the second joint 1p (3 × 1
Matrix) and R_N (ω_N) Is the Nth joint (joint 1 at the tip)
d) is a rotation matrix (3 × 3 matrix) for ω _NIs
The current joint angle of the Nth joint (the joint 1d at the tip)
Δθ_TwoIs the rotation angle of the second joint (the axis to be rotated in this process)
It is. The number of multiplications of the inter-matrix operation in Equation 4 is
It is as follows. Process e: 0 [use the calculation result for the first joint] Process f: 9 (= 3 × 3) Total number of matrix parts: 9

Next, the processing in step S3 in FIG.
By writing formula using matrix R _N expressed by Equation 5, the portion multiplication occurs a portion of the processing g underlined in Formula 5. Further, the number of multiplications in the processing g is shown. P _N = R _N (ω _N + Δθ _N ) P _N (Equation 5) where P _N is the current position matrix (3 × 1 matrix) of the Nth joint (the joint 1d at the tip), and R _N (ω _N ) is a rotation matrix (3 × 3 matrix) for the N-th joint (tip joint 1d), ω _N is the current joint angle of the N-th joint (tip joint 1d), and Δθ _N is This is the rotation angle of the second joint 1p (the axis to be rotated in this process) The number of times of multiplication between the matrices in Expression 5 is as follows: Process g: 9 (= 3 × 3) Total of matrix portions Number of times: 9

(3) Comparison of the amount of calculation between the conventional method and the present invention As described above, the number of multiplications of the inter-matrix operation in the processing using the conventional method and the present invention was compared. The total number of times is as follows. Conventional method: 12N ² times The present invention: 18N times (= 9 (N + 1) +9 (N-1)
Thus, as the degree of freedom N increases, according to the present invention,
The calculation amount is significantly smaller than that of the conventional method.

As described above, in the first embodiment of the present invention, the joint angle at which the joint at the tip of the manipulator approaches the target position in terms of distance is obtained by the least squares method one axis at a time. By rotating the joint one axis at a time in an amount proportional to the difference between the current joint angles, the joint at the end finally reaches the target position. The amount of multiplication (calculation) when calculating the joint angle increases by the square of the number of joints forming the manipulator in the conventional method using the inverse Jacobian matrix, whereas in the first embodiment of the present invention, the number of joints increases. Since the number increases in proportion to the number, the amount of calculation is significantly reduced. As a result, according to the first embodiment of the present invention, as the number of joints configuring the manipulator increases,
The calculation amount of the joint angle is greatly reduced as compared with the conventional method.

Next, a second embodiment of the present invention will be described with reference to FIGS. 5 and 6 show examples of the articulated manipulator to be inserted into a narrow part.
9 shows a control flowchart according to the second embodiment of the present invention. Hereinafter, a method of calculating a joint angle when a manipulator having N degrees of freedom according to the second embodiment is inserted into a narrow portion shown in FIGS. 5 and 6 will be described. In this embodiment, a contact sensor (not shown) is attached to the surface of all the joints of the manipulator 1c shown in FIG. FIG.
Shows a state in which a manipulator 5c having a camera (not shown) attached at the tip and a root thereof attached to the main body 5b is inserted into the tube bundle of the upper core structure 5a of the nuclear reactor provided in the nuclear power plant. ing. At this time, determining the joint angle of the manipulator 5c for inserting the manipulator 5c so as not to interfere with the tube group becomes a problem. FIG. 6 shows a state in which the manipulator 6b is attached to the distal end of the guide wire 6h, and the operator remotely operates the manipulator 6f while watching the camera 6c attached to the distal end of the manipulator 6b.
Are inserted, and the manipulator 6b itself is pushed into the pipe 6a by the guide wire feeder 6g. In the case shown in FIG. 6, as in the case of FIG. 5, the determination of the joint angle of the manipulator 6b for inserting the manipulator 6b so as not to interfere with the tube 6a becomes a problem.

In the control flowchart shown in FIG.
Is a control flowchart of the first embodiment shown in FIG.
And steps S11, S12, S15, S18, S19
Are the same, and steps S13, S14, S16, S1
7 has been added. Manipulator as shown in FIGS. 5 and 6
In the case of inserting a heat exchanger into a narrow part, the processing shown in FIG.
When the joint angle of the manipulator is determined,
become. That is, in step S13, the first joint is set to Δ
θ₁By rotating, the link between the first joint and the second joint
It is determined whether or not touches the object. When not touching
Proceeds to step S15.
Proceed to step S14, and ₁= -Kθ₁(K <1)
The first joint is slightly driven in the body and avoiding direction. In addition,
Also in two joints, in steps S16 and S17,
A similar process is performed. In this embodiment, the steps
In S12, S15 and S18, the tip position of the manipulator
Is calculated so that the robot approaches the target position.
It is conceivable that the manipulator comes into contact with the surface. This
In the case of, a part of the manipulator
Of the manipulator cannot be brought close to the target position.
This will damage the purifier. Contact with tube
Is a contact sensor installed on the surface of all joints of the manipulator.
Can be detected by Therefore, as mentioned above,
In steps S13, S14, S16 and S17, the pipe contacts
Rotate the joint slightly in a direction to avoid contact
I have to. In this way, joints that are not in contact with the tube
So that the tip of the manipulator approaches the target position.
Rotate the joints in contact with the tube to avoid contact
To avoid interference with the pipe as a whole
The tip of the purator can be brought closer to the target position.

In the second embodiment of the present invention, a least joint method is used for one joint to determine a target joint angle such that the joint at the distal end approaches the target position. The manipulator is moved by a sequential operation such that the joint is turned by an angle proportional to the difference between the joints, and then the target angle is similarly obtained for the adjacent joint and the joint is moved. For this reason, during the operation of the manipulator,
When a part of the manipulator's joint comes into contact with an obstacle around it, simple obstacle avoidance logic that turns the joint little by little in a direction to avoid contact can be easily incorporated, which results in a pipe with many obstacles. The manipulator can also be inserted. In contrast, in the conventional Jacobian matrix, the target angles of all the joints are collectively determined so that the tip position of the manipulator approaches the target position. It is difficult to insert a manipulator even in a pipe with many parts.

Next, a third embodiment of the present invention will be described with reference to FIGS. This third embodiment is similar to the second embodiment.
The embodiment is applied to a four-legged walking robot.
FIG. 8 is an overall configuration diagram showing a four-legged walking robot to which the third embodiment of the present invention is applied, and FIG. 9 shows a four-legged walking robot according to the third embodiment moving on a step having a different shape. FIG. 10 is a perspective view showing the situation, and FIGS.
12 is a control flowchart for calculating an angle of a joint when the four-legged walking robot according to the embodiment is walking.
FIG. 11 is a perspective view illustrating a posture of a four-legged walking robot during walking according to a third embodiment. As shown in FIG. 8, the four-legged walking robot 8a has N-axis joints for each leg 8b, and contact sensors (not shown) are attached to all the joints. The four legs are connected by a fixing jig 8c. Here, the joint 8d indicates a first joint that is a root joint, and the joint 8e indicates an N-th joint that is a distal joint.
The operation command to the four-legged walking robot 8a is sent from the controller 8f to the cable 8j via the communication interface 8g. The controller 8f and the communication interface 8g are connected to a power supply 8h.

As shown in FIG. 9, a four-legged walking robot 8
a is a step 9a having a different shape (see FIG. 9A) and a step 9 having a different height based on information from the contact sensor.
b, 9c (see FIGS. 9B and 9C). Next, according to FIGS. 10 to 12,
Control contents for calculating the angle of the joint when the four-legged walking robot walks according to the third embodiment will be described. First,
In step T1, the operator inputs the moving direction of the four-legged walking robot 8a. Next, in step T2, a leg 11a (a free leg) to be moved and a leg 11b (a supporting leg) that supports three four-legged walking robots are selected according to a static walking pattern that is a conventional method. Next, in step T3, the center 11d of the four-legged walking robot is moved so that the center of gravity of the four-legged walking robot falls within the triangle 11c formed by the three grounding points of the supporting legs. Next, in step T4, the tip joint 11e of the free leg 11a is vertically moved to A ₁ (m
m), the root joint 11f (first joint) to the distal joint 11e (Nth joint) are rotated by Δθ _{i in} order, one axis at a time. Here, Δθi is Δθ ₁ , Δθ in steps S12, S15, and S18 in FIG.
_2, determined by the same processing as [Delta] [theta] _N.

Next, in step T5, it is determined by the contact sensor whether or not the tip joint 11e of the free leg contacts the step while the first to Nth joints are rotating. In the case of contact, the process proceeds to step T6, and the same processing as steps T4 and T5 is repeatedly executed so that the distal end joint 11e of the free leg is further lifted vertically by ΔA ₁ (mm). Next, the process proceeds to step T7, and it is determined whether or not any joint other than the tip of the free leg contacts the step or exceeds the movable range while rotating the first to Nth joints. Step T
When it is determined in step 5 that no contact occurs, the process similarly proceeds to step T7. In step T7, in the case of a collision or exceeding the movable range, the process proceeds to step T8,
The currently rotating joint is rotated by −kΔθi (where k <1) to avoid the contact and the movement range over. afterwards,
Proceed to step T9. Also, if no contact is made or the movable range is not exceeded in step T7, step T9
Proceed to. Next, in step T9, the distal joint 11e
It is determined whether or not has finished lifting vertically by A ₁ (mm). If not, step T
4 and the same process is repeated until the lifting is completed.

Next, in step T10, the free leg 11
The tip joint 11e of a moves horizontally and the fixed stride A ₂ (m
m) The root joint 11f (the first joint) to the distal joint 11e (the Nth joint) are rotated one axis at a time by Δθ _i so as to approach the preceding target position. Here, Δθi is Δθ ₁ in steps S12, S15, and S18 in FIG.
It is obtained by the same processing as Δθ ₂ and Δθ _N. In step T10, the processes in this step are repeated until the tip joint 11e of the free leg completes the horizontal movement of A ₂ (mm) while executing steps T5 to T8. Next, in step T11, contrary to step T4, the tip joint 11e of the free leg is set to ΔA ₁ (mm).
Just lower vertically. This processing is performed by the tip joint 11e of the swing leg.
Is repeatedly executed until contacts the step surface. next,
The process proceeds to step T11, where it is determined whether or not there is a stop command from the operator. If there is, the process ends, and if not, the process proceeds to step T13. In step T13, similar processing is executed with another leg as a free leg. Specifically, the process returns to step T2 for this other leg, and the same processing is repeated until a stop command is issued from the operator.

In the third embodiment of the present invention applied to this four-legged walking robot, the distal joint can be moved to the target position by using all of the N-axis joints. Can be expanded. Since the amount of rotation of each joint is adjusted based on information from a contact sensor (not shown) attached to the joint surface of each joint, even if the height or width of the step to be moved changes, It is possible to move over without modifying the algorithm.
If a part of the joints that make up the legs exceeds the movable range due to a stepped obstacle and the leg posture becomes tight, the joint beyond the movable range is rotated in the opposite direction to allow the leg to move. By returning the joint to the inside and adjusting the amount of rotation of another joint, the joint at the tip of the leg is consequently brought closer to the target position, thereby enabling the stepping movement by walking. Conventional 4-legged walking robot (Proceedings of the 4th Annual Conference of the Robotics Society of Japan,
1986, p381-p386) are mostly composed of legs having 3-4 degrees of freedom, but the third embodiment of the present invention is a four-legged walking composed of legs having more degrees of freedom. This is particularly effective for calculating the joint angle when the robot moves.

Although the four-legged walking robot according to the third embodiment of the present invention has the same leg movement and step length (movement trajectory of the tip joint) as the conventional four-legged walking robot, according to the present embodiment, When the leg tip is moved to the target position, avoidance of step contact of the free leg and over-movement of the joint are performed sequentially, so the position and shape of the step (robot for robot Map),
By simply instructing only the moving direction by the operator, the position and shape of the step can be detected by the contact sensor and the stairs can be automatically moved. Thus, the present embodiment is
Unlike the conventionally proposed control method based on the movement map, accurate self-position measurement of the four-legged walking robot is not required, and therefore, the applicable range is wide. Furthermore, the control of a four-legged walking robot having more than four axes per leg has not been implemented so far, and in the four-legged walking robot of the present embodiment, the conventional tip joint is constantly contacted and moved. Unlike walking, in which the knee joint (for example, the (N-1) th joint) is also grounded, it is possible to get over a step that is higher than the conventional one.

[0028]

As described above, according to the present invention, there is provided a control apparatus for an articulated robot which can greatly reduce the amount of calculation for calculating a joint angle and can be inserted into a narrow portion. be able to. Further, the present invention provides
The present invention is also applicable to a four-legged walking robot.

[Brief description of the drawings]

FIG. 1 is a schematic diagram showing a general articulated robot (articulated manipulator) having redundant degrees of freedom;

FIG. 2 is a schematic diagram showing a case where the articulated robot (articulated manipulator) of FIG. 1 is inserted into a narrow tube.

FIG. 3 is a control flowchart for calculating a joint rotation angle according to the first embodiment of the present invention;

FIG. 4 is a schematic diagram showing a joint operation state when the first embodiment of the present invention is applied to an eight-degree-of-freedom manipulator;

FIG. 5 is a schematic diagram showing an articulated manipulator to which the second embodiment of the present invention is applicable, inserted in a tube bank of an upper core structure of a nuclear reactor provided in a nuclear power plant.

FIG. 6 is a schematic diagram showing an articulated manipulator to which a second embodiment of the present invention is applicable, inserted in a bending tube;

FIG. 7 is a control flowchart for calculating a joint rotation angle according to a second embodiment of the present invention;

FIG. 8 is an overall configuration diagram showing a four-legged walking robot to which a third embodiment of the present invention is applied;

FIG. 9 is a perspective view showing a state in which a four-legged walking robot according to a third embodiment of the present invention is moving on steps having different shapes.

FIG. 10 is a control flowchart for calculating joint angles during walking of a four-legged walking robot according to a third embodiment of the present invention.

FIG. 11 is a control flowchart for calculating an angle of a joint during walking of a four-legged walking robot according to a third embodiment of the present invention.

FIG. 12 is a perspective view showing a posture during walking of a four-legged walking robot according to a third embodiment of the present invention.

[Explanation of symbols]

 1c Articulated robot (articulated manipulator) 1a, 1p, 1r, 1d Joint 1b Link 1e Mount 1f Power supply 1g Controller 1h Communication interface 4f, 4g, 4h Joint 4i Target position 5a Upper core structure of reactor 5c Manipulator 6a Tube 6b Manipulator 6f Operating device 6d Power supply 6e Controller 8a Four-legged walking robot 8b Leg 8c Fixing jig 8d Root joint (first joint) 8e Tip joint (Nth joint) 8f Controller 8g Communication interface 8j Cable 11a, 11b Leg 11d Center 11f Root joint 11e Tip joint

──────────────────────────────────────────────────続 き Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) B25J 9/10 B25J 5/00 B25J 9/06 JICST file (JOIS)

Claims (2)

(57) [Claims] 1. Linking each of N joints including a motor
A control device of the articulated robot connected via
This control device is configured such that the first joint, which is the base joint, is fixed while the other joints are fixed.
Fixed small angle Δθ₁Only rotate the tip joint.
The target position of the Nth joint is_1,D_2,D_Three) And the first
The current position of the Nth joint before rotating the joint is represented by P (P_1,P
_2,P_Three), And the Nth joint when the first joint is rotated by θ.
ΔP (ΔP_1,ΔP_2,ΔP_Three) And
Position of Nth joint and target position when one joint is rotated θ
E is the square value of the distance to₁= F₁(Θ), Δ
P_Two= F_Two(Θ), ΔP_Three= F_Three(Θ) and E = (D
₁−P₁−f₁(Θ))^Two+ (D_Two−P_Two−f
_Two(Θ)) ^Two+ (D_Three−P_Three−f_Three(Θ))^TwoThe least
Is obtained by the least squares method,
Small angle Δθ₁Angle calculation means for the first joint, and the second joint to the N-th joint adjacent to the first joint.
Each joint with the other joints fixed at a predetermined minute angle.
Rotate by Δθ, and add this small angle Δθ to the first joint
Calculated by the least-squares method in the same way as
Calculate the angle of the second to Nth joints with the small angle Δθ
Means, the first function being performed until the value of E becomes equal to or less than a predetermined threshold value.
The angle calculating means of the joint to the Nth joint repeats the calculation
Multi-rotation angle calculation for each joint
Control device for joint robot. Further, when a contact sensor provided on the surface of each joint and the contact sensor detects that a link portion connected to the joint has contacted a surrounding obstacle, the rotation angle of the joint is determined. 2. The control device for an articulated robot according to claim 1, further comprising a contact avoiding unit that drives the contact by a predetermined minute angle in a direction in which the contact is avoided.