JPS633317A - Acceleration feedback device - Google Patents

Abstract

PURPOSE:To improve the control properties of an acceleration feedback device by adding an arithmetic means into an acceleration feedback loop to extract only the desired acceleration component. CONSTITUTION:A robot main body 10 is driven by motors 41-43 and the acceleration signals detected by detectors 21-23 are read into a servo processor 110 from an analog input interface 113 via an amplifier 70. While the outputs of tachogenerators 61-63 attached to each motor are also read into the processor 110 from the interface 113. The processor 110 gives operations to the target value of each servo system supplied from a other processor via a bus line 120 and the feedback signal supplied from each detecting element and drives motors 41-43 via an analog output interface 111 and a servo amplifier 90.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a motion control device for a robot, and in particular, a robot having an arm mechanism such as an articulated two-cylindrical coordinate system or a polar coordinate system, and which is fast and agile. The present invention relates to an acceleration feedback device suitable for motion control of robots that require high operational performance.

[Conventional technology]

In robots that require high motion performance, each joint is generally driven by a servo mechanism. In servomechanisms, acceleration feedback compensation is often used as a means to improve operational performance such as stability and quick dissolution. In addition, in recent years, adaptive control methods have been studied as a motion control method for articulated robots, but here too, if control is to be performed including response acceleration deviations, it is necessary to perform acceleration feedback.

Conventionally, as an example of applying an acceleration feedback device to a robot, there is a report by Japan entitled ``Artificial hand control algorithm construction method and dynamic control mode J'' in Biomechanism 3 (1975). An acceleration detector is attached to the robot's wrist, and the acceleration signal is fed back to the control system, but the acceleration signal is only used to detect when the robot's hand collides with an object. Publication No. 58-3001 and JP-A No. 60-202
No. 14 discloses a configuration in which an output signal from an acceleration detector is fed back to a position servo system. Also, FR by Kura et al.
The report entitled ``igicon Arm: Anti-vibration control of industrial robots'' describes how an acceleration detector is attached to each of the three links of the arm of an articulated robot, and these signals are returned to the servo system that drives each corresponding link. A configuration is shown. However, in all of these examples, the motion of each servo system is considered to be independent.

Furthermore, in Japanese Patent Application Laid-Open No. 59-107881, two linear acceleration detectors are provided per link, and by subtracting the surface output signal, the rotational angular acceleration signal of the servo axis is detected and fed back to the servo system. The configuration is shown. Furthermore, in Japanese Patent Application Laid-Open No. 59-69280, an orthogonal three-direction linear acceleration detector is installed at the tip of a three-degree-of-freedom orthogonal robot to detect acceleration in the direction of movement of each degree of freedom. A configuration is shown in which the acceleration along the direction is fed back to the servo system.

[Problem that the invention seeks to solve]

The above-mentioned conventional technology does not take into account the problem of interference of motion components, which always appears in multi-jointed, two-cylindrical shelf type, polar coordinate type, etc. robots that have mechanical parts including rotary joints. In a robot with a rotary joint, the movement of a certain rotary joint affects all the links closer to the hand than this joint, so the acceleration signals detected by the acceleration detectors installed on these links are In addition to the motion acceleration component of the servo system itself that drives the link, it includes components resulting from the motion of the servo system that drives other links. Therefore, in order to correctly realize acceleration feedback compensation in the servo system of the link, it is necessary to remove unnecessary signals other than components caused by the movement of the servo system of the link. If the output of the acceleration detector of each link is directly fed back to the servo system without performing this processing, the effect of acceleration feedback compensation will be impaired by the extra signal, and sufficient performance will not be obtained.

The technique disclosed in Japanese Patent Application Laid-Open No. 59-107881,
In order to accurately detect angular acceleration, the distance between the mounting positions of the two linear acceleration detectors must be sufficiently large. However, if this distance is not kept sufficiently small, the upper forearm link In this case, the effect of the centripetal acceleration on the two acceleration detectors of the upper forearm link becomes different depending on the movement of the pivot axis, so even if the difference between the two signals is taken, the effect of the movement of the pivot axis cannot be canceled out. That is,
In this configuration, highly accurate detection of the angular acceleration of the link,
There are two problems that are required of an angular acceleration return device for a robot: suppression of the influence of the motion of other links.

Furthermore, the technique disclosed in Japanese Patent Application Laid-Open No. 59-69280 is applicable only to Cartesian coordinate robots. When applied to a robot, which is the target of the present invention, and which has a mechanical system that includes rotary joints and has mutual interference of motion components, the acceleration detected by each acceleration detector of the hand will depend on the posture of the robot's arm. Since the acceleration has components in different directions, it is not possible to detect acceleration in the three fixed X, Y, and Z directions.

An object of the present invention is to eliminate the interference of motion components between links found in multi-joint nine cylindrical coordinate type robots, polar coordinate type robots, etc., which have mechanical parts including rotary joints.

An object of the present invention is to provide an acceleration feedback device capable of extracting a target acceleration component.

[Means for solving problems]

The above purpose is to provide arithmetic means for extracting only a desired acceleration component from an acceleration signal obtained by an acceleration detector installed in each link, which also includes components based on motions other than the link in question, and an acceleration feedback loop. This is achieved by inserting the

[Effect]

This calculation means not only inputs the signals obtained from the acceleration detectors installed on each link, but also inputs and refers to other necessary motion state quantity signals, and calculates the robot mechanism and the arrangement of the acceleration detectors. Based on the kinematic equation determined accordingly, calculations to extract only the necessary acceleration components are processed at high speed, and the obtained required acceleration components are outputted to be fed back to the corresponding servo control system. Due to the operation of this calculation means, the signal fed back to the actuator that drives each link is only the acceleration component corresponding to the output of the actuator, and good control is realized.

〔Example〕

Embodiments of the present invention will be described in detail below using one drawing.

FIG. 1 is a block diagram showing the configuration of a control device including an acceleration feedback device based on the present invention. In FIG. 1, the robot main body 1o includes a rotating part motor 41, an upper arm part motor 4
2. Driven by the forearm motor 43, the rotational acceleration detector 21. Upper arm acceleration detector 22. Forearm acceleration detector 23
The detected acceleration signal is read from the analog input interface 113 via the amplifier 70 into the servo processor 110, which is comprised of a microprocessor, digital signal processor, or the like. Additionally, a swing tachogenerator 61 is attached to each motor. Upper arm tacho generator 62. The output of forearm tacho generator 63 is also read into servo processor 110 from analog input interface 113. Servo processor 110
calculates the target values of each servo system supplied from another processor via the pass line 120 and the feedback signals supplied from each of the detection elements, and outputs them to the analog output interface 111. Each motor is driven via a servo amplifier 90.

FIG. 2 is a sketch of a robot body equipped with an acceleration feedback device based on the present invention. In FIG. 2, the robot main body 1o has a rotating section 11. A rotational acceleration detector 21 and an upper arm acceleration detector 2 are provided in the upper arm 12 and the forearm 13, respectively.
2. A forearm acceleration detector 23 is provided. The detection directions of these acceleration detectors coincide with the tangential direction of the rotational motion of the respective corresponding links themselves.

In the above configuration, the servo processor 110 performs processing to calculate the angular acceleration of each joint. That is, the acceleration signal detected by each acceleration detector is converted into the angular acceleration of each joint by the following calculation process.

However, for the definition of each constant and each variable, please refer to Fig. 3, which is an explanatory diagram showing the mechanism of the robot.

Among the numbers, X1' s Y2' and Ys' are the turning acceleration detectors 21. Upper arm acceleration detector 22. It is detected by the forearm acceleration detector 23. θ1. θ2 is the rotating tachogenerator 61. Upper arm tacho generator 62
Detected by θ2. θ3 is detected by the upper arm encoder S2 and the encoder 53, respectively.

Next, equations (1) to (3) will be explained using FIG. Reference coordinate system 〇-Xo, Yo, Z.

The position vector X1 of the turning acceleration detector 21 in
It is determined by the following mapping f using the joint angle vector 8.

Therefore, the acceleration vector of the turning acceleration detector is as follows.

This acceleration vector is set to the coordinate system 0 of the turning acceleration detector 21.
When the coordinates are transformed into 1'-suel, yIJ, z, /, it becomes as follows.

Therefore, the detection direction of the turning acceleration detector 21 is 7.
The component in the 1' direction is as follows.

Z engineering'=-Ω1θl...(
7) Therefore, equation (1) for determining the turning angular acceleration is derived.

Formulas for determining the upper arm angular acceleration and forearm angular acceleration are derived in the same manner, and formulas (2), 9, and (3) are obtained. Note that in a configuration in which the forearm is driven via a parallel link as in this embodiment, θ3 is more necessary than 0δ. In this case,
Instead of equation (3), θδ is determined using Ωδ. In addition, when configuring an adaptive control system, rather than the angular acceleration corresponding to each joint.

Rather, there are cases where it is desired to obtain the acceleration component of the hand in the orthogonal coordinate system, but even in this case, it can be detected by this device by changing the calculation contents. Furthermore, it is also possible to insert a filter, an integrating circuit, etc. for adjusting frequency characteristics into the acceleration feedback loop.

FIG. 4 is a flowchart showing the procedure of acceleration feedback processing based on the present invention in a servo processor using a microcomputer, digital signal processor, etc. In the figure, first, the signals of the acceleration detectors installed in the three motion links are read into the servo processor from an analog input board equipped with an A/D converter (5
01).

Next, the angular velocity signals and angle signals of each joint, which are necessary to remove interference components included in these acceleration signals, are A
The signal is read into the servo processor from an analog input port equipped with a /D converter or a digital input port equipped with a reversible counter in response to pulse input from an encoder (502). These motion state variables are subjected to arithmetic processing based on the already explained kinematic equations to remove interference terms with respect to the angular acceleration signals, and only the angular acceleration components corresponding to the desired motion of each joint are extracted. (503).

2 These angular acceleration components are negatively fed back to the corresponding joint servo systems (504), achieving highly accurate and high fidelity angular acceleration feedback. Note that the above calculation processing is performed at a sampling frequency that is sufficiently higher than the same response wave number of each joint servo system.

Next, FIG. 5 is a sketch of a robot body equipped with an acceleration feedback device as another embodiment of the present invention. In this embodiment, the α-axis acceleration detector 31. β-axis acceleration detector 32.
The γ-phase acceleration detectors 33 are collectively provided near the wrist of the forearm. In this case as well, the configuration of the control device including the acceleration feedback device is the same as that shown in FIG. 1. However, in FIG. 1, the rotation speed detector 21. Upper arm acceleration detector 22. The turning acceleration detectors 23 are the α-axis acceleration detectors 31 . β-axis acceleration detector 32. It is replaced by the γ-axis speed detector 33. In order to detect the acceleration component in the reference coordinate system with this configuration, calculations based on the following equation are executed.

Note that by changing the calculation formula with the same configuration, it is also possible to detect the angular acceleration of each joint. Of course, it may also be used in combination with an element that provides frequency characteristics, such as a filter or an integral element.

In the above embodiment, the present invention is applied to the three degrees of freedom of the arm of a vertically articulated robot, but it may also be applied to a horizontally articulated robot, a cylindrical coordinate robot, a polar coordinate robot, etc. Robots of different degrees may be used.

〔Effect of the invention〕

As described in detail above, according to the present invention, there is provided an articulated two-cylindrical coordinate system having a mechanical section including a rotary joint.

Even in polar coordinate type robots, the interference of motion components between links can be removed and the angular acceleration components of each link drive system itself and the acceleration components along the reference orthogonal coordinate system can be detected with high accuracy. It is possible to configure an acceleration feedback compensation and adaptive control system, which has the effect of realizing high response performance that could not be obtained with conventional control systems.

[Brief explanation of drawings]

FIG. 1 is a block diagram of a robot control device to which the present invention is applied, and FIG. 2 is a perspective view of a robot main body to which the present invention is applied.
Fig. 3 is an explanatory diagram showing the operation of the robot mechanism to which the present invention is applied, Fig. 4 is a flowchart showing the operation of the device of the present invention, and Fig. 5 is a perspective view of the robot body to which the present invention is applied. It is a diagram. DESCRIPTION OF SYMBOLS 10... Robot body, 21... Turning acceleration detector, 22... Upper arm acceleration detector, 23... Forearm acceleration detector, 31... α-axis acceleration detector, 32... β-axis acceleration Detector, 33... γ-axis speed detector, 7o...
Amplifier, No. 2, Fig. 10 0 Ogut, Ne, I book/l lL times/l 12 Yoash 9P 13 Maekenho 21 Power foot ■”Power O” 1 no flea dealer ζ 22
” = edict me joi no r, v, ttZ3 @ anti-jr JL needle company

Claims (1)

[Scope of Claims] 1. In an acceleration feedback device for returning an acceleration signal to an articulating servo system of a multi-joint type, cylindrical coordinate type, polar coordinate type robot, etc., which has an arm mechanism including joints that perform rotational movements. , an acceleration detection means installed on a link that is a component of the arm mechanism and detects linear acceleration acting on the installation point, and means for detecting the amount of motion state of other links in addition to the link; The signals obtained from these detection means are input and calculated, and components resulting from the motion of other links are removed from the acceleration signal detected by the acceleration detection means, and the degree of freedom of movement unique to the link is calculated. An acceleration feedback device characterized by comprising a calculation means for separating and extracting a corresponding acceleration signal. 2. Control of the tip of the arm mechanism in an acceleration feedback device for returning acceleration signals to the articulatory servo system of a multi-joint type, cylindrical coordinate type, polar coordinate type robot, etc., which has an arm mechanism including joints that perform rotational movements. A plurality of acceleration detecting means are installed near the target point and detect the linear acceleration acting on the set point, a means for detecting the amount of motion state of each joint, and signals obtained from these detecting means are input. , calculation means for calculating and converting each component of the linear acceleration of the installation point detected by the acceleration detection means into a component in each coordinate axis direction of an inertial coordinate system serving as a reference. return device.