JPS6081615A - Motor control device - Google Patents

Abstract

PURPOSE:To operate a manipulator to be used for a unclear reactor efficiently by procesing positional information from a position detector by a digital processor to constitute a position/speed control loop. CONSTITUTION:Position feedback data from the position detector are inputted through an A/D converter 17, the digital data are multiplied by a constant 18 and the multiplied value is outputted to a speed feedback operator 21 and an position comparator 19. The operator 21 finds a difference between the input and a signal obtained by passing said input through a RAM28 to obtain speed feedpack data. Then, position command data from an external device is multiplied by a constant 16, the difference 19 between the multiplied data and the position feedback data is found out and the difference 19 is multiplied by a constant 20 to form speed reference data. The difference 23 between a value obtained by multiplying the speed feedback data by a constant 22 and the reference data is found out to obtain speed error data. The obtained signal is inputted to a torque reference operator 24 and the obtained torque reference signal controls the rotation of a motor 5 through a motor driver 25.

Description

[Detailed Description of the Invention] [Technical Field of the Invention] The present invention relates to a motor control device, and more particularly, the present invention relates to a motor control device, and more specifically, a switching element controls a motor so that the motor has a desired response based on given position command data. The present invention relates to a motor control device that supplies

[Technical background of the invention and its problems]

Conventionally, in order to control a motor attached to a controlled object such as a manipulator, a control device is configured so that the controlled object generates desired characteristics based on position information, speed information, and motor torque information of the controlled object. are doing.

The above control device inputs position information from a position detector such as a potentiometer attached to the controlled object, inputs speed information from a detector such as a pulse generator attached to the motor, and inputs motor torque information from a motor armature current. It is controlled by input from the detector.

However, when the target is a manipulator that operates inside a containment vessel that houses the reactor body of a nuclear power plant, semiconductor light emitting devices, etc. are used because the target is exposed to intense radiation. It is difficult to use a detector such as a pulse generator. In addition, in order to allow the manipulator to move freely on a trolley, it is necessary to minimize the number of signal couples for operating the manipulator.

Therefore, conventionally, motor control devices have been considered that control a motor only by a position loop using a potentiometer that is not affected by radiation.

That is, in FIG. 1, 1 is the manipulator main body, 2 is the joint of the manipulator main body l, 3 is the manipulator arm,
4 is attached to manipulator arm 3? 5 is a motor that drives the manipulator arm 3; 6 is a reducer that reduces the rotational speed of the motor 5 and transmits torque to the manipulator arm 3; 7 is a position detector that detects the position of the manipulator arm 3; 8 is a A reduction gear that transmits the position of the manipulator arm 3 to the position detector 7, 9 a control unit that controls the manipulator arm 3, and 10 a control unit that transmits the position of the manipulator arm 3 from the outside.
11 is a position feedback input device that inputs one output word of the position detector 7; 12 is a motor driver that drives the motor 5; 13 is a position command input device 10; A position comparator 14 generates a command to the motor driver 12 so that the output data and the output data of the position feedback input device 11 are equal to each other, and a coupler 14 connects the manipulator main body l and the control unit 9.

FIG. 1 shows a control device for the manipulator arm 3 that constitutes a position control loop, and is configured to perform control so that the output data of the position detector 7 coincides with the target position indicated by the position command data. In the case of a configuration using only such a position control loop, the speed at which the manipulator arm 3 reaches the target position cannot be controlled.

Therefore, it has been difficult to perform tasks that require accurate variation of the speed of the manipulator arm 3, such as tightening bolts or tracking a moving object held by the hand 4 with the TV right camera.

[Object of the Invention, 1] The present invention has been made to improve the above-mentioned prior art, and it obtains position information from a position detector by calculating speed information through processing using a digital processor. The purpose of the present invention is to provide a motor control device with good control characteristics by configuring a position/velocity control loop by attaching the motor to the manipulator arm 3.

[Summary of the invention]

In order to achieve the above object, the motor control device according to the present invention generates speed reference data of the manipulator based on the difference between position command data and position feedback data of the manipulator, and generates speed reference data of the manipulator based on the change in position feedback data every reference timing period. Feedback data is generated, speed error data is generated from the difference between the speed reference data and the speed feedback data, and the speed error data is processed by a digital processor so that the motor 5 has a desired response. The present invention is characterized in that torque reference data is calculated and the motor driver is driven by the torque reference data.

[Embodiments of the invention]

Hereinafter, the present invention will be explained based on an illustrated embodiment.

FIG. 2 shows the configuration of a motor control device according to an embodiment of the present invention. In FIG. 2, the same components as in FIG. 1 are given the same reference numerals.

In FIG. 2, 15 is a position command input device that receives position command data from an external device, 16 is a constant that matches the output data of the position command input device 15 with internal calculations, and 17 is an analog output of the position detector 7. An analog-to-digital converter performs digital conversion; 18 is a constant that matches the output data of the analog-to-digital converter 17 with internal calculation; 19 is a constant that matches the output data of the position detector 7 that has been matched by the constant 18; a position comparator that compares the output data of the position command input device 15 matched by a constant 16;
20 is a constant by which the output data of the position comparator 19 is used as speed reference data; 21 is a speed feedback amount calculation for calculating the amount of change for each reference timing period of the position feedback data matched by the constant 18; 22 is a constant by which the output data of the speed feedback amount calculator 21 is multiplied to obtain speed feedback data, 23 is a speed comparator for comparing the speed reference data and the speed feedback data, and 24 is the speed comparator 23 A torque reference calculator 25 receives the output data of the torque reference calculator 24 and controls the motor 5 so that the motor has desired characteristics.
It consists of a motor driver that drives the Next, the speed feedback amount calculator 21 and the torque reference calculator 24
The structure of is described in detail. FIG. 3 shows the configuration of the SfI speed return calculation unit 21, in which 28 stores data converted into a digital signal from the position detector 7 l clocks before the reference timing period. tNAM, 3
0 is a speed calculator which calculates the difference between the R, AM data and the data of the position detector 7 manually inputted at the current clock cycle of the reference timing cycle. FIG. 4 shows the configuration of the torque reference calculation 2 (24), in which speed error data, which is the output data of the speed comparator 23, is input, and torque reference data is output to the motor driver 25. In FIG. 4, 30 is a speed error adjuster capable of improving the transient characteristics of the speed error data, and 31 is a constant that determines the characteristics of the speed error adjuster 30. .

32 is a constant that stores the accumulation of speed errors (AM); 33 is a constant that determines the gain of the torque reference calculator 24;
is a torque reference error adjuster 35 that adjusts the transient characteristics of the torque reference torque data; 36 is a constant that determines the characteristics of the torque reference error adjuster 34; 36 is a constant that stores the cumulative torque reference error; and 37 is the It is composed of a constant that determines the ratio at which the data obtained by adding the output data of the speed error adjuster 30 and the torque reference error adjuster 34 contributes to the torque reference data.

(Operation of the embodiment) Next, the operation of the motor control device of the present invention configured as described above will be explained.Since the present invention involves a digital processor, how the digital processor and the controlled object are connected is explained using a flowchart. This section explains how signals are exchanged at appropriate timings and what kind of output is produced.

As shown in FIG. 5, the control steps include a first step AI, a second step A2, . Third step A3
, an external interrupt processing A4, and a monitor step A5 for controlling these in an integrated manner. Each step AI, A2
．． Step A3 is processed at a predetermined interrupt timing of the monitor program 80, and step A4 is processed at a predetermined external interrupt timing. Each of these interrupt processes is performed in the 1st, 2nd, and 2nd stages. Controlled by third timers A7, A8, A9 and external interrupt cause AIO. The first timer A7 uses the clock of the reference timing generator, and the second timer A7 uses the clock of the reference timing generator. Third timer A8. - A9 can be obtained by dividing the clock of the reference timing generator into a plurality of frequencies.

Next, the process of step A1 will be described. Step AI
As shown in the flowchart shown in FIG.
7, multiplies it by a constant 18, and outputs it to the velocity feedback amount calculator 21 and the position comparator 19. In addition, the velocity return value calculator 21 stores the data of the position detector 17 one clock before the timer A7 [LA,
Create speed feedback data by taking the difference from M2S. Next, step A2 multiplies the position command data from the external device by a constant 16f, as shown in the flowchart shown in FIG. Create speed reference data by doing this.

Also, constant 22 is added to the speed feedback data created in step AI.
The difference between the product multiplied by the speed reference data and the speed reference data is calculated as speed error data. In addition, in step A3, a torque reference is created by performing processing as shown in the flowchart shown in FIG. The processing in step A3 is performed by replacing the operation of the speed amplifier shown in FIG. 9 in an analog circuit with a digital circuit. In other words, the transfer function G in Figure 9
(sJ can be expressed as, and if this 0 expression is expressed as a function of real time, it will be as follows. First, by transforming the 0 expression, it becomes the 0 expression.

In formula 0, the digitized data corresponding to "' REF ' + Sgnn is DTREF, DSERR,
Considering that the digit of the differential operator S has the meaning of integral, (
By transforming the formula, it can be expressed as the following formula.

(1+T2S) TRF, F=R2・(t+T,S)
・5BRH8 (Rn to Tll TREE ILa 1Tt-)=t42・5EIIRT Rzp-' Tt T
RF, F-2TI 5ERR= -F (1% 5ER
R-TnEp) When converted into digital data, if the sampling period is Δt and the state at the time when n sampling periods have passed is considered λ-, then the result is as shown in equation Q).

(t), 1dt = 1 color. ([1・2　D8゜RR(pΔt)−D, □.

, (p△t)J fist△t+(R,D8.RR(n△t
) -D,,. , (n△t) J e△t ---■
Therefore, by transforming Equation 0, Equation 49 can be obtained by 1.

('1'2 +Δt)Drngr (nΔt) −(T
I +△t ) Rq ・Dsznn (t to n-) t-
i...■ Multiply and also calculate the sampling frequency up to the current one (pΔ
Sample the value of [) up to the current one (pΔt)
is subtracted from the value multiplied by the constant E-, and then the constant T
The formula shows that it can be calculated by multiplying the rn wage.

Therefore, FIG. 4 shows a digital circuit configured as the meaning of the equation (2) is shown in FIG. Approximately 1 using timer A9
Digital calculations are performed at 00 m5ec.

Next, in step A4, external interrupt processing is performed when there is a position command from an external device.

Position command data required by the motor control device of the present invention is received from an external device, such as a host computer.

Further, the interrupt caused by the reference timing generator is set to have a higher priority than the interrupt φ caused by the external interrupt cause.

〔Effect of the invention〕

As explained above, the present invention can also control the moving speed of the manipulator etc. by inputting only position information from the position detector attached to the manipulator etc. to the digital processor.

For this reason, it is necessary to control the moving speed of the manipulator, etc., and when a speed detector cannot be attached to the manipulator, etc. because it is in a bad environment such as inside a nuclear reactor, the motor control device of the present invention can be used to control the position of the manipulator, etc. By processing the position information from the detector with a digital processor, a position/velocity control loop can be constructed, which has the effect of enabling good operation of manipulators, etc.

[Brief explanation of drawings]

Fig. 1 is a block diagram showing a control device for a manipulator arm 3 using only a conventional position detector, Fig. 2 is a block diagram showing position/velocity control using only a position detector according to the present invention, and Fig. Figure 4 is a detailed configuration diagram of the part that calculates the speed feedback amount in Figure 2, Figure 5 is a conceptual diagram showing the usage transition of the digital processor, 6, 7, 8, and 10 are flowcharts showing processing at each step of the digital processor, and FIG. 9 is a circuit diagram when the speed amplifier is configured with an analog circuit. 3... Manipulator arm 5"°°° Motor 7... Position detector 15...
Position command input device 17...Analog-digital converter 19...Position comparator 21...Speed feedback amount calculator 2
3...Speed comparator 24...Torque base $ calculator 2
5...Motor driver (7317) Agent Patent attorney Noriyuki Chika (1 person) Figure 5 Figure 6 Figure 7 Figure 8

Claims (1)

[Claims] In order to control the moving position and moving speed of a controlled object such as a manipulator only by a position detector attached to the controlled object, a position detector that detects the position of the controlled object and a position command given from the outside are provided. a speed reference calculator that generates speed reference data by calculating the data and position feedback data that is the output of the position detector; and a speed reference calculator that generates speed feedback data by calculating the difference between the position feedback data at fixed time intervals. A speed feedback calculator generates torque reference data by calculating a difference between the speed reference data and the speed feedback data. a torque reference calculator, a motor driver that inputs the torque reference data and controls the motor that drives the controlled object so that an appropriate torque is generated, and inputs the position command data and the position feedback data; a digital processor that performs the above series of arithmetic processing;
A reference timing generator that defines the processing timing of the digital processor; and the digital processor stores a program that performs a series of data processing on the digital data group in order to give the motor desired response characteristics. The motor driver is controlled by the output data of the input/output board, including a program memory, a data memory for storing a group of digital data, and an input/output port for inputting/outputting data necessary for data processing by the program. A motor control device characterized in that the motor is controlled by: