JPS591181A - Control system of locus of robot - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

DETAILED DESCRIPTION OF THE INVENTION (1) 4 Technical Fields of the Invention The present invention relates to a robot trajectory control method, particularly to a robot trajectory control method for moving a 6-degree-of-freedom articulated robot over a wide range in a Cartesian coordinate system. be.

(2) Background and Problems In general, the arms of general-purpose robots have multiple joints in order to allow the machine to automatically perform actions similar to those of a single human. In such so-called multi-jointed robots, it is necessary to be able to immediately calculate which joints need to be moved and how much to move the hand to which position. Therefore, generally using a coordinate transformation matrix.

It is calculated and controlled by a computer.

Problems in moving a 6-degree-of-freedom, multi-joint type bot on an orthogonal coordinate system will be explained using an example of a robot in which each joint has all rotational degrees of freedom, as shown in FIG.

In FIG. 1, θl to θ6 represent joint angles of rotation or bending, and P represents the position/posture vector of the hand.

Here, "+'jl, 2 is the three-dimensional coordinate value of the position, and α
, β, and γ are angles that determine the posture such as the direction of the hand.

The position/posture vector of this hand P(Z+ p+ 1*α
, β, r) and move the robot on an orthogonal coordinate system, using the total coordinate transformation matrix A A = As Am As A4 All AS between the coordinate systems fixed to each joint, It is well known to obtain the angle vector θ (θl, θ2.θ3.θ4°θ6.θ6).

However, the solution for θ is -π and θi≦π(i
= 1 to 6), there are still eight ways. That is, the second
As shown in the figure, even if the joint angles shown in Figure 2 are different from (A1) and (A2), the same position is machined, and the same 4:=(B
l) and (B2) state 4 (CI),! =(C2) also gives the same position, so there are 23=8 solutions.

However, there is a limit to the range in which the robot can move on the orthogonal coordinate system if there is only one quadrant of each solution.

In order to move continuously over a wide range, it is necessary to move the solution type over a wide range, instead of using only an orthogonal coordinate system, use a coordinate system whose coordinate value is the component of θ, and change the type of solution. The Cartesian coordinate system and other coordinate systems were used together.

In response to this, the present inventor has developed a trajectory for a robot that can move globally in the Cartesian coordinate system, that is, continuously in the entire space, by checking the existence of singular points specific to a 6-degree-of-freedom articulated robot. Patent application No. 56-21119 for control method
This is proposed in issue 5.

The relationship between the minute change δp when the hand position/posture vector p changes to the next state and the corresponding minute change δj of the joint angle vector is determined using the Jacobian matrix J.

It is expressed as δP=Jδθ. A singular point is an angular relationship where one determinant is detJ = 0, and the distribution of the singular point is determined by the mechanism of the robot.In the case of a robot with the configuration shown in the iT1 diagram, the singular point is distributed as follows. do.

■ θ2+θs /2 = nπ ■ θ3 ” nπ ■ θB ” nπ Figure 3 shows the state of the robot at the singular point that satisfies the above ■, ■, ■. Figure 3.6 shows the state of the joints at the singular point (■) above.
Figure 3 (tel) shows the states of the joints at the singular point (2) above.

For example, when an articulated robot's hand can be moved from the current position A point in the Cartesian coordinate system to the target point B, a linear path, for example, is considered in the Cartesian coordinate system. At this point, as shown in FIG.
The type of solution can be changed and, for example, by applying the method previously proposed by the inventor mentioned above, it is possible to continuously move from point A to point B. However, as shown in FIG. 4(b).

If there is no singular point on the path pasal that you are trying to move, and the limit of the joint angle θ (on the way) is reached,
A problem arises in that the target point B cannot be reached. In other words, due to the restriction that the range of joint angles θ1 (i = 1 to 6) must all be within the maximum weight π,
You will not be able to move.

(3) Purpose of the Invention The present invention aims to solve the above-mentioned problems, and the joint angle vector θ
The purpose of this study is to provide a trajectory control method that allows the type of solution to be changed and allows the robot to move continuously over a wide range on an orthogonal coordinate system.

(4) Structure of the Invention Therefore, 1. the present invention is a control system that forcibly passes through a singular point determined by the mechanism of the robot when the indicated path reaches the limit of the joint angle θi on the way. It is. In other words, the robot trajectory control method of the present invention is a control method for moving an articulated robot in an orthogonal island reference system, and when moving the robot from a certain point A to a target point B, joint angle calculation means for calculating each joint angle on the indicated trajectory;
limit detection means for detecting whether each joint exceeds a movable range based on the calculation result of the joint angle calculation means;
Singular point calculating means for determining a singular point relatively close to the indicated trajectory that becomes a point of change in the type of joint angle solution when the limit detecting means detects that there is a joint that exceeds the limit.

The present invention is characterized by comprising a trajectory modification means for modifying the indicated trajectory so that it passes through the singularity determined by the singularity calculation means. Embodiments will be described below with reference to the drawings.

(5) Embodiment of the invention Figure 5 is a diagram for explaining the concept of the system according to the invention.

In Fig. 5, when trying to move the robot's hand from point A to point B, on the pre-instructed path pass1, the type of joint angle vector θ is not changed, so the limit is reached midway. Suppose that it ends up.

In the present invention, in such a case, it is detected that the limit is reached, and the designated route is changed to, for example, the route pα88 shown in FIG.
2 or pα883 so as to pass through the singular point C, change the type of joint angle solution, and make it possible to reach point B. Note that the present invention is effective when the route taken while moving from point A to point B does not matter, that is, when the position and posture of the hand may shift along the way.

Figure A16 shows the configuration of an embodiment of the present invention.

In the figure, 1 is an arithmetic processing unit, 2 is a trajectory instruction unit, 3 is a joint angle calculation unit, 4 is a limit detection unit, 5 is a singularity calculation unit, 6 is a trajectory correction unit, 7 is a joint control unit, and 8-1 8-6 represent servo mechanisms.

The arithmetic processing unit 1 is a device that fetches and executes instructions for controlling the -pot. The trajectory instruction unit 2 specifies a trajectory of movement from the current point on point A defined by the current joint angle θi to a point on point B that gives the target hand position/posture vector P. Or, for example, by choosing a straight path in a Cartesian coordinate system.

The joint angle calculation unit 3 is notified.

The joint angle calculation unit 3 calculates the joint angle θik on the designated trajectory from point A to point B notified from the trajectory instruction unit 2. At this time, there is no need to actually move the robot. The calculation result is notified to the limit detection section 4. The limit detection section 4 is
It is checked whether any of the joint angles θi exceeds the limit range of 2, for example, π, on the way along the indicated trajectory, and when the limit is reached, it is detected. If there is no joint that applies to the limit, movement can be made only in quadrants of the same type of solution, so the trajectory information instructed by the trajectory instruction section 2 is notified to the joint control section 7. The joint control unit 7 generates a velocity function for each joint based on the instruction trajectory, and outputs a control signal to the servo mechanisms 8-1 to 8-6 arranged at each joint of the robot.

In the robot having the configuration shown in FIG. 1, it is assumed that the limit detection unit 4 detects that the joint angle θ1 reaches the limit on the instructed trajectory.

The detection result is notified to the singularity calculation unit 5. The singularity calculation unit 5 calculates the singularity C in a quadratic manner when θl reaches the limit. First, among the joint angles θsk on the indicated trajectory, 02 and 03 are found at the point where θ2+θa/2 is closest to nπ. Next, the obtained θ2 and 03 are θ2+θs/
2 ” to satisfy the relationship nπ.Then, the position and posture Ph at the changed joint angle θik
If we find the point, it becomes the singular point C as shown in Fig. 5.
It becomes a point that gives .

When the limit detection unit 4 detects that θ4 or θ6 reaches the limit, the singularity calculation unit 5 calculates the singularity C as follows. Among the joint angles θik on the indicated trajectory, find 06 at the point where θS is closest to nπ, and calculate that θ
5 is changed to θ5''nπ. Then, if the position and posture Ph at the changed joint angle θik are determined, that point becomes a point that gives a singular point C as shown in FIG.

The singular point determined by the singular point calculation section 5 is notified to the trajectory modification section 6, and the trajectory modification section 6 modifies the initial instruction trajectory so that it passes through the determined singular point. This modified trajectory is
The joint control unit 7 is notified.

The joint control unit 7 controls the servo mechanisms 8-1 to 8-6 based on the corrected trajectory. As explained with reference to FIG. 5, passing through the singular point changes the quadrant of the solution, allowing continuous movement from point A to point B.

The limit over joint detected by the p-mit detection unit 4 is θ1. If it is a joint other than θ4 or θ6, point B cannot be reached. In that case, 1 for example A
The trajectory is changed by means such as adding an arbitrary intermediate point between point and point B.

The trajectory instruction section 2 may generate an instruction trajectory from point A to point D and from point D to point B.

(6) Detailed Description of the Invention According to the present invention, an articulated robot can be continuously moved over a wide range in a Cartesian coordinate system. This is effective when exact accuracy is not required for the route during movement.

[Brief explanation of drawings]

Figure 1 is a configuration diagram of a 6-degree-of-freedom articulated robot applied to the present invention, Figure 2 is a diagram explaining the types of solutions for eight sets of joint angles, and Figure 3 is a diagram explaining the types of solutions at three types of singular points. 4 is a diagram illustrating problems in trajectory control, FIG. 5 is a diagram illustrating the concept of the method of the present invention, and FIG. 6 is a diagram showing the configuration of an embodiment of the present invention. In the figure, ■ is an arithmetic processing unit, and 3 is a joint angle calculation unit. 4 represents a limit detection section, 5 a singular point calculation section, and 6 a trajectory correction section. Patent author: Fujitsu Ltd. Representative Patent Attorney Mori 1) Hiroshi (1 other person) J-1 bacteria 40 years old tlfl (C1) (C2) 3 rX
1? G[n 4

Claims (1)

[Claims] In a control method that moves an articulated robot in a Cartesian coordinate system, joint angles are used to calculate each joint angle on the indicated trajectory from point A to point B when moving the robot from a certain point A to a target point B. a calculation means; a limit detection means for detecting whether each joint exceeds the movable range based on the calculation result of the joint angle calculation means; Singular point calculation means for finding a singular point relatively close to the indicated locus, which is a point at which the type of solution of the angle is changed, and locus modification means for correcting the indicated locus so that it passes through the singular point found by the singular point calculating means. A robot trajectory control method characterized by the following.