Simplified S-shaped trajectory planning method and motion control equipment
By simplifying the seven-stage S-curve planning method and combining the input shaping algorithm, the problem of unsatisfactory effects of low-cost motion controllers in vibration suppression is solved, and more efficient trajectory planning and vibration suppression effect is achieved. It is suitable for low-cost controllers Application.
Patent Information
- Application Number
- CN202410952712.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-16
- Publication Date
- 2024-11-12
AI Technical Summary
The low-cost motion controller has poor effect in vibration suppression, mainly due to the low computing power and the incomplete basic function model, the trajectory planning based on iterative computing has high requirements for computing power and the underlying computing library, making it difficult to achieve effective vibration suppression.
The simplified seven-stage S curve planning method is adopted to determine the maximum critical velocity and acceleration critical time by obtaining the initial velocity, initial acceleration and preset maximum rush, maximum velocity and maximum acceleration, and generate the deceleration period time according to the principle of acceleration and deceleration symmetry. , update the simplified S curve, combine the input shaping algorithm to generate the shaping output trajectory, and realize the fast non-zero S-trajectory curve planning.
It improves the control performance and vibration suppression effect of low-cost controllers, reduces the system's computing power requirements, and achieves better trajectory planning and promotion and application.
Smart Images

Figure CN118938800A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of motion control technology, and in particular to a simplified S-type trajectory planning method and a motion control device. Background Art
[0002] Residual vibration suppression of motion control equipment has always been a hot research direction in vibration suppression, with the goal of making the controlled equipment arrive smoothly and eliminating terminal inertia. Common vibration suppression methods include trajectory optimization, flexible force control, and input shaping.
[0003] For general motion control industrial equipment, due to the sensitivity to control costs, low-cost controllers are often used, which have many problems such as low computing power and imperfect basic function models. However, trajectory planning based on iterative calculations has relatively high requirements for computing power and underlying computing libraries, resulting in unsatisfactory vibration suppression effects based on low-cost controllers. Summary of the invention
[0004] An embodiment of the present invention provides a simplified seven-segment S-curve planning method, the method comprising: obtaining an initial velocity and an initial acceleration of a motion control object, determining a maximum critical velocity and an acceleration critical time according to the initial velocity, the initial acceleration, and a preset maximum jerk, a maximum velocity, and a maximum acceleration; generating a deceleration segment time according to the maximum critical velocity and the acceleration critical time in accordance with the acceleration / deceleration symmetry principle, substituting the initial velocity, the initial acceleration, the acceleration critical time, and the deceleration segment time into an initial seven-segment S-curve to obtain a critical distance; calculating a difference between the critical distance and the target distance according to a preset target distance; and updating a simplified seven-segment S-curve according to the difference between the critical distance and the target distance.
[0005] The simplified seven-segment S-curve planning method provided in the embodiment of the present invention can perform simplified trajectory planning based on the initial seven-segment S-curve to achieve quick planning of each time segment. The simplified S-shaped trajectory planning has low system requirements and low computing power requirements, and can be well implemented and promoted. The above-mentioned S-curve planning time is substituted into the input shaping algorithm to generate the shaped output trajectory, which has a good vibration suppression effect and can improve the vibration suppression effect of the low-cost controller.
[0006] Optionally, the simplified seven-segment S curve is updated according to the difference between the critical distance and the target distance, including: if the critical distance is greater than the target distance, reducing the critical time according to the ratio of the critical distance to the target distance; when the step of reducing the critical time is performed until the critical distance is equal to the target distance, the simplified seven-segment S curve is determined according to the reduced critical time; wherein the critical time includes an acceleration critical time and a deceleration time; if the critical distance is less than the target distance, the uniform acceleration time or the uniform speed time of the curve is increased according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance; when the step of increasing the uniform acceleration time or the uniform speed time is performed until the critical distance is equal to the target distance, the simplified seven-segment S curve is determined according to the increased uniform acceleration time or the uniform speed time.
[0007] The embodiment of the present invention specifically adopts a simple critical comparison + time ratio adjustment method to achieve fast non-zero starting S-shaped trajectory curve planning, which improves the control performance of the low-cost controller and is conducive to the realization of control promotion.
[0008] Optionally, reducing the critical time according to the ratio of the critical distance to the target distance includes: substituting the reduced critical time into the initial seven-segment S curve to obtain an updated critical distance; if the updated critical distance is greater than the target distance, continuing to reduce the reduced critical time according to the ratio of the updated critical distance to the target distance; if the updated critical distance is less than the target distance, increasing the uniform speed segment time of the curve according to the difference between the updated critical distance and the target distance.
[0009] After the proportion is adjusted in the embodiment of the present invention, if the critical distance is less than the target distance, the uniform speed period can be further increased to achieve trajectory curve planning.
[0010] Optionally, the increasing the uniform acceleration segment time or the uniform speed segment time of the curve according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance includes: when the maximum critical speed is less than the maximum speed, increasing the uniform acceleration segment time of the trajectory curve according to the difference between the maximum critical speed and the maximum speed until the maximum speed is reached, and calculating the increased distance according to the increased uniform acceleration segment time; if the increased distance is less than the difference between the critical distance and the target distance, calculating an updated critical distance according to the critical distance and the increased distance; if the increased distance is greater than the difference between the critical distance and the target distance, reducing the uniform acceleration segment time according to the ratio of the increased distance to the difference, and then calculating an updated critical distance according to the reduced uniform acceleration segment time; when the updated critical distance is less than the critical distance, increasing the uniform speed segment time of the curve according to the difference between the updated critical distance and the target distance to compensate for the difference between the critical distance and the target distance.
[0011] In the embodiment of the present invention, whether to increase the time of the uniform acceleration section or the uniform speed section needs to consider the relationship between the maximum critical speed and the preset maximum speed, and flexibly plan the trajectory curve.
[0012] Optionally, the uniform acceleration segment time or the uniform speed segment time of the curve is increased according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance, including: if the maximum critical speed is greater than or equal to the maximum speed, then the uniform speed segment time is increased to compensate for the difference between the critical distance and the target distance.
[0013] In the embodiment of the present invention, whether to increase the time of the uniform acceleration section or the uniform speed section needs to consider the relationship between the maximum critical speed and the preset maximum speed, and flexibly plan the trajectory curve.
[0014] Optionally, the maximum critical speed and acceleration critical time are determined according to the initial velocity, the initial acceleration, and the preset maximum jerk, maximum speed and maximum acceleration, including: if the initial velocity is less than the maximum velocity, and the initial acceleration is less than the maximum acceleration, then the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum acceleration as the critical condition, and the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum velocity as the critical condition, and the maximum critical speed is calculated according to the smaller of the two acceleration critical times; if the initial velocity is less than the maximum velocity, and the initial acceleration is greater than or equal to the maximum acceleration, then the acceleration critical time of the trajectory including only the acceleration deceleration segment is calculated; and the maximum critical speed is calculated according to the acceleration critical time.
[0015] In the embodiment of the present invention, for non-zero starting S-curve planning, the acceleration phase of the S-curve is planned in combination with the initial velocity and initial acceleration of the motion control object under the constraints of the preset maximum jerk, maximum velocity and maximum acceleration.
[0016] Optionally, the maximum critical speed and the acceleration critical time are determined according to the initial velocity, the initial acceleration, and the preset maximum jerk, the maximum speed and the maximum acceleration, including: if the initial velocity is greater than or equal to the maximum speed, and the initial acceleration is greater than or equal to zero, then the acceleration critical time of the trajectory including only the acceleration deceleration section is calculated, and the maximum critical speed is calculated according to the acceleration critical time; if the initial velocity is greater than or equal to the maximum speed, and the initial acceleration is less than zero, then the speed when the initial acceleration rises to zero is compared with the maximum speed; when the speed when the initial acceleration rises to zero is greater than or equal to the maximum speed, then the speed when the initial acceleration rises to zero is the maximum critical speed, and the time when the initial acceleration rises to zero is the acceleration critical time of the trajectory of the acceleration deceleration section; when the speed when the initial acceleration rises to zero is less than the maximum speed, the symmetrical acceleration increase section and the acceleration deceleration section are compensated to the maximum speed to obtain the acceleration critical time and the maximum critical speed.
[0017] In the embodiment of the present invention, for non-zero starting S-curve planning, the acceleration phase of the S-curve is planned in combination with the initial velocity and initial acceleration of the motion control object under the constraints of the preset maximum jerk, maximum velocity and maximum acceleration.
[0018] Optionally, the method further includes: performing oscillation identification to obtain an oscillation curve when the motion control object operates according to the simplified seven-segment S-curve, outputting the damping ratio and undamped natural frequency of the oscillation curve, and generating an input shaped pulse; performing Laplace convolution on the simplified seven-segment S-curve and the input shaped pulse to generate a shaped output trajectory.
[0019] An embodiment of the present invention provides a motion control device, including a controller; the controller is used to execute any one of the methods described above.
[0020] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is read and executed by a processor, any of the above methods is implemented.
[0021] The motion control device and computer-readable storage medium provided by the embodiments of the present invention can achieve the same technical effect as the aforementioned simplified seven-segment S-curve planning method. BRIEF DESCRIPTION OF THE DRAWINGS
[0022] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.
[0023] Figure 1 A schematic flow chart of a simplified seven-segment S-curve planning method provided by an embodiment of the present invention;
[0024] Figure 2 A schematic diagram of a process for planning a maximum critical speed and a critical time provided by an embodiment of the present invention;
[0025] Figure 3 A schematic flow chart of a simplified non-zero start trajectory planning method provided by an embodiment of the present invention. DETAILED DESCRIPTION
[0026] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0027] The present invention is further described in detail below through specific embodiments in conjunction with the accompanying drawings.
[0028] The embodiment of the present invention provides an input shaping algorithm based on simplified seven-segment S-curve planning, based on simplified non-zero starting S trajectory planning, calculates the S-shaped trajectory, and identifies the oscillation of the running trajectory; based on the terminal vibration, identifies the damping ratio and natural frequency of the oscillation curve and generates an input shaping pulse; performs Laplace convolution on the original curve and the input shaping pulse to generate a shaped output trajectory, and drives the operation.
[0029] The existing seven-segment S-curve includes acceleration increase segment, uniform acceleration segment, acceleration deceleration segment, uniform speed segment, deceleration increase segment, uniform deceleration segment, and deceleration deceleration segment. In practical applications, based on different parameters such as target distance, initial speed, acceleration, etc., the actual speed curve can be simplified into six-segment, five-segment, or four-segment S-curves.
[0030] For zero-start and zero-drop planning, we can implement position-based speed planning based on the principle of acceleration and deceleration symmetry. However, in actual engineering applications, speed switching is often performed with the initial velocity included, so trajectory planning based on non-zero start is more practical.
[0031] It is more difficult to achieve acceleration curve planning based on position constraints for non-zero-starting S-shaped trajectory planning. Not only is the calculation difficult, but there are also many constraints. Therefore, a simple critical comparison + time ratio adjustment method is used in the embodiment of the present invention to achieve fast position curve planning.
[0032] Given the planned initial velocity v0, initial acceleration a0, and jerk as a constant J, the system's 7-segment acceleration formula (1), velocity formula (2), and position formula (3) are:
[0033] Acceleration formula (1)
[0034] a:=(a0+J*t)*(heaviside(t)-heaviside(tt l ))+a mx *(heaviside(tt l )-heaviside(t-t2))+(a mx -J*(t-t2))*(heaviside(t-t2)-heaviside(t-t3))+0*(heaviside(t-t3)-heaviside(t-t4))+(-J*(t-t4))*(heaviside(t-t4)-heaviside(t-t5))+(-a mxl )*(heaviside(t-t5)-heaviside(t-t6))+(- amxl +J*(t-t6))*(heaviside(t-t6)-heaviside(t-t7))
[0035] Speed formula (2)
[0036] v:=(v0+a0*t+0.5*J*t*t)*(heaviside(t)-heaviside(tt l ))+(v l +a mx *(tt l ))*(heaviside(tt l )-heaviside(t-t2))+(v2+(a mx*(t - t2) - 0.5*J*t*t + 0.5*J*t2*t2) + J*t2*(t - t2))*(heaviside(t - t2) - heaviside(t - t3)) + v3*(heaviside(t - t3) - heaviside(t - t4)) + (v3 + (-0.5*J*t*t + 0.5*J*t4*t4) + J*t4*(t - t4))*(heaviside(t - t4) - heaviside(t - t5)) + (v5 + (-a mxl *t + a mxl *t5))*(heaviside(t - t5) - heaviside(t - t6)) + (v6 + (-a mx1 *(t - t6) + 0.5*J*t*t - 0.5J*t6*(t - t6)))*(heaviside(t - t6) - heaviside(t - t7))
[0037] Position formula (3)
[0038] TarPos := (v0*t + 0.5*a0*t*t + 1.0 / 6*J*t*t*t)*(heaviside(t) - heaviside(t - t l )) + (s1 +
[0039] ((t - t l )*(2*v l +a mx *t - a mx *t l )) / 2)*(heaviside(t - t l ) - heaviside(t - t2)) + (s2 + ((t - t2)*(-J*t*t + 2*J*t*t2 + 3*a mx *t - j*t2*t2 - 3*a mx *t2 + 6*v2)) / 6)*(heaviside(t - t2) - heaviside(t - t3)) + (s3 + v3*(t - t3))*(heaviside(t - t3) - heaviside(t - t4)) + (s4 + ((t - t4)*(-J*t*t + 2*J*t*t4 - J*t4*t4 + 6*v3)) / 6)*(heaviside(t - t4) - heaviside(t - t5)) + (s5 + ((t - t5)*(2*v5 - a mxl *t + a mxl*t5)) / 2)*(he aviside(t-t5)-heaviside(t-t6))+(s6+((t-t6)*(J*t*t-2*J*t*t6-3*a mxl *t+J*t6*t6+3*a mxl *t6+6*v6)) / 6)*(heaviside(t-t6)-heaviside(t-t7)).
[0040] In the formula, a mx is the maximum positive acceleration, a mx1 is the maximum negative acceleration; v1, v2, s1, s2, etc. are the speed and position corresponding to each node of the seven-segment S-curve.
[0041] From formulas (2) and (3), we can see that the position and speed of each segment are related to the speed and position of the previous segment. The change of the speed of the previous segment will affect the position relationship of the subsequent acceleration segment. In actual trajectory planning, trajectory planning needs to meet multiple constraints. In the position-based constraints, directly calculating the acceleration segment time based on position iteration requires high computing power and takes a long time. The method based on critical condition comparison can quickly realize the planning of each segment time.
[0042] Figure 1 A schematic flow chart of a simplified seven-segment S-curve planning method provided by an embodiment of the present invention is shown, and the method includes:
[0043] S102, obtaining an initial velocity and an initial acceleration of the motion control object, and then determining a maximum critical speed and an acceleration critical time according to the initial velocity, initial acceleration and a preset maximum jerk, maximum speed and maximum acceleration.
[0044] Among them, the maximum jerk, maximum velocity and maximum acceleration are attribute parameters of the motion control object, which can be determined by hardware conditions or actively set.
[0045] The maximum critical speed is the maximum speed that the motion control object can reach based on the above parameters, and the acceleration critical time is the time required for each stage in the process of reaching the above maximum speed, which may only include one or more stages of the acceleration increase segment, the uniform acceleration segment, and the acceleration deceleration segment. Exemplarily, in this step, the acceleration segment of the curve is planned based on the above parameters, and the deceleration segment of the curve can be planned later by the acceleration and deceleration symmetry principle.
[0046] It should be noted that, depending on the initial velocity and initial acceleration, the maximum speed that can be achieved (which may be greater than or equal to the initial velocity) is different, and the actually required stages are also different.
[0047] S104, based on the above-mentioned maximum critical speed and acceleration critical time, generate the deceleration section time according to the acceleration and deceleration symmetry principle, and substitute the initial speed, initial acceleration, acceleration critical time and deceleration section time into the initial seven-segment S curve to obtain the critical distance.
[0048] The seven-segment S-curve takes the midpoint of the uniform speed segment as the symmetry center. Each stage of the acceleration segment is symmetrical with each stage of the deceleration segment about the symmetry center, including the slope, duration, and value of each stage. After obtaining the deceleration segment time based on the above symmetry principle, the critical distance can be substituted into the initial seven-segment S-curve to calculate. The critical distance is the actual distance traveled by the motion control target based on the above parameters.
[0049] S106, calculating the difference between the critical distance and the target distance according to the preset target distance.
[0050] The preset target distance is the distance that the motion control object needs to move. If the critical distance is not equal to the target distance, the stages of the seven-segment S curve need to be adjusted based on the difference until the critical distance is equal to the target distance, and the curve planning is completed.
[0051] S108, updating a simplified seven-segment S-curve according to the difference between the critical distance and the target distance.
[0052] The seven-segment S-curve determined by the above parameters is adjusted based on whether the critical distance is greater than or less than the target distance. Optionally, if the critical distance is greater than the target distance, the time of each stage is reduced as a whole by proportionally reducing the critical time, thereby reducing the critical distance. If the critical distance is less than the target distance, the time of the uniform acceleration segment or the uniform speed segment of the above curve is increased.
[0053] Whether to increase the time of the uniform acceleration segment or the uniform speed segment needs to consider the relationship between the maximum critical speed and the preset maximum speed. When the maximum critical speed is less than the maximum speed, it means that there is still a certain amount of room for acceleration. You can increase the time of the uniform acceleration segment by increasing the speed of the motion control object to increase the critical distance. When the maximum critical speed is greater than or equal to the maximum speed, it means that there is no room for acceleration. You can increase the time of the uniform speed segment by extending the duration of the motion control object's uniform forward movement to increase the critical distance.
[0054] If the critical distance is greater than the target distance, the critical time is reduced according to the ratio of the critical distance to the target distance; when the step of reducing the critical time is executed until the critical distance is equal to the target distance, the simplified seven-segment S curve is determined according to the reduced critical time; wherein the critical time includes the acceleration critical time and the deceleration segment time.
[0055] Specifically, the critical time is reduced based on the following method, including: substituting the reduced critical time into the initial seven-segment S-curve to obtain an updated critical distance; if the updated critical distance is greater than the target distance, then continuing to reduce the reduced critical time according to the ratio of the updated critical distance to the target distance; if the updated critical distance is less than the target distance, then increasing the uniform speed segment time of the curve according to the difference between the updated critical distance and the target distance.
[0056] Among them, since the critical distance is greater than the target distance, the ratio of the critical distance to the target distance is greater than 1. The acceleration critical time and the deceleration segment time are divided by this ratio to obtain the reduced critical time. Based on this critical time, the updated critical distance can be calculated. Further, the updated critical distance is compared with the target distance. If the updated critical distance is still greater than the target distance, the critical time is reduced again in the same way; if the updated critical distance is less than the target distance, it is compensated by increasing the uniform speed segment time until the critical distance is equal to the target distance.
[0057] If the critical distance is less than the target distance, the uniform acceleration segment time or the uniform speed segment time of the curve is increased according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance; when the step of increasing the uniform acceleration segment time or the uniform speed segment time is executed until the critical distance is equal to the target distance, the simplified seven-segment S curve is determined according to the increased uniform acceleration segment time or the uniform speed segment time.
[0058] Specifically, the uniform acceleration time or uniform speed time of the added curve is determined based on the following methods to achieve simplified S-shaped trajectory planning, including:
[0059] (1) The maximum critical speed is less than the maximum speed
[0060] When the maximum critical speed is less than the maximum speed, the uniform acceleration time of the trajectory curve is increased according to the difference between the maximum critical speed and the maximum speed until the maximum speed is reached, and the increased distance is calculated according to the increased uniform acceleration time. By increasing the uniform acceleration time, the maximum critical speed can be increased to the maximum speed, thereby increasing the critical distance.
[0061] If the increased distance is less than the difference between the critical distance and the target distance, the updated critical distance is calculated based on the critical distance and the increased distance. If the increased distance is less than the difference, it means that the time of the uniform acceleration segment needs to be further increased. The updated critical distance can be obtained by summing the critical distance and the increased distance. If the increased distance is greater than the difference between the critical distance and the target distance, the uniform acceleration segment time is reduced according to the ratio of the increased distance and the difference, and then the updated critical distance is calculated based on the reduced uniform acceleration segment time. If the increased distance is greater than the difference, it means that the uniform acceleration segment time is too long, and the uniform acceleration segment time can be further reduced by using the aforementioned proportional reduction of the critical time. The updated critical distance can be calculated based on the reduced uniform acceleration segment time.
[0062] When the updated critical distance is less than the critical distance, the uniform speed section time of the curve is increased according to the difference between the updated critical distance and the target distance to compensate for the difference between the critical distance and the target distance. After the above uniform acceleration section adjustment, if the updated critical distance is less than the critical distance, the uniform speed section adjustment can be further adopted. By increasing the time of the uniform speed section, the critical distance is increased until the critical distance is equal to the target distance, and then the simplified seven-segment S curve is determined according to the increased uniform acceleration section time and uniform speed section time.
[0063] (2) The maximum critical speed is greater than or equal to the maximum speed
[0064] If the maximum critical speed is greater than or equal to the maximum speed, the uniform speed segment time is increased to compensate for the difference between the critical distance and the target distance. If the maximum critical speed is greater than or equal to the maximum speed, there is no room for acceleration. The uniform speed segment time is increased to extend the duration of the motion control object's uniform forward movement, thereby increasing the critical distance.
[0065] The simplified seven-segment S-curve planning method provided in the embodiment of the present invention can perform simplified trajectory planning based on the initial seven-segment S-curve to achieve quick planning of each time segment. The simplified S-shaped trajectory planning has low system requirements and low computing power requirements, and can be well implemented and promoted. The above-mentioned S-curve planning time is substituted into the input shaping algorithm to generate the shaped output trajectory, which has a good vibration suppression effect and can improve the vibration suppression effect of the low-cost controller.
[0066] Among them, the embodiment of the present invention specifically adopts a simple critical comparison + time ratio adjustment method to achieve fast non-zero starting S-shaped trajectory curve planning, which improves the control performance of the low-cost controller and is conducive to the realization of control promotion.
[0067] For non-zero starting S-curve planning, this embodiment combines the initial velocity and initial acceleration of the motion control object, and plans the acceleration phase of the S-curve under the condition of the preset maximum jerk, maximum velocity and maximum acceleration as constraints. Based on this, the curve planning is carried out with the size of the initial velocity and the maximum velocity, the size of the initial acceleration and the maximum acceleration, and the positive and negative of the initial acceleration as conditions, as follows:
[0068] (1) If the initial velocity is less than the maximum velocity, and the initial acceleration is less than the maximum acceleration, the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum acceleration as the critical condition, and the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum velocity as the critical condition. The maximum critical velocity is calculated based on the smaller of the two acceleration critical times. When accelerating, the maximum acceleration and the maximum velocity are used as the critical conditions respectively to generate the acceleration critical time without uniform acceleration and without uniform velocity segment, and the smaller value is substituted into the maximum critical velocity.
[0069] If the initial velocity is less than the maximum velocity, and the initial acceleration is greater than or equal to the maximum acceleration, the acceleration critical time of the trajectory including only the acceleration deceleration segment is calculated; and the maximum critical velocity is calculated based on the acceleration critical time. If the initial acceleration is greater than or equal to the maximum acceleration, there is no acceleration increase segment, and the acceleration deceleration segment is directly entered with the initial acceleration.
[0070] (2) If the initial velocity is greater than or equal to the maximum velocity, and the initial acceleration is greater than or equal to zero, the acceleration critical time of the trajectory that only includes the acceleration deceleration segment is calculated, and the maximum critical velocity is calculated based on the acceleration critical time. If the initial velocity is greater than or equal to the maximum velocity, there is no acceleration increase segment, and the acceleration deceleration segment is directly entered at the initial velocity.
[0071] If the initial velocity is greater than or equal to the maximum velocity, and the initial acceleration is less than zero, then compare the velocity when the initial acceleration rises to zero with the maximum velocity. If the initial acceleration is less than zero, the target is in a deceleration state, and its acceleration is changed to zero. If the obtained velocity is compared with the maximum velocity, if it is greater than or equal to the maximum velocity, then determine the critical acceleration time based on this state; if it is less than the maximum velocity, then determine the critical acceleration time based on this state after increasing to the maximum velocity.
[0072] When the speed when the initial acceleration rises to zero is greater than or equal to the maximum speed, the speed when the initial acceleration rises to zero is taken as the maximum critical speed, and the time when the initial acceleration rises to zero is taken as the acceleration critical time of the trajectory of the acceleration deceleration section.
[0073] When the speed when the initial acceleration rises to zero is less than the maximum speed, the acceleration increasing section and the acceleration decelerating section are used to compensate to the maximum speed, and the acceleration critical time and the maximum critical speed are obtained.
[0074] On the basis of the simplified seven-segment S curve, the terminal vibration identification and shaping can also be included, so that the controlled device can arrive smoothly, eliminate the terminal inertia, and play a role in residual vibration suppression. Based on this, the above method can also include:
[0075] When the motion control object runs according to the simplified seven-segment S-curve, oscillation identification is performed to obtain the oscillation curve, the damping ratio and undamped natural frequency of the oscillation curve are output, and an input shaped pulse is generated; Laplace convolution is performed on the simplified seven-segment S-curve and the input shaped pulse to generate a shaped output trajectory.
[0076] Exemplarily, after obtaining the seven-segment S-shaped planning time, this embodiment also needs to run the planned trajectory and obtain the actual vibration of the terminal based on a laser displacement sensor, etc.; combined with low-pass filtering, based on the attenuation response of the curve, the damping ratio and undamped natural frequency of the terminal oscillation are fitted; the original seven-segment trajectory curve and damping parameters obtained are substituted into the convolution pulse, and Laplace transform is performed. The generated convolution is inversely transformed to output the actual control motion trajectory.
[0077] The following is a detailed description of an exemplary process of an input shaping algorithm based on simplified S-curve planning provided by an embodiment of the present invention. The algorithm mainly includes three steps: (1) simplified non-zero starting trajectory planning; (2) terminal vibration, identification of oscillation curve; (3) Laplace transform and convolution output.
[0078] (1) Simplified non-zero start trajectory planning
[0079] Figure 2 FIG. 4 shows a schematic diagram of a process for planning the maximum critical speed and critical time in this embodiment. Figure 2 On the basis of Figure 3 A schematic flow chart of a simplified non-zero starting trajectory planning method in this embodiment is shown.
[0080] like Figure 2 As shown, the method comprises the following steps:
[0081] S201, input the maximum speed, maximum acceleration and jerk for a given distance.
[0082] S202, reading the initial velocity and initial acceleration.
[0083] S203, speed reduction check.
[0084] S204, determine whether the speed reduction can be completed. If yes, execute S205; if no, execute S218.
[0085] If the initial velocity is not zero, first check the maximum jerk to decelerate, and check whether the deceleration can be completed within the target distance. If not, decelerate and output an error.
[0086] S205, initial velocity and initial acceleration check.
[0087] S206, determine whether the initial speed is greater than the maximum speed. If yes, execute S210; if no, execute S207.
[0088] S207, calculating the acceleration and deceleration time with the actual maximum acceleration as the critical condition.
[0089] S208, using the maximum speed as the critical condition, calculate the acceleration and deceleration time
[0090] S209, taking the lower time, and calculating the maximum critical speed.
[0091] Based on the maximum acceleration and maximum speed, check the initial velocity and initial acceleration. If the initial velocity is less than the maximum speed, generate the critical acceleration time without uniform acceleration and without uniform speed segment from the two aspects of maximum acceleration and maximum speed, and take the smaller value to generate the maximum critical speed; among them, if the maximum initial acceleration is greater than the maximum acceleration, there is no acceleration increase segment, and directly enters the acceleration deceleration segment with the initial acceleration.
[0092] S210, determining whether the initial acceleration is greater than 0. If yes, executing S211; if no, executing S212.
[0093] S211, no acceleration adding segment, directly generating the deceleration time.
[0094] S212, when the acceleration drops to zero, determine whether the speed is greater than the maximum speed. If so, execute S213; if not, execute S214.
[0095] S213, there is no acceleration reduction segment, and the time when the acceleration rises to zero is taken as the acceleration addition segment.
[0096] S214, using symmetrical acceleration and deceleration compensation time to reach the maximum speed.
[0097] S215: Generate a maximum critical speed based on the planned time.
[0098] If the initial velocity is greater than the maximum velocity, it is necessary to calculate the velocity when the acceleration drops to zero based on the magnitude and direction of the acceleration, plan the critical acceleration time without uniform acceleration and without uniform speed segments, and substitute it to generate the maximum critical velocity.
[0099] S216: Based on the critical speed, the acceleration and deceleration time of the deceleration section is generated according to the acceleration and deceleration symmetry principle.
[0100] Based on the maximum critical speed and critical time, the deceleration time is generated according to the acceleration and deceleration symmetry principle.
[0101] S217, calculating the critical distance.
[0102] S218, execute deceleration and output an error.
[0103] like Figure 3 As shown, in Figure 2 Based on the process shown, the method also includes the following steps:
[0104] S219, determine whether the critical distance is greater than a given distance. If yes, execute S220; if no, execute S224.
[0105] Based on the generated critical time, substitute it into the 7-segment trajectory formula and output the critical distance.
[0106] S220, reduce the planning time in proportion.
[0107] S221, generating the maximum critical speed and deceleration time.
[0108] S222, update the planned distance.
[0109] If the critical distance is greater than the given distance, the critical time is proportionally reduced according to the ratio of the two.
[0110] S223, determine whether the planned distance is greater than the given distance. If so, return to execute S220; if not, execute S230.
[0111] S224, determining whether the critical maximum speed is less than the maximum speed.
[0112] S225, increasing the uniform acceleration time period based on the maximum speed difference.
[0113] S226, using an approximate method to calculate the increased distance.
[0114] S227, determine whether the approximate distance is less than a given error distance. If so, execute S229; if not, execute S228.
[0115] S228, reducing the uniform acceleration time based on the ratio.
[0116] If the critical distance is less than the given distance, it is necessary to combine the position difference and increase the uniform acceleration time to the maximum speed to approach the overall optimal performance. It is also necessary to estimate the increased distance based on the increased speed. The distance approximation formula is:
[0117] tal:=(v mx -v3) / a mx ;
[0118] dt3:=t3-t l ;
[0119] dt5:=t5-t4;
[0120] addv:=tal*a mx ;
[0121] addpos:=(2*v l +addv)*tal+addv*(dt3+dt5) (4)
[0122] Where v3 is the maximum critical speed, v mx is the maximum speed, t1, t3, etc. are the seven-segment planning time nodes, a mx is the planned maximum acceleration.
[0123] S229, update the planned distance.
[0124] S230, compensation for insufficient distance in the uniform speed segment.
[0125] Finally, the remaining distance is compensated through a uniform speed segment.
[0126] S231, output 7-segment time planning and maximum positive and negative acceleration.
[0127] (2) Terminal vibration, identification of oscillation curve
[0128] Based on the oscillation identification of the terminal, the oscillation curve is generated, and the damping ratio and undamped natural frequency are output.
[0129] The specific implementation steps are:
[0130] 2.1. Set parameters such as target position and running speed, output time planning based on simplified S-curve, and execute motion control;
[0131] 2.2. Install an infrared distance sensor at the target location, read the end distance, and generate an oscillation curve;
[0132] 2.3. Based on the generated oscillation curve, combined with the low-pass filter curve, and based on the attenuation response, the damping ratio and the undamped natural frequency are output. The attenuation response algorithm is as follows:
[0133] The damping ratio adopts the attenuation response method. The envelope of the second-order system unit impulse response is attenuated. Based on the relationship between two consecutive peaks and the damping ratio, the system damping ratio can be calculated.
[0134] Decaying response of a damped single degree of freedom:
[0135]
[0136] Where C is the amplitude, w n is the natural frequency, is the phase angle, ξ is the damping ratio.
[0137]
[0138] The displacement attenuation signal envelope of its single degree of freedom vibration is:
[0139]
[0140] The amplitude adjacent ratio is:
[0141]
[0142] T d is the vibration period. And:
[0143]
[0144] The logarithmic reduction ratio can be calculated using the ratio of two amplitudes of adjacent i cycles:
[0145]
[0146] Among them A n is the amplitude value of the nth cycle, then:
[0147]
[0148]
[0149] (3) Laplace transform and convolution output
[0150] For example, the original curve is convolved with the ZVD (Zero Vibration and Derivative Shaper) input to generate the target trajectory. The specific execution steps are as follows:
[0151] 3.1. Generate ZVD input pulse based on oscillation damping. Based on the constraint conditions, the calculation formula of ZVD input pulse impulse response R is:
[0152] R=K1*dirac(t)+K2*dirac(t-T2)+K3*dirac(t-T3) (12)
[0153] in:
[0154]
[0155] 3.2. Laplace transform and convolution, and output
[0156] The original function and the ZVD pulse response are Laplace transformed respectively, and the obtained Laplace equations F1(s) and F2(s) are multiplied and then subjected to inverse Laplace transform to output the convolved response curve.
[0157] 3.3. Verify output
[0158] Based on the response curve after convolution output, drive the device to run and check the terminal error. If it does not meet the application requirements, re-iterate the calculation.
[0159] This embodiment simplifies the S-shaped trajectory planning based on the above step (1). Since the existing motion controller has limited computing power, the trajectory planning based on iterative calculation has relatively high requirements on computing power and underlying computing library. This embodiment adopts simplified S-shaped trajectory planning, which has lower requirements on the system and computing power, and can be well implemented and promoted;
[0160] The algorithm of the above steps (2) and (3) needs to be combined with the autonomous trajectory, which is an application innovation. The final output of the shaped S-shaped trajectory is an optimized S-shaped trajectory combined with the actual working conditions.
[0161] This embodiment also provides a motion control device, including a controller; the controller is used in the above-mentioned simplified seven-segment S-curve planning method.
[0162] This embodiment also provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, each process of the above method embodiment is implemented, and the same technical effect can be achieved. To avoid repetition, it is not repeated here. The computer-readable storage medium is, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.
[0163] Of course, those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the control device through a computer, and the program can be stored in a computer-readable storage medium. When the program is executed, it may include the processes of the above-mentioned method embodiments, wherein the storage medium may be a memory, a disk, an optical disk, etc.
[0164] Although the present invention is disclosed as above, the present invention is not limited thereto. Any person skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the scope defined by the claims.
[0165] Finally, it should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.
[0166] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the motion control device disclosed in the embodiment, since it corresponds to the simplified seven-segment S-curve planning method disclosed in the above embodiment, the description is relatively simple, and the relevant parts can be referred to the method part description.
[0167] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A simplified seven-segment S-curve planning method, It is characterized in that The method comprises: Obtaining an initial velocity and an initial acceleration of the motion control object, and determining a maximum critical speed and an acceleration critical time according to the initial velocity, the initial acceleration, and a preset maximum jerk, maximum speed, and maximum acceleration; According to the maximum critical speed and the acceleration critical time, the deceleration time is generated according to the acceleration and deceleration symmetry principle, and the initial speed, the initial acceleration, the acceleration critical time and the deceleration time are substituted into the initial seven-segment S curve to obtain the critical distance; According to a preset target distance, calculating the difference between the critical distance and the target distance; According to the difference between the critical distance and the target distance, a simplified seven-segment S-curve is updated.
2. The method according to claim 1, It is characterized in that The simplified seven-segment S-curve is updated according to the difference between the critical distance and the target distance, including: If the critical distance is greater than the target distance, the critical time is reduced according to the ratio of the critical distance to the target distance; when the step of reducing the critical time is performed until the critical distance is equal to the target distance, a simplified seven-segment S curve is determined according to the reduced critical time; wherein the critical time includes an acceleration critical time and a deceleration segment time; If the critical distance is less than the target distance, the uniform acceleration segment time or the uniform speed segment time of the curve is increased according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance; when the step of increasing the uniform acceleration segment time or the uniform speed segment time is executed until the critical distance is equal to the target distance, the simplified seven-segment S curve is determined according to the increased uniform acceleration segment time or the uniform speed segment time.
3. The method according to claim 2, It is characterized in that The reducing the critical time according to the ratio of the critical distance to the target distance comprises: Substituting the reduced critical time into the initial seven-segment S-curve to obtain an updated critical distance; If the updated critical distance is greater than the target distance, then continuing to reduce the reduced critical time according to a ratio of the updated critical distance to the target distance; If the updated critical distance is less than the target distance, the uniform speed segment time of the curve is increased according to the difference between the updated critical distance and the target distance.
4. The method according to claim 2, It is characterized in that The step of increasing the uniform acceleration time or uniform speed time of the curve according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance includes: In the case where the maximum critical speed is less than the maximum speed, increasing the uniform acceleration time of the trajectory curve according to the difference between the maximum critical speed and the maximum speed until the maximum speed is reached, and calculating the increased distance according to the increased uniform acceleration time; If the increased distance is less than the difference between the critical distance and the target distance, the updated critical distance is calculated based on the critical distance and the increased distance; if the increased distance is greater than the difference between the critical distance and the target distance, the uniform acceleration period is reduced according to the ratio of the increased distance to the difference, and the updated critical distance is calculated based on the reduced uniform acceleration period; In the case where the updated critical distance is less than the critical distance, the uniform speed segment time of the curve is increased according to the difference between the updated critical distance and the target distance to compensate for the difference between the critical distance and the target distance.
5. The method according to claim 2, It is characterized in that The step of increasing the uniform acceleration time or uniform speed time of the curve according to the difference between the critical distance and the target distance to compensate for the difference between the critical distance and the target distance includes: If the maximum critical speed is greater than or equal to the maximum speed, the uniform speed period is increased to compensate for the difference between the critical distance and the target distance.
6. The method according to claim 1, It is characterized in that Determining the maximum critical speed and the critical acceleration time according to the initial speed, the initial acceleration, and the preset maximum jerk, maximum speed, and maximum acceleration includes: If the initial velocity is less than the maximum velocity, and the initial acceleration is less than the maximum acceleration, the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum acceleration as the critical condition, and the acceleration critical time of the curve including only the acceleration increase segment and the acceleration deceleration segment is calculated with the maximum velocity as the critical condition, and the maximum critical velocity is calculated according to the smaller one of the two acceleration critical times; If the initial velocity is less than the maximum velocity, and the initial acceleration is greater than or equal to the maximum acceleration, then the acceleration critical time of the trajectory including only the acceleration deceleration segment is calculated; and the maximum critical velocity is calculated based on the acceleration critical time.
7. The method according to claim 1, It is characterized in that Determining the maximum critical speed and the critical acceleration time according to the initial speed, the initial acceleration, and the preset maximum jerk, maximum speed, and maximum acceleration includes: If the initial velocity is greater than or equal to the maximum velocity, and the initial acceleration is greater than or equal to zero, then the acceleration critical time of the trajectory including only the acceleration deceleration segment is calculated, and the maximum critical velocity is calculated according to the acceleration critical time; If the initial velocity is greater than or equal to the maximum velocity, and the initial acceleration is less than zero, then the velocity when the initial acceleration rises to zero is compared with the maximum velocity; when the velocity when the initial acceleration rises to zero is greater than or equal to the maximum velocity, then the velocity when the initial acceleration rises to zero is used as the maximum critical velocity, and the time when the initial acceleration rises to zero is used as the acceleration critical time of the trajectory of the acceleration deceleration section; when the velocity when the initial acceleration rises to zero is less than the maximum velocity, the acceleration increase section and the acceleration deceleration section are symmetrically compensated to the maximum velocity to obtain the acceleration critical time and the maximum critical velocity.
8. The method according to any one of claims 1 to 7, It is characterized in that The method further comprises: When the motion control object is running according to the simplified seven-segment S-curve, oscillation identification is performed to obtain an oscillation curve, a damping ratio and an undamped natural frequency of the oscillation curve are output, and an input shaping pulse is generated; The simplified seven-segment S-curve and the input shaped pulse are Laplace convolved to generate a shaped output trajectory.
9. A motion control device, It is characterized in that The method comprises a controller; the controller is used to execute the method according to any one of claims 1 to 8.
10. A computer-readable storage medium, It is characterized in that The computer-readable storage medium stores a computer program, and when the computer program is read and executed by a processor, the method according to any one of claims 1 to 8 is implemented.
