Multi-mode adaptive fusion navigation positioning method and system in satellite navigation denial environment

Abstract

The invention discloses a multi-mode adaptive fusion navigation positioning method and system in a satellite navigation denial environment, the system adopts GNSS / IMU / visual sensors, and the method comprises the following steps: receiving data of the three sensors and completing modeling; initializing the three sensors; judging the strength of the GNSS signal by utilizing chi-square test; optimization variables are set, a residual matrix is constructed, and nonlinear optimization is carried out; setting a GNSS trust factor when the GNSS signal is weak or denial, and calculating a weight; and weighting and fusing sensor data, outputting optimal state estimation of the system, and realizing continuous navigation and positioning. According to the method, a self-adaptive weight adjustment mechanism is established, GNSS trust factors are introduced, and the weights of multi-sensor data are adjusted in a self-adaptive manner, so that the positioning difficulty under the conditions that the GNSS is subjected to deception interference, suppression interference, building blocking and the like can be overcome, and continuous navigation positioning of an unmanned system (such as a robot, an unmanned vehicle, an unmanned aerial vehicle, an unmanned ship, an unmanned underwater vehicle and the like) is realized.

Description

Technical Field

[0001] The present invention relates to the field of unmanned system navigation control, and in particular to a navigation positioning method and system with multi-modal adaptive fusion in a satellite navigation denied environment. Background Art

[0002] With the rapid development of unmanned systems (such as robots, unmanned vehicles, drones, unmanned boats, unmanned submarines, etc.), positioning and navigation technology based on multi-source sensor fusion has become the key to high-precision autonomous navigation of unmanned systems. The high-precision positioning service provided by GNSS is widely used in various unmanned systems, but in scenarios where GNSS signals are weak or denied (such as urban canyons, tunnels, bad weather, etc.), the system cannot continue to navigate stably with high precision.

[0003] There are two main navigation and positioning technologies that are currently being studied to resist GNSS denial: (1) using deep learning technology to train a neural network model when the GNSS signal is good, and when the GNSS signal is missing, the neural network model replaces the GNSS signal for output; (2) using multi-source sensor fusion technology to remove the GNSS signal when the GNSS signal is missing, build a GNSS-free navigation subsystem, and continue to complete the navigation task. IMU has the advantage of being independent of external signals, but there is a problem of cumulative errors; visual sensors can provide environmental perception and local positioning information, but their accuracy is easily affected by conditions such as lighting and environment. Therefore, multi-source information fusion technology based on GNSS / IMU / visual sensors has become an important means to improve the navigation capabilities of unmanned systems in complex environments.

[0004] Traditional GNSS / IMU / vision fusion methods generally use fixed weight configurations to process the fusion of multi-source information. This method fails to effectively adapt to dynamically changing environmental conditions. In the case of GNSS signal attenuation or denial, the fixed configuration of weights may not be able to adapt to the dynamically changing environment and changes in sensor performance, thereby reducing the overall navigation accuracy. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides a navigation and positioning method and system with multi-modal adaptive fusion in a satellite navigation denied environment. By establishing an adaptive weight adjustment mechanism and introducing a GNSS trust factor, the weight values ​​of multi-sensor data are adaptively adjusted. This can overcome the positioning difficulties caused by GNSS being deceptively interfered with, suppressed interfered with, and blocked by buildings, and achieve long-term high-precision navigation and positioning.

[0006] To achieve the above object, the technical solution of the present invention is as follows:

[0007] A navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment comprises the following steps:

[0008] Step 1: Mount the GNSS signal receiving module, IMU and visual sensor on the unmanned system, receive data, and establish measurement models of the GNSS signal receiving module, IMU and visual sensor respectively;

[0009] Step 2: Initialize and preprocess the data of the GNSS signal receiving module, IMU and visual sensor according to the established measurement model;

[0010] Step 3: Check whether the GNSS signal is abnormal. If the GNSS signal is normal, set the optimization variables according to the preprocessed data information, build the residual matrix according to the residual vectors of the three sensors, perform nonlinear optimization, minimize the residual vector of each sensor, determine the weight value of each sensor data according to the optimized residual vector, perform weighted fusion on the state vector, and obtain the optimal state estimation of the system;

[0011] Step 4: In the case of GNSS signal attenuation and denial, establish a GNSS trust factor, adaptively adjust the weight values ​​of the three sensor data, establish a navigation subsystem based on IMU and visual sensors, obtain the optimal state estimation of the system, and perform iterative updates for continuous high-precision navigation.

[0012] In the above scheme, in step 1, the measurement model of the GNSS signal receiving module is as follows:

[0013]

[0014] in, They represent the three-dimensional coordinate information of the unmanned system in the world coordinate system w after the GNSS signal receiving module solves it, and b g (t) represents the distance deviation vector related to the clock of the GNSS signal receiving module at time t;

[0015] The measurement model of IMU is as follows:

[0016]

[0017] in, Represents the three-dimensional acceleration vector of the unmanned system output by the accelerometer, a t represents the real acceleration, represents the zero bias of the accelerometer, represents the compensation for the Earth's gravitational acceleration, Represents the direction cosine matrix from the world coordinate system to the navigation coordinate system at time t, g w represents the gravity vector in the world coordinate system, represents the Gaussian noise of the accelerometer; ω tRepresents the three-dimensional angular velocity information of the unmanned system output by the gyroscope, ω t represents the true angular velocity, represents the zero bias of the gyroscope, represents the Gaussian noise of the gyroscope;

[0018] The measurement model of the visual sensor is as follows:

[0019]

[0020] R v =R v ·exp(δθ × )

[0021]

[0022] Among them, X w represents the three-dimensional coordinates of the feature point in the world coordinate system, K and [RT] represent the internal and external parameter matrices of camera calibration, respectively. Represents the pixel coordinates of the output feature points, z v Represents the actual pixel coordinates of feature points, represents Gaussian noise; R v Represents the relative rotation matrix of the two frames of image solution, R v represents the real rotation matrix, exp(δθ × ) represents the disturbance noise, obeying Represents the relative translation vector of the two frames of image solution, t v is the real translation vector, represents Gaussian noise.

[0023] In the above scheme, the specific method of step 2 is as follows:

[0024] (1) According to the measurement model of the GNSS signal receiving module, GNSS and IMU integration are performed to initialize the IMU to obtain a rough IMU bias and absolute attitude estimate, so that the absolute attitude is aligned with the local coordinate system;

[0025] (2) Perform pre-integration processing on the IMU data to obtain the pre-integration items of the IMU, which are Its specific form is as follows:

[0026]

[0027] in, Represents the coordinate system of the body from the current time to t k The rotation matrix at time, ω t They represent the acceleration vector output by the accelerometer and the angular velocity vector output by the gyroscope, respectively. Represent the zero bias of the accelerometer and gyroscope, n a 、n w represent the Gaussian noise of the accelerometer and gyroscope respectively;

[0028] (3) According to the measurement model of the visual sensor, a standard SfM solution is performed to obtain the rotation matrix R and translation vector t of each frame camera;

[0029] (4) Calibrate the relative rotation matrix between IMU and vision sensor

[0030] In the above scheme, in step 3, the optimization variables set are as follows:

[0031]

[0032] Among them, x k is the estimated parameter state at each time node, including the position of the unmanned system output by the IMU speed and posture Gyroscope bias and accelerometer bias Unmanned system position output by GNSS signal receiving module Clock distance deviation n is the number of time nodes in the set window; is the external reference between the camera frame and the IMU, is the inverse depth parameter of the landmark in its first observed keyframe.

[0033] In the above scheme, in step 3, the residual matrix is ​​constructed according to the residual vectors of the three sensors as follows:

[0034] The residual vector of IMU is as follows:

[0035]

[0036] in, δb a ,δb g Represents the position, velocity, attitude, accelerometer bias, and gyroscope bias residual of the IMU respectively. Represents the rotation matrix from the world coordinate system to the body coordinate system at time k, Represent the position, speed, and posture at time k+1 respectively. Represent the position, speed, and posture at time k+1 respectively, g w Represents the gravity compensation in the world coordinate system, are the state prediction values ​​at time k+1, are the zero bias of the accelerometer and gyroscope at time k+1, are the zero bias of the accelerometer and gyroscope at time k;

[0037] The residual vector of the visual sensor is as follows:

[0038]

[0039] in, is the projection function of the rear camera; b1 and b2 are two orthogonal bases;

[0040] The residual vector of the GNSS signal receiving module is as follows:

[0041]

[0042] in, is the measured value, is the predicted value, The rotation matrix from the body coordinate system to the world coordinate system at time k, is the lever arm relative to the IMU body coordinate system;

[0043] According to the residual vectors of the three sensors, the residual matrix is ​​constructed to minimize the residuals of all sensors and minimize the sum of the prior and Mahalanobis norms of all measurement residuals to obtain the maximum a posteriori estimate. The formula is as follows:

[0044]

[0045] Among them, r p -H p X is the prior information obtained by marginalizing the system, is the measurement residual of IMU, is the measurement residual of the visual sensor, is the residual of the GNSS measurement value, ρ is the outlier suppression function, and its principle formula is as follows:

[0046]

[0047] In the above scheme, in step 3, the residual matrix is ​​optimized, assuming that the final residual is The weight factor of each sensor is constructed using the residual. Assuming that the residual conforms to the Gaussian distribution, the mean and standard deviation of the residual of each sensor are first calculated:

[0048]

[0049] Among them, N, M, and P are the total amount of data of each sensor in the optimization period;

[0050] The residual normalization process is as follows:

[0051]

[0052] in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period;

[0053] The normalized residual is used to calculate the weights of the three sensors, and the formula is as follows:

[0054]

[0055] in, is the normalized residual of the corresponding sensor, and ε is set to a small positive constant;

[0056] The weights are normalized to ensure that the sum of the weights is 1. The formula is as follows:

[0057]

[0058] Among them, w i is the normalized weight coefficient, It is the sum of the weights of the three sensors.

[0059] In the above scheme, in step 3, the optimized sensor data is weighted fused to obtain the optimal state estimation of the system:

[0060]

[0061] Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. They are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization.

[0062] In the above scheme, the specific method of step 4 is as follows:

[0063] (1) According to the selected significance level and the critical value of the chi-square distribution, when λ≤λ0, it indicates that the GNSS signal is normal, when λ0<λ<2λ0, it indicates that the GNSS signal is weak, and when λ≥2λ0, it indicates that the GNSS signal is interrupted. The trust factor calculation formula is as follows:

[0064]

[0065] Among them, n is a positive constant, which is used as a tuning parameter. The larger n is, the lower the trust in GNSS signal attenuation is.

[0066] (2) Use the GNSS trust factor to further update the weights of the three sensors. The formula is as follows:

[0067]

[0068] in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period;

[0069] Normalize the updated weights to ensure that the sum of the weights is 1. The formula is as follows:

[0070]

[0071] (3) Substitute the updated weight factors to form a navigation subsystem based on IMU and visual sensors. Perform weighted fusion on the optimized data of each sensor to obtain the optimal state estimation of the system:

[0072]

[0073] Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. They are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization;

[0074] The fusion obtains the optimal state estimate of the system and performs continuous iterative calculations to achieve high-precision continuous navigation when GNSS signals are weak or interrupted.

[0075] A navigation and positioning system with multi-modal adaptive fusion in a satellite navigation denial environment, comprising a sensor module, a GNSS signal monitoring module, an FPGA data processing module and an industrial computer output and control module mounted on an unmanned system;

[0076] The sensor module includes a GNSS receiving module, an IMU and a visual sensor. The GNSS receiving module is responsible for obtaining the positioning information of the global navigation satellite system and providing high-precision positioning when the signal is normal; when the signal is weak or denied, it serves as an auxiliary signal source; the IMU includes an accelerometer and a gyroscope, which is responsible for measuring the acceleration and angular velocity of the unmanned system and providing high-frequency posture information independent of external signals; the visual sensor is responsible for capturing images of the surrounding environment and providing environmental perception and local positioning data;

[0077] The GNSS signal monitoring module: uses a chi-square test-based method to evaluate the strength and quality of the GNSS signal in real time, outputs a GNSS trust factor for subsequent weight factor calculation; reduces the GNSS weight value by the GNSS trust factor in the case of signal attenuation or rejection;

[0078] The FPGA data processing module uses the parallel processing capability of the FPGA to process the data of each sensor in real time and efficiently, integrates the multi-modal sensor data through an adaptive weighted fusion algorithm, and generates a pose estimation result of the unmanned system;

[0079] The industrial computer output and control module is responsible for receiving the output data of the FPGA data processing module, outputting the final navigation status information in real time, making decisions based on the navigation status information, and controlling the movement of the unmanned system in real time.

[0080] Through the above technical solution, the navigation and positioning method and system for multi-modal adaptive fusion in a satellite navigation denied environment provided by the present invention have the following beneficial effects:

[0081] 1. Improve the dynamic adaptability of the system

[0082] The present invention introduces an adaptive weight adjustment mechanism and GNSS trust factor, and uses the optimized residuals of each sensor to distribute weights. The weights of each sensor can be dynamically adjusted according to real-time sensor performance and environmental changes. This dynamic adaptability ensures the optimal state estimation of the system under different environmental conditions (such as when the GNSS signal is weak or denied), and improves the flexibility and response capability of the system.

[0083] 2. Enhance system stability

[0084] The present invention introduces a GNSS trust factor when the GNSS signal is weakened or blocked by suppression interference, deception interference, or building obstruction. This allows the system to reasonably adjust the weight distribution and appropriately increase the reliance on IMU and visual sensors when the GNSS signal quality is reduced. This method effectively improves the navigation accuracy in complex environments and ensures that the unmanned vehicle can maintain reliable positioning and navigation capabilities when the GNSS is unstable.

[0085] 3. Optimize fusion effect and improve positioning accuracy

[0086] The present invention dynamically adjusts the weights of sensors so that the system can perform weighted averaging according to the residual characteristics of each sensor, thereby avoiding excessive influence of a certain sensor on the final result and improving the positioning accuracy after fusion. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art are briefly introduced below.

[0088] Figure 1 A flowchart of a navigation and positioning method for multi-modal adaptive fusion in a satellite navigation denied environment disclosed in an embodiment of the present invention;

[0089] Figure 2 A schematic diagram of weight calculation disclosed in an embodiment of the present invention;

[0090] Figure 3 A schematic diagram of a navigation and positioning system with multi-modal adaptive fusion in a satellite navigation denied environment disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0091] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0092] The present invention provides a navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment, such as Figure 1 As shown, the following steps are included:

[0093] Step 1: Mount the GNSS signal receiving module, IMU and visual sensor on the unmanned system, receive data, and establish measurement models of the GNSS signal receiving module, IMU and visual sensor respectively.

[0094] GNSS provides global absolute position information. The measurement model of the GNSS signal receiving module is as follows:

[0095]

[0096] in, They represent the three-dimensional coordinate information of the unmanned system in the world coordinate system w after the GNSS signal receiving module solves it, and b g (t) represents the distance deviation vector related to the clock of the GNSS signal receiving module at time t; when the GNSS signal is normal, its distribution should conform to the standard Gaussian distribution with zero mean. When the GNSS signal is weak or rejected, its distribution will be greatly disturbed.

[0097] The IMU sensor includes an accelerometer and a gyroscope to calculate the position, speed, and attitude information of the unmanned vehicle. The IMU measurement model is as follows:

[0098]

[0099] in, Represents the three-dimensional acceleration vector of the unmanned system output by the accelerometer, a t represents the real acceleration, represents the zero bias of the accelerometer, represents the compensation for the Earth's gravitational acceleration, Represents the direction cosine matrix from the world coordinate system to the navigation coordinate system at time t, g w represents the gravity vector in the world coordinate system, represents the Gaussian noise of the accelerometer; ω t Represents the three-dimensional angular velocity information of the unmanned system output by the gyroscope, ω t represents the true angular velocity, represents the zero bias of the gyroscope, represents the Gaussian noise of the gyroscope;

[0100] The visual sensor is used to capture the key information between the previous and next two frames of images, including feature extraction, key point matching, camera pose estimation, etc. The measurement model of the visual sensor is as follows:

[0101]

[0102] R v =R v ·exp(δθ × )

[0103]

[0104] Among them, X w represents the three-dimensional coordinates of the feature point in the world coordinate system, K and [RT] represent the internal and external parameter matrices of camera calibration, respectively. Represents the pixel coordinates of the output feature points, z v Represents the actual pixel coordinates of feature points, represents Gaussian noise; Rv Represents the relative rotation matrix of the two frames of image solution, R v represents the real rotation matrix, exp(δθ × ) represents the disturbance noise, obeying Represents the relative translation vector of the two frames of image solution, t v is the real translation vector, represents Gaussian noise.

[0105] Step 2: Initialize and initialize the data of the GNSS signal receiving module, IMU and vision sensor according to the established measurement model.

[0106] The specific method is as follows:

[0107] (1) According to the measurement model of the GNSS signal receiving module, GNSS and IMU integration are performed to initialize the IMU to obtain a rough IMU bias and absolute attitude estimate, so that the absolute attitude is aligned with the local coordinate system;

[0108] (2) Perform pre-integration processing on the IMU data to obtain the pre-integration items of the IMU, which are Its specific form is as follows:

[0109]

[0110] in, Represents the coordinate system of the body from the current time to t k The rotation matrix at time, ω t They represent the acceleration vector output by the accelerometer and the angular velocity vector output by the gyroscope, respectively. Represent the zero bias of the accelerometer and gyroscope, n a 、n w represent the Gaussian noise of the accelerometer and gyroscope respectively;

[0111] (3) According to the measurement model of the visual sensor, a standard SfM solution is performed to obtain the rotation matrix R and translation vector t of each frame camera;

[0112] (4) Calibrate the relative rotation matrix between IMU and vision sensor

[0113] Step 3: Check whether the GNSS signal is abnormal. When the GNSS signal is normal, set the optimization variables according to the preprocessed data information, build the residual matrix according to the residual vectors of the three sensors, perform nonlinear optimization, minimize the residual vector of each sensor, determine the weight value of each sensor data according to the optimized residual vector, perform weighted fusion on the state vector, and obtain the optimal state estimation of the system.

[0114] (1) Set the variables to be optimized and construct them as a least squares problem. Optimize each variable. The optimized variables are as follows:

[0115]

[0116] Among them, x k is the estimated parameter state at each time node, including the position of the unmanned system output by the IMU speed and posture Gyroscope bias and accelerometer bias Unmanned system position output by GNSS signal receiving module Clock distance deviation n is the number of time nodes in the set window; is the external reference between the camera frame and the IMU, is the inverse depth parameter of the landmark in its first observed keyframe.

[0117] (2) First determine the residual vectors of the three sensors, then construct the overall residual matrix and optimize it.

[0118] IMU residual definition: Based on two consecutive frames b k and b k+1 The residual of the pre-integrated IMU measurement can be defined as the difference between the measured value and the state prediction value:

[0119]

[0120] in, δb a ,δb g Represents the position, velocity, attitude, accelerometer bias, and gyroscope bias residual of the IMU respectively. Represents the rotation matrix from the world coordinate system to the body coordinate system at time k, Represent the position, speed, and posture at time k+1 respectively. Represent the position, speed, and posture at time k+1 respectively, g w Represents the gravity compensation in the world coordinate system, are the state prediction values ​​at time k+1, are the zero bias of the accelerometer and gyroscope at time k+1, are the zero bias of the accelerometer and gyroscope at time k;

[0121] The residual vector of the visual sensor is as follows:

[0122]

[0123] in, is the projection function of the rear camera; b1 and b2 are two orthogonal bases;

[0124] The residual vector of the GNSS signal receiving module is as follows:

[0125]

[0126] in, is the measured value, is the predicted value, The rotation matrix from the body coordinate system to the world coordinate system at time k, is the lever arm relative to the IMU body coordinate system;

[0127] According to the residual vectors of the three sensors, the residual matrix is ​​constructed to minimize the residuals of all sensors and minimize the sum of the prior and Mahalanobis norms of all measurement residuals to obtain the maximum a posteriori estimate. The formula is as follows:

[0128]

[0129] Among them, r p -H p X is the prior information obtained by marginalizing the system, is the measurement residual of IMU, is the measurement residual of the visual sensor, is the residual of the GNSS measurement value, ρ is the outlier suppression function, and its principle formula is as follows:

[0130]

[0131] (3) Optimize the residual matrix and assume that the final residual is The weight factor of each sensor is constructed using the residual. Assuming that the residual conforms to the Gaussian distribution, the mean and standard deviation of the residual of each sensor are first calculated:

[0132]

[0133] Among them, N, M, and P are the total amount of data of each sensor in the optimization period;

[0134] The residual normalization process is as follows:

[0135]

[0136] in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period;

[0137] The normalized residual is used to calculate the weights of the three sensors, and the formula is as follows:

[0138]

[0139] in, is the normalized residual of the corresponding sensor, and ε is set to a small positive constant;

[0140] The weights are normalized to ensure that the sum of the weights is 1. The formula is as follows:

[0141]

[0142] Among them, w i is the normalized weight coefficient, It is the sum of the weights of the three sensors.

[0143] (4) Perform weighted fusion on the optimized sensor data to obtain the optimal state estimation of the system:

[0144]

[0145] Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. They are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization.

[0146] Step 4. In the case of GNSS signal attenuation and rejection, the optimization operation of step 3 is also performed. In the weight calculation stage, the GNSS trust factor is established according to the chi-square test results, and the weight factor of the GNSS data is further calculated. The weight values ​​of the three sensor data are adaptively adjusted, and a navigation subsystem based on IMU and visual sensors is established to obtain the optimal state estimation of the system, and iterative updates are performed for continuous high-precision navigation.

[0147] like Figure 2 As shown, the specific method is as follows:

[0148] (1) According to the selected significance level and the critical value of the chi-square distribution, when λ≤λ0, it indicates that the GNSS signal is normal, when λ0<λ<2λ0, it indicates that the GNSS signal is weak, and when λ≥2λ0, it indicates that the GNSS signal is interrupted. The trust factor calculation formula is as follows:

[0149]

[0150] Among them, n is a positive constant, which is used as a tuning parameter. The larger n is, the lower the trust in GNSS signal attenuation is.

[0151] (2) Use the GNSS trust factor to further update the weights of the three sensors. The formula is as follows:

[0152]

[0153] in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period;

[0154] Normalize the updated weights to ensure that the sum of the weights is 1. The formula is as follows:

[0155]

[0156] (3) Substitute the updated weight factors to form a navigation subsystem based on IMU and visual sensors. Perform weighted fusion on the optimized data of each sensor to obtain the optimal state estimation of the system:

[0157]

[0158] Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. t is the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization.

[0159] The fusion obtains the optimal state estimate of the system and performs continuous iterative calculations to achieve high-precision continuous navigation when GNSS signals are weak or interrupted.

[0160] A navigation and positioning system with multi-modal adaptive fusion in a satellite navigation denied environment, such as Figure 3 As shown, it includes a sensor module mounted on the unmanned system, a GNSS signal monitoring module, an FPGA data processing module, and an industrial computer output and control module.

[0161] Sensor module: includes GNSS receiving module, IMU and vision sensor. The GNSS receiving module is responsible for obtaining the positioning information of the global navigation satellite system and providing high-precision positioning when the signal is normal; it serves as an auxiliary signal source when the signal is weak or denied; IMU includes accelerometer and gyroscope, which is responsible for measuring the acceleration and angular velocity of the unmanned system, providing high-frequency posture information, independent of external signals; the vision sensor is responsible for capturing images of the surrounding environment, providing environmental perception and local positioning data.

[0162] GNSS signal monitoring module: Uses a chi-square test-based method to evaluate the strength and quality of GNSS signals in real time, outputs a GNSS trust factor for subsequent weight factor calculations, and reduces the GNSS weight value through the GNSS trust factor in the event of signal attenuation or rejection.

[0163] FPGA data processing module: Utilizes the parallel processing capability of FPGA to process the data of each sensor in real time and efficiently, integrates the multi-modal sensor data through an adaptive weighted fusion algorithm, and generates the pose estimation results of the unmanned system.

[0164] Industrial computer output and control module: responsible for receiving the output data of the FPGA data processing module, outputting the final navigation status information in real time, making decisions based on the navigation status information, and controlling the movement of the unmanned system in real time.

[0165] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A navigation and positioning method with multi-modal adaptive fusion in a satellite navigation denied environment, characterized in that: The steps include: Step 1: Mount the GNSS signal receiving module, IMU and visual sensor on the unmanned system, receive data, and establish measurement models of the GNSS signal receiving module, IMU and visual sensor respectively; Step 2: Initialize and preprocess the data of the GNSS signal receiving module, IMU and visual sensor according to the established measurement model; Step 3: Check whether the GNSS signal is abnormal. If the GNSS signal is normal, set the optimization variables according to the preprocessed data information, build the residual matrix according to the residual vectors of the three sensors, perform nonlinear optimization, minimize the residual vector of each sensor, determine the weight value of each sensor data according to the optimized residual vector, perform weighted fusion on the state vector, and obtain the optimal state estimation of the system; Step 4: In the case of GNSS signal attenuation and denial, establish a GNSS trust factor, adaptively adjust the weight values ​​of the three sensor data, establish a navigation subsystem based on IMU and visual sensors, obtain the optimal state estimation of the system, and perform iterative updates for continuous high-precision navigation.

2. According to claim 1, a navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment is characterized in that: In step 1, the measurement model of the GNSS signal receiving module is as follows: in, They represent the three-dimensional coordinate information of the unmanned system in the world coordinate system w after the GNSS signal receiving module solves it, and b g (t) represents the distance deviation vector related to the clock of the GNSS signal receiving module at time t; The measurement model of IMU is as follows: in, Represents the three-dimensional acceleration vector of the unmanned system output by the accelerometer, a t represents the real acceleration, represents the zero bias of the accelerometer, represents the compensation for the Earth's gravitational acceleration, Represents the direction cosine matrix from the world coordinate system to the navigation coordinate system at time t, g w represents the gravity vector in the world coordinate system, represents the Gaussian noise of the accelerometer; ω t Represents the three-dimensional angular velocity information of the unmanned system output by the gyroscope, ω t represents the true angular velocity, represents the zero bias of the gyroscope, represents the Gaussian noise of the gyroscope; The measurement model of the visual sensor is as follows: R v =R v ·exp(δθ × ) Among them, X w represents the three-dimensional coordinates of the feature point in the world coordinate system, K and [RT] represent the internal and external parameter matrices of camera calibration, respectively. Represents the pixel coordinates of the output feature points, z v Represents the actual pixel coordinates of feature points, represents Gaussian noise; R v Represents the relative rotation matrix of the two frames of image solution, R v represents the real rotation matrix, exp(δθ × ) represents the disturbance noise, obeying Represents the relative translation vector of the two frames of image solution, t v is the real translation vector, represents Gaussian noise.

3. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 1 is characterized in that: The specific method of step 2 is as follows: (1) According to the measurement model of the GNSS signal receiving module, GNSS and IMU integration are performed to initialize the IMU to obtain a rough IMU bias and absolute attitude estimate, so that the absolute attitude is aligned with the local coordinate system; (2) Perform pre-integration processing on the IMU data to obtain the pre-integration items of the IMU, which are Its specific form is as follows: in, Represents the coordinate system of the body from the current time to t k The rotation matrix at time, ω t They represent the acceleration vector output by the accelerometer and the angular velocity vector output by the gyroscope, respectively. Represent the zero bias of the accelerometer and gyroscope, n a 、n w represent the Gaussian noise of the accelerometer and gyroscope respectively; (3) According to the measurement model of the visual sensor, a standard SfM solution is performed to obtain the rotation matrix R and translation vector t of each frame camera; (4) Calibrate the relative rotation matrix between IMU and vision sensor 4. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 1, characterized in that: In step 3, the optimization variables are set as follows: Among them, x k is the estimated parameter state at each time node, including the position of the unmanned system output by the IMU speed and posture Gyroscope bias and accelerometer bias Unmanned system position output by GNSS signal receiving module Clock distance deviation n is the number of time nodes in the set window; is the external reference between the camera frame and the IMU, is the inverse depth parameter of the landmark in its first observed keyframe.

5. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 4, characterized in that: In step 3, the residual matrix is ​​constructed according to the residual vectors of the three sensors as follows: The residual vector of IMU is as follows: in, δb a ,δb g Represents the position, velocity, attitude, accelerometer bias, and gyroscope bias residual of the IMU respectively. Represents the rotation matrix from the world coordinate system to the body coordinate system at time k, Represent the position, speed, and posture at time k+1 respectively. Represent the position, speed, and posture at time k+1 respectively, g w Represents the gravity compensation in the world coordinate system, are the state prediction values ​​at time k+1, are the zero bias of the accelerometer and gyroscope at time k+1, are the zero bias of the accelerometer and gyroscope at time k; The residual vector of the visual sensor is as follows: in, is the projection function of the rear camera; b1 and b2 are two orthogonal bases; The residual vector of the GNSS signal receiving module is as follows: in, is the measured value, is the predicted value, The rotation matrix from the body coordinate system to the world coordinate system at time k, is the lever arm relative to the IMU body coordinate system; According to the residual vectors of the three sensors, the residual matrix is ​​constructed to minimize the residuals of all sensors and minimize the sum of the prior and Mahalanobis norms of all measurement residuals to obtain the maximum a posteriori estimate. The formula is as follows: Among them, r p -H p X is the prior information obtained by marginalizing the system, is the measurement residual of IMU, is the measurement residual of the visual sensor, is the residual of the GNSS measurement value, ρ is the outlier suppression function, and its principle formula is as follows:

6. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 5, characterized in that: In step 3, the residual matrix is ​​optimized, assuming that the final residual is The weight factor of each sensor is constructed using the residual. Assuming that the residual conforms to the Gaussian distribution, the mean and standard deviation of the residual of each sensor are first calculated: Among them, N, M, and P are the total amount of data of each sensor in the optimization period; The residual normalization process is as follows: in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period; The normalized residual is used to calculate the weights of the three sensors, and the formula is as follows: in, is the normalized residual of the corresponding sensor, and ε is set to a small positive constant; The weights are normalized to ensure that the sum of the weights is 1. The formula is as follows: Among them, w i is the normalized weight coefficient, It is the sum of the weights of the three sensors.

7. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 6, characterized in that: In step 3, the optimized sensor data is weighted fused to obtain the optimal state estimation of the system: Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. They are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization.

8. The navigation and positioning method of multi-modal adaptive fusion in a satellite navigation denied environment according to claim 6, characterized in that: The specific method of step 4 is as follows: (1) According to the selected significance level and the critical value of the chi-square distribution, when λ≤λ0, it indicates that the GNSS signal is normal, when λ0<λ<2λ0, it indicates that the GNSS signal is weak, and when λ≥2λ0, it indicates that the GNSS signal is interrupted. The trust factor calculation formula is as follows: Among them, n is a positive constant, which is used as a tuning parameter. The larger n is, the lower the trust in GNSS signal attenuation is. (2) Use the GNSS trust factor to further update the weights of the three sensors. The formula is as follows: in, are the i-th normalized residuals of the three sensors in this time period, They are the i-th original residuals of the three sensors in this time period; Normalize the updated weights to ensure that the sum of the weights is 1. The formula is as follows: (3) Substitute the updated weight factors to form a navigation subsystem based on IMU and visual sensors. Perform weighted fusion on the optimized data of each sensor to obtain the optimal state estimation of the system: Among them, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system obtained at time t, respectively. They are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t. They are the optimized speed information of IMU and visual sensor at time t. They are the optimized posture information of IMU and visual sensor at time t, and w imu 、w camera are the weights of IMU and visual sensor before normalization, w imu 、w camera 、w gnss They are the weights of the three sensors after normalization; The fusion obtains the optimal state estimate of the system and performs continuous iterative calculations to achieve high-precision continuous navigation when the GNSS signal is weak or interrupted.

9. A navigation and positioning system with multi-modal adaptive fusion in a satellite navigation denial environment, characterized in that: It includes sensor modules, GNSS signal monitoring modules, FPGA data processing modules and industrial computer output and control modules installed on unmanned systems; The sensor module includes a GNSS receiving module, an IMU and a visual sensor. The GNSS receiving module is responsible for obtaining the positioning information of the global navigation satellite system and providing high-precision positioning when the signal is normal; when the signal is weak or denied, it serves as an auxiliary signal source; the IMU includes an accelerometer and a gyroscope, which is responsible for measuring the acceleration and angular velocity of the unmanned system and providing high-frequency posture information independent of external signals; the visual sensor is responsible for capturing images of the surrounding environment and providing environmental perception and local positioning data; The GNSS signal monitoring module: uses a chi-square test-based method to evaluate the strength and quality of the GNSS signal in real time, outputs a GNSS trust factor for subsequent weight factor calculation; reduces the GNSS weight value by the GNSS trust factor in the case of signal attenuation or rejection; The FPGA data processing module uses the parallel processing capability of the FPGA to process the data of each sensor in real time and efficiently, integrates the multi-modal sensor data through an adaptive weighted fusion algorithm, and generates a pose estimation result of the unmanned system; The industrial computer output and control module is responsible for receiving the output data of the FPGA data processing module, outputting the final navigation status information in real time, making decisions based on the navigation status information, and controlling the movement of the unmanned system in real time.

Images