JPH0731546B2 - Positioning device for rotating body - Google Patents

Description

Detailed Description of the Invention [Industrial applications]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a positioning device such as a rotary table driven by control of a sequence controller or the like, and more particularly to a device for improving a manual feed function of a device that uses a rotation angle as a feedback signal as an absolute rotation angle.

[Prior art]

Conventionally, when controlling the angle and speed of a rotary table with a control device such as a sequence controller, positioning is performed between the control device and the drive device of the rotary table so as to reduce the burden on the control device and facilitate control. The device is interposed. Some of such positioning devices use an absolute angle output from the angle detection device in a fixed cycle as a feedback signal of the angle of the rotary table. Such an absolute angle positioning device generates a control target angle that is interpolated so as to approach the target angle at regular time intervals based on the target angle and command speed commanded by a control device such as a sequence controller, and the angle It is a device that outputs the speed signal having a magnitude corresponding to the deviation of the detected angle to the drive device so that the angle of the rotary table sequentially follows the interpolated control target angle.

[Problems to be Solved by the Invention]

The angle detection device described above detects the rotation angle of the rotary table by an absolute angle, and therefore has a detectable range. For example, as shown in FIG. 6, it is assumed that the detectable range is set to one turn (2π) of the turntable. Then,
The output signal of the angle detecting device becomes a discontinuous periodic function with the maximum detected angle as a boundary, as shown in the figure. That is, the angle of the turntable detected by the angle detection device returns to the minimum detection angle when it exceeds the maximum detection angle. Therefore, the servo control cannot be performed at an angle exceeding the maximum detection angle.
For example, when servo control is performed with the target angle of positioning set to 3π, the detected angle becomes discontinuous at the boundary and the tracking deviation jumps at the boundary, although the interpolated control target value gradually converges to 3π. Servo control is impossible. By the way, normally, the above-described rotary table positioning device has a manual feed function of rotating the rotary table in a commanded direction while a manual feed signal is being input. In that case, the range of the manual feed must be limited to the detectable range, and conversely, in order to perform the manual feed beyond the detectable range, the feed should not be performed by the servo control. For this reason, in the former case, there is a problem that the positioning in the shortest distance cannot be performed because the positioning direction is restricted.
In the latter case, especially when performing low-speed feed, the rotary table cannot be rotated at a stable speed because servo control is not performed, and it is difficult to perform highly accurate manual positioning. It was The present invention has been made in order to improve the above-mentioned drawbacks, and an object thereof is to enable manual feed by servo control beyond the detectable range of the angle detection device by absolute angle. Is.

[Means for solving problems]

The structure of the invention adopted to solve the above problems is as follows, and the structure is shown in FIG. That is, the present invention generates a control target angle interpolated so as to sequentially approach the target angle for positioning (A5), inputs the angle signal S1 from the angle detection device C, and rotates the rotor corresponding to the angle signal. In the positioning device A which outputs the speed signal S4 to the drive device B (A6) according to the deviation between the current angle and the control target angle, the means which is activated when the manual feed command signal S5 is input. If the cycle of the angular range detected by the angle detecting device C is T at the start of manual feeding, the value of the angular range is 2T.
In the range obtained by adding and subtracting, the target angle setting unit A1 for setting the target angle of the positioning, and in the initial state, the angle detected by the angle detection device C is output as it is as the current angle, When the angle detected by the angle detection device C exceeds the maximum detection angle in the positive direction or exceeds the minimum detection angle in the negative direction, T is added to each of the detected angles thereafter, or The angle obtained by subtraction is output as the current angle, and then the angle conversion unit A2 that returns the output state of the current angle to the initial state according to the determination of the determination unit A3 described later.
And the absolute value of the difference between the present angle or the corresponding interpolated control target angle output by the angle conversion unit A2 and the positioning target angle is calculated, and it is determined that the value is T or less. And a control target angle correction unit A4 that shifts the interpolated control target angle in a direction away from the positioning target angle by T when the determination unit A3 determines T or less.
And are provided, and while the manual feed command signal S5 is being input, the angle conversion unit
The speed signal S4 is output based on the deviation between the current position output by A2 and the interpolated control target position.

[Action]

The operation of the present invention will be described with reference to FIG. The cycle of the angle detected by the angle detection device C (hereinafter referred to as “detection angle”) is represented by T. For convenience of description, only the case where the feed control is performed in the forward direction will be described. When the manual feed command signal S5 is input, the target angle setting unit A1 adds a 2T to the value of the angle range that can be detected by the angle detection device C, and the target angle M for positioning (hereinafter simply referred to as ""Targetangle") is set. After that, the interpolated control target angle (hereinafter simply referred to as "control target angle") is sequentially generated (S2), the deviation from the current angle of the rotating body (hereinafter referred to as "following deviation") is calculated, and the following deviation is calculated. Accordingly, the speed signal S4 is output. In this way, the rotating body is servo-controlled from the initial angle P ₀ toward the target angle P ₄ . At this time, until the boundary P _{2 of the} detection range, the output S3 of the angle conversion unit A2 is in the initial state, the detected angle of the angle detection device C is output as it is as the current angle of the rotating body, and the servo control is not obstructed. Carried out. Further, when the rotating member crosses a boundary angle P _2, the output S1 of the angle detection apparatus C, although to jump to transition to the lowest angle, current angle by the action of the angle conversion unit A2, then the detection angle Is obtained as a value obtained by adding the period T to. That is, the present angle is given as a corresponding angle in the range from T to 2T (on the extension line from P ₂ to P ₄ ). As a result, even if the detected angle changes discontinuously across the boundary, the current angle changes continuously. Therefore, normal servo control is performed even if the boundary angle P ₂ is exceeded. Terribly, further control is performed, is that the current angle reaches the angle P ₃ in the period T with respect to the target angle P ₄ of the positioning, it is determined by the determination unit A3, the angle conversion unit A2 is the output The state is changed to the initial state in which the detected angle is the current angle as it is. That is, the current angle changes from P ₃ to P ₅ . Also, by the action of the control target angle correction unit A4,
The control target angle at that time is subtracted by T, and similarly, the control target angle changes from P ₃ to P ₅ . After that, the corrected control target angle becomes the starting point, and interpolation calculation is performed on the target angle P ₆ (the angle is equal to P ₄ ) to sequentially generate the control target angle. This state is the same as when the manual feed command signal S5 was input (P ₀ and P ₅ do not necessarily have the same angle, but the corresponding points after the second cycle have the same angle).
Becomes Therefore, by repeatedly executing the one cycle process, the manual feed by the servo control is continued without any trouble. Regarding the rotation in the negative direction, the target angle is -T in FIG.
When the detected angle exceeds the minimum value in the negative direction in the range of --2T, the value obtained by subtracting T from the detected angle thereafter becomes the current angle. When the target angle is within T,
The current angle thereafter is converted into a detected angle. Further, the control target angle at that time is corrected to a value obtained by adding T, and the next control target value is generated based on the value.

【Example】

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. FIG. 2 is a diagram showing a configuration of an apparatus using a positioning device according to an embodiment of the present invention. In FIG. 2, A is
A positioning device, B is a servo unit, and C is an angle detection device. The positioning device A mainly comprises a microprocessor unit 1 (hereinafter abbreviated as “MPU”) for performing control calculation, a ROM 2 storing a control program therefor, a data input device 3 such as a keyboard, and a battery-backed RAM 4. The RAM 4 is formed with a positioning data table PDT in which position numbers and angle data for each of a plurality of predetermined positioning points and speed data for instructing a moving speed to each point are stored. In addition, each time a detected angle is input from the angle detection device, a detected angle register DAR that stores the value, a current angle register AAR that stores this value each time the current angle of the rotary table 10 is detected, and a target angle are stored. Target angle register OAR and the target target angle of the control target angle (hereinafter referred to as "theoretical remaining movement angle") that stores the theoretical remaining movement angle register IRR and the angle difference between the current angle of the rotary table and the target angle (hereinafter "actual The actual remaining movement angle register ARR for storing the "remaining movement angle") is formed. R
The OM2 stores a tracking control program for performing a process for manual feeding according to the present invention, calculating a tracking deviation at regular time intervals, and outputting a speed signal to the register 5. The servo unit B is mainly composed of a register 5, a DA converter 7 and a drive circuit 8. The speed signal S4 output from the MPU1 is input to the register 5, converted into an analog signal by the DA converter 7, and output to the drive circuit 8.
The drive circuit 8 inputs this signal and supplies power to the servo motor 9 to rotate it. A gear mechanism 11 for rotating the rotary table 10 is mechanically connected to the output shaft of the servomotor 9. Therefore, the absolute angle of the output shaft can be regarded as corresponding to the absolute angle of the rotary table 10. Further, a speed detector 12 is connected to the output shaft of the servomotor 9 to detect the moving speed of the rotary table 10 and send a speed feedback signal to the drive circuit 8. The angle detection device C mainly includes a first resolver 13 mechanically coupled to the output shaft of the servomotor 9 and a reduction mechanism.
A second resolver 15 coupled to the first resolver 13 via 14, a resolver excitation circuit 16, a first phase comparison circuit 17 for detecting the phase angle of the first resolver 13, and a second resolver. The second phase comparison circuit 18 for detecting the phase angle of 15, the absolute angle calculation circuit 19 for calculating the absolute angle of the rotary table 10 from the outputs of both, and the absolute angle detection timing for driving the circuit 19 at a constant cycle. And a real-time clock (hereinafter abbreviated as “RTC”) 20 for providing The input shaft of the first resolver 13 rotates once when the output shaft of the servomotor 9 rotates once, and the second resolver 15 rotates the rotary table.
The input shaft makes one revolution while the ten revolutions. Therefore, the cycle (detectable range) of the detection angle of the angle detection device C is 2π. The phase difference between the output voltage of the resolver and the excitation voltage changes according to the rotation angle of the input shaft. The first phase comparison circuit 17 counts the phase difference of the output voltage of the first resolver 13 with respect to the excitation voltage by a counter, converts it into a digital value, and outputs it to the absolute angle calculation circuit 19. Similarly, the second phase comparison circuit 18
The phase difference of the output voltage of the second resolver 15 with respect to the excitation voltage is converted into a digital value and output to the absolute angle calculation circuit 19. The absolute angle calculation circuit 19 is activated each time the detection timing signal D is input from the RTC 20 in a cycle of 2 ms, the phase data is input from both phase comparison circuits 17 and 18, the absolute angle of the rotary table 10 is calculated, and the interface circuit is calculated. Output to MPU1 via (IF). When the absolute angle calculation is completed, the absolute angle calculation circuit 19 sends an interrupt signal S8, which gives the output timing of the data, to the interrupt input terminal (NMI) of the MPU1.
Is output to. When the interrupt signal S8 is input, the MPU 1 starts execution of a program for predetermined follow-up control and outputs a speed signal, as described later. The interrupt signal S8 is delayed by a fixed time with respect to the detection timing signal D output from the RTC 20, and is output in a 2 ms cycle. Reference numeral 6 is a known stored program type sequence controller which sends a position number to be positioned according to a predetermined process to the MPU 1 through an interface circuit (IF) and outputs a manual feed command signal S5. An operation panel 21 having a dial for setting a manual feed speed, a positive manual feed button, and a negative manual feed button is connected to the sequence controller 6. Next, the operation of the apparatus of this embodiment will be described based on the flowcharts shown in FIGS. The program shown in FIG. 3 is a program that is started when the manual feed command signal S5 is input, and is a branch from the main program that is started by a timer interrupt every 20 ms. First, in the first execution cycle after the manual feed button is turned on, the initial setting of steps 102 to 114 is performed. The code Z is set according to the commanded manual feed direction (104 to 108). Next, the detected angle D stored in the detected angle register DAR is read,
4πZ is added to the value to obtain the target angle M, and the value is stored in the target angle register OAR (110 to 11).
2). That is, in FIG. 5, the initial angle P ₀ is the value stored in the detected angle register DAR when the manual feed command signal S5 is input. If 4π is added, the target angle becomes within the angle range of 2T to 3T.
P ₄ is set. Further, in the case of manual feed in the negative direction, if 4π is subtracted from the initial angle P ₀ , the target angle is set within the range of −T to −2T. In this embodiment, the period T of the detected angle is 2π, and as an example of setting the target angle within a predetermined range, a method of adding / subtracting 4π to / from the initial angle P ₀ is adopted. The steps 110 to 112 correspond to the target angle setting unit A1. Next, 4π is set in the theoretical remaining movement angle register IRR as an initial value of the theoretical remaining movement angle IR (114). Next, the theoretical remaining movement angle IR is sequentially decreased by the program of FIG. 4 described later as the manual feeding progresses, and it is determined whether it is smaller than 2π, but initially it is naturally larger than 2π, so step 122 , And the first execution cycle of this program ends. The program shown in FIG. 4 is a program executed by an interrupt signal from the angle detecting device C every 2 ms. First,
The detection angle D is input from the angle detection device C, and its value is compared with the previously input value and it is determined whether or not it exceeds the boundary of the detection range (200, 202). Initially, since the boundary has not been crossed, the routine proceeds to step 206, where the flag FLG is judged. This flag is for selecting the conversion means for obtaining the current angle R from the detected angle D, and the initial value is off. Since FLG is initially off, the detected angle D is set as the current angle R as it is, and the value is stored in the current angle register AAR (208, 212). Next, the follow-up control process from step 214 onward is executed. The current angle R and the target angle M obtained in the above steps are read out, and the actual remaining movement angle AR which is the absolute value of the difference is calculated (214 to 218), and corresponding to the control target angle at that time. The theoretical residual movement angle IR is calculated (222). This value is set so that the rotary table 10 gradually accelerates to reach a steady speed at the start of the control, and gradually decelerates from the steady speed at the stop. Next, a deviation ΔR between the actual remaining movement angle AR and the theoretical remaining movement angle IR is calculated, and a speed signal is output according to the deviation (224 to 228). When the speed signal has a negative value, the rotation is in the negative direction. In this way, the first execution cycle ends. The above execution cycle is executed every time an interrupt signal is input from the angle detection device, and the rotary table is set to the target angle M.
It is controlled to be positioned at. When the follow-up control progresses and reaches the detection range boundary P ₂ , the determination in step 202 is YES.
Then, FLG is set to ON (204), and in the subsequent execution cycle, the current angle R is set to a value obtained by adding 2πZ to the detected angle D (210). Then, the follow-up control is advanced using such a current position. When the control target angle approaches the target angle M within the period T, the determination in step 116 of FIG. 3 becomes YES, FLG is reset, and 2π is added to the theoretical residual movement angle IR (118, 120). Further, when FLG is reset, the subsequent current angle R, are detected angle D (208) i.e., the current angle and the control target angle together, so that remained from P ₃ to P _5. This state is equivalent to the initial state, so
Further, the follow-up control for the control target value can be continuously performed. After all, the above process is continuously executed while the manual feed button is in the ON state, and the rotary table 10 is rotated without end while being servo-controlled. When the manual feed button is turned off, the rotation of the rotary table 10 is stopped by the processing of steps 100 and 122. The angle conversion unit A2 uses the determination unit for the processing of steps 200 to 212.
A3 is the process of steps 116 to 118, the control target angle correction unit A4
Corresponds to the processing of step 120, respectively. Further, the control target angle generation unit A5 corresponds to the processing of steps 114 and 222, and the tracking deviation calculation unit A6 similarly corresponds to the processing of steps 224 to 228.

【The invention's effect】

As described above, the present invention sets a target angle in a predetermined range, generates a control target angle interpolated with respect to this target angle, and performs a predetermined conversion on the angle detected by the angle detection device. The current angle that changes continuously is obtained, follow-up control is executed from these values, and when the target angle approaches the angle of the predetermined relationship, both the current angle and the control target angle are The correction is made so as to move away from the detection range of the detection device. Therefore, it is possible to synchronize the jumping change of the control target angle, which is the target of the follow-up control, with the current angle of the rotating body, and to change both of them continuously, so that the rotating body can be detected by the absolute angle detection. Servo control manual feed beyond the range is possible.

[Brief description of drawings]

FIG. 1 is a block diagram showing the configuration of the present invention, and FIG. 2 is a block diagram showing the configuration of a rotating body control device using a positioning device according to a specific embodiment of the present invention. FIG. 4 is a flow chart showing the processing procedure of the MPU used in the apparatus of the present embodiment, FIG. 5 is an explanatory diagram showing the operation of the apparatus of the present invention and the apparatus of the embodiment, and FIG. It is an explanatory view for explaining a fault of a conventional device. 1 ... Microprocessor unit, 9 ... Servo motor, 10 ... Rotary table, 11 ... Gear mechanism, 13 ... First
Resolver, 15 …… second resolver

Front page continuation (72) Hideki Sato Inventor Hideki Sato 1-1 Asahi-cho, Kariya city, Aichi Toyota Koki Co., Ltd. (72) Inventor Masaharu Igawa 1 Toyota-cho, Toyota-shi, Aichi Toyota Motor Co., Ltd. (72 ) Inventor Katsutoshi Naruse 1 Toyota Town, Toyota City, Aichi Prefecture, Toyota Motor Co., Ltd. (72) Inventor Yasutoku Sugito, Toyota City, Toyota City, Aichi Prefecture, Toyota City, Toyota District (72) Inventor, Masashi Ito Aichi Prefecture 1 Toyota-cho, Toyota City Toyota Motor Corporation

Claims (1)

[Claims] 1. A control target angle interpolated so as to sequentially approach a target angle for positioning is generated from a commanded target angle for positioning and a commanded speed at regular time intervals, and a rotating body is moved in one direction. When the maximum detection angle is exceeded when rotated, the periodic angle signal that sequentially increases from the minimum detection angle is input, and the angle signal is input from the angle detection device that outputs at fixed time intervals, and the angle signal is supported. According to the deviation between the present angle of the rotating body and the interpolated control target angle, by outputting a speed signal to the drive device that drives the rotating body at the command speed, A positioning device for positioning a rotating body, which is a means which is activated when a manual feed command signal is input, wherein when the manual feed is started, when the cycle of the angle range output by the angle detection device is T, In the range obtained by adding or subtracting 2T to the value of the angle range of, the target angle setting unit that sets the target angle of the positioning, and in the initial state, the angle detected by the angle detection device is the current Output as an angle, when the angle detected by the angle detection device exceeds the maximum detection angle in the positive direction, or when it exceeds the minimum detection angle in the negative direction,
The angle obtained by adding or subtracting T to each of the detected angles is output as the current angle, and then the output state of the current angle is changed to the initial value according to the determination of the determination unit described later. An angle conversion unit for returning to a state, and an absolute value of a difference between the present angle output by the angle conversion unit or a corresponding control target angle and the target angle for positioning output, and the value is T or less. And a control target angle correction that shifts the interpolated control target angle in a direction away from the positioning target angle by T when the determination unit determines T or less. And a speed signal is output based on a deviation between the current position output by the angle conversion unit and the interpolated control target position while the manual feed command signal is input. You Positioning device of the rotating body.