S-shaped Curve Closed-loop Control System, Method and Computer-readable Medium

Abstract

The S-curve closed-loop control system, method and computer-readable medium of the present invention include: generating an S-curve model; generating a first speed compensation amount based on the difference between the actual speed calculated according to the position feedback and the S-curve model and the theoretical speed obtained from the S-curve model; selecting a first observation node and a second observation node based on the S-curve model, obtaining the first actual displacement of the first observation node based on the first theoretical time and position feedback of the first observation node, obtaining the first actual time of the first observation node based on the first theoretical displacement, position feedback and time feedback of the first observation node, generating a second speed compensation amount based on the difference between the first theoretical displacement and the first actual displacement and the difference between the second theoretical time and the first actual time of the second observation node; and offsetting the speed in the next communication cycle by the first speed compensation amount and offsetting the entire speed curve between the first observation node and the second observation node by the second speed compensation amount.

Description

Technical Field

[0001] The present invention relates to an S-curve closed-loop control system, method, and computer-readable medium, and particularly to a technique for controlling the motion process of a controlled object driven by a motor using an S-curve model. Background Art

[0002] In industrial control fields such as machining, material handling, robotics, printing, packaging, and laser cutting, the driving force from a motor or the like is usually transmitted to a controlled object via transmission mechanisms such as friction wheels, belts, gears, and racks, thereby controlling the movement of the controlled object.

[0003] In such a drive system, since mechanical components such as friction wheels and belts are prone to frictional slipping during high-speed movement, and there are clearances when gears and racks mesh with each other, serious mechanical deviations will occur, making it difficult to accurately position the movement of the controlled object, and ultimately leading to accidents such as collisions of equipment, causing significant losses.

[0004] For this reason, in the prior art, the following solution has been proposed, that is: a servo amplifier with a full closed-loop function is used to servo-drive the servo motor that drives the controlled object, and an external laser sensor signal is used to assist the servo amplifier to complete accurate positioning. Summary of the Invention

[0005] Problems to be Solved by the Invention

[0006] However, since the servo amplifier has high requirements for resolution accuracy, and the detection accuracy of commonly used sensors on the market is limited, and can only be detected with an accuracy of 48 - 52 times the cycle communication period Ts of the motion controller, it is difficult to match the accuracy requirements of the servo amplifier, and thus it is difficult to complete high-precision control.

[0007] Moreover, since the laser sensor itself is relatively expensive, such a control method in the past is difficult to be widely promoted and applied in various fields.

[0008] In addition, in addition to the sliding, clearance, and mechanical wear existing in the equipment itself, since the command speed provided by the servo amplifier to the servo motor is a continuously changing sine wave, and its amplitude (the ratio of the command frequency to the output frequency) decreases as the frequency increases, it will also cause deviations and attenuations between the actual output speed of the servo motor and the given speed curve, thereby further reducing the accuracy of speed control and displacement control.

[0009] The present invention is completed to solve the above problems, and its object is to provide an S-curve closed-loop control system, an S-curve closed-loop control method, and a computer-readable medium storing a program for executing the S-curve closed-loop control method, which can achieve the movement control of a controlled object with high precision at a low cost, thereby improving the performance and safety of equipment operation.

[0010] Technical solution for solving technical problems

[0011] To solve the above technical problems, the S-curve closed-loop control system according to the first aspect of the present invention generates a speed command for controlling the movement of a controlled object according to an S-curve model, and is characterized by including: a model generation unit that generates the S-curve model according to a target position and a target speed; a first speed compensation amount generation unit that obtains a position feedback from the controlled object, calculates an actual speed based on the position feedback and the S-curve model, obtains a theoretical speed from the S-curve model, and generates a first speed compensation amount based on the difference between the actual speed and the theoretical speed; a second speed compensation amount generation unit that obtains the position feedback and a time feedback from the controlled object, selects a first observation node and a next observation node of the first observation node, i.e., a second observation node, based on the S-curve model, obtains a first actual displacement of the first observation node based on a first theoretical time of the first observation node and the position feedback, obtains a first actual time of the first observation node based on a first theoretical displacement of the first observation node, the position feedback and the time feedback, and generates a second speed compensation amount based on the difference between the first theoretical displacement and the first actual displacement, and the difference between a second theoretical time of the second observation node and the first actual time; and a speed command generation unit that compensates the S-curve model, offsets the speed in the next communication cycle by the first speed compensation amount, and offsets the entire speed curve between the first observation node and the second observation node by the second speed compensation amount, and generates the speed command according to the compensated S-curve model.

[0012] In addition, to solve the above technical problems, the S-curve closed-loop control method according to the second aspect of the present invention generates a speed command for controlling the movement of a controlled object based on an S-curve model, and is characterized by including: a model generation step in which the S-curve model is generated based on a target position and a target speed; a first speed compensation amount generation step in which position feedback is obtained from the controlled object, an actual speed is calculated based on the position feedback and the S-curve model, a theoretical speed is obtained from the S-curve model, and a first speed compensation amount is generated based on the difference between the actual speed and the theoretical speed; a second speed compensation amount generation step in which the position feedback and time feedback are obtained from the controlled object, a first observation node and a next observation node of the first observation node, i.e., a second observation node, are selected based on the S-curve model, a first actual displacement of the first observation node is obtained based on a first theoretical time of the first observation node and the position feedback, a first actual time of the first observation node is obtained based on a first theoretical displacement of the first observation node, the position feedback, and the time feedback, and a second speed compensation amount is generated based on the difference between the first theoretical displacement and the first actual displacement, and the difference between a second theoretical time of the second observation node and the first actual time; and a speed command generation step in which the S-curve model is compensated so that the speed in the next communication cycle deviates by the first speed compensation amount, and the entire speed curve between the first observation node and the second observation node deviates by the second speed compensation amount, and the speed command is generated based on the compensated S-curve model.

[0013] In addition, to solve the above technical problems, a computer-readable medium according to the third aspect of the present invention stores a program for executing the S-curve closed-loop control method according to the second aspect of the present invention.

[0014] Advantages of the Invention

[0015] According to the S-curve closed-loop control system, S-curve closed-loop control method, and computer-readable medium storing a program for executing the S-curve closed-loop control method of the present invention, the movement control of a controlled object can be achieved with high precision at low cost, thereby improving the performance and safety of equipment operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a block diagram showing the structure of the S-curve closed-loop control system according to Embodiment 1 of the present invention.

[0017] Figure 2 is a graph for explaining an ideal model of an S-curve for acceleration and deceleration.

[0018] Figure 3 It is a schematic diagram for explaining the working principle of the first speed compensation amount generation unit.

[0019] Figure 4 It is a curve graph for explaining the working principle of the actual speed calculation unit.

[0020] Figure 5 It is a schematic diagram for explaining the working principle of the second speed compensation amount generation unit.

[0021] Figure 6 It is a flowchart showing the S-shaped curve closed-loop control method according to Embodiment 1 of the present invention.

[0022] Figure 7 It is a block diagram showing the structure of the S-shaped curve closed-loop control system according to Embodiment 2 of the present invention.

[0023] Figure 8 It is a curve graph for explaining the operation of the S-shaped curve adaptive adjustment unit.

[0024] Figure 9 It is a flowchart showing the operation of the S-shaped curve adaptive adjustment unit.

[0025] Figure 10 It is a block diagram showing the structure of the S-shaped curve closed-loop control system according to Embodiment 3 of the present invention.

[0026] Figure 11 It is a flowchart showing the operation of the overshoot protection unit. Detailed Embodiments

[0027] Hereinafter, with reference to the accompanying drawings, preferred embodiments of the present invention will be described.

[0028] Embodiment 1.

[0029] Figure 1 It is a block diagram showing the structure of the S-shaped curve closed-loop control system 100 according to Embodiment 1. As Figure 1 shown, the S-shaped curve closed-loop control system 100 includes a model generation unit 1, a first speed compensation amount generation unit 2, a second speed compensation amount generation unit 3, and a speed command generation unit 4.

[0030] As shown in the figure, the model generation unit 1 obtains, for example, the target position and target speed of the moving target to be controlled later from an upper control system (not shown), and generates an S-shaped curve model based on the obtained target position and target speed. Among them, the above-mentioned target position represents the total displacement for the controlled object to move according to the S-shaped curve, and the above-mentioned target speed represents the maximum speed reached by the controlled object during the movement.

[0031] Refer to Figure 2 to illustrate a specific example of the S-curve model generated by the model generation unit 1. Figure 2 is a graph for explaining the ideal model of the acceleration / deceleration S-curve. On the upper left side of the graph is an s-t graph showing the relationship between the displacement and time of the acceleration phase of the controlled object, and on the upper right side is an s-t graph showing the deceleration phase. In the middle left side of the graph is a v-t graph showing the relationship between the speed and time of the acceleration phase of the controlled object, and in the middle right side is a v-t graph showing the deceleration phase. In the lower left side of the graph is an a-t graph showing the relationship between the acceleration and time of the acceleration phase of the controlled object, and in the lower right side is an a-t graph showing the deceleration phase. Additionally, in Figure 2 the s-t graph, v-t graph, and a-t graph are divided by four dashed lines respectively, and thus are divided into a positive acceleration phase, a constant acceleration phase, a negative acceleration phase, an acceleration / deceleration phase, a constant deceleration phase, and a negative deceleration phase from left to right in sequence.

[0032] As Figure 2 shown, the model generation unit sets the starting position and ending position in the s-t graph according to the target position, obtains the maximum speed in the v-t graph according to the target speed, and can set the maximum acceleration and maximum jerk (the slope of the acceleration) in the a-t graph based on the target position and target speed by itself, thereby completing the construction of the ideal model of the acceleration / deceleration S-curve, and using the speed curve of the v-t graph for the speed compensation described later. Additionally, the maximum acceleration and maximum jerk used when generating the S-curve model can also be input by the designer through an input device (not shown) according to the design requirements, and this will be described in detail later.

[0033] In Figure 1 the speed command generation unit 4 obtains the generated S-curve model from the model generation unit 1, generates a speed command according to the S-curve model, and sends the speed command to, for example, a servo motor that controls the movement of a controlled object (not shown). Specifically, as Figure 2As shown, the speed command generation unit 4 obtains the speed, displacement, acceleration, and required time for each segment from the ideal model, and uses them as node positions to assist the servo motor in better position closed-loop inspection. Then, according to the cycle communication period Ts of the motion controller (not shown), the speed command generation unit 4 divides the ideal model into uniform point positions, and sequentially provides each point position to the target velocity register of the motion controller in chronological order, driving the servo motor to operate in the CSV (cycle synchronous speed) and trajectory speed modes. By directly giving the speed command in this way, the response delay and oscillation of the servo position loop integral to the algorithm can be reduced. Moreover, since the S-curve model can ensure the coherence of the speed, it has a significant effect in suppressing vibration, making the starting speed of the controlled object softer and the stopping more gentle.

[0034] In addition, although not shown, the controlled object can be, for example, a robotic arm, a cargo conveying structure, a conveyor belt, or other motion mechanisms connected to the servo motor via transmission mechanisms such as friction wheels, belts, gears, and racks.

[0035] In the prior art, there is a technical solution that directly generates speed commands according to the S-curve model. However, in such prior art, there are also the following problems. First, since the motion mechanism of the controlled object and the servo motor are connected through transmission mechanisms such as friction wheels, belts, gears, and racks, there may be phenomena such as slippage and clearance in the equipment itself, which may cause the problem of inaccurate positioning of the controlled object. Second, since the command speed sent by the speed command generation unit to the servo motor is a continuously changing sine wave, its amplitude (the ratio of the command frequency to the output frequency) may decrease as the frequency increases. Therefore, there will be a deviation and attenuation between the actual output speed of the servo and the given speed curve, which will also cause the problem of inaccurate positioning of the controlled object.

[0036] In view of the problems existing in the above prior art, the S-curve closed-loop control system 100 of this embodiment obtains data such as position feedback and time feedback from the controlled object through devices such as mechanical encoders (such as magnetic grating rulers, not shown) and high-speed counters configured outside the controlled object, and the first speed compensation amount generation unit 2 and the second speed compensation amount generation unit 3 respectively generate the first speed compensation amount and the second speed compensation amount based on these feedback data. Then, the speed command generation unit 4 compensates the S-curve model based on the first speed compensation amount and the second speed compensation amount. In other words, the S-curve closed-loop control system 100 of this embodiment performs full closed-loop compensation on the speed command through two closed loops formed by the first speed compensation amount generation unit 2 and the second speed compensation amount generation unit 3, thereby achieving accurate positioning of the controlled object.

[0037] The specific method for generating the first speed compensation amount will be described below.

[0038] As Figure 1 shown, the first speed compensation amount generation unit 2 includes an actual speed calculation unit 21, a theoretical speed calculation unit 22, and a subtractor 23. The actual speed calculation unit 21 obtains position feedback from the controlled object through, for example, a magnetic scale disposed at the controlled object, and obtains the S-curve model from the model generation unit 1. The actual speed calculation unit 21 calculates the actual speed based on the obtained position feedback and the S-curve model, and sends the calculation result to one input terminal of the subtractor 23. In addition, the theoretical speed calculation unit 22 obtains the S-curve model from the model generation unit 1, obtains the theoretical speed based on the S-curve model, and sends the obtained theoretical speed to the other input terminal of the subtractor 23. The subtractor 23 calculates the difference between the actual speed and the theoretical speed, and sends the speed difference as the first speed compensation amount to the speed command generation unit 4.

[0039] Figure 3 is a schematic diagram for explaining the working principle of the first speed compensation amount generation unit 2. As Figure 3 shown, assuming that according to the S-curve model, theoretically when moving to the displacement S1, the theoretical speed of the guided object should be V1. However, due to the mechanical deviations such as slippage and clearance described above, and the signal attenuation caused by the frequency of the speed command signal being greater than the cut-off frequency of the servo motor, the actual speed when the controlled object moves to the displacement S1 is V2, and V2 < V1. Therefore, within one cycle communication period Ts of the motion controller, the controlled object that should theoretically move to the displacement S3 actually only moves to the displacement S2. At this time, if the controlled object is still driven at the new theoretical speed V3, the displacement deviation will accumulate after the next cycle communication period Ts, which will ultimately lead to inaccurate positioning.

[0040] From Figure 3 the perspective, it seems that the theoretical displacement ΔS1 = S3 - S1 within one cycle communication period can be subtracted from the actual displacement ΔS2 = S2 - S1 within one cycle communication period, and then divided by the cycle communication period Ts to obtain the speed ΔV to be compensated, that is, ΔV = (ΔS1 - ΔS2) / Ts. However, since the communication period of the sensor feedback data is not the cycle communication period Ts, but is obtained according to the performance of the sensor itself, and the communication period of commonly used sensors on the market cannot reach the cycle communication period Ts (generally 48 to 52 times the cycle communication period Ts), therefore, the speed ΔV to be compensated cannot be directly obtained through the displacement difference (ΔS1 - ΔS2) between the actual displacement and the theoretical displacement within one cycle communication period Ts.

[0041] Therefore, the actual speed calculation unit 21 in the first speed compensation amount generation unit 2 of the present embodiment obtains the relationship between the theoretical displacement and time based on the S-curve model from the model generation unit 1, solves the Cardano formula for this relationship to obtain the movement time corresponding to the position feedback from the controlled object, and substitutes this movement time into the S-curve model to calculate the actual speed.

[0042] Figure 4 It is a curve graph for explaining the working principle of the actual speed calculation unit, and it is the s-t curve in the acceleration stage. As Figure 4 shown, within the time period t1, the controlled object is in the jerk acceleration stage, within the time period t2, the controlled object is in the uniform acceleration stage, and within the time period t3, the controlled object is in the deceleration acceleration stage. If the maximum acceleration is set as a m , and the maximum jerk is set as j m , then the speed curve V(t) of the S-curve model can be expressed by the following formula (1).

[0043] [Mathematical formula 1]

[0044]

[0045] Integrating formula (1) can obtain the relationship between displacement and time, that is, formula (2)

[0046] [Mathematical formula 2]

[0047]

[0048] By solving the Cardano formula for the above displacement, the corresponding movement time can be obtained.

[0049] According to the Cardano formula, to solve the cubic equation ax 3 + bx 2 + cx + d = 0, let It can be obtained that

[0050] In the present embodiment, the actual speed calculation unit 21 compares the position feedback obtained from the controlled object with the displacement ranges of each stage of the jerk acceleration stage t1, uniform acceleration stage t2, and deceleration acceleration stage t3 in the s-t curve of the S-curve model, so as to determine which stage among the above three stages the current displacement is in.

[0051] According to the displacement comparison result:

[0052] (a) When the current displacement is in the jerk acceleration stage, that is, 0 ≤ t < t1,

[0053] In the above formula (2),

[0054]

[0055] It holds. Convert the above formula to t 3 + pt + q = 0, then we can get At this time, the unique real solution, that is, the moving time t corresponding to the position feedback, can be calculated as shown in the following formula (3).

[0056] [Mathematical formula 3]

[0057]

[0058] Substitute the moving time t into the above formula (1), and the current actual speed can be obtained

[0059] (b) When the current displacement is in the uniformly accelerated stage, that is, t1 ≤ t < t1 + t2,

[0060] In the above formula (2),

[0061]

[0062] It holds. Convert the above formula to

[0063]

[0064] , then the unique real solution, that is, the moving time t corresponding to the position feedback, can be calculated as shown in the following formula (4).

[0065] [Mathematical formula 4]

[0066]

[0067] Substitute the moving time t into the above formula (1), and the current actual speed can be obtained

[0068]

[0069] (c) When the current displacement is in the decelerated acceleration stage, that is, t1 + t2 ≤ t < t1 + t2 + t3,

[0070] In the above formula (2),

[0071]

[0072] It holds. Convert the above formula to

[0073] y 3 + py + q = 0, let Then we can get Find a, b, c respectively, that is:

[0074]

[0075]

[0076]

[0077]

[0078] Then, a unique real solution can be calculated, that is, the movement time t corresponding to the position feedback is shown in the following formula (5).

[0079] [Mathematical formula 5]

[0080]

[0081] Substituting the movement time t into the above formula (1), the current actual speed can be obtained

[0082]

[0083] The above specifically describes the method of calculating the actual speed based on position feedback and the S-curve model in the acceleration phase. Since there is symmetry between the deceleration phase and the acceleration phase, the specific calculation formula for the deceleration phase is symmetric to that of the acceleration phase, so the description is omitted here.

[0084] Return to Figure 3 , after the actual speed calculation unit 21 calculates the actual speed V2 according to the position feedback S1, the theoretical speed calculation unit 22 obtains the speed command sent by the speed command generation unit 4 to the servo motor in the first cycle communication period from the S-curve model of the model generation unit 1 as the theoretical speed V1. Then, the actual speed calculation unit 21 and the theoretical speed calculation unit 22 respectively send the actual speed V2 and the theoretical speed V1 to the subtractor 23, and the subtractor 23 calculates the first speed compensation amount ΔV1 through the following formula (6) and sends the first speed compensation amount ΔV1 to the speed command generation unit 4.

[0085] [Mathematical formula 6]

[0086] ΔV1 = V1 - V2 (6)

[0087] The speed command generation unit 4 compensates the S-curve model from the model generation unit 1 by using the received first speed compensation amount. Specifically, for example, as Figure 3As shown, the speed in the next communication cycle of the first cyclic communication cycle, i.e., the second cyclic communication cycle at S2, is offset by the first speed compensation amount. Here, if the speed command at the next cyclic communication cycle, i.e., at S3, should be V3 according to the theoretical model, then the speed command in the compensated next cyclic communication cycle, i.e., at S2, becomes V3+(V1-V2). Thus, for example, when V2<V1, through the compensation of the first speed compensation amount, the next cyclic communication cycle can be completed at a faster speed. Therefore, at the end of the next cyclic communication cycle, the actual displacement and the theoretical displacement are unified to S4, thereby eliminating the position deviation and achieving precise positioning.

[0088] The specific method for generating the second speed compensation amount will be described below.

[0089] As Figure 1 shown, the second speed compensation amount generation unit 3 includes an observation node selection unit 31, a compensation amount calculation unit 32, and a subtractor 33.

[0090] The observation node selection unit 31 obtains the S-curve model from the model generation unit 1, and based on the S-curve model, selects at least one or more time points from the s-t graph as shown in Figure 2 as observation nodes. In this embodiment, two adjacent observation nodes are sequentially defined as the first observation node and the second observation node in chronological order. In this embodiment, the time at the first observation node in the s-t graph of the S-curve model is defined as the first theoretical time (t1), the displacement at the first observation node is defined as the first theoretical displacement (S1), and the time at the second observation node in the s-t graph of the S-curve model is defined as the second theoretical time (t3). Also, the actual displacement of the controlled object at the first theoretical time (t1) is defined as the first actual displacement (S2), and the actual time when the controlled object reaches the first theoretical displacement (S1) is defined as the first actual time (t2).

[0091] Then, the observation node selection unit 31 obtains the first theoretical time (t1) according to the S-curve model, obtains the position feedback from the controlled object through a magnetic grating ruler or the like disposed on the controlled object, and obtains the first actual displacement (S2) based on the first theoretical time (t1) and the position feedback of the controlled object. The observation node selection unit 31 inputs the obtained first actual displacement (S2) and the first theoretical displacement (S1) obtained according to the S-curve model to the subtractor 33, and the subtractor 33 sends the displacement difference between the two (i.e., ΔS = S1 - S2) to the compensation amount calculation unit 32.

[0092] In addition, an observation node selection unit 31 obtains time feedback from a controlled object, for example, through a high-speed counter in an upper controller (not shown), and based on the obtained time feedback, a first theoretical displacement (S1) obtained according to an S-curve model, and position feedback obtained through a magnetic scale, obtains a first actual time (t2). Then, the observation node selection unit 31 sends the time difference between a second theoretical time (t3) obtained according to the S-curve model and the first actual time (t2) (i.e., Δt = t3 - t2) to a compensation amount calculation unit 32.

[0093] Based on the obtained displacement difference and time difference, the compensation amount calculation unit 32 calculates a second speed compensation amount and sends the calculated second speed compensation amount to a speed command generation unit 4.

[0094] Figure 5 It is a schematic diagram for explaining the working principle of the second speed compensation amount generation unit 3. As Figure 5 shown, according to the S-curve model, theoretically at the first observation node, the displacement after the first theoretical time t1 from the start of movement should be the first theoretical displacement S1. However, due to mechanical deviations such as the above-mentioned slipping and clearance, and signal attenuation caused by the frequency of the speed command signal being greater than the cut-off frequency of the servo motor, the actual time for the controlled object to move to the displacement S1 is the first actual time t2, and t2 > t1. In addition, the actual displacement of the controlled object after the first theoretical time t1 is the first actual displacement S2, and S2 < S1. Therefore, if the controlled object continues to move while maintaining such a deviation, the time required for the controlled object to reach the theoretical displacement S3 of the second observation node will surely exceed the second theoretical time t3 at this second observation node. As a result, a time deviation will occur in the movement control of the controlled object. Therefore, it is necessary to compensate the time rhythm of the S-curve model.

[0095] Therefore, in the second speed compensation amount generation unit 3 of the present embodiment, the position feedback of the controlled object at the first theoretical time t1 is set as the first actual displacement S2, and the time feedback when it is determined according to the position feedback that the controlled object reaches the first theoretical displacement S1, that is, the time consumed at this time, is set as the first actual time t2. Then, according to the following formula (7), by taking the derivative of the difference between the first theoretical displacement S1 and the first actual displacement S2 with respect to the difference between the second theoretical time t3 and the first actual time t2 at the second observation node, the second speed compensation amount ΔV2 is calculated, and the second speed compensation amount ΔV2 is sent to the speed command generation unit 4.

[0096] [Mathematical formula 7]

[0097] ΔV2 = d(S1 - S2) / d(t3 - t2) (7)

[0098] The speed command generation unit 4 compensates the S-curve model from the model generation unit 1 by using the received second speed compensation amount. Specifically, for example, as Figure 5 shown, the part (S1 - S2) where the actual displacement at the first observation node lags behind the theoretical displacement due to insufficient speed is evenly distributed over the time period (t3 - t2) from the current time t2 to the theoretical time t3 of the second observation node. In other words, the speed command generation unit 4 shifts the entire speed curve between the first observation node and the second observation node by the second speed compensation amount ΔV2, thereby correcting the time deviation. Therefore, at the theoretical displacement S3 of the second observation node, the actual time and the theoretical time are unified to t3, thus eliminating the time deviation.

[0099] Above, a method of randomly selecting two adjacent observation nodes as the first observation node and the second observation node during the entire acceleration and deceleration process has been described. In this embodiment, there is no particular limitation on how to select the observation nodes. However, considering that it is desired to use the second speed compensation amount to compensate the S-curve model throughout the acceleration and deceleration process, therefore, it is preferable that the observation node selection unit 31 of the second speed compensation amount generation unit 3 selects at least one time point from each of the accelerating phase, constant acceleration phase, decelerating acceleration phase, acceleration and deceleration phase, constant deceleration phase, and decelerating deceleration phase in the S-curve model from the model generation unit 1 as the first observation node. As a specific example, referring to Figure 2 , for example, the start time of each phase can be selected as the first observation node, and the start time of the next adjacent phase can be selected as the second observation node, so that the second observation node of one phase coincides with the first observation node of the next phase, thereby including the entire acceleration and deceleration phases within the compensation range of the second speed compensation amount. Thus, the compensation effect of the second speed compensation amount can cover the entire acceleration and deceleration process, and further improve the control accuracy of the time rhythm of the motion of the controlled object.

[0100] Next, the S-curve closed-loop control method according to this embodiment will be described.

[0101] Figure 6 is a flowchart showing the S-curve closed-loop control method according to Embodiment 1. As Figure 6 shown, first, the model generation unit 1 generates an S-curve model based on the target position and the target speed (step ST1).

[0102] Next, the first speed compensation amount generation unit 2 obtains position feedback and time feedback from the controlled object (step ST2); obtains the relationship between displacement and time based on the S-shaped curve model, and solves the Cardan formula for the relationship between displacement and time to obtain the movement time corresponding to the position feedback (step ST3); substitutes the movement time obtained in step ST3 into the S-shaped curve model to calculate the actual speed (step ST4); obtains the theoretical speed from the S-shaped curve model (step ST5); generates the first speed compensation amount based on the difference between the actual speed and the theoretical speed (step ST6).

[0103] Then, the second speed compensation amount generation unit 3 selects a first observation node and a second observation node based on the S-shaped curve model (step ST7); obtains the first actual displacement based on the first theoretical time and the position feedback of the first observation node. Specifically, sets the position feedback of the controlled object at the first theoretical time as the first actual displacement (step ST8); obtains the first actual time based on the first theoretical displacement, the position feedback, and the time feedback of the first observation node. Specifically, sets the time feedback when it is determined according to the position feedback that the controlled object reaches the first theoretical displacement as the first actual time (step ST9); generates the second speed compensation amount based on the difference between the first theoretical displacement and the first actual displacement, and the difference between the second theoretical time and the first actual time of the second observation node. Specifically, differentiates the difference between the first theoretical displacement and the first actual displacement with respect to the difference between the second theoretical time and the first actual time to calculate the second speed compensation amount (step ST10).

[0104] After that, the speed command generation unit compensates the S-shaped curve model so that the speed in the next communication cycle deviates from the first speed compensation amount (step ST11), and makes the entire speed curve between the first observation node and the second observation node deviate from the second speed compensation amount, and generates a speed command according to the compensated S-shaped curve model (step ST12).

[0105] After that, it is judged whether the controlled object reaches the target position (step ST13). When it is judged that the controlled object has not reached the target position (step ST13: No), return to step ST2, and repeat steps ST2 to ST12. When it is judged that the controlled object reaches the target position (step ST13: Yes), end the process.

[0106] As described above, according to the S-curve closed-loop control system and the S-curve closed-loop control method according to the present embodiment, the full-closed-loop control algorithm on the upper-level operation controller side by the magnetic grating ruler and the high-speed counter realizes the compensation of the position accuracy and the compensation of the time rhythm. Thus, even when there are phenomena such as sliding and clearance in the device itself, or when there is attenuation caused by the frequency of the command speed signal being greater than the cut-off frequency of the servo motor, it is also possible to compensate for the resulting positioning inaccuracy and rhythm misalignment. Therefore, it is possible to achieve the movement control of the controlled object with low cost and high precision, thereby improving the performance and safety of the device operation.

[0107] Embodiment 2.

[0108] Figure 7 FIG. is a block diagram showing the structure of the S-curve closed-loop control system 100' according to Embodiment 2. The difference between the S-curve closed-loop control system 100' of the present embodiment and the S-curve closed-loop control system 100 of Embodiment 1 is that the model generation unit 1 also generates an S-curve model with reference to the maximum acceleration and the maximum jerk, and the S-curve closed-loop control system 100' further includes an S-curve adaptive adjustment unit 5. In Figure 7 FIG., the same reference numerals are assigned to the same structures as those of the S-curve closed-loop control system 100 of Embodiment 1. Hereinafter, the S-curve closed-loop control system 100' of the present embodiment will be described centering on the differences from Embodiment 1.

[0109] As Figure 7 shown, when generating the S-curve model, the model generation unit 1 of the present embodiment not only sets the starting position, the ending position, and the maximum speed according to the target position and the target speed, but also obtains the maximum acceleration and the maximum jerk output by a designer through an input device (not shown) according to the design requirements from the outside via the S-curve adaptive adjustment unit 5 as parameters to generate the S-curve model.

[0110] However, in actual operation, due to the differences in the design experience of different designers, when the design experience of the designer is insufficient, it is possible that the input maximum acceleration and maximum jerk do not match the target position and the target speed, so that the S-curve model cannot be generated according to the input maximum acceleration, maximum jerk, target position, and target rotational speed. For this reason, as Figure 7 shown, the S-curve closed-loop control system 100' of the present embodiment is further provided with an S-curve adaptive adjustment unit 5. When the minimum speed calculated according to the input maximum acceleration and maximum jerk is still greater than the target speed, the maximum acceleration is reduced so that the minimum speed is equal to the target speed. Specifically, the S-curve adaptive adjustment unit 5 first obtains the target speed, that is, the maximum speed V, from the model generation unit 1m , and obtain the maximum acceleration a from the outside m and the maximum jerk j m . Then, according to the maximum acceleration a m and the maximum jerk j m , calculate the minimum speed V min , and compare the minimum speed V min with the maximum speed V m . When the minimum speed V min is greater than the maximum speed V m , reduce the maximum acceleration a m to the new maximum acceleration a m1 so that the minimum value V min is equal to the maximum speed V m . After that, provide the maximum jerk j m and the adjusted new maximum acceleration a m1 to the model generation unit 1 together for generating the S-curve model.

[0111] The following describes the specific actions of the adaptive adjustment of the S-curve.

[0112] Figure 8 is a graph for explaining the actions of the S-curve adaptive adjustment unit 5 and is the a-t curve in the acceleration phase. In addition, in this embodiment, the acceleration phase is taken as an example for explanation. However, since there is symmetry between the deceleration phase and the acceleration phase and their calculation processes are the same, the description of the deceleration phase is omitted here.

[0113] As Figure 8 shown, set the time of the jerk acceleration phase as t1, the time of the uniform acceleration phase as t2, and the time of the deceleration acceleration phase as t3. And set the maximum acceleration input by the designer as a m , set the maximum jerk as j m , set the target speed as V m , as Figure 8 shown, according to the definitions of acceleration and jerk, the following equations (8) and (9) can be deduced.

[0114] [Mathematical formula 8]

[0115]

[0116] [Mathematical formula 9]

[0117]

[0118] And, according to the definition of jerk, the calculation formula of jerk j(t) can be obtained as follows.

[0119] [Mathematical Formula 10]

[0120]

[0121] By integrating the jerk j(t), the calculation formula for the acceleration a(t) can be obtained as follows.

[0122] [Mathematical Formula 11]

[0123]

[0124] Furthermore, if the acceleration a(t) is further integrated, the calculation formula for the velocity V(t) can be obtained as shown in Equation (1) above.

[0125] According to the above Equation (1) and Equation (11), when t2 = 0, the integral value of the calculated a(t) is the smallest. At this time, referring to the above Equation (8), according to the maximum acceleration a m and the maximum jerk j m the minimum value V of the calculated velocity min is as shown in the following Equation (12).

[0126] [Mathematical Formula 12]

[0127]

[0128] At this time, if it is determined that V min > V m , then the new maximum acceleration a m1 can be calculated by the following Equation (13) derived from the above Equation (12) so that the minimum velocity V min is equal to the target velocity V m .

[0129] [Mathematical Formula 13]

[0130]

[0131] Then, the maximum jerk j m and the above new maximum acceleration a m1 of the S-curve adaptive adjustment unit 5 are sent to the model generation unit 1, so that the model generation unit 1 can generate an S-curve model according to the target position and the target velocity V m , and further referring to the maximum acceleration a m1 and the maximum jerk j m .

[0132] The S-curve closed-loop control method according to the present embodiment will be described below.

[0133] Figure 9is a flowchart showing the operation of the S-curve adaptive adjustment unit 5, and this flowchart can be executed, for example, before step ST1 in the flowchart of Figure 6 . As shown in Figure 9 , first, the S-curve adaptive adjustment unit 5 obtains the maximum acceleration a m and the maximum jerk j m from the outside, and obtains the target speed (i.e., the maximum speed) V m from the model generation unit 1 (step ST101).

[0134] Next, the S-curve adaptive adjustment unit 5 calculates the minimum speed V m according to the above formula (12) based on the maximum acceleration a m and the maximum jerk j min (step ST102).

[0135] Then, it is judged whether V min is greater than V m (step ST103).

[0136] When it is judged that V min is greater than V m (step ST103 "Yes"), the maximum acceleration is reduced by the above formula (13) to obtain a new maximum acceleration a m1 such that the minimum speed is equal to the target speed (step ST104), and then proceeds to step ST105.

[0137] When it is judged that V min is less than or equal to V m (step ST103 "No"), step ST104 is skipped and directly proceeds to step ST105.

[0138] Finally, the S-curve adaptive adjustment unit 5 sends the maximum acceleration a m (or the calculated new maximum acceleration a m1 ) and the maximum jerk j m to the model generation unit 1 for generating the S-curve model (step ST105).

[0139] As described above, according to the S-curve closed-loop control system and the S-curve closed-loop control method according to the present embodiment, by reducing the maximum acceleration, the minimum speed is made equal to the target speed. Thus, even when the designer sets unmatched parameters due to lack of experience, the system can automatically adaptively adjust the parameters through model analysis and perform overwriting update. Therefore, it can ensure that the model generation unit can generate a suitable S-curve model, thereby providing an accurate basis for the subsequent movement control of the controlled object.

[0140] Embodiment 3

[0141] Figure 10 is a block diagram showing the structure of the S-curve closed-loop control system 100” according to this Embodiment 3. The difference between the S-curve closed-loop control system 100” of this embodiment and the S-curve closed-loop control system 100 of Embodiment 1 is that the S-curve closed-loop control system 100” further includes an overshoot protection unit 6. In Figure 10 , the same reference numerals are assigned to the same structures as those of the S-curve closed-loop control system 100 of Embodiment 1. Hereinafter, the S-curve closed-loop control system 100” of this embodiment will be described centering on the differences from Embodiment 1.

[0142] During actual operation, the external magnetic scale may have faults such as wear, signal loss, and noise interference during high-speed operation, which may cause the controlled object to overshoot and collide with surrounding equipment, resulting in significant losses. To cope with the above fault conditions and perform overshoot protection, the S-curve closed-loop control system 100” of this embodiment further includes an overshoot protection unit 6.

[0143] As Figure 10 shown, the overshoot protection unit 6 obtains the target position from, for example, an upper-level control system (not shown), and obtains the position feedback from the controlled object through, for example, a mechanical-side encoder such as a magnetic scale disposed outside the controlled object (not shown). Then, the overshoot protection unit 6 monitors the position feedback data of the mechanical-side encoder in real time and compares the position feedback with the target position. Once the position feedback data of the mechanical-side encoder is greater than the target position, that is, when overshoot occurs, the overshoot protection unit 6 immediately sends an instruction to the speed command generation unit 4 to latch the execution process of the S-curve model after speed compensation in the speed command generation unit 4.

[0144] In addition, although not shown, the overshoot protection unit 6 can also send the fault occurrence situation to the on-site staff through an output device, and the staff judges whether to take emergency stop measures. When the on-site staff judges that emergency stop measures need to be taken, an instruction to stop the execution process of the S-curve model can be sent to the speed command generation unit 4 through an input device. At this time, the overshoot protection unit 6 can temporarily store the execution progress of the current process in an external memory and notify the on-site staff through an output device to inform the progress.

[0145] When the on-site fault is eliminated and the controlled object gets out of the overshoot state, the overshoot protection unit 6 learns of this situation by comparing the position feedback with the target position, and thus sends an instruction to the speed command generation unit 4 to release the latch on the execution process of the S-curve model. At this time, the overshoot protection unit 6 can read the previously latched execution progress from an external memory and send it to the speed command generation unit 4. Thereby, the process can be continued to enable the controlled object to complete the remaining moving distance.

[0146] Alternatively, after the on-site staff have eliminated the fault causing the overshoot, the on-site staff can instruct the overshoot protection unit 6 to resume from the breakpoint through the input device, thereby restarting the previous algorithm execution progress and completing the remaining distance.

[0147] Next, the S-curve closed-loop control method according to this embodiment will be described.

[0148] Figure 11 It is a flowchart showing the operation of the overshoot protection unit 6. As Figure 11 shown, first, the overshoot protection unit 6 obtains the position feedback from the controlled object through the magnetic grating ruler (step ST201), and obtains the target position from the upper control system (step ST202).

[0149] Next, the overshoot protection unit 6 compares the position feedback with the target position to determine whether the position feedback is greater than the target position (step ST203).

[0150] When the position feedback is greater than the target position (step ST203 "Yes"), the overshoot protection unit 6 determines that the controlled object is in the overshoot state, sends an instruction to the speed command generation unit 4 to stop the movement of the controlled object, and latches the execution process of the S-curve model (step ST204).

[0151] Then, the overshoot protection unit 6 determines whether the controlled object has got out of the overshoot state (step ST205). If it has not got out of the overshoot state (step ST205 "No"), it returns to step ST204 to continue latching the execution process. If it has got out of the overshoot state (step ST205 "Yes"), the overshoot protection unit 6 sends an instruction to the speed command generation unit 4 to release the latch on the execution process of the S-curve model (step ST206), so that the controlled object continues to move according to the S-curve model (step ST207).

[0152] On the other hand, when it is determined in step ST203 that the position feedback is less than or equal to the target position (step ST203 "No"), steps ST204 to ST206 are bypassed, and the controlled object is directly moved according to the S-curve model (step ST207).

[0153] As described above, according to the S-curve closed-loop control system and the S-curve closed-loop control method according to the present embodiment, when an overshoot fault occurs, it can timely prevent the controlled object from continuing to move, and latch the execution process of the S-curve model. Moreover, when the overshoot state is exited, the latch can be released. Therefore, it can prevent the equipment from being damaged due to overshoot, and enable the equipment to continue to complete the remaining journey along the S-curve model after the fault is eliminated.

[0154] In the above Embodiment 1 to Embodiment 3, the case of implementing the S-curve closed-loop control method of the present invention by hardware has been described, but the present invention is not limited thereto. The S-curve closed-loop control method of the present invention can also be implemented by software, or by a combination of software and hardware. In addition, a program for executing the S-curve closed-loop control method of the present invention can be stored in various computer-readable media, and loaded into, for example, a CPU when needed for execution. There is no particular limitation on the computer-readable medium. For example, optical discs such as HDD, CD-ROM, CD-R, MO, MD, DVD, IC cards, floppy disks, and semiconductor memories such as mask ROM, EPROM, EEPROM, and flash ROM can be used.

[0155] In addition, all aspects of the embodiments disclosed this time should be considered as merely illustrative and not restrictive. The scope of the present invention is represented by the claims, rather than by the above embodiments, and the scope of the present invention also includes all modifications and variations within the meaning and scope equivalent to the claims.

[0156] Industrial Applicability

[0157] As described above, according to the S-curve closed-loop control system, the S-curve closed-loop control method, and the computer-readable medium storing a program for executing the S-curve closed-loop control method according to the present invention, a controlled object driven by a permanent magnet synchronous motor conforming to the CIA 402 standard such as a stepping motor or a servo motor can be controlled to move by an auxiliary upper motion controller such as a magnetic grating ruler and a high-speed counter. Therefore, it is useful for the full closed-loop control of a general motor that only supports semi-closed loop.

[0158] Reference Numeral Explanation

[0159] 1 Model Generation Unit

[0160] 2 First Speed Compensation Amount Generation Unit

[0161] 3 Second Speed Compensation Amount Generation Unit

[0162] 4 Speed Command Generation Unit

[0163] 5 S-curve Adaptive Adjustment Unit

[0164] 6 Overshoot protection unit

[0165] 21 Actual speed calculation unit

[0166] 22 Theoretical speed calculation unit

[0167] 23 Subtractor

[0168] 31 Observation node selection unit

[0169] 32 Compensation amount calculation unit

[0170] 33 Subtractor

[0171] 100, 100’, 100” S-curve closed-loop control system

Claims

1. An S-curve closed-loop control system, which generates a speed command for controlling the movement of a controlled object according to an S-curve model, is characterized in that Comprising: A model generation unit that generates the S-curve model based on a target position and a target speed; A first speed compensation amount generation unit that obtains a position feedback from the controlled object, calculates an actual speed based on the position feedback and the S-curve model, obtains a theoretical speed from the S-curve model, and generates a first speed compensation amount based on the difference between the actual speed and the theoretical speed; A second speed compensation amount generation unit that obtains the position feedback and a time feedback from the controlled object, selects a first observation node and a next observation node of the first observation node, i.e., a second observation node, based on the S-curve model, obtains a first actual displacement of the first observation node based on a first theoretical time of the first observation node and the position feedback, obtains a first actual time of the first observation node based on a first theoretical displacement of the first observation node, the position feedback, and the time feedback, and generates a second speed compensation amount based on the difference between the first theoretical displacement and the first actual displacement, and the difference between a second theoretical time of the second observation node and the first actual time; And A speed command generation unit that compensates the S-curve model, shifts the speed in the next communication cycle by the first speed compensation amount, and shifts the entire speed curve between the first observation node and the second observation node by the second speed compensation amount, and generates the speed command according to the compensated S-curve model.

2. The S-curve closed-loop control system according to claim 1, wherein The second speed compensation amount generation unit sets the position feedback of the controlled object at the first theoretical time as the first actual displacement, sets the time feedback when it is determined according to the position feedback that the controlled object reaches the first theoretical displacement as the first actual time, and calculates the second speed compensation amount by taking the derivative of the difference between the first theoretical displacement and the first actual displacement with respect to the difference between the second theoretical time and the first actual time.

3. The S-curve closed-loop control system according to claim 1, wherein The second speed compensation amount generation unit selects at least one time point from each of an increasing acceleration stage, a constant acceleration stage, a decreasing acceleration stage, an increasing and decreasing acceleration stage, a constant deceleration stage, and a decreasing deceleration stage in the S-curve model as the first observation node.

4. The S-curve closed-loop control system according to any one of claims 1 to 3, wherein The first compensation amount generation unit obtains a relationship between displacement and time based on the S-curve model, obtains a moving time corresponding to the position feedback by solving the Cardano formula for the relationship between displacement and time, and substitutes the moving time into the S-curve model to calculate the actual speed.

5. The S-curve closed-loop control system according to any one of claims 1 to 3, wherein The model generation unit also generates the S-curve model with reference to a maximum acceleration and a maximum jerk. The S-curve closed-loop control system further includes an S-curve adaptive adjustment unit. When the minimum speed calculated based on the maximum acceleration and the maximum jerk is greater than the target speed, the S-curve adaptive adjustment unit reduces the maximum acceleration so that the minimum speed is equal to the target speed.

6. The S-curve closed-loop control system according to any one of claims 1 to 3, characterized in that it further includes an overshoot protection unit. The overshoot protection unit compares the position feedback with the target position. In the overshoot state where the position feedback is greater than the target position, the overshoot protection unit stops the controlled object from moving and latches the execution process of the S-curve model.

7. The S-curve closed-loop control system according to claim 6, characterized in that when out of the overshoot state, the overshoot protection unit releases the latch of the execution process of the S-curve model, and enables the controlled object to continue moving according to the S-curve model.

8. An S-curve closed-loop control method, which generates a speed command for controlling the movement of a controlled object according to an S-curve model, is characterized in that, including: a model generation step, in which the S-curve model is generated according to the target position and the target speed; a first speed compensation amount generation step, in which the position feedback is obtained from the controlled object, the actual speed is calculated based on the position feedback and the S-curve model, the theoretical speed is obtained from the S-curve model, and the first speed compensation amount is generated based on the difference between the actual speed and the theoretical speed; a second speed compensation amount generation step, in which the position feedback and the time feedback are obtained from the controlled object, the first observation node and the next observation node of the first observation node, i.e., the second observation node, are selected based on the S-curve model, the first actual displacement of the first observation node is obtained based on the first theoretical time of the first observation node and the position feedback, the first actual time of the first observation node is obtained based on the first theoretical displacement of the first observation node, the position feedback and the time feedback, and the second speed compensation amount is generated based on the difference between the first theoretical displacement and the first actual displacement, and the difference between the second theoretical time of the second observation node and the first actual time; and a speed command generation step, in which the S-curve model is compensated so that the speed in the next communication cycle is offset by the first speed compensation amount, and the entire speed curve between the first observation node and the second observation node is offset by the second speed compensation amount, and the speed command is generated according to the compensated S-curve model.

9. The S-curve closed-loop control method according to claim 8, characterized in that In the second speed compensation amount generation step, the position feedback of the controlled object at the first theoretical time is set as the first actual displacement, the time feedback when it is determined according to the position feedback that the controlled object reaches the first theoretical displacement is set as the first actual time, and the difference between the first theoretical displacement and the first actual displacement is differentiated with respect to the difference between the second theoretical time and the first actual time, thereby calculating the second speed compensation amount.

10. The S-curve closed-loop control method according to claim 8, wherein in the second speed compensation amount generation step, at least one time point is respectively selected from each of the jerk-up stage, constant acceleration stage, jerk-down stage, acceleration-deceleration stage, constant deceleration stage, and jerk-down stage in the S-curve model as the first observation node.

11. The S-curve closed-loop control method according to any one of claims 8 to 10, wherein in the first compensation amount generation step, based on the S-curve model, a relationship formula between displacement and time is obtained, the Cardano formula is solved for the relationship formula between displacement and time to obtain the movement time corresponding to the position feedback, and the movement time is substituted into the S-curve model to calculate the actual speed.

12. The S-curve closed-loop control method according to any one of claims 8 to 10, wherein in the model generation step, the S-curve model is further generated with reference to the maximum acceleration and the maximum jerk. The S-curve closed-loop control method further includes an S-curve adaptive adjustment step. In this S-curve adaptive adjustment step, when the minimum speed calculated according to the maximum acceleration and the maximum jerk is greater than the target speed, the maximum acceleration is reduced so that the minimum speed is equal to the target speed.

13. The S-curve closed-loop control method according to any one of claims 8 to 10, wherein it further includes an overshoot protection step. In this overshoot protection step, the position feedback and the target position are compared. In the overshoot state where the position feedback is greater than the target position, the controlled object is stopped from moving, and the execution process of the S-curve model is latched.

14. The S-curve closed-loop control method according to claim 13, wherein in the overshoot protection step, when the overshoot state is exited, the latch of the execution process of the S-curve model is released, and the controlled object continues to move according to the S-curve model.

15. A computer-readable medium storing a program for executing the S-curve closed-loop control method according to any one of claims 8 to 14.

Images