A sensor fusion adaptive control method for precision trajectory machining

Abstract

Embodiments of the present disclosure disclose a sensor fusion adaptive control method for precision trajectory machining. A specific embodiment of the method comprises: obtaining trajectory state data collected by a sensor and a target reference trajectory, calculating a time alignment acceleration feature vector and a dynamic profile error vector; calculating a curvature gradient vector of the target reference trajectory, comparing the length of the curvature gradient vector with a critical curvature threshold; when the length exceeds the threshold, suspending a peripheral device self-diagnosis and raising the priority of feedforward compensation calculation; inputting the time alignment acceleration feature vector and the dynamic profile error vector into a feedforward control algorithm, solving an axial control current gain and outputting a synchronous compensation control instruction, and adjusting task clock distribution by monitoring the curvature gradient change of the machining trajectory. The embodiment ensures that the variable direction hedging instruction is output before error accumulation, suppresses profile distortion, and avoids divergence of deviation.

Description

Technical Field

[0001] The embodiments disclosed herein relate to the field of industrial control system technology, and more specifically to a sensor fusion adaptive control method for precision trajectory machining. Background Technology

[0002] The current conventional approach involves using multiple sensors to collect position and velocity data of the kinematic pairs and transmitting it to the central controller for feedback loop calculation. The spindle controller latches the heterogeneous bus input stream according to a fixed real-time operating system clock cycle, sequentially performing position loop digital filtering, velocity loop state calculation, and feedforward current gain superposition. By adjusting the drive current of the servo motor, it follows the target motion trajectory. This approach can maintain basic position closed-loop response characteristics when machining flat or low-curvature trajectories. However, when the controlled machining axis enters a high-curvature inflection point trajectory with high-speed direction change, the dynamic load of the worktable fluctuates drastically. The predetermined scheduling architecture within the controller develops deep physical defects. Due to differences in hardware crystal oscillators and communication protocols, the multi-source heterogeneous sensors produce asynchronous sampling problems, causing random data queuing delays within the data bus. This results in discrete time misalignment between the position sampling point stream and the velocity feedback point stream transmitted to the controller. This time misalignment is converted into angular phase deviation between the control axes during the multi-axis spatial interpolation synthesis stage, damaging the spatial geometric consistency of the feedback loop input data.

[0003] At this point, if intuitive improvement schemes such as linearly increasing the bus communication frequency or brute-force increasing the feedforward gain are adopted, not only will the inherent timing delay of asynchronous sampling be not eliminated, but the queuing congestion of heterogeneous buses under the impact of sudden high-frequency computing flow will be aggravated, triggering data packet loss anomalies, causing the closed-loop system to stall due to feedback interruption. The aforementioned intuitive improvement attempts are limited to upgrading the physical performance of mechanical components and do not address the real-time scheduling mechanism of the control core. Conventional modifications at the control method level also suffer from response lag. For example, Chinese invention patent application CN121491921A discloses a polishing trajectory intelligent control method and system based on multi-sensor fusion, which predicts the trajectory adjustment parameters for the next processing cycle by analyzing the displacement sequence collected in the frequency domain. However, this cross-cycle a posteriori calculation mechanism cannot smooth out the accumulation of sudden deviations within the current control loop in a microsecond-level hard real-time control loop. Especially in high-speed direction-changing conditions, due to the lack of transient rearrangement capability of the step arrangement order within the control loop, it is impossible to complete the dynamic phase-locking of multiple heterogeneous input streams at the start of the current control cycle, resulting in physical lag in the calculated compensation amount. This makes it difficult to smooth out the contour distortion at the turning point of the direction change and may even cause resonance divergence of the servo mechanism.

[0004] Therefore, how to adaptively adjust the sequence of steps within the control loop based on trajectory geometric features and integrate heterogeneous input stamps to achieve time axis phase alignment, thereby ensuring the determinism of bus communication while suppressing abrupt contour distortion, has become the technical problem to be solved by this invention. Summary of the Invention

[0005] The summary portion of this disclosure is intended to provide a brief overview of the concepts, which will be described in detail in the detailed description portion. This summary portion is not intended to identify key or essential features of the claimed technical solutions, nor is it intended to limit the scope of the claimed technical solutions.

[0006] Some embodiments of this disclosure propose a sensor fusion adaptive control method for precision trajectory machining to address the technical problems mentioned in the background section above.

[0007] In a first aspect, some embodiments of this disclosure provide a sensor fusion adaptive control method for precision trajectory machining. The method includes: Step S1, acquiring trajectory state data collected by multi-source heterogeneous sensors through the data bus interface of a real-time operating system, and reading the target reference trajectory within a future control cycle, thereby calculating the temporal alignment acceleration feature vector and dynamic contour error vector of the current control cycle; Step S2, performing spatial difference multidimensional analytical operations on the target reference trajectory to calculate the current curvature gradient vector, and comparing the magnitude of the curvature gradient vector with a set critical curvature threshold in real-time at the hardware level; Step S3, when the curvature gradient... When the magnitude of the degree vector does not exceed the critical curvature threshold, the main control kernel of the CNC system completes the control loop cyclic calculation according to the inherent serial timing of position loop filtering, speed loop calculation, and peripheral device status self-diagnosis. When the magnitude of the curvature gradient vector crosses the critical curvature threshold, the peripheral device status self-diagnosis is suspended, and the priority of the feedforward compensation calculation is increased to before the arbitration of the main closed-loop feedback loop. In step S4, the feedforward control algorithm is used as an independent variable input to the timing-aligned acceleration feature vector and the dynamic contour error vector. The axial control current gain used to offset the motion hysteresis of the controlled motion axis is calculated, and the synchronous compensation control command is output to the drive control interface.

[0008] Preferably, step S1 specifically includes the following sub-steps: Step S11, read the position sampling data and velocity sampling data in the trajectory state data through the real-time operating system interface, use the timestamp alignment operator to eliminate the phase misalignment caused by the nondeterministic delay of the transmission bus, and generate a timing alignment state vector; Step S12, perform spatial difference between the timing alignment state vector and the target reference trajectory in the future control cycle, and calculate the timing alignment acceleration feature vector and dynamic contour error vector of the current control cycle.

[0009] Preferably, step S3, suspending the peripheral device status self-diagnosis and raising the priority of feedforward compensation calculation, specifically includes the following control sub-steps: Step S31, the central processing unit issues an interrupt instruction to suspend the peripheral device status self-diagnosis and deprive it of its allocated central processing unit clock slice; Step S32, the priority of feedforward compensation calculation in the main closed-loop feedback loop is raised to the first position, so that the feedforward compensation calculation is completed before the position loop filtering and speed loop calculation; Step S33, a convergence monitoring mechanism based on clock cycle is established. When the controlled motion axis is detected to move to the control cycle before the inflection point of the higher-order motion trajectory, the remaining available clock quota is dynamically calculated according to the real-time total load rate of the central processing unit, and the clock slice allocation of the feedforward control algorithm is dynamically constrained, so that the current central processing unit clock slice allocation is stable within the set priority channel range, ensuring that the feedforward control algorithm obtains a complete computing clock.

[0010] Preferably, in step S4, the feedforward control algorithm, which uses the time-aligned acceleration feature vector and the dynamic contour error vector as independent variables as inputs, specifically includes the following decoupling operation sub-steps: Step S41, input the time-aligned acceleration feature vector into the dynamic decoupling matrix to solve for the motion lag prediction value projected onto the spatial motion axis of the controlled motion axis; Step S42, calculate the contour distortion trend factor based on the dynamic contour error vector, and inject the contour distortion trend factor as a feedback correction gain into the feedforward control algorithm to correct the motion lag prediction value and generate the axial control current gain required to offset the motion lag prediction value; Step S43, linearly superimpose the axial control current gain with the conventional control current output by the main closed-loop feedback loop to generate a synchronous compensation control command and output it to the drive control interface.

[0011] Preferably, in step S11, eliminating phase misalignment caused by nondeterministic delay of the transmission bus using the timestamp alignment operator specifically includes the following sub-steps: Step S111, capturing data packets corresponding to position sampling data and velocity sampling data, and extracting the source hardware timestamp and bus network reception time carried in each data packet; Step S112, calculating the real-time transmission delay based on the difference between the bus network reception time and the source hardware timestamp, and inputting the real-time transmission delay into a first-order low-pass sliding filter model to eliminate nondeterministic delay jitter caused by bus contention; Step S113, performing inverse resampling interpolation on position sampling data and velocity sampling data based on the smoothed real-time transmission delay to achieve dynamic phase alignment of heterogeneous sampling data at the same control cycle starting point.

[0012] Preferably, the method further includes the following sub-steps for geometric constraints of the controlled motion axis: Step S5, while calculating the current curvature gradient vector, extracting the local maximum curvature radius of the target reference trajectory within the future control cycle; Step S6, based on the local maximum curvature radius and the maximum speed constraint value of the controlled motion axis, inversely calculating the upper limit value of the feed speed of the current control cycle; Step S7, when it is detected that the current command feed speed is greater than the upper limit value of the feed speed, calling the deceleration interpolation operator to smoothly reduce the speed of the target reference trajectory of the current control cycle, wherein, based on the ultimate mechanical stress of the controlled motion axis and the stiffness of the machine tool structure, the transient centripetal acceleration limit value is dynamically calibrated, and the transient centripetal acceleration limit value is set as the acceleration centripetal threshold, so that the transient centripetal acceleration of the controlled motion axis does not exceed the set acceleration centripetal threshold.

[0013] Preferably, in step S1, calculating the dynamic contour error vector of the current control cycle specifically includes the following real-time calculation sub-steps: Step S13, obtain the expected target position coordinates of the current control cycle output by the interpolation unit of the control system, and read the actual feedback position coordinates after time alignment; Step S14, calculate the spatial geometric distance between the expected target position coordinates and the actual feedback position coordinates, and generate a transient tracking error scalar; Step S15, project the actual feedback position coordinates onto the tangent normal plane of the target reference trajectory, solve for the shortest normal distance between the actual feedback position coordinates and the theoretical trajectory, and define the shortest normal distance as the magnitude of the dynamic contour error vector.

[0014] Preferably, the critical curvature threshold range is 0.15 mm. - ¹ to 0.45mm - ¹, the number of look-ahead cycles for the target reference trajectory within the future control cycle is 2 to 5, and the processing cycle of each control loop in the conventional serial timing is limited to 0.25ms to 1.0ms.

[0015] Preferably, the method runs on a central processing unit based on an embedded real-time operating system, and the local sampling clock at the data acquisition bus terminal is synchronized with the main clock of the control loop of the central processing unit at the microsecond level through a hardware clock synchronization signal, wherein the clock synchronization error is maintained within 2μs.

[0016] The above-described embodiments of this disclosure have the following beneficial effects: 1. In the sensor fusion adaptive control of precision trajectory machining, the spindle controller monitors the curvature gradient change of the machining trajectory and reconstructs the time allocation sequence of each calculation step within the clock control cycle. When the curvature gradient exceeds the preset threshold, the central processing unit adjusts the clock slice occupied by the peripheral diagnostic task and relocates the feedforward compensation calculation step before the arbitration step of the main feedback loop, so that the change-off offset instruction is output before the error accumulation. This kind of adaptive reordering of scheduling timing changes the clock pulse distribution structure, so that the drive signal maintains stable output at the inflection point and avoids the problem of divergence of the following deviation.

[0017] 2. This method marks the arrival timestamps of each heterogeneous input stream using a hard real-time clock and extracts historical communication delay features to estimate dynamic jitter values, calculating the transient phase lag of the fast-changing parameters relative to the low-frequency spatial position within the current control cycle. Based on this, the processor uses a timing interpolation operator to adjust the read / write pointer bias of the buffer, converting it into an aligned state feature vector synchronized with the control cycle in the internal data space. This vector works in tandem with the dynamic contour error to directly smooth signal steps and provide a smooth feature reference for subsequent control current output.

[0018] 3. This method deploys a timing monitoring unit within the control loop to measure the actual arrival time of the data stream, and sets a rigid truncation and safety arbitration mechanism for timing boundaries. When the actual arrival time is determined to exceed the threshold of 65% of the current control cycle, the arbitration rule is circuit-broken, rearranged, and compensated, and the feedforward compensation term is cleared to zero, so that the control flow smoothly returns to the conventional single-ended feedback steady-state mode. This multi-mechanism embedded anti-divergence strategy constructs a safety red line for the control loop and effectively isolates the control stall damage caused by communication anomalies. Attached Figure Description

[0019] The above and other features, advantages, and aspects of the embodiments of this disclosure will become more apparent from the accompanying drawings and the following detailed description. Throughout the drawings, the same or similar reference numerals denote the same or similar elements. It should be understood that the drawings are schematic, and elements are not necessarily drawn to scale.

[0020] Figure 1 This is a flowchart of the sensor fusion adaptive control method of the present invention; Figure 2 This is a schematic diagram of the sensor fusion adaptive control system structure of the present invention; Figure 3 This is a schematic diagram of the control loop operating mode switching of the present invention; Figure 4 This is a comparison chart of acceleration noise amplitude under different control cycle periods of the present invention. Detailed Implementation

[0021] Embodiments of this disclosure will now be described in more detail with reference to the accompanying drawings. While some embodiments of this disclosure are shown in the drawings, it should be understood that this disclosure can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of this disclosure. It should be understood that the accompanying drawings and embodiments of this disclosure are for illustrative purposes only and are not intended to limit the scope of protection of this disclosure.

[0022] It should also be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings. Unless otherwise specified, the embodiments and features described in this disclosure can be combined with each other.

[0023] It should be noted that the concepts of "first" and "second" mentioned in this disclosure are used only to distinguish different devices, modules or units, and are not used to limit the order of functions performed by these devices, modules or units or their interdependencies.

[0024] It should be noted that the terms "a" and "a plurality of" used in this disclosure are illustrative rather than restrictive, and those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0025] The names of messages or information exchanged between multiple devices in the embodiments of this disclosure are for illustrative purposes only and are not intended to limit the scope of such messages or information.

[0026] Before performing any of the operations involving the collection, storage, processing, or use of the target images disclosed herein, the relevant organizations or individuals shall fulfill their obligations, including conducting information security impact assessments, informing the information subjects, and obtaining prior authorization and consent from the information subjects.

[0027] This disclosure will now be described in detail with reference to the accompanying drawings and embodiments.

[0028] refer to Figure 1 The flowchart illustrates some embodiments of a sensor fusion adaptive control method for precision trajectory machining according to this disclosure. This sensor fusion adaptive control method for precision trajectory machining includes the following steps: Step S1: Obtain trajectory status data collected by multi-source heterogeneous sensors through the data bus interface of the real-time operating system, and read the target reference trajectory in the future control cycle, so as to calculate the timing alignment acceleration feature vector and dynamic contour error vector of the current control cycle. Step S2: Perform spatial difference multidimensional analytical operation on the target reference trajectory, calculate the current curvature gradient vector, and compare the magnitude of the curvature gradient vector with the set critical curvature threshold in real time at the hardware level. Step S3: When the magnitude of the curvature gradient vector does not exceed the critical curvature threshold, the main control kernel of the CNC system completes the control loop loop calculation according to the inherent serial timing of position loop filtering, speed loop calculation and peripheral device status self-diagnosis; when the magnitude of the curvature gradient vector crosses the critical curvature threshold, the peripheral device status self-diagnosis is suspended and the priority of feedforward compensation calculation is increased to before the arbitration of the main closed loop feedback loop. Step S4: The feedforward control algorithm is pre-loaded with the timing-aligned acceleration feature vector and the dynamic contour error vector as independent variables. The axial control current gain used to offset the motion hysteresis of the controlled motion axis is calculated, and a synchronous compensation control command is output to the drive control interface.

[0029] Preferably, step S1 specifically includes the following sub-steps: Step S11, read the position sampling data and velocity sampling data in the trajectory state data through the real-time operating system interface, use the timestamp alignment operator to eliminate the phase misalignment caused by the nondeterministic delay of the transmission bus, and generate a timing alignment state vector; Step S12, perform spatial difference between the timing alignment state vector and the target reference trajectory in the future control cycle, and calculate the timing alignment acceleration feature vector and dynamic contour error vector of the current control cycle.

[0030] Preferably, step S3, suspending the peripheral device status self-diagnosis and raising the priority of feedforward compensation calculation, specifically includes the following control sub-steps: Step S31, the central processing unit issues an interrupt instruction to suspend the peripheral device status self-diagnosis and deprive it of its allocated central processing unit clock slice; Step S32, the priority of feedforward compensation calculation in the main closed-loop feedback loop is raised to the first position, so that the feedforward compensation calculation is completed before the position loop filtering and speed loop calculation; Step S33, a convergence monitoring mechanism based on clock cycle is established. When the controlled motion axis is detected to move to the control cycle before the inflection point of the higher-order motion trajectory, the remaining available clock quota is dynamically calculated according to the real-time total load rate of the central processing unit, and the clock slice allocation of the feedforward control algorithm is dynamically constrained, so that the current central processing unit clock slice allocation is stable within the set priority channel range, ensuring that the feedforward control algorithm obtains a complete computing clock.

[0031] Preferably, in step S4, the feedforward control algorithm, which uses the time-aligned acceleration feature vector and the dynamic contour error vector as independent variables as inputs, specifically includes the following decoupling operation sub-steps: Step S41, input the time-aligned acceleration feature vector into the dynamic decoupling matrix to solve for the motion lag prediction value projected onto the spatial motion axis of the controlled motion axis; Step S42, calculate the contour distortion trend factor based on the dynamic contour error vector, and inject the contour distortion trend factor as a feedback correction gain into the feedforward control algorithm to correct the motion lag prediction value and generate the axial control current gain required to offset the motion lag prediction value; Step S43, linearly superimpose the axial control current gain with the conventional control current output by the main closed-loop feedback loop to generate a synchronous compensation control command and output it to the drive control interface.

[0032] Preferably, in step S11, eliminating phase misalignment caused by nondeterministic delay of the transmission bus using the timestamp alignment operator specifically includes the following sub-steps: Step S111, capturing data packets corresponding to position sampling data and velocity sampling data, and extracting the source hardware timestamp and bus network reception time carried in each data packet; Step S112, calculating the real-time transmission delay based on the difference between the bus network reception time and the source hardware timestamp, and inputting the real-time transmission delay into a first-order low-pass sliding filter model to eliminate nondeterministic delay jitter caused by bus contention; Step S113, performing inverse resampling interpolation on position sampling data and velocity sampling data based on the smoothed real-time transmission delay to achieve dynamic phase alignment of heterogeneous sampling data at the same control cycle starting point.

[0033] Preferably, the method further includes the following sub-steps for geometric constraints of the controlled motion axis: Step S5, while calculating the current curvature gradient vector, extracting the local maximum curvature radius of the target reference trajectory within the future control cycle; Step S6, based on the local maximum curvature radius and the maximum speed constraint value of the controlled motion axis, inversely calculating the upper limit value of the feed speed of the current control cycle; Step S7, when it is detected that the current command feed speed is greater than the upper limit value of the feed speed, calling the deceleration interpolation operator to smoothly reduce the speed of the target reference trajectory of the current control cycle, wherein, based on the ultimate mechanical stress of the controlled motion axis and the stiffness of the machine tool structure, the transient centripetal acceleration limit value is dynamically calibrated, and the transient centripetal acceleration limit value is set as the acceleration centripetal threshold, so that the transient centripetal acceleration of the controlled motion axis does not exceed the set acceleration centripetal threshold.

[0034] Preferably, in step S1, calculating the dynamic contour error vector of the current control cycle specifically includes the following real-time calculation sub-steps: Step S13, obtain the expected target position coordinates of the current control cycle output by the interpolation unit of the control system, and read the actual feedback position coordinates after time alignment; Step S14, calculate the spatial geometric distance between the expected target position coordinates and the actual feedback position coordinates, and generate a transient tracking error scalar; Step S15, project the actual feedback position coordinates onto the tangent normal plane of the target reference trajectory, solve for the shortest normal distance between the actual feedback position coordinates and the theoretical trajectory, and define the shortest normal distance as the magnitude of the dynamic contour error vector.

[0035] Preferably, the critical curvature threshold range is 0.15 mm. - ¹ to 0.45mm - ¹, the number of look-ahead cycles for the target reference trajectory within the future control cycle is 2 to 5, and the processing cycle of each control loop in the conventional serial timing is limited to 0.25ms to 1.0ms.

[0036] Preferably, the method runs on a central processing unit based on an embedded real-time operating system, and the local sampling clock at the data acquisition bus terminal is synchronized with the main clock of the control loop of the central processing unit at the microsecond level through a hardware clock synchronization signal, wherein the clock synchronization error is maintained within 2μs.

[0037] Example 1: A sensor fusion adaptive control method for precision trajectory machining is executed in an industrial control system containing a real-time control kernel. The industrial control system synchronously acquires multi-source heterogeneous sensor data via a bus interface with a period of 500μs, and sets a 1ms reference control loop in the processing kernel. In actual circular arc trajectory machining, the grating ruler installed at the end of the spindle provides feedback on the position status, and the piezoelectric accelerometer detects the dynamic acceleration of the sidewall of the cutting end. Both are transmitted to the processing kernel via the bus interface. Due to the asynchronous nature of the transmission link and the sampling mechanism, the two sets of data form a timing deviation in the buffer area.

[0038] The processing unit acquires sensor data and records the arrival time of the grating ruler data through the main station interface. Record the arrival time of the piezoelectric accelerometer data. The processor reads bus transmission delay records from the past 10 control cycles, calculates the distribution variance, and determines the current dynamic network clock jitter compensation amount accordingly. The processor determines the phase misalignment between sensor data using a phase deviation calculation formula. : ,in, To address the dynamic phase deviation within the current control loop, a network jitter sensitivity coefficient is introduced based on discrete-time signal jitter estimation. By adjusting the filter's weights for transient network congestion response, during the control system initialization phase, the processor sends 2000 sets of synchronous idle data packets to the sensor bus, records the round-trip transmission delay of each set, and calculates the statistical distribution variance. Based on the obtained variance value, the corresponding coefficient is indexed from the preset image register. If the variance value If the congestion exceeds the preset communication congestion threshold, lower the threshold. A value of 15% increases the cutoff depth of the low-pass filter, ensuring that the input phase alignment reference is unaffected by anomalous jumps in random queuing delay. The arrival time of the grating ruler data. For the arrival time of piezoelectric accelerometer data, To compensate for dynamic clock jitter, the processor calls the phase lead compensation operator, combining it with the current acceleration value. acceleration value from the previous control cycle Discretized look-ahead extrapolation of the control state yields the aligned acceleration state vector. : ,in, This is the state vector after phase alignment; This is the current cycle acceleration data; This is the acceleration data from the previous cycle; To control the loop constant, a value of 1ms is set.

[0039] The processor analyzes the target baseline trajectory through spatial difference operations, calculates the curvature gradient vector, and compares the magnitude of this vector with the critical curvature threshold of 0.35. Real-time comparison is performed when the mold length does not exceed 0.35. At that time, the processing unit sequentially executes position loop filtering, speed loop calculation, and peripheral device status self-diagnosis. When the module length exceeds 0.35... At this time, the processing unit suspends the peripheral device status self-diagnosis thread and elevates the feedforward compensation calculation step to execution before feedback arbitration. Because the embedded real-time operating system employs a priority-based preemptive scheduling mechanism, the peripheral device self-diagnosis task is typically allocated to a lower priority channel. Actual measurements show that this self-diagnosis task requires approximately 150μs of computation time within each 1ms control cycle. By explicitly issuing an interrupt signal to suspend the thread, the processor can forcibly reclaim the clock cycles originally allocated to the diagnostic task from the computing power pool, thereby providing a sufficient hard real-time time window for the computationally more complex feedforward phase compensation algorithm. This ensures that the compensation instruction is calculated within the 250μs position loop cutoff time, and the processor will align the state vector... The input is fed into the feedforward control algorithm, which calculates the axial control current gain and outputs a synchronization compensation command. This is based on the spatial geometric mapping of the multi-axis kinematic pair and the decoupling of rigid body dynamics, using a dynamic decoupling matrix. The following steps are constructed: Read the current joint coordinate vectors of each motion axis of a five-axis CNC machine tool. The current configuration Jacobian matrix of the computing system. Solving for the inverse of the Jacobian matrix yields the dynamic decoupling matrix. After aligning the time sequence, the 3D acceleration vector is multiplied by the matrix to obtain the independent feedback components corresponding to the physical axis, eliminating intermodulation interference caused by spatial axial coupling. Based on the error control first derivative prediction principle, the contour distortion trend factor is determined. The following is determined: The generation factor is calculated based on the rate of change of the magnitude of the error vector. This parameter is used to correct the feedforward current injection intensity in real time. The preset error change rate sensitivity coefficient, This is the dynamic contour error vector of the current control loop. The dynamic profile error of the previous control cycle is calculated; when the error slope increases, the compensation gain is increased proportionally. If a hard safety boundary is detected where the data reception time exceeds 650μs, the control logic resets the feedforward compensation gain to 0 and cancels the current compensation command output to ensure the stability of the system under abnormal bus conditions.

[0040] Example 2: This experiment quantitatively verifies the sensor fusion adaptive control method in a five-axis CNC machine tool simulation environment containing a precision stepper drive unit. The experimental platform is built on a real-time simulation industrial computer, which is equipped with a processor with a main frequency of 1.2GHz, a bus interface sampling period of 500μs, and a feedback control cycle period of 1ms. To simulate the complex electromagnetic interference in the industrial field, a Gaussian white noise signal with a signal-to-noise ratio of 20dB is actively superimposed at the input end of the accelerometer signal.

[0041] The experiment established three sample groups: the first group was a control group using a fixed-gain PID control algorithm, without enabling any sensor signal filtering or phase alignment mechanisms; the second group was the experimental group using the method of this invention, which deployed dynamic phase compensation and feedforward algorithms in the processing link and performed noise reduction processing on the input data in real time; the third group was an out-of-range control group, whose curvature gradient trigger threshold was set to... When processing the trajectory of a 90° sharp corner, the test group processor monitored the evolution of the curvature gradient magnitude to Based on the curvature threshold comparison logic, the system instantaneously suspends non-critical diagnostic tasks and adjusts the feedforward compensation settlement step sequence to before feedback arbitration. Key experimental data are as follows: In the 90° sharp angle region, the maximum dynamic profile error of the experimental group was 3.1 μm, compared with 18.6 μm of the control group, demonstrating a significant improvement in error suppression; in terms of filtering performance, after processing, the peak-to-peak noise amplitude of the original noisy acceleration signal decreased from... Descending to High-frequency jitter in the sensor data was effectively suppressed, and the experimental group achieved this through dynamic phase deviation measurement. The computation achieves time-axis alignment of sensor data, resulting in an aligned state vector. The variance of fluctuation under high-frequency sudden change conditions remains within 0.12.

[0042] The experimental data from the out-of-range control group indicate that when the threshold is set at 0.15... During the machining of straight sections, the system frequently entered high-priority compensation mode, resulting in a bus load rate consistently above 85%, accompanied by two instances of periodic data packet loss due to interrupt scheduling conflicts. The contour distortion at the inflection point reached 8.2 μm. This phenomenon confirms that setting the threshold too low leads to excessive squeezing of computing resources in the invalid region, reducing the overall dynamic response margin of the system. The above data confirms the stability and accuracy improvement capabilities of the method in an engineering environment. The evolution logic of each physical parameter corresponds one-to-one with the process control actions, demonstrating the synergistic effect of the feedforward compensation logic and phase alignment mechanism in suppressing nonlinear distortion. It also verifies that the curvature gradient threshold at 0.35... The optimal adaptability of the project under the given conditions.

[0043] Example 3: The current precision trajectory machining system executes a multi-source sensor state fusion control method. The system logic control unit acquires the position state sequence fed back by the high-precision grating ruler and the vibration acceleration state sequence collected by the piezoelectric accelerometer. The processing kernel is configured as a real-time control task with a period of 1ms. To eliminate the state estimation deviation caused by the difference in hardware sampling timing, the system constructs a phase synchronization correction unit. The processing unit records the trigger time of the grating ruler feedback data. Triggering time of accelerometer data And calculate the current dynamic transmission delay variance based on the bus data frame header. Calculate the phase deviation. : ,in, This is the real-time phase compensation value. The sampling time for the grating ruler data. For the accelerometer data sampling time, The variance of transmission delay fluctuation in the bus network. The network jitter sensitivity coefficient is set to 0.85, and the processor will... Substituting the linear interpolation operator, we perform phase translation and state alignment of the acceleration vector to obtain the aligned feature state vector. : ,in, The aligned acceleration vector; The acceleration sampling data is for the current moment; The acceleration sampling data is from the previous moment; To control the cycle, a value of 1ms is set. The system executes the geometric contour adaptive adjustment logic, and the difference operation module calculates the curvature gradient magnitude of the target trajectory at the current processing point. and compare it with a preset curvature threshold. Real-time comparison, settings The value is 0.35 This threshold is based on the mechanical resonant frequency of the machine tool's axial transmission system and the physical limit of the servo motor's maximum torque.

[0044] when At that time, the controller cycles in a predetermined sequence: position loop filtering—speed loop calculation—routine feedforward adjustment—equipment status self-check; when At this time, the logic control unit performs a step rearrangement: the system suspends the peripheral device status self-check task, advances the feedforward compensation settlement step sequence to before the closed-loop arbitration, and the axial control unit receives the aligned acceleration vector. Calculate the axial compensation current gain : in, The output compensation current command, where, The calculation relies on the real-time identification of the rotational inertia of the controlled motion axis. During the machining start-up phase or in a constant feed rate range, the system calls the recursive least squares method to dynamically update the estimated rotational inertia of the system by monitoring the ratio of the output torque of the servo motor to the measured acceleration. The processor according to Total transmission gain of the transmission chain The ratio relationship is used to calculate the optimal feedforward gain coefficient under the current operating condition. This ensures that the compensation current can accurately offset changes in mechanical load. The feedforward gain coefficient is calculated online based on the system's moment of inertia and transmission gain, and its unit is A·m. m.

[0045] The system has an embedded link security circuit breaker procedure. When the processing unit detects that the bus data packet integrity check code matching fails for three consecutive control cycles, or the real-time clock synchronization deviation exceeds the 650μs threshold, the arbitration logic will force an order. The system is reset to 0, and the feedforward current command channel is blocked, switching to the conventional PID feedback control mode. This action prevents unstable oscillations caused by abnormal sampling data in the axial actuator. This embodiment introduces a system based on... Dynamic phase alignment and based on By reconstructing the timing of the steps, a closed-loop real-time correction mechanism was constructed. During the 90° sharp-angle trajectory cutting process, the root mean square value of the system contour error decreased to 2.8μm, which is lower than the 15.6μm of conventional PID control. This data shows that the feedforward control command is loaded before the trajectory change, which can effectively overcome the transmission inertia lag and meet the accuracy requirements of complex contour trajectory processing.

[0046] Example 4: This invention introduces security defense logic into a precision trajectory machining system to monitor and suppress the risk of instruction step jump caused by industrial bus communication fluctuations to closed-loop feedback control in real time. The system embeds a timing boundary monitoring unit in the real-time control task loop to perform time stamp verification on the arrival time of each data sampling packet in real time.

[0047] When the processor receives data streams from multiple sensors, it synchronously triggers a timing monitoring task, and the processor controls the data according to a preset baseline cycle. Set a safety decision boundary, the boundary value is... During each control cycle, the processing unit compares the actual arrival time of the data stream with the high-precision system clock. With the start time of the control cycle Real-time determination of data transmission time in the current period The calculation formula is as follows: ,in, The actual time taken for data transmission. The moment when the data stream triggers a processor hardware interrupt. To control the start time of the loop.

[0048] When the judgment At milliseconds, the system detects that the current communication jitter has exceeded the safety margin. At this point, the processor triggers the emergency circuit breaker mechanism of the control sequence, deprives the feedforward phase compensation operator of its running priority in the current control cycle, and resets the corresponding compensation current term. Dynamic zeroing is achieved simultaneously, with the control mode command switching from adaptive fusion control mode to steady-state control mode based on single-ended encoder feedback. This switching process is implemented by rewriting the status control register in the interrupt service routine, ensuring a smooth transition of action commands within a single control loop. Furthermore, the rewriting operation specifically targets the automatic reload register and comparison matching register of the hardware timer unit. When switching from 1kHz to 4kHz operating frequency, the central processing unit writes a new count value to the timer's period control register before executing the last instruction of the current interrupt service routine, shortening the interval of the next hardware interrupt trigger signal to 250μs, thereby achieving seamless frequency switching. This provides logical-level defense against the risk of actuator resonance or servo alarm caused by network communication anomalies. Test data shows that in a test with a 20ms continuous burst packet loss interference in the bus network, the fuse mechanism completes the mode switching within 0.1ms after detecting the delay exceeding the limit, avoiding a transient overshoot of more than 5% in the mechanical spindle output current. This confirms that the system has effective self-healing and safety protection capabilities under boundary conditions.

[0049] Example 5: During the deployment of current precision trajectory machining systems in production environments, it is necessary to establish sensor thermal drift compensation and dynamic response calibration procedures based on physical benchmarks to eliminate trajectory tracking errors caused by thermal deformation and vibration characteristic offset of mechanical structures. In standby mode, the system calls the initialization calibration module, and the CNC device drives the axial feed mechanism to execute a preset calibration trajectory cycle. The grating ruler collects the actual displacement output sequence. The piezoelectric accelerometer synchronously acquires the vibration acceleration sequence of the cutting end. The processor calculates the axial thermal expansion and contraction caused by temperature fluctuations using a thermal displacement compensation model. : ,in, This is the location thermal compensation amount. The coefficient of thermal expansion of the mechanical structure. The temperature difference between the current real-time temperature and the calibration reference temperature. As the static structural displacement constant, the processor calculates it using a linear regression algorithm based on multiple sets of temperature and corresponding axial displacement deviation data. and The processor calculates the numerical value of the accelerometer and stores the parameters in a non-volatile mapping register. Simultaneously, it analyzes the spectral characteristics of the accelerometer under static conditions and calculates the noise power spectral density. If the noise amplitude output by the accelerometer exceeds 0.05 for five consecutive cycles... The controller determines that there is a looseness or abnormal gap in the mechanical connection structure of the machine tool, and then triggers the system's self-protection logic and suspends the machining command.

[0050] During the processing and operation phase, the system dynamically calibrates and corrects control parameters through a closed loop when the curvature gradient magnitude... Exceeding the threshold of 0.35 At that time, the controller adjusts according to the rate of change of the feedforward gain. Dynamically adjust control gain To suppress servo motor current oscillations caused by high-frequency cutting disturbances: in This is the corrected feedforward control gain. This is the feedforward control gain from the previous control cycle. This is the sign function, used to determine the polarity of the gain adjustment; This is the gain adjustment step factor. This is the magnitude of the phase-aligned acceleration vector.

[0051] The processor monitors the fitting deviation between the servo motor feedback current and the command gain in real time. If the deviation shows a monotonically increasing trend within a continuous 5ms time window, the processor will... The value was reduced by 20% to improve the stability of the system under the load fluctuation condition. By performing the above offline calibration and real-time calibration process, the system established the correlation mapping between thermal displacement compensation and dynamic control gain, ensuring that the axial output current always maintains a dynamic balance with the trajectory geometry under high-frequency cutting load, and achieving the expected technical effect of keeping the average contour tracking deviation at 2.5μm at the inflection point of complex trajectory.

[0052] The above description is merely a selection of preferred embodiments of this disclosure and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in the embodiments of this disclosure is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above-described features with (but not limited to) technical features with similar functions disclosed in the embodiments of this disclosure.

Claims

1. A sensor fusion adaptive control method for precision trajectory machining, characterized in that, include: Step S1: Obtain trajectory status data collected by multi-source heterogeneous sensors through the data bus interface of the real-time operating system, and read the target reference trajectory in the future control cycle, so as to calculate the temporal alignment acceleration feature vector and dynamic contour error vector of the current control cycle. Step S2: Perform spatial difference multidimensional analytical operation on the target reference trajectory, calculate the current curvature gradient vector, and compare the magnitude of the curvature gradient vector with the set critical curvature threshold in real time at the hardware level. Step S3: When the magnitude of the curvature gradient vector does not exceed the critical curvature threshold, the main control kernel of the CNC system completes the control loop loop calculation according to the inherent serial timing of position loop filtering, speed loop calculation and peripheral device status self-diagnosis; when the magnitude of the curvature gradient vector crosses the critical curvature threshold, the peripheral device status self-diagnosis is suspended and the priority of feedforward compensation calculation is increased to before the arbitration of the main closed loop feedback loop. Step S4: The feedforward control algorithm is pre-loaded with the timing-aligned acceleration feature vector and the dynamic contour error vector as independent variables. The axial control current gain used to offset the motion hysteresis of the controlled motion axis is calculated, and a synchronous compensation control command is output to the drive control interface.

2. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, wherein, Step S1 specifically includes the following sub-steps: Step S11: Read the position sampling data and velocity sampling data in the trajectory status data through the real-time operating system interface, use the timestamp alignment operator to eliminate the phase misalignment caused by the nondeterministic delay of the transmission bus, and generate a time-aligned state vector. Step S12: Based on the temporal alignment state vector and the target reference trajectory in the future control cycle, perform spatial difference calculation to solve the temporal alignment acceleration feature vector and dynamic contour error vector of the current control cycle.

3. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, characterized in that, In step S3, suspending the peripheral device status self-diagnosis and increasing the priority of feedforward compensation calculation specifically includes the following control sub-steps: Step S31: The central processing unit issues an interrupt instruction to suspend the peripheral device status self-diagnosis and preempt the central processing unit clock slice allocated to it. Step S32: Prioritize the feedforward compensation calculation in the main closed-loop feedback loop to the first position, so that the feedforward compensation calculation is performed before the position loop filtering and velocity loop settlement. Step S33: Establish a convergence monitoring mechanism based on clock cycles. When the controlled motion axis moves to the control cycle before the inflection point of the higher-order motion trajectory, dynamically calculate the remaining available clock quota according to the real-time total load rate of the central processing unit, dynamically constrain the clock slice allocation of the feedforward control algorithm, so that the current central processing unit clock slice allocation is stable within the set priority channel range, and ensure that the feedforward control algorithm obtains the complete operation clock.

4. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, characterized in that, In step S4, the feedforward control algorithm, which uses the time-aligned acceleration feature vector and the dynamic contour error vector as independent variables as inputs, specifically includes the following decoupling operation sub-steps: Step S41: Input the time-aligned acceleration feature vector into the dynamic decoupling matrix to solve for the motion hysteresis prediction value of the controlled motion axis projected onto the spatial motion axis. Step S42: Calculate the contour distortion trend factor based on the dynamic contour error vector, and inject the contour distortion trend factor as a feedback correction gain into the feedforward control algorithm to correct the motion lag prediction value and generate the axial control current gain required to offset the motion lag prediction value. Step S43: The axial control current gain is linearly superimposed with the conventional control current output by the main closed-loop feedback loop to generate a synchronous compensation control command and output it to the drive control interface.

5. The sensor fusion adaptive control method for precision trajectory machining according to claim 2, characterized in that, In step S11, eliminating the phase misalignment caused by the nondeterministic delay of the transmission bus using the timestamp alignment operator specifically includes the following sub-steps: Step S111: Capture the data packets corresponding to the position sampling data and velocity sampling data, and extract the source hardware timestamp and bus network reception time carried in each data packet; Step S112: Calculate the real-time transmission delay based on the difference between the bus network receiving time and the source hardware timestamp, and input the real-time transmission delay into a first-order low-pass sliding filter model to eliminate the nondeterministic delay jitter caused by bus contention. Step S113: Based on the smoothed real-time transmission delay, reverse resampling interpolation is performed on the position sampling data and velocity sampling data to achieve dynamic phase alignment of heterogeneous sampling data at the starting point of the same control cycle.

6. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, characterized in that, The method also includes the following sub-steps for controlling the geometry of the motion axes: Step S5: While calculating the current curvature gradient vector, extract the local maximum curvature radius of the target reference trajectory within the future control cycle. Step S6: Based on the local maximum radius of curvature and the maximum speed constraint value of the controlled motion axis, reversely calculate the upper limit value of the feed speed of the current control cycle; Step S7: When the current command feed speed is detected to be greater than the feed speed limit, the deceleration interpolation operator is called to smoothly reduce the speed of the target reference trajectory of the current control cycle. In this process, the transient centripetal acceleration limit value is dynamically calibrated based on the limit mechanical stress of the controlled motion axis and the stiffness of the machine tool structure. The transient centripetal acceleration limit value is set as the acceleration centripetal threshold value so that the transient centripetal acceleration of the controlled motion axis does not exceed the set acceleration centripetal threshold value.

7. The sensor fusion adaptive control method for precision trajectory machining according to claim 2, characterized in that, Calculating the dynamic profile error vector of the current control loop specifically includes the following real-time calculation sub-steps: Step S13: Obtain the desired target position coordinates of the current control cycle output by the interpolation unit of the control system, and read the actual feedback position coordinates after timing alignment; Step S14: Calculate the spatial geometric distance between the desired target position coordinates and the actual feedback position coordinates to generate a transient tracking error scalar. Step S15: Project the actual feedback position coordinates onto the tangent normal plane of the target reference trajectory, solve for the shortest normal distance between the actual feedback position coordinates and the theoretical trajectory, and define the shortest normal distance as the magnitude of the dynamic contour error vector.

8. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, characterized in that, The critical curvature threshold ranges from 0.15 mm - from 1 to 0.45 mm - from 1 to 0.45 mm - The number of look-ahead periods of the target reference trajectory in the future control period is 2 to 5, and the processing operation period of each control loop in the conventional serial timing is limited to 0.25 ms to 1.0 ms.

9. The sensor fusion adaptive control method for precision trajectory machining according to claim 1, characterized in that, The method runs on a central processing unit based on an embedded real-time operating system, and uses a hardware clock synchronization signal to synchronize the local sampling clock at the data acquisition bus terminal with the main clock of the central processing unit's control loop at the microsecond level, wherein the clock synchronization error is maintained within 2μs.

Images