Multi-source combined navigation method and system for coal pusher scene
By integrating GNSS, IMU, lidar, and camera data and optimizing the factor graph, the problems of cumulative positioning error and system divergence of the coal pusher in the open coal yard environment were solved, thereby improving the robustness and reliability of the navigation system and enabling stable positioning in complex environments.
Patent Information
- Application Number
- CN202511067282.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-31
- Publication Date
- 2025-11-14
AI Technical Summary
In open-air coal yards with large-scale equipment such as coal pushers, existing navigation systems suffer from GNSS signal blockage or loss of lock, monotonous ground texture, severe environmental dust obscuring, and numerous dynamic obstacles, leading to cumulative positioning errors and system divergence. Traditional methods lack sensor quality discrimination and suppression mechanisms, thus failing to meet positioning requirements.
The system employs GNSS, IMU, LiDAR, and camera data fusion, performs multi-source fusion through a factor graph framework, incorporates anomaly factor detection and removal mechanisms, dynamically reconstructs factors, and combines point cloud dynamic target recognition and removal to achieve sensor adaptive weight adjustment and tight coupling optimization.
It improves the robustness and reliability of the navigation system, adapts to the continuity and stability of navigation under conditions of weak or intermittent GNSS signal failure, is suitable for areas such as enclosed material yards and tunnel entrances, has the ability to adaptively adjust the sensor observation quality, reduces the impact of abnormal observations, and ensures the uninterruptedness and error stability of the navigation system.
Smart Images
Figure CN120949283A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the fields of GNSS / INS integrated navigation, lidar, visual positioning and multi-source fusion technology, and in particular to a multi-source integrated navigation method and system for coal pusher scenarios. Background Technology
[0002] With the development of smart mines and industrial automation, the demand for automated driving of large-scale equipment such as coal pushers is constantly increasing. However, in typical operating environments, such as open-pit coal yards, these equipment often face the following challenges: GNSS signal obstruction or loss of lock, monotonous ground texture, severe environmental dust obstruction, and the presence of numerous dynamic obstacles (such as transport vehicles and coal flows). In such environments, traditional positioning systems relying on a single sensor (such as GNSS, IMU, or lidar) are prone to cumulative positioning errors or system divergence due to sensor degradation or abnormal observations, affecting overall operational safety and efficiency.
[0003] Currently, GNSS / INS integrated navigation systems are used in some industrial scenarios, typically based on loosely coupled or tightly coupled filtering models to achieve multi-source data fusion. However, under non-ideal conditions such as fluctuating GNSS availability and the presence of dynamic targets in laser point clouds, existing methods lack mechanisms for judging and suppressing the observation quality of subsystems, failing to effectively achieve sensor weighting or failure isolation, leading to jumps or drifts in navigation solutions. Furthermore, lidar systems lack the ability to identify and process abnormal point clouds in scenarios with significant dynamic interference, making their mapping and positioning accuracy susceptible to the influence of dynamic targets. Single-sensor navigation systems cannot meet the positioning requirements of coal pusher operating environments. Summary of the Invention
[0004] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a multi-source combined navigation method and system for coal pusher scenarios. The present invention will be aimed at complex application scenarios, utilize the complementary characteristics of different sensors, and perform multi-source fusion based on the factor graph framework.
[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:
[0006] A multi-source integrated navigation method for coal pusher scenarios proposed according to the present invention includes:
[0007] Collect GNSS observation data, IMU data, camera data, and lidar data, and construct GNSS factor, inertial factor, visual factor, and lidar factor respectively;
[0008] GNSS factors, inertial factors, visual factors, and laser factors are added to the factor graph for joint optimization. Joint optimization refers to: an abnormal factor detection and elimination mechanism based on residual analysis to eliminate or reduce the weight of factors that exceed the threshold; and dynamic reconstruction of each factor based on a factor degradation detection active update mechanism according to residual size, prior confidence, and environmental adaptability to ensure the sufficiency of the navigation system optimization constraints.
[0009] As a further optimization scheme for the multi-source combined navigation method for coal pusher scenarios described in this invention, a dynamic target recognition and elimination mechanism for point clouds is added to the lidar end: through real-time point cloud clustering and inter-frame motion analysis, dynamic point cloud clusters are segmented, and then covariance dilation processing is applied to the dynamic region observation during the factor graph optimization process.
[0010] As a further optimization scheme for the multi-source integrated navigation method for coal pusher scenarios described in this invention, it includes an inertial navigation positioning estimation module, a LiDAR / vision module, a GNSS / INS integrated navigation module, and a factor graph optimization module, wherein...
[0011] The GNSS / INS integrated navigation module, the inertial navigation positioning module, and the LiDAR / vision module are used to collect GNSS observation data, inertial measurement unit (IMU) data, camera data, and LiDAR data respectively, and to construct GNSS factors, inertial factors, vision factors, and LiDAR factors.
[0012] The factor graph optimization module allows users to jointly optimize GNSS, inertial, visual, and laser factors by adding them to the factor graph. Joint optimization refers to: an abnormal factor detection and removal mechanism based on residual analysis to remove or reduce the weight of factors exceeding the threshold; and a proactive update mechanism based on factor degradation detection to dynamically reconstruct each factor according to the residual size, prior confidence, and environmental adaptability, ensuring the sufficiency of the navigation system optimization constraints.
[0013] As a further optimization scheme of the multi-source combined navigation method for coal pusher scenarios described in this invention, the inertial navigation positioning calculation module calculates the relative motion of the carrier within a preset time period through pre-integration based on the original observation data of the IMU. The measured value of the relative motion of the carrier is the IMU pre-integration.
[0014] As a further optimization of the multi-source combined navigation method for coal pusher scenarios described in this invention, the LiDAR / vision module extracts environmental features, dynamically identifies and eliminates targets based on LiDAR point cloud data, and outputs optimized local pose constraints.
[0015] As a further optimization of the multi-source integrated navigation method for coal pusher scenarios described in this invention, the GNSS / INS integrated navigation module provides GNSS observation data for global positioning reference based on GNSS satellite signals, and corrects IMU drift.
[0016] As a further optimization scheme for the multi-source combined navigation method for coal pusher scenarios described in this invention, the factor graph optimization module constructs a factor graph based on IMU pre-integration, LiDAR odometry, and GNSS observations, performs tight-coupled optimization, and jointly estimates pose, velocity, and IMU error parameters to obtain the globally optimal navigation state.
[0017] As a further optimization of the multi-source combined navigation method for coal pusher scenarios described in this invention, the inertial navigation positioning module includes an IMU consisting of a three-axis accelerometer and a three-axis gyroscope.
[0018] A computer device includes a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor executes the computer program to implement the steps of the multi-source integrated navigation method for a coal pusher scenario as described above.
[0019] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the multi-source combined navigation method for a coal pusher scenario as described above.
[0020] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:
[0021] (1) Improve the robustness and reliability of the navigation system. This invention adopts the fusion of multi-source information from GNSS, inertial measurement unit (IMU), lidar and vision. Through factor graph tight coupling modeling, the system has the ability to jointly process observation data from different sensors, which significantly improves the navigation continuity and stability under the condition of weak or intermittent failure of GNSS signal. It is particularly suitable for the positioning needs of coal pushers operating in weak satellite areas such as closed material yards and tunnel entrances.
[0022] (2) Possesses adaptive adjustment capability for sensor observation quality. Addressing issues such as dust obstruction, equipment vibration, and rapid scene changes in coal pusher scenarios, this invention introduces a factor confidence assessment mechanism and a dynamic covariance weighting strategy. It can adaptively adjust the weight of each sensor in factor graph optimization based on the current observation status of each sensor (e.g., laser point cloud density, IMU abnormal jitter), effectively reducing the impact of abnormal observations on the positioning solution. Compatible with intermittent GNSS availability scenarios, adaptive navigation mode switching. For typical working conditions with periodic GNSS signal obstruction (e.g., passing through obstructed structures, driving to the inside of a stack), this invention can perform combined filtering correction when GNSS is available, and automatically switch to laser / IMU mode when GNSS is unavailable, ensuring the uninterrupted operation and error stability of the navigation system, and improving the system's practicality and intelligence. Attached Figure Description
[0023] Figure 1 This is a system structure diagram of the present invention;
[0024] Figure 2 This is a system state flowchart of the present invention;
[0025] Figure 3 This is a flowchart of the inertial navigation integration process. Detailed Implementation
[0026] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
[0027] This invention urgently requires a combined navigation method with multi-source sensor collaborative processing capabilities, anomaly observation identification and elimination mechanisms, and adaptive factor weight adjustment strategies to meet the high robustness requirements of navigation and positioning in dynamic industrial scenarios such as coal yards. For challenging environments, positioning and navigation technologies have also been enhanced accordingly, with multi-source fusion positioning and navigation systems based on factor graph optimization becoming a widely used solution. In recent years, factor graph optimization technology has made significant progress in the field of SLAM. The fusion method based on factor graph optimization constructs a factor graph by combining the current state with the previous state, considering global consistency, and then establishes a batch optimization cost function, thereby enabling optimization using more information. In the factor graph optimization framework, the pose of the mobile robot at different time points is abstracted as nodes in the graph, while the data collected by sensors between adjacent time points establishes observation edges. Simultaneously, the pose changes of the robot in adjacent time periods form motion edges. These nodes and edges together construct a complex constraint graph. Next, a nonlinear optimization method is used to iteratively optimize the edges in the graph until they are minimized, thereby achieving accurate optimization of the global pose. Backend optimization involves more complex processing of frontend data. One approach is to directly optimize the global position. For large-scale localization and mapping tasks, a local map can be constructed, and a sliding window approach can be used to optimize the pose within the window. This method not only ensures the integrity of pose data during optimization but also cleverly avoids the computational burden on the backend caused by large datasets. By comprehensively optimizing all pose information, system accuracy can be significantly improved, and the cumulative error in large-scale scene mapping can be reduced.
[0028] A multi-source integrated navigation method for coal pusher scenarios includes:
[0029] Collect GNSS observation data, IMU data, camera data, and lidar data, and construct GNSS factor, inertial factor, visual factor, and lidar factor respectively;
[0030] GNSS factors, inertial factors, visual factors, and laser factors are added to the factor graph for joint optimization. Joint optimization refers to: an abnormal factor detection and elimination mechanism based on residual analysis to eliminate or reduce the weight of factors that exceed the threshold; and dynamic reconstruction of each factor based on a factor degradation detection active update mechanism according to residual size, prior confidence, and environmental adaptability to ensure the sufficiency of the navigation system optimization constraints.
[0031] A dynamic target recognition and elimination mechanism for point clouds is added to the lidar terminal: through real-time point cloud clustering and inter-frame motion analysis, dynamic point cloud clusters are segmented, and then covariance dilation is applied to the dynamic region observation during the factor map optimization process.
[0032] A system for a multi-source integrated navigation method for coal pusher scenarios includes an inertial navigation positioning module, a LiDAR / vision module, a GNSS / INS integrated navigation module, and a factor graph optimization module.
[0033] The GNSS / INS integrated navigation module, the inertial navigation positioning module, and the LiDAR / vision module are used to collect GNSS observation data, inertial measurement unit (IMU) data, camera data, and LiDAR data respectively, and to construct GNSS factors, inertial factors, vision factors, and LiDAR factors.
[0034] The factor graph optimization module allows users to jointly optimize GNSS, inertial, visual, and laser factors by adding them to the factor graph. Joint optimization refers to: an abnormal factor detection and removal mechanism based on residual analysis to remove or reduce the weight of factors exceeding the threshold; and a proactive update mechanism based on factor degradation detection to dynamically reconstruct each factor according to the residual size, prior confidence, and environmental adaptability, ensuring the sufficiency of the navigation system optimization constraints.
[0035] The inertial navigation positioning module calculates the relative motion of the carrier within a preset time period based on the raw observation data from the IMU through pre-integration. The measured value of the relative motion of the carrier is the IMU pre-integration.
[0036] The LiDAR / Vision module extracts environmental features from LiDAR point cloud data, performs dynamic target recognition and elimination, and outputs optimized local pose constraints.
[0037] The GNSS / INS integrated navigation module provides GNSS observation data based on GNSS satellite signals for global positioning reference and to correct IMU drift.
[0038] The factor graph optimization module constructs a factor graph based on IMU pre-integration, LiDAR odometry, and GNSS observations, performs tightly coupled optimization, and jointly estimates pose, velocity, and IMU error parameters to obtain the globally optimal navigation state.
[0039] The inertial navigation positioning module includes an IMU consisting of a three-axis accelerometer and a three-axis gyroscope.
[0040] like Figure 1 This is a system structure diagram of the present invention. Figure 2 This is a system state flowchart of the present invention. Figure 3 This is a flowchart of the inertial navigation integration process. The invention comprises three modules: an inertial navigation position estimation module, a lidar / visual odometry module, and a factor graph multi-source fusion module. Among them,
[0041] The LiDAR odometry module extracts LiDAR features and performs odometry by matching the extracted features using a feature map. Points with larger roughness values are classified as edge features. Similarly, planar features are classified according to smaller roughness values. The edge and planar features extracted from the LiDAR scan at time i are denoted as Fei and Fpi, respectively. Calculating and adding factors to the graph using each LiDAR frame is computationally challenging; therefore, the concept of keyframe selection, widely used in visual SLAM, is employed. Compared to the previous state xi, when the robot pose change exceeds a user-defined threshold, a simple yet effective heuristic selects LiDAR frame Fi+1 as the keyframe. The newly saved keyframe Fi+1 is associated with the new robot state node xi+1 in the factor graph. LiDAR frames between two keyframes are discarded. Adding keyframes in this way not only achieves a balance between mapping density and memory consumption but also helps maintain a relatively sparse factor graph, which is suitable for real-time nonlinear optimization. The feature map is maintained using a sliding window to achieve real-time performance. Before system initialization, the system is assumed to be stationary. The initial values are crucial for scan-to-map matching. The IMU pose integral from the keyframe to the current time step is used as the predicted matching value. After initializing the system error, the IMU bias, system pose, and velocity are estimated based on the factor map. After system initialization, the initial values for laser matching are derived from the IMU integral values. When a new LiDAR scan arrives, feature extraction is performed first. Edge and planar features are extracted by evaluating the roughness of points in the local region. The odometry module consists of two subsystems: a vision-inertial system (VIS) and a radar-inertial system (LIS). These two subsystems can operate independently if one fails, or they can function together when sufficient features are detected. The VIS system performs visual feature tracking and selectively extracts feature depth using LiDAR frames. Visual odometry, obtained by optimizing visual reprojection errors and IMU measurements, serves as the initial estimate for LiDAR scan matching and introduces constraints into the factor map. After point cloud distortion correction using IMU measurements, the LIS system extracts edge and planar features from the LiDAR point cloud and registers these features to a feature map maintained in a sliding window. The estimated system state in the LIS can be sent to the VIS subsystem for initialization. For loop closure, candidate matches are first identified by the VIS subsystem and further optimized by the LIS subsystem. Constraints from visual odometry, radar odometry, IMU pre-integration, and loop closure are jointly optimized in the factor graph. Finally, the optimized IMU bias term is used to propagate IMU measurements at the IMU frequency for attitude estimation. A tightly coupled LVIO framework is built on the factor graph, enabling global optimization with multi-sensor fusion and scene recognition assistance.By bypassing failed subsystems through fault detection, it becomes robust to sensor degradation.
[0042] In the inertial navigation positioning module, IMU data is used to provide a good initial pose estimate for the vehicle. Since most current LiDAR systems operate at 10Hz, with some exceeding 20Hz, the vehicle will rotate and displace between two radar point cloud frames. If the rotation and displacement are small, point cloud matching can independently calculate a relatively accurate odometer. However, if the rotation and displacement are large, point cloud matching is likely to fail. IMUs typically have higher frequencies (above 200Hz), thus providing a better initial pose value for matching two radar point clouds. However, simply using IMU data for integration will result in a large accumulated error over time, failing to provide a good initial pose estimate. Therefore, it is necessary to integrate other sensors to correct the IMU integration algorithm. In this invention, the latest odometer information is continuously obtained from the laser odometer as observation for the IMU integration algorithm, thereby achieving the purpose of correcting the IMU integration algorithm.
[0043] In the factor graph multi-source fusion module, the state estimation problem can be represented as a maximum a posteriori (MAP) problem. This is achieved by jointly optimizing the IMU pre-integration constraints, LiDAR odometry constraints, and closed-loop constraints in the factor graph using the iSAM2 library. The system state variables are recursively estimated from the IMU pre-integration. Therefore, the globally optimized system equation is:
[0044]
[0045] in, X is the prior estimate of the current frame k in the factor graph. k-1 w represents the system state variables after the previous factor graph optimization. k For zero-mean process noise, v k The measurement noise is zero-mean. The specific form of the measurement equation is sensor-dependent. Observation sources include pose changes from laser odometry and absolute position information from GPS.
[0046] (1) When observing absolute position information provided by GPS:
[0047]
[0048] The corresponding residual is:
[0049]
[0050] (2) When observing the pose transformation provided by the laser odometry:
[0051]
[0052] X′k For the pose of the current frame after laser scan-to-map matching optimization, X k-1 This is the pose after factor map optimization from the previous frame. The corresponding residual is...
[0053]
[0054] Transform it into a nonlinear least squares problem:
[0055]
[0056] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor. When the processor executes the computer program, it implements the steps of the multi-source combined navigation method for coal pusher scenarios as described above.
[0057] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the multi-source combined navigation method for coal pusher scenarios described above.
[0058] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present invention can be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.
[0059] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0060] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.
[0061] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.
[0062] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.
[0063] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.
Claims
1. A multi-source integrated navigation method for coal pusher scenarios, characterized in that, include: Collect GNSS observation data, IMU data, camera data, and lidar data, and construct GNSS factor, inertial factor, visual factor, and lidar factor respectively; GNSS factors, inertial factors, visual factors, and laser factors are added to the factor graph for joint optimization. Joint optimization refers to: an abnormal factor detection and elimination mechanism based on residual analysis to eliminate or reduce the weight of factors that exceed the threshold; and dynamic reconstruction of each factor based on a factor degradation detection active update mechanism according to residual size, prior confidence, and environmental adaptability to ensure the sufficiency of the navigation system optimization constraints.
2. The multi-source integrated navigation method for coal pusher scenarios according to claim 1, characterized in that, A dynamic target recognition and elimination mechanism for point clouds is added to the lidar terminal: through real-time point cloud clustering and inter-frame motion analysis, dynamic point cloud clusters are segmented, and then covariance dilation is applied to the dynamic region observation during the factor map optimization process.
3. A system based on the multi-source combined navigation method for a coal pusher scenario as described in claim 1, characterized in that, It includes an inertial navigation positioning module, a LiDAR / vision module, a GNSS / INS integrated navigation module, and a factor graph optimization module, among which, The GNSS / INS integrated navigation module, the inertial navigation positioning module, and the LiDAR / vision module are used to collect GNSS observation data, inertial measurement unit (IMU) data, camera data, and LiDAR data respectively, and to construct GNSS factors, inertial factors, vision factors, and LiDAR factors. The factor graph optimization module allows users to jointly optimize GNSS, inertial, visual, and laser factors by adding them to the factor graph. Joint optimization refers to: an abnormal factor detection and removal mechanism based on residual analysis to remove or reduce the weight of factors exceeding the threshold; and a proactive update mechanism based on factor degradation detection to dynamically reconstruct each factor according to the residual size, prior confidence, and environmental adaptability, ensuring the sufficiency of the navigation system optimization constraints.
4. The system of a multi-source combined navigation method for a coal pusher scenario according to claim 3, characterized in that, The inertial navigation positioning module calculates the relative motion of the carrier within a preset time period based on the raw observation data from the IMU through pre-integration. The measured value of the relative motion of the carrier is the IMU pre-integration.
5. The system of a multi-source combined navigation method for a coal pusher scenario according to claim 4, characterized in that, The LiDAR / Vision module extracts environmental features from LiDAR point cloud data, performs dynamic target recognition and elimination, and outputs optimized local pose constraints.
6. The system of a multi-source combined navigation method for a coal pusher scenario according to claim 5, characterized in that, The GNSS / INS integrated navigation module provides GNSS observation data based on GNSS satellite signals for global positioning reference and to correct IMU drift.
7. The system of a multi-source combined navigation method for a coal pusher scenario according to claim 6, characterized in that, The factor graph optimization module constructs a factor graph based on IMU pre-integration, LiDAR odometry, and GNSS observations, performs tightly coupled optimization, and jointly estimates pose, velocity, and IMU error parameters to obtain the globally optimal navigation state.
8. The system of a multi-source combined navigation method for a coal pusher scenario according to claim 3, characterized in that, The inertial navigation positioning module includes an IMU consisting of a three-axis accelerometer and a three-axis gyroscope.
9. A computer device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the multi-source combined navigation method for coal pusher scenarios as described in any one of claims 1 to 2.
10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the multi-source combined navigation method for coal pusher scenarios as described in any one of claims 1 to 2.