A control method and system for five-axis axis-constrained reverse correction path propulsion amount

Abstract

This invention discloses a control method and system for five-axis constraint reverse correction path advance amount, relating to the field of five-axis CNC machine tool motion control technology. The method includes: performing a forward mapping solution using the theoretical path advance amount to be constrained and corrected to obtain a single-cycle axis pulse increment; performing axis constraint verification and judgment based on the single-cycle axis pulse increment combined with axis increment over-limit judgment conditions to obtain the axis constraint verification and judgment result; and reverse correcting the theoretical path advance amount to be constrained and corrected according to the axis constraint verification and judgment result to obtain the control result of the five-axis constraint reverse correction path advance amount. This invention constructs a closed-loop iterative architecture of forward mapping, constraint verification, and reverse correction. By scaling the path advance amount as a whole instead of independent speed adjustment for each axis, it solves the industry pain points of multi-axis asynchrony and inaccurate point triggering in five-axis dispensing arcs and corner trajectories, while simultaneously considering trajectory fidelity, equipment hardware constraints, and embedded real-time computing performance.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for five-axis CNC machine tools, and in particular to a control method and system for the advance amount of a five-axis axis constraint reverse correction path. Background Technology

[0002] Five-axis CNC dispensing, particularly for machining irregularly shaped trajectories such as arcs and corners, demands extremely high precision in trajectory position triggering and multi-axis motion synchronization, directly determining the uniformity of the adhesive path and the yield rate of the packaged product. Currently, common five-axis CNC systems generally employ a hierarchical control architecture of "upper-level trajectory planning + lower-level controller speed look-ahead." The upper-level CNC module generates fixed theoretical trajectories, synthesized speeds, and path advancement parameters, which are then sent down to the embedded motion controller. The controller's built-in look-ahead algorithm handles speed smoothing and acceleration / deceleration adjustments. This architecture has inherent flaws in actual dispensing production, easily leading to inaccurate triggering of arc and corner trajectory positions and trajectory distortion, severely restricting the precision of precision dispensing.

[0003] The core pain point lies in the disconnect between the upper and lower control logic: the synthesis speed, trajectory length increment and path advancement of the upper layer are fixed theoretical parameters, while the speed look-ahead layer of the lower motion controller will arbitrarily tamper with the real-time running speed of a single axis. There is no unified five-axis linkage speed constraint logic, which directly destroys the multi-axis speed matching relationship. This often results in asynchronous problems where a single axis arrives in advance while the other axes arrive in a delayed state. Ultimately, this causes arc curvature offset, corner radius distortion, and inaccurate trajectory position comparison triggering, leading to process defects such as glue dispensing failure, glue accumulation, and glue path offset.

[0004] Meanwhile, traditional control schemes rely solely on single trajectory planning at the upper level, lacking underlying secondary verification and iterative correction mechanisms. This makes it impossible to counteract the inherent speed regulation interference of embedded control systems and ensure the stability of trajectory length increments at critical locations such as arcs and corners. Furthermore, traditional embedded controllers have small hardware caches and insufficient firmware computing power, making them unable to buffer large batches of motion commands or support high-frequency, high-precision iterative calculations. Conventional optimization methods such as speed reduction and fine-tuning acceleration / deceleration only address the symptoms, not the root cause, and cannot solve the trajectory distortion problem at its source.

[0005] Therefore, there is an urgent need for a control method and system for the reverse correction path propulsion amount of a five-axis constrained system to address the shortcomings of existing technologies. Summary of the Invention

[0006] The purpose of this invention is to propose a control method and system for the reverse correction path propulsion amount of a five-axis axis constraint, so as to solve the problems in the existing five-axis dispensing trajectory planning, such as multi-axis asynchrony caused by the bottom-level look-ahead modification of the single-axis speed, inaccurate triggering of the arc corner trajectory, and trajectory increment instability.

[0007] On the one hand, to achieve the above objectives, the present invention provides a method for controlling the propulsion amount of a five-axis constrained reverse correction path, comprising the following steps:

[0008] S1. Use the theoretical path propulsion amount to be constrained and corrected to perform a forward mapping solution to obtain the single-cycle axis pulse increment;

[0009] S2. Based on the single-cycle axis pulse increment combined with the axis increment over-limit judgment condition, perform axis constraint verification and judgment, and obtain the axis constraint verification and judgment result.

[0010] S3. Based on the axis constraint verification and judgment result, reverse the theoretical path advancement amount to be constrained and obtain the control result of the five-axis constraint reverse correction path advancement amount.

[0011] Optionally, S1, using the theoretical path propulsion amount to be constrained and corrected, a forward mapping solution is performed to obtain the single-cycle axis pulse increment, including:

[0012] By using the current planning velocity and current planning acceleration, the theoretical path advancement amount to be constrained and corrected is obtained;

[0013] Based on the theoretical path advance amount to be constrained and corrected, determine the candidate path position and obtain the TCP candidate pose;

[0014] Based on the TCP candidate pose, RTCP compensation and five-axis kinematics solution are used to obtain the single-cycle axis pulse increment.

[0015] Optionally, the formula for calculating the TCP candidate pose is:

[0016]

[0017]

[0018] Where j is the number of iterations for correcting the path advance within the current control cycle, and k is the sequence number of the current motion control cycle. This represents the TCP candidate pose at time k+1, the end of the k-th control cycle, during the j-th iteration. This represents the TCP pose when the path parameter is s. Let be the candidate path endpoint parameters at time k+1, the end point of the k-th control cycle during the j-th iteration. This represents the cumulative path advance of the tool along the planned trajectory at the start of the k-th control cycle. The advance amount of the candidate path to be executed in the current control cycle during the j-th iteration.

[0019] Optionally, based on the TCP candidate pose, RTCP compensation and five-axis kinematics solution are used to obtain the single-cycle axis pulse increment, including:

[0020] Construct a mapping function that integrates RTCP compensation calculation and five-axis inverse kinematics solution;

[0021] Substitute the TCP candidate pose into the mapping function that integrates RTCP compensation calculation and five-axis inverse kinematics solution to obtain the axis candidate pulse position vector;

[0022] The single-cycle axis pulse increment is obtained by combining the candidate axis pulse position vector with the reference axis pulse position.

[0023] Optionally, S2, based on the single-cycle axis pulse increment combined with the axis increment over-limit judgment condition, perform axis constraint verification and judgment, and obtain the axis constraint verification and judgment result, including:

[0024] Set the maximum allowed pulse increment within a single control cycle and obtain the condition for judging the axis increment exceeding the limit;

[0025] Based on the single-cycle axis pulse increment, several single-cycle axis pulse increments are obtained, including a first single-cycle axis pulse increment, a second single-cycle axis pulse increment, a third single-cycle axis pulse increment, a fourth single-cycle axis pulse increment, and a fifth single-cycle axis pulse increment.

[0026] Determine whether all of the single-cycle axis pulse increments meet the axis increment over-limit judgment condition. If so, obtain the axis increment compliance as the axis constraint verification judgment result; otherwise, obtain the axis increment over-limit as the axis constraint verification judgment result.

[0027] Optionally, the condition for determining if the axis increment exceeds the limit is:

[0028]

[0029] in, L represents the candidate pulse increment that the i-th execution axis needs to complete within the current control cycle. i is the maximum allowed pulse increment within a single control cycle, i is the execution axis number, j is the number of iterations for path advance correction within the current control cycle, and k is the sequence number of the current motion control cycle.

[0030] Optionally, S3, based on the axis constraint verification judgment result, reverse-correct the theoretical path advancement amount to be constrained, and obtain the control result of the five-axis constraint reverse correction path advancement amount, including:

[0031] Determine whether the axis constraint verification result is compliant with axis increment. If yes, obtain the theoretical path advancement amount to be constrained and corrected as the control result of the five-axis axis constraint reverse correction path advancement amount. Otherwise, calculate the single-axis over-limit ratio based on the single-cycle axis pulse increment and obtain the maximum axis over-limit ratio.

[0032] The theoretical path advance amount to be constrained and corrected is scaled using the maximum axis over-limit ratio to obtain the corrected theoretical path advance amount.

[0033] Optionally, the formula for calculating the single-axis over-limit ratio is:

[0034]

[0035] in, For single-axis over-limit ratio, L represents the candidate pulse increment that the i-th execution axis needs to complete within the current control cycle. i This represents the maximum allowable pulse increment within a single control cycle, where i is the execution axis number.

[0036] Optionally, scaling the theoretical path advance amount to be constrained and corrected using the maximum axis overshoot ratio to obtain the corrected theoretical path advance amount further includes:

[0037] Determine whether the corrected theoretical path advance amount is less than or equal to the preset minimum path advance amount. If so, execute the micro-advance, pause advance, or protection stop logic; otherwise, execute the first operation.

[0038] The first operation is as follows: determine whether the current iteration number exceeds the preset maximum correction number; if so, stop the iteration and execute the exception protection logic; otherwise, obtain the theoretical path advancement amount of the correction as the theoretical path advancement amount to be constrained and corrected, and return to execute the second operation.

[0039] The second operation is as follows: based on the theoretical path advance amount to be constrained and corrected, determine the candidate path position and obtain the TCP candidate pose.

[0040] On the other hand, to achieve the above objectives, the present invention provides a control system for five-axis constraint reverse correction path propulsion, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the control method for five-axis constraint reverse correction path propulsion as described in any one of claims 1-9.

[0041] Compared with the closest existing technology, the present invention has the following advantages:

[0042] This invention constructs a closed-loop iterative architecture of forward mapping, constraint verification, and reverse correction. It replaces independent speed adjustment of a single axis with overall scaling of the path advance, solving industry pain points such as multi-axis asynchrony and inaccurate point triggering in five-axis dispensing arcs and corner trajectories. Simultaneously, it balances trajectory fidelity, equipment hardware constraints, and embedded real-time computing performance. Specific effects are as follows:

[0043] (1) Ensure the accuracy of multi-axis synchronous motion: All axis coordinates are derived from the same trajectory parameters, avoiding the time difference of axis arrival caused by the individual modification of single axis speed by the underlying speed look-ahead, and improving the defects of glue path deviation and glue stacking and glue breakage process.

[0044] (2) Maintain the integrity of the original trajectory and RTCP compensation: After each correction of the feed amount, the TCP pose and axis pulse are completely recalculated, and the tool tip trajectory distortion and tool axis attitude mismatch problems caused by traditional single-axis limiting will not occur.

[0045] (3) Balancing servo constraints and motion stability: The path advance amount is scaled according to the axis over-limit ratio, and a dual protection threshold of minimum feed amount and maximum iteration number is used to avoid sudden changes in feed speed while satisfying the upper limit of axis pulse.

[0046] (4) Adapt to the computing power of self-developed embedded hardware: The algorithm is lightweight and modular, relying on a large-capacity RAM to cache massive trajectory instructions. The firmware can stably support multiple iterations in a single cycle, meeting the real-time interpolation requirements of dispensing equipment. Attached Figure Description

[0047] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0048] Figure 1 This is a flowchart of a five-axis constraint reverse correction path propulsion control method according to an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions in the embodiments of this invention will be clearly and completely described below with reference to specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0050] The terminology used in the embodiments section of this invention is for the purpose of explaining specific embodiments of the invention only, and is not intended to limit the invention.

[0051] like Figure 1 As shown, this embodiment of the invention provides a method for controlling the advance amount of a five-axis constrained reverse correction path, comprising:

[0052] S1. Use the theoretical path propulsion amount to be constrained and corrected to perform a forward mapping solution to obtain the single-cycle axis pulse increment;

[0053] Starting with the initial theoretical path propulsion amount without axis limit constraint adjustment, a complete forward solution chain is established from trajectory feed parameters to the single-cycle motion pulse increments of each of the five axes. This chain sequentially completes trajectory parameter to pose mapping, RTCP compensation, inverse kinematics solution for the five axes, and axis pulse difference calculation. Finally, candidate pulse increments for all executed axes are output, providing fundamental calculation data for subsequent axis constraint verification. The specific implementation process is as follows:

[0054] S1-1. Using the current planning velocity and current planning acceleration, obtain the theoretical path propulsion amount to be constrained and corrected;

[0055] At the beginning of each motion control cycle, the system first calculates the theoretical path propulsion without axis constraint correction based on the current planned velocity and planned acceleration, as shown in the following formula:

[0056]

[0057] in, Let v be the theoretical path advance amount to be constrained and corrected in the k-th motion control cycle. The superscript (0) indicates that axis space constraint correction has not yet been performed, and k is the sequence number of the current motion control cycle. k The planned velocity is at the current path position, and T is the duration of a single motion control cycle, for example, T=1ms. k Accelerate the planning at the current path location.

[0058] S1-2. Based on the theoretical path advancement amount to be constrained and corrected, determine the candidate path position and obtain the TCP candidate pose;

[0059] The theoretical path advance amount to be constrained and corrected is used as the candidate advance amount for the first iteration (first iteration j=0), combined with the cumulative path advance amount s that the tool has completed along the planned trajectory at the beginning of the kth control cycle. k Solve for the candidate path endpoint parameters at time k+1, the end point of the k-th control cycle, during the j-th iteration. This parameter corresponds to the candidate path position on the trajectory, and is calculated as follows:

[0060]

[0061] Where j represents the number of iterations for correcting the path advance within the current control cycle. The advance amount of the candidate path to be executed in the current control cycle during the j-th iteration.

[0062] Based on the candidate path location, the corresponding TCP candidate pose is calculated using the following formula:

[0063]

[0064] in, p(s) is the TCP candidate pose at the end of the k+1 period during the j-th iteration of the k-th control cycle; p(s) is the TCP pose with path parameter s, including TCP position and tool axis attitude; path parameter s is a unified parameter that runs through the geometric path planning, attitude planning, look-ahead velocity planning, RTCP compensation, five-axis kinematics solution and axis constraint determination process.

[0065] TCP candidate pose can be represented as:

[0066]

[0067] Where x, y, and z are the linear coordinates of the tool tip in the workpiece coordinate system, and A, B, and C are the rotational attitude angles of the tool axis. This enables the synchronous output of trajectory points and tool attitude with the same parameters, providing a complete six-dimensional input for subsequent periodic RTCP compensation.

[0068] S1-3. Based on the TCP candidate pose, use RTCP compensation and five-axis kinematics to obtain the single-cycle axis pulse increment;

[0069] RTCP compensation and five-axis kinematics solution are performed on TCP candidate poses: using the TCP candidate poses as input, coordinate transformation is completed by relying on the composite mapping function integrating RTCP compensation and five-axis inverse kinematics, and the positions of the five-axis candidate pulses are obtained. Then, the single-cycle candidate pulse increment of each axis is obtained by combining the difference between the reference axis pulse positions of the previous cycle and outputting the core axis motion parameters used for axis constraint over-limit determination. The specific implementation process is as follows:

[0070] S1-3-1. Construct a mapping function that integrates RTCP compensation calculation and five-axis inverse kinematics solution;

[0071] Define mapping function This function encapsulates two layers of continuous computation logic, relying on the mechanical topology of the five-axis machine tool and the unified tool geometry parameters to complete the calculation. The first layer has built-in RTCP compensation calculation logic, which inputs fixed geometric parameters such as tool length, machine tool rotation axis eccentricity, and oscillating head / turntable mechanical deviation to compensate for the tool tip TCP position drift caused by rotation axis oscillation. The second layer has built-in inverse kinematics solution model adapted to the current five-axis configuration, which can convert the compensated workpiece coordinate system six-dimensional TCP pose into the physical linear displacement and rotation angle corresponding to the five execution axes. At the same time, the function has built-in pulse equivalent conversion rules to uniformly convert physical displacement and rotation angle into pulse coordinates recognized by the servo drive, realizing the integrated calculation of TCP pose input, RTCP compensation, five-axis inverse kinematics, and axis pulse coordinate output, providing a standardized calculation model for subsequent rapid pose conversion.

[0072] S1-3-2. Substitute the TCP candidate pose into the mapping function that integrates RTCP compensation operation and five-axis inverse kinematics solution to obtain the axis candidate pulse position vector;

[0073] The obtained TCP candidate pose is used as a mapping function. The input is processed by the RTCP compensation and five-axis inverse kinematics continuous operation encapsulated within the execution function to obtain the candidate pulse position vector for each execution axis. The calculation formula is as follows:

[0074]

[0075] in, Let be the position vector of the candidate pulse for the execution axis in the j-th iteration of the k-th control cycle. For a five-axis system, the axis pulse position vector can be expressed as:

[0076]

[0077] in, This refers to the candidate pulse position of the i-th execution axis at the end of the k-th control cycle and the j-th iteration, where i is the execution axis number. The conventional allocation rule for axis numbers of a five-axis machine tool is that i=1,2,3 are linear translation axes (X / Y / Z axes), and i=4,5 are rotary axes (such as A / C oscillating head / rotary table axes).

[0078] S1-3-3. Obtain the single-cycle axis pulse increment based on the axis candidate pulse position vector and the reference axis pulse position;

[0079] Retrieve the axis pulse position vector that has been officially output in the previous control cycle, i.e., the reference axis pulse position vector q at the start of the k-th cycle. k This vector represents the valid axis pulse coordinates after the previous iteration converged and the servo execution was completed. For each execution axis i (i=1,2,3,4,5), the candidate pulse position of the i-th execution axis at the end of the k+1-th iteration in the k-th control cycle is used. Subtract the reference pulse coordinate q at the start of the period i,k This yields the single-cycle axis pulse increment, which is the candidate pulse increment that the i-th execution axis needs to complete within the current control cycle. The calculation formula is as follows:

[0080]

[0081] The single-cycle axis pulse increment is the amount of candidate path advancement completed within the current control cycle. The total number of pulses required to move the i-th execution axis is the core input parameter for judging the excess of subsequent axis increments and correcting the path advancement in the reverse direction.

[0082] In summary, the entire process from step S1-1 to step S1-3 is completed serially within the same millisecond-level control cycle. By relying on unified path parameters, the TCP position, tool axis posture, and RTCP compensation logic are matched throughout the process, avoiding problems such as tool tip trajectory deviation, posture asynchrony, and compensation distortion caused by layered processing. The output axis pulse increment can be directly used for axis constraint over-limit judgment, providing accurate and synchronous axis space calculation basis for the reverse iterative correction of path advance.

[0083] S2. Based on the single-cycle axis pulse increment combined with the axis increment over-limit judgment condition, perform axis constraint verification and judgment, and obtain the axis constraint verification and judgment result.

[0084] Based on the single-cycle pulse increment of each axis, the preset single-cycle maximum pulse threshold for each axis is used as the judgment benchmark. The amplitude comparison of each axis is completed according to the single-axis over-limit inequality. After traversing all five axes, the two types of working conditions are distinguished: axis over-limit and axis constraint compliance. The specific implementation process is as follows:

[0085] S2-1. Set the maximum allowed pulse increment within a single control cycle and obtain the condition for judging the axis increment exceeding the limit.

[0086] For each actuator axis i (i=1,2,3,4,5) in a five-axis system, considering hardware limits such as the servo motor's maximum speed, mechanical transmission ratio, and the controller's fixed control cycle T, let L be the maximum allowable pulse increment within a single control cycle for the i-th actuator axis. i Based on L i Define the single-axis compliance determination inequality This formula is the inequality for judging axis increment exceeding limits: if the absolute value of a single-axis pulse increment satisfies this inequality, it means that the amplitude of the axis motion is within the hardware allowable range; if If so, the single-cycle motion of the axis is determined to be out of limit.

[0087] S2-2. Based on the single-cycle axis pulse increment, obtain several single-cycle axis pulse increments, including a first single-cycle axis pulse increment, a second single-cycle axis pulse increment, a third single-cycle axis pulse increment, a fourth single-cycle axis pulse increment, and a fifth single-cycle axis pulse increment.

[0088] Read the independent single-cycle candidate pulse increments of the five execution axes obtained by solving the preceding forward mapping. The pulse increment corresponding to the linear axis X at i=1 is defined as the first single-cycle axis pulse increment, the pulse increment corresponding to the Y axis at i=2 is defined as the second single-cycle axis pulse increment, the pulse increment corresponding to the Z axis at i=3 is defined as the third single-cycle axis pulse increment, the pulse increment corresponding to the first rotation axis at i=4 is defined as the fourth single-cycle axis pulse increment, and the pulse increment corresponding to the second rotation axis at i=5 is defined as the fifth single-cycle axis pulse increment. These are summarized to form an increment set containing all five-axis motion increments, which fully covers the periodic motion amplitude data of all execution axes in the five-axis linkage, providing all calculation samples for global multi-axis constraint traversal judgment.

[0089] S2-3. Determine whether all of the single-cycle axis pulse increments meet the axis increment over-limit judgment condition. If so, obtain the axis increment compliance as the axis constraint verification judgment result; otherwise, obtain the axis increment over-limit as the axis constraint verification judgment result.

[0090] Iterate through the single-cycle axis pulse increments of the five execution axes, and substitute each set of increments into the axis increment over-limit judgment condition to complete the comparison and verification:

[0091] When all five pulse increments satisfy At that time, the axis constraint verification result of the axis increment compliance is generated, which means that the current iteration candidate path advancement can be actually executed by the five-axis mechanism. At the same time, the system receives the advancement and uses the corresponding five-axis pulse position as the formal motion command of the current control cycle.

[0092] If any set of pulse increments during the traversal process satisfies The traversal is immediately terminated, and an axis constraint verification result for axis increment exceeding the limit is generated. This means that although the current candidate path advance is valid at the geometric path level, it will cause at least one execution axis to exceed the executable capacity in a single control cycle. At this time, the system needs to reverse and reduce the path advance of the current cycle, and re-execute TCP pose calculation, RTCP compensation and five-axis kinematic solution.

[0093] In summary, steps S2-1 to S2-3 fully cover the motion limit verification of the five execution axes, with no risk of missed judgments. Relying on a unified hardware threshold to complete the global synchronous judgment, it can accurately identify the axis motion overload problem caused by the path advance amount, trigger the iterative correction process in advance, avoid the traditional single-axis end trimming from destroying the TCP trajectory, tool axis posture and RTCP compensation matching relationship, and ensure the synchronization of five-axis linkage and the geometric accuracy of the machining trajectory.

[0094] S3. Based on the axis constraint verification and judgment result, reverse the theoretical path advancement amount to be constrained and obtain the control result of the five-axis constraint reverse correction path advancement amount;

[0095] Based on the constraint determination results, the operation is performed in two branches: if the axis increment is determined to be out of limit, the global maximum scaling factor is calculated based on the over-limit ratio of each axis, and the theoretical path advancement amount to be corrected is reduced in reverse to complete the parameter iterative update; if the axis increment is determined to be compliant, the original theoretical path advancement amount is directly retained as the valid output, and finally the path advancement control result that satisfies the motion limits of all execution axes is obtained. The specific implementation process is as follows:

[0096] S3-1. Determine whether the axis constraint verification result is compliant with axis increment. If yes, obtain the theoretical path advancement amount to be constrained and corrected as the control result of the five-axis axis constraint reverse correction path advancement amount. Otherwise, calculate the single-axis over-limit ratio based on the single-cycle axis pulse increment and obtain the maximum axis over-limit ratio.

[0097] Read the output axis constraint verification result and perform branch judgment:

[0098] If the determination result is that the axis increment is compliant, it means that all axis pulse increments under the current theoretical path advance amount have not exceeded the hardware limit, and the original theoretical path advance amount to be constrained and corrected is directly used as the final legal control result.

[0099] If the determination result is that the axis increment exceeds the limit, then iterate through all five-axis single-cycle pulse increments, and for the i-th execution axis, define its single-axis limit exceedance ratio. That is, the ratio of the candidate pulse increment to the maximum allowable pulse increment, calculated as follows:

[0100]

[0101] When the time is right, it indicates that the axis is not out of limit; when If this occurs, it indicates that the axis has exceeded its limit. The system selects the maximum proportion from all executed axes, calculated as follows:

[0102]

[0103] in, This represents the maximum axis overshoot ratio during the j-th iteration.

[0104] S3-2. The theoretical path advance amount to be constrained and corrected is scaled using the maximum axis over-limit ratio to obtain the corrected theoretical path advance amount.

[0105] when At that time, the original theoretical path propulsion amount is scaled using the maximum over-limit ratio, and the calculation formula is as follows:

[0106]

[0107] in, This represents the theoretical path advancement amount after the (j+1)th iteration correction in the k-th control cycle. This is the original theoretical path advance amount to be constrained and corrected in the j-th iteration of the k-th control cycle, which is the initial feed step size of the input in the previous iteration. Due to the axis increment exceeding the limit, scaling adjustment is required.

[0108] Dividing the original propulsion amount by the global over-limit ratio yields the reduced corrected theoretical path propulsion amount. This correction logic reduces the single-cycle travel length along the trajectory and simultaneously reduces the single-cycle motion pulse increment of all execution axes, ensuring that the increment of all axes after correction synchronously falls back to the hardware allowable range. No single axis is individually trimmed throughout the process, maintaining the matching relationship between TCP path, tool axis posture, and RTCP compensation.

[0109] S3-3. Determine whether the corrected theoretical path advance amount is less than or equal to the preset minimum path advance amount. If yes, execute the micro advance, pause advance, or protection stop logic. Otherwise, execute step S3-4.

[0110] To prevent convergence under extreme conditions, a minimum path advance limit is set. Let the allowable minimum path advance be... Compare the obtained corrected propulsion amount with this:

[0111] like This indicates that the current control cycle is close to stopping, and the system triggers the fallback protection logic, which can enter the micro-propulsion, pause propulsion, or protection stop logic;

[0112] like If the feed rate meets the effective processing conditions, continue executing the maximum iteration count verification process.

[0113] S3-4. Determine whether the current iteration number exceeds the preset maximum correction number. If yes, stop the iteration and execute the exception protection logic. Otherwise, obtain the theoretical path advancement amount of the correction as the theoretical path advancement amount to be constrained and corrected, and return to execute step S1-2.

[0114] To prevent convergence under extreme conditions, a minimum path advance limit is set. Let j be the number of corrections already performed within the current control cycle, and N be the maximum allowed number of corrections. max The number of path correction iterations j in the current period is compared with the preset maximum number of iterations N. max Compare:

[0115] like This indicates that the axis constraint cannot be satisfied after multiple rounds of scaling, the iteration diverges, the system stops iterating, and enters the exception protection logic, such as deceleration and stop, suspension of distribution, or reporting of errors.

[0116] like If the current correction is not achieved, the corrected propulsion amount will be updated to the theoretical path propulsion amount to be corrected in the next iteration, and the process will jump to step S1-2 to restart the forward calculation of pose solution.

[0117] By limiting the number of iterations and setting a minimum propulsion threshold, unbounded iterations within a single motion control cycle can be avoided. This can be expressed as follows: iteration terminates when axis space constraints are met, and a maximum number of iterations and a minimum path propulsion are set as real-time protection conditions. When the real-time protection conditions are met, the candidate path position is recalculated based on the corrected theoretical path propulsion, as shown in the following formula:

[0118]

[0119] in, The endpoint parameters of the candidate path are defined at time k+1, the end point of the k-th control cycle during the (j+1)-th iteration. The advance amount of the candidate path to be executed in the current control cycle at the (j+1)th iteration.

[0120] Based on the recalculated candidate path positions, the TCP candidate poses, RTCP compensation results, five-axis pulse positions, and single-cycle pulse increments for each axis are recalculated sequentially. The essence of the correction rule is that the out-of-limit information in the axis space is not only used for alarms or end-point limiting, but is returned as feedback to the unified path parameter space to correct the path advancement rhythm of the current control cycle.

[0121] It is important to note that, from a patent protection perspective, scaling down is only one of the preferred implementation methods. A broader protection statement should be: the path advancement amount of the current control cycle should be corrected in reverse based on the degree of axial space exceedance. In specific implementations, binary search, iterative approximation, or other convergence algorithms can also be used.

[0122] In summary, steps S3-1 to S3-4 construct a self-converging closed loop with dual fallback protection of minimum feed rate and maximum iteration count. Throughout the process, motion commands for all axes are generated synchronously with unified path parameters. This fundamentally avoids problems such as TCP trajectory offset, attitude asynchrony, RTCP compensation failure, and contour distortion caused by traditional single-axis amplitude limiting. While meeting the single-cycle motion limit constraints of each servo axis, it stably ensures the synchronization of five-axis linkage and the geometric accuracy of workpiece machining, taking into account both equipment operation safety and high-precision machining requirements.

[0123] Unlike traditional methods that directly limit and prune over-limit single axes at the command output, the core of this invention is not to independently limit a single axis at the output, but to feed back the over-limit information in the real axis space to a unified path parameter space, recalculating the path advance amount for the current control cycle. Thus, while satisfying the execution capability constraints of each axis, it maintains consistency in the TCP path, tool axis attitude, RTCP compensation relationship, and five-axis linkage relationship.

[0124] Explanation of the technical principles and effects of correcting path propulsion without trimming a single axis:

[0125] Five-axis motion does not mean that five execution axes move independently of each other, but rather that multiple execution axes work together to complete the same TCP path and tool axis posture. When the unified path parameter is s, the TCP pose p is determined jointly by the geometric path model and the posture model:

[0126] p = p(s)

[0127] The five-axis pulse position vector is determined by both RTCP compensation and five-axis kinematic mapping:

[0128]

[0129] Where q(s) is the axis pulse position vector obtained after RTCP compensation and five-axis kinematics solution when the path parameter is s.

[0130] Therefore, all legal combinations of five-axis positions constitute an axis space trajectory C determined by the path parameter s:

[0131]

[0132] Where S represents the total path parameter length of the entire planned path.

[0133] If an axis exceeds its limits, and that axis is pruned independently while the other axes retain their original commands, the result will be a pruned axis position vector. This vector is usually no longer located on the original axis space trajectory C, that is... This means that the trimmed five-axis position combination usually no longer corresponds to any TCP pose on the original path. The consequences may include: TCP points deviating from the original machining path; tool axis posture and TCP position being out of sync; the RTCP compensation relationship being disrupted; the actual end-effector velocity deviating from the planned velocity; geometric distortion of local trajectories; and increased machining and servo following errors.

[0134] This invention does not directly modify the motion command of any single axis, but only corrects the uniform path propulsion amount. After correction, the system recalculates: as well as Among them, p k+1 For the TCP pose in the (k+1)th control cycle, q k+1 Let be the axis pulse position vector for the (k+1)th control cycle. Since the positions of the five execution axes always originate from the same unified path parameter, the corrected axis position vector still satisfies: .

[0135] The essence of this solution is not single-axis position trimming, but retiming in the path parameter space. Its technical effects are: the original geometric path remains unchanged; the correspondence between TCP position and tool axis posture remains unchanged; the RTCP compensation relationship remains unchanged; the five-axis synchronization relationship remains unchanged; only the distance traveled along the path in the current control cycle is adjusted, thereby reducing the actual execution speed of the current cycle.

[0136] Table 1 shows the differences between the traditional single-axis speed limiting scheme and the path advancement amount of the present invention in different dimensions. The traditional single-axis speed limiting scheme is an end-point clipping scheme in axis space, which passively limits the over-limit axis pulses only at the motion command output end, and is very easy to destroy the multi-axis linkage matching relationship. In contrast, the present invention adopts a new approach of re-timing the path parameter space in reverse according to the axis space constraints. When the present invention detects that any executed axis exceeds the limit, it will not modify the local axis motion command separately. Instead, it uses the degree of axis exceeding the limit as a feedback quantity to correct the unified path advancement amount, iteratively recalculates and generates a complete five-axis linkage command that satisfies all axis hardware constraints. Under the condition of simultaneously enabling look-ahead speed planning and RTCP real-time compensation, it can maintain the synchronization and unity of TCP machining path, tool axis posture, axis space motion limit constraints and real-time control cycle throughout the process, and fundamentally solve the machining defects such as trajectory offset, compensation distortion, posture misalignment and speed jump caused by traditional end-point clipping.

[0137] Table 1

[0138]

[0139] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0140] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0141] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0142] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0143] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the protection scope of the claims of the present invention.

Claims

1. A method for controlling the advance amount of a five-axis constrained reverse correction path, characterized in that, include: S1. Use the theoretical path propulsion amount to be constrained and corrected to perform a forward mapping solution to obtain the single-cycle axis pulse increment; S2. Based on the single-cycle axis pulse increment combined with the axis increment over-limit judgment condition, perform axis constraint verification and judgment, and obtain the axis constraint verification and judgment result. S3. Based on the axis constraint verification judgment result, reverse the theoretical path advancement amount to be constrained and obtain the control result of the five-axis constraint reverse correction path advancement amount.

2. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 1, characterized in that, S1. Using the theoretical path propulsion amount to be constrained and corrected, perform a forward mapping solution to obtain the single-cycle axis pulse increment, including: By using the current planning velocity and current planning acceleration, the theoretical path advance amount to be constrained and corrected is obtained; Based on the theoretical path advance amount to be constrained and corrected, determine the candidate path position and obtain the TCP candidate pose; Based on the TCP candidate pose, RTCP compensation and five-axis kinematics solution are used to obtain the single-cycle axis pulse increment.

3. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 2, characterized in that, The formula for calculating the TCP candidate pose is: Where j is the number of iterations for correcting the path advance within the current control cycle, and k is the sequence number of the current motion control cycle. This represents the TCP candidate pose at time k+1, the end of the k-th control cycle, during the j-th iteration. This represents the TCP pose when the path parameter is s. Let be the candidate path endpoint parameters at time k+1, the end point of the k-th control cycle during the j-th iteration. This represents the cumulative path advance of the tool along the planned trajectory at the start of the k-th control cycle. The advance amount of the candidate path to be executed in the current control cycle during the j-th iteration.

4. The method for controlling the advance amount of a five-axis constrained reverse correction path according to claim 2, characterized in that, Based on the TCP candidate pose, RTCP compensation and five-axis kinematics solution are used to obtain the single-cycle axis pulse increment, including: Construct a mapping function that integrates RTCP compensation calculation and five-axis inverse kinematics solution; Substitute the TCP candidate pose into the mapping function that integrates RTCP compensation calculation and five-axis inverse kinematics solution to obtain the axis candidate pulse position vector; The single-cycle axis pulse increment is obtained by combining the candidate axis pulse position vector with the reference axis pulse position.

5. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 1, characterized in that, S2. Based on the single-cycle axis pulse increment combined with the axis increment over-limit judgment condition, perform axis constraint verification and judgment, and obtain the axis constraint verification and judgment result, including: Set the maximum allowed pulse increment within a single control cycle and obtain the condition for judging the axis increment exceeding the limit; Based on the single-cycle axis pulse increment, several single-cycle axis pulse increments are obtained, including a first single-cycle axis pulse increment, a second single-cycle axis pulse increment, a third single-cycle axis pulse increment, a fourth single-cycle axis pulse increment, and a fifth single-cycle axis pulse increment. Determine whether all of the single-cycle axis pulse increments meet the axis increment over-limit judgment condition. If so, obtain the axis increment compliance as the axis constraint verification judgment result; otherwise, obtain the axis increment over-limit as the axis constraint verification judgment result.

6. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 5, characterized in that, The condition for determining if the axis increment exceeds the limit is: in, L represents the candidate pulse increment that the i-th execution axis needs to complete within the current control cycle. i The maximum pulse increment allowed within a single control cycle is denoted by i, where i is the execution axis number, j is the number of iterations for path advance correction within the current control cycle, and k is the sequence number of the current motion control cycle.

7. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 2, characterized in that, S3. Based on the axis constraint verification and judgment result, reverse the theoretical path advancement amount to be constrained and obtain the control result of the five-axis constraint reverse correction path advancement amount, including: Determine whether the axis constraint verification result is compliant with axis increment. If yes, obtain the theoretical path advancement amount to be constrained and corrected as the control result of the five-axis axis constraint reverse correction path advancement amount. Otherwise, calculate the single-axis over-limit ratio based on the single-cycle axis pulse increment and obtain the maximum axis over-limit ratio. The theoretical path advance amount to be constrained and corrected is scaled using the maximum axis over-limit ratio to obtain the corrected theoretical path advance amount.

8. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 7, characterized in that, The formula for calculating the single-axis over-limit ratio is: in, For single-axis over-limit ratio, L represents the candidate pulse increment that the i-th execution axis needs to complete within the current control cycle. i This represents the maximum allowable pulse increment within a single control cycle, where i is the execution axis number.

9. The method for controlling the propulsion amount of a five-axis constrained reverse correction path according to claim 7, characterized in that, The process of scaling the theoretical path advance amount to be constrained and corrected using the maximum axis over-limit ratio to obtain the corrected theoretical path advance amount further includes: Determine whether the corrected theoretical path advance amount is less than or equal to the preset minimum path advance amount. If so, execute the micro-advance, pause advance, or protection stop logic; otherwise, execute the first operation. The first operation is as follows: determine whether the current iteration number exceeds the preset maximum correction number; if so, stop the iteration and execute the exception protection logic; otherwise, obtain the theoretical path advancement amount of the correction as the theoretical path advancement amount to be constrained and corrected, and return to execute the second operation. The second operation is as follows: based on the theoretical path advance amount to be constrained and corrected, determine the candidate path position and obtain the TCP candidate pose.

10. A control system for five-axis constrained reverse correction path propulsion, characterized in that, It includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the control method for the five-axis constraint reverse correction path advance amount as described in any one of claims 1-9.

Images