A motion planning method, apparatus, medium, and motor

Abstract

The application discloses a motion planning method and device, a medium and a motor, and relates to the field of industrial automation.The scheme acquires motion planning parameters set by a user, combines current motion parameters, determines exact motion time corresponding to each motion process of a motion track, ensures that each motion time is an integer multiple of an interpolation period, and thus eliminates rounding errors.In addition, a preset relationship expression is used to determine control equations corresponding to each motion process, including displacement, speed and acceleration control equations, which further generates continuous and accurate motion control instructions within the interpolation period.According to the control equations, accurate position, speed and acceleration parameters within each period can be determined, without setting jerk values, and the control system can be ensured to smoothly and accurately move along the expected path, improving the operation stability and processing precision of the system.

Description

Technical Field

[0001] This invention relates to the field of industrial automation, and in particular to a motion planning method, device, medium, and motor. Background Technology

[0002] In CNC machining and other common motion control systems, the smoothness, accuracy, and efficiency of the machining path or trajectory are particularly important considerations. Therefore, the speed planning of the machining path is crucial for the axes. Since control systems typically use periodic output interpolation data for axis control—that is, based on predefined machining path or motion trajectory data—continuous motion control commands are calculated and generated within each interpolation cycle. These commands include parameters such as the axis's target position, velocity, and acceleration. Through interpolation algorithms, precise control of the axis position is achieved within the interpolation cycle, ensuring the system moves smoothly and accurately along the expected path.

[0003] In actual path curve calculations, the planned time for each segment may not be an integer multiple of the interpolation period. The decimal part obtained by dividing this calculated time value by the interpolation period is called rounding error. This rounding error can severely affect the stability and accuracy of the entire system's actual operation. Current technologies typically optimize rounding error by adjusting the jerk value, but the logic for setting the jerk value is complex. Summary of the Invention

[0004] The purpose of this invention is to provide a motion planning method, device, medium, and motor that uses a 5-segment S-curve acceleration / deceleration planning method. This eliminates rounding errors, does not involve setting acceleration values, and ensures that the speed during deceleration and deceleration phases increases or decreases monotonically without reversal. It also ensures that the control system performs smooth and precise motion along the expected path, improving the system's operational stability and machining accuracy. Compared to the traditional 7-segment S-curve control method, this invention significantly reduces the computational load.

[0005] On the one hand, this application provides a motion planning method, including:

[0006] Obtain the first motion planning parameters set by the user, and determine the motion trajectory based on the first motion planning parameters and the current motion parameters. 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 displacement corresponding to each motion process in the motion trajectory;

[0008] Each of the aforementioned motion times is rounded to obtain the target time; the target time is an integer multiple of the interpolation period.

[0009] The control equations for each motion process are determined based on each target time and a preset relational expression. The control equations include position control equations, velocity control equations, and / or acceleration control equations.

[0010] Motion control commands are generated according to the control equations of each motion process according to the interpolation cycle, and the motor motion is controlled according to the motion control commands; the motion control commands include position control commands, speed control commands, and / or acceleration control commands.

[0011] In one embodiment, after obtaining the first motion planning parameters set by the user, the method further includes:

[0012] The first motion planning parameters are adjusted based on the first motion planning parameters and the current motion parameters to obtain the second motion planning parameters;

[0013] Determining the motion trajectory based on the first motion planning parameters and the current motion parameters includes:

[0014] The motion trajectory is determined based on the second motion planning parameters and the current motion parameters.

[0015] In one embodiment, adjusting the first motion planning parameters based on the first motion planning parameters and the current motion parameters to obtain second motion planning parameters includes:

[0016] 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, it is determined whether the first motion planning parameters need to be adjusted, where the target displacement is the displacement 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 velocity in the first motion planning parameters.

[0018] In one embodiment, determining whether the first motion planning parameters need adjustment 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, includes:

[0019] Determine whether the direction of the current velocity is the same as the direction of the target displacement;

[0020] If they are the same, calculate the first displacement required for the current speed to reach the final speed according to the maximum planned acceleration or the maximum planned deceleration in the first motion planning parameters, and determine whether the maximum planned acceleration and / or the maximum planned deceleration and / or the final speed need to be adjusted based on the first displacement and the target displacement;

[0021] If they are not the same, calculate the displacement increment by which the current speed decelerates to zero according to the maximum planned deceleration, and determine whether the maximum planned acceleration and / or the final speed need to be adjusted based on the displacement increment and the target displacement.

[0022] In one embodiment, determining whether to adjust the maximum planned acceleration and / or the maximum planned deceleration and / or the final velocity based on the first displacement and the target displacement includes:

[0023] When the first displacement is greater than the target displacement and the current velocity is greater than the final velocity, the maximum planned deceleration is adjusted according to the first displacement and the target displacement;

[0024] When the first displacement is greater than the target displacement and the current velocity is less than the final velocity, 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, the maximum planned acceleration is kept unchanged, and the final velocity is adjusted according to the target displacement, the current velocity, and the maximum planned acceleration.

[0025] In one embodiment, determining whether the maximum planned acceleration and / or the final velocity need to be adjusted based on 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. When the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit, 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 relational expression is determined as follows:

[0029] Design a preset acceleration / deceleration model, which includes a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing deceleration, and a fifth stage of decreasing deceleration.

[0030] The first displacement equation of the acceleration process and the second displacement equation of the deceleration process of the preset acceleration and deceleration model are fitted using a fifth-order polynomial. The acceleration process includes the process corresponding to the first and second stages, and the deceleration process includes the process corresponding to the fourth and fifth stages.

[0031] The third displacement equation of the uniform motion process of the preset acceleration and deceleration model is fitted with a linear equation. The uniform motion process includes the process corresponding to the third stage. Each displacement equation represents the corresponding relationship between displacement and time in the current motion process.

[0032] The time coefficients in the first displacement equation, the second displacement equation, and the third displacement equation are determined based on the motion time and displacement corresponding to each motion process, and the motion planning parameters corresponding to the motion trajectory.

[0033] The preset relational expression is the correspondence between each of the time coefficients and the motion time and displacement corresponding to the motion process, as well as the motion planning parameters corresponding to the motion trajectory.

[0034] On the other hand, this application provides a motion planning device, comprising:

[0035] Memory, used to store computer programs;

[0036] A 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, this application provides a non-volatile storage medium on which a computer program is stored, which, when executed by a processor, implements the steps of the motion planning method as described above.

[0038] On the other hand, this application provides an electric motor including the motion planning device as described above.

[0039] This invention provides a motion planning method, device, medium, and motor. This solution obtains user-set motion planning parameters and, combined with current motion parameters, determines the precise motion time corresponding to each motion process of the motion trajectory, ensuring that each motion time is an integer multiple of the interpolation cycle, thereby eliminating rounding errors. Furthermore, it uses preset relational expressions to determine the control equations corresponding to each motion process, including control equations for displacement, velocity, and acceleration. This generates continuous and accurate motion control commands within the interpolation cycle. Based on the control equations, precise position, velocity, and acceleration parameters within each cycle can be determined without involving the setting of jerk values. This ensures that the control system performs smooth and precise motion along the expected path, improving the system's operational stability and machining accuracy. Attached Figure Description

[0040] To more clearly illustrate the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0041] Figure 1 A flowchart 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 Implementation

[0043] The core of this invention is to provide a motion planning method, device, medium, and motor. It uses a 5-segment S-curve acceleration and deceleration planning method, which eliminates rounding errors, does not involve setting acceleration values, and ensures that the speed increases or decreases monotonically during deceleration and deceleration phases without reversal. It can also ensure that the control system performs smooth and precise motion according to the expected path, thereby improving the system's operational stability and machining accuracy. Compared with the traditional 7-segment S-curve control method, it greatly reduces the amount of calculation.

[0044] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0045] On the one hand, such as Figure 1 As shown, this application provides a motion planning method, including:

[0046] S11: Obtain the first motion planning parameters set by the user, and determine the motion trajectory based on the first motion planning parameters and the current motion parameters. The motion trajectory includes the acceleration motion process and / or the uniform motion process and / or the deceleration motion process.

[0047] The principle behind this step is based on the user-defined motion planning parameters and the current motion state. By determining the motion trajectory, the entire motion process is decomposed into three stages: acceleration, constant speed, and deceleration. This step analyzes and combines the user-inputted motion requirements with the current trajectory information to ensure that the system can generate specific motion control schemes for 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, the method further includes: adjusting the first motion planning parameters according to the first motion planning parameters and the current motion parameters to obtain the 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 set motion planning parameters set by the user are first obtained. These parameters may include the set target displacement S. c Maximum planning speed V c A. Maximum planned acceleration B. Maximum planned deceleration C. Planned final velocity V e Maximum acceleration limit value A of the shaft max Maximum deceleration limit value D of the shaft max Maximum speed limit V of the shaft max Then, based on the current motion state and environmental conditions, these first motion planning parameters are adjusted to generate second motion planning parameters to optimize the motion path.

[0050] Next, based on the adjusted first motion planning parameters and the current motion parameters, the specific motion trajectory is determined. The determination of the motion trajectory takes into account acceleration, constant speed, and deceleration phases, 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 not only maintains high-precision motion control in changing working environments but also optimizes the system's response speed and motion efficiency, improving overall operational stability and machining accuracy.

[0051] In one embodiment, adjusting the first motion planning parameters according to the first motion planning parameters and the current motion parameters to obtain the second motion planning parameters includes: determining whether the first motion planning parameters need to be adjusted 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, 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 planning acceleration and / or the maximum planning deceleration and / or the final velocity in the first motion planning parameters.

[0052] In this embodiment, second motion planning parameters are dynamically adjusted based on the user-defined first motion planning parameters and the current motion state. This adjustment process ensures more precise and efficient path control during motion.

[0053] Specifically, the final velocity and target displacement in the first motion planning parameters, and the current velocity and current position in the current motion parameters are considered. The target displacement refers to the displacement from the current position to the target position. Next, the degree to which the current velocity and position conform to the expected path is evaluated. If a deviation from the expected path is detected in the current velocity or position, or if further optimization is needed, the first motion planning parameters will be adjusted.

[0054] Possible adjustments include: maximum planned acceleration, which is adjusting the maximum permissible acceleration during the acceleration phase to reach the target speed faster or more smoothly; maximum planned deceleration, which is adjusting the maximum permissible deceleration during the deceleration phase to stop or transition to a new state of motion more smoothly; and final velocity, which is adjusting the maximum speed that can be achieved during the motion.

[0055] This dynamic adjustment process can effectively address situations where the settings are inaccurate, thereby optimizing the performance and stability of the motion control system.

[0056] In one embodiment, determining whether the first motion planning parameters need to be adjusted 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, includes: 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 for the current velocity to reach 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 needs to be adjusted based on the first displacement and the target displacement; if they are not the same, calculating the displacement increment required for the current velocity to decelerate to zero according to the maximum planned deceleration, and determining whether the maximum planned acceleration and / or final velocity needs to be adjusted based on the displacement increment and the target displacement.

[0057] In this embodiment, the motion planning parameters are dynamically adjusted based on the user-defined first motion planning parameters and the current motion state to generate optimized motion planning parameters. This process involves a detailed analysis of the current speed and position relative to 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 if the direction of the current velocity is the same as the direction of the target displacement. If they are the same, calculate the first displacement required to reach the final velocity based on 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 parameters. This displacement is compared with the target displacement to determine if the maximum planned acceleration, maximum planned deceleration, or final velocity needs adjustment. The purpose of this adjustment is to ensure smooth and accurate motion control with appropriate velocity and acceleration when reaching the target position.

[0059] If the direction of the current velocity is different from the direction of the target displacement, the displacement increment required to decelerate to zero using the maximum planned deceleration will be calculated (i.e., deceleration to zero is required before reversing acceleration). The sum of this displacement increment and the target displacement is the displacement required for reversing acceleration. The maximum planned acceleration and the sum are used to determine whether the maximum planned acceleration or the final velocity needs to be adjusted. This adjustment aims to ensure precise control with appropriate speed and deceleration when reversing or stopping is required, avoiding overshoot or unnecessary motion errors.

[0060] This embodiment not only optimizes the execution of motion trajectories but also minimizes rounding errors, thereby improving overall processing quality and system efficiency.

[0061] In one embodiment, determining whether to adjust the maximum planned acceleration and / or the maximum planned deceleration and / or the final velocity based on the first displacement and the target displacement includes: 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 velocity is greater than the final velocity; adjusting the maximum planned acceleration based on the first displacement and the target displacement when the first displacement is greater than the target displacement and the current velocity is less than the final velocity; and keeping the maximum planned acceleration unchanged and adjusting the final velocity based on the target displacement, the current velocity, and the maximum planned acceleration when the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit value.

[0062] In this embodiment, motion planning parameters are dynamically adjusted based on a comparison between the first displacement and the target displacement to optimize the performance of motion control. First, the system compares the magnitudes of the first displacement and the target displacement.

[0063] If the first displacement is greater than the target displacement and the current velocity is greater than the final velocity, it indicates that the actual distance (first displacement) required to decelerate from the current velocity to the final velocity with the maximum planned deceleration exceeds the expected path length (target displacement). The maximum planned deceleration will be adjusted based on the first displacement and the target displacement.

[0064] If the first displacement is greater than the target displacement and the current velocity is less than the final velocity, it means that the actual distance (first displacement) required to accelerate from the current velocity to the final velocity with the maximum planned acceleration exceeds the expected path length (target displacement). In this case, the maximum planned acceleration will be adjusted based on the first displacement and the target displacement.

[0065] In addition, a limit judgment on the maximum planned acceleration will be performed. 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 precise control according to the user-defined requirements under various conditions, avoiding instability or unexpected situations that may occur during motion.

[0066] Through this dynamic adjustment mechanism based on real-time motion state and expected path, this embodiment can effectively optimize the motion control process, improve the system's operational stability, accuracy and efficiency, thereby better meeting the needs of complex processing or motion control.

[0067] In one embodiment, determining whether to adjust the maximum planned acceleration and / or final velocity based on the displacement increment and the target displacement includes: 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, keeping the maximum planned acceleration unchanged, and adjusting the final velocity based on the sum and the maximum planned acceleration.

[0068] In this embodiment, motion planning parameters are dynamically adjusted based on a comparison of the displacement increment and the target displacement to optimize the performance of motion control. First, the second displacement required to accelerate from zero velocity to the final velocity with the maximum planned acceleration (i.e., the displacement corresponding to the reverse acceleration process) is calculated. This calculation ensures that the expected final velocity is reached during the acceleration process, while also taking into account the actual length requirements of the motion path.

[0069] Then, evaluate the absolute value of the second displacement and 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, a limit judgment on the maximum planned acceleration will be performed. 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 and the maximum planned acceleration. This adjustment strategy ensures that stability and accuracy are maintained throughout the motion process, while achieving efficient motion control under different working conditions.

[0071] Through this dynamic adjustment mechanism based on real-time motion state and expected path length, this embodiment can effectively optimize the motion control process, improve the system's operational stability, accuracy, and efficiency, and is suitable for various complex motion control application scenarios.

[0072] S12: Determine the motion time and displacement corresponding to each motion process in the motion trajectory.

[0073] This step involves analyzing the user-defined initial motion planning parameters and current motion parameters, combined with the determined motion trajectory, to calculate the time required for each motion process and the corresponding displacement.

[0074] Specifically, the system first calculates the expected trajectory of the entire motion path based on the user-defined motion planning parameters, including the expected target position, speed requirements, and possible acceleration limits. Then, based on the current motion state, such as the current position and speed, the specific time and displacement for each stage of the actual motion process—acceleration, constant speed, and deceleration—are derived. These calculations ensure that the system can perform smooth and precise motion with appropriate speed and acceleration in each stage to achieve the desired processing effect.

[0075] S13: Round off each motion time to obtain the target time; the target time is an integer multiple of the interpolation period.

[0076] The principle behind this step is to round off the calculated motion time 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 commands can be generated and executed within fixed time intervals, thereby guaranteeing that the system can perform smooth and precise motion control according to the expected processing path.

[0077] S14: Determine the control equations for each motion process based on each target time and a preset relational expression. The control equations include position control equations, velocity control equations, and / or acceleration control equations.

[0078] The principle behind this step is to calculate the corresponding control equations for each motion process based on preset relational expressions, including position control equations, velocity control equations, and / or acceleration control equations. These control equations are formulated based on the target time and expected motion characteristics during the motion process, such as the changing patterns of acceleration, constant speed, and deceleration phases. Through these equations, the position, velocity, and acceleration of the axis at each moment can be accurately calculated to generate motion control commands that ensure smooth and precise motion, thereby achieving precise control of the motor's motion.

[0079] S15: Generate motion control commands according to the interpolation cycle based on the control equations of each motion process, and control the motor motion according to the motion control commands; the motion control commands include position control commands, speed control commands, and / or acceleration control commands.

[0080] The principle behind this step is to generate motion control commands according to the control equations determined in S14 and a preset interpolation cycle. These commands include position control commands, speed control commands, and / or acceleration control commands, used to precisely control the motor's motion. The commands generated within each interpolation cycle ensure that the shaft can move smoothly and accurately along the expected trajectory. This process not only considers the position, speed, and acceleration requirements in the control equations but also ensures the continuity and real-time performance of motion commands in a control system that periodically outputs interpolated data, thereby improving the system's operational stability and machining accuracy.

[0081] In summary, the motion planning method provided in this application obtains the user-set motion planning parameters 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 cycle, thereby eliminating rounding errors. In addition, the control equations corresponding to each motion process are determined using preset relational expressions, including control equations for displacement, velocity, and acceleration. This generates continuous and accurate motion control commands within the interpolation cycle. Based on the control equations, the precise position, velocity, and acceleration parameters within each cycle can be determined without involving the setting of jerk values. Furthermore, it ensures that the control system performs smooth and accurate motion along the expected path, improving the system's operational stability and machining accuracy.

[0082] Based on the above embodiments:

[0083] In one embodiment, the preset relational expression is determined as follows: A preset acceleration / deceleration model is designed, comprising a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing deceleration, and a fifth stage of decreasing deceleration; a fifth-order polynomial is used to fit the first displacement equation of the acceleration process and the second displacement equation of the deceleration process of the preset acceleration / deceleration model, wherein the acceleration process includes the processes corresponding to the first and second stages, and the deceleration process includes the processes corresponding to the fourth and fifth stages; a linear equation is used to fit the third displacement equation of the uniform motion process of the preset acceleration / deceleration model, wherein the uniform motion process includes the process corresponding to the third stage, and each displacement equation represents the correspondence between displacement and time during the current motion process; the time coefficients in the first, second, and third displacement equations are determined based on the motion time and displacement corresponding to each motion process, and the motion planning parameters corresponding to the motion trajectory; the preset relational expression represents the correspondence between each time coefficient and the motion time and 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 relational expression involves designing and fitting a preset acceleration / deceleration model, and determining the time coefficient in the displacement equation based on the fitting results. First, in this embodiment, the preset acceleration / deceleration model is a 5-segment acceleration / deceleration model (e.g., Figure 2 As shown in the diagram, specifically, the preset acceleration / 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, constant speed, and deceleration processes in different parts of the machining path.

[0085] Secondly, for each preset acceleration and deceleration model, a fifth-order polynomial is used for fitting during the acceleration and deceleration processes to obtain the corresponding displacement equations. These displacement equations describe the relationship between displacement and time during the motion process. Through fitting, the displacement change pattern at each stage can be accurately calculated.

[0086] Furthermore, for the uniform motion process, linear equations are used for fitting to obtain the displacement equations for the uniform motion phase. These equations are formulated based on the current motion parameters and preset motion planning parameters to ensure that the axis can move at a constant speed during the uniform motion phase.

[0087] Finally, based on the motion time, displacement, and preset motion planning parameters for each motion process, the time coefficients in the displacement equation are determined. These time coefficients reflect the relationship between time changes and displacement changes during motion and are the core part of the preset relational expression. The preset relational expression, through the correspondence between these time coefficients and the motion planning parameters, ensures the accurate generation of motion control commands, thereby enabling the system to perform smooth and precise motion along the expected path. This embodiment uses a fifth-order polynomial to fit the displacement equations of the acceleration and deceleration segments of a five-segment S-curve, verifying that the velocity of the curve fitted by this method is monotonically increasing or decreasing in the acceleration and deceleration segments without reversal.

[0088] In one specific embodiment, the motion planning method includes the following steps:

[0089] Step 1: Based on the set planning parameters S c V c A, D, V e A max D max V max With current position S s Current speed V s Calculate the constraint model;

[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:

[0092] (1) Current velocity and target displacement are in the same direction:

[0093] When |V s |>V e First displacement At that time, readjust

[0094] When |V s |<V e First displacement At that time, readjust If A′>A max ,Adjustment

[0095] (2) Current velocity and target displacement are in opposite directions:

[0096] The time required for the current speed to decelerate to zero is: Displacement increment is Let S a =|S c |+Δs;

[0097] When the second displacement At that time, readjust If A′>A max ,Adjustment

[0098] Step 2: From the current velocity V s Target displacement S c and the final velocity V recalculated in step two e Maximum planned acceleration A, maximum planned deceleration D, and final velocity V e Calculate whether the entire motion trajectory includes acceleration, uniform motion, and deceleration processes, and determine the maximum speed V. c Is it reachable? If not, calculate the maximum reachable speed V. lim .

[0099] Step 3: Calculate the motion time T for the acceleration process, the uniform motion process, and the deceleration process. acc T uni T dec and displacement S acc S uni S dec :

[0100] The motion time during acceleration is: The displacement during the acceleration process is:

[0101] The motion time during uniform motion is: The displacement during uniform motion is:

[0102] The motion time during the deceleration process is: The displacement during the deceleration process is:

[0103] Step Four:

[0104] The motion times for the calculated acceleration, uniform motion, and deceleration processes are rounded off, assuming a scan period of T. k The unit is milliseconds. The motion time units for the acceleration, uniform motion, and deceleration processes obtained in step three are seconds, and these are rounded to the nearest integer with respect to the scan 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: Set the corresponding T acc '、T dec '、S acc ',S dec Substituting the values ​​into the preset relational expression yields the corresponding parameters of the triangular acceleration or deceleration equation;

[0109] For the processing of the uniform velocity segment, the velocity of the uniform velocity segment needs to be recalculated:

[0110]

[0111] The preset relational expression is determined by fitting the displacement equations of the acceleration / deceleration segment and the uniform speed segment using a 5th-order polynomial and a linear equation.

[0112]

[0113] Formulas (2), (3), and (4) correspond to the acceleration process, the uniform motion process, and the deceleration process, respectively.

[0114] According to step three, for the accelerated motion process, the starting and ending positions and velocities are known, and the acceleration is zero at both the starting and ending points. For ease of calculation, we assume the starting time is 0 and the ending time is... Substituting these six parameters into the equation (2) yields the six time parameters of the planning location equation, which are:

[0115]

[0116] Where d is the displacement increment value of this segment, as can be seen from step three. Will Substituting (5)-(10) into the equations, we get:

[0117]

[0118] Taking the second derivative of Equation 1 yields the acceleration equation for the acceleration phase:

[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) yields

[0121]

[0122] From formula (13), the acceleration equation for the acceleration segment is a parabolic curve with respect to time t. It is always greater than 0 or less than 0, that is, the corresponding velocity curve is monotonically increasing or decreasing. Similarly, the deceleration segment curve (formula (4) is also a motion equation curve with a monotonically increasing or decreasing velocity) can be obtained. Therefore, it is entirely feasible to use this method to express the 5-segment S-shaped velocity acceleration and deceleration planning, and the acceleration curve is a smooth parabolic curve. Compared with the general acceleration being a triangular or T-shaped curve, it can improve the smoothness of the processing estimation.

[0123] For the constant velocity segment, since it connects to the acceleration segment, we can conclude that:

[0124]

[0125] Substituting formula (1) into formula (14) will yield the position curve equation parameters for the uniform velocity segment.

[0126] Step Six: Based on the recalculated curve equation parameters, and as described in Step Two, establish the position control equations for the acceleration, uniform motion, and deceleration processes, and periodically output position, velocity, and acceleration information through the controller according to the following formula.

[0127] Acceleration and deceleration processes:

[0128]

[0129] If it is an accelerated motion process, then t(k)∈[0,T′] acc For the deceleration segment, t(k)∈[0, T′]. dcc ], t(k) represents the time corresponding to the interpolation period, and k represents the kth interpolation period;

[0130] Uniform motion process:

[0131]

[0132] On the other hand, this application provides a motion planning device, comprising:

[0133] Memory, used to store computer programs;

[0134] A processor is used to implement the steps of the motion planning method described above when executing a computer program.

[0135] For a description of the motion planning device, please refer to the above embodiments; this application will not repeat it here.

[0136] On the other hand, this application provides a non-volatile storage medium on which a computer program is stored, and when the computer program is executed by a processor, it implements the steps of the motion planning method described above.

[0137] For a description of non-volatile storage media, please refer to the above embodiments; this application will not repeat it here.

[0138] On the other hand, this application provides an electric motor, including the motion planning device as described above.

[0139] For a description of the motor, please refer to the above embodiments; this application will not repeat it here.

[0140] It should also be noted that, in this specification, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0141] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily 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 invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A motion planning method, characterized in that, include: Obtain the first motion planning parameters set by the user, and determine the motion trajectory based on the first motion planning parameters and the current motion parameters. 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 displacement corresponding to each motion process in the motion trajectory; Round off each of the motion times to obtain the target time; The target time is an integer multiple of the interpolation period; The control equations for each motion process are determined based on each target time and a preset relational expression. The control equations include position control equations, velocity control equations, and acceleration control equations. Motion control commands are generated according to the control equations of each motion process and the interpolation cycle, and the motor is controlled to move according to the motion control commands; the motion control commands include position control commands, speed control commands, and acceleration control commands; The method for determining the preset relational expression is as follows: Design a preset acceleration / deceleration model, which includes a first stage of increasing acceleration, a second stage of decreasing acceleration, a third stage of zero acceleration, a fourth stage of increasing deceleration, and a fifth stage of decreasing deceleration. The first displacement equation of the acceleration process and the second displacement equation of the deceleration process of the preset acceleration and deceleration model are fitted using a fifth-order polynomial. The acceleration process includes the process corresponding to the first and second stages, and the deceleration process includes the process corresponding to the fourth and fifth stages. The third displacement equation of the uniform motion process of the preset acceleration and deceleration model is fitted with a linear equation. The uniform motion process includes the process corresponding to the third stage. Each displacement equation represents the corresponding relationship between displacement and time in the current motion process. The time coefficients in the first displacement equation, the second displacement equation, and the third displacement equation are determined based on the motion time and displacement corresponding to each motion process, and the motion planning parameters corresponding to the motion trajectory. The preset relational expression is the correspondence between each of the time coefficients and the motion time and displacement corresponding to the motion process, as well as the motion planning parameters corresponding to the motion trajectory.

2. The motion planning method as described in claim 1, characterized in that, After obtaining the user-defined initial motion planning parameters, the process also includes: The first motion planning parameters are adjusted based on the first motion planning parameters and the current motion parameters to obtain the second motion planning parameters; Determining the motion trajectory based on the first motion planning parameters and the current motion parameters includes: The motion trajectory is determined based on the second motion planning parameters and the current motion parameters.

3. The motion planning method as described in claim 2, characterized in that, The first motion planning parameters are adjusted based on the first motion planning parameters and the current motion parameters to obtain the second motion planning parameters, including: 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, it is determined whether the first motion planning parameters need to be adjusted, where the target displacement is the displacement 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 velocity in the first motion planning parameters.

4. The motion planning method as described in claim 3, characterized in that, Determining whether the first motion planning parameters need adjustment 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, includes: Determine whether the direction of the current velocity is the same as the direction of the target displacement; If they are the same, calculate the first displacement required for the current speed to reach the final speed according to the maximum planned acceleration or the maximum planned deceleration in the first motion planning parameters, and determine whether the maximum planned acceleration and / or the maximum planned deceleration and / or the final speed need to be adjusted based on the first displacement and the target displacement; If they are not the same, calculate the displacement increment by which the current speed decelerates to zero according to the maximum planned deceleration, and determine whether the maximum planned acceleration and / or the final speed need to be adjusted based on the displacement increment and the target displacement.

5. The motion planning method as described in claim 4, characterized in that, Determining whether to adjust the maximum planned acceleration and / or the maximum planned deceleration and / or the final velocity based on the first displacement and the target displacement includes: When the first displacement is greater than the target displacement and the current velocity is greater than the final velocity, the maximum planned deceleration is adjusted according to the first displacement and the target displacement; When the first displacement is greater than the target displacement and the current velocity is less than the final velocity, 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, the maximum planned acceleration is kept unchanged, and the final velocity is adjusted according to the target displacement, the current velocity, and the maximum planned acceleration.

6. The motion planning method as described in claim 4, characterized in that, Determining whether to adjust the maximum planned acceleration and / or the final velocity based on 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. When the adjusted maximum planned acceleration is greater than the maximum planned acceleration limit, the maximum planned acceleration is kept unchanged, and the final velocity is adjusted according to the sum and the maximum planned acceleration.

7. A motion planning device, characterized in that, include: Memory, used to store computer programs; A processor, configured to, when executing a computer program, implement the steps of the motion planning method as described in any one of claims 1-6.

8. A non-volatile storage medium, characterized in that, The non-volatile storage medium stores a computer program, which, when executed by a processor, implements the steps of the motion planning method as described in any one of claims 1-6.

9. An electric motor, characterized in that, Includes the motion planning device as described in claim 7.

Images