Multi-modal adaptive fusion navigation positioning method and system under satellite navigation denial environment

Abstract

The application discloses a kind of satellite navigation denial-of-service environment under multi-modal adaptive fusion navigation positioning method and system, the system uses GNSS / IMU / visual three kinds of sensors, method includes: receiving the data of three kinds of sensors and completing modeling;Three kinds of sensors are initialized;GNSS signal strength is judged using chi-square test;Optimization variable is set, residual matrix is constructed, and nonlinear optimization is carried out;GNSS signal is weakened or set GNSS trust factor when being denied, and weight is calculated;Weighted fusion sensor data is outputted system optimal state estimation, realizes continuous navigation positioning.The application establishes adaptive weight adjustment mechanism by, introduces GNSS trust factor, and the weight of multi-sensor data is adaptively adjusted, can overcome the positioning difficulty under the condition that GNSS is interfered by deception, suppression interference and building obstruction etc., realizes the continuous navigation positioning of unmanned system (such as robot, unmanned vehicle, unmanned aerial vehicle, unmanned boat, unmanned underwater vehicle etc.).

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned system navigation control, in particular to a multi-modal adaptive fusion navigation positioning method and system in a satellite navigation denial environment. BACKGROUND

[0002] With the rapid development of unmanned systems (such as robots, unmanned vehicles, unmanned aerial vehicles, unmanned boats, unmanned underwater vehicles, etc.), positioning and navigation technology based on multi-source sensor fusion has become a 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 the scenario of GNSS signal weakening or denial (such as urban canyons, tunnels, bad weather, etc.), the system cannot continue to navigate with high precision and stability.

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

[0004] Traditional GNSS / IMU / visual fusion methods generally use fixed weight configuration to process multi-source information fusion. This method cannot effectively adapt to dynamically changing environmental conditions, and in the case of GNSS signal weakening or denial, the fixed configuration of weights may not adapt to the changes in the dynamic environment and sensor performance, thereby reducing the overall navigation accuracy. SUMMARY

[0005] To solve the above technical problems, the present application provides a multi-modal adaptive fusion navigation positioning method and system in a satellite navigation denial environment, which establishes an adaptive weight adjustment mechanism and introduces a GNSS trust factor to adaptively adjust the weight values of multi-sensor data, overcoming the positioning difficulties in the case of GNSS being spoofed, jammed, and blocked by buildings, and realizing long-time high-precision navigation and positioning.

[0006] To achieve the above purpose, the technical scheme of the present application is as follows:

[0007] A multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment, comprising the following steps:

[0008] Step 1, the GNSS signal receiving module, IMU and visual sensor are carried on the unmanned system, data is received, and measurement models of the GNSS signal receiving module, IMU and visual sensor are respectively established;

[0009] Step 2, the data of the GNSS signal receiving module, IMU and visual sensor are initialized and preprocessed according to the established measurement models;

[0010] Step 3, whether the GNSS signal is abnormal is detected, under the condition that the GNSS signal is normal, optimization variables are set according to the preprocessed data information, a residual matrix is constructed according to residual vectors of the three sensors, nonlinear optimization is performed, residual vectors of each sensor are minimized, weight values of data of each sensor are determined according to the optimized residual vectors, a state vector is weighted and fused, and optimal state estimation of the system is obtained;

[0011] Step 4, under the condition that the GNSS signal is weakened and denied, a GNSS trust factor is established, weight values of data of the three sensors are adaptively adjusted, a navigation subsystem mainly based on the IMU and visual sensor is established, optimal state estimation of the system is obtained, and iterative updating is performed to continuously perform 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] wherein, respectively represent three-dimensional coordinate information of the unmanned system in a world coordinate system w after being solved by the GNSS signal receiving module, represents a distance deviation vector related to a clock of the GNSS signal receiving module at t time;

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

[0016]

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

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

[0019]

[0020]

[0021]

[0022] wherein, three-dimensional coordinates of the feature points in the world coordinate system, K and [R T] represent internal and external parameter matrices of camera calibration respectively, output pixel coordinates of the feature points, real pixel coordinates of the feature points, Gaussian noise; relative rotation matrix solved from two frames of images, real rotation matrix, exp(δθ × ) represents disturbance noise, which is subject to relative translation vector solved from two frames of images, real translation vector, 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 is performed to initialize the IMU to obtain a rough IMU bias and an absolute attitude estimation, so that the absolute attitude is aligned with the local coordinate system;

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

[0026]

[0027] wherein, rotation matrix of the body coordinate system from the current time to time, acceleration vector output by the accelerometer and angular velocity vector output by the gyroscope, bias of the accelerometer and the gyroscope, Gaussian noise of the accelerometer and the gyroscope;

[0028] (3) Based on the measurement model of the visual sensor, perform standard SfM calculation to obtain the rotation matrix R and translation vector t of the camera for each frame;

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

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

[0031]

[0032] in, It is the estimated parameter state at each time point, including the unmanned system position output by the IMU. speed and posture Gyroscope bias and accelerometer bias The location of the unmanned system output by the GNSS signal receiving module Clock distance deviation n is the number of time points in the specified window; It is the extrinsic parameter between the camera frame and the IMU. It is the inverse depth parameter of the landmark in its first observed keyframe.

[0033] In the above scheme, step 3 involves constructing the residual matrix based on the residual vectors of the three sensors as follows:

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

[0035]

[0036] in, These represent the IMU's position, velocity, attitude, accelerometer zero bias, and gyroscope zero bias residuals, respectively. The rotation matrix representing the rotation from the world coordinate system to the body coordinate system at time k. These represent the position, velocity, and attitude at time k+1, respectively. Represent the position, velocity, and attitude at time k+1, respectively, g w Represents gravity compensation in the world coordinate system. These are the predicted state values ​​at time k+1. These are the zero bias values ​​of the accelerometer and gyroscope at time k+1, respectively. These are the zero biases of the accelerometer and gyroscope at time k, respectively;

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

[0038]

[0039] wherein, is the rear camera projection function; is the two orthogonal bases;

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

[0041]

[0042] wherein, is the measured value, is the predicted value, is the rotation matrix of 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, a residual matrix is constructed, all sensor residuals are minimized, and the sum of the priori and Mahalanobis norm of all measurement residuals is minimized to obtain the maximum a posteriori estimation, and the formula is as follows:

[0044]

[0045] wherein, is the priori information obtained by system marginalization, is the measurement residual of the IMU, is the measurement residual of the vision sensor, is the residual of the GNSS measured value, and ρ is an outlier rejection function, and the principle formula is as follows:

[0046]

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

[0048]

[0049] wherein, N, M and P are respectively the total amount of data of each sensor within the optimization period.

[0050] The residual is normalized, and the formula is as follows:

[0051]

[0052] wherein, are respectively the i th normalized residual of the three sensors within the time period, are the i-th raw residuals of the three sensors in the time period, respectively;

[0053] The weights of the three sensors are calculated using the normalized residuals, as follows:

[0054]

[0055] wherein, is the normalized residual of the corresponding sensor, and ε is a very small positive number;

[0056] The weights are normalized to ensure that their sum is 1, as follows:

[0057]

[0058] wherein, is the normalized weight coefficient, is the sum of the weights of the three sensors.

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

[0060]

[0061] wherein, P(t), V(t), and q(t) are the position, velocity, and attitude information of the unmanned system at time t, respectively, are the optimized position information of the IMU, visual sensor, and GNSS signal receiving module at time t, respectively, are the optimized velocity information of the IMU and visual sensor at time t, respectively, are the optimized attitude information of the IMU and visual sensor at time t, respectively, are the weights of the IMU and visual sensor before normalization, respectively, 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, set λ≤λ0, which indicates that the GNSS signal is normal, λ0<λ<2λ0, which indicates that the GNSS signal is weak, and λ≥2λ0, which indicates that the GNSS signal is interrupted. The trust factor calculation formula is as follows:

[0064]

[0065] wherein, n is a normal number, which is a tuning parameter. The larger n is, the lower the trust degree of the GNSS signal is.

[0066] (2) Further update the three sensor weights using GNSS trust factor, the formula is as follows:

[0067]

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

[0069] The updated weights are normalized to ensure that the sum of the weights is 1, the formula is as follows:

[0070]

[0071] (3) Substitute the updated weight factor to form the navigation subsystem mainly based on IMU and visual sensor, and weight the optimized data of each sensor to obtain the optimal state estimation of the system:

[0072]

[0073] Wherein, P(t), V(t), q(t) are the position, velocity and attitude information of the unmanned system at time t, are the optimized position information of IMU, visual sensor and GNSS signal receiving module at time t, are the optimized velocity information of IMU and visual sensor at time t, are the optimized attitude information of IMU and visual sensor at time t, are the weights of IMU and visual sensor before normalization, are the weights of the three sensors after normalization; The optimal state estimation of the system is obtained by fusion, and continuous iteration operation is carried out to realize high-precision continuous navigation in the case of GNSS signal weakening and interruption.

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

[0075]

[0076] The sensor module includes a GNSS receiving module, an IMU, and a visual sensor, the GNSS receiving module is responsible for acquiring positioning information of a global navigation satellite system, and provides high-precision positioning when a signal is normal; in the case of signal weakening or rejection, it serves as an auxiliary signal source; the IMU includes an accelerometer and a gyroscope, and is responsible for measuring the acceleration and angular velocity of the unmanned system, and provides high-frequency pose information independent of external signals; the visual sensor is responsible for capturing surrounding environment images, and provides environment 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, and is used for subsequent weight factor calculation; in the case of signal weakening or rejection, the GNSS weight value is reduced through the GNSS trust factor;

[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 the 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, and outputs the final navigation state information in real time, and makes decisions according to the navigation state information, and controls the movement of the unmanned system in real time.

[0080] Through the above technical solutions, the satellite navigation rejection environment multi-modal adaptive fusion navigation positioning method and system provided by the application has the following beneficial effects:

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

[0082] The application introduces an adaptive weight adjustment mechanism and a GNSS trust factor, and uses the optimized residual errors of each sensor to distribute the weights, which can dynamically adjust the weights of each sensor according to the real-time sensor performance and environmental changes. This dynamic adaptability ensures the best state estimation of the system under different environmental conditions (such as when the GNSS signal is weakened or rejected), and improves the flexibility and response ability of the system.

[0083] 2. Enhance the stability of the system

[0084] When the GNSS signal is suppressed, interfered, deceived, blocked by buildings, and weakened or rejected, the application introduces a GNSS trust factor, so that the system can reasonably adjust the weight distribution and appropriately enhance the dependence on the IMU and visual sensor when the GNSS signal quality is reduced. This method effectively improves the navigation accuracy in complex environments, and ensures that the unmanned vehicle can still maintain reliable positioning and navigation ability when the GNSS is unstable.

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

[0086] The dynamic adjustment of the weight of the sensor enables the system to perform weighted average according to the residual characteristics of each sensor, avoids that a certain sensor excessively influences the final result, and improves the positioning accuracy after fusion. BRIEF DESCRIPTION OF DRAWINGS

[0087] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced below.

[0088] Figure 1 A flowchart of a satellite navigation denial environment multi-modal adaptive fusion navigation positioning method disclosed by the embodiments of the present application;

[0089] Figure 2 A weight calculation schematic diagram disclosed by the embodiments of the present application;

[0090] Figure 3 A satellite navigation denial environment multi-modal adaptive fusion navigation positioning system schematic diagram disclosed by the embodiments of the present application. DETAILED DESCRIPTION

[0091] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application.

[0092] The present application provides a satellite navigation denial environment multi-modal adaptive fusion navigation positioning method, as shown in Figure 1 The method comprises the following steps:

[0093] Step 1, the GNSS signal receiving module, the IMU and the visual sensor are carried on the unmanned system, data is received, and the measurement model of the GNSS signal receiving module, the IMU and the visual sensor is established respectively.

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

[0095]

[0096] Among them, The three-dimensional coordinate information of the unmanned system in the world coordinate system w after being solved by the GNSS signal receiving module respectively, The clock related distance deviation vector of the GNSS signal receiving module at t time; in the normal case of the GNSS signal, the distribution should conform to the standard Gaussian distribution with zero mean, and in the case of GNSS signal weakening and denial, the distribution will be greatly disturbed.

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

[0098]

[0099] wherein, represents a three-dimensional acceleration vector of the unmanned system output by the accelerometer, represents the true acceleration, represents the zero offset of the accelerometer, represents compensation for the acceleration of the earth's gravity, represents the direction cosine matrix of 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; represents three-dimensional angular velocity information of the unmanned system output by the gyroscope, represents the true angular velocity, represents the zero offset of the gyroscope, represents the Gaussian noise of the gyroscope;

[0100] The key information between the front and rear two frames of images is captured by the vision sensor, including feature extraction, key point matching, camera pose estimation, etc. The measurement model of the vision sensor is as follows:

[0101]

[0102]

[0103]

[0104] wherein, represents the three-dimensional coordinates of the feature points in the world coordinate system, K and [R T] represent the internal and external parameter matrices of the camera calibration, represents the output feature point pixel coordinates, represents the true feature point pixel coordinates, represents the Gaussian noise; represents the relative rotation matrix calculated from the two frames of images, represents the true rotation matrix, exp(δθ × represents the disturbance noise, which is subject to represents the relative translation vector calculated from the two frames of images, is the true translation vector, represents the Gaussian noise.

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

[0106] The specific method is as follows:

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

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

[0109]

[0110] Among them, represents the rotation matrix of the body coordinate system from the current time to time, respectively represent the acceleration vector output by the accelerometer and the angular velocity vector output by the gyroscope, respectively represent the zero offset of the accelerometer and the gyroscope, respectively represent the Gaussian noise of the accelerometer and the gyroscope;

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

[0112] (4) Calibrate the relative rotation matrix

[0113] Step 3, detect whether the GNSS signal is abnormal, and under the condition that the GNSS signal is normal, set the optimization variables according to the preprocessed data information, construct the residual matrix according to the residual vector of the three sensors, carry out nonlinear optimization, minimize the residual vector of each sensor, determine the weight value of each sensor data according to the optimized residual vector, and perform weighted fusion on the state vector to obtain the optimal state estimation of the system.

[0114] (1) Set the variables that need to be optimized, and construct it as a least squares problem to optimize each variable, and the optimization variables are as follows:

[0115]

[0116] Among them, is the estimated parameter state of each time node, including the position velocity and attitude gyroscope bias and accelerometer bias unmanned system position output by the GNSS signal receiving module clock distance bias n is the number of time nodes in the set window; is the extrinsic parameter between the camera frame and the IMU, is the inverse depth parameter of the landmark in the first observed key frame thereof.

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

[0118] IMU residual definition: according to the IMU measurement in two consecutive frames and The residual of the pre-integrated IMU measurement can be defined by the difference between the measurement value and the state prediction value according to the IMU measurement in two consecutive frames:

[0119]

[0120] wherein, respectively represent the position, velocity, attitude, accelerometer zero offset, gyroscope zero offset residual of the IMU, represents the rotation matrix from the world coordinate system to the body coordinate system at time k, respectively represent the position, velocity, attitude at time k+1, respectively represent the position, velocity, attitude at time k+1, g w represents the gravity compensation in the world coordinate system, respectively are the state prediction values at time k+1, respectively are the zero offsets of the accelerometer and gyroscope at time k+1, respectively are the zero offsets of the accelerometer and gyroscope at time k;

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

[0122]

[0123] wherein, is the back camera projection function; is the two orthogonal bases;

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

[0125]

[0126] wherein, is the measurement value, is the prediction value, is 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 error vectors of the three sensors, a residual error matrix is constructed, all sensor residual errors are minimized, and the sum of the priori and Mahalanobis norm of all measurement residual errors is minimized to obtain the maximum a posteriori estimation, as follows:

[0128]

[0129] wherein, is the priori information obtained by system marginalization, is the measurement residual error of the IMU, is the measurement residual error of the vision sensor, is the residual error of the GNSS measurement value, and p is an outlier rejection function, the principle formula of which is as follows:

[0130]

[0131] (3) The residual error matrix is optimized, and the final residual error is assumed to be The residual error is used to construct the weight factor of each sensor. Assuming that the residual error conforms to the Gaussian distribution, the mean and standard deviation of each sensor residual error are first calculated:

[0132]

[0133] wherein, N, M, and P are the total amount of data of each sensor within the optimization period, respectively;

[0134] The residual error is normalized, and the formula is as follows:

[0135]

[0136] wherein, are the i-th normalized residual error of the three sensors within the time period, respectively, are the i-th original residual error of the three sensors within the time period, respectively;

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

[0138]

[0139] wherein, is the normalized residual error of the corresponding sensor, and ε is set to a very small normal number;

[0140] The weight is normalized to ensure that the sum of the weights is 1, and the formula is as follows:

[0141]

[0142] wherein, is the normalized weight coefficient, is the sum of the weights of the three sensors.

[0143] (4) The optimized sensor data is weighted and fused to obtain the optimal state estimation of the system:

[0144]

[0145] wherein, P(t), V(t), q(t) are the position, velocity and attitude information of the unmanned system at time t, are the optimized position information of the IMU, visual sensor and GNSS signal receiving module at time t, are the optimized velocity information of the IMU and visual sensor at time t, are the optimized attitude information of the IMU and visual sensor at time t, are the weights of the IMU and visual sensor before normalization, are the weights of the three sensors after normalization.

[0146] Step 4, in the case of GNSS signal weakening and rejection, the optimization operation of step 3 is also performed, and in the weight calculation stage, the GNSS trust factor is established according to the chi-square test result, the weight factor of GNSS data is further calculated, the weight values of the three sensor data are adaptively adjusted, the navigation subsystem mainly based on IMU and visual sensor is established, the optimal state estimation of the system is obtained, and iterative updating is performed for continuous high-precision navigation.

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

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

[0149]

[0150] wherein, n is a normal number, which is a regulating parameter, the greater n is, the lower the trust degree of GNSS signal weakening is;

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

[0152]

[0153] wherein, are the i-th normalized residual of the three sensors in this time period, respectively, are the i-th original residual of the three sensors in this time period, respectively;

[0154] The updated weights are normalized to ensure that the sum of the weights is 1, as follows:

[0155]

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

[0157]

[0158] wherein, P(t), V(t), q(t) are the position, velocity and attitude information of the unmanned system at time t, respectively, are the optimized position information of the IMU, visual sensor and GNSS signal receiving module at time t, respectively, are the optimized velocity information of the IMU and visual sensor at time t, respectively, are the optimized attitude information of the IMU and visual sensor at time t, respectively, are the weights of the IMU and visual sensor before normalization, respectively, are the weights of the three sensors after normalization, respectively.

[0159] The optimal state estimation of the system is obtained by fusion and continuous iterative operation to realize high-precision continuous navigation in the case of GNSS signal weakening and interruption.

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

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

[0162] GNSS signal monitoring module: using the method based on chi-square test, real-time evaluation of GNSS signal strength and quality, output GNSS trust factor, for subsequent weight factor calculation; in the case of signal weakening or rejection, through the GNSS trust factor to reduce the GNSS weight value.

[0163] FPGA data processing module: using the parallel processing capability of FPGA, real-time, efficient processing of data of each sensor, through the adaptive weighted fusion algorithm, multi-modal sensor data integration, generate the pose estimation results of unmanned system.

[0164] Industrial computer output and control module: responsible for receiving the output data of FPGA data processing module, real-time output of the final navigation state information, and decision-making according to the navigation state information, real-time control of the motion of unmanned system.

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

Claims

1. A method for multi-modal adaptive fusion navigation and positioning in a satellite navigation denial environment, characterized in that, Comprising the following steps: Step 1, the GNSS signal receiving module, IMU and visual sensor are carried on the unmanned system, the data are received, and the measurement models of the GNSS signal receiving module, IMU and visual sensor are respectively established; Step 2, according to the established measurement model, the data of the GNSS signal receiving module, IMU and visual sensor are initialized and preprocessed; Step 3, whether the GNSS signal is abnormal is detected, under the condition that the GNSS signal is normal, the optimization variable is set according to the preprocessed data information, the residual matrix is constructed according to the residual vector of the three sensors, the nonlinear optimization is carried out, the residual vector of each sensor is minimized, the weight value of each sensor data is determined according to the optimized residual vector, the state vector is weighted and fused, and the optimal state estimation of the system is obtained; Step 4, under the condition that the GNSS signal is weakened and refused, the GNSS trust factor is established, the weight value of the data of the three sensors is adaptively adjusted, the navigation subsystem mainly based on the IMU and visual sensor is established, the optimal state estimation of the system is obtained, and iterative updating is carried out, and continuous high-precision navigation is carried out; The specific method of step 4 is as follows: (1) according to the selected significance level and the critical value of chi-square distribution, when λ≤λ0, it indicates that the GNSS signal is normal, when λ0<λ<2λ0, it indicates that the GNSS signal is weakened, and when λ≥2λ0, it indicates that the GNSS signal is interrupted, and then the trust factor calculation formula is as follows: Wherein, n is a normal number, which is used as an adjusting parameter, the greater n is, the lower the trust degree of the weakened GNSS signal is; (2) the GNSS trust factor is used to further update the weight of the three sensors, and the formula is as follows: wherein, are the i-th normalized residuals of the three sensors over the time period, respectively; The updated weight is normalized to ensure that the sum of the weights is 1, and the formula is as follows: (3) the updated weight factor is substituted to form the navigation subsystem mainly based on the IMU and visual sensor, the weighted fusion of the optimized data of each sensor is carried out, and the optimal state estimation of the system is obtained: Wherein, P(t), V(t), q(t) are the position, velocity and attitude information of the unmanned system at time t respectively, are the optimized position information of the IMU, vision sensor and GNSS signal receiving module at time t respectively, are the optimized velocity information of the IMU and vision sensor at time t respectively, are the optimized attitude information of the IMU and vision sensor at time t respectively, are the weights of the IMU and vision sensor before normalization respectively, are the weights of the three sensors after normalization respectively; The optimal state estimation of the system is fused, and continuous iterative operation is carried out to realize high-precision continuous navigation under the condition that the GNSS signal is weakened and interrupted.

2. The multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment according to claim 1, characterized in that, In step 1, the measurement model of the GNSS signal receiving module is as follows: wherein, respectively represent the three-dimensional coordinate information of the unmanned system in the world coordinate system w after being solved by the GNSS signal receiving module, represents the distance deviation vector related to the GNSS signal receiving module clock at time t. The measurement model of the IMU is as follows: wherein, a three-dimensional acceleration vector of the unmanned system representing the accelerometer output, a true acceleration, a bias of the accelerometer, a compensation for the earth gravity acceleration, a direction cosine matrix of the world coordinate system to the navigation coordinate system at time t, g w a gravity vector in the world coordinate system, a Gaussian noise of the accelerometer; a three-dimensional angular velocity information of the unmanned system representing the gyroscope output, a true angular velocity, a bias of the gyroscope, a Gaussian noise of the gyroscope; The measurement model of the visual sensor is as follows: where, represents the 3D coordinates of the feature points in the world coordinate system, K and [R T] represent the intrinsic and extrinsic parameter matrices of camera calibration respectively, represents the output pixel coordinates of the feature points, represents the real pixel coordinates of the feature points, represents the Gaussian noise; represents the relative rotation matrix solved from two frames of images, represents the real rotation matrix, exp(δθ × ) represents the disturbance noise, which is subject to represents the relative translation vector solved from two frames of images, is the real translation vector, represents the Gaussian noise.

3. The multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment according to claim 1, characterized in that, The specific method of step 2 is as follows: (1) according to the measurement model of the GNSS signal receiving module, the GNSS and IMU are integrated to initialize the IMU to obtain the rough IMU bias and absolute attitude estimation, so that the absolute attitude is aligned with the local coordinate system; (2) pre-integrating the IMU data to obtain the pre-integrated term of the IMU, which are respectively The specific form is as follows: wherein, R represents a rotation matrix of the body coordinate system from the current time to the time t, R represents a rotation matrix of the body coordinate system from the current time to the time t, respectively represent an acceleration vector of the accelerometer output and an angular velocity vector of the gyroscope output, respectively represent biases of the accelerometer and the gyroscope, respectively represent Gaussian noises of the accelerometer and the gyroscope; (3) according to the measurement model of the visual sensor, the standard SfM solution is carried out, and the rotation matrix R and translation vector t of each frame camera are obtained; (4) calibrating the relative rotation matrix of the IMU and the vision sensor 4. The multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment according to claim 1, characterized in that, In step 3, the optimization variable is set as follows: wherein, is the estimated state of the parameters of each time node, including the unmanned system position velocity and attitude gyro bias and accelerometer bias unmanned system position output by the GNSS signal receiving module clock distance bias n is the number of time nodes in the set window; is the external parameter between the camera frame and the IMU, is the inverse depth parameter of the landmark in the first observed key frame thereof.

5. The multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment according to claim 4, characterized in that, In step 3, the residual matrix is constructed according to the residual vector of the three sensors as follows: The residual vector of the IMU is as follows: wherein, respectively represent the position, velocity, attitude, accelerometer bias, gyroscope bias residual of the IMU, represents the rotation matrix from the world coordinate system to the body coordinate system at time k, respectively represent the position, velocity, attitude at time k+1, respectively represent the position, velocity, attitude at time k, g w represents the gravity compensation in the world coordinate system, respectively are the state prediction at time k+1, respectively are the biases of the accelerometer and gyroscope at time k+1, respectively are the biases of the accelerometer and gyroscope at time k; The residual vector of the visual sensor is as follows: wherein, is a rear camera projection function; is two orthogonal bases; The residual vector of the GNSS signal receiving module is as follows: wherein, is a measured value, is a predicted value, is a rotation matrix of the body coordinate system to the world coordinate system at time k, is a lever arm relative to the IMU body coordinate system; According to the residual vectors of the three sensors, a residual matrix is constructed, the residual of all sensors is minimized, and the sum of the prior and Mahalanobis norm of all measurement residuals is minimized to obtain the maximum a posteriori estimation, as follows: where, is the prior information obtained by system marginalization, is the measurement residual of the IMU, is the measurement residual of the vision sensor, is the residual of the GNSS measurement, and p is an outlier rejection function whose rationale is given by:

6. The multi-modal adaptive fusion navigation positioning method in a satellite navigation denial environment according to claim 5, characterized in that, In step 3, the residual matrix is optimized, and suppose the final residual is The weight factor of each sensor is constructed using the residual. Assuming that the residual conforms to a Gaussian distribution, first, the mean and standard deviation of the residual of each sensor are calculated: Where N, M, and P are the total amount of data of each sensor within the optimization period. Residual normalization processing is performed, as follows: wherein, are the i-th normalized residuals of the three sensors over the time period, respectively, are the i-th original residuals of the three sensors over the time period, respectively; The weights of the three sensors are calculated using the normalized residuals, as follows: wherein, is the normalized residual of the respective sensor, and ε is set to a small positive number. Weight normalization is performed to ensure that the sum of the weights is 1, as follows: wherein, is the normalized weight coefficient, is the sum of the three sensor weights.

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

8. A multi-modal adaptive fusion navigation and positioning system in a GNSS denial environment, employing the method of claim 1, characterized in that, The sensor module, GNSS signal monitoring module, FPGA data processing module, and industrial computer output and control module are included. The sensor module includes a GNSS receiving module, an IMU, and a vision sensor. The GNSS receiving module is responsible for obtaining positioning information from the global navigation satellite system and providing high-precision positioning when the signal is normal. In the case of signal weakening or rejection, it serves as an auxiliary signal source. The IMU includes an accelerometer and a gyroscope, which are responsible for measuring the acceleration and angular velocity of the unmanned system and providing high-frequency pose information independent of external signals. The vision 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 GNSS signals in real time and outputs a GNSS trust factor for subsequent weight factor calculation. In the case of signal weakening or rejection, the GNSS trust factor is used to reduce the GNSS weight value. The FPGA data processing module uses the parallel processing capability of FPGA to process the data of each sensor in real time and efficiently. Through an adaptive weighted fusion algorithm, the multi-modal sensor data is integrated to generate the 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 and outputting the final navigation state information in real time. Based on the navigation state information, decisions are made to control the motion of the unmanned system in real time.

Images