Motion planning method and device, medium and motor
Through the 5-stage S-type acceleration and deceleration planning method, the circular integer error is eliminated, the acceleration value setting is avoided, and the stability and accuracy of the system are improved, which solves the operational instability caused by circular integer error in the prior art.
Patent Information
- Application Number
- CN202410960093.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-17
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-17
AI Technical Summary
In CNC machining and motion control systems, circular errors lead to unstable system operation and reduced accuracy. The prior art optimizes by adjusting the acceleration value but is logically complex.
The 5-stage S-type acceleration and deceleration planning method is adopted to eliminate round-round errors and do not involve the setting of acceleration values. It ensures that the speed of deceleration and acceleration stages monotonically increases or decreases, avoids reversal phenomena, and determines the control equation through preset relational expressions. , generate accurate motion control instructions.
It improves the operating stability and processing accuracy of the system, reduces the calculation amount, and ensures that the control system performs smooth and precise movement according to the expected path.
Smart Images

Figure CN118915626A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of industrial automation, and in particular to a motion planning method, device, medium and motor. Background Art
[0002] In CNC machining or some common motion control system scenarios, the smoothness, accuracy, and efficiency of the machining path or machining trajectory are indicators that need to be considered in particular, so the speed planning of the machining path is self-evidently important to the axis. Since the control system uses periodic output of interpolation data to control the axis, that is, based on the pre-defined machining path or motion trajectory data, continuous motion control instructions are calculated and generated in each interpolation cycle. These motion control instructions include parameters such as the target position, speed, and acceleration of the axis. The interpolation algorithm is used to accurately control the axis position within the interpolation cycle to ensure that the system can move smoothly and accurately along the expected path.
[0003] However, when actually calculating the path curve, the planning time of each segment may not be an integer multiple of the interpolation period. The decimal part of the calculation time value divided by the interpolation period is called rounding error. This rounding error will seriously affect the stability and accuracy of the actual operation of the entire system. In the prior art, the rounding error is usually optimized by adjusting the acceleration value, but the setting logic of the acceleration value is complicated. Summary of the invention
[0004] The purpose of the present invention is to provide a motion planning method, device, medium and motor, which use a 5-segment S-shaped acceleration and deceleration planning, eliminate rounding errors, do not involve the setting of acceleration values, and the speed in the deceleration and deceleration stages is monotonically increasing or decreasing, and no reversal phenomenon will occur. It can also ensure that the control system moves smoothly and accurately along the expected path, thereby improving the operating stability and processing accuracy of the system; compared with the traditional 7-segment S-shaped curve control method, the amount of calculation is greatly reduced.
[0005] On the one hand, the present application provides a motion planning method, comprising:
[0006] Acquire a first motion planning parameter set by a user, and determine a motion trajectory according to the first motion planning parameter and a current motion parameter, wherein the motion trajectory includes an accelerated motion process and / or a uniform motion process and / or a decelerated motion process;
[0007] Determine the motion time and motion displacement corresponding to each motion process in the motion trajectory;
[0008] Rounding off each of the movement times to obtain a target time; the target time is an integer multiple of the interpolation period;
[0009] Determine a control equation for each motion process according to each of the target times and a preset relationship expression, wherein the control equation includes a position control equation, a speed control equation, and / or an acceleration control equation;
[0010] A motion control instruction is generated according to the control equation of each motion process according to the interpolation cycle, and the motor motion is controlled according to the motion control instruction; the motion control instruction includes a position control instruction, a speed control instruction, and / or an acceleration control instruction.
[0011] In one embodiment, after obtaining the first motion planning parameter set by the user, the method further includes:
[0012] Adjusting the first motion planning parameter according to the first motion planning parameter and the current motion parameter to obtain a second motion planning parameter;
[0013] Determining a motion trajectory according to the first motion planning parameter and the current motion parameter includes:
[0014] The motion trajectory is determined according to the second motion planning parameters and the current motion parameters.
[0015] In one embodiment, adjusting the first motion planning parameter according to the first motion planning parameter and the current motion parameter to obtain the second motion planning parameter includes:
[0016] Determining whether the first motion planning parameters need to be adjusted according to the final speed and target displacement in the first motion planning parameters and the current speed and current position in the current motion parameters, wherein the target displacement is the displacement corresponding to the movement from the current position to the target position;
[0017] If necessary, adjust the maximum planned acceleration, and / or the maximum planned deceleration and / or the final speed in the first motion planning parameters.
[0018] In one embodiment, determining whether the first motion planning parameters need to be adjusted according to the final speed and target displacement in the first motion planning parameters and the current speed and current position in the current motion parameters includes:
[0019] Determining whether the direction of the current speed is the same as the direction of the target displacement;
[0020] If they are the same, then calculating the first displacement required when the current speed reaches the final speed according to the maximum planned acceleration or the maximum planned deceleration in the first motion planning parameter, and determining whether it is necessary to adjust the maximum planned acceleration and / or the maximum planned deceleration and / or the final speed according to the first displacement and the target displacement;
[0021] If they are not the same, the displacement increment of the current speed decelerating to zero according to the maximum planned acceleration is calculated, and whether the maximum planned acceleration and / or the final speed needs to be adjusted is determined according to the displacement increment and the target displacement.
[0022] In one embodiment, determining whether the maximum planned acceleration and / or the maximum planned deceleration and / or the terminal velocity needs to be adjusted according to the first displacement and the target displacement includes:
[0023] When the first displacement is greater than the target displacement and the current speed is greater than the final speed, adjusting the maximum planned deceleration according to the first displacement and the target displacement;
[0024] When the first displacement is greater than the target displacement and the current speed is less than the final speed, the maximum planned acceleration is adjusted according to the first displacement and the target displacement; when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, the maximum planned acceleration is kept unchanged, and the final speed is adjusted according to the target displacement, the current speed and the maximum planned acceleration.
[0025] In one embodiment, determining whether the maximum planned acceleration and / or the final velocity needs to be adjusted according to the displacement increment and the target displacement includes:
[0026] Calculate the second displacement required to accelerate from zero to the final velocity at the maximum planned acceleration;
[0027] When the second displacement is greater than the sum of the absolute value of the target displacement and the displacement increment, the maximum planned acceleration is adjusted according to the sum and the final velocity, and when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, the maximum planned acceleration is kept unchanged, and the final velocity is adjusted according to the sum and the maximum planned acceleration.
[0028] In one embodiment, the preset relationship expression is determined as follows:
[0029] Designing a preset acceleration and deceleration model, the preset acceleration and deceleration model includes a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing acceleration, and a fifth stage of decreasing acceleration;
[0030] Using a fifth-order polynomial to fit the first displacement equation of the acceleration motion process and the second displacement equation of the deceleration motion process of the preset acceleration and deceleration model, the acceleration motion process includes the process corresponding to the first stage and the second stage, and the deceleration motion process includes the process corresponding to the fourth stage and the fifth stage;
[0031] Using a linear equation to fit a third displacement equation of the uniform motion process of the preset acceleration and deceleration model, the uniform motion process includes a process corresponding to the third stage, and each displacement equation represents a corresponding relationship between displacement and time in the current motion process;
[0032] Determine the time coefficient in the first displacement equation, the second displacement equation and the third displacement equation according to the movement time and the movement displacement corresponding to each movement process and the movement planning parameters corresponding to the movement trajectory;
[0033] The preset relationship expression is a correspondence between each of the time coefficients and the motion time and motion displacement corresponding to the motion process, and the motion planning parameters corresponding to the motion trajectory.
[0034] On the other hand, the present application provides a motion planning device, comprising:
[0035] Memory for storing computer programs;
[0036] The processor is used to implement the steps of the motion planning method as described above when executing a computer program.
[0037] On the other hand, the present application provides a non-volatile storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the motion planning method as described above are implemented.
[0038] On the other hand, the present application provides a motor, comprising the motion planning device as described above.
[0039] The present invention provides a motion planning method, device, medium and motor. The scheme obtains the motion planning parameters set by the user and determines the exact motion time corresponding to each motion process of the motion trajectory in combination with the current motion parameters, ensuring that each motion time is an integer multiple of the interpolation period, thereby eliminating the rounding error; in addition, the control equation corresponding to each motion process is determined by using a preset relationship expression, including the control equations of displacement, velocity and acceleration, and then a continuous and accurate motion control instruction is generated within the interpolation period. The precise position, velocity and acceleration parameters within each period can be determined according to the control equation, without involving the setting of the acceleration value, and can ensure that the control system moves smoothly and accurately along the expected path, thereby improving the operating stability and processing accuracy of the system. BRIEF DESCRIPTION OF THE DRAWINGS
[0040] In order to more clearly illustrate the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0041] Figure 1 A flow chart of a motion planning method provided by the present invention;
[0042] Figure 2 This is a schematic diagram of an acceleration-velocity-displacement curve provided by the present invention. DETAILED DESCRIPTION
[0043] The core of the present invention is to provide a motion planning method, device, medium and motor, which use a 5-segment S-shaped acceleration and deceleration planning to eliminate rounding errors, do not involve the setting of acceleration values, and the speed in the deceleration and deceleration stages is monotonically increasing or decreasing, and no reversal will occur. It can also ensure that the control system moves smoothly and accurately along the expected path, thereby improving the operating stability and processing accuracy of the system; compared with the traditional 7-segment S-shaped curve control method, the amount of calculation is greatly reduced.
[0044] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are 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.
[0045] On the one hand, if Figure 1 As shown, the present application provides a motion planning method, comprising:
[0046] S11: Obtain first motion planning parameters set by a user, and determine a motion trajectory according to the first motion planning parameters and current motion parameters, where the motion trajectory includes an acceleration motion process and / or a uniform speed motion process and / or a deceleration motion process.
[0047] The principle in this step is to decompose the entire motion process into three stages: acceleration, constant speed, and deceleration, based on the motion planning parameters set by the user and the current motion state, through the determination of the motion trajectory. This step analyzes and combines the motion requirements input by the user and the current trajectory information to ensure that the system can generate a specific motion control plan including each stage according to the expected path, thereby achieving smooth and precise control of the motion process.
[0048] In one embodiment, after obtaining the first motion planning parameters set by the user, it also includes: adjusting the first motion planning parameters according to the first motion planning parameters and the current motion parameters to obtain second motion planning parameters; determining the motion trajectory according to the first motion planning parameters and the current motion parameters, including: determining the motion trajectory according to the second motion planning parameters and the current motion parameters.
[0049] In this embodiment, the first motion planning parameters set by the user are first obtained. These parameters may include the set target displacement S c , maximum planned speed V c , maximum planned acceleration A, maximum planned deceleration D, planned final velocity V e , the maximum acceleration limit value of the axis A max , the maximum deceleration limit value of the axis D max , the maximum speed limit of the axis V max Etc. Subsequently, these first motion planning parameters are adjusted according to the current motion state and environmental conditions to generate second motion planning parameters to optimize the motion path.
[0050] Next, the specific motion trajectory is determined based on the adjusted first motion planning parameters and the current motion parameters. The determination of the motion trajectory takes into account the acceleration, constant speed and deceleration stages, ensuring that the motor can smoothly and accurately execute the specified path throughout the entire motion process. Through dynamic adjustment and adaptive planning, this method can not only maintain high-precision motion control in a changing working environment, but also optimize the system's response speed and motion efficiency, and improve overall operational stability and processing accuracy.
[0051] In one embodiment, the first motion planning parameters are adjusted according to the first motion planning parameters and the current motion parameters to obtain the second motion planning parameters, including: determining whether the first motion planning parameters need to be adjusted according to the final velocity and target displacement in the first motion planning parameters, and the current velocity and current position in the current motion parameters, wherein the target displacement is the path length corresponding to the movement from the current position to the target position; if necessary, adjusting the maximum planned acceleration, and / or the maximum planned deceleration and / or the final velocity in the first motion planning parameters.
[0052] In this embodiment, dynamic adjustment is performed based on the first motion planning parameters set by the user and the current motion state to generate the second motion planning parameters. This adjustment process is to ensure that more accurate and efficient path control can be achieved during the motion process.
[0053] Specifically, the final velocity and target displacement in the first motion planning parameters, as well as the current velocity and current position in the current motion parameters are taken into account. The target displacement refers to the displacement corresponding to moving from the current position to the target position. Next, the degree of conformity of the current velocity and position with the expected path is evaluated. If it is detected that the current velocity or position deviates from the expected path or needs further optimization, the first motion planning parameters will be considered for adjustment.
[0054] Possible adjustments include the maximum planned acceleration, which is to adjust the maximum allowable acceleration during the acceleration phase to reach the target speed faster or more smoothly; the maximum planned deceleration, which is to adjust the maximum allowable deceleration during the deceleration phase to stop or transition to a new state of motion more smoothly; and the final speed, which is to adjust the maximum speed that can be achieved during the motion process.
[0055] This dynamic adjustment process can effectively deal with situations where the setting parameters are inaccurate, thereby optimizing the performance and stability of the motion control system.
[0056] In one embodiment, whether the first motion planning parameters need to be adjusted is determined based on the final velocity and target displacement in the first motion planning parameters, and the current velocity and current position in the current motion parameters, including: determining whether the direction of the current velocity is the same as the direction of the target displacement; if they are the same, calculating the first displacement required when the current velocity reaches the final velocity according to the maximum planned acceleration or maximum planned deceleration in the first motion planning parameters, and determining whether the maximum planned acceleration and / or maximum planned deceleration and / or final velocity need to be adjusted based on the first displacement and the target displacement; if they are not the same, calculating the displacement increment when the current velocity decelerates to zero according to the maximum planned deceleration, and determining whether the maximum planned acceleration and / or final velocity need to be adjusted based on the displacement increment and the target displacement.
[0057] In this embodiment, the first motion planning parameters set by the user and the current motion state are dynamically adjusted to generate optimized motion planning parameters. This process involves a detailed analysis of the current speed and position and the direction and distance of the expected path to ensure that the motion control system can achieve a more accurate and efficient motion trajectory in actual operation.
[0058] First, determine whether the direction of the current velocity is the same as the direction of the target displacement. If the directions are the same, calculate the first displacement required for the current velocity to reach the final velocity according to the maximum planned acceleration (when the current velocity is less than the final velocity) or the maximum planned deceleration (when the current velocity is greater than the final velocity) in the first motion planning parameter. This displacement is compared with the target displacement to determine whether the maximum planned acceleration, maximum planned deceleration or final velocity needs to be adjusted. The purpose of the adjustment is to ensure that smooth and accurate motion control can be achieved with appropriate speed and acceleration when the target position is reached.
[0059] If the direction of the current speed is different from the direction of the target displacement, the displacement increment when the current speed is decelerated to zero speed according to the maximum planned acceleration minus the acceleration will be calculated (that is, it is necessary to decelerate to zero first and then accelerate in the reverse direction). The sum of this displacement increment and the target displacement is the displacement required for reverse acceleration. The maximum planned acceleration and the sum are used to determine whether the maximum planned acceleration or the final speed needs to be adjusted. The adjustment in this case is intended to ensure that when reverse movement or stopping is required, it can be precisely controlled at an appropriate speed and deceleration to avoid overshoot or unnecessary motion errors.
[0060] This embodiment can not only optimize the execution of the motion trajectory, but also minimize the rounding error, thereby improving the overall processing quality and system efficiency.
[0061] In one embodiment, whether the maximum planned acceleration and / or the maximum planned deceleration and / or the final velocity needs to be adjusted is determined based on the first displacement and the target displacement, including: when the first displacement is greater than the target displacement and the current speed is greater than the final velocity, adjusting the maximum planned deceleration based on the first displacement and the target displacement; when the first displacement is greater than the target displacement and the current speed is less than the final velocity, adjusting the maximum planned acceleration based on the first displacement and the target displacement; when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, keeping the maximum planned acceleration unchanged, and adjusting the final velocity based on the target displacement, the current speed and the maximum planned acceleration.
[0062] In this embodiment, according to the comparison between the first displacement and the target displacement, the motion planning parameters are dynamically adjusted to optimize the execution effect of the motion control. First, the system compares the magnitude relationship between the first displacement and the target displacement.
[0063] If the first displacement is greater than the target displacement and the current speed is greater than the final speed, it indicates that the actual movement distance (first displacement) required to decelerate from the current speed to the final speed at the maximum planned deceleration exceeds the expected path length (target displacement). The maximum planned deceleration will be adjusted according to the first displacement and the target displacement.
[0064] If the first displacement is greater than the target displacement and the current speed is less than the final speed, it means that the actual movement distance (first displacement) required to accelerate from the current speed to the final speed at the maximum planned acceleration exceeds the expected path length (target displacement). In this case, the maximum planned acceleration will be adjusted according to the first displacement and the target displacement.
[0065] In addition, the maximum planned acceleration limit is also determined. If the adjusted maximum planned acceleration exceeds the preset acceleration limit, the maximum planned acceleration will remain unchanged, and the final velocity will be adjusted based on the target displacement, current velocity, and maximum planned acceleration. This adjustment strategy ensures that precise control can be performed in accordance with the user's requirements in all situations, avoiding instability or unexpected situations that may occur during motion.
[0066] Through this dynamic adjustment mechanism based on real-time motion status and expected path, this embodiment can effectively optimize the motion control process, improve the operating stability, accuracy and efficiency of the system, and thus better meet complex processing or motion control requirements.
[0067] In one embodiment, whether the maximum planned acceleration and / or final velocity needs to be adjusted is determined based on the displacement increment and the target displacement, including: calculating the second displacement required to accelerate from zero to the final velocity with the maximum planned acceleration; when the second displacement is greater than the sum of the absolute value of the target displacement and the displacement increment, adjusting the maximum planned acceleration based on the sum and the final velocity, and when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, keeping the maximum planned acceleration unchanged, and adjusting the final velocity based on the sum and the maximum planned acceleration.
[0068] In this embodiment, the motion planning parameters are dynamically adjusted based on the comparison of the displacement increment and the target displacement to optimize the execution effect of the motion control. First, the second displacement required to accelerate from zero speed to the final speed at the maximum planned acceleration (i.e., the displacement corresponding to the reverse acceleration process) is calculated. This calculation ensures that the expected final speed can be achieved when the acceleration process is implemented, and takes into account the length requirements of the actual motion path.
[0069] Then, the absolute value of the second displacement is evaluated against the sum of the target displacement and the displacement increment. If the absolute value of the second displacement is greater than the sum of the target displacement and the displacement increment, it indicates that the actual movement distance exceeds the expected path length, and the maximum planned acceleration is adjusted accordingly.
[0070] In addition, the maximum planned acceleration limit is also determined. If the adjusted maximum planned acceleration exceeds the preset acceleration limit, the maximum planned acceleration will remain unchanged, and the final velocity will be adjusted based on the sum value and the maximum planned acceleration. This adjustment strategy ensures that stability and accuracy are always maintained during the motion process, while achieving efficient motion control under different working conditions.
[0071] Through this dynamic adjustment mechanism based on real-time motion status and expected path length, this embodiment can effectively optimize the motion control process, improve the operating stability, accuracy and efficiency of the system, and is suitable for various complex motion control application scenarios.
[0072] S12: Determine the movement time and movement displacement corresponding to each movement process in the movement trajectory.
[0073] This step is to calculate the time required for each movement process and the corresponding displacement by analyzing the first movement planning parameters and the current movement parameters set by the user and combining them with the determined movement trajectory.
[0074] Specifically, the expected trajectory of the entire motion path is calculated based on the first motion planning parameters set by the user, including the expected target position, speed requirements, and possible acceleration limits. Subsequently, the specific time and displacement of each stage of the actual motion process, namely the acceleration, uniform speed, and deceleration stages, are derived based on the current motion state, such as the current position and speed. These calculations ensure that the system can perform smooth and precise motion at the appropriate speed and acceleration in each stage to achieve the expected processing effect.
[0075] S13: Round off each movement time to obtain a target time; the target time is an integer multiple of the interpolation period.
[0076] The principle of this step is to round off the motion time calculated for each motion process to ensure that each target time is an integer multiple of the interpolation period. This step is to adapt to the control system's periodic output of interpolation data, ensuring that motion control instructions can be generated and executed within a fixed time interval, thereby ensuring that the system can perform smooth and accurate motion control according to the expected processing path.
[0077] S14: Determine a control equation for each motion process according to each target time and a preset relationship expression, the control equation including a position control equation, a speed control equation, and / or an acceleration control equation.
[0078] The principle of this step is to calculate the corresponding control equations for each motion process according to the preset relationship expression, including the position control equation, the speed control equation and / or the acceleration control equation. The formulation of these control equations is based on the target time and the expected motion characteristics during the motion process, such as the change law of the acceleration, uniform speed and deceleration stages. Through these equations, the position, speed and acceleration of the axis at each moment can be accurately calculated to generate motion control instructions that ensure smooth and precise motion, thereby achieving precise control of the motor motion.
[0079] S15: Generate motion control instructions according to the control equation of each motion process according to the interpolation cycle, and control the motor movement according to the motion control instructions; the motion control instructions include position control instructions, speed control instructions, and / or acceleration control instructions.
[0080] The principle of this step is to generate motion control instructions according to the preset interpolation cycle based on the control equation determined by S14. These instructions include position control instructions, speed control instructions and / or acceleration control instructions, which are used to accurately control the movement of the motor. Through the instructions generated in each interpolation cycle, it is ensured that the axis can move smoothly and accurately according to the expected motion trajectory. This process not only takes into account the position, speed and acceleration requirements in the control equation, but also ensures the continuity and real-time nature of the motion instructions in the control system that periodically outputs interpolation data, thereby improving the system's operating stability and processing accuracy.
[0081] In summary, the present application provides a motion planning method, which obtains the motion planning parameters set by the user and combines them with the current motion parameters to determine the exact motion time corresponding to each motion process of the motion trajectory, ensuring that each motion time is an integer multiple of the interpolation period, thereby eliminating rounding errors; in addition, the control equations corresponding to each motion process are determined using preset relationship expressions, including control equations for displacement, velocity, and acceleration, and then continuous and accurate motion control instructions are generated within the interpolation period. The precise position, velocity, and acceleration parameters within each period can be determined based on the control equations, without involving the setting of the jerk value, and can ensure that the control system moves smoothly and accurately along the expected path, thereby improving the operating stability and processing accuracy of the system.
[0082] Based on the above embodiments:
[0083] In one embodiment, the preset relationship expression is determined in the following manner: designing a preset acceleration and deceleration model, the preset acceleration and deceleration model including a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing acceleration and a fifth stage of decreasing acceleration; using a fifth-order polynomial to fit a first displacement equation of the acceleration motion process and a second displacement equation of the deceleration motion process of the preset acceleration and deceleration model, the acceleration motion process including processes corresponding to the first stage and the second stage, and the deceleration motion process including processes corresponding to the fourth stage and the fifth stage; using a linear equation to fit a third displacement equation of the uniform motion process of the preset acceleration and deceleration model, the uniform motion process including processes corresponding to the third stage, each displacement equation characterizing the corresponding relationship between displacement and time in the current motion process; determining the time coefficient in the first displacement equation, the second displacement equation and the third displacement equation according to the motion time and motion displacement corresponding to each motion process, and the motion planning parameters corresponding to the motion trajectory; the preset relationship expression is the corresponding relationship between each time coefficient and the motion time and motion displacement corresponding to the motion process, and the motion planning parameters corresponding to the motion trajectory.
[0084] In this embodiment, the process of determining the preset relationship expression involves designing and fitting the preset acceleration and deceleration model, and determining the time coefficient in the displacement equation according to the fitting result. First, the preset acceleration and deceleration model in this embodiment is a 5-stage acceleration and deceleration model (such as Figure 2 Specifically, the preset acceleration and deceleration model is divided into five stages, including different stages of gradually increasing acceleration, gradually decreasing acceleration, zero acceleration, gradually decreasing deceleration, and gradually increasing deceleration. These stages respectively describe the acceleration, uniform speed, and deceleration processes of different parts in the machining path.
[0085] Secondly, for the acceleration and deceleration process of each preset acceleration and deceleration model, a fifth-order polynomial is used to fit to obtain the corresponding displacement equation. These displacement equations describe the relationship between the displacement and time during the motion process. Through fitting, the displacement change law of each stage can be accurately calculated.
[0086] In addition, for the uniform motion process, linear equations are used for fitting to obtain the displacement equations of the uniform phase. These equations are formulated based on the current motion parameters and the preset motion planning parameters to ensure that the axis can move at a constant speed during the uniform phase.
[0087] Finally, the time coefficient in the displacement equation is determined according to the motion time, motion displacement and preset motion planning parameters of each motion process. These time coefficients reflect the relationship between time change and displacement change during the motion process, and are the core part of the preset relationship expression. The preset relationship expression ensures the accurate generation of motion control instructions through the correspondence between these time coefficients and motion planning parameters, so that the system can move smoothly and accurately along the expected path. This embodiment uses a quintic polynomial to fit the displacement equations of the acceleration and deceleration segments in the 5-segment S-shaped curve, and verifies that the speed of the curve fitted by this method in the acceleration and deceleration segments is monotonically increasing or decreasing without reversal.
[0088] In a specific embodiment, the steps of the motion planning method include:
[0089] Step 1: According to the set planning parameters S c 、V c , A, D, V e , A max , D max 、V max With the current position S s 、Current speed V s Computational constraint models;
[0090] Where V c ≤V max , A≤A max , D≤D max ;
[0091] Determine the current speed V s and final velocity V e With target displacement S c The constraints between them are:
[0092] (1) The current velocity and target displacement are in the same direction:
[0093] When |V s |>V e , the first displacement When
[0094] When |V s |<V e , the first displacement When If A′>A max ,Adjustment
[0095] (2) The current speed and target displacement are opposite:
[0096] The time to decelerate from the current speed to zero is: The displacement increment is Let S a =|S c |+Δs;
[0097] When the second displacement When If A′>A max ,Adjustment
[0098] Step 2: From the current speed V s , target displacement S c and the final velocity V recalculated in step 2 e , maximum planned acceleration A, maximum planned deceleration D, and final velocity V e Calculate whether the entire motion trajectory has an acceleration process, a uniform motion process, and a deceleration process, as well as the maximum speed V c Is it reachable? If not reachable, calculate the maximum reachable speed V lim .
[0099] Step 3: Calculate the motion time T of the acceleration process, uniform motion process and deceleration process acc 、T uni 、T dec and motion displacement S acc , S uni , S dec :
[0100] The motion time of the accelerated motion process is: The motion displacement of the accelerated motion process is:
[0101] The motion time of uniform motion is: The motion displacement of the uniform motion process is:
[0102] The motion time of the deceleration motion process is: The motion displacement of the deceleration motion process is:
[0103] Step 4:
[0104] The calculated motion time of the acceleration motion process, uniform motion process and deceleration motion process is rounded off. Assuming the scanning period is T k , in milliseconds. The motion time units of the acceleration motion process, uniform motion process and deceleration motion process solved in step 3 are in seconds, and they are rounded off and shaped with respect to the scanning period:
[0105] T acc '=lround(1000*T acc / T k )*0.001*T k ;
[0106] T dec '=lround(1000*T dec / T k )*0.001*T k ;
[0107] T uni '=lround(1000*T uni / T k )*0.001*T k ;
[0108] Step 5: The corresponding T acc '、T dec '、S acc ',S dec 'Substitute the value into the preset relational expression to obtain the corresponding triangle acceleration or deceleration equation parameters;
[0109] For the processing of uniform speed segments, the speed of the uniform speed segments needs to be recalculated:
[0110]
[0111] The preset relationship expression is determined by fitting the displacement equations of the acceleration section / deceleration section and the uniform speed section using a fifth-order polynomial and a linear equation.
[0112]
[0113] Among them, formulas (2), (3), and (4) correspond to the acceleration motion process, the uniform motion process, and the deceleration motion process, respectively.
[0114] According to step 3, for the acceleration process, the positions and velocities of the starting and ending points are known, and the starting and ending points of the acceleration are both 0. For the convenience of calculation, it is assumed that the time at the starting point is 0 and the time at the end point is Substituting these six parameters into formula (2) can obtain the six time parameters of the planning position equation, which are:
[0115]
[0116] Where d is the displacement increment value of this section, from step three, we can know Will Substituting into (5)-(10) we get:
[0117]
[0118] The acceleration equation of the acceleration section can be obtained by taking the second-order derivative of equation 1:
[0119] A(t)=S″=2a 02 +6a 03 t+12a 04 t 2 +20a 05 t 3 ;……………………(12)
[0120] Substituting formula (10) into formula (11), we can obtain
[0121]
[0122] From formula (13), we can get the acceleration equation of the acceleration stage is a parabolic curve about time t. is always greater than 0 or less than 0, that is, the corresponding speed curve is monotonically increasing or decreasing. Similarly, the deceleration curve can be obtained (Formula (4) is also a motion equation curve with a monotonically increasing or decreasing speed). Therefore, it is completely feasible to use this method to express the 5-stage S-shaped speed acceleration and deceleration planning, and the acceleration curve is a smooth parabolic curve. Compared with the general acceleration triangle or T-shaped curve, it can improve the flexibility of the processing estimation.
[0123] For the uniform speed segment, since the uniform speed segment is connected to the acceleration segment, we can get:
[0124]
[0125] Substituting formula (1) into formula (14) we can get the parameters of the position curve equation for the uniform speed segment.
[0126] Step 6: According to the recalculated curve equation parameters, establish the position control equations for the acceleration motion process, uniform motion process and deceleration motion process as described in step 2, and use the controller to periodically output the position, velocity and acceleration information according to the following formula.
[0127] Acceleration and deceleration process:
[0128]
[0129] If it is an accelerated motion process, then t(k)∈[0,T′ acc ], and the deceleration stage is t(k)∈[0,T′ dcc ], t(k) represents the time corresponding to the interpolation cycle, and k represents the kth interpolation cycle;
[0130] Uniform motion process:
[0131]
[0132] On the other hand, the present application provides a motion planning device, comprising:
[0133] Memory for storing computer programs;
[0134] The processor is used to implement the steps of the motion planning method as described above when executing a computer program.
[0135] For an introduction to the motion planning device, please refer to the above embodiments, and this application will not go into details here.
[0136] On the other hand, the present application provides a non-volatile storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the motion planning method as described above are implemented.
[0137] For an introduction to the non-volatile storage medium, please refer to the above embodiments, and this application will not go into details here.
[0138] On the other hand, the present application provides a motor, comprising the motion planning device as described above.
[0139] For the introduction of the motor, please refer to the above embodiments, and this application will not go into details here.
[0140] It should also be noted that, in this specification, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprises", "comprising" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the statement "comprising a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element.
[0141] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A motion planning method, characterized in that: include: Acquire a first motion planning parameter set by a user, and determine a motion trajectory according to the first motion planning parameter and a current motion parameter, wherein the motion trajectory includes an accelerated motion process and / or a uniform motion process and / or a decelerated motion process; Determine the motion time and motion displacement corresponding to each motion process in the motion trajectory; Rounding off each of the exercise times to obtain a target time; The target time is an integer multiple of the interpolation period; Determine the control equation of each motion process according to each target time and a preset relationship expression, wherein the control equation includes a position control equation, a speed control equation, and an acceleration control equation; A motion control instruction is generated according to the control equation of each motion process according to the interpolation cycle, and the motor motion is controlled according to the motion control instruction; the motion control instruction includes a position control instruction, a speed control instruction, and an acceleration control instruction.
2. The motion planning method according to claim 1, characterized in that: After obtaining the first motion planning parameters set by the user, the method further includes: Adjusting the first motion planning parameter according to the first motion planning parameter and the current motion parameter to obtain a second motion planning parameter; Determining a motion trajectory according to the first motion planning parameter and the current motion parameter includes: The motion trajectory is determined according to the second motion planning parameters and the current motion parameters.
3. The motion planning method according to claim 2, characterized in that: The first motion planning parameter is adjusted according to the first motion planning parameter and the current motion parameter to obtain a second motion planning parameter, including: Determining whether the first motion planning parameters need to be adjusted according to the final speed and target displacement in the first motion planning parameters and the current speed and current position in the current motion parameters, wherein the target displacement is the displacement corresponding to the movement from the current position to the target position; If necessary, adjust the maximum planned acceleration, and / or the maximum planned deceleration and / or the final speed in the first motion planning parameters.
4. The motion planning method according to claim 3, characterized in that: Determining whether the first motion planning parameters need to be adjusted according to the final speed and the target displacement in the first motion planning parameters and the current speed and the current position in the current motion parameters includes: Determining whether the direction of the current speed is the same as the direction of the target displacement; If they are the same, then calculating the first displacement required when the current speed reaches the final speed according to the maximum planned acceleration or the maximum planned deceleration in the first motion planning parameter, and determining whether it is necessary to adjust the maximum planned acceleration and / or the maximum planned deceleration and / or the final speed according to the first displacement and the target displacement; If they are not the same, the displacement increment of the current speed decelerating to zero according to the maximum planned acceleration is calculated, and whether the maximum planned acceleration and / or the final speed needs to be adjusted is determined according to the displacement increment and the target displacement.
5. The motion planning method according to claim 4, characterized in that: Determining whether the maximum planned acceleration and / or the maximum planned deceleration and / or the final velocity needs to be adjusted according to the first displacement and the target displacement includes: When the first displacement is greater than the target displacement and the current speed is greater than the final speed, adjusting the maximum planned deceleration according to the first displacement and the target displacement; When the first displacement is greater than the target displacement and the current speed is less than the final speed, the maximum planned acceleration is adjusted according to the first displacement and the target displacement; when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, the maximum planned acceleration is kept unchanged, and the final speed is adjusted according to the target displacement, the current speed and the maximum planned acceleration.
6. The motion planning method according to claim 4, characterized in that: Determining whether the maximum planned acceleration and / or the final velocity needs to be adjusted according to the displacement increment and the target displacement includes: Calculate the second displacement required to accelerate from zero to the final velocity at the maximum planned acceleration; When the second displacement is greater than the sum of the absolute value of the target displacement and the displacement increment, the maximum planned acceleration is adjusted according to the sum and the final velocity, and when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value, the maximum planned acceleration is kept unchanged, and the final velocity is adjusted according to the sum and the maximum planned acceleration.
7. The motion planning method according to any one of claims 1 to 6, characterized in that: The preset relationship expression is determined as follows: Designing a preset acceleration and deceleration model, the preset acceleration and deceleration model includes a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing acceleration, and a fifth stage of decreasing acceleration; Using a fifth-order polynomial to fit the first displacement equation of the acceleration motion process and the second displacement equation of the deceleration motion process of the preset acceleration and deceleration model, the acceleration motion process includes the process corresponding to the first stage and the second stage, and the deceleration motion process includes the process corresponding to the fourth stage and the fifth stage; Using a linear equation to fit a third displacement equation of the uniform motion process of the preset acceleration and deceleration model, the uniform motion process includes a process corresponding to the third stage, and each displacement equation represents a corresponding relationship between displacement and time in the current motion process; Determine the time coefficient in the first displacement equation, the second displacement equation and the third displacement equation according to the movement time and the movement displacement corresponding to each movement process and the movement planning parameters corresponding to the movement trajectory; The preset relationship expression is a correspondence between each of the time coefficients and the motion time and motion displacement corresponding to the motion process, and the motion planning parameters corresponding to the motion trajectory.
8. A motion planning device, characterized in that: include: Memory for storing computer programs; A processor, used to implement the steps of the motion planning method as described in any one of claims 1 to 7 when executing a computer program.
9. A non-volatile storage medium, characterized in that: The non-volatile storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the motion planning method according to any one of claims 1 to 7 are implemented.
10. A motor, characterized in that: Comprising a motion planning device as claimed in claim 8.
Citation Information
Patent Citations
Acceleration and deceleration planning method and device for numerical control system and numerical control machine tool
CN103970073A
Speed control method and device of numerical control model, computer device and storage medium
CN113495531A
S-shaped curve speed planning method, control terminal and computer readable storage medium
CN114077226A
