Multi-modal feature fused real-time movement tracking method and system for clearance machine

Abstract

The invention relates to the technical field of dynamic target processing, in particular to a warehouse cleaning machine real-time motion tracking method and system fusing multi-modal features, and the method comprises the following steps: constructing a multi-modal feature collection array to obtain the multi-modal features of a warehouse cleaning machine, establishing a time sequence motion incidence matrix and kinematics constraints of the clearance machine based on the multi-modal features; performing dual matching on the clearance machine in combination with the time sequence motion incidence matrix and the kinematics constraint to obtain a pose estimation result of the clearance machine; establishing a hybrid tracking framework of the clearance machine, and obtaining a tracking path guiding result of the clearance machine in combination with the pose estimation result; and obtaining a real-time control instruction of the clearance machine according to the tracking path guiding result, and realizing real-time tracking motion of the clearance machine according to the real-time control instruction. According to the invention, the automatic and accurate operation of the bin cleaning machine in a complex dust scene is realized, and the intelligent level of bulk material treatment is obviously improved.

Description

Technical Field

[0001] The present invention relates to the technical field of dynamic target processing, and in particular to a real-time motion tracking method and system for a warehouse clearance machine integrating multimodal features. Background Art

[0002] In industrial warehouse clearance scenarios, traditional target tracking technologies primarily rely on single-modality sensor data for equipment positioning. Typical approaches include geometric feature tracking based on LiDAR point cloud matching, using iterative closest point algorithms to estimate equipment pose, or employing a visual framework to construct motion trajectories using feature points in image sequences. Some advanced systems attempt to integrate inertial measurement unit data, achieving multi-sensor data fusion through extended Kalman filtering. Under ideal conditions, centimeter-level positioning accuracy can be achieved, but existing technologies present significant limitations when used with specialized equipment such as warehouse clearance machines, which have complex articulated structures.

[0003] First, there is the problem of perception degradation in dusty environments. When the dust concentration is too high, the laser point cloud will have data missing, resulting in feature matching failure; second, dynamic motion modeling is insufficient. The traditional motion model does not consider the coupled motion characteristics of the articulated structure, and the tracking error is large in large posture change scenarios such as loading operations; finally, multimodal data fusion is inefficient. The traditional solution has obvious timing alignment errors and cannot meet real-time control requirements.

[0004] In view of this, the present invention proposes a real-time tracking motion method and system for a warehouse clearing machine that integrates multimodal features. By acquiring the multimodal features of the warehouse clearing machine, the temporal motion association matrix and kinematic constraints of the warehouse clearing machine are established; the temporal motion association matrix and kinematic constraints are combined to perform double matching on the warehouse clearing machine to obtain the posture estimation result of the warehouse clearing machine; a hybrid tracking framework is established, and the tracking path guidance result of the warehouse clearing machine is obtained in combination with the posture estimation result; the real-time control instructions of the warehouse clearing machine are obtained according to the tracking path guidance result, so as to realize the real-time tracking motion of the warehouse clearing machine; and the real-time tracking of the warehouse clearing machine is realized in a dust interference environment. Summary of the Invention

[0005] In view of the defects in the prior art, the present invention provides a real-time tracking method and system for a warehouse clearance machine that integrates multimodal features.

[0006] In order to achieve the above-mentioned objectives, in a first aspect, the present invention provides a real-time tracking motion method for a warehouse clearing machine that integrates multimodal features, the method comprising the following steps: constructing a multimodal feature acquisition array to obtain the multimodal features of the warehouse clearing machine, and establishing a temporal motion association matrix and kinematic constraints of the warehouse clearing machine based on the multimodal features; performing double matching on the warehouse clearing machine in combination with the temporal motion association matrix and the kinematic constraints to obtain a posture estimation result of the warehouse clearing machine; establishing a hybrid tracking framework for the warehouse clearing machine, and obtaining a tracking path guidance result of the warehouse clearing machine in combination with the posture estimation result; obtaining a real-time control instruction of the warehouse clearing machine based on the tracking path guidance result, and realizing the real-time tracking motion of the warehouse clearing machine according to the real-time control instruction. The present invention constructs a motion perception system for the bin clearing machine by fusing multimodal features, effectively solving the perception degradation problem of a single sensor in a dusty environment. The dual matching mechanism of the temporal motion correlation matrix and the kinematic constraint not only captures the dynamic behavior pattern of the equipment, but also ensures that the posture estimation conforms to physical reality, which helps to improve tracking accuracy. The hybrid tracking framework integrates the advantages of Kalman filtering and particle filtering, while ensuring real-time performance, reducing the tracking error of posture changes. The final output of real-time control instructions realizes the autonomous and precise operation of the bin clearing machine in complex industrial scenarios, significantly improving the intelligence level of bulk material handling operations.

[0007] Optionally, constructing a multimodal feature acquisition array to obtain the multimodal features of the bin clearing machine, and establishing the temporal motion association matrix and kinematic constraints of the bin clearing machine based on the multimodal features, includes: combining a laser radar device, an image acquisition device, and a kinematic sensor to construct the multimodal feature acquisition array to obtain the multimodal features; obtaining the point cloud temporal motion features of the bin clearing machine based on the multimodal feature acquisition array, and establishing the temporal motion association matrix based on the point cloud temporal motion features; obtaining the articulated structure features of the bin clearing machine based on the multimodal feature acquisition array, and establishing the kinematic constraints based on the articulated structure features. The present invention constructs an omnidirectional sensing array through the multimodal fusion of laser radar, cameras, and kinematic sensors, effectively breaking through the limitations of dusty environments on single-modal sensing. Point cloud temporal feature extraction captures the dynamic mode of equipment motion, and articulated structure modeling accurately depicts the mechanical kinematic characteristics. The bimodal constraint mechanism ensures that pose estimation is both physically realistic and environmentally adaptable, significantly enhancing the autonomous operation capability of the bin clearing machine in complex industrial scenarios.

[0008] Optionally, establishing the temporal motion association matrix based on the temporal motion features of the point cloud includes: ; in, For the moment and time The temporal motion correlation matrix between is the activation function, is the weight matrix, For the moment The eigenvector of For the moment The eigenvector of For the moment and time The displacement increment between This paper introduces motion displacement increments to construct a temporal motion correlation matrix, achieving accurate spatiotemporal modeling in dynamic environments. The attention mechanism combined with the weight matrix effectively captures the temporal correlation pattern of point cloud feature vectors.

[0009] Optionally, the combination of the temporal motion association matrix and the kinematic constraint performs a dual matching on the warehouse clearing machine to obtain a pose estimation result of the warehouse clearing machine, including: performing temporal feature matching on the warehouse clearing machine based on the temporal motion association matrix to obtain a first matching result of the warehouse clearing machine; performing kinematic feature matching on the warehouse clearing machine according to the kinematic constraint to obtain a second matching result of the warehouse clearing machine; and jointly analyzing the first matching result and the second matching result to obtain the pose estimation result. The present invention achieves dual matching of the pose estimation of the warehouse clearing machine through the synergistic effect of temporal feature matching and kinematic constraint matching. The temporal motion association matrix captures the dynamic behavior pattern of the equipment, and the kinematic constraint strictly limits the mechanical feasibility. The joint analysis makes the pose estimation both environmentally adaptable and physically reasonable. It enhances the accuracy of autonomous operation of equipment in complex industrial scenarios.

[0010] Optionally, the temporal feature matching of the warehouse clearing machine based on the temporal motion association matrix is ​​performed to obtain the first matching result of the warehouse clearing machine, including: constructing a hierarchical feature extraction network, extracting features from the multimodal features based on the hierarchical feature extraction network to construct a multimodal feature vector of the warehouse clearing machine; performing real-time prediction of the multimodal feature vector based on a regression network to obtain the real-time free posture state of the warehouse clearing machine; establishing a residual adaptive dynamic threshold, and verifying the real-time free posture state according to the residual adaptive dynamic threshold to construct the first matching result. The present invention deeply mines multimodal data through a hierarchical feature extraction network, constructs a multimodal feature vector containing spatial geometry and motion semantics, and the regression network realizes real-time prediction of posture state to meet the real-time requirements of warehouse clearing operations. The residual adaptive threshold mechanism improves the detection rate of abnormal states by dynamically adjusting the verification threshold. Through synergy, the robustness of posture estimation under complex working conditions is enhanced.

[0011] Optionally, the kinematic feature matching of the warehouse clearing machine according to the kinematic constraints to obtain a second matching result of the warehouse clearing machine includes: establishing a kinematic model of the warehouse clearing machine based on the kinematic constraints, and obtaining the kinematic features of the warehouse clearing machine according to the kinematic model combined with the multimodal features; establishing a feature optimization function, and optimizing the kinematic features according to the feature optimization function to obtain the optimized motion features of the warehouse clearing machine; and obtaining the second matching result according to the kinematic model combined with the optimized motion features. The present invention achieves enhanced physical credibility of the position estimation of the warehouse clearing machine through the synergistic effect of kinematic constraint modeling and feature optimization. The kinematic model makes the position estimation consistent with the actual kinematic characteristics of the equipment. The feature optimization function fuses multimodal data and reduces the prediction error through nonlinear optimization, effectively improving the reliability of the autonomous operation of the warehouse clearing machine in a dusty environment.

[0012] Optionally, the hybrid tracking framework of the warehouse clearing machine is established, and the tracking path guidance result of the warehouse clearing machine is obtained in combination with the posture prediction result, including: establishing the hybrid tracking framework in combination with Kalman filtering and particle filtering; obtaining the real-time state vector of the warehouse clearing machine, and establishing the real-time state equation of the warehouse clearing machine in combination with the Kalman filtering to obtain the Kalman filtering prediction result; establishing a tracking error feedback mechanism, and in combination with the posture prediction result, using the particle filtering to correct and update the Kalman filtering prediction result to obtain the tracking path guidance result. The present invention achieves a balance between accuracy and efficiency of warehouse clearing machine path tracking through a hybrid architecture of Kalman filtering and particle filtering. The Kalman filtering processes linear motion components to ensure real-time performance, and the particle filtering captures nonlinear posture changes to maintain accuracy. The error feedback mechanism improves the response speed to emergencies by dynamically adjusting the filter parameters.

[0013] Optionally, obtaining a real-time control instruction for the bin clearing machine based on the tracking path guidance results, and implementing the real-time tracking movement of the bin clearing machine according to the real-time control instruction, includes: performing global path mapping based on the tracking path guidance results to obtain a tracking path deviation for the bin clearing machine; dynamically adjusting the real-time control coefficient of the bin clearing machine based on the tracking path deviation, and obtaining the real-time control instruction based on the real-time control coefficient. The present invention achieves precise closed-loop control of the bin clearing machine's movement through the dynamic coupling of the path deviation and the control coefficient. The real-time feedback mechanism constructed based on the tracking path guidance results shortens the correction response delay. The dynamic adjustment algorithm adaptively optimizes the control coefficient based on the deviation, significantly enhancing the controllability of the bin clearing machine's operation.

[0014] Optionally, dynamically adjusting the real-time control coefficient of the bin clearing machine according to the tracking path deviation, and obtaining the real-time control instruction based on the real-time control coefficient, includes: ; in, For real-time control instructions, is the proportionality coefficient, To track the path deviation, is the differential coefficient. This invention achieves precise motion control of a bin clearing machine through real-time control instructions. The proportional term ensures rapid correction of directional errors, while the differential term predicts deviation trends, effectively suppressing overshoot and oscillation, enhancing equipment operating efficiency and cost-effectiveness in complex industrial scenarios.

[0015] In the second aspect, the present invention provides a real-time tracking motion system for a warehouse clearing machine that integrates multimodal features. The system executes the real-time tracking motion method for a warehouse clearing machine that integrates multimodal features provided by the present invention. The system includes an input device, an output device, a processor, and a memory. Its benefits are: the hardware facilities integrated by the present invention have excellent performance, the input device, the output device, the processor, and the memory are interconnected, and information transmission between the various components is smooth. Through the interaction of multiple hardware facilities, an efficient information processing system is constructed. The present invention effectively improves the stability of the system through the efficient coordination of hardware facilities, realizes real-time processing and closed-loop control of multimodal data, meets the real-time requirements of warehouse clearing operations, and ensures the security and traceability of operation data, thereby improving the comprehensive operating efficiency of warehouse clearing machines in dusty environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is a flow chart of a method for real-time tracking movement of a warehouse clearance machine integrating multimodal features according to an embodiment of the present invention;

[0017] Figure 2 This is a framework diagram of a real-time motion tracking system for a warehouse clearance machine integrating multimodal features according to an embodiment of the present invention. DETAILED DESCRIPTION

[0018] Specific embodiments of the present invention will be described in detail below. It should be noted that the embodiments described herein are for illustrative purposes only and are not intended to limit the present invention. In the following description, numerous specific details are set forth to provide a thorough understanding of the present invention. However, it will be apparent to one of ordinary skill in the art that these specific details are not necessarily required to practice the present invention. In other instances, well-known circuits, software, or methods are not specifically described to avoid obscuring the present invention.

[0019] Throughout this specification, references to "one embodiment," "an embodiment," "an example," or "an example" mean that a particular feature, structure, or characteristic described in connection with the embodiment or example is included in at least one embodiment of the present invention. Therefore, appearances of the phrases "in one embodiment," "in an embodiment," "an example," or "an example" in various places throughout this specification are not necessarily all referring to the same embodiment or example. Furthermore, the particular features, structures, or characteristics may be combined in any suitable combinations and / or subcombinations in one or more embodiments or examples. Furthermore, those of ordinary skill in the art will appreciate that the figures provided herein are for illustrative purposes only and are not necessarily drawn to scale.

[0020] See Figure 1 One embodiment of the present invention provides a method for real-time tracking of a warehouse clearance machine by integrating multimodal features. The method comprises the following steps:

[0021] S1. Construct a multimodal feature acquisition array to obtain the multimodal features of the warehouse clearance machine, and establish a temporal motion association matrix and kinematic constraints of the warehouse clearance machine based on the multimodal features.

[0022] S1 specifically includes the following steps:

[0023] S11. Construct the multimodal feature acquisition array by combining a laser radar device, an image acquisition device, and a kinematic sensor to obtain the multimodal features.

[0024] In this embodiment, a multimodal feature acquisition array is constructed to obtain the multimodal features of the warehouse clearance machine, and a laser radar device, an image acquisition device and a kinematic sensor are comprehensively integrated to form a collaborative perception device of the warehouse clearance machine.

[0025] Specifically, first, based on the working environment of the warehouse clearing machine, a high-precision, high-frequency LiDAR is selected and installed on the top of the warehouse clearing machine and at key detection locations of the warehouse clearing machine. Through three-dimensional scanning, point cloud data of the environment around the warehouse clearing machine is obtained, including but not limited to the shape of the material pile, the location of obstacles, and spatial dimensions. At the same time, multiple sets of image acquisition devices are deployed, including high-definition cameras and depth cameras, covering the front, side, and top fields of view of the warehouse clearing machine, capturing visual information of the working scene from multiple angles, including material texture, color distribution, and equipment operating status. In addition, integrated kinematic sensors, including inertial measurement units, encoders, and gyroscopes, collect kinematic parameters such as the position, velocity, and acceleration of the warehouse clearing machine in real time.

[0026] Furthermore, to ensure the spatiotemporal consistency of the data, the LiDAR and image acquisition device need to be calibrated in spatiotemporal synchronization, and hardware triggering or software timestamp alignment technology is used to achieve accurate fusion of point cloud and image data. During the multimodal data fusion process, an efficient data preprocessing process is designed, including point cloud denoising, image distortion correction, and sensor data filtering to eliminate noise interference. Through the multi-sensor information fusion algorithm, the geometric features of the LiDAR, the visual features of the image, and the kinematic parameters are temporally and spatially correlated to generate a comprehensive feature set that includes environmental perception, device status, and motion trajectory, providing a data foundation for the subsequent construction of the temporal motion correlation matrix and kinematic constraint analysis.

[0027] S12. Acquire the point cloud temporal motion features of the warehouse clearance machine according to the multimodal feature acquisition array, and establish the temporal motion association matrix based on the point cloud temporal motion features.

[0028] In this embodiment, during the operation of the bin clearing machine, a multimodal feature acquisition array simultaneously collects lidar point cloud data, image data, and kinematic sensor data. The lidar scans the bin clearing machine's surroundings at high frequency, generating a three-dimensional point cloud sequence containing spatial coordinates and reflection intensities. The image acquisition device captures the texture and color information of the operating scene, assisting in the object classification of the point cloud data. The kinematic sensor records the bin clearing machine's motion parameters such as displacement, velocity, and attitude angle in real time.

[0029] Specifically, the multimodal data are aligned in time and space to establish a temporal motion correlation matrix. Timestamp synchronization is used to ensure that the point cloud frames, image frames and kinematic data correspond one to one in the time dimension. The coordinate system conversion algorithm is used to convert the lidar coordinate system, camera coordinate system and the warehouse clearance machine body coordinate system to the global coordinate system.

[0030] Furthermore, the sliding window method is used to segment the point cloud data. The local motion characteristics of the warehouse clearing machine are extracted in each window, and the dynamic parameters such as instantaneous velocity and acceleration are calculated in combination with the kinematic sensor data to form a point cloud temporal motion feature set containing spatiotemporal information. Based on the preprocessed point cloud temporal motion features, the core of constructing the temporal motion correlation matrix is ​​to quantify the correlation between the motion states of the warehouse clearing machine under different time steps.

[0031] First, feature extraction is performed on the point cloud data within each time window. Principal component analysis or point cloud registration algorithms are used to calculate the rigid body transformation matrix (including translation vectors and rotation matrices) between adjacent frames to characterize the local motion trajectory of the warehouse clearing machine. At the same time, combined with kinematic sensor data, parameters such as velocity and acceleration are mapped to the point cloud feature space to form a multidimensional feature vector. Subsequently, the similarity between feature vectors at different time steps is calculated using the autocorrelation function or mutual information algorithm to construct a temporal motion correlation matrix that satisfies the following relationship:

[0032]

[0033] in, For the moment and time The temporal motion correlation matrix between is the activation function, is the weight matrix, For the moment The eigenvector of For the moment The eigenvector of For the moment and time The displacement increment between Indicates transpose.

[0034] In an optional embodiment, in order to enhance the robustness of the matrix, kinematic constraints are introduced, and the constraints are integrated into the matrix optimization process through the Lagrange multiplier method to ensure that the correlation matrix conforms to the actual kinematic laws of the bin clearing machine.

[0035] The accuracy of the temporal motion association matrix is ​​ensured through verification and iterative optimization. On the one hand, a standard motion trajectory is generated using a simulation environment or historical operation data. The matrix prediction results are compared with the actual trajectory, and the matrix performance is evaluated using mean square error or trajectory overlap indicators. If the error exceeds a threshold, the feature extraction parameters or association calculation weights are adjusted. On the other hand, corrections are made based on the semantic segmentation results of the image acquisition device. If an obstacle is identified in the image, the motion prediction value of the corresponding area in the association matrix is ​​attenuated using a Kalman filter to avoid false associations caused by point cloud noise. In addition, for the repetitive operation scenarios of the warehouse clearing machine, a recurrent neural network is introduced to model the long-term dependencies of the temporal motion association matrix, further improving the temporal motion association matrix's ability to express periodic motion patterns. The final output temporal motion association matrix will serve as a key input to the warehouse clearing machine's motion planning and control modules, supporting the clearing machine's autonomous decision-making and collaborative operation capabilities.

[0036] S13. Acquire the hinge structure features of the warehouse clearance machine according to the multimodal feature acquisition array, and establish the kinematic constraints based on the hinge structure features.

[0037] First, a multimodal data fusion framework for extracting articulated joint features was constructed. LiDAR point cloud data was used to locate the 3D coordinates of the articulated joint, and information from the image acquisition device was combined to identify the geometric outline and connection structure of the articulated components. Kinematic sensor data was used to obtain the real-time angle, angular velocity, and torque parameters of the articulated axis. Using a feature-level fusion strategy, the joint positions obtained from point cloud segmentation were spatially aligned with the structural types identified from the image, generating a composite feature vector containing geometric parameters (articulation axis direction, joint range of motion) and kinematic parameters (maximum swing angle, moment of inertia).

[0038] Secondly, a topological model of the articulated structure based on graph theory was established. Each robotic arm of the warehouse clearance machine was simplified into nodes, and the articulated joints were converted into directed edges to construct a weighted directed graph. The edge weights were determined by the articulation type, number of degrees of freedom, and range of motion. The joint connection relationships were stored through the adjacency matrix, and the motion transmission relationship of each articulation point was described by combining the Lagrangian dynamic equation. A constraint matrix was introduced to restrict abnormal motion patterns. When a certain articulation angle exceeded the safety threshold, the diagonal elements of the matrix were set to zero, thereby achieving motion locking. The joint parameters were dynamically updated, and the multi-sensor data was fused through Kalman filtering to correct the weight parameters in the topological structure in real time.

[0039] Finally, a hierarchical kinematic constraint rule library is designed. The bottom layer is the physical constraint layer, which establishes kinematic equations based on the characteristics of the articulated structure; the middle layer is the task constraint layer, which sets motion priorities according to the warehouse clearance operation scenario. When working in a narrow space, the articulated joints are forced into a low-degree-of-freedom mode through a group of constraint equations. The top layer is the safety constraint layer, which constructs an articulated structure fault model and maps abnormal sensor data into constraint conditions. When the torque sensor value suddenly changes, the emergency braking constraint is triggered. The constraints at each layer are resolved through the quadratic programming algorithm to generate kinematic constraints, which provide boundary conditions for the trajectory planning of the warehouse clearance machine. The kinematic constraints satisfy the following relationship:

[0040]

[0041] in, is the joint rotation angle of the warehouse clearance machine, is the index variable, represents the kinematic constraint relationship, It is the hinge point of the warehouse clearance machine. is the joint angle of the bin sweeper.

[0042] S2. Perform double matching on the warehouse clearing machine in combination with the temporal motion association matrix and the kinematic constraints to obtain a pose estimation result of the warehouse clearing machine.

[0043] Among them, S2 specifically includes the following steps:

[0044] S21. Perform time sequence feature matching on the clearance machine based on the time sequence motion association matrix to obtain a first matching result of the clearance machine.

[0045] First, a multimodal feature vector is constructed hierarchically and spatially fused. A deep separable convolutional network (DSN) is used as the underlying architecture to construct a four-layer feature pyramid for multi-scale feature extraction. The first layer processes LiDAR point cloud time-series data using 3D convolution kernels to extract spatial occupancy features of the warehouse clearing machine and its working environment. The second layer utilizes 2D convolution to process image sequences, capturing articulated structural deformation and surface texture variations. The third layer deploys 1D temporal convolution to process kinematic sensor data, extracting dynamic parameters such as joint angular velocity and acceleration. Feature maps from each layer are weightedly fused using a channel-wise attention mechanism to generate a primary feature vector containing spatial, temporal, and semantic information. To address the asynchrony of multimodal data, a temporal alignment module is designed. Based on the inter-frame correspondences in the temporal motion association matrix, a dynamic time warping algorithm is used to synchronize the LiDAR and image data streams in time and space. Finally, a fully connected layer constructs a multimodal feature vector that captures the warehouse clearing machine's motion trends, environmental interactions, and structural deformation. This multimodal feature vector is embedded with both row constraints (motion continuity) and column constraints (joint coupling) of the temporal motion association matrix.

[0046] Secondly, a pose prediction network is developed based on the temporal motion correlation matrix. A two-stream regression network architecture is constructed. The main network uses gated recurrent units to process the temporal evolution of the feature vector. The hidden layer dimension is set to 256, and the number of memory units matches the number of articulated joints in the warehouse clearing machine. The temporal motion correlation matrix is ​​expanded into a temporal graph structure. A graph attention network learns the motion transfer patterns between joints and generates a temporal dependency weight matrix. This weight matrix is ​​operated on the hidden states of the gated recurrent unit to explicitly encode the kinematic constraints. The auxiliary network deploys an encoder that uses a self-attention mechanism to capture long-range temporal dependencies in the feature vector. Its position encoding combines a sine-cosine hybrid function with joint motion amplitude modulation. The output of the two-stream network is dynamically weighted by a gated fusion unit, with the fusion weight determined by the strength of the kinematic constraints at the current moment. Finally, a fully connected regression layer outputs the pose parameters. During the training phase, supervision from the temporal motion correlation matrix is ​​introduced to ensure that the prediction results strictly meet kinematic feasibility.

[0047] Finally, a residual-driven adaptive threshold verification mechanism is proposed. A three-level residual calculation system is designed: the first-level residual is the difference between the predicted pose and the actual pose at the previous moment, reflecting short-term motion fluctuations; the second-level residual is the proximity of the predicted trajectory to the kinematic constraint boundary, quantifying feasibility risks; and the third-level residual is the eigenvalue of the covariance matrix of multi-sensor observations, representing data credibility. An adaptive threshold model is constructed based on the residual sequence, and a sliding window is used to calculate the residual mean and standard deviation. A dynamic threshold is set. When the real-time residual exceeds the threshold, the verification process is triggered: the predicted pose is first smoothed and corrected using a Kalman filter. If it still exceeds the limit, alternative constraint rules are activated (such as reducing the degrees of freedom of the articulated joint); if the limit is continuously exceeded, the safety mode is activated, forcing the warehouse clearing machine to execute the predefined safety trajectory. The verification result is fed back to the feature extraction network in the form of a confidence score, forming a closed-loop optimization.

[0048] S22. Perform kinematic feature matching on the bin clearance machine according to the kinematic constraints to obtain a second matching result of the bin clearance machine.

[0049] In this embodiment, based on the kinematic constraints of the articulated structure, a coordinate transformation matrix is ​​established by combining the connecting rod length, torsion angle, offset and joint angle of each joint of the warehouse clearing machine; the sliding vector parameter is introduced to expand and correct the coordinate transformation matrix; and then the kinematic model is constructed by combining the joint spatial position, acceleration and angular velocity information obtained from the lidar point cloud data.

[0050] Specifically, the coordinate transformation matrix satisfies the following relationship:

[0051]

[0052] in, is the coordinate transformation matrix, For around Axis rotation angle The rotation matrix of For the Axis translation distance The translation matrix, For the Axis translation distance The translation matrix, For around Axis rotation angle The rotation matrix of .

[0053] The sliding vector parameters satisfy the following relationship:

[0054]

[0055] in, is the sliding vector parameter, is the unit vector of the joint sliding direction, is the translation of the joint motion, is transposed.

[0056] The sliding vector parameter is introduced to expand and correct the coordinate transformation matrix to satisfy the following relationship:

[0057]

[0058] in, is the coordinate transformation matrix after expansion and correction, is the coordinate transformation matrix, is the translation matrix of the sliding vector parameters.

[0059] The kinematic model is constructed by combining the joint spatial position, acceleration, and angular velocity information obtained from the lidar point cloud data. The kinematic model satisfies the following relationship:

[0060]

[0061] in, is the position of the clearance machine joint, is the coordinate transformation matrix after expansion and correction, is the parameter correction function, is the spatial position of the clearance machine joint, is the acceleration of the clearance machine joint, is the angular velocity of the clearance machine joint.

[0062] Furthermore, the mapping relationship between joint drive torque and motion state is established through the Lagrangian dynamic equations, converting the kinematic constraints into equality and inequality constraints. In particular, to address the nonlinear errors caused by wear in the articulated structure, a neural network compensation term is used to correct the basic kinematic model. Its input layer is the historical sequence of joint angles, and the output layer is the model error compensation. The number of hidden layer nodes is determined by genetic algorithm optimization. A complete model architecture is generated, including a forward kinematics solver and an inverse kinematics solver. The forward kinematics solver is used to verify the consistency of the model output with the actual sensor values, while the inverse kinematics solver serves as the basis for feature optimization.

[0063] In this embodiment, a two-stage feature optimization process is designed. First, a multi-objective optimization function is constructed, encompassing kinematic feasibility, physical consistency, and task relevance. The kinematic feasibility term uses a penalty function to address joint angle and velocity violations; the physical consistency term introduces contact force constraints; and the task relevance term uses weights tailored to the type of clearing operation (e.g., bulk material pushing or pile shaping). In the second stage, an improved particle swarm optimization algorithm is employed to search for the optimal feature set that satisfies all constraints. Particle positions are encoded using real-number encoding of joint angles, and the velocity update strategy incorporates an adaptive inertia weight adjustment mechanism. When particles reach a local optimum, a chaotic mapping is used to initialize the positions of some particles. The optimization process employs a dual-loop iteration: the inner loop performs single-step prediction, constraint verification, and feature correction, while the outer loop dynamically adjusts the optimization objective weights based on the operation progress. Ultimately, an optimized kinematic feature set that satisfies the constraints is output, and its stability within the confidence interval is verified using the Monte Carlo method.

[0064] In this embodiment, a three-level matching result generation mechanism is established: the basic level applies optimized motion features to perform inverse kinematics solution and generate initial pose estimation values; the intermediate level introduces a Kalman filter to fuse the kinematic model prediction values ​​with the multi-sensor measured values ​​and update the state covariance matrix; the advanced matching layer constructs an operation scene perception module, which uses a deep learning model to identify environmental characteristics such as the material pile shape and collapse risk, and dynamically adjusts the weight coefficient of the kinematic constraint.

[0065] Specifically, when working at the edge of a pile, the contact force constraint weight is increased, while joint angle constraints are relaxed to maintain operational continuity. The matching results are ultimately represented using a probability graph, with each candidate pose associated with a confidence score. The scoring function comprehensively considers model prediction error, sensor consistency, and task priority. These three scores are then fused to generate an optimal pose estimate, providing a comprehensive basis for subsequent trajectory planning.

[0066] S23. Jointly analyze the first matching result and the second matching result to obtain the posture estimation result.

[0067] In this embodiment, a joint time-series-kinematics state-space model is constructed. The time-series feature matching results (including position coordinate sequences and velocity vector fields) are mapped to the kinematic constraint matching results (including joint angle sets and contact force distributions) to obtain a state vector. Using the extended Kalman filter framework, a state transfer matrix is ​​designed to integrate the dynamic prediction capabilities of the time-series motion correlation matrix with the physical rules of the kinematic constraints. Through the covariance matrix weighting strategy, a higher confidence is assigned to the high-frequency motion components in the time-series features, and a lower process noise is set for the low-frequency rigid parameters in the kinematic constraints. A sliding window mechanism is introduced to construct a state history queue, and the posture evolution trend is captured through a long short-term memory network to correct the trajectory drift that may be caused by a single time-series matching.

[0068] Furthermore, the joint state vector is used as the optimization variable to construct a dual-objective function consisting of a temporal reprojection error term and a kinematic constraint violation. The temporal error term uses a reprojection error model to calculate the Mahalanobis distance between the predicted pose and the laser point cloud observation. The kinematic constraint violation is based on the Lagrange multiplier method, which converts the kinematic equations into equality constraints. The solution is iteratively solved using the Levenberg-Marquardt algorithm, with the damping factor dynamically adjusted in each iteration: increasing the damping factor to strengthen the constraint when the temporal error dominates, and decreasing the damping factor to relax the rigid constraints when the constraint conflict is severe. A special constraint relaxation strategy is designed to use soft constraints for temporarily unsatisfied articulation angle constraints, and using a penalty function to convert hard constraints into differentiable optimization terms.

[0069] In an optional embodiment, a confidence assessment and feedback correction mechanism for the pose estimation results is established. Based on the Monte Carlo method, random sampling is performed on the optimization process to generate a covariance ellipse of the pose distribution, and the dispersion index of the estimation result is obtained by the area of ​​the ellipse. When the dispersion exceeds the threshold, the multi-hypothesis tracking algorithm is triggered, and multiple candidate poses are retained for parallel evolution. A priori knowledge base of the warehouse clearance operation scene is introduced to match the pose patterns under typical working conditions (such as pile climbing and wall contact). The similarity between the current estimation result and the historical pattern is calculated through the dynamic time warping algorithm, and the abnormal pose is corrected. The final output includes a pose estimation result of three-dimensional coordinates, quaternion posture and confidence score, which provides an interference-resistant warehouse clearance machine pose prior for subsequent trajectory planning.

[0070] S3. Establish a hybrid tracking framework for the warehouse cleaning machine, and obtain a tracking path guidance result for the warehouse cleaning machine in combination with the pose estimation result.

[0071] In this example, a hybrid tracking framework for a warehouse clearing machine is established. Based on the nonlinear characteristics of the warehouse clearing machine's operating scenario, a master-slave filter coupling architecture is designed. An extended Kalman filter is used as the master filter to handle linearized state propagation, while a particle filter is used as the slave filter to compensate for nonlinear errors. The framework consists of three core modules: a state prediction module, an error correction module, and a decision fusion module. The state prediction module uses the warehouse clearing machine's kinematic differential equations to construct a state transition model, incorporating parameters such as joint angles, angular velocities, and body pose into the state vector. The error correction module establishes an observation model, fusing LiDAR point cloud registration results, binocular vision pose estimation, and encoder measurements to form an observation vector. The decision fusion module uses a covariance crossover algorithm to dynamically adjust the output weights of the Kalman filter and particle filter, with the weight coefficients determined by the historical error mean and variance. Specifically, to address the multi-body system characteristics of the articulated warehouse clearing machine, a joint space decoupler is embedded in the framework to decompose the warehouse clearing machine's state vector into base and boom states, enabling hierarchical filtering.

[0072] Specifically, a continuous-discrete hybrid state space model is constructed based on the dynamic equations of the warehouse clearing machine. The state equations in the continuous-time domain are described using the Newton-Euler method, and the state equations are discretized to obtain the state transition matrix and process noise covariance of the prediction step. The observation equation adopts a nonlinear form, including the lidar coordinate transformation matrix, the visual pose projection matrix, and the encoder angle mapping function. The initialization phase uses the pose estimation results to set the initial state and initial covariance. The initial state includes the estimated three-dimensional position, Euler angles, and joint angles. In the prediction-update loop, the Kalman filter performs a standard five-step process: state prediction, covariance prediction, Kalman gain calculation, state update, and covariance update.

[0073] Furthermore, a particle correction mechanism driven by tracking error feedback is designed; a dual closed-loop correction architecture is designed: the outer loop is for posture error feedback, and the inner loop is for joint error compensation. The posture error is calculated by the difference between the current Kalman filter prediction value and the particle filter optimal estimate. The posture error is input into the fuzzy controller to generate particle weight adjustment coefficients and resampling thresholds. The joint error is decomposed into corrections for each joint angle using an inverse kinematic model. The particle filter uses an adaptive resampling strategy, executing system resampling when the number of valid particles falls below a threshold.

[0074] In an optional embodiment, an adaptive fusion engine is constructed to adjust filter weights in real time based on the characteristics of the operational scenario. A scenario complexity metric is defined. The fusion process utilizes an interactive multi-model approach, maintaining multiple parallel filter branches, each corresponding to different noise statistical characteristics. Branches interact through model probabilities.

[0075] Furthermore, the path guidance results are generated and optimized; a two-level path generator is designed: the global planning layer generates a reference trajectory based on the hybrid filter output, and the local adaptation layer implements real-time trajectory correction. The global planning layer uses spline curve interpolation to connect discrete pose estimation points into a continuous path. The local adaptation layer constructs a model predictive control framework, takes the predicted state of the next N time steps as the optimization object, and solves the optimization problem containing kinematic constraints, dynamic constraints, and operation constraints. The optimization results are obtained through a quadratic programming solver, and the control instructions are updated in the time step cycle; in response to the contact force constraints unique to the clearing operation, a hybrid control term is added to the cost function, and the clearing machine operation is realized through the impedance control model. The final generated path guidance result contains the triple instructions of desired pose, joint angle, and contact force, and the path guidance result is executed by the underlying controller.

[0076] S4. Obtain a real-time control instruction of the bin clearing machine according to the tracking path guidance result, and implement real-time tracking movement of the bin clearing machine according to the real-time control instruction.

[0077] In this embodiment, accurate mapping of the global path to the local deviation is achieved; based on the path guidance results, a mapping relationship between the three-dimensional space coordinate system and the coordinate system of the warehouse clearing machine body is established. First, the iterative closest point algorithm is used to achieve coarse alignment of the lidar point cloud and the preset path, and the initial deviation between the current position of the warehouse clearing machine and the global path is obtained. A local path window is further constructed, and the path segments within the next 5 seconds are intercepted for smoothing to eliminate the discretization error of the global path. Dynamic path decomposition is implemented within the local window, and the continuous trajectory is decomposed into a sub-target sequence of three dimensions: position, attitude, and speed. The position deviation is calculated by the Euclidean distance between the origin of the current body coordinate system and the path point. The attitude deviation uses the quaternion difference method to obtain the rotation axis and angle. The speed deviation is calculated by the dot product of the expected speed vector and the actual speed vector. According to the multi-body system characteristics of the articulated warehouse clearing machine, an independent deviation calculation model for the base and arm is established. The base deviation is used as the main control quantity, and the arm deviation is used as the compensation quantity. A comprehensive deviation vector is generated through weighted fusion, including spatial position error, attitude angle error, linear velocity error and angular velocity error, providing quantitative input for subsequent control parameter adjustment.

[0078] Specifically, a dynamic parameter adjustment mechanism for multimodal control coefficients is constructed; a hierarchical parameter adjustment strategy based on deviation characteristics is designed, and a collaborative architecture is established between the controller parameter library and the fuzzy inference engine. First, a deviation-parameter mapping table is established, classifying deviations into three levels: small, medium, and large. Each level corresponds to a preset control parameter combination. For small deviations, a high-precision fine-tuning mode is adopted, increasing the proportional coefficient to improve response speed while reducing the integral coefficient to avoid overshoot. For large deviations, a fast convergence mode is switched to significantly improve the differential coefficient and suppress system inertia. A fuzzy logic controller is further introduced, taking the deviation as the input variable and generating parameter corrections using a fuzzy rule base. The fuzzy set definition uses trapezoidal membership functions, and the rule formulation follows the principle of "large deviations lead to higher gains, small deviations lead to lower gains." A joint-level parameter correction model is established to address the rotational characteristics of the articulated joints. Differentiated parameter adjustments are implemented based on the deviation contribution of each arm. The deviation weight of the arm end effector is set to 1.5 times that of the base. The final generated control coefficients include three parts: basic control parameters, fuzzy correction and joint compensation, forming a spatiotemporal adaptive control parameter matrix.

[0079] Furthermore, multi-level control instructions are generated and executed; a three-level instruction generation architecture is constructed to achieve vertical integration from high-level strategy to low-level execution. The first level is the task planning layer, where deviations and control coefficients are input into the model predictive control module to generate the optimal control sequence. The optimization objectives include path tracking accuracy, energy consumption, and joint stress, and a comprehensive cost function is constructed using a weighted sum function. The second level is the motion control layer, where the discrete control points of the optimization output are interpolated into a continuous instruction curve. A trapezoidal velocity planning algorithm is used to ensure the continuity and differentiability of the instructions. To address the multi-degree-of-freedom characteristics of the articulated structure, an inverse kinematic solution is implemented to decompose the overall control instructions into independent control variables for each joint. The solution process takes into account joint limits and singular configuration avoidance. The third level is the drive execution layer, which establishes bidirectional closed-loop control between the hydraulic system and the motor driver. To meet the contact force control requirements of the warehouse clearance operation, an impedance control module is embedded in the drive layer, which achieves the operation by adjusting the desired stiffness and damping coefficients. The final real-time control instructions output include three sets of control variables: hydraulic cylinder thrust, motor speed, and joint angle. The real-time control instructions satisfy the following relationship:

[0080]

[0081] in, For real-time control instructions, is the proportionality coefficient, To track the path deviation, is the differential coefficient.

[0082] The control instruction generation mechanism improves the path tracking accuracy of the warehouse clearing machine, shortens the joint response time, and reduces operating energy consumption in complex material yard environments through the triple guarantees of deviation quantification, parameter adaptation, and instruction layering.

[0083] See Figure 2 In an optional embodiment, the present invention provides a real-time tracking system for a warehouse clearance machine that integrates multimodal features. The system includes an input device, an output device, a processor, and a memory, wherein the hardware components are interconnected. The memory is used to store a computer program, which includes program instructions. The processor is configured to invoke the program instructions and execute the specific steps of the embodiments of the method for real-time tracking of a warehouse clearance machine that integrates multimodal features provided by the present invention. The real-time tracking system for a warehouse clearance machine that integrates multimodal features provided by the present invention has a complete structure, is objective and stable, and enhances the overall applicability and practical application capabilities of the present invention.

[0084] In summary, the method of the present invention provides a real-time tracking motion method system for a warehouse clearing machine that integrates multimodal features. By acquiring the multimodal features of the warehouse clearing machine, a temporal motion association matrix and kinematic constraints are established to perform double matching on the warehouse clearing machine, thereby obtaining the posture estimation result of the warehouse clearing machine; a hybrid tracking framework is established to obtain the tracking path guidance result; and real-time control instructions are obtained based on the tracking path guidance result to realize the real-time tracking motion of the warehouse clearing machine. The method of the present invention is easy to understand, simple to calculate, has a small workload, is convenient for engineering application, and provides a theoretical basis and technical support for the further development of dynamic target processing technology.

[0085] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some or all of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and description of the present invention.

Claims

1. A real-time tracking method for a warehouse clearance machine integrating multimodal features, characterized in that: The steps include: Constructing a multimodal feature acquisition array to obtain multimodal features of the bin clearing machine, and establishing a temporal motion association matrix and kinematic constraints of the bin clearing machine based on the multimodal features; Performing double matching on the bin clearing machine in combination with the temporal motion association matrix and the kinematic constraints to obtain a pose estimation result of the bin clearing machine; Establishing a hybrid tracking framework for the bin clearing machine, and combining the pose estimation result to obtain a tracking path guidance result for the bin clearing machine; A real-time control instruction of the bin clearing machine is obtained according to the tracking path guidance result, and the real-time tracking movement of the bin clearing machine is realized according to the real-time control instruction.

2. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 1 is characterized in that: The step of constructing a multimodal feature acquisition array to obtain multimodal features of the warehouse clearance machine and establishing a temporal motion association matrix and kinematic constraints of the warehouse clearance machine based on the multimodal features includes: Combining a laser radar device, an image acquisition device, and a kinematic sensor to construct the multimodal feature acquisition array to obtain the multimodal features; Acquiring the point cloud temporal motion features of the bin clearing machine according to the multimodal feature acquisition array, and establishing the temporal motion correlation matrix based on the point cloud temporal motion features; The hinge structure features of the bin clearing machine are acquired according to the multimodal feature acquisition array, and the kinematic constraints are established based on the hinge structure features.

3. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 2 is characterized in that: The establishing of the temporal motion association matrix based on the temporal motion features of the point cloud includes: ； in, For the moment and time The temporal motion correlation matrix between is the activation function, is the weight matrix, For the moment The eigenvector of For the moment The eigenvector of For the moment and time The displacement increment between Indicates transpose.

4. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 1 is characterized in that: The step of performing double matching on the bin clearing machine in combination with the temporal motion association matrix and the kinematic constraints to obtain a pose estimation result of the bin clearing machine includes: Performing time sequence feature matching on the warehouse clearance machine based on the time sequence motion association matrix to obtain a first matching result of the warehouse clearance machine; Performing kinematic feature matching on the bin clearance machine according to the kinematic constraints to obtain a second matching result of the bin clearance machine; The first matching result and the second matching result are jointly analyzed to obtain the posture estimation result.

5. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 4 is characterized in that: The performing time sequence feature matching on the clearance machine based on the time sequence motion association matrix to obtain a first matching result of the clearance machine includes: Constructing a hierarchical feature extraction network, and performing feature extraction on the multimodal features based on the hierarchical feature extraction network to construct a multimodal feature vector of the warehouse clearance machine; Performing real-time prediction on the multimodal feature vector based on a regression network to obtain a real-time free pose state of the bin clearing machine; A residual adaptive dynamic threshold is established, and the real-time free pose state is verified according to the residual adaptive dynamic threshold to construct the first matching result.

6. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 4 is characterized in that: The performing kinematic feature matching on the bin clearance machine according to the kinematic constraint to obtain a second matching result of the bin clearance machine includes: Establishing a kinematic model of the bin clearance machine based on the kinematic constraints, and obtaining kinematic characteristics of the bin clearance machine based on the kinematic model and the multimodal characteristics; Establishing a feature optimization function, and optimizing the kinematic characteristics according to the feature optimization function to obtain an optimized motion characteristic of the bin clearing machine; The second matching result is obtained according to the kinematic model and in combination with the optimized motion features.

7. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 1, characterized in that: The step of establishing a hybrid tracking framework for the bin clearing machine and obtaining a tracking path guidance result for the bin clearing machine in combination with the pose estimation result includes: Combining Kalman filtering and particle filtering to establish the hybrid tracking framework; Acquire the real-time state vector of the bin clearing machine, and establish the real-time state equation of the bin clearing machine in combination with the Kalman filter to obtain the Kalman filter prediction result; A tracking error feedback mechanism is established, and the Kalman filter prediction result is corrected and updated using the particle filter in combination with the posture estimation result to obtain the tracking path guidance result.

8. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 1, characterized in that: Obtaining a real-time control instruction for the bin clearing machine based on the tracking path guidance result, and implementing the real-time tracking movement of the bin clearing machine according to the real-time control instruction, includes: Performing global path mapping according to the tracking path guidance result to obtain the tracking path deviation of the warehouse clearance machine; The real-time control coefficient of the bin clearing machine is dynamically adjusted according to the tracking path deviation, and the real-time control instruction is obtained based on the real-time control coefficient.

9. The method for real-time tracking of a warehouse clearance machine by integrating multimodal features according to claim 8, characterized in that: The dynamically adjusting the real-time control coefficient of the bin clearing machine according to the tracking path deviation, and obtaining the real-time control instruction based on the real-time control coefficient, includes: ； in, For real-time control instructions, is the proportionality coefficient, To track the path deviation, is the differential coefficient.

10. A real-time tracking motion system for a warehouse clearance machine integrating multimodal features, characterized in that: The system includes an input device, an output device, a processor and a memory, wherein the input device, the output device, the processor and the memory are interconnected, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is configured to call the program instructions to execute the real-time tracking motion method of a warehouse clearance machine integrating multimodal features as described in any one of claims 1 to 9.

Images