Multi-source navigation data fusion method and system of unmanned loader and storage medium

Abstract

The invention discloses a multi-source navigation data fusion method for an unmanned loader, which comprises the following steps: data acquisition: acquiring environmental perception data of satellite navigation, an inertial measurement unit IMU, a wheel type odometer and a laser radar and camera in real time; performing space-time synchronization preprocessing, realizing multi-source data time synchronization, unifying environment sensing data to a body coordinate system, eliminating abnormal data, and complementing missing data; carrying out dynamic weight calculation, establishing an error model of each sensor, and adjusting a fusion weight by using an error reciprocal exponential weighting method; layering fusion is carried out, a satellite and an IMU are fused through bottom-layer extended Kalman filtering (EKF), a laser radar and a high-precision map are fused through middle-layer ICP, a middle-layer result and a wheel type odometer are integrated through high-layer federated filtering, and high-precision fusion is achieved; and outputting and optimizing a result, outputting navigation data, performing closed-loop optimization, monitoring the health degree of the sensor, and executing redundancy switching when a fault occurs. The system comprises a corresponding processing unit, and a storage medium stores a program for executing the method.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned equipment navigation, and more particularly relates to a multi-source navigation data fusion method and system for an unmanned loader and a storage medium. BACKGROUND

[0002] In the technical field of unmanned equipment navigation, unmanned loaders need to operate stably in complex work scenes such as mines and construction sites. Such scenes generally have problems such as satellite signal blockage (e.g. blockage by factory buildings or mountains), high environmental dust concentration, long-term continuous operation of equipment, and turning slippage caused by uneven road surfaces, which pose strict requirements on the high precision and high reliability of the navigation system.

[0003] In existing navigation solutions for unmanned loaders, single-sensor navigation has obvious limitations: satellite navigation is prone to loss of lock or positioning jumps in signal blockage areas and cannot provide stable absolute positioning; although an inertial measurement unit can output motion data at high frequencies, long-term operation will result in an increase in positioning errors due to drift accumulation; the ranging accuracy of a laser radar will decrease significantly when it is disturbed by dust, and the basic error in a clean environment also cannot meet the high-precision requirements; and a wheeled odometer will produce slippage errors due to an increase in the turning angle and an increase in the instantaneous speed, and cannot independently support precise positioning.

[0004] Traditional multi-source navigation data fusion technology further has the following shortcomings: first, the time and space synchronization process is imperfect, and multi-source data is prone to time misalignment; second, there is a lack of dynamic weight adjustment mechanism, and fixed weight fusion is mostly used, which cannot adaptively adjust the weight proportion according to the real-time performance of the sensors such as the working time of the IMU, the satellite signal strength, and the environmental interference such as dust concentration, and is prone to a decrease in the overall fusion accuracy due to the performance degradation of a certain sensor; third, the fusion architecture design is unreasonable, and a progressive layered fusion system is not constructed, which makes it difficult to effectively suppress the error transmission and accumulation of a single sensor, and lacks real-time monitoring of the health status of the sensors and a fault redundancy switching mechanism, so that once a single-point or multi-point sensor fails, the navigation system is prone to failure and cannot guarantee the continuous and safe operation of the unmanned loader.

[0005] In summary, the existing technology cannot meet the navigation needs of unmanned loaders in complex work scenes, and there is an urgent need for a multi-source navigation data fusion solution that can dynamically adapt to the environment and the performance of the sensors, has layered high-precision fusion and fault redundancy capabilities, to solve the above technical problems. SUMMARY

[0006] In order to solve the above defects or improvement requirements of the prior art, the present application provides a kind of unmanned loader multi-source navigation data fusion method, system and storage medium, by time synchronization unified data time sequence, environmental perception data is converted to machine coordinate system space reference, and with 3σ rule, abnormal data is removed, linear interpolation is completed to missing data, after calculating data weight, data is fused using three-level progressive fusion architecture, solve the problem of "data space dislocation, abnormal data interference, missing data link break" in traditional fusion, provide high-quality raw data support for unmanned loader navigation.

[0007] In order to achieve the above-mentioned purpose, according to the first aspect of the present application, a kind of unmanned loader multi-source navigation data fusion method is provided, comprising the following steps: S1. Data acquisition: real-time acquisition of multi-source navigation data provides raw data support for fusion processing;The multi-source navigation data includes satellite navigation unit data, inertial measurement unit IMU data, wheel odometer data, environmental perception unit data, the environmental perception unit data includes laser radar data and camera data; S2. Time-space synchronization preprocessing: time synchronization is carried out to multi-source data;Environmental perception unit sensor data is converted to machine coordinate system; S3. Dynamic weight calculation: according to the real-time performance of sensor and environmental disturbance, the fusion weight of each data source is dynamically adjusted; S4. Hierarchical fusion processing: three-level progressive fusion architecture is used, including: bottom fusion uses extended Kalman filter to fuse satellite navigation and IMU data, middle fusion is based on iterative closest point algorithm, laser radar point cloud and feature points of high-precision map are matched, high-level fusion uses federal filter architecture to integrate With wheel odometer data; S5. Result output and optimization: output fusion navigation data, and continuously optimize system performance through closed loop feedback.

[0008] Further, the S2 time-space synchronization preprocessing further includes using 3σ rule to remove laser radar abnormal point cloud and satellite navigation jump data, and linear interpolation is completed to missing data.

[0009] Further, the S3 according to the real-time performance of sensor and environmental disturbance, dynamically adjusts the fusion weight of each data source, including: For four types of data sources of satellite navigation, inertial measurement unit, laser radar and wheel odometer, error model associated with real-time performance of sensor and environmental disturbance is established respectively Wherein i=1,2,3,4 correspond to satellite navigation, inertial measurement unit, laser radar and wheel odometer data source in turn: Satellite navigation error model: where S is the normalized value of satellite navigation signal intensity, 0≤S≤1, S=1 represents the strongest signal, and S=0 represents signal loss, is the error coefficient in the occluded environment, is the basic positioning error in the open and unoccluded environment; IMU error model: where t is the continuous working time of the IMU, is the IMU drift coefficient, is the initial working basic error of the IMU; Lidar error model: where D is the dust concentration in the environment, is the influence coefficient of dust on the accuracy of the lidar, is the basic ranging error of the lidar in a clean environment; Wheel odometry error model: where v is the instantaneous speed of the loader, and θ is the steering angle of the loader, is the sliding coefficient of the wheel odometry; Calculate the dynamic fusion weight: adopt the exponential weighting method based on the reciprocal of the error, and the real-time error of each sensor obtained in step 1 is the input, and the fusion weight of each data source is calculated , and the sum of all weights satisfies , and the calculation formula is: ; where is the reference error threshold, which is used to avoid the situation that the error of a single sensor is too small, resulting in excessive suppression of the weight of other sensors; the setting of the exponential function is used to strengthen the weight proportion of the low-error sensor, so that the better the real-time performance of the sensor and the smaller the environmental interference , the larger ; Dynamic update of weight: the weight calculation period is synchronized with the data acquisition period of each sensor, and the latest sensor real-time performance data and environmental interference are recalculated every period and , so as to realize the dynamic adjustment of the fusion weight with the sensor performance and environmental interference.

[0010] Further, the S5 outputs the fused navigation data and continuously optimizes the system performance through closed-loop feedback, including: Navigation results: output to the loader control system through the CAN bus, including position (x, y, z), speed (v), attitude angle (θ, φ, ψ), timestamp, and system health status; Log recording: store the original sensor data, fusion results, and fault information for offline analysis; Performance optimization: based on the deviation of RTK-GPS real trajectory and fusion results, the PPO algorithm of reinforcement learning is used to iteratively optimize the parameters in weight calculation 、 , which are updated every 24 hours to adapt the system to different working scenarios.

[0011] Further, the method further comprises: monitoring the sensor health state in real time, performing redundancy switching when single-point or multi-point failure occurs, and ensuring continuous and reliable operation of the system; Health assessment: the sensor health assessment adopts a weighted scoring method, and the sensor health degree The calculation formula is: ; Among them, is the data integrity rate , is the accuracy compliance rate ; Failure determination and redundancy strategy includes: Single-point failure: when , it is determined that the sensor fails, the failed sensor is automatically excluded, and the weight of the remaining sensors is recalculated; Double-point failure: enable "IMU + laser radar" combination mode, and maintain navigation based on the relative positioning of the pre-stored path; Three or more failures: trigger emergency mode, slow down the loader to stop, and wait for manual intervention.

[0012] Further, the S4 hierarchical fusion processing includes: State quantity definition: the state vector is set as ; Among them is the three-dimensional space position, is the linear velocity component along the three axes, respectively corresponding to the roll angle, pitch angle and yaw angle, used to describe the position, velocity and spatial attitude of the loader; Input: original data from the IMU in the body coordinate system, including three-axis linear acceleration and three-axis angular velocity , which provide high-frequency motion information for extended Kalman filter EKF; Observation: navigation coordinate system data from the satellite navigation unit, including three-dimensional position observation and three-dimensional velocity observation , as an absolute accuracy benchmark; Time Update: Using IMU acceleration and angular velocity, the acceleration in the body coordinate system is converted to the acceleration in the navigation coordinate system through the attitude rotation matrix. After subtracting the influence of gravity, the velocity and position are obtained by integration. The attitude angle is updated by integrating the Euler angle kinematic equations. Then, the nonlinear model is linearized using the Jacobian matrix, combined with the IMU error model. The process noise covariance Q and the prediction error covariance are defined. Measurement Update: Satellite navigation data needs to be preprocessed by S2 spatiotemporal synchronization; the observation function is set as a linear mapping, associating the position and velocity dimensions in the state variables; the observation residuals are calculated, and the state and error covariance are corrected by Kalman gain. Output: After completing the optimal estimation of the extended Kalman filter (EKF) closed loop, the output is the smoothed pose data. That is, the corrected position, velocity, and attitude information.

[0013] Furthermore, the mid-layer fusion, based on the iterative nearest-point algorithm, matches feature points from the LiDAR point cloud with those from the high-precision map, including the following steps: Points that are stable and easily identifiable within the system's built-in high-precision map are selected as visual anchors. The pixel coordinates of these anchors are extracted from the image. Utilizing the "pixel coordinates - body coordinate system" mapping relationship within the camera, the pixel coordinates of the visual anchors are converted into 3D coordinates in the high-precision map coordinate system. The positional deviation between the visual anchors and their corresponding anchors on the map is then calculated. And the yaw angle deviation between the visual anchor point line and the map anchor point line. The initial rotation matrix is ​​obtained. Initial translation vector After iteration ; Obtain the optimal transformation matrix Then, the relative pose deviation ΔP is calculated and... Correction ΔP decomposition and calculation will optimize the best transformation matrix. Decomposed into relative translational deviation ΔT and relative rotational deviation ΔR, forming ΔP: The component is This reflects the translational deviation between the lidar point cloud and the map; Converted to Euler angles via "rotation matrix to Euler angles" This reflects attitude deviation; ultimately Verify the reliability of ΔP using camera visual anchor points: like , , If ΔP is valid, then ΔP is valid; otherwise, the ICP iteration is re-executed. After verification, press Correction get : The position is ; Attitude angle is .

[0014] Furthermore, the S4 layered fusion process, specifically the high-level fusion, includes: Pose data output from mid-layer fusion Data from wheeled odometers; the architecture logic is: middle layer Pose data is transmitted to the first sub-filter for processing, and wheel odometry data is transmitted to the second sub-filter for processing. The two local estimation results are input to the main filter. After information weighted fusion and data consistency verification, the final fused navigation result is output. ; Sub-filter #1: Processing the middle layer posture and corresponding covariance Linear Kalman filtering is used; through adjacent... Differential calculation speed predicts the current initial pose, process noise covariance Set as compatible A high-precision diagonal matrix is ​​used to ultimately output a local estimate. With covariance ; Sub-filter #2: Processes real-time data from the wheel odometer and is used according to the error model. Calculation error Extended Kalman filtering is used; state variables are defined. , Reuse of , combined Compensation for slip error constructs state prediction formula, process noise covariance and Positive correlation, ultimately outputting a local estimate With covariance ; Information allocation coefficient calculation: Let the coefficient of sub-filter number one be... The coefficients of the second sub-filter are ,satisfy ; Weighting of wheeled odometers based on their dynamic weights calculate: , ; Calculation and feedback of fusion results: Calculation of the total information matrix (Information matrix = inverse of covariance matrix, reflecting data confidence); Fusion result: ; Fusion covariance Feedback Update: Will according to , Feedback is sent to filter number one and the initial covariance of the next cycle is updated. To avoid accumulating errors.

[0015] As another aspect of the present invention, the present invention proposes a multi-source navigation data fusion system for an unmanned loader, comprising: Data acquisition unit: Real-time acquisition of multi-source navigation data to provide raw data support for fusion processing; the multi-source navigation data includes: satellite navigation unit data, inertial measurement unit (IMU) data, wheeled odometer data, and environmental perception unit data, the environmental perception unit data including lidar data and camera data; Spatiotemporal synchronization preprocessing unit: synchronizes multi-source data in time; converts sensor data from the environmental perception unit to the body coordinate system; Dynamic weight calculation unit: Dynamic weight calculation: Dynamically adjusts the fusion weights of each data source based on the real-time performance of the sensor and environmental interference; Layered fusion processing unit: Employs a three-level progressive fusion architecture, including: bottom-level fusion using extended Kalman filtering to fuse satellite navigation and IMU data; middle-level fusion based on the iterative nearest point algorithm, matching feature points from LiDAR point clouds with those from high-precision maps; and top-level fusion using a federated filtering architecture for integration. Data from wheeled odometers; Fault diagnosis and redundancy unit: Real-time monitoring of sensor health status, and redundancy switching in case of single or multiple point failures; Output and Optimization Unit: Outputs fused navigation data and continuously optimizes system performance through closed-loop feedback.

[0016] As another aspect of the present invention, the present invention proposes a computer-readable storage medium having a computer program stored thereon, the computer program being executed by a processor of the multi-source navigation data fusion method for unmanned loaders described in any of the preceding claims.

[0017] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects: 1. The present invention provides a multi-source navigation data fusion method for unmanned loaders. This method unifies the data timing through time synchronization, converts environmental perception data to a spatial reference in the machine coordinate system, and uses the 3σ criterion to remove abnormal data and linear interpolation to complete missing data (maximum completion time ≤ 100ms). This solves the problems of "data spatiotemporal misalignment, abnormal data interference, and missing data chain breakage" in traditional fusion, and provides high-quality raw data support for subsequent high-precision fusion.

[0018] 2. The multi-source navigation data fusion method for unmanned loaders of the present invention establishes error models for satellite navigation associated signal strength, IMU associated working time, lidar associated dust concentration, and wheel odometer associated speed and steering angle, respectively. The fusion weight is calculated by the inverse exponential weighting method of error, and the weight is dynamically updated synchronously with the sensor acquisition cycle. This avoids the defect of traditional fixed weights that "cannot adapt to the real-time performance fluctuations of sensors and environmental interference", allowing sensors with better performance and less interference to occupy higher weights, thereby improving the accuracy of the fusion results.

[0019] 3. The multi-source navigation data fusion method for unmanned loaders of the present invention uses an extended Kalman filter (EKF) to fuse satellite navigation and IMU data at the bottom layer, suppressing IMU drift and satellite navigation jumps; the ICP algorithm is used in the middle layer to match lidar point clouds with high-precision maps to correct the accumulated errors at the bottom layer; and federated filtering is used in the upper layer to integrate the results of the middle layer with wheel odometer data, dynamically adjusting the information allocation coefficients, thus solving the problems of "error propagation amplification and inability to fully utilize the advantages of multiple sources" in the traditional single fusion architecture.

[0020] 4. The multi-source navigation data fusion method for unmanned loaders of the present invention calculates the health of sensors, removes faulty sensors and reassigns weights when there is a single-point failure, enables relative positioning of "IMU + LiDAR" when there is a two-point failure, and triggers emergency shutdown when there is a three-point or more failure. This solves the problem of "lack of fault monitoring and direct failure after failure" in traditional systems, and ensures that the loader can still operate stably or safely transition in complex scenarios (such as temporary sensor failure), thereby improving the continuity of operation. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating a multi-source navigation data fusion method for an unmanned loader according to an embodiment of the present invention; Figure 2 This is a flowchart of the S4 layered fusion process in Embodiment 1 of the present invention; Figure 3 This is a diagram of the core architecture of high-level federated filtering in the S4 layered fusion processing of Embodiment 1 of the present invention; Figure 4 This is a schematic diagram of a multi-source navigation data fusion system for an unmanned loader according to Embodiment 3 of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other. Example

[0023] Please refer to Figure 1 S1. Data Acquisition: Real-time acquisition of multi-source navigation data to provide raw data support for fusion processing; multi-source navigation data includes: satellite navigation unit data, inertial measurement unit (IMU) data, wheeled odometer data, and environmental perception unit data, of which environmental perception unit data includes lidar data and camera data; S2. Spatiotemporal synchronization preprocessing: Synchronize multi-source data in time; uniformly convert sensor data from the environmental perception unit to the body coordinate system; S21 uses the 3σ criterion to remove abnormal point clouds from lidar and abrupt changes in satellite navigation data, and performs linear interpolation to complete missing data, with a maximum completion time of ≤100ms.

[0024] S3. Dynamic Weight Calculation: The fusion weights of each data source are dynamically adjusted based on the real-time performance of the sensors and environmental interference. S31 establishes error models that are strongly correlated with the real-time performance of sensors and environmental interference for four core data sources: satellite navigation, inertial measurement unit (IMU), lidar, and wheeled odometer. (i=1,2,3,4 correspond to the four types of data sources mentioned above in sequence): Satellite navigation error model: S is the normalized value of satellite navigation signal strength (0≤S≤1, S=1 indicates the strongest signal, S=0 indicates signal loss). The error coefficient under shading conditions. The basic positioning error in an open, unobstructed environment; IMU error model: , where t is the continuous operating time of the IMU (in hours). This is the IMU drift coefficient. This represents the initial operating baseline error of the IMU. LiDAR error model: Where D is the ambient dust concentration (unit: mg / m³). The coefficient representing the impact of dust on the accuracy of lidar. This represents the basic ranging error of lidar in a clean environment. Wheel Odometer Error Model: Where v is the instantaneous speed of the loader (unit: m / s) and θ is the steering angle of the loader (unit: °). The slip coefficient of the wheel-type odometer; Calculate the dynamic fusion weights: Employ an exponential weighting method based on the inverse of the error, using the real-time errors of each sensor obtained in step 1. Using this as input, calculate the fusion weights for each data source. And satisfy the total weight of all The calculation formula is: ; in The baseline error threshold is used to prevent excessive suppression of the weight of other sensors due to excessively small errors in a single sensor; the exponential function is used to strengthen the weight ratio of low-error sensors, so that the real-time performance of the sensors is better and less affected by environmental interference. The smaller), the corresponding The larger; Weights are dynamically updated: The weight calculation cycle is synchronized with the data acquisition cycle of each sensor. Each cycle, the weights are recalculated based on the latest real-time sensor performance data and environmental interference parameters (such as signal strength S, dust concentration D, velocity v, etc.). and This enables dynamic adjustment of fusion weights based on sensor performance and environmental interference.

[0025] Please refer to Figure 2 S4. Layered fusion processing: Achieving high-precision fusion of multi-source data through a three-level progressive fusion architecture; S41. The underlying fusion uses an extended Kalman filter (EKF) to fuse satellite navigation and IMU data, suppressing IMU drift and GPS jumps; the state variables are defined as: ; in These are the position coordinates of the object in a three-dimensional coordinate system. For the object along The linear velocity component of the shaft; The attitude angles of an object (usually corresponding to roll, pitch, and yaw in sequence) are used to describe the spatial attitude and orientation of the object.

[0026] The observation update uses GPS position and velocity as observation values, corrects the IMU integration results, and outputs smoothed pose data. ; Input (control input): Raw data from the body coordinate system of the IMU, providing high-frequency motion information for the Extended Kalman Filter (EKF), including: triaxial acceleration. Triaxial angular velocity .

[0027] Observations (measurements): Navigation coordinate system data from satellite navigation units, providing an absolute accuracy benchmark for the Extended Kalman Filter (EKF), including: three-dimensional position observations. Three-dimensional velocity observation .

[0028] The Extended Kalman Filter (EKF) achieves optimal state estimation through a closed loop of "time update (IMU-driven prediction)" and "measurement update (satellite navigation-driven correction)".

[0029] Time update (IMU-based state prediction) Using the acceleration and angular velocity of the IMU, the state at the next moment can be predicted through kinematic differential equations and numerical integration (such as Euler integral). The core task is to complete the conversion from "body coordinate system data to navigation coordinate system data": Position and velocity update: First, use the current attitude angle Calculate the attitude rotation matrix Convert the IMU's body coordinate system acceleration to the navigation coordinate system acceleration. After deducting the effect of gravity ( (where g is the acceleration due to gravity), integrating yields the velocity and position: ; For location, To improve speed, the integration step size is synchronized with the IMU sampling period to ensure high-frequency prediction.

[0030] Attitude angle update: Based on the angular velocity in the body coordinate system of the IMU, the attitude angle is updated by integrating the Euler angle kinematic equations to match the physical relationship between the IMU and the attitude. ; Because the aforementioned kinematic models (such as attitude rotation and Euler angle differentiation) are nonlinear, the Extended Kalman Filter (EKF) uses the Jacobian matrix... Linearize the nonlinear model, and combine the process noise covariance Q of the IMU with the prediction error covariance.

[0031] ; The state transition Jacobian matrix is ​​the matrix for the state prediction function f in its historical optimal state. By taking partial derivatives, the nonlinear error propagation is approximated as linear, ensuring the rationality of the error calculation; Q is the IMU process noise covariance—based on the IMU error model. set up: (IMU initial baseline error) (IMU drift coefficient), the diagonal elements of Q are appropriately amplified with the continuous operating time t of the IMU to reflect the increase in noise caused by drift.

[0032] Measurement updates (satellite navigation-based state corrections) Satellite navigation data must first undergo S2 "spatiotemporal synchronization preprocessing" (time synchronization, outlier removal, and missing data completion) to ensure spatiotemporal consistency with IMU data before being used for extended Kalman filter (EKF) correction. The specific process is as follows: Observation function This is a linear mapping, directly relating the position and velocity dimensions in the state vector: ; Satellite navigation observation noise – satellite navigation error model set up: (Obstruction environment error coefficient) (Basic error in open environment), S is the normalized value of satellite signal strength.

[0033] The variance increases as S decreases (e.g.) Time to take , Time to take Calculate the observation residuals (quantize the deviation between IMU predictions and satellite navigation observations): ; Through Kalman gain Balancing "IMU prediction error" with "satellite navigation observation error," correcting for state and covariance: Kalman gain: ; To observe the Jacobian matrix—because the observation function is linear, It is a 6×9 sparse matrix (only the 6 state dimensions corresponding to position and velocity are 1, and the rest are 0); R: Satellite navigation observation noise covariance — and The variance is consistent and dynamically adjusted according to the satellite signal strength S to ensure the accuracy of noise characterization.

[0034] The optimal pose is obtained by updating the state. : (output) That is, "smoothed pose data" (”).

[0035] Covariance update (reflecting the confidence level of the current state): (I is a 9×9 identity matrix).

[0036] S42. The mid-layer fusion is based on the iterative nearest-point algorithm, which matches the feature points of the LiDAR point cloud with those of the high-precision map, calculates the relative pose deviation ΔP, and corrects the bottom-layer fusion result. ,get The optimization goal of the mid-layer fusion algorithm is to improve the accuracy and speed of matching LiDAR point clouds with maps, thereby correcting the accumulated errors in the bottom-layer fusion results and further improving positioning accuracy. Feature type filtering: Prioritize fixed structural features in the map within the loader's operating range (such as the edge of the equipment base in the work area, ground metal positioning marks, and wall corners), and exclude points in temporary material areas and mobile equipment areas as "high-recognition visual anchor points"; Optimization of the initial transformation matrix based on camera visual anchor points (to address the issue of large initial deviations) Visual anchor point recognition and positioning: Real-time images from cameras after spatiotemporal synchronization are called up, and template matching algorithms are used to identify "high-recognition visual anchor points" marked in high-precision maps, such as red positioning marks on the ground and corner points of blue bases of equipment, and the pixel coordinates of the anchor points in the images are extracted. Anchor point 3D coordinate inversion: Utilizing the "pixel coordinates - body coordinate system" mapping relationship within the camera, the pixel coordinates of the visual anchor point are inverted into 3D coordinates in the high-precision map coordinate system (denoted as...). , ; Initial transformation matrix correction: Calculate the positional deviation between the visual anchor point and the corresponding anchor point on the map (z-axis deviation is ignored due to camera depth of field limitations):

[0037] in This refers to the two-dimensional positional deviation between the visual anchor point and the map anchor point. Real-time visual anchor The x-coordinate in the high-precision map coordinate system, For real-time visual anchor points The y-coordinate in the high-precision map coordinate system, For the corresponding anchor points pre-stored in the high-precision map x-coordinate, For the corresponding anchor points pre-stored in the high-precision map The y-coordinate; ΔA is within a reasonable range and At this point, the visual anchor point recognition is accurate, and the position of the underlying fusion output is... The deviation from the actual location on the map is extremely small (ΔA is a small note indicating that the "visual anchor point inverse calculation position" is close to the "map anchor point," and the reliability of the underlying fused position has been verified through visual anchor points). Therefore... It will be directly used as the initial translation vector for the ICP iteration. In this case, The location reference is reliable enough to provide a "low initial deviation" starting point for ICP iteration, ensuring that subsequent iterations converge quickly to high-precision results.

[0038] ΔA is within the critical range ( At this point, the visual anchor point has a small error, but it is still useful for reference. It needs to be combined with the real-time error of the LiDAR. Comprehensive judgment: If (High-quality LiDAR point cloud): Still use it directly, but more rigorous monitoring of the RMSE (root mean square error) of the ICP iteration is needed to ensure that subsequent point cloud matching can correct the small deviation caused by ΔA; if (LiDAR is highly susceptible to interference from dust, etc.): The search radius of the kd-tree needs to be adjusted, and at the same time... By using it directly, the number of corresponding point pairs is guaranteed by expanding the search range, which indirectly compensates for the influence of small errors in ΔA on the initial position.

[0039] ΔA exceeds the reasonable range At this point, there are significant errors in visual anchor point recognition (such as camera occlusion or template matching errors), or issues with the underlying fusion location. Significant deviations have accumulated. In this situation, The "direct use" will be constrained by the process: triggering the "visual anchor point re-identification" process: re-extracting visual anchor points from the image and recalculating 3D coordinates until ΔA returns to a reasonable range; if ΔA still exceeds the range after multiple re-identifications, it indicates that the underlying fusion position deviation is too large and indirect correction is required. By correcting the initial rotation matrix During the ICP iteration process, "indirect correction" is achieved through point cloud matching. The corresponding positional deviation is used to avoid iterative divergence caused by incorrect initial positions.

[0040] Attitude deviation: in, The yaw angle of the line connecting the anchor points on the map. For real-time visual anchor points The y-coordinate in the high-precision map coordinate system, Real-time visual anchor The x-coordinate in the high-precision map coordinate system, The yaw angle deviation between the line connecting the visual anchor points and the line connecting the map anchor points; For the bottom layer Correct the attitude angle: ( (Controlling the correction within ±2° to avoid over-correction) yields the corrected initial rotation matrix. Initial translation vector ; Dynamic neighborhood constraint kd-tree + visual semantic filtering (to solve the problem of mismatched corresponding points) Dynamic neighborhood radius calculation: based on real-time lidar error (by model) Calculate, where D is the environmental dust concentration (which can be obtained through the built-in sensor of the lidar), and dynamically adjust the kd-tree search neighborhood radius r: (like When r = 0.36m, When r=0.6m, ensure that the corresponding point is searched only within a reasonable error range to reduce noise interference; Visual semantic filtering: Calls the semantic segmentation results from the camera; if the LiDAR points... Semantic labels (such as "dynamic obstacles") and map feature points If the semantic labels (such as "static device") are inconsistent, or the Euclidean distance between them is greater than r, then the corresponding point pair is removed. Valid point pair verification: The number of valid corresponding point pairs retained in the end must be greater than 70% of the initial point pairs. If it is insufficient, the "visual anchor point initial registration" will be re-executed to ensure the reliability of subsequent transformation matrix calculation. Adaptive Iteration Termination and Visual Error Compensation (to address the problem of low iteration efficiency). Traditional ICP uses a fixed number of iterations or a single error threshold, which easily leads to "over-iteration (time-consuming)" or "under-iteration (low accuracy)". Iteration Error Monitoring: Calculate the "Root Mean Square Error of Point-to-Plane Distance" (RMSE) after each iteration. ; in For lidar points, is the map feature point, and n is the number of valid point pairs; if the RMSE decrease is <0.001m for 3 consecutive iterations (i.e. the accuracy improvement is approaching stagnation), then "visual error compensation" is triggered. Visual error compensation: using two visual anchor points in the real-time image from the camera (e.g., ... , ), calculate the visual observation bias of the loader relative to the map: ; The calculation method is the same as described above. , , ; (Only in the plane; the z-axis and pitch / roll angles are ignored due to camera limitations). Adaptive termination judgment: If RMSE ≤ 0.008m, terminate the iteration directly; If RMSE > 0.008m but triggers "Precision Stagnation", then use Compensate for the pose deviation of the current ICP calculation and force the iteration to terminate (compensation amount ≤ 0.02m, to avoid visual error dominating). Finally, output the current optimal transformation matrix. ; Calculation of relative pose deviation ΔP and Correction: ΔP decomposition and calculation will optimize the best transformation matrix. Decomposed into relative translational deviation ΔT and relative rotational deviation ΔR, forming ΔP: The components are (Δx, Δy, Δz), which reflect the translational deviation between the lidar point cloud and the map; The angles are converted to Euler angles using a rotation matrix to reflect the attitude deviation. final .

[0041] ΔP rationality verification (based on camera data) utilizes the "environmental perception unit data collaboration" logic to verify the reliability of ΔP using camera visual anchor points: like , , If ΔP is valid, then ΔP is valid; otherwise, the ICP iteration is re-executed. After verification, press Correction get : The position is ; Attitude angle is ; refer to Figure 3 S43. High-level convergence adopts a federated filtering architecture for integration. Combined with wheel odometer data, information allocation coefficients (and weights) are used. (Association) Dynamically adjust the fusion weights, and finally output the fused navigation result: ; The optimization goal of high-level fusion algorithms is to comprehensively utilize information from various sensors to further improve positioning accuracy and system robustness, while ensuring the real-time requirements of the system.

[0042] The federated filtering architecture specifically includes: A lightweight architecture of "two local sub-filters + one master filter" is adopted, which integrates the middle layer... Pose data is transmitted to the first sub-filter for processing, and wheel odometry data is transmitted to the second sub-filter for processing. The two local estimation results are input to the main filter. After information weighted fusion and data consistency verification, the final fused navigation result is output. .

[0043] Specific design of local sub-filters Sub-filter No. 1 (processing middle layer) pose) Input data: Pose output of mid-layer fusion , For three-dimensional position, These are roll angle, pitch angle, and yaw angle, respectively. : The corresponding covariance matrix (reflects the matching confidence of the Iterative Closest Point (ICP) algorithm).

[0044] Filtering algorithm: Linear Kalman filter (KF), because The accumulated error at the lower level has been corrected through mid-layer fusion, and the data linearity meets the requirements for linear filtering. Core implementation steps: Time update: based on Temporal continuity, through two adjacent frames Differential calculation speed (T is the fusion period, synchronized with the dynamic weight calculation period, set to 10ms), predict the initial pose of the current period: Process noise covariance Set as ; adaptation Due to its high precision characteristics, the predicted covariance is: ; Measurement update: based on the current period Based on the observed values, the predicted values ​​are corrected, and the local estimate of the first sub-filter is output. With covariance .

[0045] Sub-filter #2 (processes wheel odometer data) Input data: Wheel odometer collects real-time data: instantaneous speed v (unit: m / s), steering angle (Unit: °); Wheel odometer error (According to the preset model) calculate, (This refers to the sliding coefficient).

[0046] Filtering algorithm: Extended Kalman filter, adapted Nonlinear correlation characteristics with speed and steering angle.

[0047] State equation construction: defining state variables in

[0048] Reuse of (Value, wheeled odometers cannot collect z-axis data), combined with The formula for compensating for slip error and predicting the state is: ; in ; is the displacement increment calculated for a wheeled odometer.

[0049] Process noise settings: Process noise covariance and Positive correlation, let it be: This ensures that the noise weight is increased synchronously as the error increases; Output: After completing the nonlinear estimation, output the local estimate of the second sub-filter.

[0050] With covariance .

[0051] Calculation of information allocation coefficient Let the information allocation coefficient of the first sub-filter be... The information allocation coefficient of the second sub-filter is ,satisfy The coefficient calculation is strongly correlated with sensor performance: Basic coefficient calculation: (Information weights of wheeled odometers) Based on dynamic weights of wheeled odometers (Calculated using the preset index weighting method) and Negative correlation) determined: ; Where 0.1 is the base weight, ensuring A small amount of information is still retained for fault monitoring; Middle layer Information weight).

[0052] Fault adaptation adjustment: based on sensor health model

[0053] For data integrity rate, To calculate the health status of wheeled odometers (for accuracy compliance rate) ,like Determine if the wheel odometer is faulty and force reset. , Only retain Data participates in integration.

[0054] Data consistency verification use Verify the consistency of the estimates from the two sub-filters to avoid interference from outlier data: ; like (significance level) (6 degrees of freedom, adaptable to 6D pose data), determines data inconsistency, and lowers the threshold for abnormal data sources. Value (such as...) After being reduced to 0.1), it was re-fused.

[0055] The final result is calculated based on information-weighted fusion logic, avoiding the ill-conditioned problem of the covariance matrix. Calculate the total information matrix (information matrix = inverse of covariance matrix, reflecting data confidence level): ; Calculation of fusion results: ; Fusion covariance calculation (reflecting the confidence level of the results, for use in subsequent output):

[0056] Will according to Feedback is sent to sub-filters 1 and 2 respectively to update the initial covariance of the filter in the next cycle: , Ensure that the initial state of the sub-filter is consistent with the global fusion result to avoid accumulated errors.

[0057] S5. Output and Optimization: Output fused navigation data and continuously optimize system performance through closed-loop feedback.

[0058] Navigation results: Outputted to the loader control system via CAN bus, including position (x, y, z), speed (v), attitude angles (θ, φ, ψ), timestamp, and system health status (normal / fault); Log recording: Stores raw sensor data, fusion results, and fault information (storage time ≥ 72 hours) for offline analysis; Performance optimization: Based on the deviation between the actual trajectory and the fusion result of RTK-GPS, reinforcement learning is used to iteratively optimize the weight calculation process. , These parameters are updated every 24 hours to allow the system to adapt to different work scenarios. Example

[0059] A multi-source navigation data fusion method for unmanned loaders also includes: real-time monitoring of sensor health status, performing redundancy switching in case of single-point or multi-point failures to ensure continuous and reliable system operation; including the following steps: Health Assessment: Sensor health assessment uses a weighted scoring method. The formula for calculating sensor health Hᵢ is: ; in, For data integrity , To achieve the accuracy target rate ; Fault diagnosis and redundancy strategies include: Single point of failure: when If the sensor is found to be faulty, the faulty sensor is automatically removed, and the weights of the remaining sensors are recalculated. Two-point failure: Activate the "IMU + LiDAR" combined mode to maintain navigation based on the relative positioning of the pre-stored path; Three or more faults: Emergency mode is triggered, the loader slows down to a stop, and manual intervention is required. Example

[0060] Please refer to Figure 4 A multi-source navigation data fusion system for an unmanned loader, characterized in that it includes: Data acquisition unit: Real-time acquisition of multi-source navigation data to provide raw data support for fusion processing; the multi-source navigation data includes: satellite navigation unit data, inertial measurement unit (IMU) data, wheeled odometer data, and environmental perception unit data, the environmental perception unit data including lidar data and camera data; Spatiotemporal synchronization preprocessing unit: synchronizes multi-source data in time; converts sensor data from the environmental perception unit to the body coordinate system; Dynamic weight calculation unit: dynamically adjusts the fusion weights of each data source based on the real-time performance of the sensor and environmental interference; Layered fusion processing unit: Employs a three-level progressive fusion architecture, including: bottom-level fusion using extended Kalman filtering to fuse satellite navigation and IMU data; middle-level fusion based on the iterative nearest point algorithm, matching feature points from LiDAR point clouds with those from high-precision maps; and top-level fusion using a federated filtering architecture for integration. Data from wheeled odometers; Fault diagnosis and redundancy unit: Real-time monitoring of sensor health status, and redundancy switching in case of single-point / multi-point failure; Output and Optimization Unit: Outputs fused navigation data and continuously optimizes system performance through closed-loop feedback. Example

[0061] This fourth embodiment also provides a computer-readable storage medium storing a computer program. When executed by a processor, the computer program can implement any step of a multi-source navigation data fusion method for an unmanned loader. The computer-readable storage medium may include various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0062] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for fusing multi-source navigation data of an unmanned loader, characterized in that, Includes the following steps: S1. Data Acquisition: Real-time acquisition of multi-source navigation data to provide raw data support for fusion processing; The multi-source navigation data includes: satellite navigation unit data, inertial measurement unit (IMU) data, wheeled odometer data, and environmental perception unit data, wherein the environmental perception unit data includes lidar data and camera data. S2. Spatiotemporal synchronization preprocessing: Synchronize multi-source data in time; uniformly convert sensor data from the environmental perception unit to the body coordinate system; S3. Dynamic weight calculation: Dynamically adjust the fusion weights of each data source based on the real-time performance of the sensor and environmental interference; S4. Layered Fusion Processing: A three-level progressive fusion architecture is used, including: bottom-level fusion using extended Kalman filtering to fuse satellite navigation and IMU data; middle-level fusion based on the iterative nearest point algorithm, matching feature points from LiDAR point clouds with those from high-precision maps; and top-level fusion using a federated filtering architecture for integration. Data from wheeled odometers; S5. Output and Optimization: Output fused navigation data and continuously optimize system performance through closed-loop feedback.

2. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The S2 spatiotemporal synchronization preprocessing also includes using the 3σ criterion to remove abnormal point clouds from lidar and abrupt changes in satellite navigation data, and performing linear interpolation to complete missing data.

3. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The S3 dynamically adjusts the fusion weights of each data source based on the real-time performance of the sensors and environmental interference, including: For four types of data sources—satellite navigation, inertial measurement unit, lidar, and wheeled odometer—error models strongly correlated with sensor real-time performance and environmental interference are established. Where i=1,2,3,4 correspond to the data sources of satellite navigation, inertial measurement unit, lidar, and wheel odometer, respectively. Satellite navigation error model: Where S is the normalized value of satellite navigation signal strength, 0≤S≤1, S=1 indicates the strongest signal, and S=0 indicates signal loss. The error coefficient under shading conditions. The basic positioning error in an open, unobstructed environment; IMU error model: Where t is the continuous operating time of the IMU. This is the IMU drift coefficient. This represents the initial operating baseline error of the IMU. LiDAR error model: Where D is the concentration of ambient dust. The coefficient representing the impact of dust on the accuracy of lidar. This represents the basic ranging error of lidar in a clean environment. Wheel Odometer Error Model: Where v is the instantaneous speed of the loader, and θ is the turning angle of the loader. The slip coefficient of the wheel-type odometer; Calculate the dynamic fusion weights: Employ an exponential weighting method based on the inverse of the error, using the real-time errors of each sensor obtained in step 1. Using this as input, calculate the fusion weights for each data source. And satisfy the total weight of all The calculation formula is: ； in The baseline error threshold is used to prevent excessive suppression of the weight of other sensors due to excessively small errors in a single sensor; the exponential function is used to strengthen the weight ratio of low-error sensors, so that the real-time performance of the sensors is better and the susceptibility to environmental interference is smaller. The smaller, the corresponding The larger; Weights are dynamically updated: the weight calculation cycle is synchronized with the data acquisition cycle of each sensor, and is recalculated each cycle based on the latest real-time sensor performance data and environmental interference. and This enables dynamic adjustment of fusion weights based on sensor performance and environmental interference.

4. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The S5 outputs fused navigation data and continuously optimizes system performance through closed-loop feedback, including: Navigation results: Output to the loader control system via CAN bus, including position (x, y, z), speed (v), attitude angles (θ, φ, ψ), timestamps, and system health status; Log recording: Stores raw sensor data, fusion results, and fault information for offline analysis; Performance optimization: Based on the deviation between the actual trajectory and the fusion result of RTK-GPS, the reinforcement learning PPO algorithm is used to iteratively optimize the weight calculation process. , These parameters are updated every 24 hours to allow the system to adapt to different work scenarios.

5. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The method also includes: real-time monitoring of sensor health status, performing redundancy switching in the event of single-point or multi-point failures, and ensuring continuous and reliable operation of the system; Health Assessment: Sensor health assessment uses a weighted scoring method. The calculation formula is: ； in, For data integrity , To achieve the accuracy target rate ; Fault diagnosis and redundancy strategies include: Single point of failure: when If the sensor is found to be faulty, the faulty sensor is automatically removed, and the weights of the remaining sensors are recalculated. Two-point failure: Activate the "IMU + LiDAR" combined mode to maintain navigation based on the relative positioning of the pre-stored path; Three or more faults: Emergency mode is triggered, the loader slows down to a stop, and manual intervention is required.

6. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The S4 layered fusion process, the bottom layer fusion includes: State variable definition: The state vector is set as ; in For three-dimensional spatial position, These are the linear velocity components along the three axes. These correspond to roll angle, pitch angle, and yaw angle, respectively, and are used to describe the loader's position, speed, and spatial attitude. Input: Raw body coordinate data from the IMU, including accelerations along three axes. With triaxial angular velocity This provides high-frequency motion information for the extended Kalman filter (EKF). Observations: Navigation coordinate system data from satellite navigation units, including three-dimensional position observations. With three-dimensional velocity observation , as an absolute accuracy benchmark; Time Update: Using IMU acceleration and angular velocity, the acceleration in the body coordinate system is converted to the acceleration in the navigation coordinate system through the attitude rotation matrix. After subtracting the influence of gravity, the velocity and position are obtained by integration. The attitude angle is updated by integrating the Euler angle kinematic equations. Then, the nonlinear model is linearized using the Jacobian matrix, combined with the IMU error model. The process noise covariance Q and the prediction error covariance are defined. Measurement Update: Satellite navigation data needs to be preprocessed by S2 spatiotemporal synchronization; the observation function is set as a linear mapping, associating the position and velocity dimensions in the state variables; the observation residuals are calculated, and the state and error covariance are corrected by Kalman gain. Output: After completing the optimal estimation of the extended Kalman filter (EKF) closed loop, the output is the smoothed pose data. That is, the corrected position, velocity, and attitude information.

7. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The mid-layer fusion, based on the iterative nearest point algorithm, matches feature points from the LiDAR point cloud with those from the high-precision map, and includes the following steps: Points that are stable and easily identifiable within the system's built-in high-precision map are selected as visual anchors. The pixel coordinates of these anchors are extracted from the image. Utilizing the "pixel coordinates - body coordinate system" mapping relationship within the camera, the pixel coordinates of the visual anchors are converted into 3D coordinates in the high-precision map coordinate system. The positional deviation between the visual anchors and their corresponding anchors on the map is then calculated. And the yaw angle deviation between the visual anchor point line and the map anchor point line. The initial rotation matrix is ​​obtained. Initial translation vector After iteration ； Obtain the optimal transformation matrix Then, the relative pose deviation ΔP is calculated and... Correction ΔP decomposition and calculation will optimize the best transformation matrix. Decomposed into relative translational deviation ΔT and relative rotational deviation ΔR, forming ΔP: The component is This reflects the translational deviation between the lidar point cloud and the map; Converted to Euler angles via "rotation matrix to Euler angles" This reflects attitude deviation; ultimately Verify the reliability of ΔP using camera visual anchor points: like , , If ΔP is valid, then ΔP is valid; otherwise, the ICP iteration is re-executed. After verification, press Correction get : The position is ; Attitude angle is .

8. The multi-source navigation data fusion method for an unmanned loader according to claim 1, characterized in that, The S4 layered fusion process, including high-level fusion, includes: Pose data output from mid-layer fusion Data from wheeled odometers; the architecture logic is: middle layer Pose data is transmitted to the first sub-filter for processing, and wheel odometry data is transmitted to the second sub-filter for processing. The two local estimation results are input to the main filter. After information weighted fusion and data consistency verification, the final fused navigation result is output. ; Sub-filter #1: Processing the middle layer posture and corresponding covariance Linear Kalman filtering is used; through adjacent... Differential calculation speed predicts the current initial pose, process noise covariance Set as compatible A high-precision diagonal matrix is ​​used to ultimately output a local estimate. With covariance ; Sub-filter #2: Processes real-time data from the wheel odometer and is used according to the error model. Calculation error Extended Kalman filtering is used; state variables are defined. , Reuse of , combined Compensation for slip error constructs state prediction formula, process noise covariance and Positive correlation, ultimately outputting a local estimate With covariance ; Information allocation coefficient calculation: Let the coefficient of sub-filter number one be... The coefficients of the second sub-filter are ,satisfy ; Weighting of wheeled odometers based on their dynamic weights calculate: , ; Calculation and feedback of fusion results: Calculation of the total information matrix (Information matrix = inverse of covariance matrix, reflecting data confidence); Fusion result: ； Fusion covariance Feedback Update: Will according to , Feedback is sent to filter number one and the initial covariance of the next cycle is updated. To avoid accumulating errors.

9. A multi-source navigation data fusion system for an unmanned loader, characterized in that, include: Data acquisition unit: Collects multi-source navigation data in real time to provide raw data support for fusion processing; The multi-source navigation data includes: satellite navigation unit data, inertial measurement unit (IMU) data, wheeled odometer data, and environmental perception unit data, wherein the environmental perception unit data includes lidar data and camera data. Spatiotemporal synchronization preprocessing unit: synchronizes multi-source data in time; converts sensor data from the environmental perception unit to the body coordinate system; Dynamic weight calculation unit: Dynamic weight calculation: Dynamically adjusts the fusion weights of each data source based on the real-time performance of the sensor and environmental interference; Layered fusion processing unit: Employs a three-level progressive fusion architecture, including: bottom-level fusion using extended Kalman filtering to fuse satellite navigation and IMU data; middle-level fusion based on the iterative nearest point algorithm, matching feature points from LiDAR point clouds with those from high-precision maps; and top-level fusion using a federated filtering architecture for integration. Data from wheeled odometers; Fault diagnosis and redundancy unit: Real-time monitoring of sensor health status, and redundancy switching in case of single or multiple point failures; Output and Optimization Unit: Outputs fused navigation data and continuously optimizes system performance through closed-loop feedback.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The computer program is executed by a processor using the multi-source navigation data fusion method for unmanned loaders as described in any one of claims 1-8.

Images