Vision-IMU-Beidou-based robust estimation adaptive fusion navigation system

Abstract

The invention relates to the technical field of navigation and positioning, and discloses a robust estimation adaptive fusion navigation system based on vision-IMU-Beidou, comprising a data acquisition module, a preprocessing module, a robust estimation module, an adaptive fusion module and a navigation output module which are connected in sequence. All the modules interact with one another so as to be used for collecting visual images, IMU original data and Beidou observation data, and multi-source observation information is obtained through visual feature matching, IMU pre-integration and Beidou carrier pseudo-range double-difference processing; constructing a robust model by utilizing an IGG3 equivalence weight function, and dynamically inhibiting abnormal observation according to an observation residual error; and meanwhile, an observation noise covariance matrix of extended Kalman filtering is adaptively adjusted based on residual statistical characteristics, so that tight coupling of multi-source information is realized. According to the method, the problems of observation gross error and model error accumulation caused by illumination change, signal shielding and the like in a complex environment are effectively solved, and centimeter-level high-precision positioning and high-robustness attitude estimation are realized.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou. Background Technology

[0002] With the widespread application of autonomous devices such as drones, intelligent vehicles, and mobile robots, multi-sensor fusion navigation technology based on vision, inertial measurement units (IMU), and satellite positioning systems (BeiDou BDS) has become the mainstream solution for achieving high-precision positioning. However, each sensor suffers from observational defects and model error accumulation in complex environments. The vision system is highly susceptible to changes in lighting, missing textures, and dynamic occlusion, leading to feature matching failures and pose estimation errors. During long-term operation, the IMU is affected by temperature drift and mechanical vibration, and the zero bias and scale factor errors of the accelerometer and gyroscope accumulate over time, causing irreversible divergence in dead reckoning and a lack of absolute position reference. In scenarios such as urban canyons or tunnels, the BeiDou system is susceptible to multipath effects, signal blockage, and electromagnetic interference, and its ionospheric, tropospheric delay, and clock bias model errors are difficult to fully compensate for in dynamic environments.

[0003] Furthermore, existing multi-sensor fusion technologies have significant limitations in their weight allocation mechanisms. Traditional fixed-weight or fixed-ratio fusion methods cannot dynamically adjust according to environmental changes and the real-time status of sensors, failing to fully utilize the complementary characteristics between sensors. For example, when satellite signals deteriorate due to obstruction, if the system cannot automatically reduce its weight and compensate using vision or IMU, the overall positioning accuracy will be significantly reduced. This rigid fusion strategy makes it difficult for existing navigation systems to maintain high-precision and robust pose estimation in extreme conditions such as signal interruption and insufficient texture, failing to meet the continuous navigation requirements in complex environments. Summary of the Invention

[0004] This invention provides a robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou. By introducing the robust estimation principle and adaptive weight allocation mechanism, it effectively suppresses gross errors and model errors in multi-sensor observations, and achieves high-precision and robust pose estimation.

[0005] This invention provides a robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou, comprising a data acquisition module, a preprocessing module, a robust estimation module, an adaptive fusion module, and a navigation output module connected in sequence;

[0006] The data acquisition module is used to acquire multi-source raw data and transmit it to the preprocessing module; wherein, the multi-source raw data includes visual image data, IMU raw data and BeiDou observation data;

[0007] The preprocessing module is used to extract and match features from the visual image data to calculate the camera pose, perform pre-integration processing on the IMU raw data to obtain the state recursion, perform double-difference processing on the carrier and pseudorange in the BeiDou observation data to obtain double-difference observations, and transmit the camera pose, state recursion and double-difference observations as preprocessed observation information to the robust estimation module.

[0008] The robust estimation module is used to predict the system state based on the state recursion, calculate the multi-sensor observation residuals of the camera pose and double-difference observations relative to the predicted system state, standardize the multi-sensor observation residuals, and use the equivalent weight function to calculate the corresponding robust equivalent weights to construct a robust weight matrix. The multi-sensor observation residuals and robust weight matrix are then transmitted to the adaptive fusion module.

[0009] The adaptive fusion module is used to adaptively adjust the observation noise covariance matrix based on the statistical characteristics and robust weight matrix of the multi-sensor observation residuals, calculate the Kalman gain using the adjusted observation noise covariance matrix, and correct the system state in combination with the multi-sensor observation residuals to obtain the optimal state estimate, which is then transmitted to the navigation output module.

[0010] The navigation output module is used to parse the optimal state estimate into navigation state parameters that include position, velocity, attitude and sensor error parameters, and then output them.

[0011] Furthermore, the data acquisition module includes a visual image acquisition unit, an IMU acquisition unit, and a satellite positioning data acquisition unit, which are respectively connected to a visual sensor, an inertial measurement unit (IMU), and a BeiDou receiver, for synchronously acquiring visual image data, IMU raw data, and BeiDou observation data;

[0012] The visual image acquisition unit uses an RGB-D camera to capture RGB images in real time and generate depth maps as visual image data;

[0013] The IMU acquisition unit adopts a six-axis inertial measurement unit, which includes a three-axis accelerometer and a three-axis gyroscope, and is used to transmit the acquired acceleration and angular velocity raw data as IMU raw data through the bus;

[0014] The satellite positioning data acquisition unit is used to collect pseudorange, carrier phase and satellite ephemeris data of BeiDou satellites as BeiDou observation data.

[0015] Furthermore, the specific processing steps of the preprocessing module include:

[0016] The visual image data preprocessing involves: extracting image feature points using the ORB algorithm, filtering feature matching point pairs using the K nearest neighbor algorithm and ratio test, and performing spatial homogenization processing on the feature points using the octree method; calculating and decomposing the essential matrix based on the epipolar geometry principle to obtain the relative rotation and translation of the camera between adjacent frames, thereby obtaining the camera pose.

[0017] The IMU raw data preprocessing involves: performing pre-integration processing to integrate and compress the high-frequency acceleration and angular velocity measurements between two keyframes, calculating the position increment, velocity increment, and attitude increment relative to the previous keyframe; based on the optimal system state estimate at the previous moment, superimposing the position increment, velocity increment, attitude increment, and gravity effect, recursively calculating the predicted state vector at the current moment; simultaneously, constructing a state transition matrix based on the IMU kinematic error model, and propagating the posterior covariance of the previous moment to the current moment to obtain the predicted covariance matrix; the predicted state vector and the predicted covariance matrix together constitute the state recursion quantity.

[0018] The BeiDou observation data preprocessing involves: constructing a double-difference observation model of carrier phase and pseudorange; firstly, performing inter-station difference calculation on the observation values ​​of different receivers on the same satellite to eliminate satellite clock errors and orbital errors; then, performing inter-satellite difference calculation on the observation values ​​of different satellites on the same receiver to eliminate receiver clock errors; thereby generating the double-difference observation values ​​that are insensitive to clock errors and atmospheric delay errors.

[0019] Furthermore, the specific process of preprocessing the raw IMU data is as follows:

[0020] The measured acceleration and angular velocity They are modeled as follows:

[0021]

[0022] Among them, C b→w Let a be the rotation matrix from the carrier to the world coordinate system. t w t For true acceleration and angular velocity, g w Let b be the gravity vector in the world coordinate system. a,t b g,t For zero bias of the accelerometer and gyroscope, n a,t n g,t Gaussian white noise for IMU acceleration / angular velocity;

[0023] Between time k-1 and time k, the relative motion increment, including the position increment Δα, is calculated by recursively integrating the IMU measurements. k-1,k Velocity increment Δβ k-1,kand attitude increment Δq k-1,k ;

[0024] Using the optimal state estimate from the previous time k-1 and the calculated pre-integral increment, the system state at the current time k is predicted. The calculation formula includes recursion of position, velocity, attitude, and zero bias:

[0025]

[0026] While calculating the state prediction, the state transition matrix F is derived based on the IMU kinematic error model. k-1 The process noise covariance matrix Q k Using linearized recursive formulas The posterior covariance P from the previous moment k-1 Propagation up to the current moment yields the predicted covariance.

[0027] The calculated predicted state vector and the predicted covariance matrix Together they constitute the state recursion quantity.

[0028] Furthermore, the specific processing procedure of the robust estimation module includes:

[0029] Step 1) Using the optimal system state estimate from the previous moment, superimpose the IMU pre-integrated state recursion to calculate the predicted system state at the current moment; based on the predicted system state, calculate the expected visual and BeiDou observations at the current moment using the camera observation model and satellite geometry; subtract the camera pose from the corresponding expected observation to obtain the visual observation residual, which includes position and attitude residuals; subtract the double-difference observation output from the BeiDou double-difference processing from the corresponding expected observation to obtain the BeiDou double-difference observation residual, which includes double-difference pseudorange residuals and double-difference carrier residuals.

[0030] Step 2) Obtain the square root of the diagonal element of the covariance matrix of the observation noise of each sensor as the standard deviation. Divide each component of the visual observation residual and the Beidou double-difference observation residual by its corresponding standard deviation to obtain the dimensionless standardized residual ε. i ;

[0031] Step 3) Calculate the robust equivalent weights of each observation component using the IGG3 three-segment robust model. The calculation formula is as follows:

[0032]

[0033] Where x0 is the normal residual threshold and x1 is the abnormal residual threshold; when the absolute value of the standardized residual is less than or equal to x0, the original weights are maintained; when the absolute value of the standardized residual is between x0 and x1, the weights are reduced proportionally; when the absolute value of the standardized residual is greater than k1, the weights are reset to 0.

[0034] Step 4) Configure different abnormal residual thresholds k1 for different sensors: k1 is set to 3 for IMU observation, k1 is set to 4 for visual observation, and k1 is set to 5 for Beidou observation; the normal residual threshold k0 is uniformly set to 2; based on the calculated robust equivalent weights of each observation component, construct a diagonal robust weight matrix and transmit it to the adaptive fusion module.

[0035] Furthermore, in step 1) of the robust estimation module,

[0036] Visual Odometry (VO) observation residual calculation: Assume the pose of the camera in the world coordinate system output by the visual odometry is... and The state prediction values ​​obtained by IMU pre-integration, combined with the extrinsic parameters of the camera and IMU, have the following mapping relationship: Calculate the camera pose predicted by the IMU and the visual residual r. vo Includes positional residual r p,vo and attitude residual r θ,vo The calculation formula is as follows:

[0037]

[0038] Finally, the visual residual vector is obtained.

[0039] BeiDou double-difference observation residual calculation: Calculate the double-difference pseudorange residual r separately. ρ and double-difference carrier phase residual r φ The residual is defined as the observed value minus the expected geometric distance calculated based on the IMU-predicted location, and the calculation formula is as follows:

[0040]

[0041]

[0042] in, These are double-difference pseudorange observations; These are double-difference carrier observations, where λ is the carrier wavelength; For double-difference integer ambiguity, The predicted satellite-to-ground distance is double-differenced; ultimately, the residual vector of the BeiDou satellite system is obtained. N represents the number of satellite pairs.

[0043] Furthermore, the specific processing steps of the adaptive fusion module include:

[0044] Step 1) Define the system's state vector X as: in, and These represent the IMU's position and velocity in the world coordinate system. Let b be the axis-angle attitude of the IMU relative to the world coordinate system. g and b a The zero bias of the IMU gyroscope and accelerometer, respectively, s v N is the scale factor for visual odometry. dd For the double-difference carrier integer ambiguity of the BeiDou system;

[0045] Step 2) Using the IMU kinematic equations, combined with the posterior state estimate from the previous time step and the raw IMU data from the current time step, the prior state estimate for the current time step is recursively calculated; based on the IMU error propagation model, the state transition Jacobian matrix and noise propagation matrix are constructed, and the posterior covariance matrix from the previous time step is propagated to the current time step to obtain the prediction error covariance matrix.

[0046] Step 3) Concatenate the visual odometry pose and BeiDou double-difference observation values ​​output by the preprocessing module to construct a joint observation vector; calculate the partial derivatives of the visual observation equation and the BeiDou double-difference observation equation with respect to the state vector to construct a joint observation Jacobian matrix; subtract the observation prediction value calculated based on the prior state estimate from the joint observation vector to obtain the joint observation residual vector.

[0047] Step 4) Calculate the normalized sum of squares statistic χ using the joint observation residual vector, the joint observation Jacobian matrix, and the prediction error covariance matrix. 2 Set the chi-square distribution test threshold, and set the χ² value... 2 Compare with this threshold: if χ 2 If the value is less than the threshold, the initial observation noise covariance matrix is ​​reduced using an adjustment coefficient; if χ 2 If the value is greater than or equal to the threshold, the initial observation noise covariance matrix is ​​amplified using an adjustment coefficient and then weighted and adjusted in conjunction with the robust weight matrix input from the robust estimation module; thus obtaining the adjusted observation noise covariance matrix.

[0048] Step 5) Using the prediction error covariance matrix, the joint observation Jacobian matrix, and the adjusted observation noise covariance matrix. Calculate the Kalman gain; use the Kalman gain and the joint observation residual vector to correct the prior state estimate, and output the optimal state estimate at the current time; update the state estimate error covariance matrix as the benchmark for prediction at the next time step.

[0049] Furthermore, step 3) of the adaptive fusion module specifically includes:

[0050] Merging observation vectors: Merging the visual (VO) observation vector Z V0 and BeiDou (BDS) observation vector Z BDS Concatenate the data to construct the joint observation vector Z:

[0051]

[0052] Constructing the observation Jacobian matrix: Calculate the partial derivatives of the visual observation function and the BeiDou observation function with respect to the state vector X, and construct the joint observation Jacobian matrix H:

[0053]

[0054] in,

[0055] Calculate the original residual vector: Subtract the value based on the predicted state X from the joint observation vector Z. k+1|k Calculated observation prediction value HX k+1|k The original residual vector r is obtained:

[0056] Furthermore, step 5) of the adaptive fusion module specifically includes:

[0057] Calculate the Kalman gain:

[0058]

[0059] Among them, the adjusted This allows the gain K to automatically adapt to the current observation quality;

[0060] Update the system state: Use Kalman gain to correct the predicted state and obtain the optimal state estimate X at the current time. k+1 :

[0061] X k+1 =X k+1|k +Kr

[0062] This state vector contains core navigation parameters such as position, velocity, attitude, and sensor errors;

[0063] Updated posterior covariance: Calculate the updated state estimation error covariance matrix P. k+1This provides a benchmark for predictions at the next moment:

[0064] Furthermore, the specific processing procedure of the navigation output module includes:

[0065] The system receives the optimal state estimate output by the adaptive fusion module; analyzes the optimal state estimate to extract kinematic state parameters containing the three-dimensional position, three-dimensional velocity, and attitude information of the carrier in the world coordinate system; analyzes the optimal state estimate to extract sensor error parameters including IMU gyroscope bias, IMU accelerometer bias, visual odometry scale factor, and BeiDou dual-difference carrier integer ambiguity; and sends the kinematic state parameters and the sensor error parameters to an external control system via a communication interface for real-time navigation control or path planning.

[0066] The beneficial effects of this invention are as follows:

[0067] 1. This invention effectively suppresses gross errors in multi-sensor observations by introducing a robust estimation mechanism. Even under extreme conditions such as signal obstruction and strong electromagnetic interference, the system can still maintain a positioning accuracy of 10cm, significantly improving the navigation system's adaptability in complex environments.

[0068] 2. This invention employs an adaptive weight allocation mechanism, which can adjust the weights of each sensor in real time according to environmental changes, achieving optimal complementarity among sensors. This mechanism effectively avoids system-wide collapse due to the failure or quality degradation of a single sensor (such as satellite signals), ensuring the continuity of navigation tasks.

[0069] 3. This invention combines dual-frequency dual-difference processing technology with integer ambiguity fixing technology. By performing dual-difference processing on BeiDou observations, it can eliminate or significantly reduce systematic errors such as satellite clock error, receiver clock error, and atmospheric delay, further ensuring the high accuracy of navigation calculation. Attached Figure Description

[0070] Figure 1 This is a schematic diagram of the structure of the robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to the present invention.

[0071] Figure 2 This is a schematic diagram of the implementation process of the system of the present invention.

[0072] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0073] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0074] This invention proposes a multi-sensor fusion navigation system based on vision, inertial measurement unit (IMU) and satellite positioning system (BeiDou observation), which relates to the field of navigation and positioning technology, especially multi-sensor fusion navigation system, robust estimation method, adaptive data fusion technology and high-precision pose estimation application, and is suitable for navigation and positioning of devices such as UAVs, intelligent vehicles and mobile robots in complex environments.

[0075] To address the shortcomings of existing technologies, this invention proposes the following solutions:

[0076] 1. Robust estimation of observation information: The robust estimation principle is adopted to construct a multi-sensor observation information processing method based on equivalent weight function. By adaptively adjusting the observation weights, gross errors in the observation data are effectively suppressed, and the robustness of the system to abnormal observations is improved. For scenarios such as visual feature matching failure, abnormal IMU fluctuations, and satellite signal interruption, robust estimation can automatically reduce the weight of the corresponding observation to avoid contaminating the overall state estimation.

[0077] 2. Adaptive weight allocation: An adaptive weight allocation mechanism based on observation residuals (Mahathano distance) and model errors (covariance matrix) is designed to dynamically adjust the weights of each sensor in the fusion process; when the satellite signal quality deteriorates (e.g., HDOP value increases), the weights of vision and IMU are automatically increased; when visual features are insufficient (e.g., at night or when texture is missing), positioning continuity is maintained by relying on IMU and satellite positioning.

[0078] 3. Multi-sensor joint state modeling: The carrier pseudorange observation information from vision, IMU and satellite positioning system is unified into a nonlinear state space model. The state vector includes position, velocity, attitude, sensor errors (IMU zero bias, vision scale factor, satellite carrier double difference ambiguity), etc. The optimal state estimate is solved by robust extended Kalman filtering to improve the accuracy and stability of navigation.

[0079] like Figure 1 , 2 As shown, the present invention provides a robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou, comprising a data acquisition module, a preprocessing module, a robust estimation module, an adaptive fusion module and a navigation output module connected in sequence.

[0080] (1) Data acquisition module

[0081] The data acquisition module is used to acquire multi-source raw data and transmit it to the preprocessing module; wherein, the multi-source raw data includes visual image data (RGB / depth map), IMU raw data (acceleration, angular velocity) and BeiDou observation data (pseudorange, carrier phase, satellite ephemeris).

[0082] The data acquisition module connects to the visual sensor, inertial measurement unit (IMU), and BeiDou receiver, respectively, and is responsible for synchronously acquiring visual, kinematic, and absolute positioning information from the environment. This module connects to the core ARM processor via a hardware interface driver, achieving synchronous sampling from multiple sensors under a unified trigger signal. Specific implementation details include the following three sub-units:

[0083] ① Visual image acquisition unit: used to acquire texture and depth information of the environment.

[0084] Hardware configuration: An industrial-grade RGB-D camera is used, preferably the Intel RealSense D435i model in this embodiment. This camera supports a frame rate of 30FPS and an image resolution of 1280×720.

[0085] Data acquisition method: RGB images are transmitted via USB 3.0 interface, and the system software uses the OpenCV library for real-time capture. Depth maps are generated directly by the camera's built-in algorithm, with a measurement accuracy of ±2mm within 1 meter.

[0086] Synchronization mechanism: The visual sensor synchronizes with the IMU data through a hardware trigger signal to ensure that the timestamp consistency error between the two is less than 1ms.

[0087] ② Inertial Measurement Unit (IMU) Acquisition Unit: Used to acquire the high-frequency motion state of the carrier.

[0088] Hardware configuration: A six-axis IMU sensor is selected, which integrates a three-axis accelerometer and a three-axis gyroscope.

[0089] Sampling Frequency and Transmission: The sampling frequency is configured to 200Hz via the SPI interface, and the raw acceleration and angular velocity data are transmitted via I... 2 The data is transmitted to the processor via the C bus.

[0090] ③ Satellite positioning data acquisition unit: used to acquire absolute position reference and raw observation data.

[0091] Hardware configuration: Beidou receiver is used.

[0092] Data type: The collected data includes not only satellite ephemeris, but also pseudorange and carrier phase observations used for subsequent double-difference processing.

[0093] Sampling frequency: Set to 1Hz.

[0094] Synchronization mechanism: The PPS (pulses per second) signal is used to synchronize with the system clock to ensure that the time synchronization error is less than 100ns.

[0095] (2) Preprocessing module

[0096] The preprocessing module is used to extract and match features from the visual image data to calculate the camera pose, perform pre-integration processing on the IMU raw data to obtain the state recursion quantity, perform double-difference processing on the carrier and pseudorange in the BeiDou observation data to obtain double-difference observation values, and transmit the camera pose, state recursion quantity and double-difference observation values ​​as preprocessed observation information to the robust estimation module.

[0097] The preprocessing module receives visual images, raw IMU data, and BeiDou observation data output from the data acquisition module, and performs feature extraction, state derivation, and error elimination on these three types of data respectively, providing high-quality observation constraints for subsequent robust estimation and adaptive fusion. The processing flow of this module specifically includes the following three parts:

[0098] 1) Visual front-end processing: Extract salient feature points in the image through image processing algorithms, calculate the matching relationship between feature points, and then estimate camera motion.

[0099] a. Feature extraction: The input RGB image is preprocessed, and the ORB (Oriented FAST and Rotated BRIEF) feature extraction algorithm is used to detect significant corner points in the image as key points and calculate their corresponding descriptors.

[0100] b. Feature matching: Using the K-Nearest Neighbor (KNN) matching algorithm, the Hamming distance between the feature points in the current frame and the feature points in the previous frame is calculated, and the nearest neighbors are found for each feature point; then, the erroneous matching points are eliminated by the ratio test, and the best matching point pairs are selected.

[0101] c. Key point homogenization: In order to prevent feature points from being too dense in areas with rich image texture and missing feature points in areas with poor texture, this embodiment uses the octree method to spatially divide and manage the extracted feature points to ensure that the feature points are evenly distributed on the image plane.

[0102] d. Motion estimation: Based on the selected matching point pairs, the fundamental matrix F is calculated using the epipolar geometry principle, and then the essential matrix E is obtained by combining the camera intrinsic parameters; finally, singular value decomposition (SVD) is performed on the essential matrix E to solve for the relative rotation and translation of the camera between adjacent frames, and the camera pose estimate is obtained.

[0103] 2) IMU pre-integration processing: This involves processing high-frequency IMU data and converting it into low-frequency key inter-frame relative motion constraints, based on which system state prediction is performed. The specific implementation process is as follows:

[0104] a. IMU Raw Data Modeling: First, a mathematical model of the IMU's raw measurements is established. The IMU output measurements (including noise and zero bias) are represented as the true values ​​plus the error. Specifically, the measured acceleration... and angular velocity They are modeled as follows:

[0105]

[0106] Among them, C b→w Let a be the rotation matrix from the carrier to the world coordinate system. t w t For true acceleration and angular velocity, g w Let b be the gravity vector in the world coordinate system. a,t b g,t For the zero bias of the accelerometer and gyroscope (modeled as a random walk process), n a,t n g,t Gaussian white noise for IMU acceleration / angular velocity.

[0107] b. Pre-integration increment calculation (relative motion constraint): Between two keyframes (time k-1 to time k), the IMU measurements described above are used for recursive integration to calculate the relative motion increment, which depends only on the IMU data, including the position increment Δα. k-1,k Velocity increment Δβ k-1,k and attitude increment Δq k-1,k This process compresses high-frequency (e.g., 200Hz) IMU data between two frames into a low-frequency (e.g., 20Hz) relative motion constraint, thereby avoiding repeated integration in subsequent optimization iterations.

[0108] c. State Prediction: Using the optimal state estimate from the previous time step k-1 and the calculated pre-integral increment, the system state at the current time step k is predicted. The calculation formula includes recursions for position, velocity, attitude, and zero bias:

[0109]

[0110] Through this step, the system obtains a priori estimate of the state at the next time step, which serves as the time update benchmark for subsequent Kalman filtering.

[0111] d. Covariance propagation: While calculating the state prediction, the state transition matrix F is derived based on the IMU kinematic error model. k-1 The process noise covariance matrix Q k Using linearized recurrence formulas The posterior covariance P from the previous moment k-1 Propagation up to the current moment yields the predicted covariance. Among them, Q k This is a diagonal matrix whose elements are determined by the noise variances of the accelerometers and gyroscopes. This predictive covariance reflects the degree of uncertainty in the system's prediction of the current state.

[0112] Finally, the calculated predicted state vector and the predicted covariance matrix Together, they constitute the state recursion, which serves as the prior input for the observation update step in the subsequent robust extended Kalman filter (REKF).

[0113] 3) BeiDou carrier pseudorange double-difference processing: Differential technology is used to eliminate common errors and obtain high-precision satellite observation information.

[0114] This embodiment uses carrier phase and pseudorange observations from the BeiDou Satellite System (BDS). To eliminate systematic errors, a double difference (DD) model is constructed:

[0115] Inter-station difference calculation: First, the observations of the same satellite on different receivers (such as base station and rover station, or epoch difference) are calculated to eliminate satellite clock error and satellite orbit error.

[0116] Inter-satellite difference: The difference between the observations received by the same receiver from different satellites is then calculated to eliminate receiver clock errors.

[0117] Through the two differential operations described above, the generated double-difference observations significantly reduce the effects of ionospheric and tropospheric delays and are insensitive to clock errors, thus preserving centimeter-level precision relative position information as the system's observation input.

[0118] Finally, the preprocessing module transforms the raw multi-source sensor data into preprocessed observation information (camera pose, state recursion, and double-difference observations) with clear geometric constraints and statistical properties, and outputs it to the robust estimation module.

[0119] (3) Robust estimation module

[0120] The robust estimation module is used to predict the system state based on the state recursion, calculate the multi-sensor observation residuals of the camera pose and double-difference observations relative to the predicted system state, standardize the multi-sensor observation residuals, and use equivalent weight functions to calculate the corresponding robust equivalent weights to construct a robust weight matrix. The multi-sensor observation residuals and robust weight matrix are then transmitted to the adaptive fusion module.

[0121] The robust estimation module quantifies the observation residuals of each sensor and uses a robust weighting function to identify and suppress gross errors, thereby preventing anomalous observations from contaminating subsequent state estimations. The processing flow of this module specifically includes the following four steps:

[0122] 1) Multi-sensor observation residual calculation: Calculate the deviation between the "actual sensor observation" and the "expected observation based on IMU state recursion".

[0123] a. Visual Odometry (VO) observation residual calculation: Assume the pose of the camera output by the visual odometry in the world coordinate system is... and The state prediction value obtained by IMU pre-integration is combined with the extrinsic parameters of the camera and IMU (the mapping relationship is as follows). in, and Calculate the camera pose predicted by the IMU, taking the extrinsic parameters (translation and rotation) of the camera relative to the IMU. Visual residual r vo Includes positional residual r p,vo and attitude residual r θ,vo The calculation formula is as follows:

[0124]

[0125] Finally, the visual residual vector is obtained.

[0126] b. Calculation of BeiDou double-difference observation residuals: For the BeiDou system, calculate the double-difference pseudorange residuals r separately. ρ and double-difference carrier phase residual r φ The residual is defined as the observed value minus the expected geometric distance calculated based on the IMU-predicted location, and the calculation formula is as follows:

[0127]

[0128] in, These are double-difference pseudorange observations. This refers to the double difference in distance between satellites and ground stations (double difference between two BeiDou satellites and between stations). To observe noise; These are double-difference carrier observations, where λ is the carrier wavelength. For double-difference integer ambiguity, The predicted satellite-to-ground distance is double-differenced. The final BeiDou residual vector is obtained. (N is the number of satellite pairs.)

[0129] 2) Residual Standardization: Due to the different dimensions of position (meters), attitude (radians), and carrier phase (cycles), standardization is required. For each residual component r obtained from the above calculations... i Divide by its corresponding standard deviation σ i (from the sensor covariance matrix ∑) i (Obtained by taking the square root of the diagonal element), the dimensionless standardized residual ε is obtained.i :

[0130]

[0131] 3) IGG3 robustness weight function calculation: The core of the robustness factor is to apply a nonlinear weight function to the standardized residuals, assigning a weight of 1 to normal residuals and reducing the weight of outlier residuals according to their degree. The IGG3 (Institute of Geodesy and Geophysics III) robustness algorithm is used, which employs a three-stage weight function design:

[0132]

[0133] Where k0 is the normal residual threshold, which is generally set to 2; k1 is the abnormal residual threshold, which is generally set to 3 to 5. The robust weight of a single residual element.

[0134] This function defines three intervals:

[0135] Protecting rights area (|ε i |≤k0): The observation is considered normal, and the original weight (w=1) is retained. In this embodiment, k0 is preferably set to 2.

[0136] Reduced weight region (k0<|ε i |≤k1): The observation is considered suspicious, and the weight is reduced non-linearly according to the error magnitude to achieve "partial trust".

[0137] Rejection zone (|ε i |>k1): If there is a serious gross error, the weight is reset to zero (w=0), and the observation is directly removed.

[0138] 4) Sensor Differentiated Threshold Configuration and Matrix Construction: To adapt to the error characteristics of different sensors, this embodiment sets differentiated abnormal threshold k1:

[0139] IMU observation: k1 is set to 3; visual observation: k1 is set to 4; BeiDou observation: k1 is set to 5.

[0140] Finally, the calculated weights w of each component are used. i Construct a diagonal robust weight matrix W, including the visual weight matrix W VO and Beidou weight matrix W BDS W IMU =diag(w1,w2,...,w9), W VO =diag(w1,w2,...,w6), W BDS =diag(w1,w2,...,w 2N The matrix is ​​then transmitted to the subsequent adaptive fusion module.

[0141] (4) Adaptive fusion module

[0142] The adaptive fusion module is used to adaptively adjust the observation noise covariance matrix based on the statistical characteristics and robust weight matrix of the multi-sensor observation residuals, calculate the Kalman gain using the adjusted observation noise covariance matrix, and correct the system state in combination with the multi-sensor observation residuals to obtain the optimal state estimate, which is then transmitted to the navigation output module.

[0143] The adaptive fusion module, based on the robust extended Kalman filter (REKF) framework, is responsible for optimally fusing predicted information with multi-source observation information. The module's processing flow follows a sequence of "state definition—time update—model building—adaptive adjustment—measurement update," specifically including the following five steps:

[0144] 1) System State Vector Construction: To achieve tight coupling of multi-sensor information, this embodiment first defines a joint state vector X within the robust extended Kalman filter (REKF) framework. This state vector not only includes the motion state of the carrier but also the key error parameters of each sensor, as specifically defined below:

[0145]

[0146] in:

[0147] The position of the IMU (machine body) in the world coordinate system;

[0148] The velocity of the IMU in the world coordinate system;

[0149] The attitude of the IMU relative to the world coordinate system is represented here using axis-angle instead of quaternion representation to avoid dimensional redundancy in the covariance matrix;

[0150] The zero biases of the IMU gyroscope and accelerometer are respectively modeled as a random walk process;

[0151] The scale factor of visual odometry (VO) is used to compensate for the lack of absolute scale information in monocular vision.

[0152] The double-difference carrier integer ambiguity of the BeiDou system (BDS), where n is the number of double-difference observation pairs.

[0153] 2) System state prediction (time update): driven by IMU data, serving as the prediction component of the Kalman filter.

[0154] One-step state prediction: Utilizing the IMU's state transition function f(·), combined with the posterior state X from the previous time step. k and the current raw IMU measurement value μ k (After preprocessing), calculate the prior state estimate X at the current time. k+1|k :

[0155] X k+1|k =f(X) k ,μ k )

[0156] Covariance prediction: Based on the IMU error propagation model, update the prediction error covariance matrix P. k+1|k :

[0157]

[0158] in, The state transition Jacobian matrix; For the noise transfer matrix (w) k (For IMU Gaussian white noise); ∑ IMU This is the original noise covariance matrix of the IMU (including the measurement noise variance of the gyroscope and accelerometer).

[0159] 3) Construction of joint observation model: Before entering the adaptive update, the module needs to unify the observation information of different sensors into the same observation space.

[0160] Merging observation vectors: Merging the visual (VO) observation vector Z V0 and BeiDou (BDS) observation vector Z BDS Concatenate the data to construct the joint observation vector Z:

[0161]

[0162] Constructing the observation Jacobian matrix: Calculate the partial derivatives of the visual observation function and the BeiDou observation function with respect to the state vector X, and construct the joint observation Jacobian matrix H:

[0163]

[0164] in,

[0165] Calculate the original residual vector: Subtract the value based on the predicted state X from the joint observation vector Z. k+1|k Calculated observation prediction value HX k+1|k The original residual vector r is obtained:

[0166]

[0167] 4) Adaptive adjustment of observation noise covariance: This is performed after the observation data is obtained, and the filter parameters are dynamically adjusted according to the observation quality.

[0168] Statistical characteristic test: Calculate the normalized sum of squares statistic of residuals (Mahanobis distance) χ² 2 It is used to measure the consistency between current observations and predicted values:

[0169] χ 2 =r T HP k+1|k H T +R) -1 r

[0170] Where R is the initial observation noise covariance matrix; the significance level α is selected (preferably α = 0.05 in this embodiment), and the corresponding quantile values ​​are determined according to the chi-square distribution table. The threshold is used for adaptive testing.

[0171] Adaptive covariance adjustment: χ² 2 With chi-square distribution threshold By comparing the results and combining the robust weight matrix W input from the robust estimation module, the adjusted observation noise covariance matrix is ​​calculated.

[0172]

[0173] when When the model is well-matched, use the coefficients. Reduce the covariance and increase the weight of the observation.

[0174] when Time (model matching poor / outlier), using coefficients And combined with robust weight matrix W -1 Increase the covariance and decrease the weight of this observation. The final result is:

[0175]

[0176] Among them, R adapt This is the adaptive adjustment coefficient matrix calculated above, where ⊙ indicates the adjustment of the corresponding element.

[0177] 5) Kalman state update: using the adjusted Complete the standard EKF update process and output the final navigation result.

[0178] Calculate the Kalman gain:

[0179]

[0180] The adjusted version is used here. This allows the gain K to automatically adapt to the current observation quality.

[0181] Update the system state: Use Kalman gain to correct the predicted state and obtain the optimal state estimate X at the current time. k+1 :

[0182] X k+1 =X k+1|k +K r

[0183] The state vector contains core navigation parameters such as position, velocity, attitude, and sensor errors (IMU bias, visual scale, and double-difference ambiguity).

[0184] Updated posterior covariance: Calculate the updated state estimation error covariance matrix P. k+1 This provides a benchmark for predictions at the next moment:

[0185]

[0186] Through the above steps, the adaptive fusion module can automatically adjust the fusion weights of each sensor in complex environments such as degraded satellite signal quality (e.g., increased HDOP) or insufficient visual features, ensuring that the navigation system always maintains high accuracy and high robustness.

[0187] (5) Navigation output module

[0188] The navigation output module is used to parse the optimal state estimate into navigation state parameters that include position, velocity, attitude and sensor error parameters, and then output them.

[0189] The navigation output module converts the optimal state estimate X obtained from the Kalman filter solution into a value. k+1 The system performs analysis, coordinate transformation, and formatting to output high-frequency, high-precision full-scale navigation information to external control systems (such as UAV flight controllers and robot path planners). Specific implementation details are as follows:

[0190] ① Navigation State Parameter Parsing: The navigation output module receives the system state vector X output by the adaptive fusion module and parses it into the following three types of key information:

[0191] a. Kinematic state information:

[0192] High-precision positioning The output carrier's three-dimensional coordinates (x, y, z) in the world coordinate system. This position integrates the absolute positioning information from BeiDou and the relative displacement information from the visual / IMU, achieving centimeter-level accuracy when satellite signals are good.

[0193] Speed ​​information Output the three-dimensional velocity vector of the carrier in the world coordinate system.

[0194] Attitude information Although the internal state of the filter is represented using axis angles to optimize calculations, this module converts it to quaternion or Euler angles (roll, pitch, yaw) format at the output stage to adapt to the interface requirements of general-purpose controllers.

[0195] b. Sensor error and calibration information:

[0196] IMU zero-biased estimate (b) g ,b a ): Real-time output of zero-bias estimates from the gyroscope and accelerometer. These values ​​reflect the current drift state of the IMU and can be used for IMU error compensation in subsequent time steps, suppressing accumulated errors generated during long-term operation.

[0197] Visual scale factor (s) v ): Outputs the scale correction coefficients for the vision system. This parameter is used to monitor the scale consistency of the visual odometry, ensuring that the trajectory calculated by monocular vision is consistent with the scale of the real physical world.

[0198] Double-difference integer ambiguity (N) dd ): Outputs the double-difference ambiguity floating-point solution or fixed solution for the BeiDou carrier phase. This parameter is an important indicator for evaluating RTK positioning quality (such as whether a fixed solution is achieved).

[0199] ② Output and Application: The parsed navigation information is transmitted at a fixed frequency through a standard communication interface (such as a serial port, CAN bus, or network interface). The output data is used not only for the system's closed-loop feedback (as a benchmark for predicting the state at the next moment) but also directly supplied to external devices for decision-making.

[0200] Real-time control: Utilize the output high-frequency attitude and velocity data to achieve attitude stabilization control of drones or robots.

[0201] Path planning: Utilizes the output high-precision location data to perform accurate trajectory tracking and obstacle avoidance in complex environments (such as canyons and tunnels).

[0202] The navigation output module transforms complex internal state estimates into intuitive and usable physical quantities, achieving a complete conversion from multi-source raw data to high-precision navigation results.

[0203] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, apparatus, article, or method that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, apparatus, article, or method. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, apparatus, article, or method that includes that element.

[0204] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou, characterized in that, It includes a data acquisition module, a preprocessing module, a robust estimation module, an adaptive fusion module, and a navigation output module connected in sequence; The data acquisition module is used to acquire multi-source raw data and transmit it to the preprocessing module; wherein, the multi-source raw data includes visual image data, IMU raw data and BeiDou observation data; The preprocessing module is used to extract and match features from the visual image data to calculate the camera pose, perform pre-integration processing on the IMU raw data to obtain the state recursion, perform double-difference processing on the carrier and pseudorange in the BeiDou observation data to obtain double-difference observations, and transmit the camera pose, state recursion and double-difference observations as preprocessed observation information to the robust estimation module. The robust estimation module is used to predict the system state based on the state recursion, calculate the multi-sensor observation residuals of the camera pose and double-difference observations relative to the predicted system state, standardize the multi-sensor observation residuals, and use the equivalent weight function to calculate the corresponding robust equivalent weights to construct a robust weight matrix. The multi-sensor observation residuals and robust weight matrix are then transmitted to the adaptive fusion module. The adaptive fusion module is used to adaptively adjust the observation noise covariance matrix based on the statistical characteristics and robust weight matrix of the multi-sensor observation residuals, calculate the Kalman gain using the adjusted observation noise covariance matrix, and correct the system state in combination with the multi-sensor observation residuals to obtain the optimal state estimate, which is then transmitted to the navigation output module. The navigation output module is used to parse the optimal state estimate into navigation state parameters that include position, velocity, attitude and sensor error parameters, and then output them.

2. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou as described in claim 1, characterized in that, The data acquisition module includes a visual image acquisition unit, an IMU acquisition unit, and a satellite positioning data acquisition unit, which are respectively connected to a visual sensor, an inertial measurement unit (IMU), and a BeiDou receiver, for synchronously acquiring visual image data, IMU raw data, and BeiDou observation data; The visual image acquisition unit uses an RGB-D camera to capture RGB images in real time and generate depth maps as visual image data; The IMU acquisition unit adopts a six-axis inertial measurement unit, which includes a three-axis accelerometer and a three-axis gyroscope, and is used to transmit the acquired acceleration and angular velocity raw data as IMU raw data through the bus; The satellite positioning data acquisition unit is used to collect pseudorange, carrier phase and satellite ephemeris data of BeiDou satellites as BeiDou observation data.

3. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 2, characterized in that, The specific processing steps of the preprocessing module include: The visual image data preprocessing involves: extracting image feature points using the ORB algorithm, filtering feature matching point pairs using the K nearest neighbor algorithm and ratio test, and performing spatial homogenization processing on the feature points using the octree method; calculating and decomposing the essential matrix based on the epipolar geometry principle to obtain the relative rotation and translation of the camera between adjacent frames, thereby obtaining the camera pose. The IMU raw data preprocessing involves: performing pre-integration processing to integrate and compress the high-frequency acceleration and angular velocity measurements between two keyframes, calculating the position increment, velocity increment, and attitude increment relative to the previous keyframe; based on the optimal system state estimate at the previous moment, superimposing the position increment, velocity increment, attitude increment, and gravity effect, recursively calculating the predicted state vector at the current moment; simultaneously, constructing a state transition matrix based on the IMU kinematic error model, and propagating the posterior covariance of the previous moment to the current moment to obtain the predicted covariance matrix; the predicted state vector and the predicted covariance matrix together constitute the state recursion quantity. The BeiDou observation data preprocessing involves: constructing a double-difference observation model of carrier phase and pseudorange; firstly, performing inter-station difference calculation on the observation values ​​of different receivers on the same satellite to eliminate satellite clock errors and orbital errors; then, performing inter-satellite difference calculation on the observation values ​​of different satellites on the same receiver to eliminate receiver clock errors; thereby generating the double-difference observation values ​​that are insensitive to clock errors and atmospheric delay errors.

4. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 3, characterized in that, The specific process of preprocessing the raw IMU data is as follows: The measured acceleration and angular velocity They are modeled as follows: Among them, C b→w Let a be the rotation matrix from the carrier to the world coordinate system. t w t For true acceleration and angular velocity, g w Let b be the gravity vector in the world coordinate system. a,t b g,t For zero bias of the accelerometer and gyroscope, n a,t n g,t Gaussian white noise for IMU acceleration / angular velocity; Between time k-1 and time k, the relative motion increment, including the position increment Δα, is calculated by recursively integrating the IMU measurements. k-1,k velocity increment Δβ k-1,k and attitude increment Δq k-1,k ; Using the optimal state estimate from the previous time k-1 and the calculated pre-integral increment, the system state at the current time k is predicted. The calculation formula includes recursion of position, velocity, attitude, and zero bias: While calculating the state prediction, the state transition matrix F is derived based on the IMU kinematic error model. k-1 The process noise covariance matrix Q k Using linearized recursive formulas The posterior covariance P from the previous moment k-1 Propagation up to the current moment yields the predicted covariance. The calculated predicted state vector and the predicted covariance matrix Together they constitute the state recursion quantity.

5. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 1, characterized in that, The specific processing steps of the robust estimation module include: Step 1) Using the optimal system state estimate from the previous moment, superimpose the IMU pre-integrated state recursion to calculate the predicted system state at the current moment; based on the predicted system state, calculate the expected visual and BeiDou observations at the current moment using the camera observation model and satellite geometry; subtract the camera pose from the corresponding expected observation to obtain the visual observation residual, which includes position and attitude residuals; subtract the double-difference observation output from the BeiDou double-difference processing from the corresponding expected observation to obtain the BeiDou double-difference observation residual, which includes double-difference pseudorange residuals and double-difference carrier residuals. Step 2) Obtain the square root of the diagonal element of the covariance matrix of the observation noise of each sensor as the standard deviation. Divide each component of the visual observation residual and the Beidou double-difference observation residual by its corresponding standard deviation to obtain the dimensionless standardized residual ε. i ; Step 3) Calculate the robust equivalent weights of each observation component using the IGG3 three-segment robust model. The calculation formula is as follows: Where k0 is the normal residual threshold and k1 is the abnormal residual threshold; when the absolute value of the standardized residual is less than or equal to k0, the original weights are maintained; when the absolute value of the standardized residual is between k0 and k1, the weights are reduced proportionally; when the absolute value of the standardized residual is greater than k1, the weights are reset to 0. Step 4) Configure different abnormal residual thresholds k1 for different sensors: k1 is set to 3 for IMU observation, k1 is set to 4 for visual observation, and k1 is set to 5 for Beidou observation; the normal residual threshold k0 is uniformly set to 2; based on the calculated robust equivalent weights of each observation component, construct a diagonal robust weight matrix and transmit it to the adaptive fusion module.

6. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 5, characterized in that, In step 1) of the robust estimation module Visual Odometry (VO) observation residual calculation: Assume the pose of the camera in the world coordinate system output by the visual odometry is... and The state prediction values ​​obtained by IMU pre-integration, combined with the extrinsic parameters of the camera and IMU, have the following mapping relationship: Calculate the camera pose predicted by the IMU and the visual residual r. vo Includes positional residual r p,vo and attitude residual r θ,vo The calculation formula is as follows: Finally, the visual residual vector is obtained. BeiDou double-difference observation residual calculation: Calculate the double-difference pseudorange residual r separately. ρ and double-difference carrier phase residual r φ The residual is defined as the observed value minus the expected geometric distance calculated based on the IMU-predicted location, and the calculation formula is as follows: in, These are double-difference pseudorange observations; These are double-difference carrier observations, where λ is the carrier wavelength; For double-difference integer ambiguity, The predicted satellite-to-ground distance is double-differenced; finally, the satellite residual vector is obtained. N represents the number of satellite pairs.

7. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 1, characterized in that, The specific processing steps of the adaptive fusion module include: Step 1) Define the system's state vector X as: in, and These represent the IMU's position and velocity in the world coordinate system. Let b be the axis-angle attitude of the IMU relative to the world coordinate system. g and b a The zero bias of the IMU gyroscope and accelerometer, respectively, s v N is the scale factor for visual odometry. dd For the double-difference carrier integer ambiguity of the BeiDou system; Step 2) Using the IMU kinematic equations, combined with the posterior state estimate from the previous time step and the raw IMU data from the current time step, the prior state estimate for the current time step is recursively calculated; based on the IMU error propagation model, the state transition Jacobian matrix and noise propagation matrix are constructed, and the posterior covariance matrix from the previous time step is propagated to the current time step to obtain the prediction error covariance matrix. Step 3) Concatenate the visual odometry pose and BeiDou double-difference observation values ​​output by the preprocessing module to construct a joint observation vector; calculate the partial derivatives of the visual observation equation and the BeiDou double-difference observation equation with respect to the state vector to construct a joint observation Jacobian matrix; subtract the observation prediction value calculated based on the prior state estimate from the joint observation vector to obtain the joint observation residual vector. Step 4) Calculate the normalized sum of squares statistic χ using the joint observation residual vector, the joint observation Jacobian matrix, and the prediction error covariance matrix. 2 Set the chi-square distribution test threshold, and set the χ² value... 2 Compare with this threshold: if χ 2 If the value is less than the threshold, the initial observation noise covariance matrix is ​​reduced using an adjustment coefficient; if χ 2 If the value is greater than or equal to the threshold, the initial observation noise covariance matrix is ​​amplified using an adjustment coefficient and then weighted and adjusted in conjunction with the robust weight matrix input from the robust estimation module; thus obtaining the adjusted observation noise covariance matrix. Step 5) Using the prediction error covariance matrix, the joint observation Jacobian matrix, and the adjusted observation noise covariance matrix. Calculate the Kalman gain; use the Kalman gain and the joint observation residual vector to correct the prior state estimate, and output the optimal state estimate at the current time; update the state estimate error covariance matrix as the benchmark for prediction at the next time step.

8. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 7, characterized in that, Step 3 of the adaptive fusion module specifically includes: Merging observation vectors: Merging the visual VO observation vector Z V0 and BeiDou BDS observation vector Z BDS Concatenate the data to construct the joint observation vector Z: Constructing the observation Jacobian matrix: Calculate the partial derivatives of the visual observation function and the BeiDou observation function with respect to the state vector X, and construct the joint observation Jacobian matrix H: in, Calculate the original residual vector: Subtract the value based on the predicted state X from the joint observation vector Z. k+1|k Calculated observation prediction value HX k+1|k The original residual vector r is obtained:

9. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 7, characterized in that, Step 5 of the adaptive fusion module specifically includes: Calculate the Kalman gain: Among them, the adjusted This allows the gain K to automatically adapt to the current observation quality; Update the system state: Use Kalman gain to correct the predicted state and obtain the optimal state estimate X at the current time. k+1 : X k+1 =X k+1|k +Kr This state vector contains core navigation parameters, including position, velocity, attitude, and sensor errors. Updated posterior covariance: Calculate the updated state estimation error covariance matrix P. k+1 This provides a benchmark for predictions at the next moment:

10. The robust estimation adaptive fusion navigation system based on vision-IMU-BeiDou according to claim 1, characterized in that, The specific processing steps of the navigation output module include: The system receives the optimal state estimate output by the adaptive fusion module; analyzes the optimal state estimate to extract kinematic state parameters containing the three-dimensional position, three-dimensional velocity, and attitude information of the carrier in the world coordinate system; analyzes the optimal state estimate to extract sensor error parameters including IMU gyroscope bias, IMU accelerometer bias, visual odometry scale factor, and BeiDou dual-difference carrier integer ambiguity; and sends the kinematic state parameters and the sensor error parameters to an external control system via a communication interface for real-time navigation control or path planning.

Images