A real-time position planning and control method with continuous acceleration

Abstract

A real-time position planning and control method with continuous acceleration, comprising the steps of: obtaining a motor task, where the motor task includes the total driving distance of the motor, and dividing a number of control planning nodes according to the control period of the motor system; judging the motor state at each of the control planning nodes, and determining an acceleration control scheme for guiding the motor control acceleration based on the motor state; wherein, the acceleration control scheme is determined with the planning goal that the motor speed, motor acceleration, and driving distance difference are all zero when the motor task is completed; the motor state includes: the first state, the rising section and the positive horizontal section with positive acceleration; the second state, the falling section with positive acceleration; the third state, the stable section with zero acceleration; the fourth state, the falling section and the negative horizontal section with negative acceleration; the fifth state, the rising section with negative acceleration. The present invention simplifies the calculation process and achieves the technical effect of being able to adjust the target position in real time under the condition of continuous acceleration.

Description

Technical Field

[0001] The present invention belongs to the technical field of motion control, and particularly relates to a real-time position planning control method with continuous acceleration. Background Technique

[0002] In recent years, the servo drive system industry has put forward requirements for high speed, high precision, and high efficiency in the motion control performance of servo motors. Under this requirement, the servo motor system needs to complete the rotation from the current position to the target position at a smoothly changing rotational speed as much as possible under the motor command.

[0003] For this technical requirement, the algorithms generally used in the market are as follows: First, the step position is processed through commands to obtain a ramp position signal or a simple S-shaped position curve, and then the processed position signal is used as the given value of the servo motor position loop. However, the obtained ramp position signal after processing has the problem of being unable to ensure the continuity of the rotational speed change of the servo motor; similarly, the simple S-shaped position curve can only ensure the continuity of the rotational speed change of the servo motor, but cannot ensure the continuity of the acceleration change of the servo motor, resulting in problems such as discontinuous output torque and large mechanical structure impact of the servo system, and further leading to a short mechanical service life.

[0004] On this basis, the invention patent with the application number CN201810609530.4 discloses a speed smoothing method for a servo driver, which calculates and analyzes the step amount of speed in each cycle of the system through the proportional relationship between the position loop and the speed loop of the servo system, so as to achieve the effect of relatively smooth speed. However, this method still cannot completely ensure the continuity of the acceleration change of the servo system and cannot completely eliminate the overshoot of the speed. Similarly, the patent with the application number CN202210345009 discloses a permanent magnet synchronous motor position control method, which proposes to use the method of feedforward compensation for speed and acceleration to make the speed as smooth and continuous as possible, and neither of the above completely achieves the effect of continuous acceleration.

[0005] The literature "Simulation and Design of a Three-Closed-Loop AC Servo Motion Control System Based on MATLAB" (Chang Yanfang, Ma Xudong, Fang Fang. Simulation and Design of a Three-Closed-Loop AC Servo Motion Control System Based on MATLAB [J]. Industrial Control Computer, 2013, 26(12): 61-63+95.) and a non-symmetric S-shaped speed curve of a high-speed sorting robot disclosed in the patent with the patent application number CN202210722253 both propose a method of using S-curve acceleration and deceleration for position planning, that is, to achieve S-shaped position curve planning through the acceleration and deceleration of an S-shaped speed curve based on continuous acceleration change. Due to the continuity of the acceleration curve design of this position planning algorithm, it can effectively reduce the mechanical vibration of the mechanical device connected to the rear stage of the servo motor during the acceleration and deceleration process, ensure that the rotational speed of the servo motor always increases and decreases smoothly during the process of rotating from the current position to the target position, and thus improve the smoothness of the operation of the position servo system.

[0006] However, this position planning algorithm has the following problems in the actual implementation of the single-chip microcomputer:

[0007] First, the existing S-shaped position curve planning based on continuous acceleration is a global one-time planning algorithm, which can only respond to a new position command after completing one position command following. Therefore, it is impossible to adjust the target position in real time during the following process, and this kind of motion planning control cannot meet the servo control requirements of rapid command changes.

[0008] Second, the calculation is relatively cumbersome and the operation is complex. The existing method needs to pre-plan and calculate the acceleration, speed, and position change amount in each control cycle during the entire position planning process when receiving a new command. If there are requirements such as control time for the product driven by the position servo system, complex operations such as solving cubic equations need to be carried out within one cycle, which will greatly reduce the execution efficiency of the MCU and occupy the computing resources of the MCU. Summary of the Invention

[0009] In view of the problems existing in the prior art, the present invention proposes a real-time position planning control method with continuous acceleration, which not only solves the problem of complex operation in the motion planning control of the S-shaped position planning curve algorithm based on continuous acceleration, but also solves the problems such as inability to adapt to rapid changes in position commands. By processing the motion formula, the use of time variables in the position planning control process is avoided, so that complex operations such as solving cubic equations can be avoided, and the calculation process is simplified. At the same time, state judgment and trajectory planning are carried out in each control cycle of the MCU, with high real-time performance, which can adapt to and follow the real-time changing position commands of the servo motor and eliminate the influence of cumulative errors.

[0010] The present invention is achieved through the following technical solutions:

[0011] A real-time position planning and control method with continuous acceleration, comprising the steps:

[0012] S1. Obtain the motor task, where the motor task includes the total driving distance of the motor, and divide a number of control planning nodes according to the control period of the motor system;

[0013] S2. Judge the motor state at each of the control planning nodes, and determine an acceleration control scheme for guiding the motor control acceleration based on the motor state;

[0014] Among them, the acceleration control scheme is determined with the planning goal that the motor speed, motor acceleration, and driving distance difference are all zero when the motor task is completed; the motor state includes: the first state, the rising section and the positive horizontal section with positive acceleration; the second state, the falling section with positive acceleration; the third state, the stable section with zero acceleration; the fourth state, the falling section and the negative horizontal section with negative acceleration; the fifth state, the rising section with negative acceleration.

[0015] Among them, the slope of the rising section of the acceleration curve is k a , and the slope of the falling section is -k a .

[0016] Further preferably, the step S2 includes:

[0017] S2.1. Obtain the motor speed, motor acceleration, and acceleration slope of the current control planning node, and judge the speed difference required for the motor speed to reach the maximum speed based on the total driving distance and the driving distance difference required for calculation at the current control planning node;

[0018] S2.2. Calculate the acceleration control scheme based on the motor speed, motor acceleration, acceleration slope, motor required driving distance, and speed difference required to reach the maximum speed at the current control planning node;

[0019] S2.3. Obtain the motor speed increment and the first motor driving distance increment in the fastest case, and obtain the motor speed decrement and the second motor driving distance increment in the fastest case; the first motor driving distance increment is the distance increment generated by setting the acceleration to 0 as the planning goal in the fastest case, and the second motor driving distance increment is the distance increment generated from the control planning node with an acceleration of 0 to the completion of the task in the fastest case.

[0020] S2.4. Update the planned positions of each control planning node and the motor state at the planned positions of each control planning node based on the motor speed increment, the first motor driving distance increment, and the second motor driving distance increment described in step S2.3.

[0021] Further preferably, the calculation formula for the distance difference is as follows:

[0022] ΔP = P com (t) - P ref (t)(1)

[0023] Wherein, ΔP represents the driving distance required by the motor, P com (t) represents the target position of the motor, P ref (t) represents the position of the current control planning node.

[0024] Further preferably, in step S3, the calculation process of the rotational speed difference is as follows:

[0025] S3.1. Introduce the acceleration sign function sign(a(t)):

[0026]

[0027] Wherein, a(t) is the motor acceleration at the current control planning node;

[0028] S3.2. Calculate the rotational speed difference ΔO:

[0029] ΔO = |O max |·sign(a(t)) - O ref (t)(3)

[0030] Wherein, ΔO represents the rotational speed difference at the current control planning node, |O max | represents the motor limit rotational speed, O ref (t) represents the motor rotational speed at the current control planning node.

[0031] Further preferably, in step S2.3, the calculation of the motor rotational speed increment during the period when the motor acceleration drops to 0 in the fastest case is as follows:

[0032]

[0033] Wherein, ΔO rem represents the motor rotational speed increment.

[0034] Further preferably, in step S2.3, the calculation process of the first driving distance increment of the motor is as follows:

[0035] S2.31. Calculate the motor rotational speed at the position where the motor acceleration drops to 0 in the fastest case, and the calculation formula of the motor rotational speed is as follows:

[0036] O zero (t + t zero ) = O ref (t) + ΔO rem (6)

[0037] Wherein, tzero It represents the time required for the motor to reach the position where the acceleration drops to 0 in the fastest case.

[0038] O zero (t + t zero ) represents the motor speed at the position where the motor acceleration drops to 0 in the fastest case.

[0039] S2.32. Calculate the first driving distance increment of the motor at the position where the motor acceleration drops to 0 in the fastest case. The formula is as follows:

[0040]

[0041] where, ΔP zero represents the first driving distance increment of the motor.

[0042] Further preferably, in step S2.3, the calculation process of the second driving distance increment of the motor during the period when the motor speed drops from the fastest to 0 is as follows:

[0043]

[0044] where, ΔP ome represents the second driving distance increment of the motor.

[0045] Further preferably, in step S2.4, the motor acceleration and motor speed are updated as follows:

[0046] Motor acceleration update formula:

[0047] Δa = k a ·ΔT (12)

[0048]

[0049] where, ΔT represents the control period of the motor system, Δa represents the absolute value of the fixed increment of the motor acceleration, and a(t + ΔT) represents the motor acceleration at the control planning node of t + ΔT;

[0050] Motor speed update formula:

[0051] O ref (t + ΔT) = O ref (t) + a(t)·ΔT (16),

[0052] where, O ref (t + ΔT) represents the motor speed at the control planning node of t + ΔT.

[0053] Further preferably, the planning position update operation of each control planning node is as follows:

[0054] P ref(t + ΔT) = P ref (t) + O ref (t)·ΔT (18),

[0055] Wherein, P ref (t + ΔT) represents the motor planning position at the control planning node of t + ΔT.

[0056] Further preferably, in step S2.4, updating the motor state at the planning position of each control planning node includes the following steps:

[0057] S2.41. Select the corresponding state update formula based on the motor state at the position when the adjustment action is executed;

[0058] S2.42. Make a conditional judgment based on the selected state update formula, and update the motor state at the planning position of each control planning node based on the judgment result;

[0059] Among them, the state update formula includes:

[0060] When the start state A state = 1, the state update formula is:

[0061]

[0062] When the start state A state = 2, the state update formula is:

[0063]

[0064] When the start state A state = 3, the state update formula is:

[0065]

[0066] When the start state A state = 4, the state update formula is:

[0067]

[0068] When the start state A state = 5, the state update formula is:

[0069]

[0070] The described real-time position planning control method with continuous acceleration has the following advantages and remarkable effects compared with the existing technology:

[0071] First, the present invention makes state judgments and executes operations within each control cycle, and can adapt to and follow the given position command that changes in real time of the servo motor;

[0072] Second, by transforming and processing the motion formula, the present invention avoids using time variables in the position planning control process, thus avoiding complex operations such as solving cubic equations and simplifying the calculation process;

[0073] Third, the given position in the next control cycle of the present invention is only related to the acceleration, speed, and given position in the current control cycle. State update operations are performed in each control cycle, which has real-time performance, so the influence of cumulative error can be eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0075] Figure 1 It is a schematic diagram of the seven-segment acceleration curve of the present invention and the five states taken by the present invention;

[0076] Figure 2 It is a flowchart of the position planning algorithm of the present invention;

[0077] Figure 3 It is a flowchart of acceleration and state update in the position planning algorithm of the present invention;

[0078] Figure 4 It is an effect diagram of the target position, planned position, speed, and acceleration when the position planning algorithm of the present invention is applied to an actual servo motor. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0079] The present invention will be further described below through specific embodiments and drawings. The embodiments of the present invention are to better enable those skilled in the art to understand the present invention and do not impose any limitations on the present invention.

[0080] Embodiment 1:

[0081] Referring to Figure 2 , a real-time position planning control method with continuous acceleration, the position planning control method includes the following steps:

[0082] Referring to Figure 1 , a seven-segment acceleration trapezoidal curve is adopted, and the seven-segment acceleration trapezoidal curve is correspondingly divided into five motor states. Define the current rotational speed acceleration a(t) (unit: r / s 2 , r: mechanical revolution), the maximum allowable rotational speed acceleration a max (constant value),

[0083] The five states corresponding to the above seven-segment acceleration trapezoidal curve (state variable: A state ), are as follows:

[0084] First state: The rising segment with positive acceleration and the positive horizontal segment (A state = 1);

[0085] Second state: The falling segment with positive acceleration (A state = 2);

[0086] Third state: The stable segment with zero acceleration (A state = 3);

[0087] Fourth state: The falling segment with negative acceleration and the negative horizontal segment (A state = 4);

[0088] Fifth state: The rising segment with negative acceleration (A state = 5);

[0089] The slope k of the seven-segment acceleration curve a (unit: r / s 3 , a fixed value), and the slope of the falling segment is -k a .

[0090] S1. Obtain the motor task, where the motor task includes the driving distance P com (t) (unit: revolutions) required by the motor and the system control period ΔT, and use each system control period as a control planning node for the total task execution duration;

[0091] S2. Judge the motor state at each of the control planning nodes. Among the five states corresponding to the above-mentioned Figure 1 , select the corresponding state based on the current acceleration.

[0092] Meanwhile, determine the acceleration control scheme for guiding the motor control acceleration based on the motor state. The acceleration control scheme is such that when the motor task is completed, the motor acceleration a = 0, the motor speed O ref = 0, and the driving distance difference ΔP = 0 are all zero. By performing approach control of acceleration, speed, and distance in each control period, these three goals are finally achieved simultaneously.

[0093] An example of the limit logic for the motor speed during the planning process is as follows:

[0094] When the acceleration is greater than 0, and the rotational speed increment required for the acceleration to change from the current adjustment action execution to 0 in the shortest time is defined as the minimum remaining speed increment, and the distance increment required is defined as the minimum remaining distance increment. The rotational speed difference during the execution of the adjustment action is less than the minimum remaining speed increment. Therefore, acceleration is performed when the acceleration is greater than 0, but it cannot exceed the actual limit rotational speed. So, deceleration is required when exceeding the limit rotational speed of the motor itself. At this time, the acceleration state becomes A state = 2, and vice versa.

[0095] An example of the planning logic for the motor position during the planning process is as follows:

[0096] When the acceleration is less than 0, the driving distance difference ΔP < 0. At this moment, the rotational speed at the control time node is less than 0, and O ref (t + ΔT) < 0, and the driving distance difference is greater than the minimum remaining motor driving distance increment, which means that assuming the acceleration a changes from the current value to 0 in the shortest time, the motor rotational speed O ref (t + t zero ) is still less than 0, but the remaining position is greater than 0. To simultaneously achieve the three goals of a = 0, O ref = 0, and ΔP = 0, the acceleration a needs to increase immediately (i.e., the acceleration state A state = 5).

[0097] The specific mathematical operations are as follows:

[0098] S2.1. Obtain the motor rotational speed, motor acceleration, and acceleration slope of the current control planning node, calculate the required driving distance difference based on the required driving distance of the motor and the position of the current control planning node, and calculate the rotational speed difference based on the motor limit rotational speed and the motor rotational speed at the current control planning node;

[0099] The calculation formula for the distance difference ΔP is as follows:

[0100] ΔP = P com (t) - P ref (t)(1)

[0101] where ΔP represents the required driving distance of the motor, P com (t) represents the required driving distance of the motor, P ref (t) represents the position of the current control planning node.

[0102] The rotational speed difference is calculated as follows:

[0103] S2.11. Introduce the acceleration sign function sign(a(t)):

[0104]

[0105] where a(t) is the motor acceleration at the current control planning node;

[0106] S2.12. Calculate the rotational speed difference ΔO:

[0107] ΔO = |O max |·sign(a(t)) - O ref (t) (3)

[0108] where ΔO represents the rotational speed difference at the current control planning node, |O max | represents the motor speed limit, and O ref (t) represents the motor rotational speed at the current control planning node.

[0109] S2.2. Based on the motor rotational speed, motor acceleration, acceleration slope, position difference, and rotational speed difference at the current planned position, calculate the fastest scenario for the motor to reach the target position at the planned target; this fastest scenario is described by the motor speed limit logic in the aforementioned planning process and the motor position planning logic in the planning process, in order to complete the driving distance required by the motor under the condition of reaching the maximum speed. Completing the distance task in the fastest scenario includes two stages. The first stage: start driving, with the acceleration increasing positively, so that the motor rotational speed gradually reaches the maximum. At the moment when the motor rotational speed is the maximum, the motor acceleration also needs to drop to 0. The second stage: the motor acceleration is negative to decelerate until the speed is 0, that is, the driving distance is completed.

[0110] S2.3. Obtain the motor speed increment and the first motor driving distance increment in the fastest scenario, and obtain the motor speed decrement and the second motor driving distance increment in the fastest scenario; the first motor driving increment is set with the acceleration being 0 as the planning target, and the second motor driving distance increment is the distance increment from the acceleration being 0 to the completion of the task.

[0111] S2.31. During the period when the motor speed in the fastest scenario drops from the fastest to 0, assume that the acceleration changes from the current value to zero in the shortest time, that is, corresponding to state 3. This process is a variable-uniform speed process, and the time used for this process is t zero , and the generated speed increment is the least remaining speed increment, defined as the first speed increment. The motor driving distance increment generated during this process is defined as the first motor driving distance increment ΔP zero , and the motor speed at the end of this process is O zero (t + t zero ). The corresponding speed increment calculation process is as follows:

[0112]

[0113] Using the basic kinematic formulas and integral formulas, transform the velocity-acceleration relationship, and finally obtain ΔO without the time variable rem Calculation formula:

[0114]

[0115] At the same time, obtain O zero (t + t zero ):

[0116] O zero (t + t zero ) = O ref (t) + ΔO rem (6)

[0117] Among them, t zero represents the time required for the motor to reach the position where the acceleration drops to 0 in the fastest case,

[0118] O zero (t + t zero ) represents the motor speed at the position where the motor acceleration drops to 0 in the fastest case;

[0119] The first driving distance increment of the motor at the position where the motor acceleration drops to 0 in the fastest case is as follows:

[0120]

[0121] Using the basic kinematic formulas and integral formulas, transform the position-velocity-acceleration relationship, and finally obtain the first driving distance increment of the motor without the time variable:

[0122]

[0123] Among them, ΔP zero represents the first driving distance increment of the motor.

[0124] S2.32. Calculate the motor speed reduction and the second driving distance increment of the motor during the period when the motor speed drops from the fastest to 0 in the fastest case; assume that the speed is not 0 when the acceleration changes to 0 for the first time. At this time, assume that the speed changes from the current value to 0 in the shortest time under the premise of continuous acceleration, and the acceleration also changes to 0 at the same time as the speed changes to 0. The motor driving distance increment generated during this process is defined as the remaining process motor driving distance increment, which is defined as the second driving distance increment ΔP ome , and its calculation process is as follows:

[0125]

[0126] If O zero (t + t zero ) = Omax , which is defined that the remaining process is the incremental motor second driving distance ΔP max :

[0127]

[0128] Define the incremental motor driving distance of the whole process as the minimum remaining incremental motor driving distance ΔP rem :

[0129] ΔP rem = ΔP zero + ΔP ome (11).

[0130] The state logic judgment and specific operations of the real-time position planning control method based on continuous acceleration are realized based on the above analysis conclusions.

[0131] S2.4. Update the planned positions of each control planning node and the motor states at the planned positions of each control planning node based on the motor speed increment, the first motor driving distance increment, and the second motor driving distance increment described in step S2.3.

[0132] According to the set required motor driving distance P com (t), the motor limit speed |O max |, the current control planning node P ref (t), the current planned speed O ref (t), the current planned acceleration a(t), and the state A at which the position planning algorithm is currently located state , perform update operations on the acceleration, speed, output given position, and state (the starting time t can be any time):

[0133] (1) Motor acceleration update:

[0134] Δa = k a ·ΔT (12)

[0135]

[0136] Among them, ΔT represents the control period of the motor system, Δa represents the absolute value of the fixed increment of the motor acceleration, a(t + ΔT) represents the motor acceleration at the control planning node of t + ΔT, that is, the motor acceleration of the next control period; the ΔT is the system control period T s , which is determined by the motor frequency. At the same time, in order to limit the acceleration within the allowable range, after performing the above formula operation, a saturation function limit needs to be added:

[0137]

[0138] (2) Motor speed update:

[0139]

[0140] Discretize the above formula with the system control period T s (ΔT = T s ) as the integration step size, and obtain:

[0141] O ref (t + ΔT) = O ref (t) + a(t)·ΔT (16)

[0142] where, O ref (t + ΔT) is the given speed for the next control period;

[0143] (3) Motor planned position update:

[0144]

[0145] Discretize the above formula with the system control period ΔT as the integration step size, and obtain:

[0146] P ref (t + ΔT) = P ref (t) + O ref (t)·ΔT (18)

[0147] where, P ref (t + ΔT) is the given position output by the next control planning node.

[0148] (4) State update:

[0149] When the starting state A state = 3, the state update formula is:

[0150]

[0151] When the starting state A state = 1, the state update formula is:

[0152]

[0153] When the starting state A state = 4, the state update formula is:

[0154]

[0155] When the starting state A state = 2, the state update formula is:

[0156]

[0157] When the starting state A state = 5, the state update formula is:

[0158]

[0159] When none of the judgment conditions are met, the next control planning node still runs in the current A state state.

[0160] So far, a real-time position planning algorithm with continuous acceleration of the present invention has been completed. The control variables required by the motor motion control system in the next control cycle, the output given position, rotational speed, and acceleration of the position planning algorithm have been obtained and can be output as the control variables of the motor.

[0161] In the above steps, only simple operations and simple numerical and logical judgment operations are performed. No time variable is used, and no complex operations such as solving cubic equations are involved. The calculation is simple. At the same time, update operations are performed in each control cycle, which has real-time performance and can adapt to the real-time changes of the given position of the motor following.

[0162] Finally, combined with Figure 3 , the effectiveness of the position planning algorithm of the present invention is illustrated. In actual motor control, the experimental data is as follows:

[0163] Table 1: Explanation of system parameters and initial values

[0164]

[0165] Table 2: Explanation of target position instructions

[0166]

[0167] Refer to Figure 4, at the time node \(t = 1s\), the target position is given to the position servo system for the first time. At this time, the target position changes from \(0r\) to \(50r\), that is, the servo motor rotates \(50\) circles clockwise from the current position. When this happens, the S-shaped position planning curve output by the algorithm starts to follow the given value, and its speed curve is smooth and the acceleration curve is continuous. At \(2.75s\), the target position suddenly changes from \(50r\) to \(0r\). At this time, the S-shaped position planning curve output by the algorithm has not fully followed the previously given target position. This means that the servo motor in the actual operation has not rotated \(50\) circles when it receives a new instruction to return to the original position. In this case, the position planning algorithm immediately updates the acceleration curve and, on the premise of making the planned position output follow the target position, always keeps the speed curve smooth and the acceleration curve continuous. This reflects the real-time performance of the position planning algorithm of the present invention. At \(5.62s\), the target position is suddenly changed to \(50r\) again (even if the servo motor rotates to the position of \(50\) circles again). Finally, when the servo motor rotates to \(50\) circles, the rotational speed smoothly decelerates to zero speed.

[0168] In summary, through actual operation, it can be seen that under the control of the position planning algorithm of the present invention, while ensuring that the servo motor rotates to the given target position, the start, acceleration, and stop of the motor can all achieve smooth rotational speed switching. At the same time, the position planning algorithm of the present invention has strong real-time performance. When the target position changes suddenly, the algorithm can adapt to the real-time change of following the target position.

[0169] The described real-time position planning control method with continuous acceleration has the following advantages and remarkable effects compared with the existing technologies:

[0170] First, the present invention performs state judgment and execution operations in each control cycle, and can adapt to and follow the given position instruction that changes in real time for the servo motor;

[0171] Second, the present invention simplifies the calculation process by transforming and processing the motion formula to avoid using time variables in the position planning control process, thus avoiding complex operations such as solving cubic equations;

[0172] Third, the given position in the next control cycle of the present invention is only related to the acceleration, speed, and given position in the current control cycle. State update operations are performed in each control cycle, which has real-time performance, and thus can eliminate the influence of cumulative errors.

[0173] The embodiments described above are only used to describe the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope of the present invention.

Claims

1. A real-time position planning and control method with continuous acceleration, characterized in that, Including the steps: S1. Obtain a motor task, where the motor task includes the total driving distance of the motor, and divide a number of control planning nodes according to the control period of the motor system; S2. Judge the motor state at each of the control planning nodes, and determine an acceleration control scheme for guiding the motor control acceleration based on the motor state; Wherein, the acceleration control scheme is determined with the planning goal that the motor speed, motor acceleration, and driving distance difference are all zero when the motor task is completed; the motor state includes: the first state, the rising section and the positive horizontal section with positive acceleration; the second state, the descending section with positive acceleration; the third state, the stable section with zero acceleration; the fourth state, the descending section and the negative horizontal section with negative acceleration; the fifth state, the rising section with negative acceleration; The step S2 includes: S2.

1. Obtain the motor speed, motor acceleration, and acceleration slope of the current control planning node, and judge the speed difference required for the motor speed to reach the maximum speed based on the total driving distance and the driving distance difference required for calculation at the current control planning node; S2.

2. Calculate the acceleration control scheme based on the motor speed, motor acceleration, acceleration slope, the driving distance required for the motor, and the speed difference required to reach the maximum speed at the current control planning node; S2.

3. Obtain the motor speed increment and the first motor driving distance increment in the fastest case, and obtain the motor speed decrement and the second motor driving distance increment in the fastest case; the first motor driving distance increment is the distance increment generated by setting the acceleration to 0 as the planning goal in the fastest case, and the second motor driving distance increment is the distance increment generated from the control planning node with an acceleration of 0 to the completion of the task in the fastest case; S2.

4. Update the planned positions of each control planning node and the motor state at the planned positions of each control planning node based on the motor speed increment, the first motor driving distance increment, and the second motor driving distance increment described in step S2.

3.

2. The real-time position planning control method with continuous acceleration according to claim 1, characterized in that In the step S2.1, the calculation formula of the distance difference is as follows: ΔP = P com (t) - P ref (t)(1) Among them, ΔP represents the driving distance required by the motor, and P com (t) represents the required driving distance, and P ref (t) represents the position of the current control planning node.

3. The real-time position planning control method with continuous acceleration according to claim 2, characterized in that In the step S2.1, the calculation process of the speed difference is as follows: S2.

11. Introduce the acceleration sign function sign(a(t)): Wherein, a(t) is the motor acceleration at the current control planning node; S2.

12. Calculate the speed difference ΔO: ΔO = |O max |·sign(a(t)) - O ref (t) (3) Among them, ΔO represents the rotational speed difference of the current control planning node, |O max | represents the motor speed limit, O ref (t) represents the motor speed of the current control planning node.

4. A real-time position planning and control method with continuous acceleration according to claim 3, characterized in that, In the step S2.3, the calculation of the motor speed increment during the period when the motor acceleration drops to 0 in the fastest case is as follows: Among them, ΔO rem represents the motor speed increment.

5. A real-time position planning and control method with continuous acceleration according to claim 4, characterized in that In the step S2.3, the calculation process of the first motor driving distance increment is as follows: S2.

31. Calculate the motor speed at the position where the motor acceleration drops to 0 in the fastest case, and the calculation formula of the motor speed is as follows: O zero (t + t zero ) = O ref (t) + ΔO rem (6) where t zero represents the time required for the motor to reach the position where the acceleration drops to 0 in the fastest case, O zero (t + t zero ) represents the motor speed at the position where the motor acceleration drops to 0 in the fastest case; S2.

32. Calculate the first motor driving distance increment at the position where the motor acceleration drops to 0 in the fastest case, and the formula is as follows: Among them, ΔP zero represents the first driving distance increment of the motor.

6. A real-time position planning and control method with continuous acceleration according to claim 5, characterized in that In the step S2.3, the calculation process of the second motor driving distance increment during the period when the motor speed drops from the fastest to 0 is as follows: where, ΔP ome represents the incremental distance of the second drive of the motor.

7. A real-time position planning and control method with continuous acceleration according to claim 6, characterized in that In the step S2.4, the update operations of the motor acceleration and the motor speed are as follows: Motor acceleration update formula: Δa = k a ·ΔT (12) Where, ΔT represents the control period of the motor system, Δa represents the absolute value of the fixed increment of the motor acceleration, and a(t + ΔT) represents the motor acceleration at the control planning node of t + ΔT; Motor speed update formula: O ref (t + ΔT) = O ref (t) + a(t)·ΔT (16), Among them, O ref (t + ΔT) represents the motor speed of the control planning node at t + ΔT.

8. A real-time position planning control method with continuous acceleration according to claim 7, characterized in that, The planning position update operation of each control planning node is as follows: P ref (t + ΔT) = P ref (t) + O ref (t)·ΔT (18), where P ref (t + ΔT) represents the motor planned position at the t + ΔT control planning node.

9. A real-time position planning control method with continuous acceleration according to claim 8, characterized in that, In step S2.4, the motor states at the planning positions of each control planning node updated include the following steps: S2.

41. Based on the motor state at the position when the adjustment action is executed, select the corresponding state update formula; S2.

42. Make a conditional judgment based on the selected state update formula, and update the motor states at the planning positions of each control planning node based on the judgment result; Among them, the state update formula includes: When the starting state A state = 1, the state update formula is: When the starting state A state = 2, the state update formula is: When the start state A state = 3, the state update formula is: When the starting state A state = 4, the state update formula is: When the starting state A state = 5, the state update formula is:

Images