JP4588367B2 - Position control device - Google Patents

Description

  The present invention relates to a position control device that causes a real position of a moving body that uses a motor as a drive source to follow a target position, and in particular, calculates a command value of the output torque of the motor by distributed processing using a plurality of CPUs. The present invention relates to a position control device that performs control.

  For example, position control using PID control is widely adopted in machine tools that perform machining by moving a machining tool along a preset trajectory. According to such PID control, the position control performance can be improved by appropriately setting various parameters such as differential compensation and gain setting according to the characteristics of the machine tool.

  However, the setting of various parameters in PID control is performed by actual adjustment using an actual machine tool, and it is not easy to set parameters appropriately. In addition, the operating characteristics of the machine tool change due to various factors such as disturbances such as machine vibration and friction, individual differences of machine tools such as the magnitude of backlash, and aging. For this reason, even if an appropriate parameter is set once, the parameter may become inappropriate due to these factors, and accurate position control may be difficult, or the machine tool may be excited when moving the processing tool. is there.

  Therefore, in order to ensure highly accurate position control performance even when the above-mentioned various factors change, position control is performed using a control method based on so-called modern control theory such as adaptive control or robust control. It is possible to do it. However, when these control methods based on modern control theory are used, the calculation processing in the position control is complicated and the calculation amount is dramatically increased as compared with the case of using PID control.

Therefore, in order to perform position control at high speed, it is necessary to prepare an arithmetic unit having high-speed arithmetic processing capability. As such an arithmetic unit, a machine tool has been proposed in which distributed processing is performed on NC data by two CPUs (see, for example, Patent Document 1).
JP 2003-316408 A

  According to the machine tool that performs the distributed processing by the two CPUs described above, it is possible to increase the speed of the position control calculation processing. However, when the amount of calculation in the position control process increases as in the case of applying a control method based on modern control theory, the voltage corresponding to the target position is actually output to the motor after the target position is given. It takes longer time. For this reason, the control cycle in the position control becomes longer, resulting in inconvenience that the responsiveness of the position control is deteriorated.

  Therefore, an object of the present invention is to provide a position control device capable of shortening the control cycle when performing position control calculation processing by distributed processing using a plurality of CPUs.

The present invention has been made to achieve the above object, and includes a target position acquisition unit that acquires a target position of a moving body that moves by a driving force transmitted from the motor through a transmission mechanism using the motor as a driving source. Real position grasping means for grasping the actual position of the moving body, and executing a predetermined torque command calculation process for each predetermined control period so that the actual position of the moving body follows the target position. The present invention relates to an improvement of a position control device including torque command calculation means for calculating a torque command value that is a target value of output torque.

In the first aspect of the position control device of the present invention, the torque command calculation means is a storage means, and for each control cycle, inputs a position of the moving body and outputs a torque of the motor. The output value of the system reverse transfer function when the target position of the moving body acquired by the target position acquisition unit is input to the system reverse transfer function is input to the response model corresponding to the response characteristics of the transfer mechanism Calculating a first torque value output from the response model, and calculating a first target position output from the response model when the target position is input to the response model; A first sub CPU that stores a torque value of 1 and the first target position in the storage means; and the actual position of the moving body that is grasped by the actual position grasping means for each control cycle Biography And calculates the second torque value corresponding to the difference between the torque value output from the system inverse transfer function when entered in the function and the torque command value, storing the torque value of the second in said storage means the the second sub-CPU, for each of the control cycle, the first of said by the sub CPU first torque value and the first and the second torque value according to the second sub-CPU and the calculation of the target position Prior to the calculation of PID control, PID control is applied to the difference value between the actual position of the moving body in the previous control cycle grasped by the actual position grasping means and the target position of the moving body in the previous control cycle. Main CPU for calculating the torque command value based on the third torque value calculated in step S3 and the first torque value and the second torque value in the previous control cycle stored in the storage means. And with It is characterized in.

According to the present invention, the main CPU calculates the first torque value and the first target position by the first sub CPU and the first sub CPU by the second sub CPU in each control cycle . Prior to the calculation of the second torque value, the first torque value, the first target position, and the second torque value in the previous control cycle stored in the storage means are used. Calculate the torque command value. Therefore, the first sub CPU outputs the torque command value to the motor drive device, and the first sub CPU moves the moving body to a target position by the motor . The target position calculation process can be executed, and the second sub CPU can execute the second torque value calculation process. In this case, the moving time of the moving body and the calculation time of the first torque value, the first target value, and the second torque value can be overlapped. Thereby, the said control period can be shortened and the responsiveness of the position control of the said mobile body can be improved.

Further , according to the present invention, the first torque value calculated by the first sub-CPU is a system transfer function corresponding to the system inverse transfer function with respect to a response characteristic in the controlled object including the transfer mechanism. The second torque value calculated by the second sub CPU reflects a disturbance applied to the controlled object. Therefore, the main CPU calculates the torque command value based on the third torque value, the first torque value, and the second torque value by PID control, thereby controlling the position of the moving body. Is matched with the response characteristic required by the system inverse transfer function and the influence of the disturbance is suppressed, so that the position control of the moving body can be stabilized.

In the second aspect of the position control apparatus of the present invention, the torque command calculation means has a storage means and a predetermined output that outputs the torque of the motor by inputting the position of the moving body for each control cycle. The output value of the system reverse transfer function when the target position of the moving body acquired by the target position acquisition unit is input to the system reverse transfer function is input to the response model corresponding to the response characteristics of the transfer mechanism A first sub-unit that calculates a first torque value that is sometimes output from the response model and calculates a first target position that is output from the response model when the target position is input to the response model. a CPU, for each of the control cycle, the torque value output the actual position of the movable body which is grasped by the actual position detection means from the system inverse transfer function when entered in the system inverse transfer function Wherein the second sub-CPU for calculating a second torque value according to the difference between the torque command value, for each of the control period, the control object model of the transmission mechanism, the actual position of the movable body An H _∞ controller that inputs a deviation from the first target position as an observation amount and outputs the torque of the motor as an operation amount is input to the actual position of the moving body grasped by the actual position grasping means and the first position. A third sub CPU that calculates a third torque value output from the H _∞ controller when a deviation from a target position is input, and the first sub CPU by the first sub CPU for each control period. Preceding the calculation of the torque value of 1 and the first target position, the calculation of the second torque value by the second sub CPU, and the calculation of the third torque value by the third CPU. 2 cycles before stored in the storage means The first torque value in the control cycle, the second torque value calculated by the second sub CPU in the control cycle one cycle before, and the third sub CPU in the control cycle one cycle before And a main CPU that calculates the torque command value based on the calculated third torque value.

According to the present invention, the main CPU calculates the first torque value and the first target position by the first sub CPU and the first sub CPU by the second sub CPU in each control cycle. Prior to the calculation of the second torque value and the calculation of the third torque value by the third sub CPU, the first torque value in the previous control cycle stored in the storage means and the The torque command value is calculated using the first target position, the second torque value, and the third torque value.
Therefore, the first sub CPU outputs the torque command value to the motor drive device, and the first sub CPU moves the moving body to a target position by the motor. A target position calculation process can be executed, the second sub CPU can execute the second torque value calculation process, and the third sub CPU can calculate the third torque value. Processing can be executed.
In this case, the moving time of the moving body overlaps the calculation time of the first torque value, the first target value, the second torque value, and the third torque value. Can do. Thereby, the said control period can be shortened and the responsiveness of the position control of the said mobile body can be improved.
Further, according to the present invention, the first torque value calculated by the first sub-CPU is a system transfer function corresponding to the system inverse transfer function with respect to a response characteristic in the controlled object including the transfer mechanism. The second torque value calculated by the second sub CPU reflects a disturbance applied to the controlled object. In addition, the third torque value calculated by the third sub CPU stabilizes the position control of the moving body against fluctuations in deviation between the first target position and the actual position of the moving body. It will be made to. For this reason, the main CPU calculates the torque command value based on the first torque value, the second torque value, and the third torque value, so that the response characteristic in the position control of the moving body is obtained. Can be matched with the response characteristic required by the system inverse transfer function, the influence of disturbance can be suppressed, the robustness can be improved, and the position control of the moving body can be stabilized.
Furthermore, the calculation of the torque command value in each control cycle, the first torque value calculated in the control cycle two cycles before, the second torque value calculated in the control cycle one cycle before, From the calculation time point of the first torque value, the second torque value, and the third torque value by calculating using the third torque value calculated in the control cycle one cycle before, The delay time until the torque command value is calculated can be shortened, and the position control of the moving body can be performed with high accuracy.

Embodiments of the present invention will be described with reference to FIGS. FIG. 1 is an overall configuration diagram of a position control device according to the present invention, FIG. 2 is a calculation block diagram of a sub CPU shown in FIG. 1, FIG. 3 is a flowchart for calculating a torque command value using PID control, and FIG. FIG. 5 is a flowchart for calculating the torque command value using the _H∞ control.

  Referring to FIG. 1, the position control device 1 of the present invention obtains the actual position Fb_Pos of a machining tool (not shown, corresponding to the moving body of the present invention) chucked by a machine tool 20 from the NC control device 2. Position control is performed to follow the transmitted target position Pln_Pos, and the servo control unit 1a and the arithmetic unit 1b are used.

  The servo control unit 1 a includes a main CPU 10, an NC interface 11, and a servo interface 12. The servo control unit 1 a is communicably connected to the NC control device 2 via the NC interface 11 and communicates with the servo driver 3 via the servo interface 12. Connected as possible.

  The servo driver 3 is connected to the servo motor 21 that moves the machining tool via the transmission mechanism and the encoder 22 that is mounted on the rotation shaft of the servo motor 21, and the torque that is output from the servo motor 21 is transmitted from the control unit 1a. The electric power supplied to the servo motor 21 is controlled so as to coincide with the command value Cmd_Trq. Further, the servo driver 3 grasps the actual position Fb_Pos of the machining tool based on the pulse signal output from the encoder 22 according to the rotation of the servo motor 21, and transmits the data of the actual position Fb_Pos to the servo unit 1a. .

  The main CPU 10 then issues a torque command so that the actual position Fb_Pos of the machining tool received from the servo driver 3 via the servo interface 12 follows the target position Pln_Pos received from the NC control device 2 via the NC interface 11. The value Cmd_Trq is determined.

The configuration in which the main CPU 10 acquires the processing tool target position Pln_Pos via the NC interface 11 corresponds to the target position acquisition means of the present invention. The configuration in which the main CPU 10 grasps the actual position Fb_Pos of the machining tool via the servo interface 12 corresponds to the actual position grasping means of the present invention.

  Next, the first sub CPU 13, the second sub CPU 14, and the third sub CPU 15 included in the arithmetic unit 1b calculate parameters used for calculating the torque command value Cmd_Trq in the main CPU 10 by distributed processing. It writes and reads data shared with the main CPU 10 via the QP-RAM.

  FIG. 2A shows a model matching calculation process in the first sub CPU 13. The first sub CPU 13 inputs the target position Pln_Pos read from the QP-RAM 16 to the system reverse transfer function 40 corresponding to the response characteristic set for the machine tool 20, and outputs the output of the system reverse transfer function 40 to the machine tool. The output of the response model 41 when input to the response model 41 set according to the mechanical characteristics of the machine 20 is calculated as a model matching torque M_Trq (corresponding to the first torque value of the present invention). Further, the first sub CPU 13 calculates an output when the target position Pln_Pos is input to the response model as a model matching position M_Pos (corresponding to the first target position of the present invention). Then, the first sub CPU 13 writes the calculated model matching torque M_Trq and the model matching position M_Pos in the QP-RAM 16.

  FIG. 2B shows a disturbance observer calculation process in the second sub CPU 14. The second sub CPU 14 uses the subtractor 42 to input the actual position Fb_Pos read from the QP-RAM 16 from the torque command value Cmd_Trq read from the QP-RAM 16 to the system reverse transfer function 40. Is output to calculate the deviation ΔTrq. Then, the second sub CPU 14 performs a filtering process by the filter 43 on the deviation ΔTrq to reduce the disturbance observer torque Ob_Trq (to the second torque value of the present invention) for suppressing the influence of the disturbance applied to the machine tool 20. And the disturbance observer torque Ob_Trq is written into the QP-RAM 16.

FIG. 2C shows the processing of the H _∞ control calculation in the third sub CPU 15. The third sub CPU 15 uses the subtractor 50 from the model matching position M_Pos read from the QP-RAM 16. Then, the actual position Fb_Pos read from the QP-RAM 16 is subtracted to calculate the position deviation ΔPos. Then, the third sub CPU 15 inputs the position deviation ΔPos to the _H∞ controller 51 that inputs the deviation between the actual position of the machining tool and the target position as an observation amount and outputs the torque of the servo motor 21 as an operation amount. and to calculate the output of the H _∞ controller 51 as H _∞ torque H _∞ _Trq (corresponding to the third torque value of the present invention). The third sub CPU 15 writes the calculated H _∞ torque H _∞ _Trq in the QP-RAM 16.

  Next, a first embodiment of the position control device 1 will be described according to the flowchart shown in FIG. In the first embodiment, the position control device 1 calculates the torque command value Cmd_Trq using PID control. The main CPU 10 executes the process shown in FIG. 3A every predetermined control cycle. Further, the first sub CPU 13 executes the process shown in FIG. 3B for each control cycle, and the second sub CPU 14 executes the process shown in FIG. 3C for each control cycle. To do.

  In the first embodiment, the main CPU 10, the first sub CPU 13, and the second sub CPU 14 constitute the torque command calculation means of the present invention, and the third sub CPU 15 is not used.

  Also, (N-1) and (N) in the flowcharts shown in FIGS. 3 (a), 3 (b), and 3 (c) are acquired or calculated in the (N-1) th and Nth control cycles, respectively. Indicates the variable value. The flowcharts shown in FIGS. 3A, 3B, and 3C show processing in the Nth control cycle.

  First, the main CPU 10 calculates the model calculated by the first sub CPU 13 and written in the QP-RAM 16 in the previous control cycle (N-1th control cycle) in STEP 1 of FIG. The matching position M_Pos (N-1) and the model matching torque M_Trq (N-1), the disturbance observer torque Ob_Trq (N-1) calculated by the second sub CPU 14 and written in the QP-RAM 16, and the main CPU 10 The PID command torque PID_Trq (N−1) calculated and written in the QP-RAM 16 is read from the QP-RAM 16.

Then, in the next STEP2, the main CPU 10 calculates the torque command value Cmd_Trq (N) in the current control cycle (Nth control cycle) by the following equation (1). Cmd_Trq (N) = M_Trq (N− 1) + Ob_Trq (N-1) + PID_Trq (N-1) (1)
In subsequent STEP 3, the main CPU 10 transmits a torque command value Cmd_Trq (N) to the servo driver 3 via the servo interface 12. Then, the servo driver 3 that has received the torque command value Cmd_Trq (N) controls the power supplied to the servomotor 21 so that a torque corresponding to the torque command value Cmd_Trq (N) is obtained.

  In the next STEP 4, the main CPU 10 receives the target position Pln_Pos (N) from the NC control device 2 via the NC interface 11. In STEP 5, the main CPU 10 obtains the actual position Fb_Pos (N) from the servo driver 3 via the servo interface 12. Receive. In subsequent STEP 6, the main CPU 10 writes the target position Pln_Pos (N), the actual position Fb_Pos (N), and the torque command value Cmd_Trq (N) in the current control cycle to the QP-RAM 16.

  In the next STEP7, the main CPU 10 performs PID control processing for the deviation between the target position Pln_Pos (N) and the actual position Fb_Pos (N) to calculate the PID command torque PID_Trq (N). In STEP8, the PID command torque PID_Trq (N) is written into the QP-RAM 16. In subsequent STEP 9, the main CPU 10 performs system processing (execution of program runaway detection, power failure detection, etc. by a watchdog timer) and ends the current control cycle.

  Next, referring to FIG. 3B, when the Nth control cycle is started, the first sub CPU 13 writes the target position Pln_Pos (N) in the QP-RAM 16 by the main CPU 10 in STEP10. Wait for Then, when the target position Pln_Pos (N) is written in the QP-RAM 16 (when the processing of STEP 6 in FIG. 3A is completed), the process proceeds to STEP 11, and the first sub CPU 13 proceeds to FIG. The model matching calculation shown is performed to calculate the model matching torque M_Trq (N) and the model matching position M_Pos (N). In subsequent STEP 13, the first sub CPU 13 writes the calculated model matching torque M_Trq (N) and model matching position M_Pos (N) in the QP-RAM 16.

  Next, referring to FIG. 3C, when the N-th control cycle is started, the second sub CPU 14 determines the actual position Fb_Pos (N) and the torque command value Cmd_Trq (N) in STEP 20 by the main CPU 10. ) Is written to the QP-RAM 16. Then, when the actual position Fb_Pos (N) and the torque command value Cmd_Trq (N) are written in the QP-RAM 16 (when the processing of STEP 6 in FIG. 3A is completed), the process proceeds to STEP 21, and the second sub CPU 14 Performs the disturbance observer calculation shown in FIG. 2B to calculate the disturbance observer torque Ob_Trq (N). In subsequent STEP 23, the second sub CPU 14 writes the calculated disturbance observer torque Ob_Trq (N) in the QP-RAM 16.

FIG. 4 shows the operation timings of the main CPU 10, the first sub CPU 13 and the second sub CPU 14 when the flowcharts of FIGS. 3A, 3B and 3C described above are executed. In the timing chart shown, the horizontal axis is set to time t. Figure t ₁₀ ~t ₁₃ and t ₁₃ ~t ₁₆ is one control period.

When the control cycle at t ₁₀ is started, the main CPU10 calculates the torque command value Cmd_Trq (STEP2), terminates the transmission of the torque command value Cmd_Trq to the servo driver 3 at t ₁₁ (STEP3). Thereby, the drive of the servomotor 21 by the servo driver 3 is started.

Then, in a subsequent t _12, the data of the QP-RAM 16 by the main CPU10 when a writing (target position Pln_Pos, actual position Fb_Pos, data of the torque command value Cmd_Trq) ends, the first sub-CPU13 are QP-RAM 16 The data (target position Pln_Pos) is read from (Step 11), and the second sub CPU 14 reads the data (actual position Fb_Pos, torque command value Cmd_Trq) from the QP-RAM 16 (STEP 21).

Then, the first sub CPU 13 calculates the model matching position M_Pos and the model matching torque M_Trq (STEP 12), and the second sub CPU 14 calculates the disturbance observer torque Ob_Trq (STEP 22). In this case, the torque command value Cmd_Trq to the servo driver 3 is transmitted at t ₁₁ while the servo motor 21 is operating, by the t ₁₂ calculation processing by the first sub CPU13 and (STEP 12) the second sub CPU14 The calculation process (STEP 22) can be started. Therefore, it is possible to shorten the control cycle and improve the responsiveness of the position control.

Next, a second embodiment will be described according to the flowchart shown in FIG. In the second embodiment, the position control device 1 calculates the torque command value Cmd_Trq for the servomotor 21 using _H∞ control. Then, the arithmetic processing of the _H∞ control is executed by the third sub CPU 15 provided in the arithmetic unit 1b.

In the second embodiment, the main CPU 10, the first sub CPU 13, the second sub CPU 14, and the third sub CPU 15 constitute the torque command calculation means of the present invention.

  Also, (N-2), (N-1), and (N) in the flowcharts shown in FIGS. 5 (a) and 5 (b) are N-2th, N-1th, and Nth, respectively. The variable value acquired or calculated in the control cycle is shown. And the flowchart shown to Fig.5 (a) and FIG.5 (b) has shown the process in a Nth control period. Further, the first sub CPU 13 and the second sub CPU 14 execute the processes shown in the flowcharts of FIGS. 3A and 3B, respectively, as in the first embodiment.

First, the main CPU 10 calculates the model matching calculated by the first sub CPU 13 and written in the QP-RAM 16 in STEP 1 of FIG. 5A in the previous control cycle (N−1th control cycle). The model matching torque M_Trq (N-2) calculated by the first sub CPU 13 and written to the QP-RAM 16 in the position M_Pos (N-1) and the control cycle (N-2th control cycle) two times before. ) And the disturbance observer torque Ob_Trq (N-1) calculated by the second sub CPU 14 and written in the QP-RAM 16 in the previous control cycle (N-1th control cycle) in the control period of the (N-1 th control period), the third is calculated by the sub CPU15 and QP-RAM 16 with the written H _∞ torque H _∞ _Trq and (N), read from the QP-RAM 16 It is.

  Then, in the next STEP 51, the main CPU 10 calculates the torque command value Cmd_Trq (N) in the current control cycle (Nth control cycle) by the following equation (2).

Cmd_Trq (N) = M_Trq (N -2) + Ob_Trq (N-1) + H ∞ _Trq (N-1) ····· (2)
In subsequent STEP 52, the main CPU 10 transmits a torque command value Cmd_Trq (N) to the servo driver 3 via the servo interface 12. Then, the servo driver 3 that has received the torque command value Cmd_Trq (N) controls the power supplied to the servomotor 21 so that a torque corresponding to the torque command value Cmd_Trq (N) is obtained.

  At the next STEP 53, the main CPU 10 receives the target position Pln (N) from the NC control device 2 via the NC interface 11. At STEP 54, the main CPU 10 obtains the actual position Fb_Pos (N) from the servo driver 3 via the servo interface 12. Receive. In the subsequent STEP 55, the main CPU 10 writes the target position Pln_Pos (N), the actual position Fb_Pos (N), the torque command value Cmd_Trq (N), and the model matching position M_Pos (N-1) in the QP-RAM 16. In STEP 56, system processing (execution of program runaway detection, power failure detection, etc. by the watch dog timer is executed) is completed, and the current control cycle is terminated.

Next, referring to FIG. 5B, when the N-th control cycle is started, the third sub CPU 15 causes the main CPU 10 to execute the actual position Fb_Pos (N) and the model matching position M_Pos (N ) in STEP60. -1) is written to the QP-RAM 16. Then, when the actual position Fb_Pos (N) and the model matching position M_Pos (N-1) are written in the QP-RAM 16 (when the processing of STEP55 in FIG. 5A is completed), the process proceeds to STEP61. Is read. In STEP 62 , the third sub CPU 15 performs H∞ control calculation shown in FIG. 2C to calculate H∞ torque H∞_Trq (N). In subsequent STEP 63, the third sub CPU 15 writes the calculated H∞ torque H∞_Trq (N) in the QP-RAM 16.

Book case of the second embodiment, the third sub CPU15 calculates the H _∞ torque H _∞ _Trq, the main CPU 10, the torque command by using the H _∞ torque H _∞ _Trq by the above formula (2) By calculating the value Cmd_Trq (N), it is possible to improve the robustness of the position control device 1 and realize accurate position control.

  Also in the second embodiment, as in the first embodiment described above, the main CPU 10 transmits the torque command value Cmd_Trq (N) in STEP 52 of FIG. After the operation is started, calculation processing by the first sub CPU 13, the second sub CPU 14, and the third sub CPU 15 (STEP 12 in FIG. 3B, STEP 22 in FIG. 3C, STEP 62 in FIG. 5B). ) Can begin. Therefore, the control cycle of the position control device 1 can be shortened.

  In the present embodiment, the moving body of the present invention has been shown to control the position of a machining tool chucked on a machine tool. However, a variable value in position control is calculated by distributed processing using a plurality of CPUs. The present invention can be applied to any position control device.

In the present embodiment, as an example of executing the model matching calculation, the disturbance observer calculation, and the _H∞ control calculation as the distributed processing by the sub CPU, the calculation to be the target of the distributed processing is not limited to these. The present invention can also be applied when other types of calculation processes are executed by the sub CPU.

The whole block diagram of a position control apparatus. FIG. 2 is a calculation block diagram of a sub CPU shown in FIG. 1. The flowchart in the case of calculating a torque command value using PID control. The timing chart of main CPU and sub CPU. The flowchart in the case of calculating a torque designation value using _H∞ control.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Position control apparatus, 1a ... Servo unit, 1b ... Arithmetic unit, 2 ... NC control apparatus, 3 ... Servo driver, 10 ... Main CPU, 11 ... NC interface, 12 ... Servo interface, 13 ... 1st sub CPU, 14 ... 2nd sub CPU, 15 ... 3rd sub CPU, 21 ... Servo motor, 22 ... Encoder

Claims (2)

Target position acquisition means for acquiring a target position of a moving body that moves by a driving force transmitted from the motor through a transmission mechanism using a motor as a drive source; and an actual position determination means for determining the actual position of the moving body; Torque command calculation that calculates a torque command value that is a target value of the output torque of the motor by executing a predetermined torque command calculation process every predetermined control period so that the actual position of the moving body follows the target position A position control device comprising:
The torque command calculation means includes
Storage means;
When the target position of the moving body acquired by the target position acquisition means is input to a predetermined system reverse transfer function that inputs the position of the moving body and outputs the torque of the motor for each control cycle. When the output value of the system inverse transfer function is input to a response model corresponding to the response characteristic of the transmission mechanism, a first torque value output from the response model is calculated, and the target position is set to the response model. A first sub CPU that calculates a first target position that is output from the response model when stored in the storage model, and stores the first torque value and the first target position in the storage unit;
For each control cycle, when the actual position of the moving body grasped by the actual position grasping means is input to the system reverse transfer function, the torque value output from the system reverse transfer function and the torque command value A second sub CPU that calculates a second torque value according to the difference and stores the second torque value in the storage means;
For each of the control period, prior to the calculation of the second torque value according to the first said by the sub CPU first torque value and the first and calculation of the target position the second sub CPU, third torque value calculated by applying the PID control on the difference value between the target position of the movable body in the real position and the previous control cycle of the mobile in the previous control cycle, which is grasped by the actual position detection means And a main CPU that calculates the torque command value based on the first torque value and the second torque value in the previous control cycle stored in the storage means. Position control device. Target position acquisition means for acquiring a target position of a moving body that moves by a driving force transmitted from the motor through a transmission mechanism using a motor as a drive source; and an actual position determination means for determining the actual position of the moving body; Torque command calculation that calculates a torque command value that is a target value of the output torque of the motor by executing a predetermined torque command calculation process every predetermined control period so that the actual position of the moving body follows the target position A position control device comprising:
The torque command calculation means includes
Storage means;
When the target position of the moving body acquired by the target position acquisition means is input to a predetermined system reverse transfer function that inputs the position of the moving body and outputs the torque of the motor for each control cycle. When the output value of the system inverse transfer function is input to a response model corresponding to the response characteristic of the transmission mechanism, a first torque value output from the response model is calculated, and the target position is set to the response model. A first sub CPU that calculates a first target position that is output from the response model when it is input to the storage model, and stores the first torque value and the first target position in the storage unit;
For each control cycle, when the actual position of the moving body grasped by the actual position grasping means is input to the system reverse transfer function, the torque value output from the system reverse transfer function and the torque command value A second sub CPU that calculates a second torque value according to the difference and stores the second torque value in the storage means;
For each control cycle, the deviation between the actual position of the moving body and the first target position is input as an observation amount and the motor torque is output as an operation amount for the control target model of the transmission mechanism. to _∞ controller, wherein when the input a deviation between said first target position and the actual position of the grasping by said moving member by the actual position grasping means, the third torque value output from the H _∞ controller And a third sub CPU for storing the third torque value in the storage means;
For each control period, the first torque value and the first target position are calculated by the first sub CPU, the second torque value is calculated by the second sub CPU, and the third Prior to the calculation of the third torque value by the CPU, the first torque value in the control cycle two cycles before stored in the storage means and the second torque in the control cycle one cycle before The torque command value is calculated based on the second torque value calculated by the sub CPU and the third torque value calculated by the third sub CPU in the control cycle one cycle before. A position control device comprising a CPU.

Images