JP6763286B2 - Motor control system - Google Patents

Description

The present invention relates to a motor control system.

In recent years, for example, in order to drive a link constituting a robot arm with high accuracy, a fully closed control type servomotor has been adopted for a joint portion of the robot arm.
Patent Document 1 discloses a motor control system using a fully closed control method that performs speed control based on a speed signal of a motor and position control based on a position signal of a load driven by the motor. ..

Japanese Unexamined Patent Publication No. 2004-80973

The inventors have found the following problems with respect to the motor control system.
A fully closed control type motor control system as disclosed in Patent Document 1 usually includes a current sensor that detects the current flowing through the motor, a motor shaft rotation angle sensor that detects the rotation angle of the motor shaft of the motor, and an output shaft. In addition to the torque sensor that detects the acting torque, it is equipped with an output shaft rotation angle sensor that detects the rotation angle of the output shaft fixed to the load driven by the motor.

Here, if an abnormality occurs in any of these various sensors, for example, motor control may not be able to continue, so it is necessary to detect the abnormality in the sensor. Therefore, for example, by setting each sensor as a dual system and comparing the values of both, it is possible to detect that an abnormality has occurred in the dual system, that is, one of the two sensors. Further, in order to identify the sensor in which the abnormality has occurred, for example, each sensor may be made into a triple system and a majority vote may be performed.

However, if the number of sensors is increased as in the case of a dual system or a triple system, there is a problem that the configuration of the motor control system becomes complicated and large, and the manufacturing cost increases.

The present invention has been made in view of such circumstances, and provides a motor control system capable of detecting abnormalities of various sensors without increasing the number of sensors.

The motor control system according to one aspect of the present invention is
With the motor
A current sensor that detects the current flowing through the motor and
A first rotation angle sensor that detects the rotation angle of the motor shaft of the motor, and
A second rotation angle sensor that detects the rotation angle of the output shaft fixed to the load driven by the motor, and
A torque sensor that detects the torque acting on the output shaft and
A motor control unit that controls the motor based on at least the current detected by the current sensor is provided.
The motor control unit
Based on the first rotation angle detected by the first rotation angle sensor and the second rotation angle detected by the second rotation angle sensor, the first estimated value of the torque acting on the output shaft is calculated.
Based on the current detected by the current sensor, the second estimated value of the torque acting on the output shaft is calculated.
By comparing the torque value detected by the torque sensor with the first estimated value and the second estimated value, the current sensor, the torque sensor, the first rotation angle sensor, and the second rotation An abnormality is detected for each of the angle sensors.

In the motor control system according to one aspect of the present invention, the output shaft is acted on based on the first rotation angle detected by the first rotation angle sensor and the second rotation angle detected by the second rotation angle sensor. The first estimated value of the torque to be generated is calculated, the second estimated value of the torque acting on the output shaft is calculated based on the current detected by the current sensor, and the torque value detected by the torque sensor and the torque value detected by the torque sensor are used. By comparing the first estimated value with the second estimated value, an abnormality is detected in each of the current sensor, the torque sensor, the first rotation angle sensor, and the second rotation angle sensor. That is, it is possible to detect abnormalities of various sensors without increasing the number of sensors.

INDUSTRIAL APPLICABILITY According to the present invention, it is possible to provide a motor control system capable of detecting abnormalities of various sensors without increasing the number of sensors.

It is a block diagram which shows the motor control system which concerns on 1st Embodiment. It is a flowchart which shows the abnormality detection method of each sensor by a motor control part MC.

Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. However, the present invention is not limited to the following embodiments. Moreover, in order to clarify the description, the following description and drawings are simplified as appropriate.

(First Embodiment)
First, the motor control system according to the first embodiment will be described with reference to FIG.
FIG. 1 is a block diagram showing a motor control system according to the first embodiment. As shown in FIG. 1, the motor control system 1 according to the present embodiment includes a motor MT, a speed reducer RD, a motor shaft rotation angle sensor AS1, an output shaft rotation angle sensor AS2, a torque sensor TS, and a motor control unit MC. There is. The link LN, which is a load, is driven by the motor MT via the speed reducer RD.
The motor control system 1 according to the present embodiment is, for example, a servomotor control system provided at a joint portion of the robot arm for driving a link LN constituting the robot arm.

The motor MT is, for example, an AC (AC) servo motor. The motor MT is driven based on the control signal ctr output from the motor control unit MC. The control signal ctr is, for example, a PWM (Pulse Width Modulation) signal.
The motor MT is internally provided with a current sensor CS that detects three-phase alternating currents (hereinafter referred to as motor currents) I _u , I _v , and I _w flowing through the motor MT. The motor currents I _u , I _v , and I _w detected by the current sensor CS are fed back to the motor control unit MC.

The speed reducer RD is, for example, a wave gear device, and is provided between the motor shaft of the motor MT and the output shaft fixed to the link LN. The speed reducer RD reduces the rotation speed of the motor shaft of the motor MT to 1 / n (n: reduction ratio) and transmits it to the output shaft fixed to the link LN.

The torque sensor TS detects the torque acting on the output shaft fixed to the link LN, that is, the output shaft torque τ _l [Nm]. The output shaft torque τ _l detected by the torque sensor TS is input to the motor control unit MC.
As shown in FIG. 1, the torsional rigidity _kg from the motor shaft to the output shaft is determined based on the rigidity of the speed reducer RD and the rigidity of the torque sensor TS.

The motor shaft rotation angle sensor (first rotation angle sensor) AS1 is an encoder that detects the rotation angle of the motor shaft of the motor MT, that is, the motor shaft rotation angle (first rotation angle) θ _m [rad]. The motor shaft rotation angle θ _m detected by the motor shaft rotation angle sensor AS1 is input to the motor control unit MC.

The output shaft rotation angle sensor (second rotation angle sensor) AS2 detects the rotation angle of the output shaft fixed to the link LN driven by the motor MT, that is, the output shaft rotation angle (second rotation angle) θ _l [rad]. It is an encoder to rotate. The output shaft rotation angle θ _l detected by the output shaft rotation angle sensor AS2 is input to the motor control unit MC.

The motor control unit MC has motor currents I _u , I _v , I _w detected by the current sensor CS, output shaft torque τ _l detected by the torque sensor TS, and motor shaft rotation detected by the motor shaft rotation angle sensor AS1. The motor MT is controlled based on four signals of an angle θ _m and an output shaft rotation angle θ _l detected by the output shaft rotation angle sensor AS2.

Although not shown in FIG. 1, the motor control unit MC includes, for example, a calculation unit such as a CPU (Central Processing Unit), a RAM (Random Access Memory) and a ROM (Read Only Memory) in which various control programs and data are stored. It is equipped with a storage unit such as.

Further, the motor control unit MC can detect an abnormality of each sensor based on the signal detected by each sensor.
Specifically, when the motor control unit MC detects an abnormality in the current sensor CS, it determines that it is difficult to continue the motor control, and stops the motor MT.
Further, when the motor control unit MC detects an abnormality in any one of the three sensors, the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2, the motor control can be continued. Judge and continue motor control.
On the other hand, when the motor control unit MC detects an abnormality in any two of the three sensors, the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2, it is difficult to continue the motor control. Judgment is made and the motor MT is stopped.

The details of the abnormality detection control of each sensor by the motor control unit MC will be described below.
First, the first estimated value τ ^ _{l_est1} of the output shaft torque τ _l is calculated using the following equation (1). In this way, the difference between the rotation angle θ _m [rad] of the motor shaft connected before and after the speed reducer RD and the rotation angle θ _l [rad] of the output shaft, and the torsional rigidity _kg from the motor shaft to the output shaft Using [Nm / rad], the first estimated value τ ^ _{l_est1} [Nm] of the output shaft torque τ _l can be calculated. Here, as described above, n is the reduction ratio of the speed reducer RD.

Next, the second estimated value τ ^ _{l_est2} of the output shaft torque τ _l is calculated using the following equation (2). In this way, the second estimated value τ ^ _{l_est2} [Nm] of the output shaft torque τ _l can be calculated by multiplying the motor shaft torque τ _m [Nm] by the reduction ratio n and the reducer efficiency φ. Motor shaft torque τ _m [Nm] is the product of the motor MT of the torque constant _k t [Nm / Arms] and q-axis current value _{I q (θ m) [Arms} ]. The q-axis current value I _q (θ _m ) is determined by the rotation angle θ _m [rad] of the motor shaft and the values of the motor currents I _u , I _v , and I _w . Here, although not essential, as shown in the equation (2), subtracting the estimated value τ ^ _f [Nm] of the friction torque τ _f of the speed reducer RD from this value further improves the estimation accuracy. Estimate tau ^ _f of the frictional torque tau _f, the motor model (inertia term, viscosity term), the rotation angle of the motor shaft is estimated from the torque of the output shaft.

Next, the first estimated value τ ^ _{l_est1} of the output shaft torque τ _l calculated using the equation (1) and the second estimated value τ ^ _{l_est2} of the output shaft torque τ _l calculated using the equation (2). Is compared with the output shaft torque τ _l detected by the torque sensor TS to detect an abnormality in each sensor.

First, as shown in the following equation (3), whether or not the absolute value of the difference between the output shaft torque τ _l and the first estimated value τ ^ _{l_est1} calculated by the equation (1) is within a predetermined threshold value Δτ ₁ . judge.

If equation (3) holds, that is, is true, the torque sensor TS that detects the output shaft torque τ _l , the motor shaft rotation angle θ _m shown in equation (1), and the motor shaft that detects the output shaft rotation angle θ _l , respectively. It is determined that the rotation angle sensor AS1 and the output shaft rotation angle sensor AS2 are normal. On the other hand, if the equation (3) is not satisfied, that is, false, it is determined that at least one of the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2 is abnormal.

Next, as shown in the following equation (4), whether or not the absolute value of the difference between the output shaft torque τ _l and the second estimated value τ ^ _{l_est2} calculated by the equation (2) is within the predetermined threshold value Δτ _2. Is determined.

If equation (4) is true, the torque sensor TS that detects the output shaft torque τ _l, and the motor shaft rotation angle θ _m for calculating the q-axis current value I _q (θ _m ) shown in equation (2). It is determined that the motor shaft rotation angle sensor AS1 and the current sensor CS that detect the motor currents I _u , I _v , and I _w , respectively, are normal. On the other hand, if the equation (4) is false, it is determined that any of the torque sensor TS, the motor shaft rotation angle sensor AS1, and the current sensor CS is abnormal.

Therefore, if the equation (3) is true and the equation (4) is false, it can be determined that the current sensor CS is abnormal. In this case, it is determined that it is difficult to continue the motor control, and the motor MT is stopped. Further, if the equation (3) is false and the equation (4) is true, it can be determined that the output shaft rotation angle sensor AS2 is abnormal. In this case, the motor control is continued as it is without using the output shaft rotation angle sensor AS2. Further, if both the equation (3) and the equation (4) are false, it can be determined that the torque sensor TS or the motor shaft rotation angle sensor AS1 is abnormal.

Next, as shown in the following equation (5), the absolute value of the difference between the first estimated value τ ^ _{l_est1} calculated by the equation (1) and the second estimated value τ ^ _{l_est2} calculated by the equation (2) is predetermined. It is determined whether or not it is within the threshold value Δτ _{3 of} .

If the formula (5) is true, the motor shaft rotation angle sensor AS1 and the output shaft rotation angle sensor AS2 that detect the motor shaft rotation angle θ _m and the output shaft rotation angle θ _l shown in the formula (1) are normal. Judge that there is. At the same time, a motor shaft rotation angle sensor for detecting the motor shaft rotation angle θ _m and the motor currents I _u , I _v , and I _w for calculating the q-axis current value I _q (θ _m ) shown in the equation (2), respectively. It is determined that the AS1 and the current sensor CS are normal. That is, it is determined that the motor shaft rotation angle sensor AS1, the output shaft rotation angle sensor AS2, and the current sensor CS are normal. On the other hand, if the equation (5) is false, it is determined that any of the motor shaft rotation angle sensor AS1, the output shaft rotation angle sensor AS2, and the current sensor CS is abnormal.

Therefore, if the equations (3) and (4) are false and the equation (5) is true, it can be determined that the torque sensor TS is abnormal. In this case, the motor control is continued by using the first estimated value τ ^ _{l_est1} of the output shaft torque τ _l shown in the equation (1) without using the torque sensor TS. On the other hand, if all of the equations (3) to (5) are false, it can be determined that the motor shaft rotation angle sensor AS1 is abnormal.

When an abnormality in the motor shaft rotation angle sensor AS1 is detected, the output shaft torque τ _l [Nm] detected by the torque sensor TS is obtained by transforming the equation (1) as shown in the following equation (6). And the output shaft rotation angle θ _l [rad] detected by the output shaft rotation angle sensor AS2 is used to calculate the estimated value θ ^ _m [rad] of the motor shaft rotation angle θ _m . Even when an abnormality of the motor shaft rotation angle sensor AS1 is detected, the motor control can be continued by using the estimated value θ ^ _m of the motor shaft rotation angle θ _m .

As described above, in the motor control system according to the first embodiment, the output shaft torque τ _l detected by the torque sensor TS is compared with the first estimated value τ ^ _{l_est1} and the second estimated value τ ^ _{l_est2.} As a result, abnormalities are detected in each of the current sensor CS, the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2. That is, it is possible to detect abnormalities of various sensors without increasing the number of sensors.

Next, the abnormality detection control of each sensor by the motor control unit MC described above will be described with reference to the flowchart of FIG. FIG. 2 is a flowchart showing abnormality detection control of each sensor by the motor control unit MC.
First, as shown in FIG. 2, the motor control unit MC has a motor shaft rotation angle θ _m detected by the motor shaft rotation angle sensor AS1, an output shaft rotation angle θ _l detected by the output shaft rotation angle sensor AS2, and a torque. The output shaft torque τ _l detected by the sensor TS and the motor currents I _u , I _v , and I _w detected by the current sensor CS are acquired (step ST1).
Next, the first estimated value τ ^ _{l_est1} and the second estimated value τ ^ _{l_est2} of the output shaft torque τ _l are calculated using the equations (1) and (2) (step ST2).

Next, it is determined whether or not the equation (3) is true (step ST3). When the equation (3) is true (step ST3YES), the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2 are normal. Subsequently, it is determined whether or not the equation (4) is true (step ST4). When the equation (4) is true (step ST4YES), since the current sensor CS is also normal, all the sensors are normal, and the abnormality detection control is terminated without detecting an abnormality. On the other hand, when the equation (4) is false (step ST4NO), it is determined that it is difficult to continue the motor control because the current sensor CS is abnormal, and the motor MT is stopped (step ST5).

When the equation (3) is false (step ST3NO), any one of the torque sensor TS, the motor shaft rotation angle sensor AS1, and the output shaft rotation angle sensor AS2 is abnormal. Subsequently, it is determined whether or not the equation (4) is true (step ST6). When the equation (4) is true (step ST6YES), the torque sensor TS, the motor shaft rotation angle sensor AS1, and the current sensor CS are normal. That is, since the equation (3) is false and the equation (4) is true, it is determined that the output shaft rotation angle sensor AS2 is abnormal. In this case, the motor control is continued as it is without using the output shaft rotation angle sensor AS2.

On the other hand, when the equation (4) is false (step ST6NO), that is, when both the equations (3) and (4) are false, the torque sensor TS or the motor shaft rotation angle sensor AS1 is abnormal. Subsequently, it is determined whether or not the equation (5) is true (step ST7). When the equation (5) is true (step ST7YES), the motor shaft rotation angle sensor AS1, the output shaft rotation angle sensor AS2, and the current sensor CS are normal. Therefore, it is determined that the torque sensor TS of the torque sensor TS or the motor shaft rotation angle sensor AS1 is abnormal. In this case, the motor control is continued using the first estimated value τ ^ _{l_est1} of the output shaft torque τ _l shown in the equation (1) without using the torque sensor TS determined to be abnormal (step ST8). ..

On the other hand, when the equation (5) is false (step ST7NO), it is determined that the motor shaft rotation angle sensor AS1 of the torque sensor TS or the motor shaft rotation angle sensor AS1 is abnormal. In this case, the motor control is continued using the estimated value θ ^ _m of the motor shaft rotation angle θ _m shown in the equation (6) without using the motor shaft rotation angle sensor AS1 determined to be abnormal (step). ST9).

The present invention is not limited to the above embodiment, and can be appropriately modified without departing from the spirit.

1 Motor control system AS1 Motor shaft rotation angle sensor AS2 Output shaft rotation angle sensor CS Current sensor LN link MC Motor control unit MT Motor RD Reducer TS Torque sensor

Claims (1)

With the motor
A current sensor that detects the current flowing through the motor and
A first rotation angle sensor that detects the rotation angle of the motor shaft of the motor, and
A second rotation angle sensor that detects the rotation angle of the output shaft fixed to the load driven by the motor, and
A torque sensor that detects the torque acting on the output shaft and
A motor control unit that controls the motor based on at least the current detected by the current sensor is provided.
The motor control unit
Based on the first rotation angle detected by the first rotation angle sensor and the second rotation angle detected by the second rotation angle sensor, the first estimated value of the torque acting on the output shaft is calculated.
Based on the current detected by the current sensor, the second estimated value of the torque acting on the output shaft is calculated.
By comparing the torque value detected by the torque sensor with the first estimated value and the second estimated value, the current sensor, the torque sensor, the first rotation angle sensor, and the second rotation Detects abnormalities for each of the angle sensors,
Motor control system.

Images