Parallel multi-degree-of-freedom platform trajectory planning method and system

Abstract

The invention discloses a parallel multi-degree-of-freedom platform trajectory planning method and system, and belongs to the technical field of trajectory planning. The parallel multi-degree-of-freedom platform trajectory planning method comprises the steps that the allowable maximum movement speed on each degree of freedom is calculated; based on the allowable maximum movement speed on each degree of freedom, calculating the initial trajectory duration of each degree of freedom by adopting an S-shaped speed curve planning algorithm, and calculating the trajectory synchronization duration in combination with the target trajectory duration; synchronizing the tracks on the multiple degrees of freedom according to the track synchronization duration; whether the speed exceeds the limit or not is verified in real time, and the jerk and the maximum movement speed are dynamically adjusted when the speed exceeds the limit. And rapid planning of the full-period motion track of the parallel multi-degree-of-freedom platform is realized through collaborative planning on multiple degrees of freedom. Under the condition that the working plane constraint of the Cartesian space is guaranteed, millisecond-level precision control can be conducted on the pose of the platform, the problems of elastic deformation and residual stress of a mechanical structure are effectively avoided, the service life of the mechanism is prolonged, and operation noise is reduced.

Description

Technical Field

[0001] The present invention relates to the field of trajectory planning technology, and in particular 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 limited by volume, load-bearing capacity and error accumulation, which makes it difficult to meet the needs of modern industry. The parallel six-degree-of-freedom mechanism has gradually been applied to the field of precision motion control due to its advantages such as high control accuracy, strong load-bearing 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 the 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 robot's work efficiency, motion stability and energy consumption. In this field, the motion trajectory of the platform is usually generated by a control strategy based on the hinge space, that is, it is assumed in advance that the driving branches are independent and non-coupled, and the kinematic mathematical model (kinematic inverse 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 position and the end position of the platform movement are far apart, the ends of the six connecting rods may not form a plane in the Cartesian workspace, resulting in mechanical deformation between the connecting rod hinge and the base, or the electric cylinder spins 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 when the parallel six-degree-of-freedom platform reaches the target position and the posture at every moment during the movement, reducing its flexibility and reliability. There are also some trajectory planning algorithms that calculate the starting and ending positions of each link through inverse solution, and divide the entire trajectory into small segments through interpolation. However, this type of method still has the problem of contact deformation in a small range, and the theoretical speed of the connection point of the trajectory segment is 0, which increases energy consumption and reduces the smoothness of the platform movement. The control strategies based on hinge space used in many projects can only handle trajectory planning with a stationary starting point or a short distance. They cannot adapt to scenarios where the platform starting point has an initial velocity and acceleration or the starting and ending points are far apart, and their scope and field of application are limited.

[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 installation of additional sensors that can measure the real-time position and posture of the dynamic platform, which greatly increases the cost of the control system. In addition, this type of algorithm usually has high computational complexity, and the control accuracy of the platform depends on the accuracy of the sensor, which cannot fully meet the needs of industrial applications.

[0005] In addition, some parallel six-degree-of-freedom platform trajectory planning methods directly output the motion instructions of each electric cylinder without considering whether the motor can control the electric cylinder to complete these motion instructions; even in the scenario where the parallel six-degree-of-freedom platform is moving at a uniform speed, some electric cylinders will accelerate due to the rotation of the platform angle, which will cause the control motor current or operation to exceed the safe motion range, resulting in the platform being unable to execute normally according to the planned trajectory. The trajectory planning algorithms of some parallel six-degree-of-freedom platforms use the S-type velocity curve planning algorithm to calculate the motion trajectory on each degree of freedom and synchronize the trajectory. However, most of these existing methods assume that the S-type velocity curve planning algorithm will definitely be able to calculate successfully, and only consider the ideal state of each degree of freedom when calculating the synchronization time, lacking consideration of the situation where the S-type velocity curve planning algorithm fails to plan and the trajectory synchronization fails; at the same time, most of these algorithms pursue the minimum trajectory duration, and cannot control the trajectory duration according to actual needs, and their engineering application value is limited. Summary of the invention

[0006] The purpose of the present invention is to overcome the technical problems existing in the prior art and provide a parallel multi-degree-of-freedom platform trajectory planning method and system, which can plan the motion trajectory of the parallel multi-degree-of-freedom platform with short motion time, smooth motion trajectory, safe and controllable motion trajectory, controllable total trajectory time and multiple degrees of freedom being able to reach the target posture at the same time according to the target posture and target motion time when the parallel multi-degree-of-freedom platform is in any motion state.

[0007] The objective of the present invention is achieved through the following technical solutions: In a first aspect, a parallel multi-degree-of-freedom platform trajectory planning method is provided, comprising: Motion constraint calculation: Based on the initial position, velocity, acceleration and hardware parameters, calculate the maximum motion speed allowed for each degree of freedom; Synchronization duration calculation: Based on the maximum movement speed allowed in each degree of freedom, the S-type speed 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; 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; 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.

[0008] Preferably, the calculation of the maximum movement speed allowed in each degree of freedom includes: 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 ; Calculate the time required to reduce the speed to 0 and the position after it drops to 0 ; 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 ; 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 .

[0009] Preferably, according to the Calculate the maximum velocity of motion in each degree of freedom ,include: right Perform numerical truncation: ,in, Indicates the cutoff value.

[0010] 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: Calculate the initial trajectory duration on each degree of freedom: Taking the position after the speed is reduced to 0 as the starting point, calculate the initial S-shaped velocity curve planning parameters on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters ; Calculate the total trajectory duration on each degree of freedom when the speed is reduced to 0: , taking the position and velocity after the acceleration is reduced to 0 as the starting state, calculate the initial S-shaped velocity curve planning parameters on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters ; Total trajectory duration in each degree of freedom: ; Calculate the initial trajectory synchronization duration: Calculate the total trajectory duration The maximum value in and maximum value The corresponding index value ,calculate The maximum value in ,calculate Not included Maximum duration : ; Calculate the initial trajectory synchronization duration as: ; According to the input target trajectory duration Calculate the final trajectory synchronization duration : , Indicates taking and The maximum value in .

[0011] Preferably, the platform trajectory synchronous calculation specifically includes: S-type speed curve planning parameter calculation: Take the calculated position and speed of the last point of the trajectory as the starting point, calculate the S-type speed curve planning parameters, and calculate the S trajectory duration based on the parameters; if the S trajectory duration is a real number and is less than the remaining trajectory duration, update the maximum motion speed on each degree of freedom , and recalculate the S-type speed curve planning parameters; 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, calculate the position and speed per millisecond in the process of reducing the speed to 0, update the remaining trajectory duration, and then recalculate the S-type speed curve planning parameters 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 based on the total number of trajectory points.

[0012] Preferably, the 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, all trajectory points of the multi-degree-of-freedom platform per millisecond are calculated, and the motor motion trajectory is calculated; otherwise, it is determined whether the currently planned trajectory is the first segment of the trajectory. If it is the first segment of the trajectory, the trajectory points of the first segment of the trajectory are calculated and the calculation of the first segment of the motor motion trajectory is performed; 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, the trajectory points of the second segment of the trajectory are calculated.

[0013] Preferably, the calculation of the trajectory points of the first segment of the trajectory includes: if the total number of trajectory points is greater than N but less than N+100, then calculating all running trajectories of the multi-degree-of-freedom platform per millisecond; and recording the position, velocity and acceleration of the Nth trajectory point; if the total number of trajectory points is not less than N+100, then calculating the first N+100 trajectory points of the multi-degree-of-freedom platform per millisecond, and recording the position, velocity and acceleration of the Nth trajectory point; The trajectory point calculation of the second trajectory includes: calculating all remaining trajectory points of the multi-degree-of-freedom platform per millisecond based on the S-shaped velocity curve planning parameters after synchronization and the endpoint information of the first trajectory (the position, velocity and acceleration of the Nth trajectory point).

[0014] Preferably, the dynamically adjusting the jerk and the maximum movement speed during overspeeding includes processing overspeeding occurring during an initial acceleration or initial speed stage, and the processing overspeeding occurring during an initial acceleration or initial speed stage includes: Update the jerk for each degree of freedom : ,in, is the initial acceleration, is the jerk in the first degree of freedom, and is the acceleration adjustment factor; and The value of is the acceleration threshold; 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. , then update its value to the current maximum movement speed times.

[0015] Preferably, the dynamically adjusting the jerk and the maximum movement speed when overspeeding further includes processing the overspeeding occurring in the S trajectory calculation stage, and the processing of the overspeeding occurring in the S trajectory calculation stage includes: Will Reduced to ,in, is the speed adjustment factor; and is truncated after the maximum movement speed is updated.

[0016] In a second aspect, a parallel multi-degree-of-freedom platform trajectory planning system is provided, comprising: The motion constraint calculation module is used to calculate the maximum motion speed allowed in each degree of freedom based on the initial posture, velocity, acceleration and hardware parameters; The synchronization duration calculation module is used to calculate the initial trajectory duration of each degree of freedom based on the maximum movement speed allowed in each degree of freedom, using the S-type speed curve planning algorithm, and calculate the trajectory synchronization duration in combination with the target trajectory duration; A platform trajectory synchronization calculation module is used to synchronize 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; The motor trajectory calculation module is used to 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.

[0017] Compared with the prior art, the present invention has the following beneficial effects: 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-type speed curve planning algorithm, overcoming the problem of ignoring the coupling relationship in the traditional method; by dynamically adjusting the maximum movement speed, it ensures that multiple degrees of freedom complete the movement synchronously, and the trajectory duration can be accurately controlled according to the needs; by iteratively adjusting the S-type speed curve planning parameters, trajectory point synchronization and overspeed correction are achieved, and the acceleration or maximum movement speed is dynamically adjusted when overspeeding to ensure the safe operation of the motor. The present invention does not need to establish a dynamic model of the system and install additional sensors, thereby reducing the system cost. In addition, the present invention supports any initial motion state (including speed / acceleration), which expands the application scenarios.

[0018] 2. The motion trajectory acceleration planned by the present invention is a continuous value, there is no sudden change in acceleration, and the mechanical stress is significantly reduced; through millisecond-level precision control and real-time overspeed correction, the motion stability and equipment life are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A flow chart of a trajectory planning method for a parallel multi-degree-of-freedom platform shown in an embodiment of the present invention; Figure 2 The present invention is a flowchart of a parallel multi-degree-of-freedom platform trajectory planning system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0020] The technical solution of the present invention is clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. The components of the embodiments of the present application described and shown in the accompanying drawings can be arranged and designed in various configurations. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0021] It should be noted that the defects existing in the solutions in the above-mentioned prior art are the results obtained by the inventor after practice and careful research. Therefore, the discovery process of the above-mentioned problems and the solutions proposed in the embodiments of the present application for the above-mentioned problems below should all be the contributions made by the inventor to the present application in the process of invention and creation, and should not be understood as technical contents known to technical personnel in this field.

[0022] In view of the technical problems pointed out in the background technology, the embodiments provided by the present invention are as follows: Reference Figure 1 In an exemplary embodiment, taking a parallel six-degree-of-freedom platform as an example, a parallel six-degree-of-freedom platform trajectory planning method is provided, comprising the following steps: S1. Motion constraint calculation: Based on the initial position, velocity, acceleration and hardware parameters, calculate the maximum motion speed allowed in each degree of freedom; S2, synchronization duration calculation: Based on the maximum movement speed allowed in each degree of freedom, the S-type speed 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; S3, 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; S4, 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 it exceeds the limit.

[0023] Step S1 is mainly used to calculate the maximum movement speed allowed on degree of freedom i Although this value 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: S11, according to the input initial posture ,speed , acceleration and the jerk in each degree of freedom determined by the hardware , use the basic formula of kinematics to calculate the time required for the initial acceleration to drop to zero , and the speed after it drops to 0 and location Since the process of reducing the acceleration to 0 is linear, the changes in posture, velocity and acceleration in this process are all smooth changes without any sudden changes; S12. Calculate the time required to reduce the speed to 0 and the position after it drops to 0 : ①Calculate the time when the acceleration reaches the maximum value ,Location ,speed , acceleration :

[0024] ②Calculate the position after both acceleration and velocity drop to 0 :

[0025] ③Calculate the time required to reduce the speed to 0 : ; S13, using the general parallel multi-degree-of-freedom platform inverse solution algorithm to calculate and target pose Corresponding connecting rod length and , calculate the maximum motion time of the six electric cylinders in the parallel six-degree-of-freedom platform : , Where, d is the pitch of the electric cylinder of the six-degree-of-freedom platform; is the rated speed of the motor; It is a safety proportional coefficient, which is used to adjust the maximum motion time and the theoretical maximum operating speed of the motor in the trajectory. This value can be adjusted according to actual engineering needs; Indicates the number of unique position information that an encoder can provide within a single revolution.

[0026] S14. Calculate the initial maximum motion speed on each degree of freedom :

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

[0028] The initial maximum motion speed of each degree of freedom may approach a relatively small value in some cases, which will cause the trajectory synchronization time to become very long and affect the platform's running speed. Therefore, it is also necessary to Perform numerical truncation, truncation value Can be adjusted according to actual project needs: .

[0029] In step S2, the actual motion time required for the six-degree-of-freedom platform to complete the entire trajectory is calculated, which is also called the trajectory synchronization duration. , specifically including: S21. Calculate the initial trajectory duration on each degree of freedom ① Taking the position calculated in step S12 after the speed is reduced to 0 as the starting point, according to the maximum movement speed of 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 on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters .

[0030] ② According to the time required to reduce the speed and acceleration to 0 calculated by S11 and S12, calculate the total trajectory duration on each degree of freedom when the speed is reduced to 0: .

[0031] ③ Taking the position and speed calculated in step S11 after the acceleration is reduced to 0 as the starting state, according to the maximum movement speed on 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 on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters .

[0032] ④ Calculate the total trajectory duration on each degree of freedom: .

[0033] S22, calculate the initial trajectory synchronization duration ① Calculate the total trajectory duration without reducing the speed to 0 The maximum value in and maximum value The corresponding index value , and the total trajectory duration for reducing the speed to 0 The maximum value in .

[0034] ②Calculation Not included Maximum duration ,Right now: .

[0035] ③ Calculate the initial trajectory synchronization duration: .

[0036] S23, calculate the final trajectory synchronization duration , the final trajectory synchronization duration Initial synchronization duration and target trajectory duration The larger value of , that is: .

[0037] The trajectory synchronization duration calculated using 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 possible. At the same time, it can also adjust the maximum movement speed of the motor during trajectory execution and flexibly adjust the duration of the entire trajectory.

[0038] 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 reach the end point at the same time, thereby ensuring that the entire motion trajectory of the platform is a controllable trajectory, specifically including: S31, calculate and process the trajectory of the initial acceleration stage ① Determine 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, proceed to step S32; otherwise, proceed to the next process; ② According to the basic formula of kinematics, calculate the position and velocity calculation formula to reduce the acceleration of each degree of freedom to zero; ③ Calculate the position (i.e. trajectory information) and speed of the platform in each degree of freedom per millisecond; ④ Calculate the remaining trajectory duration by subtracting the duration of processing the initial acceleration from the trajectory synchronization duration; S32, judging whether it is necessary to reduce the initial speed to 0 according to the initial trajectory duration of each degree of freedom calculated in step S21 without reducing the speed to 0, if the value is not -1, it is not necessary to reduce the initial speed to 0, and proceeding to step S34; otherwise, proceeding to step S33; S33, calculate and process the trajectory of the initial velocity ① Taking the position and velocity of the last point of the calculated trajectory as the starting point (if no trajectory has been calculated, the starting velocity and posture are the initial velocity and initial posture), calculate the position and velocity calculation formula that reduces the velocity to 0 according to the calculation method in S12; ② Calculate the position (i.e. trajectory information) and speed of the six-degree-of-freedom platform in each degree of freedom per millisecond in the process of reducing the speed to 0; ③ Subtract the time for reducing the speed to 0 calculated in S12 from the remaining trajectory time to update the remaining trajectory time; S34, S trajectory synchronization on each degree of freedom S341, taking the position and speed of the last point of the calculated trajectory as the starting point (if no trajectory has been calculated, the starting speed and posture are the initial speed and initial posture), according to the maximum motion speed calculated in S1, the general S-type velocity curve planning algorithm is used to calculate the S-type velocity curve planning parameters for each degree of freedom, and the S trajectory duration on each degree of freedom is calculated; if the S trajectory duration is a real number, it indicates that the S planning is successful, then enter S343, otherwise enter step S342; S342: If the maximum motion speed on a certain degree of freedom is less than the planned starting speed, or the S-planning fails, the speed is reduced to 0 using the method of S33, the position (i.e., trajectory information) and speed of the process are calculated every millisecond, and the remaining trajectory duration is updated before reentering step S341; S343: If the duration of the S track is less than the duration of the remaining track, calculate the maximum movement speed adjustment coefficient , otherwise go to step S35; ,in is the duration of the S trajectory, is the remaining trajectory duration; is the maximum motion speed adjustment coefficient, which controls the speed of trajectory synchronization; in order to prevent the synchronization step from being too small, which leads to the synchronization speed of the S trajectory length being too slow, the present invention adjusts The value is truncated, that is: In engineering practice, The value of is set according to the actual situation (such as 0.5); S344. Update the maximum movement speed allowed for each degree of freedom: .

[0039] S345, recalculate the S-type speed curve planning parameters according to the updated maximum movement speed allowed on each degree of freedom and the general S-type speed curve planning algorithm, and update the S track duration on each degree of freedom. If the S track duration is a real number, it indicates that the S planning is successful, then go to S343, otherwise go to step S342.

[0040] S35, S trajectory calculation S351, calculating the total number of trajectory points on each degree of freedom according to the S-shaped speed curve planning parameters after trajectory synchronization calculated in S34 and the trajectory calculated in S31 and S33.

[0041] 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), all trajectory points of the six-degree-of-freedom platform are calculated per millisecond, and then step S4 is entered; otherwise, it is determined whether the currently planned trajectory is the first segment of the trajectory. If it is the first segment of the trajectory, step S353 is entered; if the currently planned trajectory is not the first segment of the trajectory, step S354 is entered.

[0042] S353, calculate the trajectory points of the first trajectory ① If the number of trajectory points is greater than N but less than N+100 points, calculate all the running trajectories of the six-degree-of-freedom platform per millisecond; and record the position, velocity and acceleration of the Nth trajectory point; ② If the number of trajectory points is not less than N+100 trajectory points, calculate the first N+100 trajectory points of the six-degree-of-freedom platform per millisecond, and record the posture, velocity and acceleration of the Nth trajectory point. The first N trajectory points are the trajectories that the six-degree-of-freedom platform needs to execute, and the additionally calculated trajectory points are to confirm whether the posture near the end point of the trajectory will exceed the safe operating range of the motor when it is executed. After the calculation is completed, go to step S4.

[0043] S354, calculate the trajectory points of the second trajectory, and calculate all the remaining trajectory points of the six-degree-of-freedom platform per millisecond based on the S-shaped velocity curve planning parameters calculated in S34 after trajectory synchronization and the endpoint information of the first trajectory recorded in S353 (the position, velocity and acceleration of the Nth trajectory point). After the calculation is completed, enter step S4.

[0044] It should be noted that the entire trajectory planning process is divided into two sections. The first section of trajectory planning is completed within 2 milliseconds. The trajectory points of the second section of the trajectory are calculated only after the first section of the motor motion trajectory is calculated. The purpose of this design is to allow the platform to receive the trajectory and move within 2 milliseconds. When the platform moves, the system will calculate the remaining trajectories in parallel to reduce planning delays and improve system response speed. At the same time, both the first and second sections of the trajectory are aimed at the same target posture and target time.

[0045] In step S4, the motion trajectory of the motor of the parallel six-degree-of-freedom platform when executing these trajectories is calculated based on the trajectory data per millisecond calculated in step S3, and the motion trajectory of the motor is described by the encoder value per millisecond of the motor. Specifically, it includes: S41, motor motion trajectory calculation S411, calculating the elongation of the electric cylinder corresponding to each trajectory point (the position of the platform per millisecond) according to the inverse solution algorithm of the general parallel six-degree-of-freedom platform; S412, calculating the encoder values ​​of the six motors corresponding to each trajectory point according to the extension amount of the electric cylinder and the hardware information of the electric cylinder motor (pitch, reduction ratio, encoder resolution, etc.); S413, calculating the change of the encoder value per millisecond according to the encoder values ​​of two consecutive trajectory points, which is the theoretical speed of the motor; S414, when the motor theoretical speed is greater than the motor rated speed, if the current trajectory is the first segment trajectory, then enter step S42, if it is the second segment trajectory, then enter S45; if the entire trajectory is not overspeeding, then output the calculated motor motion trajectory.

[0046] S42, overspeed processing of the first track ① Calculate the maximum value of the last trajectory point index value after processing the initial acceleration and velocity in the six degrees of freedom based on the results of S31, S33 and S35 ; ②If the first speeding trajectory point index value is less than , it indicates that the overspeed situation occurs in the initial acceleration or initial speed processing stage, and the process goes to step S43; if the first overspeed trajectory point index value is greater than , it indicates that the overspeed situation occurs in the S trajectory calculation stage, and then the process goes to step S44; S43, handle overspeed during initial acceleration or initial speed stage ① The overspeed situation at this stage is usually caused by multiple degrees of freedom accelerating and decelerating at the same time, which causes a motor to run too fast. The processing method adopted in this embodiment is to update the acceleration on each degree of freedom. ,Right now: ,in, is the initial acceleration, is the jerk in the first degree of freedom, and It is the acceleration adjustment coefficient, and its value can be adjusted according to actual engineering requirements; and The value is the jerk threshold, which is used to control the adjustment method of different jerk ranges. This value can be adjusted according to actual engineering needs; ② Update the maximum motion speed. If the current jerk on the first degree of freedom is not greater than , reset its value to the result calculated in step S1. , then update its value to the current maximum movement speed times, The value can be adjusted according to actual engineering requirements; ③After updating the maximum movement speed, re-enter step S1.

[0047] S44, processing of overspeed in the S trajectory calculation stage ① The overspeed situation in the S trajectory calculation stage usually occurs when the maximum movement speed is too high. Therefore, the solution is to reduce its maximum movement speed. Reduced to ,in, It is the speed adjustment coefficient, which can be adjusted according to actual engineering needs; ②After the value is updated, it needs to be truncated using the S15 truncation method to avoid the maximum movement speed being too small; ③ After the speed update is completed, go to step S2; S45, processing the overspeed that occurs when calculating the second segment of the trajectory, taking the position, speed and acceleration of the last point of the first segment of the trajectory recorded in step S35 as the planning starting point, re-entering step S1 and re-executing the entire process.

[0048] When the motor speed is normal, the calculated motor motion trajectory is output.

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

[0050] In another exemplary embodiment, based on the same inventive concept as the method embodiment, a parallel multi-degree-of-freedom platform trajectory planning system is provided, comprising: The motion constraint calculation module is used to calculate the maximum motion speed allowed in each degree of freedom based on the initial posture, velocity, acceleration and hardware parameters; The synchronization duration calculation module is used to plan the trajectory of each degree of freedom based on the maximum movement speed allowed in each degree of freedom using an S-shaped speed curve and calculate the trajectory synchronization duration; A platform trajectory synchronization calculation module is used to synchronize 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 S-shaped trajectory parameters; The motor trajectory calculation module is used to 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.

[0051] Reference Figure 2 After obtaining the input, each module is executed in sequence, among which 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 is running, it will jump to the motion constraint calculation module and the synchronization duration calculation module for recalculation.

[0052] The system input is the current posture, velocity, acceleration, target posture and target motion time of the platform on the parallel six-degree-of-freedom platform. 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 a discrete time point, which is used to drive the platform to move smoothly from the current posture to the target posture according to the input target motion time.

[0053] The above specific implementation methods are detailed descriptions of the present invention. It cannot be determined that the specific implementation methods of the present invention are limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, several simple deductions and substitutions can be made without departing from the concept of the present invention, which should be regarded as belonging to the protection scope of the present invention.

Claims

1. A parallel multi-degree-of-freedom platform trajectory planning method, characterized in that: include: Motion constraint calculation: Based on the initial position, velocity, acceleration and hardware parameters, calculate the maximum motion speed allowed for each degree of freedom; Synchronization duration calculation: Based on the maximum movement speed allowed in each degree of freedom, the S-type speed 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; 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; 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.

2. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 1, characterized in that: The calculation of the maximum movement speed allowed in each degree of freedom includes: According to the initial posture ,speed , acceleration and the jerk in each degree of freedom , calculate the time required to reduce the initial acceleration to 0 , and the speed after the initial acceleration drops to 0 and location ; Calculate the time required to reduce the speed to 0 and the position after it drops to 0 ; 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 ; 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 .

3. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 2, characterized in that: According to the Calculate the maximum velocity of motion in each degree of freedom ,include: right Perform numerical truncation: ,in, Indicates the cutoff value.

4. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 2, characterized in that: 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: Calculate the initial trajectory duration on each degree of freedom: Taking the position after the speed is reduced to 0 as the starting point, calculate the initial S-shaped velocity curve planning parameters on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters ; Calculate the total trajectory duration on each degree of freedom when the speed is reduced to 0: , taking the position and velocity after the acceleration drops to 0 as the starting state, calculate the initial S-shaped velocity curve planning parameters on each degree of freedom, and calculate the trajectory duration on each degree of freedom based on the parameters ; Total trajectory duration in each degree of freedom: ; Calculate the initial trajectory synchronization duration: Calculate the total trajectory duration The maximum value in and maximum value The corresponding index value ,calculate The maximum value in ,calculate Not included Maximum duration : ; Calculate the initial trajectory synchronization duration as: ; According to the input target trajectory duration Calculate the final trajectory synchronization duration : , Indicates taking and The maximum value in .

5. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 4, characterized in that: The platform trajectory synchronous calculation specifically includes: S-type speed curve planning parameter calculation: Take the calculated position and speed of the last point of the trajectory as the starting point, calculate the S-type speed curve planning parameters, and calculate the S trajectory duration based on the parameters; if the S trajectory duration is a real number and is less than the remaining trajectory duration, update the maximum motion speed on each degree of freedom , and recalculate the S-type speed curve planning parameters; 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, calculate the position and speed per millisecond in the process of reducing the speed to 0, update the remaining trajectory duration, and then recalculate the S-type speed curve planning parameters 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 based on the total number of trajectory points.

6. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 5, characterized in that: The 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, all trajectory points of the multi-degree-of-freedom platform per millisecond are calculated, and the motor motion trajectory is calculated; otherwise, it is determined whether the currently planned trajectory is the first segment of the trajectory. If it is the first segment of the trajectory, the trajectory points of the first segment of the trajectory are calculated and the calculation of the first segment of the motor motion trajectory is performed; 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, the trajectory points of the second segment of the trajectory are calculated.

7. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 6, characterized in that: The trajectory point calculation of the first trajectory includes: if the total number of trajectory points is greater than N but less than N+100, then all running trajectories of the multi-degree-of-freedom platform per millisecond are calculated; and the position, velocity and acceleration of the Nth trajectory point are recorded; if the total number of trajectory points is not less than N+100, then the first N+100 trajectory points of the multi-degree-of-freedom platform per millisecond are calculated, and the position, velocity and acceleration of the Nth trajectory point are recorded; The trajectory point calculation of the second trajectory includes: calculating all remaining trajectory points of the multi-degree-of-freedom platform per millisecond based on the S-shaped velocity curve planning parameters after synchronization and the endpoint information of the first trajectory (the position, velocity and acceleration of the Nth trajectory point).

8. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 7, characterized in that: The dynamically adjusting the jerk and the maximum motion speed when overspeeding includes processing the overspeeding occurring in the initial acceleration or initial speed stage, and the processing of the overspeeding occurring in the initial acceleration or initial speed stage includes: Update the jerk for each degree of freedom : ,in, is the initial acceleration, is the jerk in the first degree of freedom, and is the acceleration adjustment factor; and The value of is the acceleration threshold; Update the maximum motion speed: If the current acceleration on the first degree of freedom is not greater than , reset its value to the maximum motion speed calculated in the motion constraint calculation. , then update its value to the current maximum movement speed times.

9. The parallel multi-degree-of-freedom platform trajectory planning method according to claim 7, characterized in that: The dynamically adjusting the jerk and the maximum motion speed when overspeeding also includes processing the overspeeding that occurs in the S trajectory calculation stage, and the processing of the overspeeding that occurs in the S trajectory calculation stage includes: Will Reduced to ,in, is the speed adjustment factor; and is truncated after the maximum movement speed is updated.

10. A parallel multi-degree-of-freedom platform trajectory planning system, characterized in that: include: The motion constraint calculation module is used to calculate the maximum motion speed allowed in each degree of freedom based on the initial posture, velocity, acceleration and hardware parameters; The synchronization duration calculation module is used to calculate the initial trajectory duration of each degree of freedom based on the maximum movement speed allowed in each degree of freedom, using the S-type speed curve planning algorithm, and calculate the trajectory synchronization duration in combination with the target trajectory duration; A platform trajectory synchronization calculation module is used to synchronize 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; The motor trajectory calculation module is used to 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.

Images