Multi-mode motion trail real-time tracking method and system for machine control

Abstract

The invention relates to the technical field of trajectory control, in particular to a multi-modal motion trajectory real-time tracking method and system for machine control, which performs weight redistribution through channel noise variance and amplitude difference, performs weighted stacking and continuous updating in combination with Kalman filtering, reduces state estimation variance and channel bias transfer, and improves the tracking accuracy of a multi-modal motion trajectory. Time consistency and amplitude consistency of fusion track state data are improved, an overshoot peak value is inhibited, a deviation attenuation process is shortened, abnormal samples in a deviation sequence are rejected through an isolated forest, interference of outliers on parameter updating is reduced, and speed difference and acceleration difference are compared for a correction control sequence, so that the accuracy of correction control is improved. The boundary detection rate and the determinacy of the stage number are improved, the energy change rate is screened through the speed difference and the curvature value of the adjacent stages, the transfer relation is determined, the time sequence coherence and the geometric continuity of stage switching are kept, and the time sequence stability and the stage connection coherence of the control instruction are kept in the multi-degree-of-freedom space.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of trajectory control, in particular to a multi-modal motion trajectory real-time tracking method and system for machine control. BACKGROUND

[0002] The technical field of trajectory control is a technology that realizes the generation of a motion path, trajectory tracking and error correction of a target object through a computer control system. The technology is usually applied to automatic machinery, industrial robots, numerical control machine tools and unmanned devices. The core content includes path planning, dynamic feedback control, position and attitude coordination calculation and real-time compensation of trajectory error. The focus is on improving the response accuracy and execution stability of the control system, so that the target object moves continuously and smoothly according to the predetermined trajectory in a multi-degree-of-freedom space, ensuring that the multivariable control of position, speed and acceleration meets the requirements. Usually, the algorithm model and the actuator are combined to realize high-precision control of the motion process through real-time data calculation and feedback adjustment.

[0003] The core scheme of the multi-modal motion trajectory real-time tracking method for machine control establishes a multi-modal information fusion model to jointly calculate and dynamically match the motion parameters from multiple input sources, thereby generating the real-time motion trajectory of the target object and providing feedback signals to the control system. The purpose is to realize high-precision identification and control of the machine motion state in a multi-dimensional space and to realize continuous and stable trajectory tracking. The scheme can effectively reduce the trajectory drift and delay error of the traditional control system in complex scenarios, ensure that the deviation between the control output and the target path remains within a controllable range, and achieve adaptive trajectory control of the machine under different working conditions, thereby improving the overall motion precision and task execution efficiency.

[0004] The existing technology constitutes a control link with path planning, dynamic feedback, attitude coordination and error compensation. In operation, it processes multiple source inputs and multi-stage power changes with a unified loop. The timing alignment and channel consistency constraints are not sufficient. The channel amplitude difference and noise difference are accumulated and biased in a short time window, resulting in increased fusion state fluctuations. The deviation processing is mainly based on constant error, and the directionality and change rate are not well described. Parameter adjustment responds slowly to rapidly changing sections, which is prone to over-peak amplification and decay delay. The stage boundary lacks mapping and indexing based on gradient direction reversal. The interval division drifts with local fluctuations in the path, and the stage number is unstable. The existing technology has a prolonged deviation recovery time in the alternating acceleration and deceleration sections of the joints of industrial robots, and the stage beats are inconsistent and the fitting error increases in the corner path sections of numerical control machine tools, which affects the timing consistency and trajectory following stability of multivariable control. SUMMARY

[0005] The purpose of the present application is to solve the shortcomings in the prior art and to provide a multi-modal motion trajectory real-time tracking method and system for machine control.

[0006] In order to achieve the above object, the present application adopts the following technical scheme: a multi-modal motion trajectory real-time tracking method for machine control, comprising the following steps: S1: acquiring the end position coordinates, joint angle values and motion speed values through a multi-modal sensor, synchronously calculating the displacement difference and angle difference of adjacent coordinate points, arranging the calculation results in sequence to generate a time sequence, establishing a continuous trajectory state, and outputting a trajectory initial state set; S2: based on the trajectory initial state set, judging the multi-channel noise variance and amplitude difference value, reversely correcting the abnormal channel weight and redistributing, weighting and superimposing the corrected signal after Kalman filtering processing, and continuously updating to output the state sequence after fusion calculation, and generating fusion trajectory state data; S3: based on the fusion trajectory state data, recursively analyzing the control bias and output response time, combining the isolated forest bidirectional judgment bias direction and change rate, adjusting the proportional term and integral term according to the judgment result, and recombining the control signal to form a new control output to obtain a corrected control sequence; S4: based on the corrected control sequence, comparing the path segment speed difference and acceleration difference, detecting the boundary of the gradient direction reversal, mapping the comparison result into a stage boundary table and updating the interval index, outputting a continuous control stage structure, and establishing a trajectory stage division result; S5: based on the trajectory stage division result, judging the adjacent stage speed difference and curvature value, screening the candidate stage path execution energy change rate and determining the transfer relationship meeting the preset standard, generating a stage index path table and inputting into a control module, and obtaining trajectory stage switching control.

[0007] As a further scheme of the present application, the trajectory initial state set includes a position coordinate sequence, a joint angle sequence and a motion speed sequence, the fusion trajectory state data includes a channel weight distribution table, a weighted superposition sequence and a continuous update time index, the corrected control sequence includes a control bias sequence, a proportional term adjustment record and an integral term adjustment record, the trajectory stage division result includes a stage boundary table, an interval index table and a control stage structure table, and the trajectory stage switching control includes a stage index path table, a stage speed difference record and a curvature value record.

[0008] As a further scheme of the present application, the specific steps of outputting the trajectory initial state set are: Through the multi-modal sensor input signal, the end position coordinates, joint angle values and motion speed values are extracted, and the sampling points information is recorded and the index table is established by using time trigger mode, the spatial distance between adjacent coordinate points is measured and the angle difference is calculated, the continuous data is integrated and the time sequence is arranged, and the coordinate difference sequence set is generated; Based on the coordinate difference sequence set, time axis indexes are matched and continuous sequences are connected, interval deviation between adjacent samples is corrected by using linear interval interpolation method, abnormal points are modified after data comparison, values between multi-dimensional variables are mapped and corresponding matrix is generated, and initial state set of trajectory is established.

[0009] As a further scheme of the present application, the specific steps for generating the fusion trajectory state data are: Based on the initial state set of trajectory, signal amplitude of multiple channels is read and time interval difference is counted, amplitude waveform is recorded by using independent channel sampling method and time index table is established, amplitude difference between continuous samples of each channel is calculated to form time sequence, amplitude difference and variance between channels are compared and channel difference evaluation set is generated; Based on the channel difference evaluation set, channel weight is adjusted and abnormal channel is corrected, abnormal channel weight is calibrated by using proportional coefficient matching method and correction value is calculated, channel weight table is updated after weight replacement and redistribution operation, data consistency is reviewed and index is recorded, and channel correction and distribution set is generated; Based on the channel correction and distribution set, signal superposition and time sequence are updated, channel correction signal is subjected to noise suppression by using Kalman filter, each channel correction signal is superposed point by point by using channel weighted integration method and time mapping table is established, sequence smoothing connection and continuous state correction are performed, sequence is output after data is recorded synchronously, and fusion trajectory state data is generated.

[0010] As a further scheme of the present application, the specific execution process of the Kalman filter is that, based on the channel correction and distribution set, time sequence of each channel correction signal is taken as input data, state prediction value between adjacent sampling points is calculated and prediction error is recorded, then state increment at current time is calculated by using prediction error and state estimation value is updated, prediction and update operations are recursively executed on time sequence, continuous smoothing of state quantity of each channel is completed, and filtered signal sequence is obtained.

[0011] As a further scheme of the present application, the specific steps for obtaining the correction control sequence are: Based on the fusion trajectory state data, control deviation quantity is collected and output response time is measured, target output value and actual execution value are compared, difference sequence is recorded, time interval between continuous sampling points is measured and time response table is established, difference value and time data are matched and sorted, abnormal deviation points are detected by using isolated forest and abnormal samples are removed, and control deviation analysis set is generated. Based on the control deviation analysis set, deviation direction and change rate are judged, positive and negative directions of deviation are detected by using sign differentiation and change sign is recorded, change rate amplitude is calculated after deviation between adjacent samples is differentiated, rate interval is divided and direction reversal is identified, and deviation dynamic judgment set is generated. Based on the deviation dynamic determination set, the proportional term and integral term parameters are adjusted, the signal combination is controlled, the control compensation value is calculated by generating the adjustment amount of each term, the time sequence table is established by superimposing the control signal and the compensation value, the control output is reconstructed and the sequence is recorded, and the modified control sequence is generated.

[0012] As a further scheme of the present application, the specific implementation process of the isolated forest is that, based on the fusion trajectory state data, the recorded difference sequence and the time response table are taken as input data, a subset is randomly selected in the sample set and a plurality of partition tree structures are established, the average path length of each sample in each partition tree is calculated and the result is recorded, the isolated score of the sample is calculated according to the distribution characteristics of the path length, the sample with a score exceeding a set threshold is marked as an abnormal sample and is removed, and the remaining data is retained to form a deviation sequence without abnormalities.

[0013] As a further scheme of the present application, the specific steps for establishing the trajectory stage division result are: Based on the modified control sequence, the path segment speed difference and the acceleration difference are compared, the path segment speed value is extracted and the interval between adjacent sampling points is recorded, the speed change rate and the acceleration change rate are calculated to obtain a difference table, the gradient direction of the continuous interval is identified and the reverse boundary is located, and a gradient boundary detection set is generated; Based on the gradient boundary detection set, the comparison result is mapped and the interval index is updated, the reverse boundary points are corresponded to the time interval to generate a mapping table, the stage structure is rearranged after splitting the interval, the stage number is corrected and arranged, the continuous output of the sequence is obtained, and a trajectory stage division result is generated.

[0014] As a further scheme of the present application, the specific steps for obtaining the trajectory stage switching control are: Based on the trajectory stage division result, the speed difference and the curvature value of adjacent stages are judged, the speed sequence is extracted and the average speed of the stage is calculated, the path segment geometric coordinates are extracted, the curvature radius is measured and the curvature value is recorded, and the stage difference index table is established after synchronously comparing the speed difference and the curvature change rate to generate a stage dynamic feature set; Based on the stage dynamic feature set, the energy change rate of the candidate stage path is screened and the transfer relationship is determined, the path energy change rate is calculated and the numerical matrix is recorded, the energy threshold is compared to screen the stage group meeting the conditions and establish a transfer index table, the path index is generated and transmitted to the control module, and the trajectory stage switching control is generated.

[0015] The multi-modal motion trajectory real-time tracking system for machine control is used to execute the multi-modal motion trajectory real-time tracking method for machine control, and the system comprises: Trajectory acquisition module: based on multi-modal sensor input signals, synchronously acquire the execution end position coordinates, joint angles and motion speed, then time register, calculate the displacement difference and angle difference of adjacent coordinate points, arrange to generate time sequence, and establish the initial state set of trajectory; Signal fusion module: based on the initial state set of trajectory, judge the multi-channel noise variance and amplitude difference value, correct the abnormal channel weight and redistribute, weight superposition and continuous time update to the corrected signal, and generate fusion trajectory state data; Control correction module: based on the fusion trajectory state data, analyze the control deviation and output response time, judge the deviation direction and change rate, adjust the proportional term and integral term parameters according to the judgment result, recombine the control signal, and generate the corrected control sequence; Stage division module: based on the corrected control sequence, compare the path segment speed difference and acceleration difference, detect the gradient direction reversal boundary, map the comparison result to the stage boundary table and update the interval index, and establish the trajectory stage division result; Switching control module: based on the trajectory stage division result, judge the adjacent stage speed difference and curvature value, select the candidate stage path energy change rate and determine the transfer relationship meeting the preset standard, generate the stage index path table, and obtain the trajectory stage switching control.

[0016] Compared with the prior art, the advantages and positive effects of the present application are: 1. In the present application, the weight redistribution is performed by the channel noise variance and amplitude difference value, and the weighted superposition and continuous update are performed by combining Kalman filtering, which reduces the state estimation variance and channel bias transmission, improves the time consistency and amplitude consistency of the fusion trajectory state data, suppresses the overshoot peak value and shortens the deviation decay process; 2. In the present application, the isolated forest is used to remove the abnormal samples in the deviation sequence, reduce the disturbance of outliers to parameter update, compare the speed difference and acceleration difference of the corrected control sequence, locate the gradient direction reversal boundary and generate the stage boundary table and interval index, and improve the boundary detection rate and the certainty of stage number; 3. In the present application, the energy change rate is selected by the adjacent stage speed difference and curvature value, and the transfer relationship is determined, the speed step and curvature mutation amplitude at switching are constrained, the time sequence continuity and geometric continuity of stage switching are maintained, the boundary distinguishability and switching controllability at the path gradient reversal are strengthened, and the time sequence stability of control instruction and the continuity of stage connection are maintained in the multi-degree-of-freedom space. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 The working process diagram of the present application; Figure 2 The system flowchart of the present application. DETAILED DESCRIPTION

[0018] In order to make the objects, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and not to limit the present application.

[0019] Embodiment one Please refer to Figure 1 The present application provides a technical solution: a multi-modal motion trajectory real-time tracking method for machine control, comprising the following steps: S1: acquiring the end position coordinates, joint angle values and motion speed values through a multi-modal sensor, synchronously calculating the displacement difference and angle difference of adjacent coordinate points, arranging the calculation results in sequence to generate a time sequence, establishing a continuous trajectory state, and outputting a trajectory initial state set; S2: based on the trajectory initial state set, judging the multi-channel noise variance and amplitude difference value, reversely correcting the abnormal channel weight and redistributing, weighting and superimposing the corrected signal after Kalman filtering processing, and continuously updating to output the state sequence after fusion calculation, generating fusion trajectory state data; S3: based on the fusion trajectory state data, recursively analyzing the control bias and output response time, combining the isolated forest bidirectional judgment bias direction and change rate, adjusting the proportional term and integral term according to the judgment result, and recombining the control signal to form a new control output to obtain a corrected control sequence; S4: based on the corrected control sequence, comparing the path segment speed difference and acceleration difference, detecting the boundary of the gradient direction reversal, mapping the comparison result into a stage boundary table and updating the interval index, outputting a continuous control stage structure, and establishing a trajectory stage division result; S5: based on the trajectory stage division result, judging the adjacent stage speed difference and curvature value, screening the candidate stage path execution energy change rate and determining the transfer relationship meeting the preset standard, generating a stage index path table and inputting into a control module to obtain trajectory stage switching control.

[0020] The trajectory initial state set includes a position coordinate sequence, a joint angle sequence and a motion speed sequence, the fusion trajectory state data includes a channel weight distribution table, a weighted superposition sequence and a continuous update time index, the corrected control sequence includes a control bias sequence, a proportional term adjustment record and an integral term adjustment record, the trajectory stage division result includes a stage boundary table, an interval index table and a control stage structure table, and the trajectory stage switching control includes a stage index path table, a stage speed difference record and a curvature value record.

[0021] The specific steps of outputting the trajectory initial state set are: Through the multi-modal sensor input signal, the end position coordinates, joint angle values and motion speed values are extracted, and each item is synchronously collected and numerically registered. A time trigger method is used to record the sampling point information and establish an index table. The spatial distance between adjacent coordinate points is measured and the angle difference is calculated. The continuous data is integrated and arranged in time sequence to generate a coordinate difference sequence set. Based on the coordinate difference sequence set, the time axis index is matched and the continuous sequence is connected. The linear interval interpolation method is used to correct the interval deviation between adjacent samples. After comparing the data, the abnormal points are modified. The values between multi-dimensional variables are mapped and the corresponding matrix is generated. The initial state set of the trajectory is established. Based on the multi-modal sensor input signal, the end position coordinates, joint angle values and motion speed values are extracted. The sampling frequency is set to 1000 Hz and the time interval is set to 1 ms. The time index number of each sampling point is recorded. The three-dimensional coordinate data and angle data of the sampling point are synchronously stored in the index table. The square sum of the three-dimensional value difference of adjacent coordinate points is calculated to obtain the displacement difference. The angle change value is obtained by directly subtracting the front and rear two groups of angle values. The displacement difference and the angle difference are arranged in time sequence and integrated into a continuous sequence. The index numbers are sorted from small to large in turn to generate a coordinate difference sequence set. Based on the coordinate difference sequence set, the time interval deviation between adjacent samples is corrected. The time axis unit interval is set to 1 ms. Taking the value interval of two sampling points as the reference, the time proportion difference is calculated and the interpolation points are generated in proportion within the interval. The value of each interpolation result is compared to ensure the continuity of the time sequence. The abnormal value detection method is used to identify abnormal points with a deviation of 3 times the standard deviation from the mean value as the threshold. After replacing the abnormal points with the mean value of the adjacent samples, sliding smoothing processing is performed. The smoothing window width is set to 5 sampling points. The three-dimensional position, angle and speed values are integrated into a five-column data matrix. The matrix columns are X coordinate, Y coordinate, Z coordinate, angle and speed in turn. The corresponding row records are established according to the time index to generate the initial state set of the trajectory.

[0022] The specific steps of generating the fused trajectory state data are as follows: Based on the initial state set of the trajectory, the multi-channel signal amplitude is read and the time interval difference is calculated. The amplitude waveform is recorded using the independent channel sampling method and the time index table is established. The amplitude difference between consecutive samples of each channel is calculated to form a time sequence. The amplitude difference and variance between channels are compared to generate a channel difference evaluation set. Based on the channel difference evaluation set, the channel weight is adjusted and the abnormal channel is corrected. The abnormal channel weight is calibrated using the proportional coefficient matching method and the correction value is calculated. After weight replacement and redistribution, the channel weight table is updated. The data consistency is reviewed and the index is recorded to generate a channel correction and distribution set. Based on the channel correction distribution set, update the signal superposition and time sequence, use Kalman filter to suppress noise of the channel correction signal, superimpose each channel correction signal point by point through channel weighting integration method and establish time mapping table, perform sequence smoothing connection and continuous state correction, record data synchronously and output sequence, generate fusion trajectory state data; Based on the initial state set of trajectory, read the amplitude data of each signal channel and calculate the difference, set the sampling frequency to 500 Hz and the sampling time to 60 seconds, independently sample the amplitude waveform of each channel and record the time index number, the time index is numbered in milliseconds, the number of channels is set to 8, calculate the amplitude difference between each channel continuous sample, the difference value is in volts, calculate the average amplitude difference and standard deviation in each time section, arrange the calculation results in channel number order, integrate the data, combine the difference table, variance table and time index table into a unified data structure, compare the channel numbers of the combined data structure and cross calculate the amplitude difference, generate the channel difference evaluation set; Based on the channel difference evaluation set, adjust the channel weight and correct the abnormal channel, set the initial value of the proportion coefficient to 1.000, the upper and lower threshold range to ±0.200, read the amplitude difference average of the detected abnormal channel and compare it with the reference channel amplitude average, determine the correction coefficient and update the weight, replace the original channel weight with the correction coefficient and redistribute the proportion value of the remaining channels, review the consistency of the updated weight table to ensure that the weight sum is equal to 1.000, record the index number, correction value and updated weight value of each channel, generate the channel correction distribution set; Based on the channel correction distribution set, use Kalman filter algorithm to suppress noise and update time sequence of the correction signal, set the initial state estimate value to 0, process noise variance to 0.001, observation noise variance to 0.005, filter step to every 1 millisecond, calculate the predicted state and predicted covariance in the prediction stage, calculate the Kalman gain and correct the state estimate value in the update stage, recursively operate the channel data in time order, superimpose the filtered signal point by point according to the channel weight value and generate the time mapping index table, perform sliding smoothing connection processing on the superimposed signal sequence, the smoothing window width is set to 7 sampling points, record the data of each group of update results synchronously and output the time sequence, generate fusion trajectory state data.

[0023] The specific execution process of Kalman filter is based on the channel correction distribution set, taking the time sequence of each channel correction signal as input data, calculating the state prediction value between adjacent sampling points and recording the prediction error, then calculating the current state increment using the prediction error and updating the state estimate value, sequentially performing prediction and update operations on the time sequence, completing the continuous smoothing of each channel state quantity, and obtaining the filtered signal sequence; The Kalman filter is according to the formula:

[0024] wherein: denotes the posterior state estimate at time , taking position or position-velocity one-dimensional state quantities, denotes the prior state estimate at time , obtained by recursion from the previous time, denotes the prior variance at time , being a scalar quantification of the prior uncertainty, denotes the observation coefficient, being a linear mapping coefficient from state to observation, denotes the observation at time , being a scalar observation incoming to the filter update, denotes the observation noise variance at time , being a scalar quantification of the observation uncertainty, denotes the gain adjustment coefficient, being a direct scaling coefficient on the magnitude of the Kalman gain, denotes the observation weight coefficient, being a weight scaling coefficient on the observation noise variance; the process is executed as follows: after obtaining the prior state estimate at time and the prior variance , the observation at the same time and the observation noise variance are read, the prior state is mapped to the observation domain by the observation coefficient to obtain the predicted observation term , the residual term is constructed, the scalar gain proportion is formed with the prior variance as the numerator term and as the denominator term, the gain adjustment coefficient is used to scale the proportion to obtain the state increment , the increment is added to the prior state estimate , and the result is recorded as .

[0025] The specific steps to obtain the modified control sequence are as follows: Based on the fused trajectory state data, the control bias quantity is collected and the output response time is determined, the target output value and the actual execution value are compared, the difference value sequence is recorded, the time interval between consecutive sampling points is determined and the time response table is established, the difference value and the time data are matched and sorted, the abnormal bias points are detected by the isolation forest and the abnormal samples are removed, and the control bias analysis set is generated; Based on the control deviation analysis set, the deviation direction and the change rate are determined, the positive and negative directions of the deviation are detected and recorded by symbol differentiation, the deviation between adjacent samples is differentiated, the change rate amplitude is calculated, the rate interval is divided and the direction reversal is identified, and a deviation dynamic judgment set is generated; Based on the deviation dynamic judgment set, the proportional term and integral term parameters are adjusted and the control signal is combined, the adjustment amount of each term is calculated to generate a control compensation value, the control signal and the compensation value are superimposed to establish a time sequence table, the control output is reconstructed and recorded, a modified control sequence is generated; Based on the fusion trajectory state data, the target output value and the actual execution value are compared point by point, the difference is registered, the time length is recorded as 60 seconds, the time index number and the difference number field are generated for each sampling point, a difference sequence table containing difference, time, and channel number is established, an isolation forest algorithm is used for anomaly detection, the number of trees is set to 100, the sample size is set to 256, the anomaly ratio is set to 1%, the random seed is set to 42, the path depth and anomaly score of each sample are calculated, the 95th percentile value corresponding to the upper quantile interval based on the average path depth of the sample is taken as the anomaly threshold, samples with anomaly scores higher than the statistical threshold are marked as abnormal and removed, the sample identification table and the anomaly label table are output, and the control deviation analysis set is generated. Based on the control deviation analysis set, the direction and the change rate are determined, the time step is set to 1 millisecond, the difference between adjacent samples is processed in time sequence, the sign change is recorded and represented by positive, negative and zero to indicate the direction type, the rate amplitude unit is set to error per millisecond, the rate interval threshold is set to low speed 0 to 0.5, medium speed 0.6 to 1.5, and high speed 1.6 to 3.0, the adjacent comparison is performed on the direction mark sequence, the sign flip point is recorded and the reverse position index is established, a data table containing time index, direction symbol, rate amplitude, interval number, and reverse mark is generated, and a deviation dynamic judgment set is generated. Based on the deviation dynamic judgment set, the proportional and integral parameters are updated and the control signal is combined, the initial value of the proportional parameter is set to 1.2, the initial value of the integral parameter is set to 0.08, the adjustment step table is set to low speed interval plus 0.05, medium speed interval plus 0.10, and high speed interval plus 0.15, the upper limit of the parameter is 2.0 and the lower limit is 0.5, the integral accumulation window length is 64 samples, and the saturation value range is plus or minus 100. The time index, parameter number and adjustment amount of each parameter are recorded to generate a compensation value matrix, the compensation value and the control signal are superimposed in time sequence, the time resolution is set to 1 millisecond, a time sequence table is established for the superimposed signal, containing time index, proportional parameter, integral parameter, compensation value and output, the reconstructed control sequence is output, and a modified control sequence is generated.

[0026] The specific execution process of Isolation Forest is as follows: based on the fused trajectory state data, the recorded difference sequence and time response table are used as input data. A subset is randomly selected from the sample set and multiple partition tree structures are built. The average path length of each sample in each partition tree is calculated and the results are recorded. The isolation score of the sample is calculated based on the distribution characteristics of the path length. Samples with scores exceeding the set threshold are marked as abnormal samples and removed. The remaining data are retained to form a deviation sequence without abnormalities. An isolated forest, according to the formula:

[0027] in: Indicates the first The anomaly score for each sample is a dimensionless metric output by the Isolation Forest algorithm. Indicates the first The time weighting coefficient for each sample is set to 1.05 in this embodiment. The summation symbol represents summation over all. The cumulative operation of multiplying the path depth and weight of isolated trees. Indicates the first The weights of the isolated trees, where For a tree index, let and satisfy , Indicates the first The sample at the th The path depth in an isolated tree is the number of splits a sample traverses from the root node to a leaf node. Indicates the first A bias penalty term for each sample, with a value of 0.25. This represents the normalization constant used to standardize the path depth to a dimensionless value. In this embodiment, an empirical value of 4.88 is used. Indicates the first A response correction term for each sample is used to adjust the offset related to the system response time in the calculation, and its value is 0.50. This represents the number of isolated trees, defined as the number of independently generated trees in the random forest; in this step, we take 100. This represents the sample index, indicating the number of the sample currently being tested, ranging from 1 to the total number of samples. Represents a tree index, indicating the first... An isolated tree, ranging from 1 to ; Execution process: First, number each sample. To implement isolated forest construction, in each isolated tree 256 sample nodes are randomly selected and a splitting threshold is established. The path depth of the sample in the current tree is calculated by comparing features. and recorded in the path depth matrix in turn, the tree weight is read from the weight table and the weight sum is ensured to be 1, the time weight coefficient is read from the time parameter table the deviation compensation term is read from the deviation penalty table the response correction term is read from the response registration table and the normalization constant is read from the constant table the path depth weighted sum is calculated and multiplied by the time weight coefficient and the deviation penalty term is added to form the numerator part, the normalization constant is added to the response correction term to form the denominator part, and the numerator is divided by the denominator to obtain the power exponent term, and the exponential function is calculated with 2 as the base the obtained value is recorded to the abnormal score table.

[0028] The specific steps of establishing the trajectory stage division result are as follows: based on the modified control sequence, the path segment speed difference and the acceleration difference are compared, the path segment speed value is extracted and the adjacent sampling point interval is recorded, the speed change rate and the acceleration change rate are calculated to obtain the difference table, the gradient direction of the continuous interval is identified and the reverse boundary is located, and the gradient boundary detection set is generated; based on the gradient boundary detection set, the comparison result is mapped and the interval index is updated, the reverse boundary point is corresponded to the time interval to generate the mapping table, the interval is split and the stage structure is rearranged, the stage number is corrected and arranged, the sequence is continuously output, and the trajectory stage division result is generated; based on the modified control sequence, the path segment speed difference and the acceleration difference are compared, the speed value of each path segment is extracted and the time index is recorded, the speed difference of adjacent sampling points is calculated and the acceleration change rate is calculated according to the time step, the speed unit is meter per second, and the acceleration unit is meter per second square, the sign change of the speed change rate is analyzed, the sign threshold is set to be greater than 0.02 in the positive direction and less than-0.02 in the reverse direction, the sliding window width is set to be 3 sampling points, the sign flip position is detected and the time index and the channel number are recorded, and the gradient boundary detection set is generated; based on the gradient boundary detection set, the comparison result is mapped and the index is updated, the single interval length is set to be 100 milliseconds, the reverse boundary point is mapped to the trajectory interval according to the time index and the number sequence is generated, which increases from 1 to 600, the starting time, the ending time and the stage number are recorded for each interval, the interval order is corrected and the number is updated, the whole structure information is integrated and output, and the trajectory stage division result is generated.

[0029] The specific steps of obtaining the trajectory stage switching control are as follows: Based on the trajectory stage division result, the adjacent stage speed difference and curvature value are judged, the speed sequence is extracted, and the stage average speed is calculated, the path segment geometric coordinates are extracted, the curvature radius is measured, and the curvature value is recorded, the stage difference index table is established after synchronous comparison of the speed difference and the curvature change rate, and the stage dynamic feature set is generated; Based on the stage dynamic feature set, the energy change rate of the candidate stage path is screened and the transfer relationship is determined, the path energy change rate is calculated, and the numerical matrix is recorded, the energy threshold is compared to screen the stage group meeting the conditions and establish the transfer index table, the path index is generated and transmitted to the control module, and the trajectory stage switching control is generated. Based on the trajectory stage division result, the adjacent stage speed difference and curvature value are judged, the speed sequence of each stage is extracted and the start and end time index is recorded, the stage average speed is obtained by dividing the speed sum of each stage by the sample number, the speed unit is meter per second, the three-dimensional geometric coordinate data x, y, z of the path segment are extracted and arranged in time sequence, the coordinate information between every three consecutive points is read, the arc radius is calculated, the radius unit is millimeter, the curvature value is recorded by taking the reciprocal of the curvature radius of the sampling segment, the average curvature value of each stage is recorded, the speed difference and the curvature change rate are compared synchronously, the curvature change threshold is set to 0.02, the speed difference threshold is set to 0.1, the stage difference index table is established for the items exceeding the threshold, the fields include stage number, time index, average speed, curvature value and change identification, the record data set is output, and the stage dynamic feature set is generated. Based on the stage dynamic feature set, the candidate stage path is screened and the transfer relationship is determined, the stage dynamic energy and potential energy difference value of each stage reading speed and curvature data is calculated, the mass value is 5 kg, the gravity acceleration is 9.8, the energy unit is joule, the energy sampling interval is set to 100 milliseconds, the energy change rate is obtained by dividing the energy difference value of adjacent stages by the time interval, and recorded to the numerical matrix, the matrix row represents the stage number, and the list represents the time interval number, the threshold is set to 20 joules per second, the stages with energy change rate lower than the threshold are marked as candidate group, the candidate stage path is numbered and the transfer index table is established, the index fields include the start stage number, the target stage number and the energy change rate value, the effective path number in the transfer index table is sorted and transmitted to the control module, the stage path control data is output, and the trajectory stage switching control is generated.

[0030] Please refer to Figure 2 , a multi-modal motion trajectory real-time tracking system for machine control, the system comprising: Trajectory acquisition module: based on multi-modal sensor input signals, synchronously acquire execution end position coordinates, joint angles and motion speeds, and perform time registration, calculate the displacement difference and angle difference of adjacent coordinate points, arrange to generate time sequence, and establish trajectory initial state set; Signal fusion module: based on the initial state set of trajectory, judge the multi-channel noise variance and amplitude difference, correct the abnormal channel weight and reallocate, weight the corrected signal, superimpose and continuously update, generate fusion trajectory state data; Control correction module: based on the fusion trajectory state data, analyze the control deviation and output response time, judge the deviation direction and change rate, adjust the proportional and integral parameters according to the judgment result, recombine the control signal, and generate the correction control sequence; Stage division module: based on the correction control sequence, compare the path segment speed difference and acceleration difference, detect the gradient direction reversal boundary, map the comparison result to the stage boundary table and update the interval index, and establish the trajectory stage division result; Switching control module: based on the trajectory stage division result, judge the adjacent stage speed difference and curvature value, select the candidate stage path energy change rate and determine the transfer relationship meeting the preset standard, generate the stage index path table, and obtain the trajectory stage switching control.

[0031] The above is only the preferred embodiment of the present application, not other forms of the present application, any skilled in the art may use the above disclosed technical content to change or modify as equivalent embodiments applied to other fields, but any simple modification, equivalent change and modification of the above embodiments without departing from the technical scheme content of the present application, according to the technical essence of the present application, still belongs to the protection scope of the technical scheme of the present application.

Claims

1. A method for real-time tracking of multi-modal motion trajectories for machine control, characterized in that, The method comprises the following steps: S1: obtaining the execution end position coordinates, joint angle values and motion speed values through a multi-modal sensor, synchronously calculating the displacement difference and angle difference of adjacent coordinate points, arranging the calculation results in sequence to generate a time sequence, establishing a continuous trajectory state, and outputting a trajectory initial state set; S2: based on the trajectory initial state set, judging the multi-channel noise variance and amplitude difference value, reversely correcting the abnormal channel weight and redistributing, weighting and superimposing the corrected signal after Kalman filtering processing, and continuously updating to output the state sequence after fusion calculation, and generating a fusion trajectory state data; S3: based on the fusion trajectory state data, recursively analyzing the control bias and output response time, combining the isolated forest bidirectional judgment bias direction and change rate, adjusting the proportional term and integral term according to the judgment result, and recombining the control signal to form a new control output to obtain a corrected control sequence; S4: based on the corrected control sequence, comparing the path segment speed difference and acceleration difference, detecting the boundary of the gradient direction reversal, mapping the comparison result into a stage boundary table and updating the interval index, outputting a continuous control stage structure, and establishing a trajectory stage division result; S5: based on the trajectory stage division result, judging the adjacent stage speed difference and curvature value, screening the candidate stage path execution energy change rate and determining the transfer relationship meeting the preset standard, generating a stage index path table and inputting into a control module to obtain a trajectory stage switching control.

2. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The trajectory initial state set includes a position coordinate sequence, a joint angle sequence and a motion speed sequence, the fusion trajectory state data includes a channel weight distribution table, a weighted superposition sequence and a continuous update time index, the corrected control sequence includes a control bias sequence, a proportional term adjustment record and an integral term adjustment record, the trajectory stage division result includes a stage boundary table, an interval index table and a control stage structure table, and the trajectory stage switching control includes a stage index path table, a stage speed difference record and a curvature value record.

3. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific steps of outputting the trajectory initial state set are: Through the multi-modal sensor input signal, the execution end position coordinates, joint angle values and motion speed values are extracted, and the sampling point information is recorded and the index table is established by using the time trigger mode. The spatial distance between adjacent coordinate points is measured and the angle difference is calculated. The continuous data is integrated and the time sequence is arranged to generate a coordinate difference sequence set; Based on the coordinate difference sequence set, the time axis index is matched and the continuous sequence is connected. The interval deviation between adjacent samples is corrected by using the linear interval interpolation method. The abnormal points are modified after comparing the data. The values between multi-dimensional variables are mapped and the corresponding matrix is generated. The trajectory initial state set is established.

4. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific steps of generating the fusion trajectory state data are: Based on the trajectory initial state set, the multi-channel signal amplitude is read and the time interval difference is counted. The amplitude waveform is recorded by using the independent channel sampling method and the time index table is established. The amplitude difference between the continuous samples of each channel is calculated to form a time sequence. The amplitude difference and variance between channels are compared to generate a channel difference evaluation set; Based on the channel difference evaluation set, the channel weight is adjusted and the abnormal channel is corrected, the proportional coefficient matching method is used to calibrate the abnormal channel weight and calculate the correction value, the channel weight table is updated after the weight replacement and reallocation operation, the data consistency is reviewed and the index is recorded, and the channel correction allocation set is generated; Based on the channel correction allocation set, the signal superposition and time sequence are updated, the Kalman filter is used for noise suppression of the channel correction signal, each channel correction signal is superposed point by point through the channel weighted integration method and the time mapping table is established, the sequence smoothing connection and continuous state correction are carried out, and the sequence is output after the data is recorded synchronously, and the fusion trajectory state data is generated.

5. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific execution process of the Kalman filter is that, based on the channel correction allocation set, the time sequence of each channel correction signal is taken as the input data, the state prediction value between adjacent sampling points is calculated and the prediction error is recorded, then the state increment at the current time is calculated by using the prediction error and the state estimation value is updated, the prediction and update operations are recursively executed on the time sequence, the continuous smoothing of each channel state quantity is completed, and the filtered signal sequence is obtained.

6. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific steps for obtaining the correction control sequence are: Based on the fusion trajectory state data, the control bias quantity is collected and the output response time is measured, the target output value and the actual execution value are compared, the difference value sequence is recorded, the time interval between consecutive sampling points is measured and the time response table is established, the difference value and the time data are matched and sorted, the isolated forest is used to detect abnormal deviation points and the abnormal samples are removed, and the control bias analysis set is generated; Based on the control bias analysis set, the deviation direction and the change rate are judged, the deviation positive and negative directions are detected through the sign differentiation and the change sign is recorded, the deviation between adjacent samples is differentiated to calculate the change rate amplitude, the rate interval is divided and the direction reversal is identified, and the deviation dynamic judgment set is generated; Based on the deviation dynamic judgment set, the proportional term and the integral term parameters are adjusted and the signal combination is controlled, the control compensation value is calculated by calculating the adjustment quantity of each term, the time sequence table is established by superimposing the control signal and the compensation value, the control output is reconstructed and the sequence is recorded, and the correction control sequence is generated.

7. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific execution process of the isolated forest is that, based on the fusion trajectory state data, the recorded difference value sequence and the time response table are taken as the input data, a subset is randomly selected in the sample set and a plurality of partition tree structures are established, the average path length of each sample in each partition tree is calculated and the result is recorded, the isolated score of the sample is calculated according to the distribution characteristics of the path length, the sample with a score exceeding a set threshold is marked as an abnormal sample and is removed, and the remaining data is retained to form an abnormal-free deviation sequence.

8. The multi-modal motion trajectory real-time tracking method for machine control of claim 1, wherein, The specific steps for establishing the trajectory stage division result are: Based on the correction control sequence, the path segment speed difference and the acceleration difference are compared, the path segment speed value is extracted and the interval between adjacent sampling points is recorded, the speed change rate and the acceleration change rate are calculated to obtain the difference table, the gradient direction of the continuous interval is identified and the reversal boundary is located, and the gradient boundary detection set is generated; Based on the gradient boundary detection set, the mapping comparison result is mapped and the interval index is updated, the reverse boundary point is corresponded to the time interval to generate a mapping table, the interval is split and the stage structure is rearranged, the stage number is corrected and arranged, the sequence is continuously output, and the trajectory stage division result is generated.

9. The multi-modal motion trajectory real-time tracking method for machine control according to claim 1, wherein, The specific steps of obtaining the trajectory stage switching control are: Based on the trajectory stage division result, the adjacent stage speed difference and curvature value are judged, the speed sequence is extracted and the stage average speed is calculated, the path segment geometric coordinates are extracted, the curvature radius is measured and the curvature value is recorded, the stage difference index table is established after synchronous comparison of the speed difference and the curvature change rate, and the stage dynamic feature set is generated; Based on the stage dynamic feature set, the energy change rate of the candidate stage path is screened and the transfer relationship is determined, the path energy change rate is calculated and the numerical matrix is recorded, the energy threshold is compared to screen the stage group meeting the conditions and establish the transfer index table, the path index is generated and the control module is transmitted, and the trajectory stage switching control is generated.

10. A multimodal motion trajectory real-time tracking system for machine control, characterized in that, The multi-modal motion trajectory real-time tracking method for machine control according to any one of claims 1-9, the system comprises: A trajectory acquisition module: based on multi-modal sensor input signals, the execution end position coordinates, joint angles and motion speeds are synchronously acquired and time-registered, the adjacent coordinate point displacement difference and angle difference are calculated, the time sequence is arranged and generated, and the trajectory initial state set is established; A signal fusion module: based on the trajectory initial state set, the multi-channel noise variance and amplitude difference value are judged, the abnormal channel weight is corrected and redistributed, the corrected signal is weighted and superimposed and continuously time-updated, and the fusion trajectory state data is generated; A control correction module: based on the fusion trajectory state data, the control bias and output response time are analyzed, the bias direction and change rate are judged, the proportional term and integral term parameters are adjusted according to the judgment result, the control signal is recombined, and the correction control sequence is generated; A stage division module: based on the correction control sequence, the path segment speed difference and acceleration difference are compared, the gradient direction reverse boundary is detected, the comparison result is mapped as a stage boundary table and the interval index is updated, and the trajectory stage division result is established; A switching control module: based on the trajectory stage division result, the adjacent stage speed difference and curvature value are judged, the energy change rate of the candidate stage path is screened and the transfer relationship meeting the preset standard is determined, the stage index path table is generated, and the trajectory stage switching control is obtained.

Images