MOTOR CONTROL SYSTEM

Abstract

A motor control system includes a motor, a rotational angle sensor that detects a rotational angle of a motor shaft of the motor, a torque sensor that detects shaft torsion torque between the motor shaft, and an output shaft fixed to a load member driven by the motor, and a motor controller that controls the motor. The motor controller estimates a rotational angle of the output shaft, based on the rotational angle of the motor shaft, the shaft torsion torque, and torsional rigidity obtained in advance with respect to a region between the motor shaft and the output shaft, and controls the motor, using the estimated rotational angle of the output shaft.

Claims

1. A motor control system comprising: a motor; a rotational angle sensor that detects a rotational angle of a motor shaft of the motor; a torque sensor that detects shaft torsion torque between the motor shaft, and an output shaft fixed to a load member driven by the motor; and a motor controller that controls the motor, wherein the motor controller estimates a rotational angle of the output shaft, based on the rotational angle of the motor shaft detected by the rotational angle sensor, the shaft torsion torque detected by the torque sensor, and a torsional rigidity obtained in advance with respect to a region between the motor shaft and the output shaft, and the motor controller controls the motor, using the rotational angle of the output shaft.

2. The motor control system according to claim 1, wherein a value of the torsional rigidity is changed, according to the shaft torsion torque detected by the torque sensor.

3. The motor control system according to claim 1, wherein at least one of a rotational angular velocity and a rotational angular acceleration of the output shaft is estimated, based on the rotational angle of the motor shaft detected by the rotational angle sensor, the shaft torsion torque detected by the torque sensor, and the torsion rigidity obtained in advance with respect to the region between the motor shaft and the output shaft, and the rotational angle of the output shaft is estimated, based on at least one of the rotational angular velocity and the rotational angular acceleration of the output shaft.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0012] Features, advantages, and technical and industrial significance of exemplary embodiments will be described below with reference to the accompanying drawings, in which like numerals denote like elements, and wherein:

[0013] FIG. 1 is a block diagram showing a motor control system according to a first embodiment;

[0014] FIG. 2 is a graph showing change of torsional angle relative to shaft torsion torque; and

[0015] FIG. 3 is a block diagram showing relationships among parameters (motor shaft rotational angle .sub.M, motor shaft torque .sub.M, output shaft rotational angle .sub.L, output shaft torque .sub.L) in the motor control system shown in FIG. 1.

DETAILED DESCRIPTION OF EMBODIMENTS

[0016] Some embodiments of the disclosure will be described in detail with reference to the drawings. It is, however, to be understood that this disclosure is not limited to the following embodiments. Also, the following description and the drawings are simplified as needed, for the sake of clarity of explanation.

[0017] <Configuration of Motor Control System>

[0018] Initially, referring to FIG. 1, a motor control system according to a first embodiment will be described. FIG. 1 shows the motor control system according to the first embodiment. As shown in FIG. 1, the motor control system 1 according to this embodiment includes a motor MT, a speed reducer RD, a motor shaft rotational angle sensor AS, a torque sensor TS, and a motor controller MC. A link LN as a load member is driven by the motor MT, via the speed reducer RD. The motor control system 1 according to this embodiment is a control system of a servomotor provided in a joint of a robot arm, for driving the link LN that constitutes the robot arm, for example.

[0019] The motor MT is an AC servomotor, for example. The motor MT drives the link LN based on a control signal ctr transmitted from the motor controller MC. The control signal ctr is a Pulse Width Modulation (PWM) signal, for example. The motor MT incorporates a current sensor CS that detects three-phase alternating current (which will be called motor current) I.sub.u, I.sub.v, I.sub.w, that flows through the motor MT. The motor current I.sub.u, I.sub.v, I.sub.w detected by the current sensor CS is fed back to the motor controller MC. In FIG. 1, torque applied to a motor shaft, or motor shaft torque .sub.M, is also indicated.

[0020] The speed reducer RD is a strain wave gearing, for example, and is provided between the motor shaft of the motor MT and an output shaft fixed to the link LN. The speed reducer RD reduces the rotational speed of the motor shaft of the motor MT to 1/r (r: reduction ratio), and transmits the resulting rotation to the output shaft fixed to the link LN.

[0021] The torque sensor TS detects shaft torsion torque .sub.msr produced between the motor shaft of the motor MT and the output shaft fixed to the link LN. The shaft torsion torque .sub.msr is a product SK.sub.strain of the amount of strain S measured with a strain gauge of the torque sensor TS, and a coefficient of rigidity K.sub.strain of a strain measuring unit. The shaft torsion torque .sub.msr detected by the torque sensor TS is transmitted to the motor controller MC. In FIG. 1, torque applied to the output shaft, or output shaft torque .sub.L, is also indicated.

[0022] The motor shaft rotational angle sensor AS is an encoder that detects the rotational angle of the motor shaft of the motor MT, or motor shaft rotational angle .sub.M. The motor shaft rotational angle .sub.M detected by the motor shaft rotational angle sensor AS is transmitted to the motor controller MC.

[0023] The motor controller MC controls the motor MT, based on three signals, more specifically, the motor current I.sub.u, I.sub.v, I.sub.w, detected by the current sensor CS, shaft torsion torque .sub.msr detected by the torque sensor TS, and the motor shaft rotational angle .sub.M detected by the motor shaft rotational angle sensor AS.

[0024] Here, the motor controller MC estimates the rotational angle of the output shaft fixed to the link LN, or output shaft rotational angle .sub.L, based on the shaft torsion torque .sub.msr and the motor shaft rotational angle .sub.M, and controls the motor MT. A method of estimating the output shaft rotational angle .sub.L by the motor controller MC will be described in detail later.

[0025] Although not illustrated in FIG. 1, the motor controller MC includes a computing unit, such as CPU (Central Processing Unit), and a storage unit, such as RAM (Random Access Memory) and ROM (Read Only Memory), in which various control programs, data, etc. are stored.

[0026] <Method of Estimating Output Shaft Rotational Angle .sub.L>

[0027] In the following, the method of estimating the output shaft rotational angle .sub.L by the motor controller MC will be described. The angle of torsion from the motor shaft to the output shaft can be expressed as .sub.Mr.sub.L, using the motor shaft rotational angle .sub.M, output shaft rotational angle .sub.L, and the reduction ratio r. A relationship as indicated below in Eq. (1) is established among the torsional angle .sub.Mr.sub.L, torsional rigidity K of a region from the motor shaft to the output shaft, and the shaft torsion torque .sub.msr detected by the torque sensor TS.

[00001]$\begin{array}{cc}{}_M-r.Math.{}_L=\frac{{}_{\mathrm{msr}}}{K}& \mathrm{Eq}..Math.\left(1\right)\end{array}$

[0028] Accordingly, the output shaft rotational angle .sub.L can be estimated according to the following equation (2) derived from Eq. (1) above.

[00002]$\begin{array}{cc}{}_L=\frac{{}_M}{r}-\frac{{}_{\mathrm{msr}}}{r.Math.K}& \mathrm{Eq}..Math.\left(2\right)\end{array}$

[0029] Thus, the motor control system according to the first embodiment estimates the output shaft rotational angle .sub.L, using the torsional rigidity K of the region from the motor shaft to the output shaft, and the shaft torsion torque .sub.msr detected by the torque sensor TS, in addition to the rotational angle .sub.M of the motor shaft and the reduction ratio r. Therefore, position control can be performed with higher accuracy, as compared with the case where .sub.M/r obtained by dividing the rotational angle .sub.M of the motor shaft by the reduction ratio r is simply used as the rotational angle .sub.L of the output shaft. Namely, the motor control system according to the first embodiment can perform position control with high accuracy, without using any output shaft rotational angle sensor.

[0030] As shown in FIG. 1, the torsional rigidity K used for estimation of the output shaft rotational angle .sub.L is determined based on the rigidity possessed by the speed reducer RD and the rigidity possessed by the torque sensor TS. A value of torsional rigidity K obtained in advance is stored in the storage unit within the motor controller MC. While the value of the torsional rigidity K may be set as a constant value, it actually changes according to a value of the shaft torsion torque. Therefore, the value of the torsional rigidity K used for estimation of the output shaft rotational angle .sub.L may be changed, according to the value of the shaft torsion torque .sub.msr detected by the torque sensor TS, so that the output shaft rotational angle .sub.L is estimated with higher accuracy.

[0031] <Method of Determining Torsional Rigidity K>

[0032] Referring next to FIG. 2, one example of the method of determining a value of the torsional rigidity K will be described. FIG. 2 is a graph showing change of the torsional angle relative to the shaft torsion torque. As indicated by an actual curve in FIG. 2, the torsional angle changes such that the amount of increase of the torsional angle relative to the amount of increase of the shaft torsion torque is gradually reduced as the shaft torsion torque becomes larger. The value of the torsional rigidity K at each value of torsion torque is an inverse of a slope of a tangent of the actual curve, and gradually changes. Therefore, if the actual curve is to be reproduced, the data amount of values of the torsional rigidity K becomes enormously large.

[0033] Thus, as indicated by approximate broken lines in FIG. 2, change of the torsional angle relative to the shaft torsion torque is approximated by broken lines. For example, as shown in FIG. 2, the torsional rigidity K is equal to K.sub.1 in a section where the value of the shaft torsion torque is between 0 and T.sub.1, and the torsional rigidity K is equal to K.sub.2 in a section where the value of the shaft torsion torque is between T.sub.1 and T.sub.2, while the torsional rigidity K is equal to K.sub.3 in a section where the value of the shaft torsion torque is between T.sub.2 and T.sub.3.

[0034] In this case, when the value of the shaft torsion torque .sub.msr detected by the torque sensor TS is between 0 and T.sub.1, the torsional angle .sub.Mr.sub.L can be expressed by the following equation (3), using the torsional rigidity K=K.sub.1.

[00003]$\begin{array}{cc}{}_M-r.Math.{}_L=\frac{{}_{\mathrm{msr}.Math..Math.1.Math.}}{K_1}& \mathrm{Eq}..Math.\left(3\right)\end{array}$

[0035] Accordingly, the output shaft rotational angle .sub.L can be estimated, according to the following equation (4) derived from Eq. (3) above.

[00004]$\begin{array}{cc}{}_L=\frac{{}_M}{r}-\frac{{}_{\mathrm{msr}.Math..Math.1}}{r.Math.K_1}& \mathrm{Eq}..Math.\left(4\right)\end{array}$

[0036] Also, when the value of the shaft torsion torque .sub.msr detected by the torque sensor TS is between T.sub.1 and T.sub.2, the torsional angle .sub.Mr.sub.L can be expressed by the following equation (5), using the torsional rigidity K=K.sub.2, and the torsional angle .sub.1 at the shaft torsion torque T.sub.1.

[00005]$\begin{array}{cc}{}_M-r.Math.{}_L={}_1+\frac{{}_{\mathrm{msr}}-T_1}{K_2}& \mathrm{Eq}..Math.\left(5\right)\end{array}$

[0037] Accordingly, the output shaft rotational angle .sub.L can be estimated, according to the following equation (6) derived from Eq. (5) above.

[00006]$\begin{array}{cc}{}_L=\frac{{}_M}{r}-\frac{{}_1}{r}-\frac{{}_{\mathrm{msr}}-T_1}{r.Math.K_2}& \mathrm{Eq}..Math.\left(6\right)\end{array}$

[0038] Similarly, when the value of the shaft torsion torque .sub.msr detected by the torque sensor TS is between T.sub.2 and T.sub.3, the torsional angle .sub.Mr.sub.L can be expressed by the following equation (7), using the torsional rigidity K=K.sub.3, and the torsional angle .sub.2 at the shaft torsion torque T.sub.2.

[00007]$\begin{array}{cc}{}_M-r.Math.{}_L={}_2+\frac{{}_{\mathrm{msr}}-T_2}{K_3}& \mathrm{Eq}..Math.\left(7\right)\end{array}$

[0039] Accordingly, the output shaft rotational angle .sub.L can be estimated according to the following equation (8) derived from Eq. (7) above.

[00008]$\begin{array}{cc}{}_L=\frac{{}_M}{r}-\frac{{}_2}{r}-\frac{{}_{\mathrm{msr}}-T_2}{r.Math.K_3}& \mathrm{Eq}..Math.\left(8\right)\end{array}$

[0040] As described above, values of the shaft torsion torque are divided into two or more sections, and the torsional rigidity K in each section is set to a constant value, so that the amount of data of the torsional rigidity K can be reduced, and the output shaft rotational angle .sub.L can be estimated with high accuracy.

[0041] Next, referring to FIG. 3, a motor control system according to a second embodiment will be described. FIG. 3 shows relationships among parameters (motor shaft rotational angle .sub.M, motor shaft torque .sub.M, output shaft rotational angle .sub.L, output shaft torque .sub.L) in the motor control system shown in FIG. 1. In FIG. 3, S means differential, and 1/S means integral.

[0042] As shown in FIG. 3, K(.sub.Mr.sub.L) as the product of the torsional angle .sub.Mr.sub.L and the torsional rigidity K between the motor MT and the link LN is detected by the torque sensor TS as the shaft torsion torque .sub.msr.

[0043] Referring to FIG. 3, a motion equation of rotation of the motor MT can be represented by the following equation (9), using the moment of inertia J.sub.M of the motor MT, and the coefficient of viscous friction B.sub.M.

J.sub.M.Math.{umlaut over ()}.sub.M+B.sub.M.Math.{dot over ()}.sub.M=.sub.M.sub.msr=.sub.MK.Math.(.sub.Mr.Math..sub.L)Eq. (9)

[0044] Referring to FIG. 3, a motion equation of rotation of the link LN can be represented by the following equation (10), using the moment of inertia J.sub.L of the link LN, and the coefficient of viscous friction B.sub.L.

J.sub.L.Math.{umlaut over ()}.sub.L+B.sub.L.Math.{dot over ()}.sub.L=.sub.L+r.Math..sub.msr=.sub.M+r.Math.K.Math.(.sub.Mr.Math..sub.L)Eq. (10)

[0045] From Eq. (9) and Eq. (10) above, an equation of state of the motor control system can be expressed by the following equation (11) using matrices.

[00009]$\begin{array}{cc}\frac{d}{\mathrm{dt}}\left[\begin{array}{c}{}_M\\ {\stackrel{.}{}}_M\\ {}_L\\ {\stackrel{.}{}}_L\\ {}_L\end{array}\right]=\left[\begin{array}{ccccc}0& 1& 0& 0& 0\\ -\frac{K}{J_M}& -\frac{B_M}{J_M}& \frac{r.Math.K}{J_M}& 0& 0\\ 0& 0& 0& 1& 0\\ \frac{r.Math.K}{J_L}& 0& -\frac{r^2.Math.K}{J_L}& -\frac{B_L}{J_L}& \frac{1}{J_L}\\ 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{}_M\\ {\stackrel{.}{}}_M\\ {}_L\\ {\stackrel{.}{}}_L\\ {}_L\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{J_M}\\ 0\\ 0\\ 0\end{array}\right].Math.{}_M& \mathrm{Eq}..Math.\left(11\right)\end{array}$

[0046] If the matrices and variable (motor shaft torque .sub.M) in Eq. (11) are denoted by symbols indicated below, Eq. (11) can be expressed as the following equation (12).

[00010]$\begin{array}{cc}\left[\begin{array}{c}{}_M\\ {\stackrel{.}{}}_M\\ {}_L\\ {\stackrel{.}{}}_L\\ {}_L\end{array}\right]=x,\left[\begin{array}{ccccc}0& 1& 0& 0& 0\\ -\frac{K}{J_M}& -\frac{B_M}{J_M}& \frac{r.Math.K}{J_M}& 0& 0\\ 0& 0& 0& 1& 0\\ \frac{r.Math.K}{J_L}& 0& -\frac{r^2.Math.K}{J_L}& -\frac{B_L}{J_L}& \frac{1}{J_L}\\ 0& 0& 0& 0& 0\end{array}\right]=A,.Math.\left[\begin{array}{c}0\\ \frac{1}{J_M}\\ 0\\ 0\\ 0\end{array}\right]=B,{}_M=u.Math..Math.\stackrel{.}{x}=\mathrm{Ax}+\mathrm{Bu}& \mathrm{Eq}..Math.\left(12\right)\end{array}$

In Eq. (12), x denotes variables, and u denotes an input. Namely, the motor shaft torque .sub.M in Eq. (11) is a torque command value as an input.

[0047] On the other hand, an output equation with respect to the motor shaft rotational angle .sub.M and shaft torsion torque .sub.msr that can be measured can be expressed by the following equation (13).

[00011]$\begin{array}{cc}\left[\begin{array}{c}{}_M\\ {}_{\mathrm{msr}}\end{array}\right]=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ K& 0& -r.Math.K& 0& 0\end{array}\right]\left[\begin{array}{c}{}_M\\ {\stackrel{.}{}}_M\\ {}_L\\ {\stackrel{.}{}}_L\\ {}_L\end{array}\right]& \mathrm{Eq}..Math.\left(13\right)\end{array}$

[0048] If the matrices in Eq. (13) are denoted by symbols indicated below, Eq. (13) can be expressed as the following equation (14).

[00012]$\begin{array}{cc}\left[\begin{array}{c}{}_M\\ {}_{\mathrm{msr}}\end{array}\right]=y,\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ K& 0& -r.Math.K& 0& 0\end{array}\right]=C,\left[\begin{array}{c}{}_M\\ {\stackrel{.}{}}_M\\ {}_L\\ {\stackrel{.}{}}_L\\ {}_L\end{array}\right]=x.Math..Math.y=\mathrm{Cx}& \mathrm{Eq}..Math.\left(14\right)\end{array}$

[0049] The variables x can be estimated according to the following equation (15), using an observer method, from Eq. (12) as the equation of state and Eq. (14) as the output equation. Here, since the output shaft rotational angle .sub.L is included in the variables x, the output shaft rotational angle .sub.L can be estimated. In the following equation (15), estimated value of the variables x are expressed as x.

{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu+L(yC{circumflex over (x)})Eq. (15)

[0050] As indicated in Eq. (15), in the motor control system according to the second embodiment, the variables x are estimated by using differentials of the variables x. As described above, the variables x include the rotational angular velocity of the output shaft obtained by differentiating the rotational angle .sub.L of the output shaft, in addition to the rotational angle .sub.L of the output shaft. Therefore, in the motor control system according to the second embodiment, the output shaft rotational angle .sub.L is estimated by using at least one of the rotational angular velocity of the output shaft obtained by differentiating the rotational angle .sub.L of the output shaft, and the rotational angular acceleration of the output shaft obtained by differentiating the rotational angle .sub.L of the output shaft twice.

[0051] The above-indicated equation (Eq.) (15) means that estimated values Cx of the measurement values y using the estimated values x of the variables x are compared with the actual measurement values y, and the estimated values x are corrected according to differences yCx between these values. In Eq. (15), constant L is a gain that determines the amount of correction. While the estimated values x converge more quickly as L is larger, the estimated values x may not converge due to noise of the measurement values y if L is excessively large.

[0052] As described above, in the motor control system according to the second embodiment, the estimated values Cx of the measurement values y are compared with the actual measurement values y, and the estimated values x are corrected according to the differences yCx between these values. Therefore, the system of the second embodiment has higher noise resistance, as compared with the system of the first embodiment that directly uses measurement values. Accordingly, the motor control system according to the second embodiment can perform position control with high accuracy, without using the output shaft rotational angle sensor.

[0053] It is to be understood that this disclosure is not limited to the above embodiments, but the embodiments may be modified as needed without departing from the scope of the disclosure.