JP2014119903A - Control device, control program, and control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a novel control device and control program and a control method thereof which allow a moving object to be controlled along an intended trajectory even in the case of the moving object having low rigidity.SOLUTION: The control device is provided which moves a control object portion of a moving object along a prescribed trajectory. The control device includes: calculation means which acquires a state value of the control object portion and calculates an estimation amount from the acquired state value of the control object portion; generation means which generates a movement instruction to the moving object on the basis of a controlled variable about the control object portion and the compensation amount; and correction means which corrects the compensation amount calculated by the calculation means, on the basis of a feature quantity generated in the controlled variable.

Description

  The present invention relates to a control device, a control program, and a control method for moving a control target part of a moving body along a predetermined trajectory.

  A control device such as a programmable logic controller (hereinafter referred to as “PLC”) is used for controlling machines and equipment. A drive mechanism such as a servo motor is controlled by such a control device, and the moving body performs various operations according to the movement of the drive mechanism. If such a moving body has a sufficiently high rigidity and the driving mechanism can be operated, the moving body can be regarded as an integral part. The above problems can occur.

  For example, a configuration is assumed in which a table is provided with a table on which an object is mounted and a wheel is provided at the bottom, and the wheel is driven. In this case, it is originally necessary to move the table to a target position, but the positional relationship between the carriage and the table may change with the movement. As a result, even if only the position of the carriage is controlled along the target trajectory, the table position may deviate from the original target trajectory. The following prior arts are known for control methods such as when the rigidity of the moving body is low.

  Japanese Patent Application Laid-Open No. 2004-005469 (Patent Document 1) reduces vibration generated due to low rigidity of a moving body, ensures vibration damping performance regardless of the characteristics of a command pattern and a controlled object, and realizes high positioning accuracy. An electric motor control method is disclosed.

  Japanese Patent Laid-Open No. 11-301815 (Patent Document 2) discloses a speed / position control method for a stacker crane that can cause the stacker crane to travel to a target position in a short time with less vibration of the vertical frame.

  Japanese Patent Laid-Open No. 11-155295 (Patent Document 3) discloses a vibration suppression control device that can be realized without changing the characteristics of the phase adjuster and is not affected by the speed control compensator.

JP 2004-005469 A Japanese Patent Laid-Open No. 11-301815 JP-A-11-155295

  An object of the present invention is to provide a novel control device, a control program, and a control method thereof that can perform control along an intended trajectory even for a mobile body having low rigidity.

  According to an aspect of the present invention, a control device for moving a control target part of a moving body along a predetermined trajectory is provided. The control device acquires the state value of the control target part, calculates the compensation amount from the acquired state value of the control target part, and based on the control amount and the compensation amount for the control target part, Generating means for generating a movement command for the control means, and correcting means for correcting the compensation amount calculated by the calculating means based on the feature amount generated in the control amount.

  Preferably, the correction unit corrects the compensation amount so as to cancel the forced vibration component included in the control amount.

  Preferably, the correction unit suppresses the compensation amount in a section in which the control amount is corrected corresponding to the vibration generated in the control target part.

  Alternatively, preferably, the correction unit adjusts the compensation amount to be actually applied by changing the gain.

  Preferably, the correction unit adjusts the compensation amount after matching the compensation amount and the output dimension of the correction unit.

  More preferably, the correction unit corrects the compensation amount based on at least one of speed, acceleration, and jerk caused by the control amount.

  If another situation of this invention is followed, the control program for moving the control object site | part of a moving body along a predetermined track | orbit will be provided. The control program acquires the state value of the control target part to the computer, calculates the compensation amount from the acquired state value of the control target part, and the control amount and the compensation amount for the control target part, A step of generating a movement command for the moving body and a step of correcting the compensation amount calculated in the calculating step based on the feature amount generated in the control amount are executed.

  According to still another aspect of the present invention, a control method for moving a control target part of a moving body along a predetermined trajectory is provided. The control method acquires the state value of the control target part, calculates a compensation amount from the acquired state value of the control target part, and the control amount and the compensation amount for the control target part. The method includes a step of generating a movement command and a step of correcting the compensation amount calculated in the calculating step based on a feature amount generated in the control amount.

  According to the present invention, even a moving body having low rigidity can be controlled along the intended trajectory.

It is a schematic diagram which shows the structure of the control system which concerns on this Embodiment. It is a schematic diagram which shows the structure of the control apparatus which concerns on this Embodiment. It is a schematic diagram for demonstrating the control logic which concerns on this Embodiment. It is a block diagram which shows the basic control structure which concerns on this Embodiment. It is a flowchart which shows the basic process sequence which concerns on this Embodiment. It is a control block diagram which shows the example 1 of mounting of the control system which concerns on embodiment of this invention. It is a figure which shows the example of a simulation result of the vibration suppression effect by the control logic shown in FIG. It is a control block diagram which shows the example 2 of mounting of the control system which concerns on embodiment of this invention. It is a figure for demonstrating the time change of the gain by the control logic shown in FIG. It is a figure which shows the example of a simulation result of the vibration suppression effect by the control logic shown in FIG.

  Embodiments of the present invention will be described in detail with reference to the drawings. In addition, about the same or equivalent part in a figure, the same code | symbol is attached | subjected and the description is not repeated.

<A. System configuration>
As an embodiment of the present invention, a control system for controlling a moving body in which distortion between a plurality of parts cannot be ignored is considered.

  FIG. 1 is a schematic diagram showing a configuration of a control system 1 according to the present embodiment. Referring to FIG. 1, a control system 1 according to the present embodiment is directed to carry in / out goods in a factory or a distribution warehouse. More specifically, the control system 1 includes a stacker crane 100 that is a moving body and an arrangement rack 150. The stacker crane 100 is configured to take out an article at a position on the placement rack 150 and move it to another position, take out an article at a position on the placement rack 150 and carry it out to another process, or from other processes. A process of placing the received article at a designated position on the placement rack 150 is executed.

  As an operation of the stacker crane 100, the stacker crane 100 moves along a rail 120 (the extension direction of the rail 120 is referred to as “Y-axis direction” for convenience of description). The rail 120 does not need to be laid on a straight line, and is laid in an arbitrary shape. Further, the stacker crane 100 has a table 106 that moves in the vertical direction (referred to as “X-axis direction” for convenience of explanation). The stacker crane 100 accesses the target position of the placement rack 150 by combining the movement of the table 106 in the X-axis direction and the movement along the Y-axis direction. The stacker crane 100 includes a carriage unit 102 on which wheels are arranged, and a main body unit 104 for moving the table 106 in the X-axis direction.

  In the control system 1 shown in FIG. 1, the stacker crane 100 is a moving body. A control device to be described later performs control for moving a table 106 (hereinafter, also referred to as “control target part”) of the stacker crane 100 that is a moving body along a predetermined trajectory. In other words, the control device gives a control command to wheels mounted on the carriage unit 102 (second part), a motor for driving the table 106, and the like so that the table 106 moves along the intended track.

<B. Configuration of control device>
Next, the configuration of the control device 200 in the control system 1 shown in FIG. 1 will be described. FIG. 2 is a schematic diagram showing the configuration of the control device 200 according to the present embodiment.

  Referring to FIG. 2, control device 200 calculates and calculates a control command for controlling the operation of stacker crane 100. Typically, control device 200 is realized by a programmable logic controller (hereinafter referred to as “PLC”) or the like. Of course, you may mount not using PLC but using a personal computer (PC) and various arithmetic processing units.

  The stacker crane 100 further includes a motor 108 for driving wheels disposed on the carriage unit 102 and a motor 110 for driving the table 106 in the vertical direction. The motor 108 incorporates an encoder for feeding back its rotational position. Motors 108 and 110 are driven by pulse signals from servo drivers 18 and 20, respectively. The servo drivers 18 and 20 output designated pulse signals in accordance with instructions from the control device 200.

  Control device 200 according to the present embodiment can be realized as a so-called sensorless configuration (a configuration in which a detection unit that detects shaking of cart unit 102 is not mounted). However, a detection unit for detecting the behavior of the carriage unit 102 may be provided, and control may be performed based on the detection result from the detection unit. FIG. 2 shows a configuration having a strain sensor 112 for measuring the twist generated in the main body 104 as an option.

  The control device 200 includes a CPU (Central Processing Unit) unit 10 that performs main arithmetic processing, an A / D (Analog to Digital) unit 12 that acquires measurement results of the strain sensor 112, and an encoder mounted on the motor 108. A pulse counter unit 14 for detecting and counting a pulse signal to be output and a field bus interface (I / F) unit 16 for communicating with a field device such as a servo driver 40 are included.

  The CPU unit 10 includes, as main components, a microprocessor, a volatile memory such as a RAM (Random Access Memory), and a nonvolatile memory such as an HDD (Hard Disk Drive) and a ROM (Read Only Memory). Typically, in the control device 200, processing as described below is realized by the CPU unit 10 executing a program. However, all or part of the processing realized by the program may be realized by dedicated hardware.

  The A / D unit 12 acquires an output value (analog value) from the strain sensor 112 and converts it into a digital value that can be processed by the CPU unit 10.

  The pulse counter unit 14 receives and counts a pulse signal from an encoder built in the motor 108. This count value is output to the CPU unit 10. In the configuration example shown in FIG. 1, information about the encoder built in the motor 108 may be transmitted through a network such as a field bus.

  The fieldbus interface (I / F) unit 16 is connected to servo drivers 18 and 20 via a network. As the field network, various industrial Ethernets (registered trademark) such as EtherCAT (registered trademark), Profinet IRT, MECHATRLINK (registered trademark) -III, Powerlink, SERCOS (registered trademark) -III, and CIP Motion can be used. Alternatively, a proprietary network such as DeviceNet or CompoNet / IP (registered trademark) may be used.

  The control device 200 may include an input / output unit and a special unit (not shown). These units are configured to exchange data with each other via a PLC system bus (not shown).

  The control device 200 according to the present embodiment can increase the accuracy of control of the stacker crane 100 by adopting a control logic as will be described later. Details of this control logic will be described below.

<C. Basic concept>
Next, the basic concept of the control logic according to the present embodiment will be described.

  In the present embodiment, control is not performed so that the carriage unit 102 on which the wheel is disposed moves along the intended trajectory, but the table 106 that is the control target portion moves along the intended trajectory. Control. That is, in the stacker crane 100, the carriage unit 102 and the table 106 cannot be regarded as moving together, and even if the carriage unit 102 is moved along the target trajectory that is the controlled variable, The tip may not actually be positioned at the target location. Therefore, in the present embodiment, whether or not the movement is along the target trajectory that is the control amount is evaluated not on the carriage unit 102 but on the tip of the table 106.

  FIG. 3 is a schematic diagram for explaining the control logic according to the present embodiment. Even if the carriage unit 102 is moved in accordance with the target trajectory due to a torsion due to insufficient rigidity between the carriage unit 102 and the table 106, the trajectory on which the table 106 moves is the original target trajectory (table target trajectory: Control amount). Therefore, in the present embodiment, the corrected target trajectory (which may have some offset with respect to the table target trajectory shown in FIG. 3) is taken into account such a deviation from the target trajectory that occurs in the control target portion. Is prepared in advance. That is, a target trajectory for the table 106 is set, and a target trajectory for the carriage unit 102 (hereinafter also referred to as “corrected target trajectory”) is calculated from the set target trajectory. Specifically, the control device 200 calculates a transfer function from the table 106 to the cart unit 102 and multiplies the transfer function by the target track to set a target track related to position control for the cart unit 102.

  Using such a corrected target trajectory, the stacker crane 100 as a moving body is controlled (position control and / or speed control). Such control is typically realized by a control structure as shown in FIG. FIG. 4 is a block diagram showing a basic control structure according to the present embodiment.

  Referring to FIG. 4, the control device 200 is a device for moving the table 106 (control target part) of the stacker crane 100 (moving body) along a predetermined trajectory. ) Correction unit 204, compensation unit 208, position controller 212, speed controller 214, torque controller 216, and feedback (FB) correction unit 218.

  First, the target trajectory for the table 106 is corrected by the adding unit 202 with the correction amount from the feedforward correction unit 204, and a corrected target trajectory is calculated. More specifically, the corrected target trajectory is calculated by multiplying the inverse model of the moving object by the target trajectory of the table 106.

  Subsequently, the compensation unit 208 calculates a compensation amount Δθ based on a deviation ε (also referred to as “table position deviation”) with respect to the table 106, and feeds back the compensation amount Δθ. This deviation ε corresponds to a deviation from the model of the moving body. As described above, the compensation unit 208 acquires the state value of the table 106 that is the control target part, and calculates the compensation amount from the acquired state value of the control target part.

  The table position deviation corresponds to residual vibration generated in the stacker crane 100 (table 106). This residual vibration is typically calculated by subtracting the table position from the target position.

  Specifically, the compensation amount Δθ is corrected by the adding unit 202 and set as a target value for the position controller 212. As described above, in FIG. 4, the compensation is performed in the preceding stage of the position controller 212, but feedback may be made to at least one of the position controller 212, the speed controller 214, and the torque controller 216. That is, the compensation amount is calculated based on the difference between the target trajectory and the corrected target trajectory. The calculation result is fed back to at least one of the position loop, the speed loop, and the torque loop.

  The position controller 212 calculates an output value (speed target value) based on the difference between the target position and the position of the carriage unit 102. The speed controller 214 calculates an output value (torque target value) according to the speed target value from the position controller 212. The torque controller 216 calculates an output value (drive command) according to the torque target value from the speed controller 214 and gives it to the carriage unit 102. These controllers generate a movement command for the moving body based on the target trajectory and the compensation amount for the control target part.

  The feedback correction unit 218 corrects the magnitude of the compensation amount Δθ when the compensation amount Δθ, which is a feedback amount, becomes a disturbance factor of the behavior of the moving body. The inventor of the present application has found a new problem that when the compensation amount Δθ is applied to the corrected target trajectory, the damping trajectory is disturbed by feedback and the damping effect is reduced. The reason for disturbing the vibration control trajectory in this way is to cancel all the forced vibration components generated by the corrected target trajectory by feeding back all the forced vibration components and free vibration components generated in the control target part without distinguishing them. This is thought to be because of this. Therefore, the feedback correction unit 218 corrects the compensation amount calculated by the compensation unit 208 serving as a calculation unit based on the feature amount generated in the target trajectory. Thereby, the disturbance component produced by the competition between the corrected target trajectory and the feedback control can be suppressed.

<D. Basic processing procedure>
Next, a basic processing procedure according to the present embodiment will be described. FIG. 5 is a flowchart showing a basic processing procedure according to the present embodiment. Each step of this flowchart is typically realized by the CPU unit 10 of the control device 200 executing a program.

  Referring to FIG. 5, CPU unit 10 of control device 200 first generates a target trajectory for table 106 (step S100). Typically, the target trajectory for each section is stored in a storage device inside or outside the CPU unit 10, and the CPU unit 10 reads the target trajectory to be processed from the storage device. Alternatively, only the start point and the end point related to the movement are specified, and the CPU unit 10 may dynamically generate the target trajectory from these two points.

  Subsequently, the CPU unit 10 calculates a corrected target trajectory by multiplying the target trajectory acquired in step S100 by a transfer function (model formula) from the table 106 to the carriage unit 102 (step S102). That is, the CPU unit 10 executes correction processing by the feedforward correction unit 204 shown in FIG. In other words, the CPU unit 10 calculates the corrected target trajectory by performing feedforward correction on the target trajectory.

  Subsequently, the CPU unit 10 calculates a dynamic behavior target of the stacker crane 100 from the difference between the target trajectory generated in step S100 and the corrected target trajectory acquired in step S102 (step S104). Further, the CPU unit 10 calculates a deviation between the position predicted by the control target model and the actually measured value for the stacker crane 100 (step S106). The CPU unit 10 is mounted on the carriage unit 102 based on the difference between the dynamic behavior target calculated in step S104 and the deviation between the position predicted by the control target model calculated in step S106 and the actual measurement value. A compensation amount for controlling the motor 108 (drive mechanism) is calculated (step S108).

  The processing in steps S104 to S108 corresponds to the processing for calculating the compensation amount Δθ based on the deviation ε for the table 106 shown in FIG.

  Subsequently, the CPU unit 10 controls the position, speed, torque, and the like of the motor 108 (drive mechanism) mounted on the carriage unit 102 based on the compensation amount calculated in step S108 (step S110).

  Then, the CPU unit 10 determines whether or not the end point of the current target trajectory has been reached (step S112). If the end point of the current target trajectory has not been reached (NO in step S112), the processes in and after step S104 are repeated.

  On the other hand, when the end point of the current target trajectory is reached (YES in step S112), the CPU unit 10 determines whether another target trajectory exists (step S114).

  When another target trajectory exists (in the case of YES in step S114), the CPU unit 10 generates another target trajectory for the table 106 (step S116), and executes the processing from step S102 onward again. To do.

  On the other hand, if another target trajectory does not exist (NO in step S114), the process ends.

  The movement control of the stacker crane 100 is performed by the processing procedure as described above. Hereinafter, Implementation Examples 1 and 2 that further embody the processing procedure illustrated in FIG. 5 will be described.

<E. Mounting Example 1>
As an implementation example 1, an implementation example will be described in which the forced vibration component generated by the corrected target trajectory is estimated, and only the free vibration component is extracted by subtracting the estimated value from the speed estimation value of the control target part, and then feedback is performed. .

  FIG. 6 is a control block diagram showing a first implementation example of the control system 1 according to the embodiment of the present invention. In FIG. 6, for convenience of explanation, the control logic is expressed in the form of a control block diagram. FIG. 7 is a diagram illustrating an example of a simulation result of the vibration suppression effect by the control logic illustrated in FIG.

  With reference to FIG. 6, the implementation example 1 of the control system 1 includes a target trajectory generation module 300, a corrected target trajectory generation module 302, a driver speed control module 314, and an estimation module 340. In FIG. 6, the characteristics of the carriage unit 102 and the table 106 are expressed in the form of transfer functions.

  The target trajectory generation module 300 outputs a target trajectory for the table 106 (control target part). This target trajectory is preferably expressed by a relatively large number of orders, for example, the fifth order. The corrected target trajectory generation module 302 calculates a corrected target trajectory for the table 106 from the target trajectory output by the target trajectory generation module 300. Specifically, the corrected target trajectory generation module 302 calculates a corrected target trajectory in consideration of an acceleration component for the target trajectory and a correction component for the target trajectory. This correction component corresponds to a vibration component that will occur in the table 106 (control target part) that is a control target part when the carriage unit 102 is moved according to the target trajectory. In the present embodiment, the target trajectory is corrected so as to cancel out this vibration component.

  In the addition element 308, the command value for the speed control is corrected using a compensation amount that takes into account fluctuations occurring in the table 106.

  Then, speed control is executed by the subtraction element 310, the gain adjustment module 312 and the driver speed control module 314. That is, in the subtraction element 310, a deviation between the target value and the position yM of the carriage unit 102 is calculated. A speed target value is calculated from this deviation by the gain adjustment module 312 and input to the driver speed control module 314. The driver speed control module 314 calculates an operation amount τ according to the input speed target value according to a predetermined arithmetic expression, and outputs the calculated operation amount τ to the motor 108 for driving the carriage unit 102. As a result, the carriage unit 102 and the table 106 connected to the carriage unit 102 move.

  The observer control system for the table 106 includes a configuration for acquiring the state value of the table 106 that is a control target part. In the example shown in FIG. 6, the speed of the table 106 is estimated using the estimation module 340. The speed of the table 106 is an example of a state value. Instead, the position, acceleration, jerk, and the like of the table 106 may be estimated.

  Specifically, the estimation module 340 acquires the operation amount τ output from the driver speed control module 314 and the position yM of the carriage unit 102, and multiplies transfer functions 342 and 344 corresponding to these values, respectively. The subtraction element 346 calculates the difference between them. The difference is multiplied by the transfer function 348 for adjusting the dimension, and the estimated speed of the table 106 is calculated. Based on the estimated speed of the table 106, a compensation amount related to the observer control is calculated.

  In the present embodiment, the target trajectory output from the target trajectory generation module 300 is input to the forced vibration component calculation module 320, and the forced vibration component calculation module 320 calculates the forced vibration component. Specifically, the forced vibration component calculation module 320 corresponds to a function that estimates the speed of the table 106 from the position of the carriage unit 102, and is generated from the corrected target trajectory output from the corrected target trajectory generation module 302 in the control target part. Calculate the forced vibration component.

  In the subtraction element 330, the forced vibration component calculated by the forced vibration component calculation module 320 is subtracted from the table estimated speed output from the estimation module 340, and the result is used to calculate the compensation amount.

  That is, the subtraction result in the subtraction element 330 is input to the addition element 308 via the phase adjuster 334 and the gain adjuster 338. The phase adjuster 334 controls the phase of the compensation amount from the table 106 to the carriage unit 102.

  In FIG. 6, an estimation module 340, a phase adjuster 334, and a gain adjuster 338 correspond to a calculation unit that acquires the state value of the control target part and calculates the compensation amount from the acquired state value of the control target part. . The addition element 308, the subtraction element 310, the gain adjustment module 312, and the driver speed control module 314 correspond to a generation unit that generates a movement command for the moving body based on the target trajectory and the compensation amount for the control target part. The forced vibration component calculation module 320 and the subtraction element 330 correspond to correction means for correcting the compensation amount based on the feature amount generated in the target trajectory.

  More specifically, the forced vibration component calculation module 320 corresponding to the correction unit corrects the compensation amount so as to cancel the forced vibration component included in the target trajectory. The section in which such a forced vibration component is generated substantially corresponds to a section in which the table target trajectory and the corrected target trajectory do not coincide as shown in FIG. In other words, the forced vibration component calculation module 320 suppresses the compensation amount in the section that is corrected corresponding to the vibration generated in the control target part in the target trajectory.

  The effect of adopting the control logic shown in FIG. 6 is shown in FIG. FIG. 7 shows the behavior when a target trajectory given by a quintic function is set. FIG. 7A shows an example in which the forced vibration component calculation module 320 and the subtraction element 330 shown in FIG. 6 are not provided and the control logic for directly inputting the output of the estimation module 340 to the phase adjuster 334 is adopted. FIG. 7B shows an example when the control logic shown in FIG. 6 is adopted.

  Comparing FIG. 7 (a) and FIG. 7 (b), by suppressing the forced vibration component, the settling time within ± 5 mm can be shortened from 2.7 seconds to about 1.2 seconds. Recognize. Further, it can be seen that by using the control logic of this implementation example, the gain reduction due to the band-pass filter does not occur, and thereby the peak value of the disturbance response characteristic can be improved by about 33%.

  According to this implementation example, the free vibration component and the forced vibration component in the control target part of the moving body are selectively suppressed, so that the settling time from disturbance can be shortened.

<F. Mounting example 2>
As an implementation example 2, a description will be given of an implementation example in which the amount of compensation for the target trajectory is not canceled by suppressing the compensation amount in the section in which the target trajectory is corrected.

  FIG. 8 is a control block diagram showing a mounting example 2 of the control system 1 according to the embodiment of the present invention. In FIG. 8, for convenience of explanation, the control logic is expressed in the form of a control block diagram. FIG. 9 is a diagram for explaining a temporal change in gain by the control logic shown in FIG. FIG. 10 is a diagram illustrating a simulation result example of the vibration damping effect by the control logic illustrated in FIG.

  Referring to FIG. 8, the implementation example 2 of the control system 1 includes a target trajectory generation module 300, a corrected target trajectory generation module 302, a driver speed control module 314, an estimation module 340, a second-order differentiator 304, A gain adjuster 336, a phase adjuster 334, and a gain adjuster 338 are included. In FIG. 8, the characteristics of the carriage unit 102 and the table 106 are expressed in the form of transfer functions.

  The speed control loop shown in FIG. 8 shows a configuration in which feedback is performed by adjusting the phase and gain of the vibration component of the table. Detailed description of the same parts as those of the mounting example 1 shown in FIG. 6 will not be repeated.

  In this implementation example, the estimation module 340, which is a disturbance observer, estimates the disturbance torque, and adjusts the phase and gain so that the phase is opposite to the vibration of the table, and the compensation amount is obtained.

  The corrected target trajectory generation module 302 calculates a corrected target trajectory for the table 106 from the target trajectory output by the target trajectory generation module 300. The estimation module 340 calculates (estimates) the actual value of the vibration component for the table 106 (control target part).

  In this implementation example, the compensation amount calculated by the subtraction element 330 is not input to the addition element 308 as it is, but is corrected by multiplying a variable gain depending on the situation. That is, by changing the gain, the compensation amount is suppressed in the section where the target trajectory is corrected, so that the shaping effect of the target trajectory is not offset.

For example, the gain adjustment unit 336 uses the acceleration of the target trajectory to suppress the compensation amount. Since the acceleration of the target trajectory divided by ω _a ² (the output component of the modified target trajectory generation module 302) is the vibration suppression trajectory component, the gain adjustment unit 336 suppresses the compensation amount during the period in which the acceleration exists in the target trajectory. . More specifically, the gain adjustment unit 336 includes a gain adjustment module 322, an absolute value conversion module 324, an addition element 328, and a multiplication / division module 332.

  The vibration suppression trajectory component output from the corrected target trajectory generation module 302 is multiplied by the gain K2, the absolute value module 324 calculates the absolute value of the result obtained, and the addition element 328 gains the gain K1. Is added. Here, the gain K2 is sufficiently larger than the gain K1 (that is, K2 >> K1). For example, the gain K1 can be set to “1” and the gain K2 can be set to “1000”. A value output from the addition element 328 becomes a gain for correcting the compensation amount.

  The multiplication / division module 332 corrects the compensation amount output from the subtraction element 330 by multiplying the gain output from the addition element 328 and then outputs the correction amount to the phase adjuster 334.

  That is, the adding element 328 divides the compensation amount by (absolute value of gain obtained by multiplying acceleration of target trajectory by large gain (gain K2) + gain K1). By dividing the compensation amount by a value obtained by multiplying the acceleration of the target trajectory by a large gain (gain K2), it is possible to suppress the compensation amount in the section that is corrected corresponding to the vibration generated in the control target part in the target trajectory.

  FIG. 9 shows an example of gain adjustment in the gain adjustment unit 336. As shown in FIG. 9A, in a section where the damping trajectory component (which is normalized) is relatively large, a value substantially equal to the gain K2 becomes a gain, and the damping trajectory component is relatively In a small section, a value substantially equal to the gain K1 is the gain. In the multiplication / division module 332, since the compensation amount is divided by this gain, the compensation amount is suppressed in a section where the gain is relatively large, and the input compensation amount is output as it is in a section where the gain is relatively small. Will be.

  The absolute value is calculated by the absolute value conversion module 324 in order to prevent the compensation amount from becoming oscillating. The addition element 328 adds the gain K1 to the divisor value when the damping trajectory component (the acceleration of the target trajectory) becomes zero or close to it in the multiplication / division module 332. This is to prevent the zero percent from occurring.

  As described above, in FIG. 8, the estimation module 340, the phase adjuster 334, and the gain adjuster 338 acquire the state value of the control target part and calculate the compensation amount from the acquired state value of the control target part. It corresponds to the calculation means. The addition element 308, the subtraction element 310, the gain adjustment module 312, and the driver speed control module 314 correspond to a generation unit that generates a movement command for the moving body based on the target trajectory and the compensation amount for the control target part. The gain adjustment unit 336 corresponds to a correction unit that corrects the compensation amount based on the feature amount generated in the target trajectory.

  More specifically, the gain adjustment unit 336 corresponding to the correction unit adjusts the compensation amount to be actually applied by changing the gain. In other words, the gain adjustment unit 336 suppresses the compensation amount in the section that is corrected corresponding to the vibration generated in the table 106 that is the control target part in the target trajectory.

  The effect of adopting the control logic shown in FIG. 8 is shown in FIG. FIG. 10 shows the behavior when a target trajectory given by a quintic function is set. FIG. 10A shows an example when the control logic that directly inputs the output of the subtraction element 330 to the phase adjuster 334 without using the gain adjustment unit 336 shown in FIG. b) shows an example when the control logic shown in FIG. 8 is adopted.

  Compared with FIG. 10A and FIG. 10B, by suppressing the forced vibration component, the settling time within ± 5 mm can be shortened from about 2.7 seconds to about 1.0 seconds. Recognize.

  In the implementation example 1 described above, only the free vibration component is extracted using the controlled object model and the compensation amount is adjusted. However, in the implementation example 2, in principle, the feedback amount is extremely reduced during acceleration of the target trajectory. It was adopted. By making the amount of feedback extremely small during operation of a drive mechanism such as a motor, it is not affected by model errors, and parameter adjustment can be made unnecessary. Furthermore, the operation of the drive mechanism in the transient state can be stabilized as compared with the first mounting example. That is, the gain used for the divisor in the multiplication / division module 332 affects the compensation amount at the final stage of the acceleration section and is related to the load overshoot amount. If fine adjustment of the load overshoot amount is unnecessary, it is sufficient to set the gain K2 to a large value. At this time, each of the divisor and the dividend input to the multiplication / division module 332 is a value having a dimension of speed. That is, a value having the same dimension is input to the multiplication / division module 332 and division is performed. Thereby, the control system can be stabilized. That is, the compensation amount is adjusted after the compensation amount and the output dimension of the correction means (gain adjusting unit 336) are matched.

  According to this mounting example, there is an advantage that the effect of the vibration control trajectory at the time of start-up is not lost and the behavior of the drive mechanism is not vibrational.

<G. Modification Example of Mounting Example 2>
In the implementation example 2 described above, in the gain adjustment unit 336 in FIG. 8, if the acceleration of the target trajectory is A and the gain is K, 1 / (A * K + 1) times or 1 / (A * K + 1) times the compensation amount. A configuration in which the compensation amount is suppressed by multiplying by (| A | × K + 1) is shown.

  However, the present invention is not limited to such a configuration, and an arbitrary feature amount may be used as the feature amount of the target trajectory. That is, instead of or in addition to the acceleration of the target trajectory, the speed or jerk (jerk) of the target trajectory may be used.

  For example, if the jerk of the target trajectory is J and the speed is V, the compensation amount may be suppressed by multiplying the following gain, for example.

(1) 1 / (| V | × K + 1)
(2) 1 / (| J | × K + 1)
(3) 1 / {(| V | + | A |) × K + 1}
(4) 1 / {(| V | + | A | + | J |) * K + 1}
Thus, instead of or in addition to acceleration, velocity or jerk (jerk) and / or a linear combination thereof may be used. In short, any mounting method may be employed as long as the target trajectory can be extracted.

<H. Other implementation examples>
In the mounting examples 1 and 2 described above, the so-called sensorless configuration has been described. However, a configuration using the strain sensor 112 for measuring the state value generated in the table 106 and / or the main body 104 may be employed. Further, not limited to the strain sensor 112, any measuring means may be employed. For example, it is possible to employ a means for measuring displacement by contacting an object such as an acceleration sensor, or a means for measuring displacement without contact using a laser beam.

  In the mounting examples 1 and 2, the mounting example in which the compensation amount is reflected in the speed control of the driving mechanism has been described. A mounting example in which the compensation amount is reflected in the position control or torque control of the drive mechanism can be adopted by the same method. Further, a mounting example in which the compensation amount is reflected in the position control of the driving mechanism, a mounting example in which the compensation amount is reflected in the speed control of the driving mechanism, and a mounting example in which the compensation amount is reflected in the torque control of the driving mechanism may be appropriately combined. .

  Moreover, although the application example to the stacker crane has been described in detail in the above description, the application destination is not limited to the stacker crane. For example, the present invention can be applied to a machine in which a main body is provided on a carriage and a paint is applied to an object using an injection mechanism mounted on the main body.

<I. Advantage>
According to the control device according to the present embodiment, it is possible to perform control along an appropriate target trajectory even for a mobile body having low rigidity. In addition, the control along the higher trajectory target can be realized by exhibiting the damping performance by the feedback control.

  The embodiment disclosed this time should be considered as illustrative in all points and not restrictive. The scope of the present invention is defined by the terms of the claims, rather than the description above, and is intended to include any modifications within the scope and meaning equivalent to the terms of the claims.

  1 control system, 10 CPU unit, 14 pulse counter unit, 18, 40 servo driver, 100 stacker crane, 102 truck unit, 104 body unit, 106 table, 108, 110 motor, 112 strain sensor, 120 rail, 150 placement rack, 200 Controller, 202 Adder, 204 Feedforward (FF) Correction Unit, 208 Compensation Unit, 212 Position Controller, 214 Speed Controller, 216 Torque Controller, 218 Feedback (FB) Correction Unit, 300 Target Trajectory Generation Module, 302 Correction Target Orbit generation module, 304, 308, 328 Addition element, 306, 334 Phase adjuster, 338 Gain adjuster, 310, 330, 346 Subtraction element, 312, 322 Gain adjustment module Le, 314 driver speed control module, 320 forced vibration component calculating module, 324 absolute value module 332 multiplication and division module, 336 the gain adjustment unit, 340 estimation module, 342,348 transfer function.

Claims (8)

A control device for moving a control target part of a moving body along a predetermined trajectory,
A calculation unit that obtains a state value of the control target part and calculates a compensation amount from the acquired state value of the control target part;
Generating means for generating a movement command for the moving body based on a control amount and a compensation amount for the control target part;
And a correction unit that corrects a compensation amount calculated by the calculation unit based on a feature amount generated in the control amount.   The control device according to claim 1, wherein the correction unit corrects the compensation amount so as to cancel a forced vibration component included in the control amount.   The control device according to claim 1, wherein the correction unit suppresses the compensation amount in a section in which the control amount is corrected corresponding to vibration generated in the control target part.   The control device according to claim 1, wherein the correction unit adjusts a compensation amount to be actually applied by changing a gain.   The control device according to claim 4, wherein the correction unit adjusts the compensation amount after combining the compensation amount and an output dimension of the correction unit.   The control device according to claim 4, wherein the correction unit corrects the compensation amount based on at least one of speed, acceleration, and jerk generated by the control amount. A control program for moving a control target part of a moving body along a predetermined trajectory, the control program being
Obtaining a state value of the control target part and calculating a compensation amount from the acquired state value of the control target part;
Generating a movement command for the moving body based on a control amount and a compensation amount for the control target part;
A control program for executing a step of correcting a compensation amount calculated in the calculating step based on a feature amount generated in the control amount. A control method for moving a control target part of a moving body along a predetermined trajectory,
Obtaining a state value of the control target part and calculating a compensation amount from the acquired state value of the control target part;
Generating a movement command for the moving body based on a control amount and a compensation amount for the control target part;
And a step of correcting the compensation amount calculated in the calculating step based on a feature amount generated in the control amount.

Images