A surveying and mapping navigation positioning correction method of a multi-sensor fusion algorithm

Abstract

This application discloses a mapping navigation positioning correction method, system, computer device, and storage medium based on a multi-sensor fusion algorithm. The method includes: acquiring satellite navigation data, inertial measurement data, and visual image data; preprocessing and aligning the data in a spatiotemporal manner; inputting the aligned satellite navigation and inertial measurement data into a first sub-filter of a federated Kalman filter for estimation; constructing a visual inertial odometry using the aligned visual image and inertial measurement data and inputting it into a second sub-filter for estimation; fusing the estimation results of the two sub-filters in a main filter to obtain a preliminary positioning result; registering the preliminary positioning result with a local environmental map to obtain a relative pose, and using this as an observation to perform closed-loop correction on the main filter; constructing and weighting the corrected pose based on the output of a fuzzy logic system; and outputting the final positioning result. This application improves the positioning accuracy and robustness in complex mapping environments.

Description

Technical Field

[0001] This application relates to the field of surveying and navigation technology, specifically a surveying and navigation positioning correction method, system, computer equipment, and storage medium based on a multi-sensor fusion algorithm. Background Technology

[0002] With the development of surveying and navigation technology, single sensors are no longer sufficient to meet the high-precision positioning requirements in complex environments, and multi-sensor fusion technology has become the mainstream solution. Existing multi-sensor fusion methods typically employ loose or tight combinations to fuse data from sensors such as global navigation satellite systems, inertial measurement units, and visual cameras to improve positioning accuracy and robustness.

[0003] However, in complex mapping environments (such as urban canyons, forest cover, and indoor-outdoor transition zones), multi-source sensor data suffers from spatiotemporal asynchrony, and each sensor is susceptible to environmental interference, leading to fluctuations in data quality. Traditional fusion methods are unable to effectively suppress the continuous growth of accumulated errors, resulting in significant drift in positioning results after long-term operation and a severe decrease in positioning accuracy. Summary of the Invention

[0004] Based on this, it is necessary to provide a mapping, navigation, and positioning correction method, system, computer equipment, and storage medium that can achieve spatiotemporal synchronization and alignment and suppress cumulative drift errors by using a multi-sensor fusion algorithm to address the aforementioned technical problems.

[0005] Firstly, a mapping, navigation, and positioning correction method based on a multi-sensor fusion algorithm is provided, the method comprising: Acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data from the visual camera; The satellite navigation data, the inertial measurement data, and the visual image data are subjected to gross error detection and outlier removal preprocessing, and the preprocessed data are spatiotemporally synchronized and aligned. The spatiotemporally synchronized satellite navigation data and inertial measurement data are input into the first sub-filter of the federated Kalman filter for estimation; the spatiotemporally synchronized visual image data and inertial measurement data are tightly coupled to construct a visual inertial odometry, and the pose information of the visual inertial odometry is input into the second sub-filter of the federated Kalman filter for estimation; the estimation results of the first sub-filter and the second sub-filter are fused in the main filter of the federated Kalman filter to obtain the primary positioning result; A local environmental map is constructed based on the visual image data and the inertial measurement data. The initial positioning result is registered with the local environmental map to obtain the relative pose obtained by registration as the registration result. The registration result is used as the observation value to perform closed-loop correction on the main filter to obtain the corrected pose. A fuzzy logic system is constructed, whose input features include data quality parameters of each sensor, and outputs the confidence level of each sensor and the information allocation factor of the main filter; the corrected pose is adjusted by weighted averaging based on the confidence level and the information allocation factor, and the final positioning result is output.

[0006] In one embodiment, the step of constructing a local environmental map based on the visual image data and the inertial measurement data, and registering the primary positioning result with the local environmental map, includes: Construct keyframes based on tight coupling between vision and inertial, each keyframe including 3D map points and pre-integration constraints of inertial measurement units, wherein the 3D map points acquire real-scale information through binocular vision or RGB-D cameras; The keyframes are managed using a sliding window, wherein the size of the sliding window is adjusted according to the texture richness of the visual image data, and the managed keyframes constitute the local environmental map. The initial localization result is visually relocalized and matched with the keyframes in the local environment map to obtain the matching inlier rate, and the relative pose of the current image frame relative to the keyframes in the local environment map is calculated. The relative pose and the matched inlier rate are output as the registration result.

[0007] In one embodiment, the method further includes: When satellite navigation data cannot be obtained and the matching in-point rate is lower than the preset matching threshold, dead reckoning is performed using the inertial measurement unit. During the dead reckoning, continuous attempts are made to reacquire the satellite navigation data and the visual image data, and the reacquired visual image data is re-matched with the keyframes in the local environmental map for visual relocation. When the in-point matching rate obtained by the satellite navigation data recovery acquisition or the visual repositioning matching is higher than the preset matching threshold, the recovered satellite navigation data is fused with the current dead reckoning result.

[0008] In one embodiment, fusing the recovered satellite navigation data with the current dead reckoning result includes: A transition window duration is set. During the transition window duration, the weight of the recovered satellite navigation data is linearly increased from 0 to 1, while the weight of the current dead reckoning result is linearly decreased from 1 to 0.

[0009] In one embodiment, fusing the estimation results of the first sub-filter and the second sub-filter in the main filter includes: Health monitoring is performed on each sub-filter, and the health status is dynamically calculated based on visual feature tracking rate, number of satellite navigation satellites, and position accuracy strength. When the health of any sub-filter is not lower than a preset health threshold, the sub-filter is marked as available; The fusion weights of the sub-filters marked as usable are calculated based on the estimated covariance of each sub-filter. In the main filter, the estimation results of the sub-filters that are marked as available are weighted and fused according to their corresponding fusion weights.

[0010] In one embodiment, the output confidence levels of each sensor and the information allocation factor of the main filter include: The fuzzy logic system uses slow-layer adaptation to select the corresponding parameter preset library based on the output of the environment classifier. The environment classifier determines the environment category in real time based on visual semantic segmentation and the dynamic characteristics of the inertial measurement unit. The parameter preset library includes the initial information allocation factor and sliding window size of the main filter. The output of the slow-layer adaptation is smoothly transitioned through a low-pass filter to obtain the initial information allocation factor of the main filter. The fuzzy logic system, through mid-level adaptive adjustment, dynamically adjusts the initial information allocation factor based on the estimated covariance and residual statistics of the first and second sub-filters, thereby obtaining and outputting the information allocation factor of the main filter; The fuzzy logic system, through low-level adaptive adjustment, dynamically adjusts the observation noise covariance of the second sub-filter based on the registration residual generated during the registration process between the primary positioning result and the local environmental map. It then updates the confidence level of the visual sensor based on the adjusted observation noise covariance, updates the confidence level of the satellite navigation sensor based on the estimated covariance of the first sub-filter, updates the confidence level of the inertial measurement unit based on the zero bias rate of change of the inertial measurement unit, and outputs the confidence level of each sensor.

[0011] In one embodiment, the spatiotemporal synchronization alignment of the preprocessed data includes: The data are time-aligned by a combination of hard triggering and inertial measurement unit pre-integration. The asynchronous data are then fused by a weighted fusion method to obtain the inertial measurement unit pre-integration projection result and the linear interpolation compensation result, and a hysteresis comparator is used to control the weight coefficient of the weighted fusion method.

[0012] Secondly, a mapping, navigation, and positioning correction system based on a multi-sensor fusion algorithm is provided, the system comprising: The data acquisition module is used to acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data collected by the visual camera. The data processing module is used to perform gross error detection and outlier removal preprocessing on the satellite navigation data, the inertial measurement data, and the visual image data, and to perform spatiotemporal synchronization alignment on the preprocessed data. The collaborative filtering module includes a first sub-filter, a second sub-filter, and a main filter. The first sub-filter is used to estimate the spatiotemporally synchronized satellite navigation data and inertial measurement data. The second sub-filter is used to tightly couple the spatiotemporally synchronized visual image data and inertial measurement data to construct a visual inertial odometry (VIO), and to estimate the pose information of the VIO. The main filter is used to fuse the estimation results of the first sub-filter and the second sub-filter to obtain a primary positioning result. The pose registration module is used to construct a local environmental map based on the visual image data and the inertial measurement data, register the primary positioning result with the local environmental map, and obtain the relative pose obtained by registration as the registration result. The closed-loop correction module is used to perform closed-loop correction on the main filter using the registration result as the observation value to obtain the corrected pose. The fuzzy logic system module has input features including data quality parameters of each sensor and outputs the confidence level of each sensor and the information allocation factor of the main filter. The result acquisition module is used to adjust the weights of the corrected pose using a weighted average method based on the confidence level and the information allocation factor, and output the final localization result.

[0013] Thirdly, a computer device is provided, including a memory and a processor, wherein the memory is communicatively connected to the processor, and the memory stores a computer program that can run on the processor, wherein when the processor executes the computer program, it implements the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm described above.

[0014] Fourthly, a computer-readable storage medium is provided, on which a computer program is stored, wherein when the computer program is executed by a processor, it implements the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm described above.

[0015] The aforementioned multi-sensor fusion algorithm for mapping, navigation, and positioning correction, including its method, system, computer equipment, and storage medium, avoids state jumps during asynchronous data fusion by combining hard triggering with pre-integration of the inertial measurement unit (IMU) for spatiotemporal synchronization alignment, ensuring the consistency and continuity of data input. It achieves tightly coupled estimation of multi-source heterogeneous data through a federated Kalman filter architecture for hierarchical estimation and fusion of data from the global navigation satellite system, IMU, and visual image data, improving fusion accuracy. By constructing a local environmental map and registering the initial positioning results with it, the relative pose obtained from the registration is used as an observation value for closed-loop correction of the main filter, suppressing the system's accumulated drift error. Furthermore, by constructing a fuzzy logic system, inputting sensor data quality parameters, and outputting the confidence levels of each sensor and the main filter information allocation factor, it achieves dynamic adaptive adjustment of fusion weights, thus maintaining stable positioning accuracy under complex environmental changes and ultimately outputting high-precision, highly robust positioning results. Attached Figure Description

[0016] Figure 1 This is an application environment diagram of a mapping, navigation, and positioning correction method based on a multi-sensor fusion algorithm in one embodiment. Figure 2 This is a flowchart illustrating a mapping, navigation, and positioning correction method based on a multi-sensor fusion algorithm in one embodiment. Figure 3 This is a flowchart illustrating the spatiotemporal synchronization alignment steps in one embodiment; Figure 4 This is a flowchart illustrating the steps of constructing a local environmental map in one embodiment; Figure 5 This is a flowchart illustrating the estimation result fusion step in one embodiment; Figure 6 This is a flowchart illustrating the steps for outputting confidence and information allocation factors in one embodiment; Figure 7 This is a block diagram of a mapping, navigation, and positioning correction system based on a multi-sensor fusion algorithm in one embodiment. Figure 8 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation

[0017] To facilitate understanding of the technical solutions provided in the embodiments of this application, the background technology involved in the embodiments of this application will be described below.

[0018] In the field of surveying and navigation, single sensors (such as Global Navigation Satellite System GNSS) are prone to signal loss or accuracy degradation in complex environments (such as urban canyons or forest cover), while inertial measurement units (IMUs) suffer from error accumulation over time, and visual cameras are significantly affected by changes in lighting and texture. Therefore, adopting multi-sensor fusion technology to complement each other's advantages has become the mainstream approach to improve positioning accuracy and robustness.

[0019] However, existing fusion methods still have the following core shortcomings when dealing with complex surveying and mapping environments: 1. Data Spatiotemporal Asynchrony Issue: Heterogeneous sensors such as GNSS, IMU, and visual cameras have differences in data sampling frequency, time reference, and spatial coordinate system. Traditional time alignment methods (such as linear interpolation) are prone to introducing state jumps when the sensor data rate changes or is asynchronous, resulting in jitter or even divergence in the pose estimation of the fusion, affecting the stability of the system.

[0020] 2. Difficulty in effectively suppressing accumulated errors: In scenarios where GNSS signals are long-term limited (such as in indoor-outdoor transition zones) or where visual features are lacking, relying solely on loosely combined or traditionally tightly combined fusion frameworks (such as a single Kalman filter) is insufficient to effectively correct the continuous drift generated by the system. Even with the introduction of visual inertial odometry, errors will still accumulate over long periods of operation. The lack of an active, closed-loop correction mechanism leads to significant drift in the final positioning results during long-endurance or large-scale operations, failing to meet the requirements of high-precision surveying and navigation.

[0021] 3. Lack of adaptability to data quality fluctuations: In complex environments, the data quality of each sensor (such as the number of GNSS satellites, visual feature tracking rate, and IMU zero bias stability) changes dynamically. Existing methods mostly use fixed fusion weights or rely on empirical threshold switching, which cannot dynamically adjust the information allocation factors and observation noise according to the real-time confidence of each sensor. It is difficult to maintain the optimality of the fusion strategy in the whole scene, and the positioning results are easily affected by low-quality data.

[0022] To address the aforementioned technical problems, embodiments of this application provide a mapping, navigation, and positioning correction method, system, computer equipment, and storage medium based on a multi-sensor fusion algorithm.

[0023] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. It should be noted that the collection of satellite navigation data, inertial measurement data, and visual image data involved in the embodiments of this application is conducted under the premise of obtaining authorization from relevant users or complying with relevant laws and regulations, and the scope of data collection and usage methods have been explicitly authorized.

[0024] The mapping, navigation, and positioning correction method based on multi-sensor fusion algorithms provided in this application can be applied to, for example... Figure 1 In the application environment shown, the mobile mapping terminal 102 communicates with the server 104 via a network. The mobile mapping terminal 102 integrates a Global Navigation Satellite System receiver, an inertial measurement unit, and a visual camera to acquire satellite navigation data, inertial measurement data, and visual image data. It then performs corresponding data preprocessing, spatiotemporal synchronization alignment, federated Kalman filtering, local map construction and loop closure correction, and fuzzy logic system adaptive adjustment, ultimately outputting high-precision mapping, navigation, and positioning results. The server 104 can be a standalone server or a server cluster composed of multiple servers. It receives data, map information, or positioning results uploaded by the mobile mapping terminal 102 and can perform some backend optimization or map update tasks. The mobile mapping terminal 102 can be, but is not limited to, a mapping drone, robot, handheld mapping device, vehicle navigation terminal, smartphone, or portable wearable device equipped with the aforementioned sensors.

[0025] Firstly, in the process of multi-sensor fusion navigation and positioning, different sensors (such as global navigation satellite systems, inertial measurement units, and visual cameras) have different sampling frequencies and clock sources, making it difficult to strictly align the data in time. Especially when the vehicle is in high dynamic motion, the direct fusion of asynchronous data can introduce pose jumps, disrupting the continuity of the estimation. Therefore, in one embodiment, a mapping navigation and positioning correction method based on a multi-sensor fusion algorithm is provided, which is applied to... Figure 1 The mobile mapping terminal (i.e., the carrier mentioned in the text) will be used as an example for explanation. Figure 2 As shown, the method includes the following steps: Step S1: Acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data acquired by the visual camera.

[0026] In this step, the Global Navigation Satellite System (GNSS) receiver receives satellite signals and outputs satellite navigation data, including but not limited to longitude, latitude, altitude, velocity, time, and quality parameters such as the number of satellites and position accuracy. The Inertial Measurement Unit (IMU) measures the vehicle's three-axis angular velocity and three-axis acceleration, outputting inertial measurement data. A visual camera (which can be monocular, binocular, or RGB-D) acquires visual image data of the surrounding environment.

[0027] Step S2: Perform gross error detection and outlier removal preprocessing on satellite navigation data, inertial measurement data, and visual image data, and perform spatiotemporal synchronization alignment on the preprocessed data.

[0028] In this step, gross error detection and outlier removal can employ existing statistical testing methods, such as detection methods based on the absolute deviation of the median, to remove outliers caused by momentary sensor malfunctions or external interference, thus ensuring the quality of subsequent data fusion. The specific implementation of spatiotemporal synchronization alignment can be achieved using any method in the existing technology, such as an alignment method based on a combination of hard triggering and inertial measurement unit pre-integration; this embodiment does not limit this approach.

[0029] Step S3: Input the spatiotemporally synchronized satellite navigation data and inertial measurement data into the first sub-filter of the federated Kalman filter for estimation; tightly couple the spatiotemporally synchronized visual image data and inertial measurement data to construct a visual inertial odometry, and input the pose information of the visual inertial odometry into the second sub-filter of the federated Kalman filter for estimation; fuse the estimation results of the first and second sub-filters in the main filter of the federated Kalman filter to obtain the primary positioning result.

[0030] In this step, the federated Kalman filter comprises a main filter and multiple parallel-operating sub-filters. Each sub-filter can operate independently, avoiding system failure due to a single filter malfunction, thus enhancing fault tolerance and robustness. The first sub-filter employs a tightly coupled approach, fusing pseudorange and carrier phase observations from the global navigation satellite system with the pre-integrated values ​​from the inertial measurement unit (IMU) to obtain a state estimate for the satellite navigation / inertial integrated navigation system, including position, velocity, attitude, and IMU zero bias. The visual-inertial odometry (VIO) is implemented using any existing visual-inertial tightly coupled method, such as a multi-state constrained Kalman filter framework. By extracting feature points from the visual image and jointly optimizing them with the IMU pre-integrated values, a visual / inertial pose estimate is obtained. This embodiment does not limit this approach. The main filter employs an information fusion strategy, weighting and fusing the estimation results of the two sub-filters according to their respective information allocation factors. The information allocation factors are dynamically calculated based on the sub-filter estimation covariance; sub-filters with smaller covariance and higher estimation accuracy are assigned higher weights.

[0031] Step S4: Construct a local environmental map based on visual image data and inertial measurement data. Register the initial positioning result with the local environmental map to obtain the relative pose obtained from the registration. Use the registration result as the observation value to perform closed-loop correction on the main filter to obtain the corrected pose.

[0032] In this step, the construction of the local environmental map can be achieved using any existing visual inertial mapping method, such as sparse map construction based on keyframes or dense map construction based on voxels. This embodiment does not limit this method.

[0033] The registration process involves finding the corresponding position of the current image frame in the local map through visual relocalization or direct image matching, and calculating the relative pose of the current frame relative to the map coordinate system. This relative pose obtained through registration can be considered a high-precision observation value, which is then used as the observation value input to the main filter of the federated Kalman filter. Closed-loop correction is applied to the state variables of the main filter (i.e., the current pose), effectively suppressing the cumulative drift caused by long-term operation or large-scale movement, thus obtaining the corrected accurate pose. The registration process can be implemented using any existing image registration or point cloud registration method, such as perspective n-point calculation based on feature matching, point cloud registration based on the iterative nearest point algorithm, etc. This embodiment does not limit this approach.

[0034] Step S5: Construct a fuzzy logic system whose input features include the data quality parameters of each sensor, and whose outputs are the confidence scores of each sensor and the information allocation factor of the main filter; use a weighted average method to adjust the weights of the corrected pose based on the confidence scores and information allocation factors, and output the final positioning result.

[0035] In this step, the sensors include the satellite navigation sensors of the Global Navigation Satellite System, the inertial measurement unit, and the visual sensor of the visual camera. Satellite navigation data quality parameters include the number of satellites, position accuracy strength, and signal-to-noise ratio. Inertial measurement data quality parameters include the rate of change of zero bias and noise variance. Visual image data quality parameters include the number of feature points, tracking inlier rate, and image brightness change rate. Confidence level is used to characterize the reliability of sensor data in the current environment, and information allocation factor is used for subsequent weight adjustment. The fuzzy logic system can be implemented using any existing fuzzy logic inference architecture, such as the Mamdani-type fuzzy system or the Sugeno-type fuzzy system; this embodiment does not limit this.

[0036] The formula for calculating the final location result is: .

[0037] The technical solution in this embodiment realizes tight coupling estimation of multi-source sensor data, closed-loop suppression of cumulative drift error, and dynamic adaptive adjustment of fusion weights. It effectively solves the problems of spatiotemporal asynchrony of multi-source data, difficulty in eliminating cumulative error, and inability of fixed weights to adapt to environmental changes, thereby improving the positioning accuracy and robustness in complex surveying and mapping environments.

[0038] In the spatiotemporal synchronization alignment of multi-sensor data, the fusion method of asynchronous data directly affects the smoothness of the alignment result. Inappropriate fusion weights can lead to pose jumps, disrupting the continuity of state estimation. Therefore, in one embodiment, such as... Figure 3 As shown, spatiotemporal synchronization alignment of the preprocessed data includes: The data are time-aligned by combining hard triggering and inertial measurement unit pre-integration. The asynchronous data are then fused with the inertial measurement unit pre-integration projection result and the linear interpolation compensation result using a weighted fusion method to obtain the compensated pose. A hysteresis comparator is used to control the weight coefficients of the weighted fusion method.

[0039] In detail, the hard-triggered method refers to triggering the visual camera's exposure and the inertial measurement unit's sampling using the second pulse signal from the Global Navigation Satellite System, ensuring strict synchronization of the data acquisition times of each sensor. For asynchronous data that cannot be synchronized by hard triggering, inertial measurement unit pre-integration is used for compensation. Inertial measurement unit pre-integration refers to integrating the inertial measurement data between two keyframes to obtain the relative pose change. The pre-integration process can be implemented using any method in the prior art, such as the manifold integration method based on Lie algebras; this embodiment does not limit this approach.

[0040] The inertial measurement unit (IMU) pre-integration projection result refers to the pose estimate obtained by projecting the IMU pre-integrated values ​​from the previous synchronization time to the current asynchronous time. Let the previous synchronization time be... The current asynchronous time is The pre-integral quantity of the inertial measurement unit is from arrive relative rotation and relative displacement The pre-integral projection result is and ,in and for Position at any given moment.

[0041] Linear interpolation compensation results refer to pose estimates obtained by linear interpolation of the poses at two adjacent synchronization moments. Let the two adjacent synchronization moments be... and , The corresponding poses are respectively and The result of linear interpolation compensation is: and ,in It is a spherical linear interpolation function. This is the normalized time difference.

[0042] The residual is defined as the difference between the pre-integrated projection result of the inertial measurement unit and the linear interpolation compensation result, including rotation and translation differences. The preset adjustment threshold and preset recovery threshold can take different values; for example, the adjustment threshold can be 0.1 rad and 0.5 m, and the recovery threshold can be 0.05 rad and 0.2 m. The preset adjustment frame number and preset recovery frame number can be set according to the system response speed; for example, the adjustment can be set to 3 frames, and the recovery to 5 frames.

[0043] The weighted fusion method is calculated using the following formula: in, is the weighting coefficient, with a value range of [0,1]. When When, the pre-integrated projection results of the inertial measurement unit are used entirely; when At that time, linear interpolation is used to compensate for the results. The hysteresis comparator only decreases when the residual of the pre-integrated projection of the inertial measurement unit continuously exceeds the preset adjustment threshold for a preset number of adjustment frames. The value is set, and is only increased when the residual is continuously below the preset recovery threshold for a preset number of recovery frames. The value of .

[0044] The preset adjustment frame count refers to the number of frames required for the residual of the inertial measurement unit's pre-integration projection to continuously exceed the preset adjustment threshold. The recovery frame count refers to the number of frames required for the residual of the inertial measurement unit's pre-integration projection to continuously fall below the preset recovery threshold. Both avoid frequent weight fluctuations caused by single-frame noise. For example, in one specific implementation, the preset adjustment frame count can be set to 3 frames, and the preset recovery frame count can be set to 5 frames.

[0045] For example, setting the initial This means that the pre-integrated projection results of the inertial measurement unit are used by default. Let the residual sequence be... ,when When the number of consecutive occurrences reaches the adjusted frame rate, Decrease by one step (e.g., 0.1); when When the number of frames to be recovered is reached consecutively, Increase by one step (e.g., 0.1); otherwise, keep the current value. Unchanged. By controlling the hysteresis comparator, fluctuations in the residual around the threshold are avoided. The frequent jumps ensured the stability of the fusion process.

[0046] Through the technical solution of this embodiment, the weight coefficients of weighted fusion are dynamically adjusted according to the residual quality, the hysteresis comparator suppresses the weight fluctuations caused by single-frame noise, ensures the smoothness and continuity of the compensated pose, and solves the problem of state jump during asynchronous data fusion.

[0047] In the process of constructing and registering a local environmental map, the construction method and registration accuracy directly affect the effectiveness of loop closure correction. If the map construction quality is poor or the registration is inaccurate, accumulated drift errors cannot be effectively suppressed, and incorrect constraints may even be introduced, leading to positioning divergence. Therefore, in one embodiment, such as... Figure 4As shown, a local environmental map is constructed based on visual image data and inertial measurement data. The initial positioning results are then registered with the local environmental map, including: Construct keyframes based on tight coupling between vision and inertial. Each keyframe includes 3D map points and pre-integration constraints of inertial measurement units. The 3D map points obtain real-scale information through binocular vision or RGB-D cameras. A sliding window is used to manage keyframes, where the size of the sliding window is adjusted according to the texture richness of the visual image data, and the managed keyframes are used to form a local environmental map. The initial localization results are visually relocalized and matched with keyframes in the local environment map to obtain the matching inlier rate, and the relative pose of the current image frame with respect to the keyframes in the local environment map is calculated. The relative pose and the matching inlier rate are output as the registration results.

[0048] In detail, binocular vision calculates depth information by comparing the parallax of the images from the left and right cameras, while the RGB-D camera directly acquires depth information using infrared structured light or time-of-flight methods, thus ensuring that the points on the 3D map have a true physical scale and avoiding the scale blur problem of monocular vision.

[0049] Secondly, texture richness is quantified using image gradient information and the number of feature points. When texture richness is high, a larger sliding window is used to retain more keyframes; when texture is scarce, a smaller sliding window is used to control computational complexity. The managed keyframes form a local environmental map. Specifically, the sliding window maintains the sequence of most recent keyframes in chronological order. When a new keyframe is added, the oldest keyframe is removed from the end of the window, while retaining keyframes with sufficient co-location with the current frame.

[0050] It is feasible to use a bag-of-words model for keyframe retrieval in visual relocalization matching. First, feature points of the current frame are extracted using an accelerated robust feature algorithm. The feature point descriptors are then matched with visual words in the bag-of-words model to find the keyframe with the highest similarity to the current frame. Then, a random sampling consensus algorithm is used to remove mismatched points and obtain the in-point matching rate. Finally, the relative pose of the current frame relative to the matching keyframe is obtained by solving the perspective n-point problem.

[0051] Furthermore, a matching threshold can be preset according to the actual application scenario. For example, in a texture-rich indoor environment, the matching threshold can be set to 30 inliers or an inlier rate of 15%; in a texture-sparse outdoor environment, the matching threshold can be appropriately reduced. The matching threshold can be adjusted offline or online adaptively. When the inlier rate is higher than the preset matching threshold, the calculated relative pose is used as the observation value and input into the main filter of the federated Kalman filter for closed-loop correction; when the inlier rate is lower than or equal to the preset matching threshold, it indicates that the current matching quality is poor, and closed-loop correction is not performed to avoid introducing erroneous constraints.

[0052] Through the technical solution of this embodiment, the construction of local environmental maps is efficient, and the adaptive adjustment of the sliding window ensures both the sufficiency of map information and controls the amount of computation. The quality of visual relocation matching is effectively guaranteed, thereby improving the accuracy and reliability of closed-loop correction and effectively suppressing the growth of cumulative drift error.

[0053] In the event of satellite navigation signal interruption and visual repositioning matching failure, the system will lose its absolute positioning reference. If basic positioning capabilities cannot be maintained, positioning will completely fail. Therefore, in one embodiment, the method further includes: When satellite navigation data cannot be obtained and the matching in-point rate is lower than the preset matching threshold, dead reckoning is performed using an inertial measurement unit. During dead reckoning, we continuously attempted to reacquire satellite navigation data and visual image data, and then re-visually re-matched the reacquired visual image data with keyframes in the local environmental map. When the in-point matching rate obtained by satellite navigation data recovery or re-visual repositioning is higher than the preset matching threshold, the recovered satellite navigation data is fused with the current dead reckoning result to achieve a smooth transition.

[0054] In detail, dead reckoning refers to using acceleration and angular velocity measured by an inertial measurement unit (IMU) to calculate the current attitude from the known attitude at the previous moment through integration. Specifically, based on the kinematic equations of the IMU, acceleration and angular velocity are integrated to obtain the recursive relationship between position, velocity, and attitude. Because IMUs have bias and noise, dead reckoning errors accumulate over time; therefore, it is used as a fallback solution rather than a long-term reliance.

[0055] For continuous attempts to reacquire data, a timer can be set to periodically check the signal status of the satellite navigation receiver and the quality of the visual image data. When satellite signal recovery or visual image quality improvement is detected, the recovery process is triggered.

[0056] In addition, when performing visual relocation matching again, since the vehicle may have been displaced during dead reckoning, it is necessary to match the current image frame with historical key frames in order to re-establish the association with the local map.

[0057] Secondly, the fusion of the recovered satellite navigation data and the current dead reckoning result adopts a weighted approach, with the weight of the recovered sensor data gradually increasing and the weight of the current dead reckoning result gradually decreasing to avoid abrupt changes in state. The fusion method can be implemented using any smooth transition method in the existing technology, such as linear weighting, exponential smoothing, etc., and this embodiment does not limit it to this method.

[0058] The technical solution of this embodiment can maintain basic positioning capability even under the dual extreme conditions of satellite signal interruption and visual failure, and achieve a smooth transition when conditions are restored, thus ensuring the continuity and stability of positioning and avoiding positioning interruption caused by sensor failure.

[0059] During the fusion of sensor data recovery and dead reckoning results, improper fusion methods can lead to abrupt changes in the state, causing jumps in the positioning results and affecting the continuity of subsequent positioning. Therefore, in one embodiment, the recovered satellite navigation data is fused with the current dead reckoning result, including: Set a transition window duration. Within the transition window duration, the weight of the recovered satellite navigation data will increase linearly from 0 to 1, while the weight of the current dead reckoning result will decrease linearly from 1 to 0.

[0060] In detail, the transition window duration can be set according to the dynamics of the carrier. For example, a shorter transition window (e.g., 0.5 seconds) can be used when the dynamics are high, and a longer transition window (e.g., 2 seconds) can be used when the dynamics are low. The linear change of the weights ensures the smoothness of the fusion process and avoids abrupt pose changes caused by weight jumps.

[0061] Through the technical solution of this embodiment, the transition during sensor data recovery is smooth and there are no state jumps, which ensures the continuity and stability of the positioning results and avoids pose jumps caused by sudden changes in weights.

[0062] During the fusion of sub-filter estimation results by the main filter of the federated Kalman filter, if the sub-filters produce incorrect estimates due to sensor degradation, direct fusion will contaminate the entire positioning result. Therefore, in one embodiment, such as... Figure 5 As shown, the estimation results of the first sub-filter and the second sub-filter are fused in the main filter, including: Health monitoring is performed on each sub-filter, and the health is dynamically calculated based on visual feature tracking rate, number of satellite navigation satellites, and position accuracy. When the health of any sub-filter is not lower than the preset health threshold, the sub-filter is marked as available; Calculate the fusion weights of the sub-filters marked as usable based on the estimated covariance of each sub-filter; In the main filter, the estimation results of the sub-filters that are marked as available are weighted and fused according to their corresponding fusion weights.

[0063] In detail, the visual feature tracking rate is the ratio of the number of successfully tracked feature points in the current frame to the total number of feature points, reflecting the quality of visual data. The number of navigation satellites and their position accuracy reflect the quality of Global Navigation Satellite System (GNSS) data. Health can be calculated using a weighted summation method, for example: in , , For the preset weighting coefficients, and .

[0064] The preset health threshold can be set according to actual needs, for example, it can be set to 0.5. Sub-filters with a health level below 0.5 are considered unusable.

[0065] In this process, the calculation of the fusion weights follows the covariance weighting principle. For available sub-filters, the fusion weights... Proportional to the trace of the inverse of the covariance matrix, i.e. ,in For the first The estimated covariance matrix of each sub-filter. This represents the trace of the matrix. This calculation method allows sub-filters with higher estimation accuracy (smaller covariance) to receive higher fusion weights. The weighted fusion formula based on the fusion weights is: .

[0066] Through the technical solution of this embodiment, the main filter of the federated Kalman filter can automatically identify and isolate degraded sensors, and fuse only the estimation results of available sub-filters, thereby avoiding low-quality data from polluting the fusion results and improving robustness and positioning accuracy.

[0067] In the process of allocating confidence scores and master filter information to various sensors in a fuzzy logic system, sensor data quality is significantly affected by environmental changes. If adaptive adjustments cannot be made at different time scales, the fusion weights will not match the actual situation. Therefore, in one embodiment, such as... Figure 6 As shown, the outputs include the confidence scores of each sensor and the information allocation factor of the main filter, including: The fuzzy logic system selects the corresponding parameter preset library based on the output of the environment classifier through slow layer adaptation. The environment classifier determines the environment category in real time based on visual semantic segmentation and the dynamic characteristics of the inertial measurement unit. The parameter preset library includes the initial information allocation factor and sliding window size of the main filter. The output of the slow layer adaptation is smoothly transitioned through a low-pass filter to obtain the initial information allocation factor of the main filter. The fuzzy logic system dynamically adjusts the initial information allocation factor based on the estimated covariance and residual statistics of the first and second sub-filters through mid-level adaptive adjustment, thereby obtaining and outputting the information allocation factor of the main filter. The fuzzy logic system, through bottom-level adaptation, dynamically adjusts the observation noise covariance of the second sub-filter based on the registration residual generated during the registration process between the primary positioning result and the local environmental map. It then updates the confidence of the visual sensor based on the adjusted observation noise covariance, updates the confidence of the satellite navigation sensor based on the estimated covariance of the first sub-filter, and updates the confidence of the inertial measurement unit based on the zero bias rate of change of the inertial measurement unit, finally outputting the confidence of each sensor.

[0068] In detail, the fuzzy logic system in this embodiment is a hierarchical fuzzy logic system, which adopts a hierarchical uncertainty suppression architecture for adaptive adjustment to output the confidence of each sensor and the information allocation factor of the main filter.

[0069] The environment classifier determines the environment category in real time based on visual semantic segmentation and the dynamic characteristics of the inertial measurement unit (IMU). Visual semantic segmentation uses a lightweight convolutional neural network to perform semantic segmentation on the image, identifying categories such as roads, buildings, trees, and sky. The IMU dynamic characteristics include acceleration amplitude, angular velocity amplitude, and motion frequency. Combining this information, the environment classifier outputs the current environment category, such as urban canyon, open area, forest cover, or indoor environment. A parameter preset library pre-stores initial information allocation factors and sliding window sizes for the main filter under different environment categories. The low-pass filter uses a first-order hysteresis filter, with the filter coefficients set according to the rate of environmental change to avoid parameter abrupt changes caused by frequent switching of environment categories.

[0070] The middle-layer adaptive method employs a covariance-weighted adjustment approach. When the estimated covariance of a sub-filter is small, its information allocation factor is increased accordingly; when the residual statistic of a sub-filter increases abnormally, its information allocation factor is decreased accordingly. The adjusted information allocation factor satisfies the normalization condition, i.e., the sum of the information allocation factors of all sub-filters is 1.

[0071] The registration residual reflects the quality of visual relocalization matching. When the registration residual is large, it indicates that the visual observation quality has decreased. The observation noise covariance of the second sub-filter should be increased, and the weight of visual observation in the filtering should be reduced. Conversely, when the registration residual is small, the observation noise covariance should be reduced.

[0072] The confidence level can be taken as the inverse normalized value of the observation noise covariance; the smaller the observation noise covariance, the higher the confidence level. The confidence level of the satellite navigation sensor is updated based on the estimated covariance of the first sub-filter; the smaller the estimated covariance, the higher the confidence level. The confidence level of the inertial measurement unit (IMU) is updated based on the zero-bias rate of change; the smaller the zero-bias rate of change, the more stable the IMU, and the higher the confidence level.

[0073] Through the technical solution of this embodiment, the fuzzy logic system with a hierarchical uncertainty suppression architecture achieves end-to-end adaptive adjustment from the environment level, sensor level to the observation level, enabling the system to maintain optimal fusion performance under different environments, different sensor states, and different observation qualities, effectively solving the problem that fixed weights cannot adapt to environmental changes.

[0074] In applications of fuzzy logic systems, fuzzy rule bases typically rely on expert experience, making it difficult to cover all complex scenarios and limiting the system's adaptability in unknown environments. To address this, one embodiment employs reinforcement learning to fine-tune the fuzzy rule base of the fuzzy logic system online. The fuzzy rule base contains a set of if-then rules, such as "if the satellite navigation signal quality is high and the visual image quality is high, then the information allocation factor is biased towards satellite navigation," etc. Reinforcement learning obtains reward signals through interaction with the environment and automatically adjusts the confidence or membership function parameters of the rules.

[0075] The reward function for reinforcement learning is: in, This is a consensus reward based on Mahalanobis distance for multi-source residuals. Mahalanobis distance is a distance metric that measures the similarity between two sample sets, taking into account the correlation between the data. Multi-source residual vectors ,in For satellite navigation observation residuals, For the pre-integrated residual of the inertial measurement unit, The visual reprojection residual. Mahalanobis distance is defined as... ,in The residual mean vector, Let be the residual covariance matrix. Take the negative value of the Mahalanobis distance. The smaller the Mahalanobis distance, the better the consistency between sensors and the higher the reward.

[0076] When a loop closure or a baseline truth value exists, the negative logarithm of the absolute error is used; when no loop closure or baseline truth value exists, the interior point rate of the local map matching the global baseline map is used. When a known loop closure or external baseline truth value exists... When it does not exist, .

[0077] This is a measure of the consistency between the fused trajectory and the high-frequency motion of the inertial measurement unit. Let the second derivative of the fused trajectory be... The acceleration measured by the inertial measurement unit is ,but The smaller the difference, the smoother the trajectory, and the higher the reward.

[0078] To calculate the complexity penalty, we take the normalized value of the processing time of the current frame; the longer the processing time, the greater the penalty.

[0079] The weighting coefficients are dynamically adjusted based on the uncertainty of the environment classifier's output. When environmental uncertainty is high, the weighting coefficients are increased. weight When a reliable truth value exists, increase weight When the carrier has high dynamics, increase weight When computing resources are strained, increase weight .

[0080] Additionally, the reward function can include nonparametric consistency detection, which uses kernel density estimation to estimate the distribution of the multi-source residual vector. Kernel density estimation is a nonparametric probability density estimation method that does not assume the data follows a Gaussian distribution. The residual distribution is obtained through kernel density estimation. Compare it with the preset Gaussian distribution Compare the two and calculate the Kullback-Leibler divergence. .when When the value exceeds a preset threshold, it indicates that the residual distribution deviates significantly from the Gaussian assumption, at which point the threshold should be lowered. The weights are adjusted to avoid misleading consistency rewards based on Mahalanobis distance.

[0081] Through the technical solution of this embodiment, the rule base of the fuzzy logic system can be self-optimized online. The reward function comprehensively considers sensor consistency, positioning accuracy, trajectory smoothness and computational efficiency. It also compensates for the failure of the Gaussian assumption through non-parametric detection, enabling the system to adapt to more complex and changeable environments and solving the problem of insufficient adaptability of the fixed rule base.

[0082] In scenarios where visual data is limited or higher positioning accuracy is required, relying solely on visual sensors may not provide sufficient observation information, leading to a decrease in positioning accuracy. Therefore, in one embodiment, the method further includes: Acquire point cloud data collected by lidar; A laser inertial odometry is constructed by tightly coupling point cloud data with inertial measurement data, and the pose information of the laser inertial odometry is input into the third sub-filter of the federated Kalman filter for estimation. A laser point cloud map is constructed. When registering the initial positioning results with the local environmental map, the laser point cloud map and the local environmental map are jointly registered to obtain the laser point cloud registration result and the registration result obtained by visual relocalization matching. Based on the confidence level output by the fuzzy logic system, the laser point cloud registration result or the registration result obtained by visual relocalization matching is selected for closed-loop correction.

[0083] In detail, lidar acquires three-dimensional point cloud information of the surrounding environment by emitting laser beams and receiving reflected signals. It features high ranging accuracy and is unaffected by lighting conditions.

[0084] The laser inertial odometry is implemented using any laser-inertial tight coupling method in the prior art, such as the iterative nearest point algorithm or the normal distribution transformation algorithm, to register two consecutive frames of point clouds to obtain the relative pose between frames, and then combined with the pre-integration of the inertial measurement unit for joint optimization to obtain the pose estimate of the laser / inertial combination. This embodiment does not limit this.

[0085] Laser point cloud maps are managed using voxel grids, with each voxel storing the geometric features of the point cloud (such as planes and edges), which facilitates rapid registration and map compression.

[0086] During joint registration, the laser point cloud is first registered with the visual map, and the visual image is registered with the laser map separately, resulting in two registration results. Then, based on the confidence level output by the fuzzy logic system, the registration result with higher confidence is selected for closed-loop correction. Specifically, when the visual image quality is high (e.g., rich texture, sufficient illumination), the confidence level of the visual sensor output by the fuzzy logic system is high, and the registration result obtained by visual relocalization matching is selected; when the visual image quality is poor (e.g., weak texture, overexposure, or underexposure), the confidence level of the laser radar is high, and the laser point cloud registration result is selected.

[0087] Through the technical solution of this embodiment, the lidar, as an optional enhancement sensor, can provide reliable supplementary information when visual data is limited. The joint registration strategy dynamically selects the optimal correction source based on the sensor confidence level, which further improves the environmental adaptability and positioning accuracy of this method and solves the problem of decreased positioning accuracy under visually limited conditions.

[0088] It should be understood that, although Figure 2-6 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 2-6 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0089] Secondly, based on the same inventive concept, this application also provides a system for implementing the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm described above. The solution provided by this system is similar to the implementation scheme described in the above method. Therefore, the specific limitations in one or more embodiments of the mapping, navigation, and positioning correction system of the multi-sensor fusion algorithm provided below can be found in the limitations of the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm described above, and will not be repeated here.

[0090] In one embodiment, such as Figure 7 As shown, a mapping, navigation, and positioning correction system based on a multi-sensor fusion algorithm includes a data acquisition module, a data processing module, a collaborative filtering module, a pose registration module, a closed-loop correction module, a fuzzy logic system module, and a result acquisition module.

[0091] The data acquisition module is used to acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data collected by the visual camera. The data processing module is used to perform gross error detection and outlier removal preprocessing on satellite navigation data, inertial measurement data, and visual image data, and to perform spatiotemporal synchronization alignment on the preprocessed data. The collaborative filtering module includes a first sub-filter, a second sub-filter, and a main filter. The first sub-filter is used to estimate the spatiotemporally synchronized satellite navigation data and inertial measurement data. The second sub-filter is used to tightly couple the spatiotemporally synchronized visual image data and inertial measurement data to construct a visual inertial odometry (VIO) and estimate the pose information of the VIO. The main filter is used to fuse the estimation results of the first and second sub-filters to obtain the primary positioning result. The pose registration module is used to construct a local environmental map based on visual image data and inertial measurement data, register the initial positioning results with the local environmental map, and obtain the relative pose obtained by registration as the registration result. The closed-loop correction module is used to perform closed-loop correction on the main filter using the registration result as the observation value to obtain the corrected pose. The fuzzy logic system module takes as input the data quality parameters of each sensor and outputs the confidence level of each sensor and the information allocation factor of the main filter. The result acquisition module is used to adjust the weights of the corrected pose using a weighted average method based on confidence level and information allocation factor, and output the final localization result.

[0092] In one embodiment, the pose registration module includes a map building unit.

[0093] The map construction unit is used to construct keyframes based on a tightly coupled vision-inertial system. Each keyframe includes 3D map points and pre-integration constraints from the inertial measurement unit (IMU). The 3D map points acquire real-scale information through binocular vision or an RGB-D camera. The map construction unit uses a sliding window to manage the keyframes, where the size of the sliding window is adjusted according to the texture richness of the visual image data, and the managed keyframes form a local environmental map. The map construction unit performs visual relocalization matching between the initial localization results and the keyframes in the local environmental map to obtain the inlier rate, and calculates the relative pose of the current image frame relative to the keyframes in the local environmental map. The map construction unit outputs the relative pose and the inlier rate as the registration results.

[0094] In one embodiment, the result acquisition module includes a disconnection error correction unit.

[0095] The system includes several components: a disconnection correction unit, used for dead reckoning via inertial measurement unit (IMU) when satellite navigation data is unavailable and the in-point matching rate is below a preset matching threshold; a continuous attempt to reacquire satellite navigation and visual image data during dead reckoning, followed by visual re-matching of the reacquired visual image data with keyframes in the local environmental map; and a fusion of the recovered satellite navigation data with the current dead reckoning result when the in-point matching rate obtained from recovered satellite navigation data acquisition or re-matching exceeds a preset matching threshold.

[0096] In one embodiment, the disconnection correction unit includes a result transition subunit.

[0097] The result transition subunit is used to set the transition window duration. Within the transition window duration, the weight of the recovered satellite navigation data is linearly increased from 0 to 1, while the weight of the current dead reckoning result is linearly decreased from 1 to 0.

[0098] In one embodiment, the collaborative filtering module includes an estimation result fusion unit.

[0099] The estimation result fusion unit monitors the health of each sub-filter, dynamically calculating the health based on visual feature tracking rate, number of navigation satellites, and position accuracy. It marks any sub-filter as usable when its health is not lower than a preset health threshold. The unit also calculates the fusion weights for the marked usable sub-filters based on their estimated covariance. Finally, it performs a weighted fusion of the estimation results of the marked usable sub-filters within the main filter, according to their respective fusion weights.

[0100] In one embodiment, the fuzzy logic system module includes an adjustment factor output unit.

[0101] The adjustment factor output unit is used to select the corresponding parameter preset library based on the output of the environment classifier through slow-layer adaptive processing of the fuzzy logic system. The environment classifier determines the environment category in real time based on visual semantic segmentation and the dynamic characteristics of the inertial measurement unit. The parameter preset library includes the initial information allocation factor and sliding window size of the main filter. The output of the slow-layer adaptive processing is smoothed through a low-pass filter to obtain the initial information allocation factor of the main filter. The adjustment factor output unit is used to dynamically adjust the initial information allocation factor based on the estimated covariance and residual statistics of the first and second sub-filters through mid-layer adaptive processing of the fuzzy logic system to obtain and output the information allocation factor of the main filter. The adjustment factor output unit is used to dynamically adjust the observation noise covariance of the second sub-filter based on the registration residual generated during the registration process between the primary positioning result and the local environmental map through low-layer adaptive processing of the fuzzy logic system. The adjusted observation noise covariance is used to update the confidence of the visual sensor, the estimated covariance of the satellite navigation sensor, and the zero bias rate of change of the inertial measurement unit, and output the confidence of each sensor.

[0102] In one embodiment, the data processing module includes a synchronization alignment processing unit.

[0103] The synchronous alignment processing unit is used to perform time alignment of each data by combining hard triggering and inertial measurement unit pre-integration. It also uses a weighted fusion method to fuse the inertial measurement unit pre-integration projection result and the linear interpolation compensation result of asynchronous data to obtain the compensated pose. Furthermore, a hysteresis comparator is used to control the weight coefficients of the weighted fusion method.

[0104] Specific limitations regarding the mapping, navigation, and positioning correction system based on multi-sensor fusion algorithms can be found in the limitations of the mapping, navigation, and positioning correction method based on multi-sensor fusion algorithms described above, and will not be repeated here. Each module in the aforementioned mapping, navigation, and positioning correction system based on multi-sensor fusion algorithms can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in hardware or independent of the processor in a computer device, or stored in software in the memory of a computer device, so that the processor can call and execute the corresponding operations of each module.

[0105] In one embodiment, a computer device is provided, which may be a server, and its internal structure diagram may be as follows: Figure 8 As shown, the computer device includes a processor, memory, network interface, and database connected via a system bus. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and database. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores all data related to the system. The network interface is used for communication with external terminals via a network connection. When executed by the processor, the computer program implements a mapping, navigation, and positioning correction method based on a multi-sensor fusion algorithm.

[0106] Those skilled in the art will understand that Figure 8 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0107] Thirdly, a computer device is provided, including a memory and a processor, the memory being communicatively connected to the processor, and the memory storing a computer program executable on the processor, wherein the processor, when executing the computer program, implements the steps of the above-described method embodiments and any of the methods.

[0108] Fourthly, a computer-readable storage medium is provided, on which a computer program is stored, which, when executed by a processor, implements the steps of the above-described method embodiments and any of the methods.

[0109] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0110] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0111] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A surveying navigation positioning correction method of a multi-sensor fusion algorithm, characterized in that, The method includes: Acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data from the visual camera; The satellite navigation data, the inertial measurement data, and the visual image data are subjected to gross error detection and outlier removal preprocessing, and the preprocessed data are spatiotemporally synchronized and aligned. The spatiotemporally synchronized satellite navigation data and inertial measurement data are input into the first sub-filter of the federated Kalman filter for estimation; the spatiotemporally synchronized visual image data and inertial measurement data are tightly coupled to construct a visual inertial odometry, and the pose information of the visual inertial odometry is input into the second sub-filter of the federated Kalman filter for estimation; the estimation results of the first sub-filter and the second sub-filter are fused in the main filter of the federated Kalman filter to obtain the primary positioning result; A local environmental map is constructed based on the visual image data and the inertial measurement data. The initial positioning result is registered with the local environmental map to obtain the relative pose obtained by registration as the registration result. The registration result is used as the observation value to perform closed-loop correction on the main filter to obtain the corrected pose. A fuzzy logic system is constructed, wherein the input features of the fuzzy logic system include the data quality parameters of each sensor, and the outputs are the confidence scores of each sensor and the information allocation factor of the main filter; the corrected pose is adjusted by weighted averaging based on the confidence scores and the information allocation factor, and the final positioning result is output.

2. The method of claim 1, wherein, The step of constructing a local environmental map based on the visual image data and the inertial measurement data, and registering the primary positioning result with the local environmental map, includes: Construct keyframes based on tight coupling between vision and inertial, each keyframe including 3D map points and pre-integration constraints of inertial measurement units, wherein the 3D map points acquire real-scale information through binocular vision or RGB-D cameras; The keyframes are managed using a sliding window, wherein the size of the sliding window is adjusted according to the texture richness of the visual image data, and the managed keyframes constitute the local environmental map. The initial localization result is visually relocalized and matched with the keyframes in the local environment map to obtain the matching inlier rate, and the relative pose of the current image frame relative to the keyframes in the local environment map is calculated. The relative pose and the matched inlier rate are output as the registration result.

3. The method of claim 2, wherein, The method also includes: When satellite navigation data cannot be obtained and the matching in-point rate is lower than the preset matching threshold, dead reckoning is performed using the inertial measurement unit. During the dead reckoning, continuous attempts are made to reacquire the satellite navigation data and the visual image data, and the reacquired visual image data is re-matched with the keyframes in the local environmental map for visual relocation. When the in-point matching rate obtained by the satellite navigation data recovery acquisition or the visual repositioning matching is higher than the preset matching threshold, the recovered satellite navigation data is fused with the current dead reckoning result.

4. The method of claim 3, wherein, The process of fusing the recovered satellite navigation data with the current dead reckoning results includes: A transition window duration is set. During the transition window duration, the weight of the recovered satellite navigation data is linearly increased from 0 to 1, while the weight of the current dead reckoning result is linearly decreased from 1 to 0.

5. The method of claim 1, wherein, The step of fusing the estimation results of the first sub-filter and the second sub-filter in the main filter includes: Health monitoring is performed on each sub-filter, and the health status is dynamically calculated based on visual feature tracking rate, number of satellite navigation satellites, and position accuracy strength. When the health of any sub-filter is not lower than a preset health threshold, the sub-filter is marked as available; The fusion weights of the sub-filters marked as available are calculated based on the estimated covariance corresponding to each sub-filter. In the main filter, the estimation results of the sub-filters that are marked as available are weighted and fused according to their corresponding fusion weights.

6. The method of claim 1, wherein, The confidence levels of each output sensor and the information allocation factor of the main filter include: The fuzzy logic system uses slow-layer adaptation to select the corresponding parameter preset library based on the output of the environment classifier. The environment classifier determines the environment category in real time based on visual semantic segmentation and the dynamic characteristics of the inertial measurement unit. The parameter preset library includes the initial information allocation factor and sliding window size of the main filter. The output of the slow-layer adaptation is smoothly transitioned through a low-pass filter to obtain the initial information allocation factor of the main filter. The fuzzy logic system, through mid-level adaptive adjustment, dynamically adjusts the initial information allocation factor based on the estimated covariance and residual statistics of the first and second sub-filters, thereby obtaining and outputting the information allocation factor of the main filter; The fuzzy logic system, through low-level adaptive adjustment, dynamically adjusts the observation noise covariance of the second sub-filter based on the registration residual generated during the registration process between the primary positioning result and the local environmental map. It then updates the confidence level of the visual sensor based on the adjusted observation noise covariance, updates the confidence level of the satellite navigation sensor based on the estimated covariance of the first sub-filter, updates the confidence level of the inertial measurement unit based on the zero bias rate of change of the inertial measurement unit, and outputs the confidence level of each sensor.

7. The method according to any one of claims 1 to 6, characterized in that, The spatiotemporal synchronization and alignment of the preprocessed data includes: The data are time-aligned by a combination of hard triggering and inertial measurement unit pre-integration. The asynchronous data are then fused by a weighted fusion method to obtain the inertial measurement unit pre-integration projection result and the linear interpolation compensation result, and a hysteresis comparator is used to control the weight coefficient of the weighted fusion method.

8. A mapping navigation positioning correction system of a multi-sensor fusion algorithm, characterized in that, The system includes: The data acquisition module is used to acquire satellite navigation data from the Global Navigation Satellite System, inertial measurement data from the inertial measurement unit, and visual image data collected by the visual camera. The data processing module is used to perform gross error detection and outlier removal preprocessing on the satellite navigation data, the inertial measurement data, and the visual image data, and to perform spatiotemporal synchronization alignment on the preprocessed data. The collaborative filtering module includes a first sub-filter, a second sub-filter, and a main filter. The first sub-filter is used to estimate the spatiotemporally synchronized satellite navigation data and inertial measurement data. The second sub-filter is used to tightly couple the spatiotemporally synchronized visual image data and inertial measurement data to construct a visual inertial odometry (VIO), and to estimate the pose information of the VIO. The main filter is used to fuse the estimation results of the first sub-filter and the second sub-filter to obtain a primary positioning result. The pose registration module is used to construct a local environmental map based on the visual image data and the inertial measurement data, register the primary positioning result with the local environmental map, and obtain the relative pose obtained by registration as the registration result. The closed-loop correction module is used to perform closed-loop correction on the main filter using the registration result as the observation value to obtain the corrected pose. The fuzzy logic system module takes as input the data quality parameters of each sensor and outputs the confidence level of each sensor and the information allocation factor of the main filter. The result acquisition module is used to adjust the weights of the corrected pose using a weighted average method based on the confidence level and the information allocation factor, and output the final localization result. 9.A computer device, comprising a memory and a processor, the memory being connected with the processor in communication, and the memory storing a computer program capable of running on the processor, characterized in that, When the processor executes the computer program, it implements the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm as described in any one of claims 1 to 7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by the processor, it implements the mapping, navigation, and positioning correction method of the multi-sensor fusion algorithm as described in any one of claims 1 to 7.

Images