A method for perception and mine truck trajectory prediction based on open-pit mining loading robot

Abstract

This invention belongs to the field of perception technology for unmanned operations in open-pit mines, specifically a method for perception and mine truck trajectory prediction based on an open-pit mine loading robot. The method includes: S1 collecting point cloud data, grayscale images, inertial measurement unit (IMU) data, and absolute pose data of the open-pit mine operation scene; S2 filtering dust and noise points from the collected point cloud data using a combined filtering algorithm to obtain purified point cloud data; S3 extracting point cloud features, image features, and motion features of the target based on the purified point cloud data, the grayscale image, and the IMU data, and identifying and classifying the mine truck, other operating equipment, and personnel through a multi-feature fusion recognition model; S4 calculating the absolute position coordinates of the mine truck in the geodetic coordinate system, its relative position coordinates with respect to the loading robot, and the attitude angle of the mine truck; and S5 predicting the future trajectory of the mine truck.

Description

Technical Field

[0001] This invention belongs to the field of perception technology for unmanned operations in open-pit mines, specifically a method for perception and mine truck trajectory prediction based on open-pit mine loading robots. Background Technology

[0002] Open-pit mines are characterized by open spaces, dense equipment, frequent personnel movement, high dust concentrations, and complex lighting conditions. Traditional manual loading operations suffer from low efficiency, high safety risks, and harsh working environments, making unmanned loading robots a growing trend in the industry. LiDAR, as the core sensing device for unmanned operations, can quickly acquire three-dimensional environmental information. However, its application in open-pit mines faces three major challenges: First, the dusty environment causes severe noise interference in LiDAR data, affecting target recognition accuracy; second, the diverse types and similar shapes of equipment in the mine area make it easy for personnel to confuse small devices, making accurate identification difficult; and third, the flexible movement of mining trucks makes it difficult to control their position and trajectory in real time, increasing the risk of collisions between the loading robot and the truck.

[0003] In existing technologies, lidar target recognition often relies on single features, resulting in insufficient accuracy to meet the demands of complex mining environments. Mine truck pose estimation typically uses single GNSS positioning, which is inaccurate in obstructed mining conditions. Trajectory prediction is often based on simple motion models, making it difficult to adapt to complex motion states such as acceleration and turning of mining trucks. Dust and noise removal often employs single filtering algorithms, making it difficult to balance effective filtering with target point cloud retention. Furthermore, existing solutions often confuse the functions of the lidar's built-in IMU and the main IMU, leading to unclear positioning references and confused data synchronization logic, further impacting the reliability of the sensing system. Therefore, there is an urgent need for an integrated sensing technology that clearly distinguishes the functions of the two IMUs and provides accurate positioning references to address these technical pain points and support the safe and efficient operation of unmanned loading and unloading operations. Summary of the Invention

[0004] To address the aforementioned problems, this invention provides a method for sensing and predicting the trajectory of a loading robot in an open-pit mine.

[0005] This invention adopts the following technical solution: a method for sensing and predicting the trajectory of a loading robot in an open-pit mine, comprising: S1: Collect point cloud data, grayscale images, inertial measurement unit data, and absolute pose data of open-pit mining operation scenarios. All collected data are synchronized via timestamps. S2: Based on the spatial distribution characteristics, corresponding grayscale image features, and data from the built-in inertial measurement unit of the lidar, the collected point cloud data is filtered out by a combined filtering algorithm to remove dust and noise points, thus obtaining purified point cloud data. S3: Based on the purified point cloud data, the grayscale image, and the inertial measurement unit data, extract the point cloud features, image features, and motion features of the target, and identify and classify mining trucks, other operating equipment, and personnel through a multi-feature fusion recognition model; S4: Based on the identified mining truck point cloud data, the purified point cloud data, and the absolute pose of the loading robot, calculate the absolute position coordinates of the mining truck in the geodetic coordinate system, the relative position coordinates of the mining truck with respect to the loading robot, and the attitude angle of the mining truck. S5: Based on the pose estimation results of the mining truck and its historical motion data, a time-series prediction model is used to predict the future motion trajectory of the mining truck.

[0006] In some embodiments, in step S1: The point cloud data and grayscale images are acquired by the lidar on the loading robot. The inertial measurement unit data is collected through the inertial measurement unit built into the lidar on the loading robot; The absolute pose data is acquired through the GNSS+IMU navigation module of the shovel robot, including the absolute position coordinates of the shovel robot in the geodetic coordinate system. With attitude angle.

[0007] In some embodiments, step S2 includes: S21: Dynamic radius statistical filtering removes noise points; Calculate the dust concentration coefficient K using the grayscale variance of a grayscale image: ; Calculate the neighborhood radius R: ; For each point in point cloud data C Search for the number of point clouds k within its neighborhood radius R, and calculate the distance from the neighboring points to the nearest point. The average distance; Calculate the mean of the average distances between all neighboring points. and standard deviation Set threshold range If point of Points outside this range are identified as noise points and removed, while the point cloud after initial filtering is retained. ; S22: Adaptive threshold filtering of dust concentration removes noise points to obtain purified point cloud; Point cloud extraction based on grayscale image assistance Corresponding grayscale value The difference in grayscale value distribution between the target area and the dust area was statistically analyzed to construct an adaptive grayscale threshold. The formula is: ,in, This represents the mean grayscale value corresponding to the point cloud. The standard deviation of the grayscale value. For adaptive adjustment coefficient ; Set acceleration threshold and angular velocity threshold If you dot the clouds Simultaneously satisfy , , ,in , If the acceleration and angular velocity corresponding to the i-th point are given, then it is determined to be a dust noise point and removed, thus obtaining the purified point cloud. .

[0008] In some embodiments, step S3 includes: S31: Cleaned point cloud Feature extraction was performed on grayscale images and inertial measurement unit data to obtain purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features; S32: Perform maximum-minimum normalization on the purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features extracted in S31, and map all feature values ​​to the dimensionless interval [0,1] to eliminate the dimensional differences between different features. The neck feature fusion module of the input CMT-3D model strengthens the weights of key features through an attention mechanism and outputs the target recognition results, including: target category, target bounding box coordinates, and recognition confidence.

[0009] In some embodiments, step S31 includes: S311: From Extracting the geometric and statistical features of the target, including: Volume feature V: Calculated based on the target point cloud bounding box. The bounding box is determined by the maximum / minimum coordinates of the target point cloud, as shown in the formula: In the formula, These represent the maximum and minimum values ​​in the X direction of the target point cloud, respectively. These are the maximum and minimum values ​​in the Y direction of the target point cloud, respectively; These are the maximum and minimum values ​​in the Z direction of the target point cloud, respectively. Normal vector features : Perform plane fitting on the target point cloud and solve for the normal vector features using the covariance matrix; covariance matrix The formula is: right Eigenvalue decomposition is performed, and the eigenvector corresponding to the smallest eigenvalue is the normal vector. ; Point cloud density The ratio of the number of target point clouds to their volume. ; S312: Extract the texture and edge features of the target from the grayscale image, including the mean of the grayscale histogram and the edge gradient magnitude; S313: Extract the target's motion characteristics, including acceleration, from inertial measurement unit data. angular velocity The acceleration of a single point in the point cloud extracted in step S22. angular velocity , where i is the index of a point in the point cloud, calculates the overall average acceleration of all points within the target region. With overall average angular velocity As a core parameter of the target's motion characteristics, the calculation formula is: , Where N is the total number of point clouds within the target area; S314: Extract occlusion compensation features, use the Poisson reconstruction algorithm to complete the geometric contour of the incomplete target point cloud, and extract the volume features and normal vector features of the completed target. S315: The Retinex algorithm is used to perform illumination equalization on the backlit grayscale image, and texture features and edge gradient magnitudes are extracted again.

[0010] In some embodiments, after the CMT-3D model outputs preliminary target recognition results in step S32: To distinguish mining trucks with similar shapes from other operating equipment, a secondary discrimination mechanism is constructed based on volumetric feature V and motion characteristics: Mining Card Volume And acceleration during uniform motion ; Other operating equipment volume And acceleration ; Personnel volume Furthermore, the acceleration fluctuates significantly. .

[0011] In some embodiments, step S4 includes: S41: Calculate the absolute position coordinates of the mining card : in, The coordinates of the centroid of the mining card in the lidar coordinate system are obtained by cloud computing of the mining card's regional points. The point cloud registration correction is obtained through iterative nearest-point algorithm optimization. This is the transformation matrix from the robot's body coordinate system to the geodetic coordinate system. The transformation matrix is ​​between the lidar coordinate system and the loading robot's body coordinate system; S42: Calculate the relative position coordinates of the mining card: Position coordinates of the mining truck relative to the shovel robot Based on the robot's absolute position Absolute position of mining card The formula for calculating the difference is: relative distance The formula is: relative azimuth The formula is: In the formula, A positive value indicates the mining truck is on the right side of the robot, and a negative value indicates it is on the left side; S43: Calculate the mining truck's attitude parameters: Mining truck attitude parameters include pitch angle Roll angle Heading angle Normal vectors based on mining card point clouds The formula for calculating the relationship with the geodetic coordinate system is as follows: in, Let be the normal vector of the mining truck body surface; This is the heading correction amount for the mining truck itself. The heading angle of the mining truck in the geodetic coordinate system; The heading angle of the loading robot in the geodetic coordinate system; The relative azimuth angle between the mining truck and the loading robot; Mining truck's own heading correction amount.

[0012] In some embodiments, in step S41, the objective function of the iterative nearest point algorithm is to minimize the distance error between the mining card point cloud and the standard mining card point cloud model, as expressed in the formula: In the formula, Let j be the coordinates of the j-th point in the actual point cloud of the mining truck. is the coordinate of the corresponding point in the standard mine card point cloud model, and k is the number of registration points.

[0013] In some embodiments, step S5 includes: S51: Extract the historical pose data of the mining truck over the past time period T, including its absolute position in the time series. Heading angle Speed ​​of movement acceleration ; S52: Extract and classify the operational intent features of mining trucks, including: Rate of change of relative distance with the loading robot Rate of change of heading angle And the matching results of the mine area roadway nodes; The determination of mining truck operation intent is based on quantitative characteristics, and the determination rule is as follows: like , If the loading lane node is matched, it is determined that there is an intention to load. like and If so, it is determined to be an intention to avoid; The determined task intent features are encoded as follows: loading intent [1,0,0], avoidance intent [0,1,0], and other intents [0,0,1]. S53: Kalman filtering is used to reduce noise in historical motion data; S54: The noise-reduced motion data The features are concatenated with the operational intent feature encoding, input into an LSTM model, and a time-series prediction model is constructed to output the future mining truck model. absolute position coordinates at time ; During the LSTM model training phase, differentiated loss function weights are set for different task intent features: the weight for loading intent is 1.5; the weight for avoidance intent is 1.3; and the weight for other intents is 1.0. S55: Absolute position coordinates The rules are revised to ensure that the predicted trajectory conforms to the mine area's lane planning. The revision formula is as follows: in, These are the predicted coordinates of the mine card after correction according to the mining area operation rules; For the mine area roadway The coordinates of the center position at that moment; This is the rule correction coefficient, with a range of values. The system adaptively adjusts based on the accuracy of the mine area roadway planning. The original absolute position coordinates output by the LSTM model; S56: Based on Calculate the future movement direction of the mining truck and speed The formulas are as follows: in, The time interval between adjacent prediction times. for The three-dimensional coordinate components; S57: Based on the revision of mining operation rules, a new operation intention revision item is added to perform a second precise revision of the predicted trajectory. The revision formula is as follows: in, The final predicted location coordinates of the mining card after double correction based on the operating rules and operating intentions; The predicted location coordinates of the mine card after only being corrected according to the mining area operation rules. ; To correct the intention, load the intention. , intention to avoid ; The coordinates of standard trajectory nodes in the mining area corresponding to the operational intent are set in advance based on the mining area operation plan. Final output mining card future Predicted trajectory and direction of motion after double correction at time and speed of movement .

[0014] In some embodiments, step S53 includes: S531: Adaptive adjustment process noise covariance matrix Q; in, The pre-defined basic process noise covariance matrix; This is the process noise adjustment factor; The real-time acceleration of the mining truck at time t; S532: Adaptive adjustment of the observation noise covariance matrix R; in: The pre-defined basic observation noise covariance matrix; The noise adjustment factor is used for observation. Let be the efficiency of the lidar point cloud at time t, calculated using the following formula: , For the effective number of point clouds, This represents the total number of point clouds; S533: Compensation for inertial measurement unit data observations of lidar; Real-time acceleration acquired using data from an inertial measurement unit and angular velocity Predicting the position and velocity of mining trucks through numerical integration: in, The sampling time interval for inertial measurement unit data; The predicted values ​​from the inertial measurement unit (IMU) data are weighted and fused with the lidar observation values ​​to obtain the compensated observation vector. : in: These are the observation vectors directly acquired by the lidar; The observation vectors predicted by the IMU; The fusion coefficient is positively correlated with the effectiveness of the lidar point cloud. S534: Based on the above adaptive mechanism, a filter state equation and an observation equation are constructed, and noise reduction is achieved through a "prediction-update" iterative process.

[0015] Compared with the prior art, the present invention has the following beneficial effects: 1. It is clarified that the built-in IMU of the lidar is only used for point cloud motion compensation and auxiliary feature extraction, and the GNSS+IMU combined navigation module of the loading robot is used as the main positioning reference of the system, which solves the problems of IMU function confusion and unclear positioning reference in the existing solutions.

[0016] 2. A combined filtering algorithm is adopted, which combines point cloud spatial features, grayscale features and motion features of the LiDAR built-in IMU. The filtering effect is better than that of a single filtering algorithm, and the target point cloud can be preserved to the greatest extent.

[0017] 3. By adopting a multi-feature fusion model and making secondary discrimination based on volume and motion features, the problem of confusing the identification of equipment and personnel in the mining area is solved. The recognition accuracy and real-time performance meet the requirements of unmanned operation.

[0018] 4. By combining GNSS reference and point cloud registration, and based on coordinate transformation and attitude calculation formulas, accurate calculation of absolute / relative pose is achieved.

[0019] 5. An improved Kalman filter + LSTM model is adopted, combined with the correction of mining operation rules, and based on speed, acceleration and prediction formulas, to realize reliable prediction of the complex motion trajectory of mining trucks, providing lead time for loading robot operation decisions.

[0020] This invention can be directly applied to unmanned loading and unloading operations in open-pit mines, and can also be extended to similar complex scenarios such as ports and mine transportation, with broad engineering application prospects and market value. Attached Figure Description

[0021] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] The shovel-loading robot of this invention is equipped with an Ouster 128-line LiDAR, which simultaneously collects point cloud data, grayscale images, and data from the LiDAR's built-in inertial measurement unit. This data is then timestamped and aligned with the absolute pose data output by the robot's own GNSS+IMU integrated navigation module, constructing an integrated perception system encompassing "dust and noise removal—equipment and personnel identification—mining truck pose estimation—mining truck trajectory prediction." The Ouster 128-line LiDAR is installed on the upper right guardrail of the robot's cab and the front left guardrail of the robot body, ensuring unobstructed radar coverage of the 360° operating environment. The LiDAR and the shovel-loading robot's GNSS+IMU integrated navigation module are connected via Ethernet for synchronous data acquisition. The acquisition frequencies are set as follows: point cloud 10Hz, grayscale image 10Hz, LiDAR built-in IMU 100Hz, and robot navigation module 10Hz.

[0024] A method for sensing and predicting the trajectory of a loading robot in an open-pit mine includes: S1: Collect point cloud data, grayscale images, inertial measurement unit data, and absolute pose data of open-pit mining operation scenarios. All collected data are synchronized via timestamps. S2: Based on the spatial distribution characteristics, corresponding grayscale image features, and data from the built-in inertial measurement unit of the lidar, the collected point cloud data is filtered out by a combined filtering algorithm to remove dust and noise points, thus obtaining purified point cloud data. S3: Based on the purified point cloud data, the grayscale image, and the inertial measurement unit data, extract the point cloud features, image features, and motion features of the target, and identify and classify mining trucks, other operating equipment, and personnel through a multi-feature fusion recognition model; S4: Based on the identified mining truck point cloud data, the purified point cloud data, and the absolute pose of the loading robot, calculate the absolute position coordinates of the mining truck in the geodetic coordinate system, the relative position coordinates of the mining truck with respect to the loading robot, and the attitude angle of the mining truck. S5: Based on the pose estimation results of the mining truck and its historical motion data, a time-series prediction model is used to predict the future motion trajectory of the mining truck.

[0025] In a specific embodiment, in step S1: the point cloud data and grayscale image are acquired by the lidar on the shovel robot; the inertial measurement unit data is acquired by the inertial measurement unit built into the lidar on the shovel robot; the absolute pose data is acquired by the GNSS+IMU navigation module of the shovel robot, including the absolute position coordinates of the shovel robot in the geodetic coordinate system. With attitude angle.

[0026] In a specific embodiment, step S2 includes: S21: Dynamic radius statistical filtering removes noise points; Calculate the dust concentration coefficient K using the grayscale variance of a grayscale image: ; Calculate the neighborhood radius R: ; For each point in point cloud data C Search for the number of point clouds k within its neighborhood radius R, and calculate the distance from the neighboring points to the nearest point. The average distance; Calculate the mean of the average distances between all neighboring points. and standard deviation Set threshold range If point of Points outside this range are identified as noise points and removed, while the point cloud after initial filtering is retained. .

[0027] S22: Adaptive threshold filtering of dust concentration removes noise points to obtain purified point cloud; Point cloud extraction based on grayscale image assistance Corresponding grayscale value The difference in grayscale value distribution between the target area and the dust area was statistically analyzed to construct an adaptive grayscale threshold. The formula is: ,in, This represents the mean grayscale value corresponding to the point cloud. The standard deviation of the grayscale value. For adaptive adjustment coefficient ; Set acceleration threshold and angular velocity threshold If you dot the clouds Simultaneously satisfy , , ,in , If the acceleration and angular velocity corresponding to the i-th point are given, then it is determined to be a dust noise point and removed, thus obtaining the purified point cloud. .

[0028] In a specific embodiment, step S3 includes: S31: Cleaned point cloud Feature extraction was performed on grayscale images and inertial measurement unit data to obtain purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features.

[0029] Step S31 includes: S311: From Extracting the geometric and statistical features of the target, including: Volume feature V: Calculated based on the target point cloud bounding box. The bounding box is determined by the maximum / minimum coordinates of the target point cloud, as shown in the formula: In the formula, These represent the maximum and minimum values ​​in the X direction of the target point cloud, respectively. These are the maximum and minimum values ​​in the Y direction of the target point cloud, respectively; These are the maximum and minimum values ​​in the Z direction of the target point cloud, respectively. Normal vector features : Perform plane fitting on the target point cloud and solve for the normal vector features using the covariance matrix; covariance matrix The formula is: right Eigenvalue decomposition is performed, and the eigenvector corresponding to the smallest eigenvalue is the normal vector. ; Point cloud density The ratio of the number of target point clouds to their volume. .

[0030] S312: Extract the texture and edge features of the target from the grayscale image, including the mean of the grayscale histogram and the edge gradient magnitude.

[0031] S313: Extract the target's motion characteristics, including acceleration, from inertial measurement unit data. angular velocity The acceleration of a single point in the point cloud extracted in step S22. angular velocity , where i is the index of a point in the point cloud, calculates the overall average acceleration of all points within the target region. With overall average angular velocity As a core parameter of the target's motion characteristics, the calculation formula is: , , where N is the total number of point clouds within the target area.

[0032] S314: Extract occlusion compensation features, use the Poisson reconstruction algorithm to complete the geometric contour of the incomplete target point cloud, and extract the volume features and normal vector features after completion.

[0033] S315: The Retinex algorithm is used to perform illumination equalization on the backlit grayscale image, and texture features and edge gradient magnitudes are extracted again.

[0034] S32: Perform max-min normalization on the purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features extracted in S31, mapping all feature values ​​to the dimensionless interval [0,1] to eliminate the dimensional differences between different features; input the neck feature fusion module of the CMT-3D model, strengthen the weight of key features through the attention mechanism, and output the target recognition results, including: target category, target bounding box coordinates, and recognition confidence.

[0035] In step S32, after the CMT-3D model outputs the preliminary target recognition results: To distinguish mining trucks with similar shapes from other operating equipment, a secondary discrimination mechanism is constructed based on volumetric feature V and motion characteristics: Mining Card Volume And acceleration during uniform motion ; Other operating equipment volume And acceleration (Frequent starts, stops, and turns during operation); Personnel volume Furthermore, the acceleration fluctuates significantly. .

[0036] To address the issue of mining trucks in open-pit mines having similar appearances to other operating equipment, the initial identification results of the CMT-3D model are screened and corrected using volumetric and motion features to eliminate misidentification results and improve the accuracy of target classification.

[0037] In a specific embodiment, step S4 includes: S41: Calculate the absolute position coordinates of the mining card : in, The coordinates of the centroid of the mining card in the lidar coordinate system are obtained by cloud computing of the mining card's regional points. The point cloud registration correction is obtained through iterative nearest-point algorithm optimization. This is the transformation matrix from the robot's body coordinate system to the geodetic coordinate system. The transformation matrix is ​​between the lidar coordinate system and the loading robot's body coordinate system; The objective function of the iterative nearest point algorithm is to minimize the distance error between the mine card point cloud and the standard mine card point cloud model, as shown in the formula: In the formula, Let j be the coordinates of the j-th point in the actual point cloud of the mining truck. Here are the coordinates of the corresponding points in the standard mine card point cloud model, and k is the number of registration points. S42: Calculate the relative position coordinates of the mining card: Position coordinates of the mining truck relative to the shovel robot Based on the robot's absolute position Absolute position of mining card The formula for calculating the difference is: relative distance The formula is: relative azimuth The formula is: In the formula, A positive value indicates the mining truck is on the right side of the robot, and a negative value indicates it is on the left side; S43: Calculate the mining truck's attitude parameters: Mining truck attitude parameters include pitch angle Roll angle Heading angle Normal vectors based on mining card point clouds The formula for calculating the relationship with the geodetic coordinate system is as follows: in, Let be the normal vector of the mining truck body surface; This is the heading correction amount for the mining truck itself. The heading angle of the mining truck in the geodetic coordinate system; The heading angle of the loading robot in the geodetic coordinate system; The relative azimuth angle between the mining truck and the loading robot (calculated in step S42), (∈[−π,π], positive values ​​indicate the mining card is on the right side of the robot, negative values ​​indicate it is on the left side). Mining truck's own heading correction amount.

[0038] In a specific embodiment, step S5 includes: S51: Extract the historical pose data of the mining truck over the past time period T, including its absolute position in the time series. Heading angle Speed ​​of movement acceleration ; S52: Extract and classify the operational intent features of mining trucks, including: Rate of change of relative distance with the loading robot Rate of change of heading angle And the matching results of the mine area roadway nodes; The determination of mining truck operation intent is based on quantitative characteristics, and the determination rule is as follows: like , If the loading lane node is matched, it is determined that there is an intention to load. like and If so, it is determined to be an intention to avoid; The determined task intent features are encoded as follows: loading intent [1,0,0], avoidance intent [0,1,0], and other intents [0,0,1]. S53: Kalman filtering is used to denoise the historical motion data; the adaptive improved Kalman filtering algorithm is used to denoise the historical pose data (absolute position, heading angle, motion speed, acceleration) of the mining truck extracted in S51; the core improvement of the adaptive improved Kalman filtering algorithm is the dynamic adjustment of the covariance matrix + compensation of the observation value of the lidar IMU data: (1) Adaptively adjust the process noise covariance matrix Q: increase Q when the mining truck accelerates / turns, and decrease Q when moving at a constant speed; (2) Adaptively adjust the observation noise covariance matrix R: increase R when the dust concentration is high and the point cloud efficiency is low, and decrease R when the environment is good; (3) fuse the acceleration and angular velocity data of the lidar built-in IMU to compensate the filtered observation value and eliminate the observation value jump caused by dust occlusion. Finally, the filtering state equation and the observation equation are constructed, and the denoised mining truck motion data is output through the prediction-update iteration process. .

[0039] Specifically, it includes: S531: Adaptive adjustment of process noise covariance matrix Q The process noise covariance matrix Q is used to characterize the uncertainty of the mining truck's motion state, and its value is dynamically adjusted according to the real-time motion state of the mining truck: ① When the mining truck is in a variable acceleration / turning state (real-time acceleration) When the motion state changes drastically, the uncertainty of the process increases significantly; ② When the mining truck is traveling at a constant speed (real-time acceleration) When the motion state is stable, the uncertainty of the process is significantly reduced.

[0040] Adjust the formula: in, The pre-defined basic process noise covariance matrix; This is the process noise adjustment factor (tested and calibrated, with a range of values). ); The real-time acceleration of the mining truck at time t is extracted from IMU data; The greater the acceleration, The larger the value, the more the filtering algorithm will trust the "state prediction value" rather than the "sensor observation value" in order to adapt to the rapid changes in the movement state of the mining truck.

[0041] S532: Adaptive adjustment of the observation noise covariance matrix R; The observation noise covariance matrix R is used to characterize the uncertainty of the sensor observation data, and its value is dynamically adjusted according to the effectiveness of the lidar point cloud: ① When dust concentration is high and lidar point cloud efficiency is low At that time, the noise intensity of the observation data increases, and the reliability decreases; ② When the environment is favorable and the lidar point cloud efficiency is high At that time, the noise intensity of the observation data is reduced, and the reliability is improved.

[0042] Adjust the formula: in: The pre-defined basic observation noise covariance matrix; To observe the noise adjustment factor (tested and calibrated, with a range of values) ); Let be the efficiency of the lidar point cloud at time t, calculated using the following formula: ( For the effective number of point clouds, (Total number of point clouds).

[0043] The lower the point cloud efficiency, The larger the value, the less confidence the filtering algorithm will have in the observed data, thus suppressing the noise impact caused by dust interference.

[0044] S533: Compensation for lidar IMU data observations; To eliminate the jumps in observations caused by dust obstruction, motion data from the lidar's built-in IMU is used to compensate for the Kalman-filtered observations. The specific steps are as follows: ①IMU motion prediction: Utilizing real-time acceleration data collected by an IMU and angular velocity Predicting the position and velocity of mining trucks through numerical integration: in, The IMU sampling time interval; ② Weighted fusion of observations: The IMU predictions and lidar observations are weighted and fused to obtain the compensated observation vector. : in: These are the observation vectors directly acquired by the lidar; The observation vectors predicted by the IMU; The fusion coefficient is positively correlated with the effectiveness of the lidar point cloud. That is, the higher the point cloud efficiency, the greater the weight of the lidar observation value.

[0045] S534: Based on the above adaptive mechanism, the filter state equation and observation equation are constructed, and noise reduction is achieved through a "prediction-update" iterative process. ① Equation of state: in This is the predicted state value. To predict the covariance matrix, A is the state transition matrix, and B is the control matrix. To control the quantity.

[0046] ② Observation equation: in, For Kalman gain, Let X(t|t) be the observation matrix, and X(t|t) be the state update values. To update the covariance matrix; By iteratively executing the above prediction and update steps, the final output is the noise-reduced mining truck motion data. This provides a reliable input for subsequent trajectory prediction.

[0047] S54: The noise-reduced motion data Features are concatenated with the work intention feature encoding and used as joint input features for the LSTM time series prediction model; these features are then input into the LSTM network to construct the time series prediction model and output the future mining truck information. absolute position coordinates at time ; During the LSTM model training phase, differentiated loss function weights are set for different operational intent features to enhance the prediction accuracy of core operational intents in the mining area. The weight rules are as follows: (1) Loading intent: weight 1.5 (core loading operation in the mining area, requiring the highest prediction accuracy); (2) Avoidance intent: weight 1.3 (critical for collision prevention of mining equipment, requiring high prediction accuracy); (3) Other intents: weight 1.0 (normal driving, waiting, and other routine states, basic prediction accuracy). By setting different weights for different intents (such as 1.5, 1.3, and 1.0), it is equivalent to "amplifying" or "reducing" the penalty for prediction errors of such intents, thereby guiding the model to prioritize ensuring the prediction accuracy of key operational scenarios.

[0048] S55: Absolute position coordinates The rules are revised to ensure that the predicted trajectory conforms to the mine area's lane planning. The revision formula is as follows: in, These are the predicted coordinates of the mine card after correction according to the mining area operation rules; For the mine area roadway The coordinates of the center position at that moment; This is the rule correction coefficient, with a range of values. The system adaptively adjusts based on the accuracy of the mine area roadway planning. The original absolute position coordinates output by the LSTM model.

[0049] S56: Based on Calculate the future movement direction of the mining truck and speed The formulas are as follows: in, The time interval between adjacent prediction times. for The three-dimensional coordinate components.

[0050] S57: Based on the revision of mining operation rules, a new operation intention revision item is added to perform a second precise revision of the predicted trajectory. The revision formula is as follows: in, The final predicted location coordinates of the mining card after double correction based on the operating rules and operating intentions; The predicted location coordinates of the mine card after only being corrected according to the mining area operation rules. ; To correct the intention, load the intention. , intention to avoid ; The coordinates of standard trajectory nodes in the mining area corresponding to the operational intent are set in advance based on the mining area operation plan; the final output is the future coordinates of the mining card. Predicted trajectory and direction of motion after double correction at time and speed of movement .

[0051] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for sensing and predicting the trajectory of a loading robot in an open-pit mine, characterized in that, include: S1: Collect point cloud data, grayscale images, inertial measurement unit data, and absolute pose data of open-pit mining operation scenarios. All collected data are synchronized via timestamps. S2: Based on the spatial distribution characteristics, corresponding grayscale image features, and data from the built-in inertial measurement unit of the lidar, the collected point cloud data is filtered out by a combined filtering algorithm to remove dust and noise points, thus obtaining purified point cloud data. S3: Based on the purified point cloud data, the grayscale image, and the inertial measurement unit data, extract the point cloud features, image features, and motion features of the target, and identify and classify mining trucks, other operating equipment, and personnel through a multi-feature fusion recognition model; S4: Based on the identified mining truck point cloud data, the purified point cloud data, and the absolute pose of the loading robot, calculate the absolute position coordinates of the mining truck in the geodetic coordinate system, the relative position coordinates of the mining truck with respect to the loading robot, and the attitude angle of the mining truck. S5: Based on the pose estimation results of the mining truck and its historical motion data, a time-series prediction model is used to predict the future motion trajectory of the mining truck.

2. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, In step S1: The point cloud data and grayscale images are acquired by the lidar on the loading robot. The inertial measurement unit data is collected through the inertial measurement unit built into the lidar on the loading robot; The absolute pose data is acquired through the GNSS+IMU navigation module of the shovel robot, including the absolute position coordinates of the shovel robot in the geodetic coordinate system. With attitude angle.

3. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, Step S2 includes: S21: Dynamic radius statistical filtering removes noise points; Calculate the dust concentration coefficient K using the grayscale variance of a grayscale image: ; Calculate the neighborhood radius R: ; For each point in point cloud data C Search for the number of point clouds k within its neighborhood radius R, and calculate the distance from the neighboring points to the nearest point. The average distance; Calculate the mean of the average distances between all neighboring points. and standard deviation Set threshold range If point of Points outside this range are identified as noise points and removed, while the point cloud after initial filtering is retained. ; S22: Adaptive threshold filtering of dust concentration removes noise points to obtain purified point cloud; Point cloud extraction based on grayscale image assistance Corresponding grayscale value The difference in grayscale value distribution between the target area and the dust area was statistically analyzed to construct an adaptive grayscale threshold. The formula is: ,in, This represents the mean grayscale value corresponding to the point cloud. The standard deviation of the grayscale value. For adaptive adjustment coefficient ; Set acceleration threshold and angular velocity threshold If you dot the clouds Simultaneously satisfy , , ,in , If the acceleration and angular velocity corresponding to the i-th point are given, then it is determined to be a dust noise point and removed, thus obtaining the purified point cloud. .

4. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, Step S3 includes: S31: Cleaned point cloud Feature extraction was performed on grayscale images and inertial measurement unit data to obtain purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features; S32: Perform maximum-minimum normalization on the purified point cloud features, grayscale image features, and inertial measurement unit auxiliary features extracted in S31, and map all feature values ​​to the dimensionless interval [0,1] to eliminate the dimensional differences between different features. The neck feature fusion module of the input CMT-3D model strengthens the weights of key features through an attention mechanism and outputs the target recognition results, including: target category, target bounding box coordinates, and recognition confidence.

5. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 4, characterized in that, Step S31 includes: S311: From Extracting the geometric and statistical features of the target, including: Volume feature V: Calculated based on the target point cloud bounding box. The bounding box is determined by the maximum / minimum coordinates of the target point cloud, as shown in the formula: In the formula, These represent the maximum and minimum values ​​in the X direction of the target point cloud, respectively. These represent the maximum and minimum values ​​in the Y direction of the target point cloud, respectively. These are the maximum and minimum values ​​in the Z direction of the target point cloud, respectively. Normal vector features : Perform plane fitting on the target point cloud and solve for the normal vector features using the covariance matrix; covariance matrix The formula is: right Eigenvalue decomposition is performed, and the eigenvector corresponding to the smallest eigenvalue is the normal vector. ; Point cloud density The ratio of the number of target point clouds to their volume. ; S312: Extract the texture and edge features of the target from the grayscale image, including the mean of the grayscale histogram and the edge gradient magnitude; S313: Extract the target's motion characteristics, including acceleration, from inertial measurement unit data. angular velocity The acceleration of a single point in the point cloud extracted in step S22. angular velocity , where i is the index of a point in the point cloud, calculates the overall average acceleration of all points within the target region. With overall average angular velocity As a core parameter of the target's motion characteristics, the calculation formula is: , Where N is the total number of point clouds within the target area; S314: Extract occlusion compensation features, use the Poisson reconstruction algorithm to complete the geometric contour of the incomplete target point cloud, and extract the volume features and normal vector features of the completed target. S315: The Retinex algorithm is used to perform illumination equalization on the backlit grayscale image, and texture features and edge gradient magnitudes are extracted again.

6. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 4, characterized in that, In step S32, after the CMT-3D model outputs the preliminary target recognition results: To distinguish mining trucks with similar shapes from other operating equipment, a secondary discrimination mechanism is constructed based on volumetric feature V and motion characteristics: Mining Card Volume And acceleration during uniform motion ; Other operating equipment volume And acceleration ; Personnel volume Furthermore, the acceleration fluctuates significantly. .

7. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, Step S4 includes: S41: Calculate the absolute position coordinates of the mining card : in, The coordinates of the centroid of the mining card in the lidar coordinate system are obtained by cloud computing of the mining card's regional points. The point cloud registration correction is obtained through iterative nearest-point algorithm optimization. This is the transformation matrix from the robot's body coordinate system to the geodetic coordinate system. The transformation matrix is ​​between the lidar coordinate system and the loading robot's body coordinate system; S42: Calculate the relative position coordinates of the mining card: Position coordinates of the mining truck relative to the shovel robot Based on the robot's absolute position Absolute position of mining card The formula for calculating the difference is: relative distance The formula is: relative azimuth The formula is: In the formula, A positive value indicates the mining truck is on the right side of the robot, and a negative value indicates it is on the left side; S43: Calculate the mining truck's attitude parameters: Mining truck attitude parameters include pitch angle Roll angle Heading angle Normal vectors based on mining card point clouds The formula for calculating the relationship with the geodetic coordinate system is as follows: in, This is the normal vector of the mining truck body surface; This is the heading correction amount for the mining truck itself. The heading angle of the mining truck in the geodetic coordinate system; The heading angle of the loading robot in the geodetic coordinate system; The relative azimuth angle between the mining truck and the loading robot; Mining truck's own heading correction amount.

8. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, In step S41, the objective function of the iterative nearest point algorithm is to minimize the distance error between the mine card point cloud and the standard mine card point cloud model, as shown in the formula: In the formula, Let j be the coordinates of the j-th point in the actual point cloud of the mining truck. is the coordinate of the corresponding point in the standard mine card point cloud model, and k is the number of registration points.

9. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 1, characterized in that, Step S5 includes: S51: Extract the historical pose data of the mining truck over the past time period T, including its absolute position in the time series. Heading angle Speed ​​of movement acceleration ; S52: Extract and classify the operational intent features of mining trucks, including: Rate of change of relative distance with the loading robot Rate of change of heading angle And the matching results of the mine area roadway nodes; The determination of mining truck operation intent is based on quantitative characteristics, and the determination rule is as follows: like , If the loading lane node is matched, it is determined that there is an intention to load. like and If so, it is determined to be an intention to avoid; The determined task intent features are encoded as follows: loading intent [1,0,0], avoidance intent [0,1,0], and other intents [0,0,1]. S53: Kalman filtering is used to reduce noise in historical motion data; S54: The noise-reduced motion data The features are concatenated with the operational intent feature encoding, input into an LSTM model, and a time-series prediction model is constructed to output the future mining truck model. absolute position coordinates at time ; During the LSTM model training phase, differentiated loss function weights are set for different task intent features: the weight for loading intent is 1.5; the weight for avoidance intent is 1.3; and the weight for other intents is 1.

0. S55: Absolute position coordinates The rules are revised to ensure that the predicted trajectory conforms to the mine area's lane planning. The revision formula is as follows: in, These are the predicted coordinates of the mine card after correction according to the mining area operation rules; For the mine area roadway The coordinates of the center position at that moment; This is the rule correction coefficient, with a range of values. The system adaptively adjusts based on the accuracy of the mine area roadway planning. The original absolute position coordinates output by the LSTM model; S56: Based on Calculate the future movement direction of the mining truck and speed The formulas are as follows: in, The time interval between adjacent prediction times. for The three-dimensional coordinate components; S57: Based on the revision of mining operation rules, a new operation intention revision item is added to perform a second precise revision of the predicted trajectory. The revision formula is as follows: in, The final predicted location coordinates of the mining card after double correction based on the operating rules and operating intentions; The predicted location coordinates of the mine card after only being corrected according to the mining area operation rules. ; To correct the intention, load the intention. , intention to avoid ; The coordinates of standard trajectory nodes in the mining area corresponding to the operational intent are set in advance based on the mining area operation plan. Final output mining card future Predicted trajectory and direction of motion after double correction at time and speed of movement .

10. The method for perception and mine truck trajectory prediction based on open-pit mining loading robot according to claim 9, characterized in that, Step S53 includes: S531: Adaptive adjustment process noise covariance matrix Q; in, The preset basic process noise covariance matrix; This is the process noise adjustment factor; The real-time acceleration of the mining truck at time t; S532: Adaptive adjustment of the observation noise covariance matrix R; in: The pre-defined basic observation noise covariance matrix; The noise adjustment factor is used for observation. Let be the efficiency of the lidar point cloud at time t, calculated using the following formula: , For the effective number of point clouds, This represents the total number of point clouds; S533: Compensation for inertial measurement unit data observations of lidar; Real-time acceleration acquired using data from an inertial measurement unit and angular velocity Predicting the position and velocity of mining trucks through numerical integration: in, The sampling time interval for inertial measurement unit data; The predicted values ​​from the inertial measurement unit (IMU) data are weighted and fused with the lidar observation values ​​to obtain the compensated observation vector. : in: These are the observation vectors directly acquired by the lidar; The observation vectors predicted by the IMU; The fusion coefficient is positively correlated with the effectiveness of the lidar point cloud. S534: Based on the above adaptive mechanism, a filter state equation and an observation equation are constructed, and noise reduction is achieved through a "prediction-update" iterative process.

Images