A multi-source fusion robust navigation and reliability quantitative evaluation system based on a preset sparse map

Abstract

The application discloses a kind of fusion navigation and reliability quantitative evaluation system based on pre-set sparse map.The system is fused positioning module by multi-source sensor, and is fused vision, inertia and optical flow data based on pre-set sparse feature map;Reliability quantitative evaluation module is used to calculate feature distribution balance degree, trajectory coverage and feature stability index, and generate navigation map quality index;Decision control module divides reliability level according to navigation map quality index, dynamically adjusts unmanned aerial vehicle flight speed and task mode;Abnormal processing module triggers degradation flight and monitors pose uncertainty when signal source fails, executes return or hovering strategy when threshold is exceeded.The application realizes high robustness positioning and hierarchical safety control of unmanned aerial vehicle in complex environment.

Description

Technical Field

[0001] This invention belongs to the field of UAV navigation and control technology, and relates to a fusion navigation and reliability quantification evaluation system based on a pre-set sparse map. Background Technology

[0002] With the widespread application of drones in power line inspection, warehousing and logistics, and emergency rescue, high-precision positioning and navigation are core prerequisites for the safe flight of UAVs. Currently, most mainstream UAV navigation relies on GNSS or dense map relocation based on lidar / vision. However, in scenarios such as indoor warehouses, tunnels, or urban canyons with tall buildings, GNSS signals are lacking, and the environment often has missing textures, repetitive structures, or dynamic airflow interference, making positioning based on a single sensor prone to failure. For example, in areas with sparse features such as white walls or open halls, insufficient visual feature points often cause UAV position drift or even crashes.

[0003] While existing fusion navigation technologies combine inertial measurement units (IMUs) with visual / laser data, they mostly employ fixed noise models and lack real-time quantification of perception quality. When sensor data quality deteriorates due to severe vibration, sudden changes in lighting, or rapid maneuvering, traditional filters, if unable to adjust weights in time, can introduce erroneous observations into the system, causing fusion divergence and leading to uncontrollable drones.

[0004] Furthermore, existing technologies lack a tiered response mechanism when encountering location anomalies, often resorting to emergency landings or blind return-to-home maneuvers without dynamically adjusting flight speed, hovering, or retreating along a high-reliability trajectory based on reliability levels. This approach not only reduces operational efficiency but may also lead to collisions due to accumulated errors and lack of monitoring. Therefore, a UAV navigation system capable of real-time quantification of navigation reliability, adaptive adjustment of fusion strategies, and tiered flight control is needed. Summary of the Invention

[0005] To address the problems existing in the background technology, this invention proposes a fusion navigation and reliability quantification evaluation system based on a pre-set sparse map.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows: a fusion navigation and reliability quantification evaluation system based on a pre-set sparse map, comprising:

[0007] The map loading module is configured to load a preset sparse feature map of the target area, wherein the preset sparse feature map contains multiple feature points with global coordinates and their corresponding feature descriptors;

[0008] The multi-source sensor fusion positioning module is configured to acquire multiple sensor data of the mobile platform in real time, and obtain pose observation information through at least two of the following methods: visual relocalization, inertial navigation and optical flow velocimetry, based on the preset sparse feature map. Then, according to the real-time confidence of each information source, the pose observation information is adaptively weighted and fused to output the optimal pose estimate.

[0009] The reliability quantification assessment module is configured to calculate and output a quantitative index characterizing the current navigation reliability in real time based on the feature distribution status of the current positioning area in the preset sparse feature map, the historical trajectory coverage of the mobile platform, and the stability of the currently matched feature points.

[0010] The decision control module is configured to generate and execute corresponding navigation control strategies based on the quantitative indicators.

[0011] Specifically, the reliability quantification assessment module is configured as follows:

[0012] The analysis region is defined with the current optimal pose estimate as the center;

[0013] Calculate the feature distribution balance of the matched map feature points within the analysis area across different sub-regions;

[0014] The degree to which the movement trajectory of the mobile platform covers the analysis area during a preset time period prior to the current moment is calculated to obtain the trajectory coverage.

[0015] Calculate the feature stability index of the map feature points matched at the current time based on their historical matching success rate and the freshness of the matching time.

[0016] Based on preset weights, the feature distribution balance, trajectory coverage, and feature stability index are weighted and summed to generate the quantitative index.

[0017] Specifically, the feature distribution balance is obtained by dividing the analysis area into a preset number of grids, counting the number of matching feature points falling into each grid, and calculating their information entropy after normalization.

[0018] The trajectory coverage is calculated based on marking the coverage of the grid in the analysis area, applying a time decay weight to the coverage status of each grid, and then calculating the proportion of effectively covered grids.

[0019] The feature stability index is obtained by weighting the matching success rate of each currently matched feature point within its historical observation window and combining it with the time decay factor from the current time.

[0020] Specifically, the multi-source sensor fusion positioning module includes a confidence calculation unit, which is configured to independently and in real time calculate the confidence of each information source;

[0021] The visual repositioning confidence is calculated based on the number of feature points matched between the current frame and the preset sparse feature map, the matching accuracy, and the uniformity of the spatial distribution of the matching points on the image plane.

[0022] The inertial navigation confidence level is calculated based on the vibration intensity, temperature change, and time interval of the inertial measurement unit and the last visual reset.

[0023] The confidence level of optical flow velocimetry is calculated based on the texture richness, illumination stability, and consistency of the optical flow vector in the optical flow image.

[0024] Specifically, the multi-source sensor fusion localization module further includes an adaptive fusion unit, which is configured to map the real-time confidence of each information source to the noise covariance matrix or information matrix of the corresponding observation, and to perform a filtering algorithm or graph optimization algorithm based on the updated noise covariance matrix or information matrix to dynamically adjust the fusion weight of each observation in the optimal pose estimation.

[0025] Specifically, the adaptive fusion unit is further configured to perform the following fusion mode based on the real-time confidence level:

[0026] First fusion mode: When the confidence level of visual relocation is higher than the first threshold and the confidence level of optical flow velocity measurement is higher than the second threshold, the visual relocation, inertial navigation and optical flow velocity measurement observations are tightly coupled and fused.

[0027] Second fusion mode: When the visual repositioning confidence is lower than the first threshold but higher than the third threshold, the visual repositioning and inertial navigation observations are loosely coupled and fused, and the fusion weight of the optical flow velocity observations is independently adjusted according to the optical flow velocity confidence.

[0028] Third fusion mode: When the visual repositioning confidence is lower than the third threshold, the visual repositioning observation is disabled, and the inertial navigation and optical flow velocity measurement observations are fused.

[0029] Specifically, the decision control module is configured as follows:

[0030] The quantitative indicators are compared with multiple preset reliability level thresholds to determine the reliability level of the current navigation.

[0031] Based on the reliability level, at least one of the control parameters of the mobile platform, including the maximum permissible flight speed, pose update frequency, and mission execution mode, is dynamically adjusted.

[0032] Specifically, the decision control module is further configured as follows:

[0033] When the quantitative indicator falls below the safety threshold, an early warning signal is triggered, and a safety strategy is generated to guide the mobile platform to return along a historical high-reliability trajectory or hover in place.

[0034] Specifically, the pre-built sparse feature map is constructed in advance in the following manner:

[0035] A mobile platform equipped with positioning sensors and feature acquisition sensors is used to collect image sequences of the target area and their corresponding positioning data along a preset path.

[0036] Extract feature points with descriptors from the image sequence;

[0037] Based on the positioning data, calculate the three-dimensional coordinates of each feature point in the global coordinate system;

[0038] The three-dimensional coordinates of the feature points and their descriptors are associated and stored to form the preset sparse feature map.

[0039] Specifically, the aforementioned fusion navigation and reliability quantification evaluation system based on a pre-set sparse map further includes an anomaly handling module, which is configured as follows:

[0040] Monitor the data streams from each sensor and the confidence level of each information source;

[0041] When the confidence level of any information source is continuously lower than the failure threshold within a preset number of consecutive frames, the information source is determined to be faulty, and the multi-source sensor fusion positioning module is notified to adjust the fusion strategy.

[0042] When the visual relocation source fails and cannot be recovered, a degraded operation state is triggered. In this state, the system performs pose estimation based on inertial navigation data or a combination of inertial navigation and optical flow velocity measurement data, and estimates pose uncertainty in real time according to a preset error growth model. When the uncertainty exceeds a preset safety threshold, the decision control module is triggered to execute a safety strategy.

[0043] Compared with the prior art, the present invention has the following beneficial effects: the system dynamically adjusts the fusion weights of visual, inertial and optical flow data based on feature distribution balance, trajectory coverage and feature stability index, avoids fusion divergence caused by low-quality observation, and improves the positioning robustness of UAVs in feature sparse or complex airflow environments.

[0044] The system adaptively adjusts the UAV's maximum permissible flight speed, attitude update frequency, and mission mode based on the real-time reliability level. This ensures that the UAV operates efficiently with high reliability and automatically reduces speed or enters a cautious flight mode when reliability deteriorates, achieving a balance between flight efficiency and safety.

[0045] When critical information sources such as vision fail, the system automatically switches to a degraded calculation mode based on inertia and optical flow, and uses a preset error growth model to monitor pose uncertainty in real time. Once the uncertainty exceeds the limit, a precise safety strategy is immediately triggered to eliminate the risk of crash caused by blind landing or loss of control over errors, and to enhance the survivability of the UAV under extreme conditions. Attached Figure Description

[0046] Figure 1 This is a block diagram of a fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to the present invention;

[0047] Figure 2 This is a flowchart of a fusion navigation and reliability quantification evaluation method based on a pre-set sparse map according to the present invention. Detailed Implementation

[0048] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0049] like Figures 1-2 As shown, the technical solution adopted by the present invention is as follows: A fusion navigation and reliability quantification evaluation system based on a pre-set sparse map, comprising:

[0050] The map loading module is configured to load a preset sparse feature map of the target area. The preset sparse feature map contains multiple feature points with global coordinates and their corresponding feature descriptors.

[0051] The map loading module loads a pre-set sparse feature map of the target area. The data structure of the pre-set sparse feature map contains multiple feature points and their corresponding feature descriptors. Each feature point has global coordinates, which refer to the spatial position information of the feature point in a unified global reference system (such as the WGS-84 coordinate system or a local N / A coordinate system), used to provide an absolute position reference for the navigation system. The feature descriptor is a data vector characterizing the local appearance or geometric properties of the feature point, used for feature matching during online navigation to identify the current pose.

[0052] Specifically, the pre-built sparse feature map is constructed in advance in the following manner:

[0053] A mobile platform equipped with positioning sensors and feature acquisition sensors is used to collect image sequences of the target area and their corresponding positioning data along a preset path.

[0054] Extract feature points with descriptors from the image sequence.

[0055] Based on the positioning data, the three-dimensional coordinates of each feature point in the global coordinate system are calculated.

[0056] The three-dimensional coordinates of the feature points and their descriptors are associated and stored to form the preset sparse feature map.

[0057] The construction process utilizes a mobile platform equipped with positioning sensors and feature acquisition sensors. The positioning sensors acquire high-precision location information, such as RTK GNSS receivers or total stations; the feature acquisition sensors acquire environmental visual or geometric data, such as cameras or LiDAR.

[0058] The mobile platform moves along a preset path, which is a pre-planned trajectory covering the target area. During the movement, the mobile platform synchronously acquires image sequences of the target area and their corresponding positioning data. The image sequence refers to a series of image frames arranged in chronological order; the positioning data refers to the position and orientation information that is strictly synchronized in time with each frame in the image sequence.

[0059] The system extracts feature points from image sequences. The extraction process involves using feature detection algorithms (such as FAST, Harris corner detection, or deep learning feature detection networks) to identify salient points in the image and calculating a feature descriptor for each identified feature point. The calculation of the feature descriptor aims to generate vectors with rotation invariance, scale invariance, and illumination robustness, so as to accurately match the same physical feature point under different viewpoints.

[0060] The system calculates the 3D coordinates of each feature point in the global coordinate system based on positioning data. The global coordinate system is a reference coordinate system consistent with the output of the positioning sensor. The calculation process typically utilizes multi-view geometry principles, combining the intrinsic and extrinsic parameters of the feature acquisition sensor with the pose provided by the positioning data, and solves for the 3D spatial position of the feature point through triangulation or bundle adjustment. The 3D coordinates are represented as follows: Its unit is usually the meter (m).

[0061] The system stores the 3D coordinates and feature descriptors of each feature point in association. Association storage means that in the data structure, the 3D coordinates and feature descriptors are bound together by a unique feature point ID, forming key-value pairs or structure records. The set of all feature points processed and stored in association through the above steps ultimately forms a pre-defined sparse feature map. This pre-defined sparse feature map is saved as a file for the map loading module to read and use during system runtime.

[0062] The multi-source sensor fusion positioning module is configured to acquire multiple sensor data from the mobile platform in real time, and obtain pose observation information through at least two of the following methods: visual relocalization, inertial navigation, and optical flow velocimetry, based on the preset sparse feature map. Then, according to the real-time confidence level of each information source, the pose observation information is adaptively weighted and fused to output the optimal pose estimate.

[0063] In this embodiment, the sensor group configured in the multi-source sensor fusion positioning module specifically includes: a visual camera (supporting optical flow-assisted algorithm), an inertial measurement unit (IMU), a barometer (for high-altitude accurate ranging and altitude maintenance), a rangefinder (infrared or ultrasonic, for low-altitude obstacle avoidance and terrain following altitude maintenance), a magnetometer (for direction detection and correction), and a GNSS receiver.

[0064] The system dynamically adjusts the sensor fusion strategy based on flight altitude:

[0065] Low-altitude mode (altitude < 50 meters): The system mainly integrates data from the inertial IMU, rangefinder (as the primary altitude source, with barometer as an auxiliary), and magnetometer. In this mode, the vision system is primarily used for optical flow velocity measurement assistance.

[0066] High-altitude mode (height ≥ 50 meters): The system mainly integrates visual (analyzing sparse features below for pose comparison and positioning, with optical flow assistance), inertial IMU, barometer (as the main altitude source) and magnetometer data.

[0067] Specifically, for GNSS signal interference scenarios, the system utilizes the high reliability features of sparse maps for anti-interference processing: when the deviation between the visual repositioning result and the GNSS data exceeds a threshold, and the current area has rich sparse map features, the system automatically determines that the GNSS is interfered with, reduces the GNSS position confidence or even removes it, and seamlessly switches to the Non-GPS navigation mode based on sparse maps; after driving out of the interference area, the GNSS weight is automatically restored.

[0068] Furthermore, the information provided by each sensor can be divided into multiple parallel dimensions according to the type of physical quantity. The system maintains multiple information sources for each dimension to enhance robustness.

[0069] Altitude dimension: including GNSS altitude, barometer altitude, and rangefinder altitude.

[0070] Location dimension: including GNSS location, inertial navigation estimated location, and visual relocation location based on sparse map.

[0071] Horizontal velocity dimension: including GNSS horizontal velocity, inertial navigation integral velocity, and visual velocity measurement based on sparse maps (such as horizontal velocity calculated by optical flow method).

[0072] Vertical velocity dimension: including GNSS vertical velocity, inertial navigation vertical velocity, barometer differential velocity, and rangefinder differential velocity.

[0073] Directional dimension: including GNSS heading angle, magnetometer direction, and visual direction based on sparse map (i.e., nose orientation calculated through consecutive frames or relocation).

[0074] The system independently calculates the confidence level for multiple information sources in each dimension, and dynamically adjusts the weight of each information source based on the confidence level during the fusion positioning process. This ensures that reliable navigation solutions can still be maintained through other information sources even if any information source fails or its accuracy decreases.

[0075] Visual relocalization refers to the process of obtaining absolute pose by matching the current image frame with a preset sparse feature map; inertial navigation refers to the process of calculating relative pose changes by integrating inertial measurement unit data; optical flow velocimetry refers to the process of estimating velocity by analyzing pixel motion between consecutive image frames.

[0076] Subsequently, the multi-source sensor fusion positioning module adaptively weights and fuses the pose observation information based on the real-time confidence level of each information source. The real-time confidence level is a quantitative indicator that characterizes the reliability of the current sensor data, and its value ranges from [0,1].

[0077] The fusion process employs a weighted algorithm, with weights positively correlated with real-time confidence levels, ultimately outputting the optimal pose estimate. The optimal pose estimate includes a position vector. and attitude quaternions The units are meters (m) and dimensionless.

[0078] Specifically, the multi-source sensor fusion positioning module includes a confidence calculation unit, which is configured to independently and in real time calculate the confidence of each information source.

[0079] The visual repositioning confidence is calculated based on the number of feature points matched between the current frame and the preset sparse feature map, the matching accuracy, and the uniformity of the spatial distribution of the matching points on the image plane.

[0080] The inertial navigation confidence level is calculated based on the vibration intensity, temperature change, and time interval of the inertial measurement unit and the last visual reset.

[0081] The confidence level of optical flow velocimetry is calculated based on the texture richness, illumination stability, and consistency of the optical flow vector in the optical flow image.

[0082] It should be noted that optical flow velocimetry is a specific implementation of visual algorithm-based velocimetry, which estimates the speed of a moving platform by utilizing pixel motion between consecutive image frames. Under conditions of rich texture and stable lighting, optical flow velocimetry can provide a smooth horizontal velocity estimate, but the error may accumulate over time. Besides optical flow, other visual velocimetry algorithms can also be used, such as inter-frame displacement estimation based on feature matching, which also falls under the category of visual velocimetry. Its confidence calculation principle is similar to that of optical flow velocimetry, both relying on image texture quality and the consistency of motion estimation.

[0083] In addition, the confidence calculation unit is also configured to calculate the magnetometer confidence and GNSS confidence.

[0084] The magnetometer confidence level is calculated based on the consistency between the magnetometer readings and the visual orientation estimation based on a sparse map. Specifically, the system compares the nose direction obtained at the current moment through visual relocalization or continuous frame motion estimation with the heading angle output by the magnetometer, and calculates the deviation angle between the two. The smaller the deviation, the higher the magnetometer confidence level; if the deviation exceeds a preset threshold, the magnetometer confidence level is reduced to avoid heading errors caused by geomagnetic interference.

[0085] The GNSS confidence level is calculated based on satellite signal strength, positioning accuracy factor (DOP), and deviation from the visual repositioning result. When visual repositioning is available and the current area has rich sparse map features, the system compares the GNSS position with the visual repositioning position. If the deviation exceeds a preset deviation threshold, it is determined that GNSS may be interfered with, and the GNSS confidence level is reduced accordingly. Conversely, if the deviation is within a reasonable range, the GNSS confidence level is maintained or increased. When the GNSS signal is completely lost or the confidence level remains below the failure threshold, the system automatically removes GNSS information and switches to Non-GPS navigation mode.

[0086] Furthermore, the multi-source sensor fusion positioning module includes a confidence calculation unit. The confidence calculation unit is configured to independently, in parallel, and in real time calculate the confidence of each information source.

[0087] Independent computation means that the calculation processes for visual repositioning confidence, inertial navigation confidence, and optical flow velocity confidence are independent of each other, and each is calculated based on its own input data source, ensuring that a decline in the performance of a single sensor does not affect the accuracy of the confidence assessment of other sensors.

[0088] Visual repositioning reliability It is a function of three factors: the first is the number of feature points that match the current frame with the preset sparse feature map, denoted as... Second, matching accuracy, denoted as The first is the root mean square value of the reprojection error; the second is the spatial uniformity of the matching points on the image plane, denoted as . The calculation formula is defined as follows: .

[0089] in, The larger, The higher; The smaller, The higher; The higher the value (i.e., the more dispersed the feature points are, and the wider the image area they cover), the better. The higher. It can be quantified by calculating the variance of the distance from the centroid of the matching point to each point or the entropy value of the image grid coverage.

[0090] Inertial navigation confidence It is a function of three factors: one is the vibration intensity of the inertial measurement unit, denoted as... The first is usually calculated from the energy or variance of the high-frequency components of accelerometers and gyroscopes; the second is temperature change, denoted as... 1) The absolute value of the difference between the current IMU temperature and the calibrated reference temperature; 2) The time interval from the last visual reset, denoted as . The calculation formula is defined as follows: .

[0091] in, The smaller, The higher; The smaller, The higher; The shorter, The higher the value, the greater the cumulative error in inertial navigation, because prolonged periods without visual correction will lead to increased errors.

[0092] Optical flow velocimetry confidence It is a function of three factors: the first is the texture richness of the optical flow image, denoted as... The first is usually calculated from the mean or variance of the image gradient magnitude; the second is illumination stability, denoted as... The first is calculated from the correlation of the brightness histograms or the average brightness difference between two adjacent frames; the second is the consistency of the optical flow vector, denoted as... It is calculated from the proportion of interior points after fitting the global motion model using the RANSAC algorithm. The calculation formula is defined as follows: .

[0093] in, The higher, The higher; The higher the temperature (the smaller the change in light intensity), the better. The higher; The higher, The higher.

[0094] Specifically, the multi-source sensor fusion localization module further includes an adaptive fusion unit, which is configured to map the real-time confidence of each information source to the noise covariance matrix or information matrix of the corresponding observation, and to perform a filtering algorithm or graph optimization algorithm based on the updated noise covariance matrix or information matrix to dynamically adjust the fusion weight of each observation in the optimal pose estimation.

[0095] The multi-source sensor fusion localization module also includes an adaptive fusion unit. The adaptive fusion unit is configured to map the real-time confidence level of each information source to the noise covariance matrix of the corresponding observation. or information matrix In the extended Kalman filter architecture, the mapping relationship is defined as: ,in Based on the basic noise covariance matrix, For real-time confidence level, This is the scaling factor. This represents an exponential function.

[0096] This formula shows that as the real-time confidence level decreases, the noise covariance matrix... The weight of this observation increases exponentially, thus reducing its weight. Under the factor graph optimization framework, the mapping relationship is defined as: ,in Based on the basic information matrix.

[0097] The adaptive fusion unit is based on the updated noise covariance matrix. or information matrix The filtering algorithm or graph optimization algorithm is executed to achieve dynamic adjustment of the fusion weights of each observation in the optimal pose estimation.

[0098] Specifically, the adaptive fusion unit is further configured to perform the following fusion mode based on the real-time confidence level:

[0099] First fusion mode: When the confidence level of visual relocation is higher than the first threshold and the confidence level of optical flow velocity measurement is higher than the second threshold, the visual relocation, inertial navigation and optical flow velocity measurement observations are tightly coupled and fused.

[0100] Second fusion mode: When the visual repositioning confidence is lower than the first threshold but higher than the third threshold, the visual repositioning and inertial navigation observations are loosely coupled and fused, and the fusion weight of the optical flow velocity observations is independently adjusted according to the optical flow velocity confidence.

[0101] Third fusion mode: When the visual repositioning confidence is lower than the third threshold, the visual repositioning observation is disabled, and the inertial navigation and optical flow velocity measurement observations are fused.

[0102] The adaptive fusion unit executes one of three fusion modes based on real-time confidence. The trigger condition for the first fusion mode is a Boolean logic expression: if the visual repositioning confidence... First threshold And the confidence level of optical flow velocity measurement Second threshold If so, the first fusion mode will be executed. First threshold. Second threshold It is a preset scalar constant with a value range of (0,1).

[0103] In the first fusion mode, the adaptive fusion unit tightly couples and fuses the observations from visual relocalization, inertial navigation, and optical flow velocimetry. Tightly coupled fusion refers to directly constructing a residual function in the state estimator by combining the original observation data (such as feature point pixel coordinates and optical flow pixel displacements) with the IMU pre-integral quantities to obtain the optimal pose estimation with the highest accuracy.

[0104] The trigger condition for the second fusion mode is a Boolean logic expression: if the third threshold... Visual repositioning reliability ≤ First threshold Then the second fusion mode is executed. Third threshold. It is a preset scalar constant, and satisfies .

[0105] In the second fusion mode, the adaptive fusion unit loosely fuses visual relocalization and inertial navigation observations. Loosely coupled fusion means that the vision module independently calculates the pose observations first, and then uses these pose observations as input to a filter to fuse with the inertial navigation data.

[0106] Meanwhile, the adaptive fusion unit adjusts the optical flow velocimetry confidence level. Independently adjust the fusion weights of optical flow velocimetry observations, if If the value is higher, the optical flow velocity constraint is retained; if A lower value will reduce its weighting coefficient in the cost function.

[0107] The trigger condition for the third fusion mode is a Boolean logic expression: if the visual relocation confidence level... ≤Third threshold Then the third fusion mode will be executed.

[0108] In the third fusion mode, the adaptive fusion unit disables visual relocation observations, meaning that visual relocation-related residual terms or observation update steps are removed from the fusion algorithm. The system only fuses inertial navigation and optical flow velocimetry observations. This mode is suitable for situations where visual features are extremely scarce or lighting conditions are poor, resulting in low visual relocation reliability. In extremely low-level scenarios, short-term navigation capability is maintained by relying on the high-frequency characteristics of inertial navigation and the relative motion constraints of optical flow velocimetry.

[0109] The reliability quantification assessment module is configured to calculate and output a quantitative index characterizing the current navigation reliability in real time based on the feature distribution status of the current positioning area in the preset sparse feature map, the historical trajectory coverage of the mobile platform, and the stability of the currently matched feature points.

[0110] The reliability quantification assessment module is configured to calculate and output a quantitative index characterizing the current navigation reliability in real time. This quantitative index is defined as the Navigation Map Quality Index (NQI) in this paper. The calculation of the NQI is based on three dimensions of input data: first, the feature distribution state of the current positioning area in a preset sparse feature map; second, the historical trajectory coverage of the mobile platform; and third, the stability of the currently matched feature points. The feature distribution state reflects the spatial uniformity of environmental features; the historical trajectory coverage reflects the sufficiency of the system's exploration of the current area; and the stability of the currently matched feature points reflects the long-term reliability of feature matching. By integrating these three dimensions, the reliability quantification assessment module outputs the NQI with a value ranging from [0,1]. The closer the value is to 1, the higher the navigation reliability.

[0111] Specifically, the reliability quantification assessment module is configured as follows:

[0112] The analysis region is defined with the current optimal pose estimate as the center.

[0113] Calculate the feature distribution balance of the matched map feature points within the analysis area across different sub-regions.

[0114] The degree to which the mobile platform's trajectory covers the analysis area during a preset time period prior to the current moment is calculated to obtain the trajectory coverage.

[0115] Calculate the feature stability index of the map feature points matched at the current time based on their historical matching success rate and the freshness of the matching time.

[0116] Based on preset weights, the feature distribution balance, trajectory coverage, and feature stability index are weighted and summed to generate the quantitative index.

[0117] The reliability quantification assessment module estimates the current optimal pose. Horizontal position coordinates Define the analysis area centered on the data. Current optimal pose estimation This is the real-time pose data output by the preceding multi-source sensor fusion positioning module. Analysis Area It is a A square or circular region with its geometric center, its dimensions This is a preset value, usually in meters (m). This dimension... The settings are based on the effective sensing range of sensors on typical mobile platforms (such as drones or robots) to ensure the analysis area. It can cover the main environmental features that the sensor can observe at the current moment.

[0118] Feature distribution uniformity (FDE) is used for quantitative analysis of regions. The uniformity of the distribution of matched map feature points across different sub-regions. If map feature points are clustered on one side, the feature distribution uniformity (FDE) is low, indicating that the localization solution is easily affected by local errors; if map feature points are evenly distributed across all sub-regions, the feature distribution uniformity (FDE) is high, and the localization geometry is more robust. The FDE value ranges from [0,1].

[0119] Trajectory Coverage TCD is used to quantify the mobile platform at the current moment. Previous preset time period Within, its trajectory affects the analysis area. Coverage extent. Preset time period. It is a scalar constant with the unit of seconds (s). Its value is based on the effective historical trajectory duration that the system needs to remember in a typical task. It is usually set to tens of seconds to several minutes to balance the computational load and the effectiveness of historical information.

[0120] A higher TCD (Trajectory Coverage Degree) indicates that the system has a more comprehensive grasp of the map information for that area, resulting in a higher relocation success rate. The TCD value ranges from [0,1].

[0121] The Feature Stability Index (FSI) is calculated based on two attributes of the map feature points matched at the current time: one is the historical matching success rate. The first is the probability that the feature point has been successfully matched in past observations; the second is the freshness of the matching time. The Feature Stability Index (FSI) refers to the proximity of the most recent successful match of a feature point to the current time. The FSI aims to eliminate mismatched features that occasionally match but are unstable in the long term, retaining highly reliable feature points. The FSI value ranges from [0,1].

[0122] The reliability quantification assessment module, based on preset weights, performs a weighted summation of Feature Distribution Equalization (FDE), Trajectory Coverage (TCD), and Feature Stability Index (FSI) to generate the Navigation Map Quality Index (NQI). The calculation formula is as follows: .

[0123] in, , , These are the preset weights corresponding to Feature Distribution Equalization (FDE), Trajectory Coverage (TCD), and Feature Stability Index (FSI), respectively, and they satisfy the constraints. , The specific values ​​of the preset weights are set by the system designers based on the degree of dependence of each indicator on the application scenario.

[0124] Specifically, the feature distribution balance is obtained by dividing the analysis area into a preset number of grids, counting the number of matching feature points falling into each grid, and calculating their information entropy and then normalizing it.

[0125] The trajectory coverage is calculated based on marking the coverage of the grid in the analysis area, applying a time decay weight to the coverage status of each grid, and then calculating the proportion of effectively covered grids.

[0126] The feature stability index is obtained by weighting the matching success rate of each currently matched feature point within its historical observation window and combining it with the time decay factor from the current time.

[0127] First, the reliability quantification assessment module will analyze the region. Divide into a preset number of grids, denoted as Grids, of which For the number of rows, Let be the column numbers, all of which are positive integers.

[0128] Secondly, the statistics fall into each grid. (in The number of matching feature points within a range is denoted as . Next, the probability distribution for each grid cell is calculated. Then, calculate the information entropy. The formula is: ,in To prevent small constants from causing singularities in logarithmic operations, the value is taken as... Finally, normalization is performed to obtain the feature distribution balance. : The uniformity of the normalized feature distribution The value range is [0,1]. The closer the value is to 1, the more uniform the distribution of feature points.

[0129] The system maintains a region of analysis. The corresponding grid coverage state diagram. When the movement trajectory of the mobile platform passes through a certain grid... At that time, mark the grid as covered and update its time decay weights. The maximum value is 1.0. For other covered but not revisited grids, a time decay weight is applied.

[0130] Time decay weight The calculation formula is: ,in For the current moment, The last time the grid was covered. This is the time decay factor, in units of Its value is determined by the effective retention time of trajectory information in typical tasks, and is usually set to a positive value that keeps the half-life in the range of tens of seconds. If the grid is never covered, then The trajectory coverage TCD is calculated as the sum of the effective coverage weights of all grids and the sum of the maximum possible weights (i.e., the total number of grids). The ratio of ) .

[0131] For each currently matched feature point The system maintains a historical observation window, the size of which is denoted as . , indicating the past This is one observation opportunity. The system records the number of successful matches for this feature point within the historical observation window and calculates the recent matching success rate. : .

[0132] At the same time, a time decay factor is introduced. : ,in This is the time interval since the last successful match of this feature point. The characteristic stability time decay coefficient, in units of Its value is determined based on the time scale at which the feature point remains stable in a typical environment, and is usually set to a positive value that makes the half-life range from several seconds to tens of seconds. This feature point stability score The calculation is as follows: The Feature Stability Index (FSI) is the average stability score of all currently matched feature points. ,in This represents the total number of feature points matched at the current time.

[0133] The decision control module is configured to generate and execute corresponding navigation control strategies based on the quantitative indicators.

[0134] The decision control module is configured to receive a quantitative indicator, namely the Navigation Map Quality Index (NQI) output by the preceding reliability quantification assessment module. Based on the NQI value, the decision control module generates and executes a corresponding navigation control strategy. The navigation control strategy is a set of control instructions used to adjust the motion state and task behavior of the mobile platform to ensure system safety in the event of changes in navigation reliability.

[0135] Specifically, the decision control module is configured as follows:

[0136] The quantitative indicators are compared with multiple preset reliability level thresholds to determine the reliability level of the current navigation.

[0137] Based on the reliability level, at least one of the control parameters of the mobile platform, including the maximum permissible flight speed, pose update frequency, and mission execution mode, is dynamically adjusted.

[0138] The decision control module compares the Navigation Map Quality Index (NQI) with multiple preset reliability level thresholds. Let the preset reliability level threshold set be... ,in And all are scalar constants whose values ​​are within the range (0,1). The comparison logic is as follows: If Then the reliability level of the current navigation is determined. Level 1 (highest reliability); if Then determine the reliability level. Level 2; and so on, if Then determine the reliability level. for Level (lowest reliability). Reliability level. It is a discrete state variable used to characterize the reliability of the current navigation system.

[0139] The decision control module determines the reliability level. The control parameters of the mobile platform are dynamically adjusted. The adjustment targets include at least one of the following: (i) the maximum permissible flight speed. The unit is meters per second (m / s); the second is the pose update frequency. The unit is Hertz (Hz); thirdly, the task execution mode. , which is an enumerated variable. The adjustment logic is a mapping relationship: if the reliability level If the value is high, then set a higher maximum permissible flight speed. Normal pose update frequency and full-featured task execution mode If the reliability level If the speed is lower, the maximum permissible flight speed will be reduced. Increase pose update frequency To improve the accuracy of state estimation, or to switch to a conservative task execution mode. (e.g., suspending complex operations).

[0140] Specifically, the decision control module is further configured as follows:

[0141] When the quantitative indicator falls below the safety threshold, an early warning signal is triggered, and a safety strategy is generated to guide the mobile platform to return along a historical high-reliability trajectory or hover in place.

[0142] The decision control module monitors the navigation map quality index (NQI) and safety thresholds. The relationship. Safety threshold. It is a preset scalar constant, with a value range of (0,1), and is usually set to the minimum value or lower of the reliability level threshold. The judgment logic is: if Then the decision control module performs the following operations: First, it triggers an early warning signal, which is an audible and visual alarm or a remote communication alarm; Second, it generates a security policy.

[0143] The safety strategy includes two options: Option A guides the mobile platform to return along a historical high-reliability trajectory, which refers to a sequence of path points recorded by the system in past operations where the Navigation Map Quality Index (NQI) was higher than the higher threshold of the preset reliability level thresholds; Option B commands the mobile platform to hover in place, i.e., maintain its current position and attitude. The decision control module selects one of Option A and Option B to execute based on constraints such as remaining battery power and distance from the take-off and landing points.

[0144] Furthermore, the decision control module is configured to plan an optimal flight path based on a pre-set sparse feature map and a real-time updated navigation map quality index during the mission planning phase or flight. The optimal flight path prioritizes areas with high feature distribution balance, sufficient historical trajectory coverage, and high feature stability to reduce the risk of positioning failure due to a lack of environmental features during flight. Specifically, the system can use the navigation map quality index as a weight in the cost map and generate a path that traverses high-reliability areas as much as possible using a path search algorithm (such as the A* algorithm or Dijkstra's algorithm), thereby improving overall flight safety.

[0145] Specifically, a fusion navigation and reliability quantification evaluation system based on a pre-set sparse map further includes an anomaly handling module, which is configured as follows:

[0146] Monitor the data streams from each sensor and the confidence level of each information source;

[0147] When the confidence level of any information source remains below the failure threshold for a consecutive preset number of frames, the information source is determined to be faulty, and the multi-source sensor fusion positioning module is notified to adjust the fusion strategy.

[0148] When the visual relocation source fails and cannot be recovered, a degraded operation state is triggered. In this state, the system performs pose estimation based on inertial navigation data or a combination of inertial navigation and optical flow velocity measurement data, and estimates pose uncertainty in real time according to a preset error growth model. When the uncertainty exceeds a preset safety threshold, the decision control module is triggered to execute a safety strategy.

[0149] The anomaly handling module is configured to monitor the data streams from each sensor and the confidence levels of each information source in real time. The data streams from each sensor include raw acceleration and angular velocity data from the inertial measurement unit, image frame data from the camera, etc. The confidence levels of each information source include the visual repositioning confidence level defined above. Inertial navigation confidence and confidence level of optical flow velocimetry The exception handling module obtains the above data through polling or interruption, which serves as the input basis for subsequent fault determination.

[0150] The exception handling module performs the following Boolean logic judgment: If any information source exists, its corresponding confidence level is within a preset number of consecutive frames. The internal temperature remains below the failure threshold. If the information source fails to respond, it is determined to be invalid. (Preset number of consecutive frames) It is a positive integer scalar used to filter out transient noise interference and ensure the stability of failure determination; failure threshold. It is a scalar constant ranging from [0,1], representing the lowest acceptable confidence level. The decision logic expression is: If ,but ,in The confidence level of a certain information source. This serves as the index for the current frame. Once a failure is determined, the anomaly handling module immediately generates a failure notification signal and sends it to the multi-source sensor fusion localization module. Upon receiving the failure notification signal, the multi-source sensor fusion localization module adjusts its fusion strategy. For example, in the Kalman filter, it sets the observation noise covariance matrix of the failure information source to infinity, or removes the corresponding residual term from the factor graph, thereby eliminating the influence of the failure information source in the optimal pose estimation.

[0151] The anomaly handling module monitors the status of the visual repositioning source. If the visual repositioning source is determined to be faulty and its confidence level does not recover to a valid range within the preset recovery observation window, it is considered unrecoverable, and the anomaly handling module triggers a degraded operation state. In the degraded operation state, the system stops using visual repositioning data and instead performs pose estimation based solely on inertial navigation data, or based on a combination of inertial navigation data and optical flow velocity measurement data.

[0152] Pose estimation refers to the process of deriving the current position and attitude using kinematic integrals or relative odometry. Simultaneously, the system uses a pre-defined error growth model. Real-time estimation of pose uncertainty Pre-defined error growth model It is a mathematical function that describes the increase of pose error with time or distance in the absence of absolute observation correction. Its typical expression is as follows: or ,in The initial error covariance when entering the degraded state. For model parameters, Duration of degraded operation. Pose uncertainty. The size of the error ellipsoid is usually represented by the trace or the largest eigenvalue of the error covariance matrix.

[0153] The exception handling module continuously compares pose uncertainties. With the preset safety threshold The judgment logic is: if If the exception handling module triggers the decision control module to execute the security policy, the preset security threshold will be used. It is a scalar constant set based on the maximum allowable positioning error for the mission. Safety strategies include operations such as returning along historical high-reliability trajectories or hovering in place, as defined above, to prevent collisions or mission failures due to excessive positioning errors.

[0154] Specifically, for GNSS sources, the anomaly handling module also executes the above monitoring logic. When the GNSS confidence level remains below the failure threshold and visual relocation is available, the system determines that the GNSS is faulty or interfered with, notifies the multi-source sensor fusion positioning module to remove GNSS observation information, and switches to a Non-GPS navigation mode based on a sparse map. GNSS observation is reactivated once the GNSS confidence level returns to normal.

[0155] This embodiment provides a specific implementation of a fusion navigation and reliability quantification assessment system based on a pre-built sparse map, applied to the navigation task of an autonomous mobile robot (AMR) in an indoor warehousing and logistics scenario. The AMR is equipped with an inertial measurement unit (IMU), a monocular camera, and an embedded computing platform. The embedded computing platform uses an NVIDIA Jetson AGX Orin processor and runs a software system based on the ROS 2 framework. This software system instantiates a multi-source sensor fusion localization module, a reliability quantification assessment module, a decision control module, and an anomaly handling module.

[0156] 1. System initialization and data acquisition.

[0157] Upon system startup, a pre-built sparse feature map is loaded, containing the 3D coordinates and descriptors of key corner points and texture features within the warehouse area. The multi-source sensor fusion localization module acquires acceleration and angular velocity data from the inertial measurement unit and image frame data from the monocular camera in real time. Based on the pre-built sparse feature map, the multi-source sensor fusion localization module executes visual relocalization, inertial navigation, and optical flow velocimetry algorithms respectively.

[0158] In this embodiment, visual relocation confidence The calculation parameters are set as follows: number of matching feature points The typical effective range is 50 to 200; matching accuracy The reprojection error threshold is set to 2.0 pixels (based on typical engineering experiments, at a camera resolution of 1920x1080, 2.0 pixels corresponds to an angular error of approximately 0.5 degrees); spatial distribution uniformity. The confidence level is obtained by dividing the image into a 4x4 grid and calculating the entropy. In the calculation, vibration intensity Temperature change was calculated from the variance of the high-frequency components of the accelerometer. The reference value is the deviation from the calibrated temperature of 25°C, and the time interval. The unit is seconds. Confidence level for optical flow velocimetry. In the calculation of texture richness Illumination stability based on the mean gradient magnitude of the Sobel operator Based on histogram correlation coefficient, optical flow vector consistency Based on RANSAC in-point ratio.

[0159] 2. Adaptive fusion and pose estimation.

[0160] The confidence calculation unit in the multi-source sensor fusion localization module independently calculates the visual repositioning confidence. Inertial navigation confidence and confidence level of optical flow velocimetry The adaptive fusion unit maps the aforementioned confidence level to a noise covariance matrix. The specific mapping formula is as follows: ,in For the corresponding real-time confidence level, This is the basic noise covariance matrix, with coefficient 5 representing a typical engineering experimental value, used to rapidly increase the noise weight when the real-time confidence level decreases.

[0161] The adaptive fusion unit performs fusion mode switching according to the following Boolean logic:

[0162] First fusion mode: If visual repositioning reliability is... >0.7 (first threshold) And the confidence level of optical flow velocimetry >0.6 (Second threshold) If the original data of the three observations are combined, then tightly coupled fusion is performed to jointly optimize the original data of the three observations.

[0163] Second fusion mode: If 0.4 (third threshold) Visual repositioning reliability If the value is less than or equal to 0.7, loosely coupled fusion is performed, fusing the pose calculated by visual relocalization as the observation with the inertial navigation, and then adjusting the confidence level based on optical flow velocimetry. Dynamically adjust the weighting coefficients of optical flow velocity constraints .

[0164] Third fusion mode: If visual repositioning reliability If the value is less than or equal to 0.4, visual relocation observations are disabled, and only inertial navigation and optical flow velocity data are fused.

[0165] The final output is the optimal pose estimate. , including location and attitude quaternions .

[0166] 3. Quantitative assessment of reliability.

[0167] The reliability quantification assessment module estimates the current optimal pose. A square analysis area with sides of 10 meters was defined centered on the central point. The radius is set based on the typical operating radius of a warehouse robot.

[0168] Feature distribution uniformity (FDE) calculation: The analysis area... Divided into The grid is used to count the number of matching feature points falling into each grid and calculate the probability distribution. Then, the information entropy is calculated: Normalization yields the characteristic distribution balance: .

[0169] Trajectory Coverage TCD Calculation: For the analysis area The grid is used for coverage marking. A preset time period is set. The time decay factor is 60 seconds (based on a typical task cycle). Set as (Corresponding to a half-life of approximately 14 seconds). Calculate the time decay weight for each grid cell. Finally, the trajectory coverage TCD is obtained as the ratio of the sum of effective coverage weights to the total number of grids, 25.

[0170] Feature Stability Index (FSI) calculation: The historical observation window size is set to 20 frames. The recent matching success rate for each matching feature point is calculated. Set the characteristic stability time decay coefficient. for Calculate the stability score for each feature point. The average value is used to obtain the characteristic stability index (FSI).

[0171] Finally, the reliability quantification assessment module evaluates reliability based on preset weights. (Based on typical engineering experimental values, the feature distribution has the greatest impact on positioning geometric accuracy), calculate the navigation map quality index: .

[0172] 4. Decision control and anomaly handling.

[0173] The decision control module receives the navigation map quality index (NQI).

[0174] Level Determination and Parameter Adjustment: Set the preset reliability level threshold as follows: .

[0175] NQI ≥ 0.8, reliability level Level 1, setting the maximum permissible flight speed. The pose update frequency is 2.0 m / s. 50Hz, task execution mode To operate at full speed.

[0176] If 0.5 ≤ NQI < 0.8, the reliability level is... Level 2, setting the maximum permissible flight speed. The pose update frequency is 1.0 m / s. 100Hz, task execution mode To ensure proper operation.

[0177] NQI < 0.5, reliability level Level 3, with a maximum permissible flight speed set. It is 0.5 m / s.

[0178] Security policy trigger: Set security threshold The value is 0.3. If NQI < 0.3, the decision control module triggers a warning signal (audio-visual alarm) and generates a safety strategy: guide the autonomous mobile robot (AMR) to return to the charging dock along a historical high-reliability trajectory (i.e., the path points recorded when NQI > 0.8 in the past), or perform local hovering if the battery level is below 20%.

[0179] The exception handling module runs in parallel, monitoring the confidence level of each information source. A preset number of consecutive frames is set. For 10 frames (approximately 0.33 seconds, assuming a frame rate of 30Hz), the failure threshold is... It is 0.1.

[0180] Failure determination: If the visual repositioning reliability is low... If the value is less than 0.1 for 10 consecutive frames, the anomaly handling module determines that the visual relocation source has failed and notifies the multi-source sensor fusion positioning module to set the observation noise covariance matrix of that source to infinity.

[0181] Degraded Operation and Error Estimation: If the visual relocation source fails and does not recover within 30 seconds, the anomaly handling module triggers a degraded operation state. In this state, the system performs pose estimation solely based on a combination of inertial navigation data and optical flow velocity measurement data.

[0182] Uncertainty monitoring: The system employs a pre-defined error growth model. : (unit: (The coefficient 0.05 represents a typical engineering experimental value, reflecting the error divergence rate without visual correction). Real-time estimation of pose uncertainty. (Trace the covariance matrix). Set a preset safety threshold. for (The standard deviation of the corresponding position is approximately 1 meter). If The exception handling module immediately triggers the decision control module to execute the safety policy, forcing the autonomous mobile robot (AMR) to stop moving and requesting human intervention.

[0183] Through the above specific implementation methods, this embodiment demonstrates how the system can achieve highly reliable autonomous navigation in complex warehousing environments by using adaptive fusion of multi-source sensors, real-time reliability quantification assessment, and hierarchical decision-making and anomaly handling mechanisms, thereby avoiding positioning loss and safety accidents caused by sensor failure or insufficient environmental features.

[0184] Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A fusion navigation and reliability quantification evaluation system based on a pre-set sparse map, characterized in that, include: The map loading module is configured to load a preset sparse feature map of the target area, wherein the preset sparse feature map contains multiple feature points with global coordinates and their corresponding feature descriptors; The multi-source sensor fusion positioning module is configured to acquire multiple sensor data of the mobile platform in real time, and obtain pose observation information through at least two of the following methods: visual relocalization, inertial navigation and optical flow velocimetry, based on the preset sparse feature map. Then, according to the real-time confidence of each information source, the pose observation information is adaptively weighted and fused to output the optimal pose estimate. The reliability quantification assessment module is configured to calculate and output a quantitative index characterizing the current navigation reliability in real time based on the feature distribution status of the current positioning area in the preset sparse feature map, the historical trajectory coverage of the mobile platform, and the stability of the currently matched feature points. The decision control module is configured to generate and execute corresponding navigation control strategies based on the quantitative indicators.

2. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 1, characterized in that, The specific configuration of the reliability quantification assessment module is as follows: The analysis region is defined with the current optimal pose estimate as the center; Calculate the feature distribution balance of the matched map feature points within the analysis area across different sub-regions; The degree to which the movement trajectory of the mobile platform covers the analysis area during a preset time period prior to the current moment is calculated to obtain the trajectory coverage. Calculate the feature stability index of the map feature points matched at the current time based on their historical matching success rate and the freshness of the matching time. Based on preset weights, the feature distribution balance, trajectory coverage, and feature stability index are weighted and summed to generate the quantitative index.

3. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 2, characterized in that: The feature distribution balance is obtained by dividing the analysis area into a preset number of grids, counting the number of matching feature points falling into each grid, and calculating their information entropy and then normalizing it. The trajectory coverage is calculated based on marking the coverage of the grid in the analysis area, applying a time decay weight to the coverage status of each grid, and then calculating the proportion of effectively covered grids. The feature stability index is obtained by weighting the matching success rate of each currently matched feature point within its historical observation window and combining it with the time decay factor from the current time.

4. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 1, characterized in that, The multi-source sensor fusion positioning module includes a confidence calculation unit, which is configured to independently and in real time calculate the confidence of each information source. The visual repositioning confidence is calculated based on the number of feature points matched between the current frame and the preset sparse feature map, the matching accuracy, and the uniformity of the spatial distribution of the matching points on the image plane. The inertial navigation confidence level is calculated based on the vibration intensity, temperature change, and time interval of the inertial measurement unit and the last visual reset. The confidence level of optical flow velocimetry is calculated based on the texture richness, illumination stability, and consistency of the optical flow vector in the optical flow image.

5. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 4, characterized in that, The multi-source sensor fusion localization module further includes an adaptive fusion unit, which is configured to map the real-time confidence of each information source to the noise covariance matrix or information matrix of the corresponding observation, and execute a filtering algorithm or graph optimization algorithm based on the updated noise covariance matrix or information matrix to dynamically adjust the fusion weight of each observation in the optimal pose estimation.

6. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 5, characterized in that, The adaptive fusion unit is also configured to perform the following fusion mode based on the real-time confidence level: First fusion mode: When the confidence level of visual relocation is higher than the first threshold and the confidence level of optical flow velocity measurement is higher than the second threshold, the visual relocation, inertial navigation and optical flow velocity measurement observations are tightly coupled and fused. Second fusion mode: When the visual repositioning confidence is lower than the first threshold but higher than the third threshold, the visual repositioning and inertial navigation observations are loosely coupled and fused, and the fusion weight of the optical flow velocity observations is independently adjusted according to the optical flow velocity confidence. Third fusion mode: When the visual repositioning confidence is lower than the third threshold, the visual repositioning observation is disabled, and the inertial navigation and optical flow velocity measurement observations are fused.

7. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 1, characterized in that, The decision control module is configured as follows: The quantitative indicators are compared with multiple preset reliability level thresholds to determine the reliability level of the current navigation. Based on the reliability level, at least one of the control parameters of the mobile platform, including the maximum permissible flight speed, pose update frequency, and mission execution mode, is dynamically adjusted.

8. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 7, characterized in that, The decision control module is also configured to: When the quantitative indicator falls below the safety threshold, an early warning signal is triggered, and a safety strategy is generated to guide the mobile platform to return along a historical high-reliability trajectory or hover in place.

9. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 1, characterized in that, The pre-built sparse feature map is constructed in advance in the following way: A mobile platform equipped with positioning sensors and feature acquisition sensors is used to collect image sequences of the target area and their corresponding positioning data along a preset path. Extract feature points with descriptors from the image sequence; Based on the positioning data, calculate the three-dimensional coordinates of each feature point in the global coordinate system; The three-dimensional coordinates of the feature points and their descriptors are associated and stored to form the preset sparse feature map.

10. The fusion navigation and reliability quantification evaluation system based on a pre-set sparse map according to claim 1, characterized in that, It also includes an exception handling module, which is configured as follows: Monitor the data streams from each sensor and the confidence level of each information source; When the confidence level of any information source is continuously lower than the failure threshold within a preset number of consecutive frames, the information source is determined to be faulty, and the multi-source sensor fusion positioning module is notified to adjust the fusion strategy. When the visual relocation source fails and cannot be recovered, a degraded operation state is triggered. In this state, the system performs pose estimation based on inertial navigation data or a combination of inertial navigation and optical flow velocity measurement data, and estimates pose uncertainty in real time according to a preset error growth model. When the uncertainty exceeds a preset safety threshold, the decision control module is triggered to execute a safety strategy.

Images