JP2003340755A - Gain setting method in controller for servo motor, method for verifying effectiveness of the controller, and robot control method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To design a controller for setting a gain parameter in an off-line manner while considering factors which cannot be included in a mathematical formula, and for suppressing generation of parasitic vibration in a robot to change the dynamic characteristic depending on the attitude and the position. <P>SOLUTION: A finite number of selection points for a part to be controlled are selected in advance in a series of motions of a robot, and a non-linear dynamic characteristic of the robot is linearized for each selection point in the vicinity thereof. The system identification is performed for each selection point in the vicinity thereof from input-output data when the robot is actually moved, the difference between a linearized model and the identified model is acquired as non-certainty to represent the parasitic vibration characteristic of the robot, a gain cross frequency is selected to avoid generation of the resonance mode attributable to the non-certainty, the phase margin is sufficiently ensured, and the gain in the controller at each selection point is determined. <P>COPYRIGHT: (C)2004,JPO

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a gain setting method, a controller effectiveness verification method, and a robot control method in a servo motor control controller.
When the dynamic characteristics of a robot are modeled by system identification of the robot, the magnitude of factors that cannot be included is grasped in advance, and the dynamic characteristics depend on the posture and position while suppressing the occurrence of parasitic vibrations due to the factors. The present invention relates to a controller design method for enabling control of a robot to be changed, and also includes verification methods related thereto.

[0002]

2. Description of the Related Art When a servomotor for moving each joint is individually driven to operate a robot, each motor is operated so as to satisfy a command from a main body of a control device to cause the robot to perform a desired behavior. , Then a servo driver is used to control the movement of each motor. The servo driver is normally based on PID control, but when a command from the main body of the control device, for example, information on a position to be reached or a limit torque is input, the magnitude of the current to be supplied to the motor is calculated, and the current value is used to operate The position of the motor is detected, feedback is applied, and control is performed so that the desired position is achieved.

By the way, even if a robot is moved to a desired position, a servo driver (hereinafter referred to as a controller) that outputs a signal for driving a motor has a problem of how to give its speed and acceleration, for example. is there. Normally, it is necessary to select and adjust the gain parameter so that the performance and operation specifications of the robot to be controlled are met, and that it is possible to bring out a situation suitable for displacing the held workpiece. It

However, whether the gain parameter is appropriate or not is determined by actually moving the robot by using the controller and observing its behavior to judge good or bad one by one. That is, if the robot exhibits an undesired movement or causes unnecessary vibration,
Change the gain parameter. Make sure that there is no problem even if you change the motion pattern, such as doubling the speed within the range required by the robot.

However, in the method of finding the gain parameter while moving the real robot in this way, not only preparation for applying the controller to the robot is necessary, but also the movement according to the command value based on the teaching data etc. Need to be reproduced at the production site. As a result, the production line of the product is wrinkled by temporarily stopping the operation line of the robot, and in some cases extremely complicated work is required. In addition, various data are collected from the behavior of the robot that follows the controller, brought back to the laboratory or design room for analysis, and it is possible to reduce the amount of work and time required to check the availability of the controller. Absent.

There is a demand for avoiding such operations and works, and for that reason, attempts have been made for a long time to obtain controller evaluations in a laboratory while avoiding verification by an actual robot. In this case, a robot model with the dynamic characteristics of the robot used (kinetic equation reflecting the dynamic characteristics)
It is intended to reduce the labor of controller design by making it and simulating it by moving or calculating it on a personal computer. Since the characteristics of the robot can be grasped, the controller can be designed by using the characteristics, and the quality can be confirmed by simulation.

In such a case, if the robot is a completely rigid body, it is possible to make a model that is exactly the same as a real robot. However, it is often the case that a robot model cannot be a perfect copy of a real robot. Even if it can be copied, if it is a robot that changes its dynamic characteristics depending on its posture and position, its dynamic characteristics become non-linear, which is a complicated calculation equation such as higher order or polynomial. turn into.

[0008] Factors involved are fluctuations in load inertia caused by changes in the positions of the robot joints, fluctuations in interference torque caused by acceleration motion of other joints caused by changes in the position / acceleration of the robot joints, and positions of the robot joints. -Variation of disturbance torque due to centrifugal force or Coriolis force caused by change in speed, variation of disturbance torque due to influence of gravitational acceleration caused by position change of each joint of the robot, and the like.

By the way, even if the dynamic characteristics of the robot can be modeled even though it is a non-linear equation of motion, it takes a lot of time for its calculation and a computer with extremely large calculation capability is required. The feasibility is poor. On top of that, it's just a calculation move,
It is inevitable that there will be some difference from the actual robot.

Furthermore, the dynamic characteristics of the robot include the installation foundation of the robot, the twisting and distortion of members such as the robot arm,
Characteristics due to elastic deformation factors such as unexpected expansion and contraction of the power transmission system, torque fluctuations due to uneven tooth contact of the reduction gear, and the like are also weighted. However, it is a difficult technique to fold inevitable vibrations due to them into a mathematical formula. Considering this, it can be seen that there is a limit in reproducing the motion of the robot by the equation of motion.

Therefore, the model is simplified to some extent so that it can be put to practical use in calculation. However, the simplification of the mathematical formula makes the behavior of the robot model further away from the real robot. Furthermore, if there is an elastic system or the like as described above, a large error remains between the robot model and the real robot, and the use of the linear motion equation is greatly restricted.

By the way, as described above, the dynamic characteristics of the robot arm differ depending on the posture and position. For example, a controller designed to behave in one position does not move the arm well when moving to another position.
That is, although the dynamic characteristics are different between the extended state and the contracted state of the arm, it is not always successful if the controller designed in the extended position of the arm is applied to the contracted state of the arm. This makes it impossible to operate the robot so that it can operate over a wide range and handle the workpiece with stable movement.

In such a case, a method generally considered is a gain schedule. In the above example, this is a technique of gradually connecting the controller created when the arm is contracted and the controller created when the arm is extended and moving. That is, information on the position or orientation of the robot is taken into the controller, and the controller is switched according to the position or orientation of the robot.

[0014]

The idea based on such a gain schedule is disclosed in, for example, Japanese Patent Laid-Open No. 5-25000.
As disclosed in Japanese Patent No. 5, even if the simulation can be carried out by using it, there is no guarantee that it will always work when applied to a real robot. There are several possible reasons for this, the most important of which is that there are swaying points on the arms and other parts, which cause vibrations. In other words, the robot has a resonance vibration (parasitic vibration), and if the control in that part is unsatisfactory, it stimulates the resonance mode, that is, it assists the generation of vibration or promotes vibration. However, the behavior of the robot is deteriorated.

Incidentally, a robot and a simulator (robot model) are fundamentally different. Even if the nonlinear equation of motion can be calculated, the theoretically derived model cannot include the element of the resonant behavior. Furthermore, as described in Japanese Patent Laid-Open No. 5-77176, the simplified model such as linearization only widens the difference from the real robot. That is, even if the robot model is simulated without knowing where the resonance frequency is and how large it is, it is impossible to design a controller that is sufficient to reproduce the actual robot.

By the way, in Japanese Patent No. 3200059, an identification signal having a frequency component in a wide band is input to a motor to drive a joint, whereby a joint displacement signal is collected to identify a dynamic characteristic. Has been introduced. It is also disclosed that the use of the multi-decimation identification method can find that a resonance point exists in a certain band.

Incidentally, the above-mentioned Japanese Patent Laid-Open No. 5-7717.
In JP-A-6-1994, a transfer function model representing a mechanical resonance characteristic in a high frequency band of a linearized multiple input / output system is identified online by using a manipulated variable which is control input data and a motor rotation angle which is a controlled variable. Is listed. In this case, there is an advantage that the gain parameter does not need to be reset by trial and error even if the load variation or the characteristic of the robot changes with time. However, since it is an online process, it is inevitable to disturb the production line as described above.

The present invention has been made in view of the above-mentioned problems, and an object thereof is to enable the setting of the gain parameter off-line while considering the factors that cannot be included in the mathematical formula even when the system is identified. , Thereby avoiding a decrease in the operating rate of the production line and shortening the gain parameter setting time and cost while avoiding an increase in the calculation capacity of the controller. That is.

In addition, even a robot whose dynamic characteristics are changed depending on its posture and position can be made to behave in a non-linear manner in the robot model. It is to provide a method of verifying the effectiveness of a controller that can improve the simulation accuracy. As a result, the robot can be controlled by the controller obtained according to the gain setting method and the controller effectiveness verification method in the servo motor control controller, while avoiding the occurrence of parasitic vibration of the robot. , To be able to reproduce the movements quickly and accurately.

[0020]

According to the present invention, the magnitude of factors that cannot be included when the dynamic characteristics of a robot are modeled by system identification of the robot is grasped in advance,
It is applied to a gain setting method in a controller for controlling a servo motor of a robot, which changes dynamic characteristics depending on a posture and a position while suppressing generation of parasitic vibration due to the factor. The feature is that, with reference to FIG. 1 and FIG. 3, a finite number of specific positions or postures taken by the control target portion are selected in advance in a series of movements of the robot [S in FIG. 3].
1]. The nonlinear dynamic characteristics of the robot are linearized in the vicinity of each of the selected specific positions or postures [S22 in FIG. 1]. From the input / output data when the robot is actually moved, the system is identified in the vicinity of each specific position or posture [S24]. The difference between the linearized model obtained by linearization and the identified model obtained by system identification is taken as the uncertainty that exhibits the parasitic vibration characteristic of the robot [S25]. A gain crossover frequency is selected to avoid the occurrence of a resonance mode due to uncertainty, and a sufficient phase margin is secured to determine the gain of the controller at a specific position or posture [S26]. In this way, each servo motor that causes the robot to perform a series of operations can be controlled based on the gain schedule method, taking into consideration the parasitic vibration characteristics of the robot.

When the operation of the robot based on the set gain cannot meet the performance and operation specifications of the robot by either advancing or delaying the phase,
This means that either the already selected position or orientation is changed, or a new specific position or orientation is added, and the gain is determined again based on those positions or orientations.

If the controller obtained by the gain setting method in the servo motor control controller is applied to the robot, the operation of the robot can be controlled while suppressing the occurrence of parasitic vibration.

The non-linear robot model is operated by using the controller obtained by the gain setting method in the servo motor control controller, and the effectiveness of the control by the controller is verified by the simulation using the non-linear robot model.

The non-linear robot model is set as a group of linearized models for each specific position or posture, and during the operation of the non-linear robot model in the simulation, the uncertainty in the position or posture at that time is calculated by the linearized model at that time. It is only necessary to reproduce the movement extremely close to that of the actual robot by adding it to the result.

It is needless to say that the controller verified by the effectiveness verification method of the servo motor control controller can be applied to the robot for control, and since it has already been verified, the robot is highly stable in operation and extremely reliable. Can be operated as a secured robot.

[0026]

BEST MODE FOR CARRYING OUT THE INVENTION A gain setting method, a controller effectiveness verification method and a robot control method in a controller for controlling a servo motor according to the present invention will be described below in detail with reference to the drawings showing the embodiments. explain. Figure 1
Knows in advance the magnitude of factors that cannot be included when modeling the dynamic characteristics of a robot by system identification of the robot, and suppresses the occurrence of parasitic vibration due to that factor while depending on the posture and position. 3 is a flowchart showing a gain setting method in a controller that can control a servo motor of a robot that changes dynamic characteristics by using a controller.

Before the description of the gain setting method, the overall handling of the controller to be obtained by the present invention will be described. The main point is the design method of the gain schedule control system that explicitly incorporates the parasitic vibration mode of the robot. That is, the robot includes elements that adversely affect dynamic characteristics such as changes in dynamic characteristics due to posture, friction and viscosity, and expansion and contraction of the belt. These factors reduce the motion performance of the robot and affect the operation speed and position accuracy. The demands for robots to be faster and more accurate, the size of workpieces conveyed by robots, and the secondary factors that are derived from them are now often not negligible. .

Therefore, the present invention basically defines a factor (uncertainty) having the above-mentioned adverse effect by using gain scheduling which is effective for a non-linear system, and combines this with a non-linear equation of motion of the robot,
We propose a method of gain schedule control system design that explicitly incorporates parasitic vibration modes of a robot.

First, gain scheduling is effective for controlling a non-linear system, such as a robot, whose dynamic characteristics change as the operating point moves. The gain scheduling generally consists of the following steps. (1) It is preferable that the equation of motion is idealized so that it can be calculated in a short time in the mathematical formula. Therefore, the nonlinear system is linearized at some operating points. (2) A controller is designed corresponding to each of the obtained linear systems. (3) The designed plurality of controllers are sequentially scheduled. By the way, when a nonlinear system is linearized at some appropriate freezing points to design a control system, the obtained operating range of each controller is effectively limited to a narrow range. The gain scheduling described above attempts to make up for such drawbacks.

By the way, it is difficult to express the dynamic characteristics of the robot, which is a control target necessary for the simulation, with sufficient accuracy even by using a detailed nonlinear motion equation. It is difficult to theoretically guarantee the stability or control performance of the entire control system due to the fact that the feedback loop has a variable parameter such as the posture θ. Confirmation of sex is required. If the motion performance of the robot is prioritized, the non-linear model of the robot becomes a higher order, and it becomes necessary to take many motion points (freezing points) for linearization.

Therefore, as will be described in detail later, the series of flow in the present invention cannot be expressed even if the nonlinear motion equation of the robot is derived, the dynamic characteristic of the real robot is measured by system identification, and the nonlinear motion equation of the robot is used. It traces the process of extracting the parasitic vibration modes, modeling them (uncertainty), and incorporating the parasitic vibration modes into the simulation. Hereinafter, description will be made with reference to FIGS. 2 and 3.

FIG. 2 shows an example of the wafer transfer robot 1, which has a structure in which the hand on which the wafer is mounted moves back and forth in the X-axis direction. A first link 3, a second link 4, and a third link 5 are provided to move the hand link 2, and an angle θ formed by the first link 3 and the vertical axis Y is given to other links as shown in the drawing. Then, θ will be treated as the posture of the robot, and its movement will be grasped thereafter.

FIG. 3 is a flow chart showing the entire processing including the method according to the present invention. I will explain the outline. First, in a series of movements of the robot, a finite number of specific positions or postures taken by the hand, which is the control target portion, are selected in advance
It is treated as a selection point in the sense of the freezing point described above (step 1 of the flowchart, hereinafter referred to as S1 and the like).

The more the selection points are, the more faithfully the motion of the robot can be reproduced, but the posture θ is set to, for example, three points such as 0 degree, 25.4 degrees, and 59 degrees because of the large amount of calculation. Squeezed. These are some points that are suitable to cover the entire range of motion of the hand and allow some dynamics to be seen. For example, in the case where the characteristics do not change significantly up to a certain point and the characteristics change abruptly from a certain point, the point is selected by inferring this.

Based on these selection points, a servomotor for causing the robot to perform a series of operations is based on the gain schedule method in consideration of the parasitic vibration characteristic of the robot by the procedure described later. The gain parameter is set so that it can be controlled [S2]. Of course, if one robot has several motors, the same applies to the controller for each motor.

Next, the non-linear robot model is operated using the gain-set controller, and the effectiveness of the control by the controller is verified by simulation using this non-linear robot model [S3]. Since it does not use a real robot, it can be easily simulated on a personal computer. In this case, the nonlinear robot model is set as a set of linearized models for each selection point (θ _t : t = 1, 2, 3), and the uncertainty at the selection point at that time is calculated during the operation of the nonlinear robot model in the simulation. , By adding to the calculation result of the linearized model at that time, a motion extremely close to that of an actual robot is reproduced. Incidentally, this point will be described in more detail later.

By the way, the operation of the robot based on the set gain, for example, the speed and the acceleration are not exhibited according to the performance and the operation specification, or parasitic vibration appears (the vibration mode is excited) and the wafer is transferred. In [S4], it is judged through observation in the simulation, etc.

For example, if acceleration is taken as an example, it is assumed that a pattern paying attention to it is assumed, and if the simulation result is roughly within the range as shown in FIG. Become. In some cases, it is also considered whether the settling time is satisfied. If there is no problem, it is judged that it can be used as a controller [S5],
For example, if the behavior shown by the solid line in FIG.
The gain is adjusted by advancing or delaying the phase on the Bode diagram until the one satisfying the specifications such as the performance and operation of the robot is found [S6]. Although the processing here is not described in detail, for example, step 61 of FIG.
It may be performed by a known procedure such as No. 64 to 64.

If the performance of the robot and the operation specification of the wafer transfer robot are not satisfied nonetheless, any of the selection points that have been adopted so far is changed, and the gain is determined based on those postures. Do it again. In some cases, a new selection point is added, and the uncertainty in the posture is also reflected to reset the gain setting [S7]. By the way, although it is expressed as uncertainty, the fact is that the uncertainty is quantitative, as will be described later. This is because it is the difference between the equation of motion and system identification. However, even if the cause is an elastic deformation factor, it means that it is not known what the cause is.

Now, a gain setting method in the servo motor control controller according to the present invention will be described. A specific example will be described later, and will be briefly described with reference to the flowchart of FIG. When the selection point is determined in S1 of FIG. 3, the process proceeds to S21 of FIG. Then, first, the nonlinear dynamic characteristic of the robot is linearized near each selected point [S22]. Based on the linear equation of motion, the dynamic characteristic G (θ _t , s) near each selected point is derived [S23]. Note that θ _t means the posture at each selected point, and G (θ _t , s) thereof is represented by the equation (7) described later.

Next, from the input / output data when the wafer transfer robot is actually moved, system identification is performed for each selected point in the vicinity thereof [S24]. The input / output data are the torque input of the motor and the encoder angle output. As an identification method, there is a case where an ARX model is adopted and a one-step prediction error is used as an evaluation criterion, a parametric model or a nonparametric model is used, and any known system identification method may be adopted. In the example shown later, A
The identification method based on the RX model is adopted. It is unavoidable that there are various errors in any system identification, but they can be ignored if they are sufficiently small.

The identification model obtained by such system identification is as close as possible to a real robot, and the difference between this and the linearization model obtained by the above-mentioned linearization is the parasitic model of the robot. It is considered as an uncertainty that exhibits a vibration characteristic and is derived as a weighting function W (θ _t , s) [S25]. This is represented by equation (8) described below.

In order to avoid the occurrence of a resonance mode due to the uncertainty, the gain crossover frequency is selected and the phase margin is sufficiently secured to determine the gain of the controller at the selection point C (θ _t , s) [S2
6]. This is represented by the equation (9) described later, and its block diagram is, for example, as shown in FIG.

This makes it possible to control each servo motor that causes the wafer transfer robot to perform a series of operations based on the gain schedule method, taking into consideration the parasitic vibration characteristics of the robot. Just in case,
In the case of a robot whose dynamic characteristics change depending on the posture, system identification must be performed for each certain posture θ.
With respect to (θ _t , s), the operation is performed so that the compensation function is exhibited for each posture θ _t .

If the controller described above or the controller verified by the simulation described above is applied to the robot and the corresponding servo motor is controlled,
It goes without saying that the robot can be a suitable robot that does not hinder the transfer of wafers and can be operated at a desired speed and acceleration. Of course, the present invention is not limited to the wafer transfer robot, and can be applied to other various multi-axis robots without particular restrictions.

Here, in the simulation verification by the non-linear robot model, the weighting of the uncertainty of the motion of the non-linear model will be described. The intention is to prevent the parasitic vibration mode from being expressed when the robot arm is actually moved by identifying the parasitic model and incorporating it in the simulator. Therefore, the parasitic elements attached to the robot arm are explicitly expressed and designed by incorporating them into the simulator. In other words, a certain design is done as a cycle, and it is verified whether or not this actually moves on a simulator instead of a robot.

When making a simulator, what kind of parasitic element is present from the identification result of the robot arm is taken into the simulator as information. Then, the simulator will behave like a robot, if not the robot itself. And then verify,
Provide information to affirm or modify what you have designed through it. By doing so, it becomes easy to design a control system that satisfies the robot performance and operation specifications.

When the characteristics greatly differ depending on the posture θ of the robot, it is often impossible to freely move the robot with one controller. Therefore, information about the posture θ of the robot having a parasitic vibration mode is fetched into the controller, and the controller is switched according to the posture of the robot. This is the general way of building a gain schedule.

In the present invention, the simulation model that can be connected by the gain scheduling method is also controlled according to the posture θ. It should be noted that the uncertainty is incorporated at that time, and because the uncertainty is added, it is possible to design the controller that the control system is stable with respect to the sequentially moving posture θ at each point. It becomes.

By the way, since the posture θ comes in real time, it can be controlled by feedback. On the other hand, the magnitude of uncertainty depends on the posture θ. Since the uncertainty itself must also be fed back and reflected in the parameter θ, it is necessary to issue information about what posture θ the robot model is currently trying to reproduce and to extract the magnitude of the uncertainty corresponding to it. To do. FIG. 7, which will be described later, shows this.

As a result, the verification can be performed in a manner closer to that when an actual robot that actually exists is used. By the way, the selection points, for example, three are roughly given as representative points, but it can be thought that they will change each time, and the point is to switch like a switch, However, the selection can be made appropriately.

How the actual robot behaves at each point is expressed in the form of a model. Equation (1) described later is a non-linear differential equation, which corresponds to the non-linear model. However, for convenience of calculation, it is linearized for each selected point θ _t , and this is G in Eq. (7).
It is given by (θ _t , s). This shows how it moves with θ _t , and you can see that it is in the sun.

Next, the magnitude W (θ _t , s) of the uncertainty, which allows only rough information to be obtained, is obtained. As shown in FIG. 17, the value is the difference between G (θ _t , s) represented by the broken line and the entire linearized model considering the uncertainty of FIG. 6 represented by the solid line. When G (θ _t , s) is interpolated, W (θ _t , s) is also interpolated with respect to θ _t .

Although the selection point has been described as the posture θ _t , the position of the hand may be taken as the direct selection point. The position is 0 mm, 2 × in the example of FIG.
350 sin 25.4 ° = 300 mm, 2 × 350 sin5
Three points of 9 ° = 600 mm will be adopted.

[0055]

[Embodiment] The following will describe an actual example of a specific design method and effectiveness verification method of a control system configured by explicitly considering parasitic vibration characteristics of the robot for trajectory tracking control of the wafer transfer robot. .

First, regarding the wafer transfer robot, by performing system identification, it is possible to know the magnitude W (θ, s) of the uncertainty in the vicinity of each posture θ. That is, the dynamic characteristics of the wafer transfer robot in the vicinity of each posture θ are as shown in FIG. 6, a linear system G (θ _t , s),
It is expressed using the uncertainty magnitude W (θ _t , s) and an arbitrary variation Δ (s) of 1 or less. The magnitude of uncertainty varies depending on the posture θ of the robot. The method shown in FIG. 7 is proposed as a framework for verifying the performance of the gain scheduling control system configured based on the above.

The processing described below will be briefly introduced here. First, a nonlinear equation of motion is derived. By linearizing this nonlinear equation of motion, the dynamic characteristic G (θ _t , s) in the vicinity of each posture θ _t is obtained. System identification is performed using the input and output data obtained by the experiment. By comparing the system identification and the linearized model, uncertainty in the vicinity of the orientation theta _t size W (theta _t,
s) is given. The controller C (θ _t , s) in each fixed posture θ _t is designed. The effectiveness of the constructed gain scheduling control system is verified by simulation. At that time, first, τ which becomes the reference input to the robot
ask for _ref . A scheduling method with uncertainty W (θ _t , s) is given, and a simulation using this method is performed. The individual processes will be described below based on actual examples.

Incidentally, the description of FIG. 7 is added a little. In the figure, the posture θ from the robot model 1M to W (θ, s)
Is important. This means that even in the simulation, the uncertainty is obtained using the parameters when the system is identified, so the characteristics of the robot will change if the posture θ changes. That is, if θ is different, the model changes and the magnitude of uncertainty also changes.
The posture θ is put in the weighting function W (θ, s), and a different one is corresponded to each time. After all, the robot model is changed depending on the posture.

Therefore, when the controller 6 also reaches a certain position, it can be switched using the posture θ as a parameter according to the gain schedule. That is, by teaching the current model position, both the controller and the uncertainty are used corresponding to θ at that time. Since the magnitude of uncertainty is obtained by system identification, the difference obtained from it and the equation of motion, and eventually the difference becomes the same as the actual output.

Where W (θ,
Although it is supplemented by s), it is easy to obtain an understanding if we teach θ at that time and present the shortfall. Since the simulator changes θ from 0, for example, with respect to the target value input θ _ref , the flow to Δ (s) is also secured in order to teach θ at that time also to the uncertainty part. There is.

[Modeling] In order to analyze and control the dynamic characteristics of the robot, a motion equation of the robot is required. The equation of motion of the robot is derived by the Lagrange method or the Newton-Euler method, and generally a nonlinear equation of motion is obtained. Here, the motion equation of the wafer transport robot is derived by the Lagrangian method. The derived equation of motion becomes the basis for later control system design and simulation.

[Mechanical Model of Wafer Transfer Robot] Although the wafer transfer robot is a two-degree-of-freedom manipulator that moves in a plane, consider the linear motion of the hand of the wafer transfer robot 1 shown in FIG. Each parameter of the robot is given as follows. Then, the equation of motion of the wafer transfer robot is derived from the Lagrangian equation of motion.

[Table 1]

[Equation 1] 1st link, 2nd link, 3rd link and minion link luck
Kinetic energy T₁, T ₂, T₃, T_FourAnd

[Equation 2] Further, since each link and the hand are rotated by θ, when the total sum of rotational energy is T ₅ ,

[Equation 3] Is. In the following, I = I ₁ + I ₂ + I ₃ + I ₄ .
Letting T = T ₁ + T ₂ + T ₃ + T ₄ + T ₅ , Lagrange's equation of motion

[Equation 4] The equation of motion of this model is obtained from this. Q is a generalized force, but in this case it is a torque. Therefore, the equation of motion is

[Equation 5] And a non-linear differential equation is obtained.

[Linearization of Equation of Motion] It is difficult to design the control system by treating the equation (1) as it is. Therefore, the linearization of Expression (1) is performed. The linearization of the controlled object is a certain posture θ
_{At t} ,

[Equation 6] Is. Linearization is performed assuming movement near this equilibrium point. Equation (1) can be written as:

[Equation 7]

[Equation 8] Becomes From equation (7), it can be seen that the linearized equation of motion depends on the equilibrium point θ _t of the link angle, and has a quadratic integral characteristic from the transfer function expression of equation (7).

[System Identification by ARX Model] The ARX model is used for system identification. Input-output relation in ARX model

[Equation 9] Becomes The parameters may be determined so that this one-step prediction error becomes small. In the ARX model, the parameters are determined by using the method of least squares.

The ARX model is the most basic model in system identification. Therefore, system identification of the wafer transfer robot is performed using the ARX model. The position of the robot's hand is 0 [mm], 300 [mm], 600 [m
Identification is performed for three poses in m]. Since the robot moves linearly by moving the motor 1 axis, the input is the torque and the output is the angle of the motor 1 axis in the identification. Random input is input to the motor 1 axis, the angle of the motor 1 axis is measured, and the input / output data is used for identification. The input sampling time in the identification experiment is 0.005 [S] (200 [H
z]).

In each posture, as the data processing before identification, the average value of the data is set to 0, and in order to remove the high frequency disturbance,
Decimate by removing the two following one. As a result, the data sampling time is 0.
005 [S] (200 [Hz]) to 0.015 [S] (20
0/3 [Hz]). Furthermore, a third-order low-pass filter up to 150 [rad / s] is applied and processed. In addition, the model order is 12th. MATLAB for specific calculation
The System Idenfification toolbox of AB was used. The frequency characteristics of the identified discrete time model are shown in FIGS.
Shown in. Further, the obtained identification model is converted into a continuous time model as shown in FIGS. 11 to 13.

It is necessary to examine the validity of the identified model. Here, the frequency response result is compared and examined. Since the output of the frequency response is measured in terms of the angular velocity with respect to the torque input, the units must be matched. Therefore, a model is applied to the identified model for comparison. The results are shown in FIGS. 14 to 16. It can be seen that it fits better than the figure.

Further, FIGS. 17 to 19 show a comparison between the model obtained by system identification and the transfer function derived from the Lagrange's equation of motion. It can be seen that the gain characteristics are relatively matched in the high frequency band except for the vibration mode, but the phase characteristics are not matched at all.

[Estimation of Uncertainty] From the results obtained by system identification, it is found that the dynamic characteristics of the robot have vibration characteristics that cannot be represented by the nominal model (7) obtained by the Lagrangian method, especially in the high frequency band. . 21 to 23, each posture θ ₁ = 0 [deg]
, Θ ₂ = 25.4 [deg], θ ₃ = 59 [deg]
The nominal model G (θ _i , s), i = 1, 2,
3 and the results obtained by system identification

[Equation 10] Shows the comparison.

In order to describe the difference between the results obtained by the system identification and the nominal model shown in FIGS. 21 to 23, the expression by additive modeling error is used here.

[Equation 11] Is adopted (see FIG. 20). Where G (θ _i ,
s) and W (θ _i , s) represent the nominal model and the weighting function expressing the magnitude of uncertainty in each posture. Also, Δ (s) is

[Equation 12] It is an arbitrary variation that satisfies Note that, here, the weighting function W (θ _i , s) is set to the difference in FIGS.
Ie

[Equation 13] Determined by. As a system expression that includes this uncertainty,
Instead of the additive modeling error described above, for example, a multiplicative modeling error, a representation using irreducible decomposition, or structural uncertainty can be adopted.

[Control System Design] A controller is designed by the loop shaping method based on the above-mentioned equation (7) of the linearized mathematical model of the wafer transfer robot. From the system identification result, it is known that the robot has a parasitic resonance mode in the high frequency range. From this, it is expected that if the gain crossover frequency is set too large, destabilization will occur. Therefore, the gain crossover frequency is set to 10 [rad /
s] Below, the controller is designed so as to secure a sufficient phase margin. Further, here, the controller is designed so that the gain crossover frequency and the phase margin have constant values with respect to changes in the posture θ _t .

The concretely designed scheduling controller is as follows.

[Equation 14] When using the designed scheduling controller,
The open loop transfer function L (θ _i , s) does not depend on the posture θ _t . From this, it is expected that a constant control performance is maintained even when the dynamic characteristics change due to the change in the posture of the robot. 24 to 26 are Bode diagrams of the controlled object and each open-loop transfer function when the scheduling parameter θ _t is given. The solid line is the controlled object G (θ _i , s),
It is a Bode diagram of i = 1,2,3, and a broken line is a Bode diagram of an open loop transfer function L (θ _i , s), i = 1,2,3. It can be seen that the controlled object (solid line) changes its characteristic depending on the scheduling parameter θ _t , while the open-loop transfer function (broken line) holds a constant characteristic.

[Simulation] When a nonlinear system is linearized at a certain equilibrium point and a controller is designed based on the linear control system design theory, its effective range is in the vicinity of the equilibrium point. It is considered that the designed controller does not have a sufficient response when the purpose of control requires an operation larger than the effective range of linearization. Gain scheduling is introduced to compensate for such drawbacks.

When actually moving the controlled object, a target trajectory in consideration of the purpose and conditions of control is given, and in the case of a robot in particular, the target trajectory of the hand is given. Here, a target trajectory of the hand of the wafer transfer robot is given, and a control simulation that follows the trajectory is performed. The control system is shown in Fig. 7.
Build like. The target operation of control is moved by the reference input torque τ _ref , and the difference from the target value at a certain hand position (link angle) during operation is calculated by the scheduling controller C.
This is a control system that works to reduce (θ, s).

Since the entire operation is performed by the feedforward control and the difference from the target value is reduced by the feedback control, it is considered that high speed and highly accurate control can be performed. The reference input torque τ _ref is obtained by the inverse dynamics method. The reference input in the feedback control simulates the target value response of the link angle. In the simulation, in order to verify the effectiveness of gain scheduling, the parameter θ in the designed scheduling controller C (θ, s) is used.
Simulation is performed when is fixed and verified.

[Reference Input] As an operation mode of the wafer transfer robot, here, the hand position z is changed from z = 0 [mm] to z = 680 [mm], that is, the link angle θ = 0.
Assume a movement from [deg] to θ = 76.3 [deg]. Also, the upper limit of the acceleration of the link hand position is 700 [m
m / s ² ]. The target trajectories beyond this are

[Equation 15] Given in.

28, 30 and 32 show the target position, target speed and target acceleration of the given hand. Figure 2
7, FIG. 29 and FIG. 31 show the target trajectory of the hand with the formula (1
1), (12), and (13) are the link angles, angular velocities, and angular accelerations obtained by substitution. FIG. 33 is a reference input of torque obtained by substituting the target trajectory of the link into the equation of motion.

[Simulation Results] A robot simulation including a modeling error is performed using the scheduled weighting function W (θ, s). Further, in the controller C (θ, s), θ = 0 ° or 76.3 °
Perform a simulation using the controller fixed to. In the simulation considering the modeling error, the weighting function is scheduled as follows. The entire weighting function scheduled for θ is W (θ, s)
And

[Equation 16]

FIG. 34 shows a simulation result of control by gain scheduling. Further, FIG. 35 shows a simulation result when θ = 0 ° is fixed in each controller, and FIG. 36 is a simulation result when θ = 76.3 ° is fixed. Also see the simulation results for acceleration response. The acceleration output must not exceed the reference input so that the wafer does not slip. The results are shown in FIGS. 37 to 39.

Although it is possible to follow the reference input of which controller and the acceleration does not exceed the reference input, it is not preferable because the acceleration is oscillating when it is fixed at θ = 0 °. Comparing the gain scheduling with the case of fixing at θ = 76.3 °, the gain scheduling has a smaller difference from the reference input. When the controller is installed, such a difference is expected to be larger. Therefore, gain scheduling is effective. The designed controller was designed by loop shaping so that the gain crossover frequency would not be too large for the modeling error. Considering the modeling error in the high frequency band, it is necessary to design a controller such as a notch filter that reduces the gain in the high frequency band, especially the gain at the resonance frequency.

[0081]

As can be seen from the above description, according to the gain setting method in the controller for controlling the servo motor according to the present invention, the parasitic vibration mode extraction which cannot be expressed even by using the nonlinear motion equation of the robot is extracted. And the modeling becomes easy. Then, the gain crossover frequency can be selected to avoid the occurrence of the resonance mode due to the uncertainty, and the phase margin can be sufficiently secured to determine the gain of the controller at a specific position or posture.

Therefore, each servo motor that causes the robot to perform a series of operations can be controlled based on the gain scheduling method, taking into consideration the parasitic vibration characteristics of the robot. By dividing the dynamic characteristics of the robot into a non-linear motion equation and a parasitic vibration mode, higher order models can be avoided, and the demand for higher performance of the controller can be suppressed and cost can be reduced. This can be a controller that eliminates resonances that the PID servo driver cannot handle.

When the operation of the robot based on the set gain cannot satisfy the performance and operation specifications of the robot by advancing or delaying the phase,
Change any of the already selected positions or orientations, or add new specific positions or orientations,
Gains can be redetermined based on their position or orientation. Even if the performance and operation specifications of the robot cannot be satisfied by any means, it is possible to find out what kind of specifications allow the desired movement.

If the nonlinear robot model is operated using the controller obtained by the gain setting method in the servo motor control controller and the uncertainty is incorporated into the simulation, the nonlinear robot model is simulated.
The effectiveness of control by the controller can be verified while rotating one cycle. Although the design of the controller is slightly more complicated than the case where only the nominal model is used as in the past, the fear of deterioration of stability and control performance when adapting to an actual machine is eliminated. Of course, the order of the model can be reduced and the adaptability to a real robot can be increased.

In this case, a set of linearized models for each specific position or orientation is set as the nonlinear robot model, and during the operation of the nonlinear robot model in the simulation, the uncertainty in the position or orientation at that time is linearized at that time. Since it is added to the calculation result of the model, it is possible to reproduce a motion extremely close to that of an actual robot and confirm the stability by simulation, that is, the effectiveness of the controller can be accurately verified.

Needless to say, whether the controller obtained by the gain setting method in the servo motor control controller or the controller verified by the effectiveness verification method of the servo motor control controller can be applied to a robot and controlled. In this case, it is possible to suppress the parasitic vibration that the robot has and to make the behavior that conforms to the performance and operation specifications of the robot possible. As a result, it will greatly contribute to the improvement of efficiency such as speeding up of processing by the robot.

[Brief description of drawings]

FIG. 1 is a flowchart showing a gain setting method in a servo motor control controller according to the present invention.

FIG. 2 is an example of a wafer transfer robot having a structure in which a hand link for mounting a wafer advances and retreats in the X-axis direction.

FIG. 3 is a flowchart showing the overall processing of a gain setting method and a controller effectiveness verification method according to the present invention.

FIG. 4 is a block diagram of a compensator (controller) whose gain is determined.

FIG. 5 is a flowchart showing an example of gain adjustment by changing the phase.

FIG. 6 is a linearized model diagram in which uncertainty is considered.

FIG. 7 is a verification block diagram that employs an uncertain nonlinear model proposed as a framework for verifying the performance of a gain scheduling control system.

FIG. 8 shows frequency characteristics in the discrete-time model identified when x = 0 [mm].

FIG. 9 shows frequency characteristics of the discrete-time model identified when x = 300 [mm].

FIG. 10 shows frequency characteristics of the discrete-time model identified when x = 600 [mm].

FIG. 11 shows frequency characteristics in the continuous-time model identified when x = 0 [mm].

FIG. 12 shows frequency characteristics of the continuous-time model identified when x = 300 [mm].

FIG. 13 shows frequency characteristics of the continuous-time model identified when x = 600 [mm].

FIG. 14 is a comparison diagram with a frequency response in a discrete-time model at x = 0 [mm].

FIG. 15 is a comparison diagram with the frequency response in the discrete-time model at x = 300 [mm].

FIG. 16 is a comparison diagram with a frequency response in a discrete-time model at x = 600 [mm].

FIG. 17 is a comparison diagram with a linearized model at x = 0 [mm].

FIG. 18 is a comparison diagram with a linearized model at x = 300 [mm].

FIG. 19 is a comparison diagram with a linearized model at x = 600 [mm].

FIG. 20 is a block diagram illustrating introduction of a model error.

FIG. 21 is a Bode diagram of the nominal model G (θ _i , s) and the system identification model in the posture θ ₁ = 0 [deg].

FIG. 22 Attitude θ₂= 25.4 [deg]
Minal model G (θ _i, S) and the system identification model
Diagram.

FIG. 23 is a Bode diagram of the nominal model G (θ _i , s) and the system identification model in the posture θ ₃ = 59 [deg].

FIG. 24 is a Bode diagram of the controlled object and each open-loop transfer function when the attitude θ _t = 0 [deg] is given.

FIG. 25 is a Bode diagram of the controlled object and each open-loop transfer function when the posture θ _t = 25.4 [deg] is given.

FIG. 26 is a Bode diagram of the controlled object and each open-loop transfer function when the posture θ _t = 59 [deg] is given.

FIG. 27: Reference input (angle) of link.

FIG. 28: Reference input of hand position (target position).

FIG. 29: Reference input of link (angular velocity).

FIG. 30. Reference input of hand position speed (target speed).

FIG. 31: Reference input of link (angular acceleration).

FIG. 32 is a reference input of a hand position acceleration (target acceleration).

FIG. 33. Torque reference input.

FIG. 34 is a control simulation result by gain scheduling.

FIG. 35 is a simulation result when the controller is fixed at θ = 0 °.

FIG. 36 is a simulation result when θ is fixed at 76.3 °.

FIG. 37 is an acceleration response diagram based on gain scheduling.

FIG. 38 is an acceleration response diagram when θ = 0 °.

FIG. 39 is an acceleration response diagram when θ = 76.3 °.

FIG. 40 is a response diagram showing a state in which the acceleration response deviates from the assumed acceleration pattern and the robot performance or operation specification cannot be satisfied.

[Explanation of symbols]

1 ... Wafer transfer robot, 1M ... Robot model, 2 ...
Minor link, 3 ... First link, 4 ... Second link, 5 ... Third link, 6 ... Controller, θ ... First link and vertical axis Y
The angle (posture) formed by and.

   ─────────────────────────────────────────────────── ─── Continued front page    F-term (reference) 3C007 KS21 LS20 LT13 LV24 LW08                       MT05

Claims (7)

[Claims] 1. A posture and a position are preliminarily grasped for magnitudes of factors that cannot be included when the dynamic characteristics of the robot are modeled by system identification of the robot, and parasitic vibrations caused by the factors are suppressed. In the method of setting the gain in the controller for controlling the servo motor of the robot, which changes the dynamic characteristics depending on the The nonlinear dynamic characteristics possessed are linearized in the vicinity of each of the selected specific positions or postures, and from the input / output data when the robot is actually moved, the system is set in the vicinity of each of the specific positions or postures described above. The difference between the identified linearization model obtained by the linearization and the identification model obtained by the above system identification is calculated by the robot. The gain crossover frequency is selected to avoid the occurrence of a resonance mode due to the uncertainty, and a sufficient phase margin is secured to prevent the occurrence of a resonance mode caused by the parasitic vibration characteristic. The feature is that each servo motor that determines the gain of the controller and causes the robot to perform a series of operations can be controlled based on the gain scheduling method after explicitly considering the parasitic vibration characteristics of the robot. Gain setting method for servo motor controller. 2. If the robot motion based on the set gain cannot meet the robot performance and motion specifications by either advancing or retarding the phase, the position or posture already selected. 2. The gain setting method for a servo motor control controller according to claim 1, wherein any one of the above is changed and the gain is determined again based on the position or the posture thereof. 3. A position or orientation that has already been selected when the robot motion based on the set gain cannot meet the robot performance and motion specifications by either advancing or delaying the phase. 2. The gain setting method in the servo motor control controller according to claim 1, wherein a new specific position or posture is added to, and the gain is determined again based on the position or posture. 4. A robot control method, characterized in that the controller obtained by the gain setting method in the servo motor control controller according to claim 1 is applied to a robot for control. 5. A non-linear robot model is operated using a controller obtained by the gain setting method in the servo motor control controller according to claim 1, and the controller is operated by a simulation using the non-linear robot model. A method for verifying the effectiveness of a servo motor control controller, characterized in that the effectiveness of the control is verified. 6. The non-linear robot model is set as a group of linearized models for each of the specific positions or orientations, and in the simulation, during the operation of the non-linear robot model, the uncertainty in the position or orientation at that time is then calculated. 6. The method of verifying the effectiveness of a controller for servo motor control according to claim 5, wherein a motion extremely close to that of an actual robot is reproduced by adding it to the calculation result of the linearized model of. 7. A robot control method, wherein a controller verified by the effectiveness verification method of the servo motor control controller according to claim 5 or 6 is applied to a robot for control.