JP2019170114A - Control apparatus of motor, robot, and control method of the motor - Google Patents

Abstract

To miniaturize an overshoot by smoothly operating a motor at an acceleration and a deceleration with a simple procedure even if a disturbance or a load is changed, and reduce a stabilization time.SOLUTION: A reference determination part of a control apparatus of a motor, determines a reference rotational angle of the motor as a command value for rotating the motor. In the reference determination part, an actual rotational angle of the motor delays from the just before reference rotational angle determined by the reference determination part, and a current reference rotational angle is determined by adding an angle increase to a rotational speed of the motor to the just before reference rotational angle in the case where a delay amount is smaller than a prescribed threshold value. The just before reference rotational angle is determined as the current rotational angle in the case where the delay amount is larger than the threshold value.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a motor control device, a robot, and a motor control method.

  For example, as a method for controlling a motor incorporated in a mobile robot or an articulated robot, it is known to control the torque, rotation angle, rotation speed, or jerk (jerk) of the motor according to an operation reference profile (Patent Document 1). 2). The motion reference profile defines changes in motor torque, rotation angle, rotation speed or jerk over time. The motion reference profile is pre-designed based on the target rotational angle, motor performance (eg, maximum achievable rotational acceleration, maximum achievable rotational deceleration, maximum achievable rotational speed) and load applied to the motor. The In general, a triangular or trapezoidal motion reference profile for speed change and an S-curve motion reference profile for angle change are known.

US Pat. No. 8,810,777 U.S. Pat. No. 9,041,337

  In order to design an appropriate operating reference profile, it is desirable not only to know the performance of the motor but also to predict the load applied to the motor. This is because the operation of the motor is limited according to the load.

  However, accurately predicting the load can be difficult and time consuming. In addition, disturbances may occur during the operation of the motor, and the load may change. The reason for the disturbance is, for example, that an object driven by a motor hits an obstacle. The reason for the change in load is that the load applied to the motor changes when the posture of the arm changes. Disturbances and load changes are more difficult to predict. Therefore, by using a pre-designed motion reference assuming a certain operating condition, the actual position cannot follow the reference position, resulting in a large difference between the reference position and the actual position. It can happen. As a result, the system can suffer from poor control performance such as large overshoot, long settling time, and long duration of saturation of the motor drive signal. .

  In addition, during the use of a pre-designed motion reference profile, if there are disturbances or load changes, rapid motor acceleration or deceleration may be required. As a result, the lifetime of motor parts or related parts such as gears may be shortened.

  In controlling the motor, it is desirable that the motor operates smoothly. It is also desirable to minimize overshoot, shorten the settling time to the target position, and reduce the drive signal saturation time. These goals should be achieved despite disturbances and load changes.

  In the technique described in Patent Document 1, the inertia and friction coefficient of the motor are estimated during the operation of the motor, and the control parameters of the motor are dynamically adjusted based on the estimation. However, this technique increases the calculation burden, does not contribute to the reduction of overshoot and the settling time, and is less robust against disturbances and load changes.

  In the technique described in Patent Document 2, the duration of each segment (stage) in the motion profile is calculated, the duration is further corrected, and the motion criterion is recalculated using the corrected duration. However, with this technique, the computational burden is excessive, and the robustness against disturbances and load changes is low.

  Therefore, the present invention can operate the motor smoothly during acceleration and deceleration, minimize overshoot, and shorten the settling time even if there is a disturbance or load change. It is an object of the present invention to provide a motor control device, a robot, and a motor control method.

  A motor control device according to an aspect of the present invention obtains an actual rotation angle of a motor at a predetermined cycle, and determines a reference rotation angle of the motor at a predetermined cycle as a command value for rotating the motor. A determination unit, wherein the reference determination unit has an actual rotation angle of the motor that is delayed from a previous reference rotation angle determined by the reference determination unit, and a delay amount is smaller than a predetermined threshold value. Determining a current reference rotation angle by adding an angle increment corresponding to the rotation speed of the motor to the immediately preceding reference rotation angle, and the actual rotation angle is delayed from the immediately preceding reference rotation angle. If the delay amount is larger than the threshold value, the immediately preceding reference rotation angle is determined as the current reference rotation angle.

  In the present invention, even if there is a disturbance or a change in the load applied to the motor, the current reference rotation angle is determined based on the reference rotation angle determined immediately before. Therefore, the difference between the reference rotation angle and the actual rotation angle is determined. Can always be reduced. Therefore, it is possible to minimize the overshoot, shorten the settling time, reduce the saturation of the drive signal supplied to the motor, and smooth the acceleration and deceleration of the motor. It is possible to improve the life of parts and parts related to motors. Further, the current reference rotation angle can be easily determined by using the previous reference rotation angle or the current reference rotation angle as it is or by adding an angle increment corresponding to the rotation speed.

FIG. 1 is a block diagram showing a motor control apparatus according to an embodiment of the present invention. FIG. 2 is a flowchart illustrating a reference rotation angle determination process according to the embodiment. FIG. 3 is a flowchart illustrating a reference angle position determination process according to the embodiment.

Embodiments according to the present invention will be described below with reference to the accompanying drawings.
<Configuration of control device>
FIG. 1 is a block diagram showing a motor control apparatus 1 according to an embodiment of the present invention. The control device 1 according to the present embodiment is provided in the mobile robot in order to control the motor 2 that rotates the wheels of the mobile robot. The control device 1 gives a command value to the motor controller 4 that drives the motor 2. The command values are a reference angular position (reference rotation angle) θ _r and a reference rotation speed ω _r at which the motor 2 is to be rotated.

The motor controller 4 is a combination of a drive circuit and a processor. The motor controller 4 sends a drive signal for controlling the motor 2 to the motor 2 so that the motor 2 rotates to the reference angular position θ _r given from the control device 1 at the reference rotation speed ω _r given from the control device 1. Supply.

  The control device 1 includes a reference determination unit 7 and a memory 8. The reference determination unit 7 is a processor and operates by reading and executing a program stored in a recording medium (for example, the memory 8). Therefore, the program (program code) read from the recording medium implements the functions of the embodiment. Moreover, the recording medium which recorded the said program can comprise this invention.

In order to control the motor 2, various parameters are stored in the memory 8. The various parameters include a target angular position θ _{d at which} the motor 2 is to be finally rotated and a target rotational speed ω _{d at} which the motor 2 is to be rotated. The various parameters also include the reference angular position θ _r and the reference rotation speed ω _r determined by the reference determination unit 7.

The reference determination unit 7 determines a reference angular position θ _r and a reference rotation speed ω _r to be given to the motor controller 4. In order to determine the current reference angle position θ _r (t), the reference determination unit 7 obtains, that is, reads out the previous reference rotation angle θ _r (t−1) determined by the reference determination unit 7 from the memory 8.

Based on the immediately preceding reference rotation angle θ _r (t−1) determined by the reference determination unit 7, the reference determination unit 7 determines the current reference rotation angle θ _r (t) at which the motor 2 should be rotated for a predetermined period ( The sampling period is periodically determined by T. Then, the reference determination unit 7 periodically instructs the motor controller 4 at the sampling period T so that the motor 2 rotates at the determined current reference rotation angle θ _r (t). Therefore, the reference rotation angle θ _r determined by the reference determination unit 7 is a command rotation angle as a command value for rotating the motor 2. The reference determination unit 7 stores the determined current reference rotation angle θ _r (t) in the memory 8.

Here, the current reference rotation angle θ _r (t) means the reference rotation angle θ _r at the current sampling time t for controlling the motor 2. The immediately preceding reference rotation angle θ _r (t-1), refers to the reference rotation angle theta _r during the immediately preceding sampling time t-1 (only the previous sampling time sampling period T of the current sampling time t). In the following description, the reference rotation angle is referred to as a reference angle position.

Further, in order to determine the current reference rotational speed ω _r (t), the reference determining unit 7 acquires, that is, reads out the reference rotational speed ω _r (t−1) immediately before determined by the reference determining unit 7 from the memory 8. To do.

Based on the reference rotation speed ω _r (t−1) immediately before the motor 2 determined by the reference determination section 7, the reference determination section 7 determines the current reference rotation speed ω _r (t) to rotate the motor 2. It is determined periodically by the sampling period T. Then, the reference determination unit 7 periodically instructs the motor controller 4 at the sampling period T so that the motor 2 rotates at the reference rotation speed ω _r (t). Therefore, the reference rotation speed ω _r determined by the reference determination unit 7 is a command rotation speed as a command value for rotating the motor 2. The reference determining unit 7 stores the determined current reference rotational speed ω _r (t) in the memory 8.

Here, the current reference rotational speed ω _r (t) means the reference rotational speed ω _r at the current sampling time t for controlling the motor 2. The reference rotational speed immediately before ω _r (t-1), refers to the standard rotational speed omega _r of the immediately preceding sampling time t-1 (only the previous sampling time sampling period T of the current sampling time t).

The control device 1 cooperates with the measuring unit 5. The measurement unit 5 includes a speed calculation unit 10 and a rotary encoder unit 12. The rotary encoder unit 12 measures the angular position of the motor 2. The rotary encoder unit 12 is arranged in the vicinity of the motor 2 or the wheel or in the motor 2. The rotary encoder unit 12 periodically supplies the actual rotation angle θ as an angular position signal representing the angular position of the motor 2 to the motor controller 4, the reference determination unit 7, and the speed calculation unit 10 with a sampling period T. Therefore, the reference determination unit 7 periodically acquires the current actual rotation angle θ (t) of the motor 2 at the sampling period T. The reference determination unit 7 uses the current actual rotation angle θ (t) to determine the reference angle position θ _r (t) and the reference rotation speed ω _r (t). The current actual rotation angle θ (t) means the actual rotation angle θ of the motor 2 at the current sampling time t. In the following description, the actual rotation angle is referred to as an actual angular position.

The speed calculation unit 10 periodically calculates the current actual rotational speed ω (t) of the motor 2 with a sampling period T based on the angular position signal by a known method. The rotational speed ω is an estimated value, that is, a measured value. Here, the rotational speed ω is referred to as an actual rotational speed in order to distinguish it from the reference rotational speed ω _r . Then, the speed calculation unit 10 periodically supplies the current actual rotational speed ω (t) of the motor 2 to the motor controller 4 at the sampling period T. The current actual rotational speed ω (t) means the actual rotational speed ω of the motor 2 at the current sampling time t. The motor controller 4 is supplied with the current actual rotation angle θ (t) from the rotary encoder unit 12, is supplied with the current actual rotation speed ω (t) from the speed calculation unit 10, and is supplied with a reference from the reference determination unit 7. An angular position θ _r (t) and a reference rotational speed ω _r (t) are supplied. The processor of the motor controller 4 sets the current actual rotation angle θ (t), the current actual rotation speed ω (t), the reference angular position θ _r (t), and the reference rotation speed ω _r (t). Based on this, a drive signal is generated according to a predetermined control algorithm, and the drive signal is supplied to the drive circuit. As a result, the motor 2 rotates to the reference angular position θ _r (t) at the reference rotation speed ω _r (t).

<Parameters and variables>
Next, parameters and variables used in this specification will be described.

θ _r : Reference angular position.
ω _r : Reference rotation speed.
θ: Actual angular position.
ω: Actual rotation speed.
The reference angular position θ _r , the reference rotational speed ω _r , the actual angular position θ, and the actual rotational speed ω are as already described.

θ _d : Target angular position of the motor 2 The target angular position θ _d is an angular position where the motor 2 should be finally rotated. The target angular position θ _d is input by an administrator of the control device 1 or generated by a system controller (not shown). The target angular position θ _d is stored in the memory 8. The reference determination unit 7 reads out the target angular position θ _d from the memory 8 and uses it to determine the current reference angular position θ _r (t) and the current reference rotational speed ω _r (t).

ω _d : target rotation speed of the motor 2 The target rotation speed ω _d is a desired speed at which the motor 2 should be rotated, and is equal to or lower than the rated maximum speed of the motor 2. The target rotation speed ω _d is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the target rotational speed ω _d may be a default value stored in advance in the memory 8. The reference determination unit 7 reads the target rotation speed ω _d from the memory 8 and uses it to determine the current reference rotation speed ω _r (t).

a ₁ : Acceleration coefficient. When the motor 2 is accelerated, a speed increment (a constant positive value) is added to the rotational speed of the motor 2. The speed increment is a value obtained by multiplying the acceleration coefficient a ₁ by the sampling period T. The acceleration coefficient a ₁ is a constant positive value. The acceleration coefficient a ₁ is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the acceleration coefficient a ₁ may be a default value stored in the memory 8 in advance. The reference determination unit 7 reads the acceleration coefficient a ₁ from the memory 8 and uses it to determine the current reference rotational speed ω _r (t) when accelerating the motor 2.

a ₂ : Deceleration coefficient. When the motor 2 is decelerated, a speed decrement (a constant positive value) is subtracted from the rotational speed of the motor 2. The speed decrement is a value obtained by multiplying the deceleration coefficient a ₂ by the sampling period T. The deceleration coefficient a ₂ is a constant positive value. The deceleration coefficient a ₂ is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the deceleration coefficient a ₂ may be a default value stored in the memory 8 in advance. The reference determination unit 7 reads the deceleration coefficient a ₂ from the memory 8 and uses it to determine the current reference rotational speed ω _r (t) when the motor 2 is decelerated.

θ _e : Position error. The position error θ _e is the difference between the immediately preceding reference angular position θ _r (t−1) and the current actual angular position θ (t), and θ _e = θ _r (t−1) −θ (t) Defined.

θ _th : Position delay threshold. The position delay threshold value θ _th is a positive threshold value that the reference determination unit 7 compares with the position error θ _e when determining the current reference angle position θ _r (t). The position delay threshold value θ _th is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the position delay threshold value θ _th may be a default value stored in the memory 8 in advance. The reference determination unit 7 reads the position delay threshold value θ _th from the memory 8 and uses the position delay threshold value θ _th when determining the current reference angle position θ _r (t).

θ _dec : Deceleration start boundary angle. The deceleration start boundary angle θ _dec is a positive value. The deceleration start boundary angle θ _dec is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the deceleration start boundary angle θ _dec may be a default value stored in the memory 8 in advance.

ω _low : Low rotation speed. The low rotational speed ω _low is a constant speed at which the motor 2 should be rotated in the vicinity of the target angular position θ _d , and is lower than the target rotational speed ω _d . For example, the low rotation speed ω _low is ¼ of the target rotation speed ω _d . The low rotation speed ω _low is input by the administrator of the control device 1 and stored in the memory 8. Alternatively, the low rotation speed ω _low may be a default value stored in the memory 8 in advance. The reference determination unit 7 reads the deceleration start boundary angle θ _dec and the low rotational speed ω _low from the memory 8, and the value obtained by subtracting the current actual angular position θ (t) from the target angular position θ _d is the deceleration start boundary angle. When smaller than θ _dec , the reference rotational speed ω _r (t) is lowered to the low rotational speed ω _low . That is, when the current actual angular position θ (t) enters the deceleration start boundary angle θ _{dec immediately} before the target angular position θ _d , the reference determination unit 7 rotates the reference rotational speed ω _r (t) at a low speed. Reduce to speed ω _low .

T: Sampling period. The sampling period T is 2 ms, for example. The reference determination unit 7 acquires the current actual angular position θ (t) of the motor 2 at the sampling period T, generates the current reference angular position θ _r (t) at the sampling period T, and generates the current reference rotational speed. ω _r (t) is periodically determined by the sampling period T.

Hereinafter, the control method of the motor 2 according to the embodiment will be described. In the following description, it is assumed that the motor 2 is rotated only in one direction. However, the present invention can also be applied to rotate the motor 2 in the reverse direction. In the following description, the initial actual angular position θ and the initial actual rotational speed ω are assumed to be zero, and the initial reference angular position θ _r and the initial reference rotational speed ω _r are zero. However, the present invention is applicable to any initial position and any initial rotational speed.

<Determination processing of reference rotation speed>
With reference to FIG. 2, the determination process of the reference | standard rotation speed which concerns on embodiment is demonstrated.

When the sampling time is reached (when the determination in step S1 becomes affirmative), the reference determination unit 7 determines whether or not the immediately preceding reference angular position θ _r (t−1) has reached the target angular position θ _d ( Step S2). The reference angle position (command angle position) determination process will be described later.

If the determination in step S2 is negative, the process proceeds to step S3. In step S3, the reference determining unit 7, a value obtained by subtracting the actual angular position of the current theta (t) from the target angular position theta _d is determined whether the deceleration start boundary angle theta _dec above.

If the determination in step S3 is affirmative, the process proceeds to step S4, and the reference determining unit 7 determines that the immediately preceding reference rotational speed ω _r (t−1) determined by the reference determining unit 7 is the target rotational speed ω _d. Judge whether it is smaller.

If the determination in step S4 is affirmative, the process proceeds to step S5. In step S5, the reference determination unit 7 determines the current reference rotation speed ω _r (t) by adding a speed increment to the immediately preceding reference rotation speed ω _r (t−1) determined by the reference determination unit 7. To do. That is, when the reference rotation speed ω _r (t−1) immediately before the motor 2 is smaller than the target rotation speed ω _d , the reference determination unit 7 sets the reference rotation speed ω _r (t−1) immediately before the motor 2. Is determined to increase the current reference rotational speed ω _r (t). The speed increment is a value obtained by multiplying the acceleration coefficient a ₁ by the sampling period T.
In step S <b> 5, the reference determination unit 7 supplies the current reference rotational speed ω _r (t) determined by the reference determination unit 7 to the motor controller 4 as a command value for rotating the motor 2. After step S5, the process returns to step S1 and waits until the next sampling time.

If the determination in step S4 is negative, the process proceeds to step S6. In step S6, the reference determination unit 7 determines the target rotation speed ω _d as the current reference rotation speed ω _r (t). That is, when the reference rotation speed ω _r (t−1) immediately before the motor 2 determined by the reference determination unit 7 is equal to or higher than the target rotation speed ω _d , the reference determination unit 7 determines that the current reference rotation speed ω _{It is} determined to maintain the target rotational speed ω _d as _r (t).
In step S <b> 6, the reference determination unit 7 supplies the current reference rotation speed ω _r (t) determined by the reference determination unit 7 to the motor controller 4 as a command value for rotating the motor 2. After step S6, the process returns to step S1 and waits until the next sampling time.

On the other hand, if the determination in step S3 is negative, the process proceeds to step S7. That is, when the value obtained by subtracting the actual angular position of the current theta (t) from the target angular position theta _d the deceleration start boundary angle theta _dec smaller, the process proceeds to step S7. In step S <b> 7, the reference determining unit 7 determines whether or not the immediately preceding reference rotational speed ω _r (t−1) determined by the reference determining unit 7 is greater than the low rotational speed ω _low .

If the determination in step S7 is affirmative, the process proceeds to step S8. In step S8, the reference determining unit 7 subtracts the speed decrement from the immediately preceding reference rotational speed ω _r (t−1) determined by the reference determining unit 7 to obtain the current reference rotational speed ω _r (t). decide. That is, after the current actual angular position θ (t) reaches a predetermined deceleration start angle (target angular position θ _d −θ _dec ), the immediately preceding reference rotational speed ω _r (t−1) is a predetermined low speed. If it is greater than the speed threshold ω _low , the reference determination unit 7 lowers the current reference rotational speed ω _r (t) by subtracting the speed decrement from the immediately preceding reference rotational speed ω _r (t−1). To be determined. The speed decrement is a value obtained by multiplying the deceleration coefficient a ₂ by the sampling period T.
In step S <b> 8, the reference determination unit 7 supplies the current reference rotation speed ω _r (t) determined by the reference determination unit 7 to the motor controller 4 as a command value for rotating the motor 2. After step S8, the process returns to step S1 and waits until the next sampling time.

If the determination in step S7 is negative, the process proceeds to step S9. In step S9, the reference determination unit 7 determines the low rotation speed ω _low as the current reference rotation speed ω _r (t). That is, when the current actual angular position θ (t) reaches the predetermined deceleration start angle θ _dec and the immediately preceding reference rotational speed ω _r (t−1) is equal to or lower than the low rotational speed threshold ω _low. The reference determination unit 7 determines to maintain the low rotation speed ω _low as the current reference rotation speed ω _r (t).
In step S <b> 9, the reference determination unit 7 supplies the current reference rotation speed ω _r (t) determined by the reference determination unit 7 to the motor controller 4 as a command value for rotating the motor 2. After step S9, the process returns to step S1 and waits until the next sampling time.

On the other hand, if the determination in step S2 is affirmative, the process proceeds to step S10. In step S10, the reference determination unit 7 determines the current reference rotation speed ω _r (t) to be zero. That is, after the immediately preceding reference angular position θ _r (t−1) reaches the target angular position θ _d of the motor 2, the reference determining unit 7 determines the current reference rotational speed ω _r (t) to zero. .
In step S <b> 10, the reference determination unit 7 supplies the current reference rotation speed ω _r (t) determined by the reference determination unit 7 to the motor controller 4 as a command value for rotating the motor 2. After step S10, the process ends.

The operation of the motor 2 in accordance with the reference rotational speed determination process according to the embodiment includes five segments (stages) of acceleration, constant speed, deceleration, low speed rotation, and stop. The acceleration segment is executed by repeating the operation from step S2, S3, S4 to S5. In the acceleration segment, the reference rotational speed ω _r (t−1) immediately before the motor 2 reaches the target rotational speed ω _d until the reference rotational speed ω _r (t−1) immediately before the motor 2 is increased by a constant speed. By adding (a ₁ × T), the rotation speed of the motor 2 continues to increase.

The constant speed segment operates in steps S2, S3, triggered by the previous reference rotational speed ω _r (t−1) reaching the target rotational speed ω _d (determination of step S4 is negative). This is executed by repeating the process of S4 and S6. In the constant speed segment, the target rotational speed ω _d is maintained as the current reference rotational speed ω _r (t), and the motor 2 continues to rotate at the constant speed ω _d .

The operation of the deceleration segment is triggered by the current actual angular position θ (t) determined by the reference determination unit 7 reaching a predetermined deceleration start angle (determination of step S3 becomes negative). It is executed by repeating the process of S2, S3, and S7 and then proceeding to S8. The deceleration segments, reference rotational speed ω _r (t-1) immediately before the motor 2 until lowered to a low rotational speed omega _low, down a constant speed from a reference rotational speed immediately before the motor 2 ω _r (t-1) By pulling the minute (a ₂ × T), the rotation speed of the motor 2 continues to decrease.

In the low-speed rotation segment, after the current actual angular position θ (t) determined by the reference determination unit 7 reaches a predetermined deceleration start angle, the immediately preceding reference angular position θ _r (t−1) has a low rotation speed. The operation is executed by repeating the steps S2, S3, S7 to S9, triggered by the drop to ω _low (determination of step S7 becomes negative). In the low-speed rotation segment, the low rotation speed ω _low is maintained as the current reference rotation speed ω _r (t), and the motor 2 continues to rotate at a constant low rotation speed ω _low .

The stop segment indicates that the immediately preceding reference angular position θ _r (t−1) determined by the reference determining unit 7 reaches the target angular position θ _d of the motor 2 (the determination in step S2 becomes affirmative). As an opportunity, the operation is executed by proceeding to step S10. In the stop segment, the current reference rotational speed ω _r (t) is determined to be zero, and the rotation of the motor 2 is stopped.

According to the reference rotational speed determination process according to the embodiment, the current reference rotational speed ω _r (t) can be easily and dynamically determined. Specifically, in the acceleration segment, the current reference rotation speed ω _r (t) can be easily and dynamically determined from the immediately preceding reference rotation speed ω _r (t−1) and a predetermined acceleration coefficient a _1. Yes (step S5). In the constant speed segment, the current reference rotational speed ω _r (t) can be easily and dynamically determined from the target rotational speed ω _d (step S6). In the deceleration segment, the current reference rotational speed ω _r (t) can be easily and dynamically determined from the immediately preceding reference rotational speed ω _r (t−1) and the predetermined deceleration coefficient a ₂ (step S8). . In the low-speed rotation segment, the current reference rotation speed ω _r (t) can be easily and dynamically determined from the low rotation speed ω _low (step S9).

According to the process of determining the reference rotation speed according to the embodiment, the current reference rotation speed ω _r (t) is easily and dynamically determined from a given parameter and / or a given variable according to a predetermined state. Can be determined.

<Determination processing of reference angular position>
With reference to FIG. 3, the determination process of the reference | standard angle position (command angle position) which concerns on embodiment is demonstrated. The reference angular position determination process is executed in parallel with the reference rotation speed determination process. More precisely, the reference angular position determination process is started after the reference rotational speed determination process.

When the reference determination unit 7 reaches the sampling time (when the determination in step S11 becomes affirmative), the reference angular position θ _r (t−1) immediately before determined by the reference determination unit 7 becomes the target angular position θ of the motor 2. It is determined whether or not _d has been reached (step S12). If the determination in step S12 is negative, the process proceeds to step S13. In step S <b> 13, the reference determination unit 7 determines that the current actual angular position θ (t) of the motor 2 acquired from the rotary encoder unit 12 is the reference angular position θ _r (t It is determined whether it is larger than -1). That is, in step S13, the reference determination unit 7 determines whether or not the current actual angular position θ (t) is ahead of the previous reference angular position θ _r (t−1) determined by the reference determination unit 7. Judge.

If the determination in step S13 is negative, the process proceeds to step S14. In step S14, the reference determination unit 7 determines whether or not the immediately preceding reference angular position θ _r (t−1) is larger than the value obtained by adding the position delay threshold θ _th to the current actual angular position θ (t). . That is, in step S14, the reference determining unit 7, the position error theta _e determines whether the position is greater than a delay threshold theta _th.

If the determination in step S14 is affirmative, the process proceeds to step S15. In step S15, the reference determination unit 7 determines the immediately previous reference angular position θ _r (t−1) as the current reference angular position θ _r (t). That is, when the current actual angular position θ (t) is behind the previous reference angular position θ _r (t−1) and the delay amount (position error θ _e ) is greater than the threshold θ _th , The reference determination unit 7 determines the immediately previous reference angular position θ _r (t−1) as the current reference angular position θ _r (t).
In step S15, the reference determination unit 7 uses the current reference angular position (current command angular position) θ _r (t) determined by the reference determination unit 7 as a command value for rotating the motor 2. This is supplied to the driver 4. After step S15, the process returns to step S11 and waits until the next sampling time.

On the other hand, if the determination in step S14 is negative, the process proceeds to step S16. In step S <b> 16, the reference determination unit 7 determines the current reference angular position θ _r (t) by adding an angle increment corresponding to the rotation speed of the motor 2 to the immediately preceding reference angular position θ _r (t−1). To do. That is, the current actual angular position θ (t) matches the previous reference angular position θ _r (t−1), or the current actual angular position θ (t) is the previous reference angular position θ _r (t -1), but when the delay amount (position error θ _e ) is equal to or smaller than the threshold θ _th , the reference determination unit 7 changes the angle increment to the immediately preceding reference angular position θ _r (t−1). In addition, the current reference angular position θ _r (t) is determined. The angle increment is, for example, a value obtained by multiplying the current reference rotation speed ω _r (t) determined by the reference rotation speed determination process by the sampling period T. However, the angle increment may be a value obtained by multiplying the current actual angular position θ (t) by the sampling period T.
In step S <b> 16, the reference determination unit 7 uses the current reference angular position (current command angular position) θ _r (t) determined by the reference determination unit 7 as a command value for rotating the motor 2. This is supplied to the driver 4.

On the other hand, if the determination in step S13 is affirmative, the process proceeds to step S17. In step S17, the reference determination unit 7 determines the current reference angular position θ _r (t) by adding an angle increment to the current actual angular position θ (t). That is, when the current actual angular position θ (t) of the motor 2 is ahead of the immediately preceding reference angular position θ _r (t−1), the reference determining unit 7 determines that the current actual angular position is The current reference angular position θ _r (t) is determined by adding an angle increment to θ (t). The angle increment is, for example, a value obtained by multiplying the current reference rotation speed ω _r (t) determined by the reference rotation speed determination process by the sampling period T. However, the angle increment may be a value obtained by multiplying the current actual angular position θ (t) by the sampling period T.
In step S <b> 17, the reference determining unit 7 uses the current reference angular position (current command angular position) θ _r (t) determined by the reference determining unit 7 as a command value for rotating the motor 2. This is supplied to the driver 4. After step S17, the process returns to step S11 and waits until the next sampling time.

If the determination in step S12 is affirmative, the process proceeds to step S18. In step S18, the reference determination unit 7 determines the target angular position θ _d as the current reference angular position θ _r (t). That is, when the immediately preceding reference angular position θ _r (t−1) has reached the target angular position θ _d of the motor 2, the reference determining unit 7 sets the target angular position θ _d to the current reference angular position θ. Determine as _r (t).
In step S <b> 18, the reference determination unit 7 uses the current reference angular position (current command angular position) θ _r (t) determined by the reference determination unit 7 as a command value for rotating the motor 2. This is supplied to the driver 4. After step S18, the process ends.

  According to the determination processing of the reference angular position according to the embodiment, even if there is a disturbance or a change in the load applied to the motor 2, the motor 2 is operated without sudden acceleration or sudden deceleration. Thus, the settling time can be shortened, and the life of the parts of the motor 2 and the parts related to the motor 2 can be improved.

Specifically, when the current actual angular position θ (t) is behind the previous reference angular position θ _r (t−1) and the delay amount (position error θ _e ) is larger than the threshold θ _th. In step S15, the reference determination unit 7 determines the immediately previous reference angular position θ _r (t−1) as the current reference angular position θ _r (t). In other words, the reference angle position does not increase until the delay amount (position error θ _e ) becomes equal to or less than the threshold value θ _th . In this case, the position error θ _e can always be maintained within the threshold θ _th . Therefore, saturation of the drive signal supplied from the motor controller 4 to the motor 2 can be avoided or reduced, overshoot can be minimized, and settling time can be shortened.

The current actual angular position θ (t) matches the previous reference angular position θ _r (t−1), or the current actual angular position θ (t) is the previous reference angular position θ _r (t−1). ), But the delay amount (position error θ _e ) is equal to or smaller than the threshold θ _th , the reference determination unit 7 adds the angle increment to the immediately preceding reference angular position θ _r (t−1). Thus, the current reference angular position θ _r (t) is determined (step S16). In this case, it is considered that the actual angular position θ (t) can follow the reference angular position. By adding an angle increment to the immediately preceding angular position θ _r (t−1), the current reference position θ _r (t) is updated, so that the reference angular position gradually increases toward the target angular position θ _d. .

When the current actual angular position θ (t) of the motor 2 is ahead of the immediately preceding reference angular position θ _r (t−1), the reference determination unit 7 determines the current actual angular position θ ( The current reference angular position θ _r (t) is determined by adding the angle increment to t) (step S17). In this case, the current reference angular position θ _r (t) is determined simply by adding an angle increment to the current actual angular position θ (t). Therefore, the motor 2 is not suddenly decelerated. Further, the current reference angular position θ _r (t) can be easily determined by simply adding an angle increment to the current actual angular position θ (t).

The angle increment used in steps S16 and S17 is, for example, a value obtained by multiplying the current reference rotation speed ω _r (t) determined in the reference rotation speed determination process by the sampling period T. However, the angle increment may be a value obtained by multiplying the current actual angular position θ (t) by the sampling period T. In any case, the angle increment can be easily calculated, and the current reference angular position θ _r (t) can be easily determined from the angle increment in steps S16 and S17.

<Modification>
Although the embodiment of the present invention has been described above, the above description is not intended to limit the present invention, and various modifications including deletion, addition, and replacement of components are conceivable within the technical scope of the present invention. .

  For example, the control device 1 according to the above-described embodiment is provided in the mobile robot in order to control the motor 2 that rotates the wheels of the mobile robot. However, you may utilize this invention for control of the motor of an articulated robot, a vehicle, or a conveying apparatus. The machine element driven by the motor 2 is not limited to a wheel, and may be an arm of an articulated robot, a roller of a transfer device, or another machine element.

  In the control device 1 or the motor controller 4, each function executed by the processor may be executed by hardware instead of the processor, or may be programmable such as FPGA (Field Programmable Gate Array) and DSP (Digital Signal Processor). It may be executed by a logic device.

  In the above embodiment, the rotary encoder unit 12 derives the current actual angular position θ (t) of the motor 2, and the angular position signal indicating the angular position θ (t) is transmitted to the motor controller 4, the reference determination unit 7, And supplied to the speed calculator 10. However, the calculation unit 10 calculates the current actual angular position θ (t) and the current actual rotational speed ω (t) of the motor 2 based on the angular position signals from other types of angle sensors. Also good.

The acceleration coefficient a ₁ used in step S5 of the reference rotational speed determination process (FIG. 2) is a constant value in the above embodiment. However, the acceleration coefficient a ₁ may be a variable value that changes in accordance with the rotational speed of the motor 2. For example, when the rotational speed of the motor 2 approaches the target rotational speed ω _d , the acceleration coefficient a ₁ may be decreased.
The deceleration coefficient a ₂ used in step S8 of the reference rotational speed determination process (FIG. 2) is a constant value in the above embodiment. However, the deceleration coefficient a ₂ may be a variable value that changes in accordance with the rotational speed of the motor 2. For example, when the rotational speed of the motor 2 approaches the low rotational speed ω _low , the deceleration coefficient a ₂ may be decreased.

The low rotational speed ω _low used in step S9 of the reference rotational speed determination process (FIG. 2) is a constant value in the above embodiment. However, the low rotational speed ω _low may be a variable value that changes in accordance with the angular position of the motor 2. In this case, the low rotational speed ω _low used as the speed reference in step S9 may be different from the low rotational speed ω _low used as the threshold value in step S7. For example, when the angular position of the motor 2 approaches the target angular position theta _d, it may reduce low rotational speed omega _low to be used as a speed reference at step S9.

The symbol “>” in step S14 of the reference angle position determination process (FIG. 3) may be replaced with “≧”. That is, when the delay amount (position error θ _e ) is equal to the threshold value θ _th , the process proceeds to step S16 in the above embodiment, but in this case, the process may proceed to step S15.

The item which concerns on the other aspect of this invention is enumerated.
1. A reference determining unit that acquires an actual rotation angle of the motor at a predetermined cycle and determines a reference rotation angle of the motor at the predetermined cycle as a command value for rotating the motor;
The reference determining unit
If the actual rotation angle of the motor is ahead of the previous reference rotation angle determined by the reference determination unit, the current reference is added by adding the angle increment to the actual rotation angle. Determine the rotation angle,
Motor control device.

2. The reference determining unit
If the immediately preceding reference rotation angle has reached the target rotation angle of the motor, the target rotation angle is determined as the current reference rotation angle;
The motor control device according to Item 1.

3. A reference determining unit that determines a reference rotation speed of the motor at the predetermined period as a command value for rotating the motor;
The reference determining unit
When the reference rotation speed immediately before the motor determined by the reference determination unit is smaller than the target rotation speed, the current reference rotation speed is determined by adding a speed increment to the reference rotation speed immediately before the motor. And
When the immediately preceding reference rotation speed is equal to or higher than the target rotation speed, the target rotation speed is determined as the current reference rotation speed;
Motor control device.

4). The reference determining unit
After the actual rotation angle of the motor reaches a predetermined deceleration start angle, if the immediately preceding reference rotation speed determined by the reference determining unit is greater than a predetermined low rotation speed threshold, the immediately preceding reference rotation Determining the current reference rotational speed by subtracting the speed decrement from the speed;
After the actual rotation angle reaches a predetermined deceleration start angle, if the immediately preceding reference rotation speed is equal to or lower than the low rotation speed threshold, the low rotation speed is determined as the current reference rotation speed.
Item 4. The motor control device according to Item 3.

5. The reference determining unit
After the previous reference rotation angle reaches the target rotation angle of the motor, the current reference rotation speed is determined to be zero.
Item 5. The motor control device according to item 3 or 4.

6). At least one motor;
The control device according to any one of items 1 to 5 attached to the at least one motor;
A mechanical element driven by the at least one motor;
Robot with

7). Obtaining the actual rotation angle of the motor at a predetermined period;
Determining a reference rotation angle of the motor at the predetermined period as a command value for rotating the motor;
In determining the reference rotation angle,
If the actual rotation angle of the motor is ahead of the previously determined previous reference rotation angle, the current reference rotation angle is determined by adding the angle increment to the actual rotation angle. To
Motor control method.

8). Determining a reference rotational speed of the motor as a command value for rotating the motor at a predetermined period;
In determining the reference rotational speed,
If the previously determined reference rotational speed immediately before the motor is smaller than the target rotational speed, a current reference rotational speed is determined by adding a speed increment to the immediately preceding reference rotational speed of the motor;
When the immediately preceding reference rotation speed is equal to or higher than the target rotation speed, the target rotation speed is determined as the current reference rotation speed;
Motor control method.

DESCRIPTION OF SYMBOLS 1 Control apparatus 2 Motor 4 Motor controller 7 Reference | standard determination part 8 Memory 10 Speed calculation part 12 Rotary encoder unit

Claims (11)

A reference determining unit that acquires an actual rotation angle of the motor at a predetermined cycle and determines a reference rotation angle of the motor at the predetermined cycle as a command value for rotating the motor;
The reference determining unit
If the actual rotation angle of the motor is delayed from the previous reference rotation angle determined by the reference determination unit and the amount of delay is smaller than a predetermined threshold, the angle corresponding to the rotation speed of the motor Determining the current reference rotation angle by adding the increment to the previous reference rotation angle;
When the actual rotation angle is delayed from the immediately preceding reference rotation angle and the delay amount is larger than the threshold, the immediately preceding reference rotation angle is determined as the current reference rotation angle;
Motor control device. The reference determining unit
If the actual rotation angle of the motor is ahead of the previous reference rotation angle, the current reference rotation angle is determined by adding the angle increment to the actual rotation angle;
The motor control device according to claim 1. The reference determining unit
If the immediately preceding reference rotation angle has reached the target rotation angle of the motor, the target rotation angle is determined as the current reference rotation angle;
The motor control device according to claim 1. The reference determining unit determines a reference rotational speed of the motor as a command value for rotating the motor at the predetermined period,
The angle increment is obtained by multiplying the current reference rotation speed determined by the reference determination unit by the predetermined period.
The motor control device according to any one of claims 1 to 3. The reference determining unit determines a reference rotational speed of the motor as a command value for rotating the motor at the predetermined period,
The reference determining unit
When the reference rotation speed immediately before the motor determined by the reference determination unit is smaller than the target rotation speed, the current reference rotation speed is determined by adding a speed increment to the reference rotation speed immediately before the motor. And
When the immediately preceding reference rotation speed is equal to or higher than the target rotation speed, the target rotation speed is determined as the current reference rotation speed;
The motor control device according to any one of claims 1 to 4. The reference determining unit
After the actual rotation angle reaches a predetermined deceleration start angle, if the immediately preceding reference rotation speed determined by the reference determining unit is greater than a predetermined low rotation speed threshold, the immediately preceding reference rotation speed is Determine the current reference rotational speed by subtracting the speed decrement;
After the actual rotation angle reaches a predetermined deceleration start angle, if the immediately preceding reference rotation speed is equal to or lower than the low rotation speed threshold, the low rotation speed is determined as the current reference rotation speed.
The motor control device according to claim 5. The reference determining unit
After the previous reference rotation angle reaches the target rotation angle of the motor, the current reference rotation speed is determined to be zero.
The motor control device according to claim 5 or 6. The motor control according to any one of claims 4 to 7, wherein the reference determination unit outputs the current reference rotation speed determined by the reference determination unit as a command value for rotating the motor. apparatus. The motor control according to any one of claims 1 to 8, wherein the reference determination unit outputs the current reference rotation angle determined by the reference determination unit as a command value for rotating the motor. apparatus. At least one motor;
The control device according to any one of claims 1 to 9, attached to the at least one motor;
A mechanical element driven by the at least one motor;
Robot with Obtaining the actual rotation angle of the motor at a predetermined period;
Determining a reference rotation angle of the motor at the predetermined period as a command value for rotating the motor;
In determining the reference rotation angle,
When the actual rotation angle of the motor is delayed from the previously determined reference rotation angle and the amount of delay is smaller than a predetermined threshold, the angle increment corresponding to the rotation speed of the motor is set to the immediately preceding To determine the current reference rotation angle by adding to the reference rotation angle of
When the actual rotation angle is delayed from the immediately preceding reference rotation angle and the delay amount is larger than the threshold, the immediately preceding reference rotation angle is determined as the current reference rotation angle;
Motor control method.

Images