JPH01136214A - Servo control method for articulated robot - Google Patents

Abstract

PURPOSE:To stably execute control even disturbance, etc. exists by adding the output signal of slip condition control to a target angle acceleration signal. CONSTITUTION:The servo control of respective shaft motors 14 of an articulated robot is composed of an angle loop gain element 4, a relay circuit 8, an angle loop gain element 9, an angle acceleration loop gain element 13 and first and second integrating elements 15 and 16, etc. The slip condition control is executed by a target angle signal thetar of respective joints based on a track, along which an arm tip is moved, a target angle speed signal thetar', a real joint angle (theta) by feedback, an angle deviation (x) with a real joint angle speed signal theta' and a switching function Zw to cause an angle speed deviation -x' to be zero. The switching signal Zw is added to an angle speed commanding signal in order to drive a motor 14 of the respective joints. In such a case, the slip condition control is executed by the switching function Zw so as to condense an angle deviation (x) to be zero also. A switching signal (u) is added to the angle acceleration commanding signal. Thus, the angle deviation (x) is corrected.

Description

[Detailed Description of the Invention] (Industrial Application Field) The present invention relates to a servo control method for an articulated robot, and in particular, utilizes the slipping phenomenon observed in a relay control system to control an arm tip along a target trajectory. Concerning improvements to what was made.

(Prior Art) Conventionally, the motion control system of an articulated robot includes nonlinearity, variable parameters, and interference characteristics, and therefore control is generally not easy. For this reason, various compensation methods have been considered, but methods such as feedforward control, non-interference, and linearization require accurate understanding of dynamics parameters. In addition, adaptive control such as model exemplar type requires quite complex algorithms, and learning methods require several trials before obtaining the desired output, making real-time control difficult.

Therefore, in recent years, with the practical use of direct drive systems without gears, the sliding phenomenon of the relay control system has been proactively utilized to control the arm's tip position to the target position (slip state control (S).
A method for controlling lidlng mode has been proposed. In this slip state control, when the controlled object is constrained to a hyperplane set in the state space, the control device parameters are switched on both sides of the hyperplane (switching surface), so that the controlled object is in the vicinity of the hyperplane. It is a control that repeats minute vibrations and is restrained, and is characterized by stability (robustness) that is insensitive to fluctuations in controlled parameters, load disturbances, external disturbances, etc.

When applying slip state control to motion control of an articulated robot, a method is generally adopted in which the output torque of a servo motor is determined by a switching function.

(Problems to be Solved by the Invention) By the way, when the above slip state control is adopted, the output torque itself of the motor of each joint can be accurately controlled by current feedback or the like.

However, there are effects such as torque generated by other axis motors, centrifugal force, and Coriolis, and these effects cannot be removed by current feedback. The problem is that it is difficult to control the trajectory with high precision.

The present invention has been made in view of the above, and its purpose is to apply slip state control to a feedback control system to prevent effects of other shaft motors, disturbances, load fluctuations, etc. The objective is to obtain a robust control function.

(Means for Solving the Problems) In order to achieve the above object, the solving means of the present invention is a servo control method for an articulated robot. To control the slip state using a switching function that converges to zero the angular deviation between the target angular velocity of each joint and the actual joint angular velocity due to feedback, and the angular velocity deviation between the target angular velocity of each joint and the actual joint angular velocity due to feedback, according to the angular velocity signal. Next, after adding this switching signal to the angular acceleration command signal for driving the joint drive mechanism, the drive mechanism of each joint is driven according to the added value so that the arm tip follows the target trajectory. It consists in controlling to match.

(Operation) According to the above method, the present invention eliminates the target angle signal of each joint based on the trajectory to be moved by the arm tip, the actual joint angle based on the target angular velocity signal and feedback, the angular deviation from the actual joint angular velocity, and the angular velocity deviation. Sliding state control is performed using a switching function, and the output switching signal is added to an angular acceleration command signal for driving each joint drive mechanism.

In that case, slip state control is performed using a switching function that converges not only the angular velocity deviation but also the angular deviation to zero, and the switching signal for the slip state control is added to the angular acceleration command signal, so that the angular error can be reduced. In other words, the angular deviation is corrected by high-gain slip state control, which provides a stable but strong restraining force against the effects of torque, centrifugal force, and Coriolis generated by motors on other axes, as well as external disturbances and load fluctuations. Robust control with

(Example) Hereinafter, an example of the present invention will be described based on the drawings.

FIG. 1 shows a block diagram of one joint when the present invention is applied to a multi-joint robot, and θr.

θr, υ" are the target angle signal, target angular velocity signal, and target of the joints that are sequentially output based on the speed plan drawn up by a planner (not shown) based on the target trajectory signal in which the robot's arm tip should move, respectively. This is an angular acceleration signal.

(3) is the first summing point where the angular deviation X is calculated by calculating the algebraic difference between the actual joint angle signal θ fed back from the joint side and the target angle signal θ, and (4) is the first addition point where the angle deviation X is calculated. Angle loop gain element that multiplies deviation X by angle loop gain C.
(7) calculates the algebraic difference between the multiplication result Cox of the angle loop gain element (4) and the angular velocity deviation - statement (C-x ten statements). The third summing point (8) is a relay circuit for switching the output signal ((11X ten sentences) of the third summing point (7) in a sliding state.Here, the corresponding relay circuit (8) In this case, the slip state is controlled using the input signal ZW as a switching function, and the switching function zw is expressed as ZW - C - x (1).

Here, the switching function ZW is set as follows.

If zw>0, then w<O If zw<Q, then w>0 (2) Or, if zyy>9 then father<-C-sentence If ZW<0 then father>-C-sentence (3) is satisfied. In other words, it is designed so that 2zw-iw<0 (4). That is, as shown in Fig. 2, it is designed so that the state points converge on the switching line and enter the so-called sliding mode,
), the slip state control is performed using a switching function ZW that converges the angular deviation I am trying to find something.

Next, (9) multiplies the above angular velocity deviation - statement by the angular velocity loop gain Kv to obtain the angular acceleration command signal Or' (=-Kv
This is an angular velocity loop gain element that outputs - statement).

In addition, (10) calculates the algebraic difference between the target angular acceleration signal υr and the actual joint angular acceleration signal υ, and calculates the angular acceleration deviation − father (−
This is the fourth addition point for calculating υr - υ), and the angular acceleration deviation signal is used to perform angular acceleration feedback compensation. At this time, the actual joint angular acceleration signal υ fed back from the joint side is detected by a method using an accelerometer, a method of differentiating the angular velocity, a method using an observer, etc. The detection error n is weighted. therefore,
The signal value output from the fourth summing point (10) is −
(Father and son 〇).

(11) is the switching signal U from the relay circuit (11), the output signal υ'' of the angular velocity loop gain element (9), and the angular acceleration output from the fourth summing point (10). It is the fifth addition point for calculating the algebraic sum with the deviation signal -(+n), and the following output signal zo--Kv-Bunichi-chi-n+u(5
) is output.

Furthermore, (13) is an angular acceleration loop gain element that calculates the torque signal by multiplying the output signal ZQ from the fifth summing point (11) by the angular acceleration loop gain Ka; (14)
is a motor as a drive mechanism that drives the joint according to the torque signal output from the angular acceleration loop gain element (13), and (15) is a motor that integrates the actual joint angular acceleration signal υ of the motor (14) to calculate the joint The first integral element that obtains the angular velocity of 0, (
16) is the actual joint angular velocity signal θ of the first integral element (15)
The joint angle θ, which is the output value of the second integral element (16), is input to each arm and converted into three-dimensional coordinates. There is.

In the figure, when the target angular velocity signal θ is output based on the velocity plan of the robot arm tip, the second summing point (
In step 5), the angular velocity deviation between the target angular velocity signal 0 and the actual joint angular velocity signal 0 is calculated, and further, the angular velocity deviation signal is calculated by the angular velocity loop gain element (19).
The sentence is multiplied by the angular velocity loop gain Kv, and an angular acceleration command signal υr' (--Kv-*) is output.

On the other hand, at the first addition point (3), the angular deviation X between the actual joint angle signal θ and the target angle signal θr is calculated, and the angular loop gain element (4) adds the angular loop gain C to the angular deviation signal is multiplied and the signal C-x is output, at the third addition point (7), the second addition point (5
) is subtracted, and the calculation result zw (-C-x ten sentences) is used as a switching function in the relay circuit (8
) is used to control the slip state.

Also, at the fourth addition point (10), the deviation between the target angular velocity signal υ and the actual joint angular acceleration signal υ is calculated, and the angular acceleration deviation signal containing the detection error n of each actual joint acceleration υ is calculated.
father+n) is output.

Then, at the fifth summing point (11), the angular acceleration command signal υr' (m-Kv- statement), the output value U of the slip state control by the switching function zw, and the angular acceleration deviation signal - (father and son 〇) are combined. After the algebraic sum is calculated, the control signal ZQ shown in equation (5) above is finally output, and after being converted into a torque signal by the angular acceleration loop gain element (13), the motor ( 14) is rotationally driven, and servo control of the articulated robot is performed so that the arm tip matches the target trajectory.

Therefore, in the above embodiment, the switching function zw (=
Since the slip state is controlled by the C-x+ statement) and the output signal U is added to the angular acceleration signal υr', it is possible to control the slip state by converging only the angular velocity deviation statement to zero, as in the conventional method. Compared to determining the state control output as the torque output to the motor (14), the angular error, that is, the angular deviation Centrifugal force,
Robust control with stable but strong restraining force is possible against the effects of Coriolis interference, disturbances, load fluctuations, etc.

In addition, in the above embodiment, feedback compensation of the actual joint angular acceleration υr of the motor (14) is performed, so that the influence of torque generated by other axis motors, centrifugal force, interference such as Coriolis, disturbance, load fluctuation, etc. However, an angular acceleration command signal that cancels out these effects is fed back, and the effects of these effects are reduced.

In addition, in the above embodiment, if the amount of discontinuity associated with switching of the switching function ZQ used for slip state control is set to be larger than the detection error n of acceleration, the deviation of the arm tip from the trajectory due to the detection error n can be reduced. It can be effectively prevented.

(Effects of the Invention) As explained above, according to the servo control method for an articulated robot of the present invention, not only the angular velocity deviation of each joint but also the
Since the slip state control is performed to converge the angular deviation to zero, and the output signal of the slip state control is added to the target angular acceleration signal, torque, centrifugal force, Coriolis, etc. generated by the motors of other axes can be reduced. It is possible to perform robust control that is not only stable but also has strong restraint against the effects of actions, disturbances, and load fluctuations.

[Brief explanation of the drawing]

The drawings show an embodiment of the present invention, and FIG. 1 is a block diagram showing a control system for one joint of a multi-joint manipulator, and FIG. 2 is a switching characteristic diagram of slip state control in a relay circuit. (8)...Relay circuit, (14)...Motor (drive mechanism) Patent applicant: Daikin Industries, Ltd.
Before Rinto 1) Hiro゛.

Claims (1)

[Claims] (1) According to the target angle signal and target angular velocity signal of each joint based on the trajectory that the arm tip should move, the angular deviation between the target angle of each joint and the actual joint angle based on feedback, and the target angular velocity of each joint and feedback A switching signal for controlling the slip state is determined using a switching function that converges the angular velocity deviation from the actual joint angular velocity to zero, and then, after adding this switching signal to the angular acceleration command signal for driving the joint drive mechanism, A servo control method for an articulated robot that controls the arm tip to match a target trajectory by driving the drive mechanism of each joint according to the added value.