System dynamic characteristic adaptive trajectory planning method based on FPTOC

Abstract

The invention discloses a system dynamic characteristic adaptive trajectory planning method based on FPTOC, which belongs to the technical field of motion control, and comprises the following steps: S1, calculating trajectory planning key parameters of the FPTOC according to a trajectory planning constraint set value and equivalent system parameters for judging system dynamic characteristics; s2, estimating a speed feed-forward value, and performing amplitude limiting processing on the speed feed-forward value to obtain an expected speed; s3, obtaining an FPTOC output; s4, calculating the output acceleration, the output speed and the output position of the trajectory planning; through the above mode, the technical problem that the trajectory planning method is not matched with the dynamic characteristics of the system is solved, the equivalent bandwidth and damping parameters of the system are considered from the source, the generated trajectory planning instruction is matched with the dynamic capability of the system, overshoot and oscillation are avoided, and the motion control precision is improved.

Description

Technical Field

[0001] This invention relates to the field of motion control technology, and more specifically to an adaptive trajectory planning method for system dynamic characteristics based on FPTOC. Background Technology

[0002] Trajectory planning is a core component of motion control systems. Its goal is to generate a theoretically continuous, smooth, and feasible sequence of position, velocity, and acceleration commands based on task requirements. Excellent trajectory planning commands can effectively reduce shocks and vibrations generated by the actuators, lower tracking errors, and improve the overall performance of the system.

[0003] Traditional trajectory planning methods, such as polynomial interpolation and trapezoidal velocity planning, are mostly based on purely mathematical calculation models. These methods typically consider only kinematic constraints or simple Newton's second law dynamics models under ideal assumptions. Their core idea is to construct a mathematically continuous and smooth trajectory curve. However, traditional purely mathematical trajectory planning methods do not consider the dynamic characteristics of the controlled object itself, especially the system's closed-loop bandwidth and damping coefficient. In actual physical systems, servo drives, motors, mechanical structures, and other components constitute a dynamic system with finite response capabilities and specific damping characteristics. When the trajectory planning command changes too rapidly or contains high-frequency components, the limited system bandwidth cannot fully track the command, leading to response lag, overshoot, or even mechanical resonance, which will seriously affect the system's positioning accuracy, stability, and lifespan.

[0004] To address the aforementioned issues, there is an urgent need for a method that can fundamentally solve the shortcomings of traditional trajectory planning. This method needs to break through the limitations of pure mathematical calculations and use the dynamic characteristics of the control system as the key parameter for trajectory planning. This would generate trajectory planning instructions that match the dynamic capabilities of the system and can be effectively executed, thus solving the problem of mismatch between trajectory planning instructions and system response in existing technologies. Summary of the Invention

[0005] The technical problem to be solved by this invention is to address the mismatch between trajectory planning methods and system dynamic characteristics. By considering the equivalent system parameters corresponding to the system's equivalent bandwidth and damping parameters under the system's dynamic characteristics from the source, the generated trajectory planning instructions can be matched with the system's dynamic capabilities, avoiding overshoot and oscillation, and improving motion control accuracy.

[0006] The technical solution adopted by this invention to solve the technical problem is: an adaptive trajectory planning method for system dynamic characteristics based on FPTOC, comprising the following steps:

[0007] S1: Calculate the key trajectory planning parameters of FPTOC based on the trajectory planning constraint settings and the equivalent system parameters used to judge the dynamic characteristics of the system.

[0008] The key parameters for trajectory planning include: FPTOC linear region width. FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term ;

[0009] The relevant calculation formulas are as follows:

[0010] (1)

[0011] (2)

[0012] (3)

[0013] (4)

[0014] in, For the natural frequency of the equivalent second-order system, For the damping of the equivalent second-order system, For acceleration constraints, For speed constraints, For calculating acceleration coefficients, The acceleration constraint is the calculation period. The speed constraint The natural frequency of the equivalent second-order system and equivalent second-order system damping The equivalent system parameters constituted by the acceleration constraints Speed ​​constraints Acceleration calculation coefficients Operation cycle This constitutes the trajectory planning constraint setting value;

[0015] S2: Based on the currently set position command The set position command is estimated in real time using a second-order phase-locked loop. Corresponding velocity feedforward value The velocity feedforward value The calculation formula is as follows:

[0016] (5)

[0017] in, , , All of these are velocity estimation coefficients calculated based on the equivalent system parameters and the trajectory planning constraint settings. For the previous operation cycle Position instructions For the previous operation cycle The calculated estimated speed, For the first two operation cycles The estimated speed obtained from the calculation;

[0018] The estimated velocity feedforward value is transmitted. intermediate variables, where, , , Then, based on the trajectory planning constraint settings, the velocity feedforward value is adjusted. Achieve the desired speed by performing amplitude limiting. ;

[0019] S3: Based on FPTOC input Based on FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term The acceleration constraint The desired speed Calculate the linear domain output of FPTOC and FPTOC nonlinear domain output ;

[0020] (6)

[0021] (7)

[0022] (8)

[0023] in, This is the current position instruction. For the previous operation cycle Output position of trajectory planning;

[0024] Based on FPTOC linear region width Make a judgment, if but ,like Then further judgment and If the relationship, but ,like but The FPTOC linear domain output is obtained based on calculation. and FPTOC nonlinear domain output FPTOC output for obtaining the equivalent system parameters ;

[0025] S4: Based on the FPTOC output Output speed of trajectory planning in the previous calculation cycle and output position Calculate the output acceleration of trajectory planning Output speed of trajectory planning and the output position of trajectory planning In order to obtain trajectory planning instructions that match the system's dynamic capabilities.

[0026] As a preferred embodiment of the present invention, the speed estimation coefficient The calculation formula is as follows:

[0027] (9).

[0028] As a preferred embodiment of the present invention, the speed estimation coefficient The calculation formula is as follows:

[0029] (10).

[0030] As a preferred embodiment of the present invention, the speed estimation coefficient The calculation formula is as follows:

[0031] (11).

[0032] As a preferred technical solution of the present invention, the desired speed For speed constraints Submit the desired position command The expected speed at that time.

[0033] As a preferred embodiment of the present invention, the velocity feedforward value in S2 Achieve the desired speed by performing amplitude limiting. Specifically, it includes:

[0034] If the velocity feedforward value The absolute value exceeds the speed constraint. Then, based on the aforementioned velocity feedforward value... Symbol output expected speed Otherwise, the desired speed Equal to velocity feedforward value The formula is as follows:

[0035] (12);

[0036] Where sign is the sign function.

[0037] As a preferred embodiment of the present invention, the output acceleration of trajectory planning is calculated in S4. Specifically, it includes:

[0038] According to the FPTOC output and the output speed of trajectory planning in the previous calculation cycle. Calculate the acceleration ratio for trajectory planning Using the sign function to measure the acceleration ratio Amplitude limiting is performed to obtain the acceleration ratio that meets the trajectory planning constraints. According to the acceleration ratio and acceleration constraints Calculate the output acceleration of trajectory planning ;

[0039] in, (13)

[0040] (14).

[0041] As a preferred embodiment of the present invention, the output acceleration The calculation formula is as follows:

[0042] (15)

[0043] in, To meet the acceleration ratio constraints of trajectory planning, For acceleration constraints.

[0044] As a preferred embodiment of the present invention, the output speed of the trajectory planning in S4 is calculated. Specifically, it includes:

[0045] According to the output acceleration Operation cycle Output speed and speed constraints Calculate the output speed of trajectory planning The output speed The calculation formula is as follows:

[0046] (16).

[0047] As a preferred embodiment of the present invention, the output position of the trajectory planning in S4 is calculated. Specifically, it includes:

[0048] According to the output position Output speed Operation cycle The output position of the calculated trajectory planning The output position The calculation formula is as follows:

[0049] (17).

[0050] The beneficial effects of this invention are reflected in:

[0051] By using a second-order phase-locked loop to estimate the velocity feedforward value corresponding to the position command in real time, and combining it with the optimized acceleration command output by FPTOC to form feedforward compensation, the system's ability to respond to rapidly changing commands is improved.

[0052] By using the equivalent system parameters corresponding to the equivalent system bandwidth and equivalent damping as key parameters for trajectory planning, the generated planning instructions strictly match the system's dynamic tracking capability, thus solving the system oscillation problem from the source and improving system stability.

[0053] By setting decision conditions based on error thresholds, the system can adaptively select a smaller control output when it enters the nonlinear domain or approaches the target state, effectively avoiding overshoot and ensuring a smooth system response. Attached Figure Description

[0054] Figure 1 This is a schematic diagram of the steps of the present invention;

[0055] Figure 2 This is a block diagram of the second-order phase-locked loop system of the present invention;

[0056] Figure 3 This is a flowchart illustrating the FPTOC output decision-making process logic of the present invention.

[0057] Figure 4 This is a diagram showing the position response effect of the step command in this invention;

[0058] Figure 5 This is a diagram illustrating the speed response effect of the step command in this invention.

[0059] Figure 6 This is a diagram showing the acceleration response effect of the step command in this invention;

[0060] Figure 7 This is a diagram illustrating the position response effect of the sinusoidal command in this invention.

[0061] Figure 8 This is a diagram illustrating the speed response effect of the sinusoidal command in this invention.

[0062] Figure 9 This is a diagram showing the acceleration response effect of the sinusoidal command in this invention. Detailed Implementation

[0063] The invention will now be described in further detail with reference to the accompanying drawings.

[0064] like Figure 1-9 As shown, an adaptive trajectory planning method for system dynamic characteristics based on FPTOC includes the following steps:

[0065] S1: Calculate the key trajectory planning parameters of FPTOC based on the trajectory planning constraint setpoints and the equivalent system parameters used to judge the dynamic characteristics of the system. FPTOC refers to improved approximate time optimal control. Trajectory planning is performed in the corresponding control system based on improved approximate time optimal control.

[0066] The key parameters for trajectory planning include: FPTOC linear region width. FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term ;

[0067] The relevant calculation formulas are as follows:

[0068] (1)

[0069] (2)

[0070] (3)

[0071] (4)

[0072] in, For the natural frequency of the equivalent second-order system, For the damping of the equivalent second-order system, For acceleration constraints, For speed constraints, For calculating acceleration coefficients, The acceleration constraint is the calculation period. The speed constraint The natural frequency of the equivalent second-order system and equivalent second-order system damping The equivalent system parameters constituted by the acceleration constraints Speed ​​constraints Acceleration calculation coefficients Operation cycle This constitutes the trajectory planning constraint setting value;

[0073] S2: Based on the currently set position command The set position command is estimated in real time using a second-order phase-locked loop. Corresponding velocity feedforward value The velocity feedforward value The calculation formula is as follows:

[0074] (5)

[0075] To make the calculation expression clearer, a velocity estimation coefficient is introduced. , , ,in, , , All of these are velocity estimation coefficients calculated based on the equivalent system parameters and the trajectory planning constraint settings. For the previous operation cycle Position instructions For the previous operation cycle The calculated estimated speed, For the first two operation cycles The estimated speed obtained from the calculation;

[0076] Specifically:

[0077] The speed estimation coefficient The calculation formula is as follows:

[0078] (9)

[0079] The speed estimation coefficient The calculation formula is as follows:

[0080] (10)

[0081] The speed estimation coefficient The calculation formula is as follows:

[0082] (11)

[0083] The estimated velocity feedforward value is transmitted. intermediate variables, where, , , Then, based on the trajectory planning constraint settings, the velocity feedforward value is adjusted. Achieve the desired speed by performing amplitude limiting. The desired speed For speed constraints Submit the desired position command Expected speed at that time;

[0084] Specifically:

[0085] If the velocity feedforward value The absolute value exceeds the speed constraint. Then, based on the aforementioned velocity feedforward value... Symbol output expected speed Otherwise, the desired speed Equal to velocity feedforward value The formula is as follows:

[0086] (12);

[0087] Where sign is the sign function;

[0088] S3: Based on FPTOC input Based on FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term The acceleration constraint The desired speed Calculate the linear domain output of FPTOC and FPTOC nonlinear domain output ;

[0089] (6)

[0090] (7)

[0091] (8)

[0092] in, This is the current position instruction. For the previous operation cycle Output position of trajectory planning;

[0093] Based on FPTOC linear region width Make a judgment, if but ,like Then further judgment and If the relationship, but ,like but The FPTOC linear domain output is obtained based on calculation. and FPTOC nonlinear domain output FPTOC output for obtaining the equivalent system parameters ;

[0094] S4: Based on the FPTOC output Output speed of trajectory planning in the previous calculation cycle and output position Calculate the output acceleration of trajectory planning Output speed of trajectory planning and the output position of trajectory planning In order to obtain trajectory planning instructions that match the dynamic capabilities of the system, and thus obtain high-precision trajectory motion;

[0095] Specifically:

[0096] According to the FPTOC output and the output speed of trajectory planning in the previous calculation cycle. Calculate the acceleration ratio for trajectory planning Using the sign function to measure the acceleration ratio Amplitude limiting is performed to obtain the acceleration ratio that meets the trajectory planning constraints. According to the acceleration ratio and acceleration constraints Calculate the output acceleration of trajectory planning ;

[0097] in, (13)

[0098] (14)

[0099] The output acceleration The calculation formula is as follows:

[0100] (15)

[0101] in, To meet the acceleration ratio constraints of trajectory planning, For acceleration constraints;

[0102] According to the output acceleration Operation cycle Output speed and speed constraints Calculate the output speed of trajectory planning The output speed The calculation formula is as follows:

[0103] (16)

[0104] According to the output position Output speed Operation cycle The output position of the calculated trajectory planning The output position The calculation formula is as follows:

[0105] (17)

[0106] Furthermore, the calculation period variable is assigned the following values:

[0107] (18)

[0108] (19).

[0109] The trajectory planning constraint settings and equivalent system parameters are as follows: , , , , , For example;

[0110] S1: Calculate the key trajectory planning parameters of FPTOC based on the trajectory planning constraint settings and the equivalent system parameters used to judge the dynamic characteristics of the system.

[0111] The key parameters for trajectory planning include: FPTOC linear region width. FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term ;

[0112] The relevant calculation formulas are as follows:

[0113] (1)

[0114] (2)

[0115] (3)

[0116] (4)

[0117] in, For the natural frequency of the equivalent second-order system, For the damping of the equivalent second-order system, For acceleration constraints, For speed constraints, For calculating acceleration coefficients, The acceleration constraint is the calculation period. The speed constraint , specifically , , , , , The calculated width of the FPTOC linear region FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term Specifically , , , ;

[0118] S2: Based on the currently set position command The set position command is estimated in real time using a second-order phase-locked loop. Corresponding velocity feedforward value The velocity feedforward value The calculation formula is as follows:

[0119] (5)

[0120] in, , , All of these are velocity estimation coefficients calculated based on the equivalent system parameters and the trajectory planning constraint settings. For the previous operation cycle Position instructions For the previous operation cycle The calculated estimated speed, For the first two operation cycles The estimated speed obtained from the calculation;

[0121] Specifically:

[0122] The speed estimation coefficient The calculation formula is as follows:

[0123] (9)

[0124] The speed estimation coefficient The calculation formula is as follows:

[0125] (10)

[0126] The speed estimation coefficient The calculation formula is as follows:

[0127] (11)

[0128] The estimated velocity feedforward value is transmitted. intermediate variables, where, , , Then, based on the trajectory planning constraint settings, the velocity feedforward value is adjusted. Achieve the desired speed by performing amplitude limiting. The desired speed For speed constraints Submit the desired position command Expected speed at that time;

[0129] Specifically:

[0130] If the velocity feedforward value The absolute value exceeds the speed constraint. Then, based on the aforementioned velocity feedforward value... Symbol output expected speed Otherwise, the desired speed Equal to velocity feedforward value The formula is as follows:

[0131] (12);

[0132] Where sign is the sign function;

[0133] S3: Based on FPTOC input Based on FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation term The acceleration constraint The desired speed Calculate the linear domain output of FPTOC and FPTOC nonlinear domain output ;

[0134] (6)

[0135] (7)

[0136] (8)

[0137] in, This is the current position instruction. For the previous operation cycle Output position of trajectory planning;

[0138] like Figure 3 As shown, the width of the linear region based on FPTOC Make a judgment, if but ,like Then further judgment and If the relationship, but ,like but The FPTOC linear domain output is obtained based on calculation. and FPTOC nonlinear domain output FPTOC output for obtaining the equivalent system parameters ;

[0139] S4: Based on the FPTOC output Output speed of trajectory planning in the previous calculation cycle and output position Calculate the output acceleration of trajectory planning Output speed of trajectory planning and the output position of trajectory planning In order to obtain trajectory planning instructions that match the dynamic capabilities of the system, and thus obtain high-precision trajectory motion;

[0140] Specifically:

[0141] According to the FPTOC output and the output speed of trajectory planning in the previous calculation cycle. Calculate the acceleration ratio for trajectory planning Using the sign function to measure the acceleration ratio Amplitude limiting is performed to obtain the acceleration ratio that meets the trajectory planning constraints. According to the acceleration ratio and acceleration constraints Calculate the output acceleration of trajectory planning ;

[0142] in, (13)

[0143] (14)

[0144] The output acceleration The calculation formula is as follows:

[0145] (15)

[0146] in, To meet the acceleration ratio constraints of trajectory planning, For acceleration constraints;

[0147] According to the output acceleration Operation cycle Output speed and speed constraints Calculate the output speed of trajectory planning The output speed The calculation formula is as follows:

[0148] (16)

[0149] According to the output position Output speed Operation cycle The output position of the calculated trajectory planning The output position The calculation formula is as follows:

[0150] (17)

[0151] Furthermore, the calculation period variable is assigned the following values:

[0152] (18)

[0153] (19).

[0154] The trajectory planning method described in this invention was simulated using Simulink, with step position commands and sinusoidal position commands as inputs. The method was compared with a trajectory planning method based on pure mathematical calculations that do not consider the system's equivalent bandwidth and equivalent damping. Figures 4-9 As shown, the method described in this invention has a smooth trajectory without overshoot, and performs particularly well in terms of acceleration response, with no acceleration oscillation.

[0155] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A system dynamic characteristic adaptive trajectory planning method based on FPTOC, characterized in that: Includes the following steps: S1: Calculate the key trajectory planning parameters of FPTOC based on the trajectory planning constraint settings and the equivalent system parameters used to judge the dynamic characteristics of the system. The key parameters for trajectory planning include: FPTOC linear region width. FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation item ; The relevant calculation formulas are as follows: （1） （2） （3） （4） in, For the natural frequency of the equivalent second-order system, For the damping of the equivalent second-order system, For acceleration constraints, For speed constraints, For calculating acceleration coefficients, The acceleration constraint is the calculation period. The speed constraint The natural frequency of the equivalent second-order system and equivalent second-order system damping The equivalent system parameters constituted by the acceleration constraints Speed ​​constraints Acceleration calculation coefficients Operation cycle This constitutes the trajectory planning constraint setting value; S2: Based on the currently set position command The set position command is estimated in real time using a second-order phase-locked loop. Corresponding velocity feedforward value The velocity feedforward value The calculation formula is as follows: （5） in, , , All of these are velocity estimation coefficients calculated based on the equivalent system parameters and the trajectory planning constraint settings. For the previous operation cycle Position instructions For the previous operation cycle The calculated estimated speed, For the first two operation cycles The estimated speed obtained from the calculation; The estimated velocity feedforward value is transmitted. intermediate variables, where, , , Then, based on the trajectory planning constraint settings, the velocity feedforward value is adjusted. Achieve the desired speed by performing amplitude limiting. ; S3: Based on FPTOC input Based on FPTOC proportional gain FPTOC Differential Gain FPTOC speed deviation item The acceleration constraint The desired speed Calculate the linear domain output of FPTOC and FPTOC nonlinear domain output ; （6） （7） （8） in, This is the current position instruction. For the previous operation cycle Output position of trajectory planning; Based on FPTOC linear region width Make a judgment, if but ,like Then further judgment and If the relationship, but ,like but The FPTOC linear domain output is obtained based on calculation. and FPTOC nonlinear domain output FPTOC output for obtaining the equivalent system parameters ; S4: Based on the FPTOC output Output speed of trajectory planning in the previous calculation cycle and output position Calculate the output acceleration of trajectory planning Output speed of trajectory planning and the output position of trajectory planning In order to obtain trajectory planning instructions that match the system's dynamic capabilities.

2. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 1, characterized in that: The speed estimation coefficient The calculation formula is as follows: （9）。 3. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 2, characterized in that: The speed estimation coefficient The calculation formula is as follows: （10）。 4. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 3, characterized in that: The speed estimation coefficient The calculation formula is as follows: （11）。 5. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 4, characterized in that: The desired speed For speed constraints Submit the desired position command The expected speed at that time.

6. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 5, characterized in that: The velocity feedforward value in S2 Achieve the desired speed by performing amplitude limiting. Specifically, it includes: If the velocity feedforward value The absolute value exceeds the speed constraint. Then, based on the aforementioned velocity feedforward value... Symbol output expected speed Otherwise, the desired speed Equal to velocity feedforward value The formula is as follows: （12）； Where sign is the sign function.

7. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 1, characterized in that: The output acceleration of trajectory planning is calculated in S4. Specifically, it includes: According to the FPTOC output and the output speed of trajectory planning in the previous calculation cycle. Calculate the acceleration ratio for trajectory planning Using the sign function to measure the acceleration ratio Amplitude limiting is performed to obtain the acceleration ratio that meets the trajectory planning constraints. According to the acceleration ratio and acceleration constraints Calculate the output acceleration of trajectory planning ; in, (13) （14）。 8. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 7, characterized in that: The output acceleration The calculation formula is as follows: （15） in, To meet the acceleration ratio constraints of trajectory planning, For acceleration constraints.

9. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 8, characterized in that: The output speed of trajectory planning is calculated in S4. Specifically, it includes: According to the output acceleration Operation cycle Output speed and speed constraints Calculate the output speed of trajectory planning The output speed The calculation formula is as follows: （16）。 10. The adaptive trajectory planning method for system dynamic characteristics based on FPTOC according to claim 9, characterized in that: The output position of the trajectory planning calculation in S4 Specifically, it includes: According to the output position Output speed Operation cycle The output position of the calculated trajectory planning The output position The calculation formula is as follows: （17）。

Images