A Trajectory Planning Method and System for a Parallel Multi-Degree-of-Freedom Platform

Abstract

The present invention discloses a trajectory planning method and system for a parallel multi-degree-of-freedom platform, belonging to the technical field of trajectory planning, including: calculating the maximum allowable movement speed on each degree of freedom; based on the maximum allowable movement speed on each degree of freedom, using an S-shaped speed curve planning algorithm to calculate the initial trajectory duration of each degree of freedom, and combining with the target trajectory duration to calculate the trajectory synchronization duration; synchronizing the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; real-time checking whether the speed exceeds the limit, and dynamically adjusting the jerk and the maximum movement speed when the speed is exceeded. Through the collaborative planning on multiple degrees of freedom, the rapid planning of the full-cycle movement trajectory of the parallel multi-degree-of-freedom platform is realized. The present invention can control the pose of the platform with millisecond-level precision under the condition of ensuring the constraints of the Cartesian space working plane, effectively avoid the elastic deformation and residual stress problems of the mechanical structure, improve the service life of the mechanism and reduce the running noise.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory planning, and particularly to a trajectory planning algorithm for a parallel multi-degree-of-freedom platform, which realizes rapid planning of the full-cycle motion trajectory of the parallel multi-degree-of-freedom platform through collaborative planning on multiple degrees of freedom. Background Art

[0002] With the development of intelligent manufacturing equipment, the requirements for mechanical systems have gradually evolved into small size, high positioning accuracy, and fast dynamic response speed. Traditional serial mechanisms are difficult to meet the needs of modern industry due to problems such as volume, load capacity, and error accumulation. Parallel six-degree-of-freedom mechanisms have gradually been applied to the field of precision motion control due to their advantages of high control accuracy, strong load capacity, and no error accumulation. Typical application scenarios include high-end scenarios such as optical telescopes, aerospace simulations, and medical and rehabilitation equipment.

[0003] In a parallel six-degree-of-freedom platform, the trajectory planning algorithm is the basis of its motion control. The motion trajectory of the platform directly affects the working efficiency, motion smoothness, and energy consumption of the robot, etc. In this field, a control strategy based on the hinge space is usually used to generate the motion trajectory of the platform, that is, it is assumed in advance that the driving branches are independent and non-coupled relationships, and the kinematic mathematical model (inverse kinematic equation) of the mechanism is established according to the geometric parameters of the mechanism. This type of method controls the six motors of each platform separately without considering their coupling relationship, ignoring the mechanical constraint relationship between the six branches. When the starting pose and the ending pose of the platform motion are far apart, the ends of the six connecting rods may not form a plane within the Cartesian working space, resulting in mechanical deformation between the connecting rod hinges and the base, or the electric cylinder generating self-rotation due to internal stress, thus affecting the control accuracy and the service life of the machine. In addition, this type of method cannot accurately control the time for the parallel six-degree-of-freedom platform to reach the target pose and the pose at each moment during the motion process, reducing its flexibility and reliability in use. There are also some trajectory planning algorithms that calculate the starting position and the ending position of each connecting rod through inverse solution, and divide the entire trajectory into small trajectory segments by interpolation. However, this type of method still has problems of contact deformation in a small range, and the theoretical speed of the connection points of the trajectory segments is 0, increasing the energy consumption while reducing the motion smoothness of the platform. Many control strategies based on the hinge space used in engineering can only handle trajectory planning with a starting point at rest or a short distance, and cannot adapt to scenarios where the platform has an initial velocity and acceleration at the starting point or the distance between the starting point and the ending point is far, with a limited scope and field of adaptation.

[0004] Although the traditional workspace-based control strategy takes into account the coupling relationship between the six electric cylinders, this type of method usually requires the establishment of a dynamic model of the system and the additional installation of sensors that can measure the real-time pose of the moving platform, resulting in a significant increase in the cost of the control system. Moreover, this type of algorithm usually has a high computational complexity, and the control accuracy of the platform depends on the accuracy of the sensors, which cannot fully meet the requirements of industrial applications.

[0005] In addition, some trajectory planning methods for parallel six-degree-of-freedom platforms directly output the motion commands of each electric cylinder without considering whether the motor can control the electric cylinder to complete these motion commands. Even in the scenario where the parallel six-degree-of-freedom platform is moving at a constant speed, some electric cylinders will accelerate due to the rotation of the platform angle, resulting in the current of the control motor or the operation exceeding the safe motion range, causing the platform to be unable to execute normally according to the planned trajectory. Some trajectory planning algorithms for parallel six-degree-of-freedom platforms use the S-shaped speed curve planning algorithm to calculate the motion trajectories on each degree of freedom and perform trajectory synchronization. However, most of the existing methods assume that the S-shaped speed curve planning algorithm will surely succeed in calculation, and only consider the ideal state on each degree of freedom when calculating the synchronization time, lacking the consideration of the situation where the S-shaped speed curve planning algorithm fails in planning and the trajectory synchronization fails. At the same time, most of these algorithms pursue the shortest trajectory duration and cannot control the trajectory duration according to actual needs, so the engineering application value is limited. Summary of the Invention

[0006] The object of the present invention is to overcome the technical problems existing in the prior art, and provides a trajectory planning method and system for a parallel multi-degree-of-freedom platform, which can plan a motion trajectory of a parallel multi-degree-of-freedom platform with a short motion time, a smooth motion trajectory, a safe and controllable motion trajectory, a controllable total trajectory time, and multiple degrees of freedom that can reach the target pose simultaneously when the parallel multi-degree-of-freedom platform is in any motion state.

[0007] The object of the present invention is achieved by the following technical solutions:

[0008] In the first aspect, a trajectory planning method for a parallel multi-degree-of-freedom platform is provided, including:

[0009] Motion constraint calculation: Based on the initial pose, speed, acceleration, and hardware parameters, calculate the maximum allowable motion speed on each degree of freedom;

[0010] Synchronization duration calculation: Based on the maximum allowable motion speed on each degree of freedom, use the S-shaped speed curve planning algorithm to calculate the initial trajectory duration of each degree of freedom, and calculate the trajectory synchronization duration in combination with the target trajectory duration;

[0011] Platform trajectory synchronization calculation: synchronizing the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; wherein, trajectory point synchronization and overspeed correction are achieved by iteratively adjusting the input parameters of the S-type speed curve planning algorithm;

[0012] Motor trajectory calculation: Convert the trajectory data obtained by synchronously calculating the platform trajectory into motor encoder instructions, check in real time whether the speed exceeds the limit, and dynamically adjust the acceleration and maximum movement speed when overspeeding.

[0013] Preferably, the calculation of the maximum movement speed allowed in each degree of freedom includes:

[0014] According to the initial posture ,speed , acceleration and the jerk in each degree of freedom determined by the hardware , calculate the time required to reduce the initial acceleration to 0 , and the speed after the initial acceleration drops to 0 and location ;

[0015] Calculate the time required to reduce the speed to 0 and the position after it drops to 0 ;

[0016] The general parallel multi-degree-of-freedom platform inverse solution algorithm is used to calculate and target pose Corresponding connecting rod length and , calculate the maximum motion time of each electric cylinder in the parallel multi-degree-of-freedom platform ;

[0017] Calculate the initial maximum velocity of motion in each degree of freedom , and according to the Calculate the maximum velocity of motion in each degree of freedom .

[0018] Preferably, according to the Calculate the maximum velocity of motion in each degree of freedom ,include:

[0019] right Perform numerical truncation: ,in, Indicates the cutoff value.

[0020] Preferably, the S-shaped velocity curve planning algorithm is used to calculate the initial trajectory duration of each degree of freedom, and the trajectory synchronization duration is calculated in combination with the target trajectory duration, including:

[0021] Calculate the initial trajectory duration for each degree of freedom: Starting from the pose after reducing the speed to 0, calculate the initial S-shaped speed curve planning parameters for each degree of freedom, and calculate the trajectory duration for each degree of freedom based on these parameters. ; Calculate the total trajectory duration for each degree of freedom when reducing the speed to 0: , Starting from the position and speed after reducing the acceleration to 0 as the starting state, calculate the initial S-shaped speed curve planning parameters for each degree of freedom, and calculate the trajectory duration for each degree of freedom based on these parameters. ;

[0022] The total trajectory duration for each degree of freedom: ;

[0023] Calculate the initial trajectory synchronization duration: Calculate the maximum value in the total trajectory duration and the index value corresponding to the maximum value , calculate the maximum value , calculate the maximum duration not including : ;

[0024] Calculate the initial trajectory synchronization duration as: ;

[0025] According to the input target trajectory duration calculate the final trajectory synchronization duration : , denotes taking and the maximum value in.

[0026] Preferably, the platform trajectory synchronization calculation specifically includes:

[0027] S-shaped speed curve planning parameter calculation: Starting from the position and speed of the last point of the calculated trajectory, perform S-shaped speed curve planning parameter calculation, and calculate the S-trajectory duration based on these parameters; if the S-trajectory duration is a real number and less than the remaining trajectory duration, update the maximum movement speed for each degree of freedom , and re-perform S-shaped speed curve planning parameter calculation; if the S-trajectory duration is a real number and equal to the remaining trajectory duration, perform S-trajectory calculation; if the S-trajectory duration is an imaginary number, calculate the position and speed per millisecond during the process of reducing the speed to 0, update the remaining trajectory duration, and then re-perform S-shaped speed curve planning parameter calculation until the S-trajectory duration is equal to the remaining trajectory duration;

[0028] S - trajectory calculation: Calculate the total number of trajectory points and perform trajectory calculation based on the total number of trajectory points.

[0029] Preferably, the performing trajectory calculation based on the total number of trajectory points includes:

[0030] If the total number of trajectory points is less than the number of trajectory points N that the hardware device can calculate within 2 milliseconds, calculate all the trajectory points of the multi - degree - of - freedom platform per millisecond and perform motor motion trajectory calculation; otherwise, determine whether the currently planned trajectory is the first - stage trajectory. If it is the first - stage trajectory, calculate the trajectory points of the first - stage trajectory and perform the calculation of the first - stage motor motion trajectory. After the calculation of the first - stage motor motion trajectory is completed, re - enter the current step. If the currently planned trajectory is not the first - stage trajectory, calculate the trajectory points of the second - stage trajectory.

[0031] Preferably, the calculation of the trajectory points of the first - stage trajectory includes: If the total number of trajectory points is greater than N but less than N + 100, calculate all the operating trajectories of the multi - degree - of - freedom platform per millisecond; and record the pose, velocity, and acceleration of the Nth trajectory point. If the total number of trajectory points is not less than N + 100, calculate the first N + 100 trajectory points of the multi - degree - of - freedom platform per millisecond and record the pose, velocity, and acceleration of the Nth trajectory point.

[0032] The calculation of the trajectory points of the second - stage trajectory includes: Calculate all the remaining trajectory points of the multi - degree - of - freedom platform per millisecond according to the S - type speed - curve planning parameters after synchronization and the end - point information of the first - stage trajectory (the pose, velocity, and acceleration of the Nth trajectory point).

[0033] Preferably, the dynamically adjusting the jerk and the maximum motion speed during overspeed includes handling the overspeed occurring in the initial acceleration or initial velocity stage. The handling of the overspeed occurring in the initial acceleration or initial velocity stage includes:

[0034] Update the jerk on each degree of freedom : , where is the initial jerk, is the jerk on the first degree of freedom, and are the jerk adjustment coefficients; and The values of are the jerk thresholds;

[0035] Update the maximum motion speed: If the current jerk on the first degree of freedom is not greater than , reset its value to the maximum motion speed calculated in the motion constraint calculation. If it is greater than , update its value to times

[0036] Preferably, the dynamic adjustment of jerk and maximum movement speed during overspeed further includes handling overspeed occurring in the S-trajectory calculation stage. Handling overspeed occurring in the S-trajectory calculation stage includes:

[0037] Reduce to , where is the speed adjustment coefficient; and truncate it after the maximum movement speed is updated.

[0038] In a second aspect, a trajectory planning system for a parallel multi-degree-of-freedom platform is provided, including:

[0039] A motion constraint calculation module for calculating the maximum allowable movement speed on each degree of freedom based on the initial pose, speed, acceleration, and hardware parameters;

[0040] A synchronization duration calculation module for calculating the initial trajectory duration of each degree of freedom using the S-shaped speed curve planning algorithm based on the maximum allowable movement speed on each degree of freedom, and calculating the trajectory synchronization duration in combination with the target trajectory duration;

[0041] A platform trajectory synchronization calculation module for synchronizing the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; wherein, by iteratively adjusting the input parameters of the S-shaped speed curve planning algorithm, trajectory point synchronization and overspeed correction are achieved;

[0042] A motor trajectory calculation module for converting the trajectory data obtained from the platform trajectory synchronization calculation into motor encoder commands, real-time checking whether the speed exceeds the limit, and dynamically adjusting the jerk and maximum movement speed during overspeed.

[0043] Compared with the prior art, the beneficial effects of the present invention are:

[0044] 1. The present invention proposes a multi-degree-of-freedom collaborative planning framework, which realizes synchronous control by dynamically adjusting the input parameters of the S-shaped speed curve planning algorithm, overcoming the problem of ignoring the coupling relationship in traditional methods; by dynamically adjusting the maximum movement speed, it ensures that multiple degrees of freedom complete the movement synchronously, and at the same time, the trajectory duration can be accurately controlled according to requirements; by iteratively adjusting the S-shaped speed curve planning parameters, trajectory point synchronization and overspeed correction are achieved, and at the same time, the jerk or maximum movement speed is dynamically adjusted during overspeed to ensure the safe operation of the motor. The present invention does not require establishing a dynamic model of the system and installing additional sensors, reducing the system cost. In addition, the present invention supports any initial motion state (including speed / acceleration), expanding the application scenario.

[0045] 2. The acceleration of the motion trajectory planned by the present invention is a continuous value, without sudden changes in acceleration, significantly reducing mechanical stress; through millisecond-level precision control and real-time overspeed correction, the motion smoothness and equipment life are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a flowchart of a trajectory planning method for a parallel multi-degree-of-freedom platform shown in an embodiment of the present invention;

[0047] Figure 2 It is a working flowchart of a trajectory planning system for a parallel multi-degree-of-freedom platform shown in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0048] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. The components of the embodiments of the present application described and illustrated herein can be arranged and designed in various different configurations. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0049] It should be noted that all the defects existing in the above prior art solutions are the results obtained by the inventor through practice and careful research. Therefore, the discovery process of the above problems and the solutions proposed by the embodiments of the present application below to the above problems should be the contributions made by the inventor to the present application during the invention creation process, and should not be understood as the technical content known to those skilled in the art.

[0050] For the technical problems pointed out in the background art, the embodiments provided by the present invention are as follows:

[0051] Referring to Figure 1 , in an exemplary embodiment, taking a parallel six-degree-of-freedom platform as an example, a trajectory planning method for a parallel six-degree-of-freedom platform is provided, including the following steps:

[0052] S1. Motion constraint calculation: Based on the initial pose, speed, acceleration, and hardware parameters, calculate the maximum allowable motion speed on each degree of freedom;

[0053] S2. Synchronization duration calculation: Based on the maximum allowable motion speed on each degree of freedom, use the S-shaped speed curve planning algorithm to calculate the initial trajectory duration of each degree of freedom, and calculate the trajectory synchronization duration in combination with the target trajectory duration;

[0054] S3. Platform trajectory synchronization calculation: Synchronize the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; wherein, by iteratively adjusting the input parameters of the S-shaped speed curve planning algorithm, trajectory point synchronization and overspeed correction are achieved.

[0055] S4. Motor trajectory calculation: Convert the trajectory data obtained by synchronous calculation of the platform trajectory into motor encoder commands, check in real time whether the speed exceeds the limit, and dynamically adjust the jerk and maximum movement speed when the speed is excessive.

[0056] Step S1 is mainly used to calculate the maximum movement speed allowed in the degree of freedom i , which is intuitively only related to the movement speed of the current degree of freedom. This speed is also an important parameter to ensure the safe operation of the motor. Step S1 specifically includes:

[0057] S11. According to the input initial pose , speed , acceleration and the jerk on each degree of freedom determined based on the hardware , use the basic kinematic formula to calculate the time required to reduce the initial acceleration to 0 , as well as the speed and position after reducing to 0. Since the process of reducing the acceleration to 0 is linear, the pose, speed, and acceleration changes during this process are all smooth changes without any mutations;

[0058] S12. Calculate the time required to reduce the speed to 0 and the position after reducing to 0:

[0059] ① Calculate the time , position , speed , acceleration when the acceleration reaches the maximum value:

[0060] ② Calculate the position after both the acceleration and speed are reduced to 0:

[0061] ③ Calculate the time required to reduce the speed to 0: ;

[0062] S13. Use the general inverse kinematics algorithm for parallel multi-degree-of-freedom platforms to calculate and the corresponding link lengths of the target pose and , and calculate the maximum movement time of the six electric cylinders in the parallel six-degree-of-freedom platform: ,

[0063] where d is the pitch of the electric cylinder of the six-degree-of-freedom platform; is the rated speed of the motor; is the safety ratio coefficient, which is used to adjust the maximum movement time and the theoretical maximum operating speed of the motor in the trajectory, and this value can be adjusted according to the actual engineering requirements; represents the number of unique position information that the encoder can provide within a single revolution.

[0064] S14. Calculate the initial maximum movement speed for each degree of freedom :

[0065] S15. Calculate the maximum movement speed for each degree of freedom

[0066] In some cases, the initial maximum movement speed for each degree of freedom may approach a relatively small value, resulting in a very long synchronization time for the trajectory and affecting the operating speed of the platform. Therefore, it is also necessary to perform numerical truncation on The truncation value can be adjusted according to the actual engineering requirements:

[0067] In step S2, calculate the actual movement time required for the six-degree-of-freedom platform to complete the entire trajectory. This time is also called the trajectory synchronization duration , specifically including:

[0068] S21. Calculate the initial trajectory duration for each degree of freedom

[0069] ① Taking the pose after reducing the speed to 0 calculated in step S12 as the starting point, according to the maximum movement speed for each degree of freedom calculated in step S1 and the general S-shaped speed curve planning algorithm, calculate the initial S-shaped speed curve planning parameters for each degree of freedom, and calculate the trajectory duration for each degree of freedom based on this parameter .

[0070] ② According to the time required to reduce the speed and acceleration to 0 calculated in S11 and S12, calculate the total trajectory duration for each degree of freedom in the case of reducing the speed to 0: .

[0071] ③ Taking the position and speed after reducing the acceleration to 0 calculated in step S11 as the starting state, according to the maximum movement speed for each degree of freedom calculated in step S1 and the general S-shaped speed curve planning algorithm, calculate the initial S-shaped speed curve planning parameters for each degree of freedom, and calculate the trajectory duration for each degree of freedom based on this parameter .

[0072] ④ Calculate the total trajectory duration for each degree of freedom: .

[0073] S22. Calculate the initial trajectory synchronization duration

[0074] ① Calculate the total trajectory duration without reducing the speed to 0 the maximum value in and the maximum value the corresponding index value , and the total trajectory duration with the speed reduced to 0 the maximum value in .

[0075] ② Calculate the maximum duration in excluding , that is: .

[0076] ③ Calculate the initial trajectory synchronization duration as: .

[0077] S23. Calculate the final trajectory synchronization duration , the final trajectory synchronization duration is the larger value between the initial synchronization duration and the target trajectory duration , that is: .

[0078] The trajectory synchronization duration calculated by the above calculation method can not only meet the synchronization requirements, but also ensure that the six-degree-of-freedom platform can move to the target position in the shortest time as much as possible. At the same time, it can also adjust the maximum movement speed of the motor during the trajectory execution and flexibly adjust the duration of the entire trajectory.

[0079] Step S3 synchronizes the trajectories on the six degrees of freedom according to the trajectory synchronization duration calculated in step S2, so that the trajectories on the six degrees of freedom can all reach the end point at the same time, thereby ensuring that the entire movement trajectory of the platform is a controllable trajectory, specifically including:

[0080] S31. Calculate and process the trajectories in the initial acceleration stage

[0081] ① Judge whether the value of the initial acceleration on each degree of freedom is greater than the control accuracy. If it is less than the control accuracy, go to step S32; otherwise, enter the next process;

[0082] ② Calculate the position and velocity calculation formula for reducing the acceleration to 0 on each degree of freedom according to the basic formula of kinematics;

[0083] ③ Calculate the position (i.e., trajectory information) and velocity of the platform on each degree of freedom per millisecond;

[0084] ④ Calculate the remaining trajectory duration by subtracting the duration of processing the initial acceleration from the trajectory synchronization duration;

[0085] S32. Determine whether to reduce the initial velocity to 0 based on the initial trajectory duration of not reducing the velocity to 0 in each degree of freedom calculated in step S21. If this value is not -1, then there is no need to reduce the initial velocity to 0, and proceed to step S34; otherwise, proceed to step S33;

[0086] S33. Calculate the trajectory for processing the initial velocity

[0087] ① Take the position and velocity of the last point of the calculated trajectory as the starting point (if no trajectory has been calculated yet, the starting velocity and pose are the initial velocity and initial pose), and calculate the position and velocity calculation formula for reducing the velocity to 0 according to the calculation method in S12;

[0088] ② Calculate the position (i.e., trajectory information) and velocity of each degree of freedom of the six-degree-of-freedom platform moving per millisecond during the process of reducing the velocity to 0;

[0089] ③ Update the remaining trajectory duration by subtracting the duration of reducing the velocity to 0 calculated in S12 from the remaining trajectory duration;

[0090] S34. S trajectory synchronization for each degree of freedom

[0091] S341. Take the position and velocity of the last point of the calculated trajectory as the starting point (if no trajectory has been calculated yet, the starting velocity and pose are the initial velocity and initial pose), and calculate the S-shaped velocity curve planning parameters for each degree of freedom using the general S-shaped velocity curve planning algorithm according to the maximum movement velocity calculated in S1, and calculate the S trajectory duration for each degree of freedom; if the S trajectory duration is a real number, that is, it indicates that the S planning is successful, then proceed to S343, otherwise proceed to step S342;

[0092] S342. If the maximum movement velocity in a certain degree of freedom is less than the starting velocity of the plan, or the S planning fails, then use the method of S33 to reduce the velocity to 0, calculate the position (i.e., trajectory information) and velocity per millisecond of this process, and re-enter step S341 after updating the remaining trajectory duration;

[0093] S343. If the S trajectory duration is less than the remaining trajectory duration, then calculate the maximum movement velocity adjustment coefficient , otherwise proceed to step S35; , where is the S trajectory duration, is the remaining trajectory duration; is the maximum movement speed adjustment coefficient, which controls the speed of trajectory synchronization. To prevent the synchronization step size from being too small, resulting in an overly slow synchronization speed for the S-trajectory duration, the present invention truncates the value of i.e.: In engineering practice, the value of is set according to the actual situation (such as 0.5);

[0094] S344. Update the maximum allowable movement speed for each degree of freedom: .

[0095] S345. Re-calculate the S-curve speed profile parameters according to the updated maximum allowable movement speed for each degree of freedom and the general S-curve speed profile algorithm, and update the S-trajectory duration for each degree of freedom. If the S-trajectory duration is a real number, indicating successful S-planning, then enter S343; otherwise, enter step S342.

[0096] S35. S-Trajectory Calculation

[0097] S351. Calculate the total number of trajectory points for each degree of freedom based on the S-curve speed profile parameters after trajectory synchronization calculated in S34 and the trajectories calculated in S31 and S33.

[0098] S352. If the total number of trajectories is less than the number of trajectory points N that the hardware device can calculate within 2 milliseconds (in this implementation, this value is set to 2000), then calculate all the trajectory points of the six-degree-of-freedom platform per millisecond, and then enter step S4; otherwise, determine whether the currently planned trajectory is the first segment of the trajectory. If it is the first segment of the trajectory, then enter step S353; if the currently planned trajectory is not the first segment of the trajectory, then enter step S354.

[0099] S353. Calculate the trajectory points of the first segment of the trajectory

[0100] ① If the number of trajectory points is greater than N but less than N + 100 points, then calculate all the operating trajectories of the six-degree-of-freedom platform per millisecond; and record the pose, speed, and acceleration of the Nth trajectory point.

[0101] ② If the number of trajectory points is not less than N + 100 trajectory points, then calculate the first N + 100 trajectory points of the six-degree-of-freedom platform per millisecond, and record the pose, speed, and acceleration of the Nth trajectory point. Among them, the first N trajectory points are the trajectories that the six-degree-of-freedom platform needs to execute, and the extra calculated trajectory points are to confirm whether the pose near the end of this segment of the trajectory will exceed the safe operating range of the motor when executed. After the calculation is completed, enter step S4.

[0102] S354. Calculate the trajectory points of the second - stage trajectory. Calculate all the remaining trajectory points of the six - degree - of - freedom platform per millisecond based on the S - curve speed - curve planning parameters after trajectory synchronization calculated in S34 and the information of the end point of the first - stage trajectory (the pose, speed, and acceleration of the Nth trajectory point) recorded in S353. After the calculation is completed, proceed to step S4.

[0103] It should be noted that the entire trajectory - planning process is divided into two stages. The first - stage trajectory planning is completed within 2 milliseconds, and the trajectory points of the second - stage trajectory are calculated only after the motor motion trajectory of the first - stage is calculated. The purpose of this design is to enable the platform to receive the trajectory and start moving within 2 milliseconds. While the platform is moving, the system will calculate the remaining trajectory in parallel to reduce the planning delay and improve the system response speed. At the same time, both the first - stage trajectory and the second - stage trajectory are targeted at the same target pose and target time.

[0104] In step S4, calculate the motor motion trajectory of the parallel six - degree - of - freedom platform when executing these trajectories based on the trajectory data per millisecond calculated in step S3. The motor motion trajectory is described by the encoder values of the motor per millisecond. Specifically, it includes:

[0105] S41. Motor motion - trajectory calculation

[0106] S411. Calculate the elongation of the electric cylinder corresponding to each trajectory point (the pose of the platform per millisecond) according to the inverse - solution algorithm of the general parallel six - degree - of - freedom platform.

[0107] S412. Calculate the encoder values of the six motors corresponding to each trajectory point based on the elongation of the electric cylinder and the hardware information of the electric - cylinder motor (pitch, reduction ratio, encoder resolution), etc.

[0108] S413. Calculate the change in encoder values per millisecond based on the encoder values of two consecutive trajectory points, which is the theoretical speed of the motor.

[0109] S414. When the theoretical speed of the motor is greater than the rated speed of the motor, if the current trajectory is the first - stage trajectory, proceed to step S42; if it is the second - stage trajectory, proceed to S45; if the entire trajectory does not exceed the speed limit, output the calculated motor motion trajectory.

[0110] S42. Overspeed handling of the first - stage trajectory

[0111] ① Calculate the maximum value among the index values of the last trajectory points in the six degrees of freedom after processing the initial acceleration and speed based on the results of S31, S33, and S35. ;

[0112] ② If the index value of the first overspeed trajectory point is less than , it indicates that the overspeed situation occurs in the initial acceleration or initial speed processing stage, and proceed to step S43; if the index value of the first overspeed trajectory point is greater than , it indicates that the overspeed situation occurs in the S trajectory calculation stage, then proceed to step S44 for processing;

[0113] S43. Handle the overspeed in the initial acceleration or initial speed stage

[0114] ① Usually, in this stage, the overspeed situation is caused by simultaneous acceleration and deceleration in multiple degrees of freedom, resulting in an excessive operating speed of a certain motor. The processing method adopted in this embodiment is to update the jerk on each degree of freedom , that is: , where is the initial jerk, is the jerk on the first degree of freedom, and are jerk adjustment coefficients, and their values can be adjusted according to actual engineering requirements; and are jerk threshold values, used to control the adjustment method for different jerk ranges, and this value can be adjusted according to actual engineering requirements;

[0115] ② Update the maximum movement speed. If the current jerk on the first degree of freedom is not greater than , then reset its value to the result calculated in step S1. If it is greater than , then update its value to times the current maximum movement speed, can be adjusted according to actual engineering requirements;

[0116] ③ After updating the maximum movement speed, re-enter step S1.

[0117] S44. Handle the overspeed in the S trajectory calculation stage

[0118] ① Usually, the overspeed situation in the S trajectory calculation stage occurs when the maximum movement speed is too large. Therefore, the processing method is to reduce its maximum movement speed, that is, reduce to , where is the speed adjustment coefficient, which can be adjusted according to actual engineering requirements;

[0119] ② After updating this value, it needs to be truncated using the truncation method of S15 to avoid the maximum movement speed being too small;

[0120] ③ After the speed update is completed, proceed to step S2;

[0121] S45. Handle the overspeed that occurs during the calculation of the second - stage trajectory. Use the position, speed, and acceleration of the last point of the first - stage trajectory recorded in step S35 as the planning starting point, and re - enter step S1 to re - execute the entire process.

[0122] After the motor speed is normal, output the calculated motor motion trajectory.

[0123] It should be noted that the method of the present invention is not only applicable to the parallel six - degree - of - freedom platform, but also can be extended to other multi - degree - of - freedom platforms for use.

[0124] In another exemplary embodiment, based on the same inventive concept as the method embodiment, a trajectory planning system for a parallel multi - degree - of - freedom platform is provided, including:

[0125] A motion constraint calculation module, configured to calculate the maximum allowable motion speed on each degree of freedom based on the initial pose, speed, acceleration, and hardware parameters;

[0126] A synchronization duration calculation module, configured to plan the trajectories of each degree of freedom using an S - shaped speed curve based on the maximum allowable motion speed on each degree of freedom, and calculate the trajectory synchronization duration;

[0127] A platform trajectory synchronization calculation module, configured to synchronize the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; wherein, by iteratively adjusting the S - shaped trajectory parameters, trajectory point synchronization and overspeed correction are achieved;

[0128] A motor trajectory calculation module, configured to convert the trajectory data obtained from the platform trajectory synchronization calculation into motor encoder commands, real - time check whether the speed exceeds the limit, and dynamically adjust the jerk and the maximum motion speed when overspeed occurs.

[0129] Refer to Figure 2 , after obtaining the input, each module executes in sequence. The motor trajectory calculation module is also connected to the motion constraint calculation module and the synchronization duration calculation module. In some scenarios, when the motor trajectory calculation module runs, it will jump to the motion constraint calculation module and the synchronization duration calculation module for recalculation.

[0130] The system input is the current pose, speed, acceleration of the upper platform on the parallel six - degree - of - freedom platform, the target pose, and the target motion time. The output result is the encoder value trajectory of the six motors during the entire motion process, that is, the position command of each motor at discrete time points, which is used to drive the platform to smoothly move from the current pose to the target pose according to the input target motion time.

[0131] The above specific embodiments are detailed descriptions of the present invention. It cannot be determined that the specific embodiments of the present invention are only limited to these descriptions. For those of ordinary skill in the technical field to which the present invention pertains, without departing from the concept of the present invention, several simple deductions and substitutions can still be made, and all should be regarded as belonging to the protection scope of the present invention.

Claims

1. A trajectory planning method for a parallel multi-degree-of-freedom platform, characterized in that, Including: Motion constraint calculation: Based on the initial pose, velocity, acceleration, and hardware parameters, calculate the maximum allowable motion speed for each degree of freedom. Calculating the maximum allowable movement speed V in each degree of freedom i max , including: calculating the initial maximum movement speed in each degree of freedom and based on the said calculating the maximum allowable movement speed V in each degree of freedom i max ; Pair Perform numerical truncation: where τ represents the truncation value; Synchronization duration calculation: Based on the maximum allowable motion speed for each degree of freedom, use the S-shaped velocity curve planning algorithm to calculate the initial trajectory duration for each degree of freedom, and calculate the trajectory synchronization duration in combination with the target trajectory duration. Platform trajectory synchronization calculation: Synchronize the trajectories of multiple degrees of freedom according to the trajectory synchronization duration; wherein, by iteratively adjusting the input parameters of the S-shaped velocity curve planning algorithm, trajectory point synchronization and over-speed correction are achieved; the platform trajectory synchronization calculation specifically includes: S-shaped velocity curve planning parameter calculation: Starting from the position and velocity of the last point of the calculated trajectory, perform S-shaped velocity curve planning parameter calculation, and calculate the S-trajectory duration according to this parameter; if the S-trajectory duration is a real number, it indicates that the S-trajectory planning is successful, where when the S-trajectory duration is less than the remaining trajectory duration, calculate the maximum motion speed adjustment coefficient: Among them is the duration of the S trajectory, T i S is the remaining trajectory duration; λ i is the maximum movement speed adjustment coefficient, updating the maximum allowable movement speed for each degree of freedom: V i max = λ i V i max , and recalculate the S-shaped speed curve planning parameters; to prevent the synchronization step size from being too small, resulting in too slow synchronization speed of the S trajectory duration, truncate the value of λ i , that is If the S-trajectory duration is a real number and equal to the remaining trajectory duration, perform the S-trajectory calculation; if the S-trajectory duration is an imaginary number or the maximum motion speed of a certain degree of freedom is less than the planned starting speed or the S-trajectory planning fails, calculate the position and velocity per millisecond during the process of reducing the speed to 0, update the remaining trajectory duration, and then re-perform the S-shaped velocity curve planning parameter calculation until the S-trajectory duration is equal to the remaining trajectory duration. S-trajectory calculation: Calculate the total number of trajectory points, and perform trajectory calculation according to the total number of trajectory points; wherein, the entire trajectory planning process is divided into two segments, the first segment of trajectory planning is completed within 2 milliseconds, and the trajectory points of the second segment are calculated only after the first segment of the motor motion trajectory is calculated. Motor trajectory calculation: Convert the trajectory data obtained from the platform trajectory synchronization calculation into motor encoder commands, real-time check whether the speed exceeds the limit, and dynamically adjust the jerk and maximum motion speed when over-speeding. The dynamically adjusting the jerk and maximum motion speed when over-speeding includes: Handling the over-speed in the first segment of the trajectory: Handling the over-speed that occurs during the initial acceleration or initial velocity stage. Handling the over-speed that occurs during the S-trajectory calculation stage. Handling the over-speed that occurs during the calculation of the second segment of the trajectory.

2. The trajectory planning method of the parallel multi-degree-of-freedom platform according to claim 1, characterized in that The calculating the maximum allowable motion speed for each degree of freedom includes: According to the initial pose P i S , velocity V i S , acceleration and the jerk J on each degree of freedom i , calculate the time T required to reduce the initial acceleration to 0 i A , as well as the velocity V after the initial acceleration is reduced to 0 i A and the position P i A ; Calculate the time T required to reduce the speed to 0 i V and the position P after reducing the speed to 0 i V ; Calculate P using the general inverse solution algorithm for parallel multi-degree-of-freedom platforms i V and the target pose P i E The corresponding connecting rod lengths and Calculate the maximum movement time of each electric cylinder in the parallel multi-degree-of-freedom platform 3. The trajectory planning method of the parallel multi-degree-of-freedom platform according to claim 2, wherein The using the S-shaped velocity curve planning algorithm to calculate the initial trajectory duration for each degree of freedom and calculating the trajectory synchronization duration in combination with the target trajectory duration includes: Calculate the initial trajectory duration for each degree of freedom: Starting from the pose after reducing the speed to 0, calculate the initial S-curve speed profile parameters for each degree of freedom, and calculate the trajectory duration for each degree of freedom based on these parameters Calculate the total trajectory duration for each degree of freedom when reducing the speed to 0: Taking the position and velocity after reducing the acceleration to 0 as the starting state, calculate the initial S-shaped velocity curve planning parameters for each degree of freedom, and calculate the trajectory duration T for each degree of freedom according to these parameters i S ; Total trajectory duration for each degree of freedom: Calculate the initial trajectory synchronization duration: Calculate the maximum value of the total trajectory duration T i init in and the maximum value corresponding index value i max , calculate the maximum value in Calculate excluding i in max the maximum duration Calculate the initial trajectory synchronization duration as: According to the input target trajectory duration T E Calculate the final trajectory synchronization duration T sync : Indicates taking and T E the maximum value in 4. The trajectory planning method of the parallel multi-degree-of-freedom platform according to claim 1, characterized in that The performing trajectory calculation according to the total number of trajectory points includes: If the total number of trajectory points is less than the number of trajectory points N that the hardware device can calculate within 2 milliseconds, calculate all the trajectory points of the multi-degree-of-freedom platform per millisecond, and perform the motor motion trajectory calculation; otherwise, judge whether the currently planned trajectory is the first segment of the trajectory. If it is the first segment of the trajectory, calculate the trajectory points of the first segment and perform the calculation of the first segment of the motor motion trajectory; after the calculation of the first segment of the motor motion trajectory is completed, re-enter the current step; if the currently planned trajectory is not the first segment of the trajectory, calculate the trajectory points of the second segment.

5. The trajectory planning method of the parallel multi-degree-of-freedom platform according to claim 4, wherein The calculation of the trajectory points of the first - stage trajectory includes: if the total number of trajectory points is greater than N but less than N + 100, calculate all the running trajectories of the multi - degree - of - freedom platform per millisecond; and record the pose, velocity, and acceleration of the Nth trajectory point; if the total number of trajectory points is not less than N + 100, calculate the first N + 100 trajectory points of the multi - degree - of - freedom platform per millisecond, and record the pose, velocity, and acceleration of the Nth trajectory point. The calculation of the trajectory points of the second - stage trajectory includes: according to the S - shaped velocity curve planning parameters after synchronization and the end - point information of the first - stage trajectory, calculate all the remaining trajectory points of the multi - degree - of - freedom platform per millisecond, and the end - point information of the first - stage trajectory includes the pose, velocity, and acceleration of the Nth trajectory point.

6. The trajectory planning method for a parallel multi-degree-of-freedom platform according to claim 1, wherein The overspeed handling during the initial acceleration or initial velocity stage includes: Update the jerk in each degree of freedom J i : Among them, is the initial jerk, J1 is the jerk on the first degree of freedom, and α1 and α2 are jerk adjustment coefficients; the values of τ1 and τ2 are jerk thresholds; Update the maximum motion speed: if the current jerk on the first degree of freedom is not greater than τ2, reset its value to the maximum motion speed calculated in the motion constraint calculation; if it is greater than τ2, update its value to ω times the current maximum motion speed.

7. The trajectory planning method of the parallel multi-degree-of-freedom platform according to claim 1, characterized in that The overspeed handling during the S - trajectory calculation stage includes: Reduce V i max to μV i max , where μ is the speed adjustment coefficient; and truncate it after the maximum movement speed is updated.

8. A parallel multi-degree-of-freedom platform trajectory planning system for implementing the parallel multi-degree-of-freedom platform trajectory planning method according to any one of claims 1-7, characterized in that including: A motion constraint calculation module for calculating the maximum allowable motion speed on each degree of freedom based on the initial pose, velocity, acceleration, and hardware parameters. A synchronization duration calculation module for calculating the initial trajectory duration of each degree of freedom using the S - shaped velocity curve planning algorithm based on the maximum allowable motion speed on each degree of freedom, and calculating the trajectory synchronization duration in combination with the target trajectory duration. A platform trajectory synchronization calculation module for synchronizing the trajectories on multiple degrees of freedom according to the trajectory synchronization duration; among them, by iteratively adjusting the input parameters of the S - shaped velocity curve planning algorithm, trajectory point synchronization and overspeed correction are realized. A motor trajectory calculation module for converting the trajectory data obtained from the platform trajectory synchronization calculation into motor encoder commands, real - time checking whether the speed exceeds the limit, and dynamically adjusting the jerk and maximum motion speed when overspeed occurs.

Images