S-shaped trajectory planning method, device, computer equipment and storage medium

Abstract

The present application relates to an S-shaped trajectory planning method, device, computer equipment and storage medium, which obtains the current motion parameter value of the tool, the original planned trajectory and the modified planned trajectory parameter value during the movement of the tool, determines the target speed and the terminal speed, replans the S-shaped speed change trajectory, and then uses the newly planned S-shaped speed change trajectory for subsequent interpolation. The deviation caused by real-time environmental factors to the interpolation process is reduced, and the accuracy of interpolation is improved.

Description

Technical Field

[0001] The present invention relates to the field of motion control, and in particular to an S-shaped trajectory planning method, device, computer equipment and computer-readable storage medium. Background Art

[0002] The S-shaped speed change control strategy has continuous acceleration during the speed change process, which reduces the flexible impact introduced by the acceleration mutation and improves the stability of the machine tool. Therefore, it is widely used in the field of CNC. The traditional S-shaped trajectory planning is to calculate the speed-time curve according to the parameter information of each tool path segment in an offline environment according to the selected speed change strategy. When interpolating, it is interpolated according to the speed-time curve.

[0003] In the actual machining process, the actual tool path trajectory may deviate from the pre-planned tool path trajectory due to errors caused by other factors such as motor performance and cutting heat, and the parameter information used for speed planning may change. At this time, continuing to use the original planned trajectory for interpolation may cause deviations or even cause the tool path trajectory to deviate completely from expectations. Summary of the invention

[0004] Based on this, it is necessary to provide an S-shaped trajectory planning method, device, computer equipment and computer-readable storage medium to address the above technical problems.

[0005] An S-shaped trajectory planning method, comprising:

[0006] Get the current motion parameter values ​​and original planned trajectory of the tool.

[0007] During the movement of the tool, the modified planned trajectory parameter values ​​are obtained.

[0008] According to the modified planned trajectory parameter values, the current motion parameter values ​​and the original planned trajectory, the target speed and terminal speed of the S-shaped speed change trajectory are determined.

[0009] The current operating speed segment of the tool is determined based on the target speed, terminal speed and current motion parameter values.

[0010] According to the current operating speed section, the target operating speed section included in the S-shaped speed change trajectory is determined.

[0011] According to the target running speed segment, the target speed, the terminal speed and the current motion parameter value, a target motion parameter value of the target running speed segment is determined.

[0012] According to the target motion parameter value, the S-shaped speed change trajectory is determined.

[0013] The tool is controlled to continue moving according to the S-shaped speed change trajectory.

[0014] An S-shaped trajectory planning device, comprising:

[0015] The parameter value acquisition module is used to obtain the current motion parameter value and the original planned trajectory of the tool.

[0016] The parameter value acquisition module is also used to acquire the modified planned trajectory parameter value during the movement of the tool.

[0017] The speed determination module is used to determine the target speed and terminal speed of the S-shaped speed change trajectory according to the modified planned trajectory parameter value, the current motion parameter value and the original planned trajectory.

[0018] The current running speed segment determination module is used to determine the current running speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value.

[0019] The target operating speed segment determination module is used to determine the target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment.

[0020] The target motion parameter value determination module is used to determine the target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed and the current motion parameter value.

[0021] The speed change trajectory planning module is used to determine the S-shaped speed change trajectory according to the target motion parameter value.

[0022] The tool control module is used to control the tool to continue moving according to the S-shaped speed change trajectory.

[0023] A computer device includes a memory and a processor, wherein a computer program is executed by the processor to implement various method embodiments of an S-shaped trajectory planning method.

[0024] A computer-readable storage medium stores a computer program, and the computer program is executed by a processor to implement various method embodiments of an S-shaped trajectory planning method.

[0025] The above-mentioned S-shaped trajectory planning method, device, computer equipment and storage medium obtain the current motion parameter value of the tool and the modified planned trajectory parameter value during the movement of the tool, calculate the target speed and terminal speed of the planned S-shaped speed change trajectory, determine the current running speed segment of the tool, and then calculate the relevant parameter values ​​of the running speed segment of the S-shaped speed change trajectory to obtain the S-shaped speed change trajectory, and use the S-shaped speed change trajectory obtained during the movement of the tool to control the tool to continue moving. The tool speed change trajectory is recalculated in real time, which reduces the deviation caused by real-time environmental factors to the interpolation process and improves the accuracy of the interpolation process. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1It is a complete schematic diagram of a 7-segment S-shaped speed change curve;

[0027] Figure 2 A schematic diagram of a flow chart of an S-shaped trajectory planning method in one embodiment;

[0028] Figure 3 is a structural block diagram of an S-shaped trajectory planning device in one embodiment;

[0029] Figure 4 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0030] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0031] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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.

[0032] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly. The connection between the components can be direct or indirect.

[0033] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0034] like Figure 1 As shown in the figure, it is a complete 7-segment S-shaped speed change curve. The S-shaped speed change curve includes: displacement-time image (1a), velocity-time image (1b), acceleration-time image (1c) and jerk-time image (1d). The S-shaped speed change curve can be divided into 7 subdivided speed segments according to the size and arrangement of the jerk. The specific expression form of the subdivided speed segment can be a function. The speed change law of each subdivided speed segment is as follows:

[0035] At time 0-t 1 The stage jerk is the maximum positive jerk J m , the acceleration increases linearly from 0 to the maximum positive acceleration A m At this time, the speed increases at a variable acceleration, and this stage is called the acceleration stage.

[0036] At time t 1 -t 2 The stage jerk suddenly changes to 0, and the acceleration is the maximum positive acceleration A m , the speed is at the maximum positive acceleration A m Linear increase, this stage is called uniform acceleration stage.

[0037] At time t 2 -t 3 The stage jerk suddenly changes to the reverse maximum jerk -J m , the acceleration is from the positive maximum acceleration A m The speed decreases evenly to 0, at which point the speed increases to the maximum speed V m , this stage is called the deceleration stage.

[0038] At time t 3 -t 4 The stage jerk suddenly changes to 0, the acceleration is 0, and the speed maintains the maximum speed V m The tool displacement increases linearly, and this stage is called the uniform speed stage.

[0039] At time t 4 -t 5 The stage jerk suddenly changes to the reverse maximum jerk -J m , the acceleration increases linearly from 0 to the reverse maximum acceleration - A m , at this time the speed is the maximum speed V m The speed decreases, and this stage is called the acceleration and deceleration stage.

[0040] At time t 5 -t 6 The stage jerk suddenly changes to 0, and the acceleration is the reverse maximum acceleration - A m , the speed is the maximum acceleration in the reverse direction - A m This stage is called the uniform deceleration stage.

[0041] At time t 6 -t 7 The stage jerk suddenly changes to the positive maximum jerk J m , the acceleration is determined by the reverse maximum acceleration - A m The speed decreases evenly to 0, at which point the speed decelerates to the terminal speed Ve , the tool's running distance reaches the length S of the tool path segment. This stage is called the deceleration stage.

[0042] The maximum positive jerk J m , maximum positive acceleration A m and maximum speed V m It is a parameter determined based on the performance and efficiency of the servo system. It may be modified during the actual operation of the tool, resulting in the tool being unable to complete the S-shaped speed change within the set tool path distance and reach the terminal speed V at the end of the set tool path segment. e When the S-shaped speed change trajectory is actually used for tool interpolation, the tool acceleration may not reach the maximum positive acceleration A because the length of the tool segment to be interpolated is too small. m Or reverse maximum acceleration -A m , resulting in the lack of uniform acceleration or uniform deceleration phase; the tool speed may not reach the maximum speed V because the length of the tool segment to be interpolated is too small. m , resulting in the lack of a uniform speed stage, when the maximum speed that the tool can reach is the target speed V t When the tool speed reaches the maximum speed V m When the target speed V t Equal to the maximum speed V m The jerk is also called the rate of change of acceleration during jerk.

[0043] In one embodiment, Figure 2 FIG. 1 is a flow chart of an S-shaped trajectory planning method, an S-shaped trajectory planning method, applied to a machine tool numerical control system, comprising:

[0044] Step 102, obtaining the current motion parameter value and the original planned trajectory of the tool.

[0045] The tool motion trajectory is a broken line composed of extremely small steps. It is the tool motion trajectory planned by the machine tool CNC system according to a certain method. The tool moves and processes according to the planned tool motion trajectory, which is called interpolation. The interpolation between two adjacent points is a tool path segment. The tool movement process can be understood as the interpolation process.

[0046] The parameter values ​​of the current tool motion are obtained, including but not limited to the current speed, current acceleration, current jerk and other parameter values ​​that describe the tool motion. The original planned trajectory is obtained, which means obtaining the planned trajectory and planning equation that the current tool is using for interpolation. The original planned trajectory can be used to calculate the parameter values ​​such as the distance that the tool has interpolated in this tool path segment.

[0047] Step 104 , during the movement of the tool, obtaining the modified planned trajectory parameter values.

[0048] The modified planning trajectory parameter values ​​may be due to errors caused by factors such as motor performance and cutting heat, or due to the user modifying the feed rate and other factors. The modified planning trajectory parameter values ​​include but are not limited to the total length of the tool path segment, the maximum speed V m , maximum positive acceleration A m , maximum positive acceleration J m . The maximum speed, maximum acceleration and maximum jerk are parameters determined according to the performance and efficiency of the servo system. The maximum speed indicates the maximum speed that the tool can reach due to objective reasons of the machine tool during S-shaped speed change. When the tool speed reaches the maximum speed during S-shaped speed change, the corresponding acceleration is 0. The maximum acceleration indicates the maximum acceleration that the tool can accelerate to during S-shaped speed change. The maximum jerk indicates the maximum jerk that the tool can reach during S-shaped speed change. During different stages of S-shaped speed change, the jerk of the tool takes the positive maximum jerk, the reverse maximum jerk and 0 for movement. When the modified planning trajectory parameter value can be obtained, it means that the original planning equation parameters have changed. Continuing to interpolate according to the original planning trajectory will not be able to complete the S-shaped speed change process within the remaining length of the tool path segment, resulting in the acceleration of the tool not being 0 when it runs to the end of the tool path segment, resulting in a flexible impact. Therefore, the S-shaped speed change trajectory needs to be replaced so that the acceleration of the tool is 0 when it runs to the end of the tool path segment within the remaining length.

[0049] Step 106, determining the target speed and terminal speed of the S-shaped speed change trajectory according to the modified planned trajectory parameter values, the current motion parameter values ​​and the original planned trajectory.

[0050] Among them, the S-shaped speed change trajectory in the embodiment refers to the re-planned tool motion trajectory. When the tool performs S-shaped speed change, it will accelerate to the target speed and then decelerate. The target speed is generally the maximum speed, but in some cases the tool cannot accelerate to the maximum speed. At this time, a target speed needs to be determined. The terminal speed indicates the speed of the tool at the end of an S-shaped speed change. At the same time, the terminal speed is also the speed at the end of a tool track segment. At this time, the corresponding acceleration needs to be 0 to prevent the tool from producing flexible impact.

[0051] Specifically, the original planned trajectory and the current motion parameter values ​​are known to calculate the interpolation distance that the tool has completed, and the remaining length is obtained by subtracting the interpolation distance that the tool has completed from the total length of the tool path segment. The maximum speed, maximum acceleration, and maximum jerk in the re-planned S-shaped speed change trajectory are known to the modified planned trajectory parameter values ​​and the original planned trajectory. Finally, the terminal speed can be determined based on the remaining length, maximum speed, maximum acceleration, maximum jerk, and the characteristics of the S-shaped speed change trajectory, and then the target speed can be determined.

[0052] Step 108, determining the current operating speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value.

[0053] Among them, the current running speed segment means that the current position of the tool is the starting position of the S-shaped speed change trajectory. At this time, the tool corresponds to the speed change stage in the S-shaped speed change trajectory. The current speed and acceleration of the tool are used as the starting speed and starting acceleration of the re-planned S-shaped speed change trajectory. The current running speed segment can be understood as a function including unknown parameters.

[0054] The current operating speed segment can be a post-speed change stage, a pre-speed change stage or a uniform speed stage. The pre-speed change stage, the uniform speed stage and the post-speed change stage can also be understood as functions including unknown parameters. Among them, the post-speed change stage is a speed change stage from a state where the speed is equal to the target speed and the acceleration is equal to 0 to a terminal speed. The uniform speed stage is a stage where the speed reaches the target speed and the acceleration is 0. The pre-speed change stage is a speed change stage from a starting speed to a target speed and the acceleration is 0.

[0055] Specifically, the judgment standard is whether the tool has reached the uniform speed stage in the S-shaped speed change trajectory. If the current speed of the tool does not reach the target speed, or the acceleration is not 0, it means that the tool still needs to change speed from the starting speed to the target speed, and the uniform speed stage with acceleration of 0 indicates that the current tool is in the previous speed change.

[0056] When the current speed of the tool reaches the target speed and the acceleration is 0, it is necessary to determine whether the tool is in the uniform speed stage or has completed the entire S-shaped speed change trajectory and reached the end point. If the target speed is not equal to 0, it means that the tool still needs to move at a uniform speed, that is, the current tool is in the uniform speed stage. If the target speed is 0 at this time, the tool will not continue to move at a speed of 0, indicating that there is no uniform speed stage.

[0057] When the current speed of the tool reaches the target speed, and the acceleration is 0, and the target speed is 0, it is necessary to determine whether the tool is in the post-speed change stage or the tool has completed the entire S-shaped speed change trajectory and reached the end point. If the end speed is equal to the target speed, it means that the tool has completed the entire S-shaped speed change trajectory and reached the end point, indicating that there is no post-speed change stage. In fact, when the modified planned trajectory parameter value appears, it means that the remaining length continues to be interpolated according to the original planned trajectory. At the end of the tool path segment, the acceleration cannot be 0, so this situation does not exist. The end speed is not equal to the target speed, indicating that the tool still needs to change speed from the target speed and the acceleration is 0 to the end speed, that is, the tool is currently in the post-speed change stage.

[0058] Step 110, determining a target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment.

[0059] The target running speed segment represents the speed segment actually included in the S-shaped speed change trajectory, which can be understood as a piecewise function. For example, the S-shaped speed change trajectory includes a front speed change stage, and may also include a uniform speed stage and a rear speed change stage.

[0060] Specifically, according to the current running speed segment, the first target running speed segment included in the S-shaped speed change trajectory is obtained, and according to the current motion parameter value, it is determined whether the tool has completed the S-shaped speed change, and whether the S-shaped speed change trajectory includes other target running speed segments. The target running speed segments included in the S-shaped speed change trajectory will appear in chronological order. For example, according to the chronological order, the front speed change stage, the uniform speed stage, and the rear speed change stage appear successively.

[0061] Step 112: Determine the target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed and the current motion parameter value.

[0062] The S-shaped speed change trajectory is a piecewise function. The S-shaped speed change trajectory can be determined by obtaining the definition domain and parameters of each target operating speed segment.

[0063] Specifically, the starting point of the S-shaped speed change trajectory is assigned a value according to the current motion parameter value to obtain the starting point parameter value of the first target running speed segment. Given the starting point parameter value, target speed, terminal speed, modified planning trajectory parameter value, and target running speed segment, the running time of the target running speed segment can be obtained, and then the end time, end speed, running distance, and end acceleration of the running speed segment can be obtained, that is, the motion parameter value of each running speed segment can be obtained.

[0064] Step 114: Determine an S-shaped speed change trajectory according to the target motion parameter value.

[0065] Specifically, the S-shaped speed change trajectory is a piecewise function, and the specific parameters of each segment can be obtained to determine the S-shaped speed change trajectory. The target running speed segments are sorted in chronological order, and the target motion parameter values ​​of the target running speed segments are brought into the sorted target running speed segments to obtain the S-shaped speed change trajectory.

[0066] Step 116: Control the tool to continue moving according to the S-shaped speed change trajectory.

[0067] Specifically, the CNC system uses an S-shaped speed-changing trajectory to replace the original planned trajectory to control the tool and complete the interpolation of the remaining length of this tool path segment.

[0068] The above-mentioned S-shaped trajectory planning method determines the target speed and terminal speed according to the current motion parameter value, the original planned trajectory and the modified planned trajectory parameter value, so as to satisfy the tool to complete the S-shaped speed change in the remaining length of the tool path segment. Then, according to the target speed, terminal speed and current motion parameter value, determine the current operating speed segment and the target operating speed segment. First, the current operating speed segment is obtained according to the calculation, and then the target operating speed segment is obtained according to the current operating speed segment. Such a calculation process can reduce the amount of calculation and improve the response speed. Finally, the motion parameter value of each target operating speed segment is determined to obtain the S-shaped speed change trajectory. The obtained S-shaped speed change trajectory is used to replace the original planned trajectory for interpolation to complete the interpolation of the remaining length of the tool path segment, ensuring that the acceleration is 0 at the end of the tool path segment, reducing flexible impact, improving machine tool stability, and optimizing processing quality.

[0069] In one embodiment, determining the target speed and terminal speed of the S-shaped speed change trajectory according to the modified planned trajectory parameter value, the current motion parameter value and the original planned trajectory includes:

[0070] Step A2, determining the remaining length of the tool path segment according to the original planned trajectory and the current motion parameter value.

[0071] The remaining length is the distance that the tool has not interpolated in the tool path segment. The original planned trajectory is the planned trajectory that the tool has interpolated in the tool path segment before. After some planned trajectory parameters are modified, a new S-shaped speed change trajectory needs to be re-planned to interpolate the remaining length.

[0072] Specifically, the distance that the tool has traveled is determined according to the original planned trajectory and the current motion parameter value, and the remaining length is obtained by subtracting the distance that the tool has traveled from the total length of the tool path segment.

[0073] Step A4, determining a first critical displacement according to the modified planned trajectory parameter value and the original planned trajectory, wherein the first critical displacement is a minimum distance for the tool to complete S-shaped speed change under the current motion parameter value.

[0074] Among them, the minimum distance that the tool can complete the S-shaped speed change is also called the first critical displacement S1. The first critical displacement refers to the shortest displacement required for the tool to accelerate from the starting speed to the maximum speed at the positive maximum jerk, and then decelerate from the maximum speed to the terminal speed at the reverse maximum jerk. When calculating the first critical displacement, there are two situations, one is the situation where the maximum acceleration cannot be reached during the S-shaped speed change, and the other is the situation where the maximum acceleration can be reached. For two different situations, the expression of the first critical displacement is shown in formula (1):

[0075]

[0076] Where: S 1is the first critical displacement, V t is the target speed, V e is the terminal velocity, J m is the maximum positive acceleration, A m is the maximum forward speed.

[0077] Specifically, the target speed, terminal speed and modified planned trajectory parameter values ​​are substituted into the above formula (1) to calculate the minimum distance required for the tool to complete the S-shaped speed change, that is, the first critical displacement S 1 .

[0078] Step A6: Compare the remaining length with the first critical displacement to obtain a first comparison result.

[0079] The first comparison result is used to indicate the difference between the remaining length and the first critical displacement.

[0080] Specifically, the value of the remaining length is compared with the first critical displacement to determine the magnitude of the remaining length and the first critical displacement. When the remaining length is greater than or equal to the first critical displacement, it means that the tool can complete the S-shaped speed change under the condition of the remaining length. When the remaining length is less than the first critical displacement, it means that the tool cannot complete the S-shaped speed change under the condition of the remaining length.

[0081] Step A8: Determine the target speed and terminal speed of the S-shaped speed change trajectory according to the first comparison result, the modified planned trajectory parameter value, the current motion parameter value and the remaining length.

[0082] Specifically, when the tool cannot reach the maximum speed, it means that the tool needs to complete the S-shaped speed change before reaching the maximum speed, that is, the terminal speed is reached before the tool reaches the maximum speed, and the corresponding acceleration is 0 when the terminal speed is reached. According to the modified planning trajectory parameter value, the current motion parameter value and the remaining length, a new terminal speed is recalculated to replace the original terminal speed, so that the tool can complete the S-shaped speed change under the remaining length, and the new terminal speed is used as the target speed of the tool path segment. When the tool can reach the maximum speed, the maximum speed can be used as the target speed, and a new terminal speed is recalculated to replace the original terminal speed, so that the tool can complete the S-shaped speed change under the remaining length.

[0083] In this embodiment, the remaining length is compared with the first critical displacement to determine whether the target speed is the maximum speed, and then the target speed and the terminal speed are calculated. The complexity of the calculation is reduced, and after the target speed and the terminal speed are obtained, the framework of the S-shaped speed change trajectory to be calculated subsequently is basically determined.

[0084] In one embodiment, the modified planned trajectory parameter value includes a maximum speed. According to the first comparison result, the modified planned trajectory parameter value, the current motion parameter value and the remaining length, the target speed and the terminal speed of the S-shaped speed change trajectory are determined, including:

[0085] Step B2: When the remaining length is less than the first critical displacement, calculate the target speed and the terminal speed so that the remaining length is equal to the second critical displacement. The second critical displacement is the shortest displacement required for the tool to accelerate to the first boundary speed at the positive maximum acceleration under the current motion parameter value, and then decelerate from the first boundary speed to the terminal speed at the reverse maximum jerk; the first boundary speed is equal to the sum of the larger of the starting speed and the terminal speed and the speed increment.

[0086] Specifically, when the remaining length is less than the first critical displacement and the tool cannot reach the maximum speed, it means that the terminal speed is reached before the tool reaches the maximum speed, and the S-shaped speed change is completed. The terminal speed needs to be corrected, that is, a new terminal speed is calculated to replace the original terminal speed. At this time, the first boundary speed also needs to be calculated. At this time, the S-shaped speed change path is that the tool accelerates to the first boundary speed at the starting speed with the positive maximum acceleration, and then decelerates from the first boundary speed to the terminal speed with the reverse maximum acceleration. The shortest displacement required at this time is the second critical displacement S 2 . Make the second critical displacement S 2 Equal to the remaining length can ensure that the tool can complete the S-shaped speed change under the remaining length. According to the relevant parameters, the first boundary speed and the second critical displacement S 2 The terminal speed can be determined by the calculation formula. At this time, the terminal speed is equal to the target speed.

[0087] The first boundary speed is equal to the sum of the larger of the starting speed and the end speed and the speed increment. The calculation formula of the first boundary speed is as follows:

[0088]

[0089] Where: V b is the first boundary speed, V c is the starting speed, V e is the terminal velocity, A m is the maximum positive acceleration, J m is the maximum positive jerk.

[0090] The second critical displacement S 2 The shortest displacement required for the tool to accelerate from the starting speed to the first boundary speed at the maximum jerk, and then decelerate from the first boundary speed to the terminal speed at the maximum jerk. The second critical displacement S 2It also represents the critical displacement of changing from the starting speed to the target speed with an acceleration of 0, and then directly changing to the terminal speed with an acceleration of 0 without entering the uniform speed stage. The formula for calculating the second critical displacement is shown in formula (2):

[0091]

[0092] Where: S 2 is the second critical displacement, V c is the starting speed, V b is the first boundary speed, V e is the terminal velocity, A m is the maximum positive acceleration, J m is the maximum positive jerk.

[0093] Step B4: When the remaining length is greater than or equal to the first critical displacement, the maximum speed is used as the target speed of the S-shaped speed change trajectory, and the terminal speed of the S-shaped speed change trajectory is calculated so that the remaining length is equal to the first critical displacement.

[0094] Specifically, when the remaining length is greater than or equal to the first critical displacement, the tool can reach the maximum speed, which means that the tool reaches the terminal speed when it reaches the maximum speed or after the tool reaches the maximum speed and then decelerates, and the S-shaped speed change is completed. Then it is necessary to correct the target speed, that is, use the maximum speed as the target speed, calculate a new terminal speed to make the first critical displacement S 1 Equal to the remaining length. Substituting the target speed, the modified planned trajectory parameter value, the current motion parameter value and the remaining length into formula (1) for calculation, a new terminal speed can be obtained.

[0095] This embodiment provides a method for calculating the target speed and the terminal speed after determining whether the tool can reach the maximum speed. When the maximum speed can be reached, the maximum speed is used as the target speed, which reduces some calculations and the time for calculating the S-shaped speed change trajectory, making the system more responsive and improving the accuracy of the interpolation process.

[0096] In one embodiment, the current running speed segment of the tool is determined according to the target speed, the terminal speed and the current motion parameter value, including:

[0097] Step C2: When the current speed in the current motion parameter value is not equal to the target speed, or the current acceleration in the current motion parameter value is not equal to 0, it is determined that the tool is currently in the front speed change stage.

[0098] Among them, the current operating speed segment may be the front speed change stage, the uniform speed stage or the rear speed change stage. The current motion parameter value includes but is not limited to the current speed, the current acceleration, and the current jerk value. Specifically, if the tool is currently in the front speed change stage, it is required that the tool speed has not reached the target speed, or the acceleration is not 0. Therefore, it is necessary to determine whether the current speed is equal to the target speed. When the current speed is not equal to the target speed, or the current acceleration is not equal to 0, the tool is currently in the front speed change stage.

[0099] Step C4: When the tool is currently in the pre-speed change stage, the target speed is not equal to 0 and the remaining length is not equal to the second critical displacement, it is determined that the tool is currently in the uniform speed stage; the second critical displacement is the critical displacement of the tool changing from the starting speed to the target speed and then to the terminal speed without passing through the uniform speed stage.

[0100] Specifically, the uniform speed stage is the stage in which the target speed is reached, the acceleration is 0, and the target speed is maintained for uniform motion. Therefore, to determine whether the tool is currently in the uniform speed stage, it is necessary to determine whether the tool is currently not in the pre-speed change stage, that is, whether the speed reaches the target speed. If the target speed is 0 during the uniform speed stage, it means that the tool stops moving. Therefore, when the target speed is 0, the tool is not currently in the uniform speed stage. Only when the current speed is the target speed and the target speed is not 0, the tool is currently in the uniform speed stage. When the remaining length is equal to the second critical displacement, it means that the tool changes speed from the starting speed to the target speed and then to the terminal speed, just completing the S-shaped speed change, and there is no uniform speed stage.

[0101] Step C6: When the tool is not currently in the front speed change stage and the uniform speed stage, and the terminal speed is not equal to the target speed, it is determined that the tool is currently in the rear speed change stage.

[0102] Specifically, firstly, to determine whether the tool is in the post-speed change stage, it is necessary to first determine that the tool is not currently in the pre-speed change stage and the uniform speed stage, so the current speed of the tool is required to be equal to the target speed, and the current acceleration in the current motion parameter value is 0, and the target speed is equal to 0. Then, it is determined whether the post-speed change stage is included, so the terminal speed is required to be not equal to the target speed.

[0103] In this embodiment, the S-shaped speed change trajectory is classified according to acceleration into a front speed change stage, a uniform speed stage, and a rear speed change stage. The subdivided speed segments included in the front speed change stage and the rear speed change stage are then classified according to the jerk. The two classifications can reduce the complexity of the calculation and improve the response speed of the system.

[0104] In one embodiment, according to the current running speed segment, determining the target running speed segment included in the S-shaped speed change trajectory includes:

[0105] Step D2: When the tool is currently in the front speed change stage, and the target speed and the terminal speed are both 0, it is determined that the S-shaped speed change trajectory includes the front speed change stage.

[0106] Specifically, when the tool is currently in the front speed change stage, it may include the subsequent uniform speed stage and the rear speed change stage, but when the terminal speed is equal to the target speed, it means that the terminal speed is reached after the front speed change stage reaches the target speed, and the S-shaped speed change is completed. That is, there is no uniform speed stage and the rear speed change stage, only the front speed change stage is included.

[0107] Step D4: When the tool is currently in the front speed change stage, the terminal speed is equal to the target speed and is not 0, and the remaining length is not equal to the second critical displacement, it is determined that the S-shaped speed change trajectory includes the front speed change stage and the uniform speed stage.

[0108] Specifically, the tool is currently in the front speed change stage. When the tool runs to the target speed and the acceleration is 0, it is necessary to determine whether it includes a uniform speed stage and a rear speed change stage. When the terminal speed is equal to the target speed, it means that the terminal speed has been reached. It is necessary to determine whether it is necessary to perform a uniform speed movement according to the terminal speed to complete the S-shaped speed change, that is, whether it includes a uniform speed stage. If the target speed is 0 at this time, there is no uniform speed stage; if the target speed is not 0, it is necessary to determine whether the remaining length is equal to the second critical displacement. When the remaining length is equal to the second critical displacement, it means that the tool has reached the target speed, and the S-shaped speed change is just completed when the acceleration is 0, and there is no uniform speed stage. When the remaining length is not equal to the second critical displacement, it means that the tool has reached the target speed, and after the acceleration is 0, it continues to perform a uniform speed movement according to the target speed to complete the S-shaped speed change, including the uniform speed stage.

[0109] Step D6: When the tool is currently in the front speed change stage, and the target speed is not 0, and the target speed is not equal to the terminal speed, and the remaining length is not equal to the second critical displacement, determine that the S-shaped speed change trajectory includes the front speed change stage, the uniform speed stage and the rear speed change stage.

[0110] Specifically, when the target speed is not 0 and the remaining length is not equal to the second critical displacement, it means that the tool can be shifted to the target speed and the acceleration is 0, so it includes the uniform speed stage. When the target speed is not equal to the terminal speed, it means that it is necessary to shift from the target speed to the terminal speed, so it includes the post-speed shift stage. It is determined that the S-shaped speed change trajectory includes the front speed change stage, the uniform speed stage and the post-speed change stage.

[0111] Step D8: When the tool is currently in a uniform speed stage, and the terminal speed is equal to the target speed, it is determined that the S-shaped speed change trajectory includes a uniform speed stage.

[0112] Specifically, when the tool is currently in the uniform speed stage, it means that there is no front speed change stage, and there is no need to determine whether it includes the uniform speed stage, but only whether it includes the rear speed change stage. When the terminal speed is equal to the target speed, it is determined that there is no need for the rear speed change stage. Therefore, it is determined that the S-shaped speed change trajectory includes the uniform speed stage.

[0113] Step D10: When the tool is currently in a uniform speed stage, and the terminal speed is not equal to the target speed, it is determined that the S-shaped speed change trajectory includes a uniform speed stage and a post-speed change stage.

[0114] Specifically, when the terminal speed is not equal to the target speed, it indicates that the speed needs to be changed from the target speed to the terminal speed, and thus a post-speed change phase is included. Determining the S-shaped speed change trajectory includes a uniform speed phase and a post-speed change phase.

[0115] Step D12: When the tool is currently in the post-speed change stage, determine that the S-shaped speed change trajectory includes the post-speed change stage.

[0116] Specifically, when the tool is currently in the rear speed change stage, it means that the S-shaped speed change trajectory does not include the front speed change stage and the rear speed change stage. After completing the rear speed change stage, the speed can reach the terminal speed. Therefore, there is no need to judge further to determine that the S-shaped speed change trajectory includes the rear speed change stage.

[0117] This embodiment provides a detailed method for determining the speed steps included in the S-shaped speed change trajectory according to the current operating speed segment. Taking the current operating speed segment as the first target operating speed segment, the subsequent operating speed segments are discussed according to the occurrence order of the operating speed segments in the S-shaped speed change trajectory, so that the target operating speed segment can be quickly determined.

[0118] In one embodiment, the modified planned trajectory parameter values ​​include the maximum acceleration and the maximum jerk. According to the current running speed segment, determining the target running speed segment included in the S-shaped speed change trajectory also includes:

[0119] Step E2: When the S speed change trajectory includes a front speed change stage, determine the critical speed at which the tool can reach the maximum acceleration in the front speed change stage according to the current motion parameter value, maximum acceleration, maximum jerk, target speed and terminal speed.

[0120] Specifically, the critical speed at which the tool can reach the maximum acceleration in the front speed change stage is obtained and compared with the maximum speed, i.e., the target speed, that the tool can reach in the front speed change stage. According to the comparison result, it can be determined whether the tool can reach the maximum acceleration in the front speed change stage.

[0121] Step E4: Compare the critical speed and the target speed to obtain a second comparison result.

[0122] Specifically, the critical speed and the target speed are compared to obtain a second comparison result.

[0123] Step E6: Determine the subdivided speed segments included in the front speed change stage according to the second comparison result.

[0124] Specifically, when the second comparison result is that the critical speed is less than or equal to the target speed, it is determined that the acceleration of the tool in the front speed change stage can reach the maximum positive acceleration. This indicates that a subdivided speed segment of uniform acceleration or uniform deceleration is included in the front speed change stage. When the second comparison result is that the critical speed is greater than the target speed, it is determined that the acceleration of the tool in the front speed change stage cannot reach the maximum positive acceleration in the modified planned trajectory parameter value, and the acceleration of the tool begins to decrease before it increases to the maximum positive acceleration. This indicates that a subdivided speed segment of uniform acceleration or uniform deceleration is not included in the front speed change stage.

[0125] The high critical speed V is calculated based on the previous speed change stage, the current motion parameter value and the modified planned trajectory parameter value. ht With low critical speed V lt .

[0126] Among them, the high critical speed V ht As: From the starting state (speed V c , acceleration A c ) and accelerates in an S-shaped manner, reaching the maximum positive acceleration A halfway. m , accelerate to the speed reached when the acceleration is 0.

[0127] Low critical speed V lt As: From the starting state (speed V c , acceleration A c ) and decelerates in an S-shaped manner, reaching the maximum acceleration in the reverse direction - A m , accelerate to the speed reached when the acceleration is 0.

[0128] The following is the high critical speed V ht With low critical speed V lt The calculation formula is:

[0129]

[0130] Where: V c is the starting speed, A c is the starting acceleration, A m is the maximum positive acceleration, J m Maximum positive jerk.

[0131] When V t >V ht This means that the tool can reach the maximum positive acceleration by changing the speed at the starting speed and starting acceleration at the maximum positive jerk, V t is the target speed;

[0132] or V t <V lt , indicating that the tool can achieve the reverse maximum acceleration by changing the speed at the starting speed and starting acceleration at the reverse maximum jerk.

[0133] When V t ∈[V lt ,V ht ], indicating that the tool cannot reach the maximum acceleration when accelerating or decelerating at the starting speed and starting acceleration with the maximum jerk. Among them, being able to reach the maximum acceleration means being able to reach the maximum acceleration in the positive direction or being able to reach the maximum acceleration in the reverse direction.

[0134] According to the target running speed segment, the target speed, the terminal speed and the current motion parameter value, the target motion parameter value of the target running speed segment is determined, including:

[0135] Step E8: Determine the motion parameter values ​​of the subdivided speed segments included in the previous speed change stage according to the subdivided speed segments included in the previous speed change stage, the target speed, the terminal speed and the current motion parameter values.

[0136] Specifically, according to the subdivided speed segments included in the front speed change stage, determine whether the front speed change stage includes a uniform speed change stage, that is, whether the maximum acceleration can be achieved. If the front speed change stage can achieve the maximum acceleration, the starting speed, starting acceleration, running time and other motion parameter values ​​of each subdivided stage of the front speed change stage are calculated according to the maximum acceleration, the front speed change stage and the target speed. If there is no uniform speed stage in the front speed change stage and the maximum acceleration cannot be achieved, the maximum acceleration that can be achieved in the front speed change stage and the starting speed, starting acceleration and running time and other motion parameter values ​​of each subdivided stage are calculated according to the front speed change stage and the target speed.

[0137] This embodiment provides a method for specifically determining the subdivided speed segments included in the front speed change stage after determining the target running speed segment, and also provides a method for determining the motion parameter values ​​of the subdivided speed segments in the front speed change stage according to the subdivided speed segments included in the front speed change stage. Dividing the front speed change stage into subdivided speed segments and then calculating the motion parameter values ​​can reduce the difficulty of direct calculation, increase the calculation speed, and make it easier to obtain the motion parameter values ​​of the front speed change stage.

[0138] In one embodiment, the modified planned trajectory parameter values ​​include the maximum acceleration and the maximum jerk. According to the current running speed segment, determining the target running speed segment included in the S-shaped speed change trajectory also includes:

[0139] Step F2: when the S speed change trajectory includes a post-speed change phase, determining a first speed change amount according to the target speed and the terminal speed;

[0140] Specifically, the first speed change is the actual speed change of the tool in the post-speed change stage, which is equal to the difference between the target speed and the terminal speed, expressed as ΔV t-e express.

[0141] Step F4: Determine a second speed variation according to the maximum acceleration and the maximum jerk.

[0142] Specifically, the second velocity change represents the velocity change when the forward maximum acceleration is reduced to 0 by the reverse maximum jerk after the forward maximum acceleration is reached, which is equal to the ratio of the square of the forward maximum acceleration to the forward maximum jerk, expressed as express.

[0143] Step F6: Compare the first speed change amount with the second speed change amount to obtain a third comparison result.

[0144] Specifically, the values ​​of the first speed change amount and the second speed change amount are compared to obtain a third comparison result.

[0145] Step F8: Determine the subdivided speed segments included in the rear shifting stage according to the third comparison result.

[0146] Specifically, the rear shifting stage is a stage of decelerating from a state where the speed is equal to the target speed and the acceleration is equal to 0 to the terminal speed. Therefore, the target speed must not be lower than the terminal speed. The first speed change is equal to the difference between the target speed and the terminal speed. This difference represents the maximum speed change in the rear shifting stage. When the third comparison result is Then, the speed segment included in the rear speed change stage is determined according to whether the rear speed change stage can reach the maximum acceleration.

[0147] According to the target running speed segment, target speed, terminal speed and current motion parameter value, the target motion parameter value is determined, including:

[0148] Step F10: Determine the motion parameter values ​​of the subdivided speed segments included in the post-shift phase according to the subdivided speed segments included in the post-shift phase, the target speed, the terminal speed, and the current motion parameter values.

[0149] Specifically, according to the subdivided speed segments included in the rear speed change stage, determine whether the rear speed change stage includes a uniform speed change stage, that is, whether the maximum acceleration can be achieved. If the rear speed change stage can achieve the maximum acceleration, the starting speed, starting acceleration, and running time of each subdivided stage of the rear speed change stage are calculated according to the maximum acceleration, the front speed change stage, and the target speed. If there is no uniform speed stage in the rear speed change stage, that is, the maximum acceleration cannot be achieved, the maximum acceleration that can be achieved in the rear speed change stage and the starting speed, starting acceleration, and running time of each subdivided stage are calculated according to the rear speed change stage and the target speed.

[0150] This embodiment provides a simple method for determining the subdivided speed segments included in the rear speed change stage after determining the target running speed segment, and also provides a method for determining the motion parameter values ​​of the subdivided speed segments in the rear speed change stage. Dividing the rear speed change stage into subdivided speed segments and then calculating the motion parameter values ​​can reduce the difficulty of direct calculation, increase the calculation speed, and more easily obtain the motion parameter values ​​of the rear speed change stage.

[0151] In one embodiment, the critical speed includes: a high critical speed reached when the tool can reach the maximum forward acceleration in the front speed change stage, and a low critical speed reached when the tool can reach the maximum reverse acceleration in the front speed change stage. Determining the subdivided speed segments included in the front speed change stage according to the second comparison result includes:

[0152] When the target speed is greater than the high critical speed or the target speed is less than the low critical speed, it is determined that the front speed change stage includes a subdivided speed segment with a jerk equal to the forward maximum jerk, a subdivided speed segment with a jerk equal to the reverse maximum jerk, and a subdivided speed segment with a jerk equal to 0. When the target speed is greater than the low critical speed and less than the high critical speed, it is determined that the front speed change stage includes a subdivided speed segment with a jerk equal to the forward maximum jerk and a subdivided speed segment with a jerk equal to the reverse maximum jerk.

[0153] Specifically, when the front speed change stage is subdivided, the first subdivided speed stage needs to be determined according to the current acceleration. c ≥0, calculate the speed-up boundary speed V b↑ , when V t >V ht The front speed change stage includes three subdivided speed sections: acceleration stage, uniform acceleration stage and deceleration stage. When V t <V lt The front speed change stage includes three subdivided speed sections: acceleration and deceleration stage, uniform deceleration stage and deceleration stage. t ∈[V lt ,V ht ], the tool cannot reach the maximum acceleration in the front speed change stage.t ≥V b↑ The front speed change stage includes two subdivided speed sections: the acceleration stage and the deceleration stage. At this time, when V t <V b↑ The front speed change stage includes three subdivided speed stages: deceleration stage, acceleration and deceleration stage, and deceleration and deceleration stage. Since the jerk in the deceleration and acceleration stage is the same as that in the acceleration and deceleration stage, both are the reverse maximum jerk - J m , when performing calculations, the deceleration phase can be incorporated into the acceleration phase and regarded as a speed change phase.

[0154] When the starting point acceleration A c <0, calculate the deceleration boundary speed V b↓ , when V t >V ht The front speed change stage includes four subdivided speed sections: deceleration stage, acceleration stage, uniform acceleration stage and deceleration stage. The jerk in the deceleration stage is the same as that in the acceleration stage, which is the maximum positive jerk J. m , can be combined into one segment when performing calculations. t <V lt The front speed change stage includes three subdivided speed sections: acceleration and deceleration stage, uniform deceleration stage and deceleration stage. t ∈[V lt ,V ht ], the tool cannot reach the maximum acceleration in the front speed change stage. t ≥V b↓ The front speed change stage includes three sub-speed stages: deceleration stage, acceleration stage and deceleration stage. The acceleration of the deceleration stage is the same as that of the acceleration stage, both of which are the positive maximum jerk J. m , can be combined into one segment during calculation. t <V b↓ The front speed change stage includes two subdivided speed sections: the deceleration and acceleration stage and the deceleration and reduction stage.

[0155] Among them, the speed-up boundary speed V b↑ =From the starting state (starting speed V c , starting point acceleration A c >0), and the fastest speed change is achieved when the acceleration is 0 according to the S-shaped speed change strategy.

[0156] Speed ​​reduction boundary speed V b↓ =From the starting state (starting speed V c , starting point acceleration A c <0), and the fastest speed change is achieved when the acceleration is 0 according to the S-shaped speed change strategy.

[0157] The following is the speed-up boundary speed Vb↑ The calculation formula is:

[0158]

[0159] Where: V c is the starting speed, A c is the starting acceleration, J m Maximum positive jerk.

[0160] The following is the deceleration boundary speed V b↓ The calculation formula is:

[0161]

[0162] Where: V c is the starting speed, A c is the starting acceleration, J m Maximum positive jerk.

[0163] This embodiment provides a specific method for determining the subdivided speed segments included in the front speed change stage after determining the target operating speed segment. The subdivided speed segments of the front speed change stage can be obtained simply and quickly, and has the advantages of less calculation and fast response.

[0164] In one embodiment, according to the third comparison result, determining the subdivided speed segments included in the rear speed change stage includes: when the third comparison result is that the first speed change amount is greater than the second speed change amount, determining that the rear speed change stage includes an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration stage. When the third comparison result is that the first speed change amount is less than or equal to the second speed change amount, determining that the rear speed change stage includes an acceleration and deceleration stage and a deceleration stage.

[0165] Specifically, when the third comparison result is When the third comparison result is The maximum acceleration cannot be achieved in the later speed change phase. When , the post-speed shifting stage can reach the maximum acceleration, so the post-speed shifting stage includes three subdivided speed sections: acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage. The rear speed change stage cannot reach the maximum acceleration, so the rear speed change stage includes two subdivided speed sections: the acceleration and deceleration stage and the deceleration and deceleration stage.

[0166] This embodiment provides a specific method for determining the subdivided speed segments included in the rear speed change stage after determining the target operating speed segment. The subdivided speed segments of the rear speed change stage can be obtained simply and quickly, and has the advantages of less calculation and fast response.

[0167] In one embodiment, the target running speed segment includes subdivided speed segments, and the target motion parameter value of the target running speed segment is determined according to the target running speed segment, the target speed, the terminal speed, and the current motion parameter value, including:

[0168] Step G2: Using the current motion parameter value as the motion parameter value of the starting point of the S-shaped speed change trajectory.

[0169] Specifically, the current motion parameter value is assigned to the motion parameter value of the starting point of the S-shaped speed change trajectory, and the assigned parameters include the current speed and the current acceleration, that is, the starting speed and starting acceleration of the starting point of the first subdivided speed segment of the S-shaped speed change trajectory are obtained.

[0170] Step G4: According to the motion parameter value of the starting point of the S-shaped speed change trajectory, the target speed, the terminal speed, the modified planned trajectory parameter value and the subdivided speed segment, the motion parameter value of the subdivided speed segment is determined in sequence.

[0171] Specifically, the parameter value at the starting point of the S-shaped speed change trajectory is the motion parameter value at the starting point of the first subdivided speed segment in the S-shaped speed change trajectory. The motion parameter value of the subdivided speed segment is determined according to the subdivided speed segment, the target speed and the terminal speed. The ending speed and the ending acceleration of the first subdivided speed segment are used as the starting speed and the starting acceleration of the next subdivided speed segment, and the motion parameter values ​​of the subsequent subdivided speed segments are calculated in sequence according to the sequence of the subdivided speed segments. Among them, the step of calculating the motion parameter values ​​of the subsequent subdivided speed segments in sequence is that the motion parameter values ​​of the subdivided speed segments also include the running time, the running distance, the ending speed and the ending acceleration, and the motion parameter values ​​of some subdivided speed segments can be directly obtained according to the subdivided speed segment, thereby reducing the difficulty of calculation.

[0172] This embodiment provides a method for calculating the motion parameter value of each subdivided speed segment in turn, taking the end speed and end acceleration of the previous subdivided speed segment as the starting speed and starting acceleration of the next subdivided speed segment, and combining each subdivided speed segment to determine the motion parameter value of each subdivided speed segment, which can quickly determine the motion parameter value of each subdivided speed segment in the S-shaped speed change trajectory, thereby quickly determining the S-shaped speed change trajectory.

[0173] In one embodiment, according to the motion parameter value of the starting point of the S-shaped speed change trajectory, the target speed, the terminal speed, the modified planned trajectory parameter value and the subdivided speed segment, the motion parameter value of the subdivided speed segment is determined in sequence, including:

[0174] Step H2: taking the starting speed and starting acceleration in the parameter values ​​of the starting point motion of the S-shaped speed change trajectory as the starting speed and starting acceleration of the first subdivided speed segment, and determining the intra-segment acceleration of the first subdivided speed segment.

[0175] Specifically, according to each subdivided speed segment, the jerk of the subdivided speed segment can be easily obtained.

[0176] Step H4: Determine the running time, running distance, ending speed and ending acceleration of the first subdivided speed segment according to the starting speed, starting acceleration and intra-segment jerk of the first subdivided speed segment.

[0177] The jerk within each subdivided speed segment is determined according to the S-shaped speed change trajectory characteristics, and can be the maximum jerk in the positive direction, 0 or the maximum jerk in the reverse direction according to the different subdivided speed segments. One or more of the jerk within the subdivided speed segment, the acceleration at the starting point or the acceleration at the end point can be directly obtained through the subdivided speed segment.

[0178] Specifically, first determine the running time of the subdivided speed segment according to the starting speed, starting acceleration, jerk within the segment and subdivided speed segment. Then determine the ending acceleration according to the running time of the subdivided speed segment, the jerk within the segment and the starting acceleration. Then determine the ending speed according to the starting speed, starting acceleration, ending acceleration and running time. Finally, determine the running distance according to the known parameter values. Among them, according to the subdivided speed segment, some parameter values ​​can be directly obtained, such as the starting acceleration and ending acceleration of the uniform acceleration stage are both positive maximum acceleration.

[0179] Step H6: Use the ending speed and ending acceleration of the first subdivided speed segment as the starting speed and starting acceleration of the next subdivided speed segment.

[0180] Specifically, the end point of the first subdivided speed segment is the starting point of the next subdivided speed segment, and the end speed and end acceleration of the first subdivided speed segment are assigned as the starting speed and starting acceleration of the next subdivided speed segment.

[0181] Step H8: Assign and calculate each subsequent subdivided speed segment in turn to determine the running time, running distance, end speed and end acceleration of each subdivided speed segment of the S-shaped speed change trajectory; obtain the motion parameter values ​​of all subdivided speed segments included in the S-shaped speed change trajectory.

[0182] Specifically, the ending speed and ending acceleration of each subsequent subdivided speed segment are used as the starting speed and starting acceleration of the next subdivided speed segment, and the starting speed and starting acceleration of each subdivided speed segment can be obtained. According to the segment jerk, starting acceleration or ending point acceleration of the subdivided speed segment, it is easy to find the running time of the subdivided speed segment, and then calculate other motion parameter values ​​until the motion parameter values ​​of all subdivided speed segments included in the S-shaped speed change trajectory are obtained. The following is a specific calculation method for the motion parameter values ​​of the subdivided speed segments in the embodiment:

[0183] (1) If the acceleration of the front speed change stage can reach the maximum positive acceleration, the subdivided speed segments included in the front speed change stage are the acceleration stage, the uniform acceleration stage and the deceleration stage, or the subdivided speed segments included in the front speed change stage are the deceleration stage, the uniform acceleration stage and the deceleration stage, the motion parameter value of each subdivided speed segment is calculated as follows:

[0184] Acceleration phase (deceleration phase): The starting speed of the acceleration phase (deceleration phase) is V c , the starting acceleration is A c , the acceleration in the segment is the maximum positive acceleration J m , the end acceleration is the maximum positive acceleration A m Calculate the running time t of the acceleration phase (deceleration phase) Aacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0185]

[0186] Uniform acceleration stage: The starting speed of the uniform acceleration stage is the ending speed V of the previous subdivided speed segment end , the starting point acceleration is the maximum positive acceleration A m , the acceleration in the segment is 0, and the running time t of the acceleration phase is calculated Acon , the tool travel distance is L, the end speed is V end-1 The calculation formula is as follows:

[0187]

[0188] Deceleration and acceleration phase: The starting speed of the deceleration and acceleration phase is the ending speed V of the previous subdivided speed segment. end-1 , the starting point acceleration is the maximum positive acceleration A m , the jerk in the segment is the maximum jerk in the reverse direction - J m , calculate the running time t of the acceleration phase Adec , the tool travel distance is L, the end speed is V end-2 The calculation formula is as follows:

[0189]

[0190] The calculation of the front shift stage is completed.

[0191] (2) If the acceleration of the front speed change stage can reach the maximum acceleration in the reverse direction, and the subdivided speed segments included in the front speed change stage are the acceleration and deceleration stage, the uniform deceleration stage, and the deceleration and deceleration stage, the motion parameter value calculation method of each subdivided speed segment is as follows:

[0192] Acceleration and deceleration stage: The starting speed of the acceleration and deceleration stage is Vc , the starting acceleration is A c , the jerk in the segment is the maximum jerk in the reverse direction - J m , the end acceleration is the reverse maximum acceleration - A m Calculate the running time t of the acceleration and deceleration phase Dacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0193]

[0194] Uniform deceleration stage: The starting speed of the uniform deceleration stage is V end , the starting acceleration and the ending acceleration are both the reverse maximum acceleration - A m , the jerk in the segment is 0. Calculate the running time t of the uniform deceleration stage Dcon , the tool travel distance is L, the end speed is V end-1 The calculation formula is as follows:

[0195]

[0196] Deceleration stage: The starting speed of the deceleration stage is V end-1 , the starting acceleration is the maximum acceleration in the reverse direction - A m , the acceleration in the segment is the maximum positive acceleration J m , the end acceleration is 0. Calculate the running time t of the deceleration phase Ddec , the tool travel distance is L, the end speed is V end-2 The calculation formula is as follows:

[0197]

[0198] The calculation of the front shift stage is completed.

[0199] (3) If the acceleration of the front speed change stage cannot reach the maximum acceleration, the subdivided speed segments included in the front speed change stage are the acceleration stage and the deceleration stage, or the subdivided speed segments included in the front speed change stage are the deceleration stage and the deceleration stage, the motion parameter value of each subdivided speed segment is calculated as follows:

[0200] Acceleration phase (deceleration phase): First, use the following formula to calculate the maximum acceleration that can be achieved:

[0201]

[0202] The starting speed of the acceleration phase (deceleration phase) is V c , the starting acceleration is A c , the acceleration in the segment is the maximum positive acceleration J m , the final acceleration is Am-new Calculate the running time t of the acceleration phase (deceleration phase) Aacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0203]

[0204] Deceleration and acceleration phase: The starting speed of the deceleration and acceleration phase is V end , the starting acceleration is A m-new , the acceleration in the segment is the maximum jerk in the reverse positive direction - J m , the end acceleration is 0. Calculate the running time t of the deceleration phase Adec , the tool travel distance is L, the end speed is V end-1 The calculation formula is as follows:

[0205]

[0206] The calculation of the front shift stage is completed.

[0207] (4) If the acceleration of the front speed change stage cannot reach the maximum acceleration, the subdivided speed segments included in the front speed change stage are the deceleration and acceleration stage and the deceleration and deceleration stage, or the subdivided speed segments included in the front speed change stage are the acceleration and deceleration stage and the deceleration and deceleration stage, the motion parameter values ​​of each subdivided speed segment are calculated as follows:

[0208] Deceleration phase (acceleration and deceleration phase): First, use the following formula to calculate the actual maximum acceleration that can be achieved:

[0209]

[0210] Among them, A m-new is the actual maximum acceleration.

[0211] The starting speed of the deceleration phase (acceleration and deceleration phase) is V c , the starting acceleration is A c , the jerk in the segment is the maximum jerk in the reverse direction - J m , the end acceleration is the actual maximum acceleration A m-new Calculate the running time t of the deceleration phase (acceleration and deceleration phase) Dacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0212]

[0213] Deceleration stage: The starting speed of the deceleration stage is V end , the starting acceleration is the actual maximum acceleration A m-new, the acceleration in the segment is the maximum positive acceleration J m , the end acceleration is 0. Calculate the running time t of the deceleration phase Ddec , the tool travel distance is L, the end speed is V end-1 The calculation formula is as follows:

[0214]

[0215] The calculation of the front shift stage is completed.

[0216] (5) When the S-shaped speed change trajectory includes a uniform speed stage, the motion parameter value calculation method of the uniform speed stage is as follows:

[0217] Uniform speed stage: The starting speed of the uniform speed stage is V t , the starting acceleration is 0, the jerk in the segment is 0, and the ending acceleration is 0. Calculate the running time t of the uniform speed stage univel , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0218]

[0219] The calculation of the uniform velocity phase ends.

[0220] (6) If the acceleration of the post-speed change stage can reach the maximum acceleration in the reverse direction, and the post-speed change stage includes the subdivided speed segments of the deceleration and acceleration stage, the uniform deceleration stage, and the deceleration and deceleration stage, the motion parameter value of each subdivided speed segment is calculated as follows:

[0221] Acceleration and deceleration stage calculation: The starting speed of the acceleration and deceleration stage is V t The starting speed of the acceleration and deceleration phase must be the target speed given previously. The starting acceleration is 0, and the jerk is the reverse maximum jerk - J m ; Calculate the running time t of the acceleration and deceleration phase Dacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0222]

[0223] Calculation of uniform deceleration stage: The starting speed of the uniform deceleration stage is V end , the starting acceleration is the maximum speed in the reverse direction - A m , the acceleration is 0; calculate the running time t of the uniform deceleration stage Dcon , the tool travel distance is L, the end speed is V end-1 The calculation formula is as follows:

[0224]

[0225] Calculation of deceleration stage: The starting speed of the deceleration stage is V end-1 , the starting acceleration is the maximum speed in the reverse direction - A m , jerk is the maximum positive jerk J m The running time of the deceleration phase is t Ddec =A m / J m , the end speed is V e , the calculation of the post-speed shifting stage is completed.

[0226] (7) If the acceleration in the post-shift stage cannot reach the maximum acceleration, calculate the maximum acceleration that can be achieved as When the subdivided speed segments included in the post-speed change stage are the deceleration and acceleration stage and the deceleration and acceleration stage, the motion parameter value calculation method of each subdivided speed segment is as follows:

[0227] Acceleration and deceleration stage calculation: The starting speed of the acceleration and deceleration stage is V t , the starting acceleration is 0, and the jerk is the maximum jerk in the reverse direction - J m ; Calculate the running time t of the acceleration and deceleration phase Dacc , the tool travel distance is L, the end speed is V end The calculation formula is as follows:

[0228]

[0229] Calculation of deceleration stage: The starting speed of the deceleration stage is V end , the starting acceleration is Jerk is the maximum positive jerk J m , the duration of the deceleration phase The end speed is V e , the calculation of the post-speed shifting stage is completed.

[0230] In this embodiment, a detailed method is provided for sequentially determining the motion parameter values ​​of each subdivided speed segment included in an S-shaped speed change trajectory. The parameter values ​​of the starting points and end points of some subdivided speed segments can be obtained according to the subdivided speed segments, and then the unknown parameter values ​​in the subdivided speed segments are calculated, thereby reducing the amount of calculation, improving the response speed of the system, and thus improving the accuracy of the machine tool.

[0231] It should be understood that, although the steps in the above flowchart are sequentially displayed according to the indication of the arrows, and the steps are sequentially displayed according to the indication of the numbers, these steps are not necessarily executed sequentially in the order indicated by the arrows or numbers. Unless otherwise clearly stated in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0232] In one embodiment, Figure 3 , which is a structural block diagram of an S-shaped trajectory planning device in an embodiment, including a parameter value acquisition module 202, a speed determination module 204, a current running speed segment determination module 206, a target running speed segment determination module 208, a target parameter value determination module 210, a speed change trajectory planning module 212, and a tool control module 214, wherein:

[0233] The parameter value acquisition module 202 is used to acquire the current motion parameter value and the original planned trajectory. The parameter value acquisition module is also used to acquire the modified planned trajectory parameter value during the motion of the tool.

[0234] The speed determination module 204 is used to determine the target speed and terminal speed of the S-shaped speed change trajectory according to the modified planned trajectory parameter values, the current motion parameter values ​​and the original planned trajectory.

[0235] The current running speed segment determining module 206 is used to determine the current running speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value.

[0236] The target operating speed segment determining module 208 is used to determine the target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment.

[0237] The target motion parameter value determination module 210 is used to determine the target motion parameter value according to the target running speed segment, the target speed, the terminal speed and the current motion parameter value.

[0238] The speed change trajectory planning module 212 is used to determine the S-shaped speed change trajectory according to the target motion parameter value.

[0239] The tool control module 214 is used to control the tool to continue moving according to the S-shaped speed change trajectory.

[0240] The above-mentioned S-shaped trajectory planning device determines the target speed and terminal speed according to the previous motion parameter value, the original planned trajectory and the modified planned trajectory parameter value to satisfy the tool to complete the S-shaped speed change in the remaining length of the tool track segment. Then, according to the target speed, terminal speed and current motion parameter value, determine the current operating speed segment and the target operating speed segment. Then, the target operating speed segment is segmented to determine all the subdivided speed segments included in the S-shaped speed change trajectory. Finally, the motion parameter value of each subdivided speed segment of the S-shaped speed change trajectory is determined to obtain the S-shaped speed change trajectory. Use the obtained S-shaped speed change trajectory to replace the original planned trajectory to complete the interpolation of the remaining length of the tool track segment, ensure that the acceleration is 0 at the end of the tool track segment, reduce flexible impact, improve machine tool stability, and optimize processing quality.

[0241] In one embodiment, the speed determination module 204 is used to determine the remaining length of the tool path segment according to the original planned trajectory and the current motion parameter value; determine the first critical displacement according to the modified planned trajectory parameter value and the original planned trajectory; compare the remaining length with the first critical displacement to obtain a first comparison result; determine the target speed and terminal speed of the S-shaped speed change trajectory according to the first comparison result, the modified planned trajectory parameter value, the current motion parameter value and the remaining length.

[0242] In this embodiment, the speed determination module 204 compares the remaining length with the first critical displacement to determine whether the target speed is the maximum speed and then calculates the target speed and the terminal speed. The complexity of the calculation is reduced, and after obtaining the target speed and the terminal speed, the framework of the S-shaped speed change trajectory to be calculated subsequently is basically determined.

[0243] In one embodiment, the modified planned trajectory parameter value includes a maximum speed, and the speed determination module 204 is further used to: when the remaining length is less than the first critical displacement, calculate the terminal speed and the target speed so that the remaining length is equal to the second critical displacement, the second critical displacement being the shortest displacement required for the tool to accelerate to the first boundary speed under the current motion parameter value, and then decelerate from the first boundary speed to the terminal speed; the first boundary speed is equal to the sum of the larger of the starting speed and the terminal speed and the speed increment; when the remaining length is greater than or equal to the first critical displacement, use the maximum speed as the target speed of the S-shaped speed change trajectory, and calculate the terminal speed of the S-shaped speed change trajectory so that the remaining length is equal to the first critical displacement.

[0244] In this embodiment, the speed determination module 204 first determines whether the tool can reach the maximum speed. When it is determined that the tool can reach the maximum speed, the maximum speed is used as the target speed, which reduces some calculation amounts and the time for calculating the S-shaped speed change trajectory, making the system more responsive and improving the accuracy of the interpolation process.

[0245] In one embodiment, the current running speed segment determination module 206 is used to determine that the tool is currently in the front speed change stage when the current speed in the current motion parameter value is not equal to the target speed, or the current acceleration in the current motion parameter value is not equal to 0. When the tool is not currently in the front speed change stage, the target speed is not equal to 0 and the remaining length is not equal to the second critical displacement, it is determined that the tool is currently in the uniform speed stage; the second critical displacement is the critical displacement of the tool changing from the starting speed to the target speed and then to the terminal speed without passing through the uniform speed stage. When the tool is not currently in the front speed change stage and the uniform speed stage, and the terminal speed is not equal to the target speed, it is determined that the tool is currently in the post speed change stage.

[0246] In this embodiment, the current running speed segment determination module 206 classifies the S-shaped speed change trajectory according to the acceleration, and divides the S-shaped speed change trajectory into a front speed change stage, a uniform speed stage, and a rear speed change stage. Then, the subdivided speed segments included in the front speed change stage and the rear speed change stage are classified according to the jerk. The two classifications can reduce the complexity of the calculation and improve the response speed of the system.

[0247] In one embodiment, the target operating speed segment determination module 208 is used to determine that the S-shaped speed change trajectory includes the front speed change stage when the tool is currently in the front speed change stage, and the terminal speed and the target speed are both 0. When the tool is currently in the front speed change stage, the terminal speed is equal to the target speed and is not 0, and the remaining length is not equal to the second critical displacement, it is determined that the S-shaped speed change trajectory includes the front speed change stage and the uniform speed stage. When the tool is currently in the front speed change stage, the target speed is not 0, and the target speed is not equal to the terminal speed, and the remaining length is not equal to the second critical displacement, it is determined that the S-shaped speed change trajectory includes the front speed change stage, the uniform speed stage, and the rear speed change stage. When the tool is currently in the uniform speed stage, and the terminal speed is equal to the target speed, it is determined that the S-shaped speed change trajectory includes the uniform speed stage. When the tool is currently in the uniform speed stage, and the terminal speed is not equal to the target speed, it is determined that the S-shaped speed change trajectory includes the uniform speed stage and the rear speed change stage. When the tool is currently in the rear speed change stage, it is determined that the S-shaped speed change trajectory includes the rear speed change stage.

[0248] This embodiment provides a device for determining the speed steps included in the S-shaped speed change trajectory after the current speed segment. The current speed segment is taken as the first target speed segment, and the subsequent speed segments are discussed according to the occurrence order of the speed segments in the S-shaped speed change trajectory, so that the target speed segment can be quickly determined.

[0249] In one embodiment, the modified planned trajectory parameter values ​​include maximum acceleration and maximum jerk. The target operating speed segment determination module 208 is also used to determine the critical speed that the tool can reach when the maximum acceleration is reached in the front speed change stage according to the current motion parameter value, maximum acceleration, maximum jerk, target speed and terminal speed when the S speed change trajectory includes the front speed change stage. The critical speed and the target speed are compared to obtain a second comparison result, and the subdivided speed segments included in the front speed change stage are determined according to the second comparison result. The target parameter value determination module 210 is used to determine the motion parameter values ​​of the subdivided speed segments included in the front speed change stage according to the subdivided speed segments included in the front speed change stage, the target speed, the terminal speed and the current motion parameter value.

[0250] In this embodiment, the target operating speed segment determination module 208 determines the subdivided speed segments included in the front speed shift stage. The target parameter value determination module 210 determines the subdivided speed segment motion parameter values ​​in the front speed shift stage according to the subdivided speed segments included in the front speed shift stage. After the front speed shift stage is divided into subdivided speed segments, the motion parameter values ​​are calculated, which can reduce the difficulty of direct calculation, improve the calculation speed, and make it easier to obtain the motion parameter values ​​of the front speed shift stage.

[0251] In one embodiment, the modified planned trajectory parameter values ​​include the maximum acceleration and the maximum jerk. The target operating speed segment determination module 208 is also used to determine the first speed change according to the target speed and the terminal speed when the S speed change trajectory includes the rear speed change stage; and determine the second speed change according to the maximum acceleration and the maximum jerk. A third comparison result is obtained by comparing the first speed change and the second speed change, and the subdivided speed segments included in the rear speed change stage are determined according to the third comparison result. The target parameter value determination module 210 is also used to determine the motion parameter values ​​of the subdivided speed segments included in the rear speed change stage according to the subdivided speed segments included in the rear speed change stage, the target speed, the terminal speed, and the current motion parameter value.

[0252] In this embodiment, the target operating speed segment determination module 208 determines the subdivided speed segments included in the rear speed change stage. The target parameter value determination module 210 determines the motion parameter values ​​of the subdivided speed segments in the rear speed change stage. Dividing the rear speed change stage into subdivided speed segments and then calculating the motion parameter values ​​can reduce the difficulty of direct calculation, increase the calculation speed, and make it easier to obtain the motion parameter values ​​of the rear speed change stage.

[0253] In one embodiment, the critical speed includes: a high critical speed reached when the tool can reach the maximum forward acceleration in the front speed change stage, and a low critical speed reached when the tool can reach the maximum reverse acceleration in the front speed change stage. The target operation speed segment determination module 208 is also used to determine that the front speed change stage includes a subdivided speed segment with a jerk equal to the maximum forward jerk, a subdivided speed segment with a jerk equal to the maximum reverse jerk, and a subdivided speed segment with a jerk equal to 0 when the target speed is greater than the high critical speed or the target speed is less than the low critical speed. When the target speed is greater than the low critical speed and less than the high critical speed, it is determined that the front speed change stage includes a subdivided speed segment with a jerk equal to the maximum forward jerk and a subdivided speed segment with a jerk equal to the maximum reverse jerk.

[0254] This embodiment provides a device for determining the subdivided speed segments included in the front speed change stage after determining the target operating speed segment. The subdivided speed segments of the front speed change stage can be obtained simply and quickly, and has the advantages of less calculation and fast response.

[0255] In one embodiment, the target operating speed segment determination module 208 is further configured to determine that the rear speed change stage includes an acceleration / deceleration stage, a uniform deceleration stage, and a deceleration / reduction stage when the third comparison result is that the first speed change is greater than the second speed change. When the third comparison result is that the first speed change is less than or equal to the second speed change, determine that the rear speed change stage includes an acceleration / deceleration stage and a deceleration / reduction stage.

[0256] This embodiment provides a device for determining the subdivided speed segments included in the rear speed change stage after determining the target operating speed segment. The device can simply and quickly obtain the subdivided speed segments of the rear speed change stage and has the advantages of less calculation and fast response.

[0257] In one embodiment, the running speed segment includes subdivided speed segments, and the target motion parameter value determination module 210 is used to use the current motion parameter value as the motion parameter value of the starting point of the S-shaped speed change trajectory. According to the motion parameter value of the starting point of the S-shaped speed change trajectory, the target speed, the terminal speed, the modified planned trajectory parameter value, and the subdivided speed segments included in the running speed segment, the motion parameter values ​​of the subdivided speed segments are determined in sequence.

[0258] In this embodiment, the target motion parameter value determination module 210 uses the end speed and end acceleration of the previous subdivided speed segment as the starting speed and starting acceleration of the next subdivided speed segment, and determines the motion parameter value of each subdivided speed segment in combination with each subdivided speed segment. It can quickly determine the motion parameter value of each subdivided speed segment in the S-shaped speed change trajectory, thereby quickly determining the S-shaped speed change trajectory.

[0259] In one embodiment, the target motion parameter value determination module 210 is also used to use the starting speed and starting acceleration in the motion parameter value of the starting point of the S-shaped speed change trajectory as the starting speed and starting acceleration of the first subdivided speed segment, and determine the intra-segment jerk of the first subdivided speed segment; determine the running time, running distance, end speed and end acceleration of the first subdivided speed segment according to the starting speed, starting acceleration and intra-segment jerk of the first subdivided speed segment. Use the end speed and end acceleration of the first subdivided speed segment as the starting speed and starting acceleration of the next subdivided speed segment; assign and calculate each subsequent subdivided speed segment in turn to determine the running time, running distance, end speed and end acceleration of each subdivided speed segment of the S-shaped speed change trajectory; and obtain the motion parameter values ​​of all subdivided speed segments included in the S-shaped speed change trajectory.

[0260] In this embodiment, the target motion parameter value determination module 210 can obtain the parameter values ​​of the starting point and the end point of some subdivided speed segments according to the subdivided speed segments, and then calculate the unknown parameter values ​​in the subdivided speed segments, and sequentially determine the motion parameter value of each subdivided speed segment included in the S-shaped speed change trajectory. The motion parameter value of each subdivided speed segment can be quickly obtained, the response speed of the system is improved, and thus the accuracy of the machine tool is improved.

[0261] In one embodiment, a computer device is provided. The computer device may be a terminal device, and its internal structure diagram may be as follows: Figure 4 As shown. The computer device includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be achieved through WIFI, an operator network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, an S-shaped trajectory planning method, device, computer device and storage medium are implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a button, trackball or touchpad set on the computer device housing, or an external keyboard, touchpad or mouse, etc.

[0262] Those skilled in the art will understand that Figure 4The structure shown in the figure is merely a block diagram of a portion of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. Specifically, the computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0263] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps of the above-mentioned S-shaped trajectory planning method embodiments when executing the computer program.

[0264] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned S-shaped trajectory planning method embodiments are implemented.

[0265] In one embodiment, a computer program product or computer program is provided, the computer program product or computer program includes computer instructions, the computer instructions are stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device performs the steps in the above-mentioned S-shaped trajectory planning method embodiments.

[0266] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes in the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0267] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. An S-shaped trajectory planning method, characterized in that: include: Obtain the current motion parameter value and original planned trajectory of the tool; During the movement of the tool, the modified planned trajectory parameter values ​​are obtained; Determining the remaining length of the tool path segment according to the original planned trajectory and the current motion parameter value; Determine a first critical displacement according to the modified planned trajectory parameter value and the original planned trajectory, wherein the first critical displacement is a minimum distance that the tool can complete an S-shaped speed change under the current motion parameter value; The remaining length is compared with the first critical displacement to obtain a first comparison result; the remaining length is obtained by subtracting the distance the tool has traveled from the total length of the tool path segment; the first critical displacement refers to the shortest displacement required for the tool to accelerate to the maximum speed at the positive maximum jerk at the starting speed, and then decelerate from the maximum speed to the terminal speed at the reverse maximum jerk; The modified planned trajectory parameter value includes the maximum speed; the current motion parameter value includes a current speed that is equal to the starting speed of the S-shaped speed change trajectory; Determining a target speed and an end speed of the S-shaped speed change trajectory according to the first comparison result, the modified planned trajectory parameter value, the current motion parameter value, and the remaining length; When the remaining length is less than the first critical displacement, the target speed and the terminal speed are calculated so that the remaining length is equal to a second critical displacement, wherein the second critical displacement is the shortest displacement required for the tool to accelerate to a first boundary speed and then decelerate from the first boundary speed to the terminal speed under the current motion parameter value; The first boundary speed is equal to the sum of the larger of the starting speed and the terminal speed and the speed increment; When the remaining length is greater than or equal to the first critical displacement, taking the maximum speed as the target speed of the S-shaped speed change trajectory, and calculating the terminal speed of the S-shaped speed change trajectory so that the remaining length is equal to the first critical displacement; The expression of the first critical displacement is shown in formula (1): The second critical displacement formula is shown in formula (2): The calculation formula of the first boundary speed is as follows: In the above formula: S1 is the first critical displacement, S2 is the second critical displacement, V t is the target speed, V e is the terminal velocity, V c is the starting speed, V b is the first boundary speed, A m is the maximum positive acceleration, J m is the maximum positive acceleration; Determining a current operating speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value; Determining a target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment; Determining a target motion parameter value for the target running speed segment according to the target running speed segment, the target speed, the terminal speed, and the current motion parameter value; Determining the S-shaped speed change trajectory according to the target motion parameter value; The tool is controlled to continue moving according to the S-shaped speed change trajectory.

2. The method according to claim 1, characterized in that Determining the current operating speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value includes: When the current speed in the current motion parameter value is not equal to the target speed, or the current acceleration in the current motion parameter is not equal to 0, determining that the tool is currently in the front speed change stage; When the tool is not currently in the pre-speed change stage, the target speed is not equal to 0 and the remaining length is not equal to the second critical displacement, it is determined that the tool is currently in the uniform speed stage; the second critical displacement is the critical displacement of the tool changing from the starting speed to the target speed and then to the terminal speed without passing through the uniform speed stage; When the tool is not currently in the front speed change stage and the uniform speed stage, and the terminal speed is not equal to the target speed, it is determined that the tool is currently in the rear speed change stage.

3. The method according to claim 2, characterized in that The step of determining the target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment comprises: When the tool is currently in the front speed change stage, and the terminal speed and the target speed are both 0, determining that the S-shaped speed change trajectory includes the front speed change stage; When the tool is currently in the front speed change stage, and the terminal speed is equal to the target speed and is not 0, and the remaining length is not equal to the second critical displacement, determining that the S-shaped speed change trajectory includes the front speed change stage and the uniform speed stage; When the tool is currently in the front speed change stage, and the target speed is not 0, and the target speed is not equal to the terminal speed, and the remaining length is not equal to the second critical displacement, it is determined that the S-shaped speed change trajectory includes the front speed change stage, the uniform speed stage, and the rear speed change stage; When the tool is currently in the uniform speed stage, and the terminal speed is equal to the target speed, determining that the S-shaped speed change trajectory includes the uniform speed stage; When the tool is currently in the uniform speed stage, and the terminal speed is not equal to the target speed, determining that the S-shaped speed change trajectory includes the uniform speed stage and the post-speed change stage; When the tool is currently in the rear speed change stage, it is determined that the S-shaped speed change trajectory includes the rear speed change stage.

4. The method according to claim 3, characterized in that: The modified planned trajectory parameter values ​​include maximum acceleration and maximum jerk; The step of determining the target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment further includes: When the S-shaped speed change trajectory includes the front speed change stage, determining the critical speed at which the tool can reach the maximum acceleration in the front speed change stage according to the current motion parameter value, the maximum acceleration, the maximum jerk, the target speed, and the terminal speed; Comparing the critical speed with the target speed to obtain a second comparison result; Determining the subdivided speed segments included in the front speed shift stage according to the second comparison result; The step of determining the target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed, and the current motion parameter value comprises: The motion parameter values ​​of the subdivided speed segments included in the front speed shift stage are determined according to the subdivided speed segments included in the front speed shift stage, the target speed, the terminal speed and the current motion parameter value.

5. The method according to claim 3, characterized in that: The modified planned trajectory parameter values ​​include maximum acceleration and maximum jerk; The step of determining the target operating speed segment included in the S-shaped speed change trajectory according to the current operating speed segment further includes: When the S-shaped speed change trajectory includes the rear speed change stage, determining a first speed change amount according to the target speed and the terminal speed; determining a second velocity variation according to the maximum acceleration and the maximum jerk; Compare the first speed change amount with the second speed change amount to obtain a third comparison result; According to the third comparison result, determining the subdivided speed segments included in the rear speed shifting stage; The step of determining the target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed, and the current motion parameter value comprises: The motion parameter values ​​of the subdivided speed segments included in the rear speed shift stage are determined according to the subdivided speed segments included in the rear speed shift stage, the target speed, the terminal speed and the current motion parameter value.

6. The method according to claim 4, characterized in that The critical speed includes a high critical speed reached when the tool can reach the maximum acceleration in the forward direction during the front speed change stage, and a low critical speed reached when the tool can reach the maximum acceleration in the reverse direction during the front speed change stage; The step of determining the subdivided speed segments included in the front speed shifting stage according to the second comparison result comprises: When the target speed is greater than the high critical speed or the target speed is less than the low critical speed, determining that the front shift stage includes a subdivided speed segment with a jerk equal to a forward maximum jerk, a subdivided speed segment with a jerk equal to a reverse maximum jerk, and a subdivided speed segment with a jerk equal to 0; When the target speed is greater than the low critical speed and less than the high critical speed, it is determined that the front shift stage includes a subdivided speed segment with a jerk equal to a forward maximum jerk and a subdivided speed segment with a jerk equal to a reverse maximum jerk.

7. The method according to claim 5, characterized in that Determining the subdivided speed segments included in the rear speed shifting stage according to the third comparison result comprises: When the third comparison result is that the first speed change is greater than the second speed change, determining that the rear speed change stage includes an acceleration and deceleration stage, a uniform deceleration stage, and a deceleration and deceleration stage; When the third comparison result is that the first speed change is less than or equal to the second speed change, it is determined that the rear speed change stage includes an acceleration and deceleration stage and a deceleration and deceleration stage.

8. The method according to claim 1, characterized in that The target operating speed segment includes subdivided speed segments; The step of determining the target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed, and the current motion parameter value comprises: Using the current motion parameter value as the motion parameter value of the starting point of the S-shaped speed change trajectory; The motion parameter values ​​of the subdivided speed segments are determined in sequence according to the motion parameter values ​​of the starting point of the S-shaped speed change trajectory, the target speed, the terminal speed, the modified planned trajectory parameter values ​​and the subdivided speed segments.

9. The method according to claim 8, characterized in that The step of sequentially determining the motion parameter values ​​of the subdivided speed segments according to the motion parameter values ​​of the starting point of the S-shaped speed change trajectory, the target speed, the terminal speed, the modified planned trajectory parameter values, and the subdivided speed segments comprises: The starting point speed and the starting point acceleration in the motion parameter values ​​of the starting point of the S-shaped speed change trajectory are used as the starting point speed and the starting point acceleration of the first subdivided speed segment, and the intra-segment jerk of the first subdivided speed segment is determined; Determine the running time, running distance, ending speed, and ending acceleration of the first subdivided speed segment according to the starting speed of the first subdivided speed segment, the starting acceleration, and the jerk within the segment; The end speed and the end acceleration of the first subdivided speed segment are used as the starting speed and the starting acceleration of the next subdivided speed segment; Each subsequent subdivided speed segment is assigned and calculated in turn to determine the running time, running distance, end speed and end acceleration of each subdivided speed segment, and obtain the motion parameter value of each subdivided speed segment.

10. An S-shaped trajectory planning device, characterized in that: The device comprises: A parameter value acquisition module is used to obtain the current motion parameter value and the original planned trajectory of the tool; The parameter value acquisition module is also used to acquire the modified planned trajectory parameter value during the movement of the tool; A speed determination module is used to determine the remaining length of the tool path segment according to the original planned trajectory and the current motion parameter value; determine the first critical displacement according to the modified planned trajectory parameter value and the original planned trajectory, the first critical displacement being the minimum distance that the tool can complete the S-shaped speed change under the current motion parameter value; compare the remaining length with the first critical displacement to obtain a first comparison result; obtain the remaining length by subtracting the distance that the tool has traveled from the total length of the tool path segment; the first critical displacement refers to the shortest displacement required for the tool to accelerate to the maximum speed at the positive maximum acceleration at the starting speed, and then decelerate from the maximum speed to the terminal speed at the reverse maximum jerk; the modified planned trajectory parameter value includes the maximum speed; the current speed included in the current motion parameter value is equal to the starting speed of the S-shaped speed change trajectory; according to the first ratio The target speed and terminal speed of the S-shaped speed change trajectory are determined by comparing the result, the modified planned trajectory parameter value, the current motion parameter value and the remaining length; when the remaining length is less than the first critical displacement, the target speed and the terminal speed are calculated so that the remaining length is equal to the second critical displacement, the second critical displacement being the shortest displacement required for the tool to accelerate to the first boundary speed under the current motion parameter value and then decelerate from the first boundary speed to the terminal speed; the first boundary speed is equal to the sum of the larger of the starting speed and the terminal speed and the speed increment; when the remaining length is greater than or equal to the first critical displacement, the maximum speed is used as the target speed of the S-shaped speed change trajectory, and the terminal speed of the S-shaped speed change trajectory is calculated so that the remaining length is equal to the first critical displacement; The expression of the first critical displacement is shown in formula (1): The second critical displacement formula is shown in formula (2): The calculation formula of the first boundary speed is as follows: In the above formula: S1 is the first critical displacement, S2 is the second critical displacement, V t is the target speed, V e is the terminal velocity, V c is the starting speed, V b is the first boundary speed, A m is the maximum positive acceleration, J m is the maximum positive acceleration; A current running speed segment determining module, used for determining the current running speed segment of the tool according to the target speed, the terminal speed and the current motion parameter value; a target operating speed segment determining module, configured to determine a target operating speed segment included in an S-shaped speed change trajectory according to the current operating speed segment; a target motion parameter value determination module, configured to determine a target motion parameter value of the target running speed segment according to the target running speed segment, the target speed, the terminal speed and the current motion parameter value; A speed change trajectory planning module, used for determining an S-shaped speed change trajectory according to the target motion parameter value; The tool control module is used to control the tool to continue moving according to the S-shaped speed change trajectory.

11. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 9 are implemented.

12. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 9 are implemented.

Images