A robust adaptive VIO navigation positioning method suitable for GNSS denial environment

Abstract

This invention proposes a robust adaptive VIO navigation and positioning method suitable for GNSS-denied environments. First, it utilizes an IMU and camera to acquire the acceleration, angular velocity, and surrounding environmental information of UGVs. Then, it establishes a measurement constraint model for VIO based on the epipolar geometry and trifocal tensor geometry relationships between multiple images. Next, in the filtering algorithm, the H∞ criterion is introduced into the CMSCKF to improve the robustness of the VIO filtering algorithm. Based on the system uncertainty, a three-stage robust adaptive VIO filtering algorithm is adopted, comprehensively utilizing the standard Kalman filter algorithm and the H∞ filter algorithm to complement each other's advantages, thereby improving the overall performance of the filtering algorithm. This results in VIO integrated navigation not only having high filtering accuracy but also good robustness, thus improving the navigation and positioning performance of UGVs in complex environments.

Description

Technical Field

[0001] This invention relates to the field of vehicle navigation and positioning technology in GNSS denied environments, specifically a robust adaptive VIO navigation and positioning method suitable for GNSS denied environments, used to improve the navigation and positioning accuracy of vehicles when GNSS signals cannot be received. Background Technology

[0002] In recent years, unmanned ground vehicles (UGVs) have been the focus of extensive research. Their most common navigation and positioning method is to combine a Global Navigation Satellite System (GNSS) and an Inertial Navigation System (INS) into a GNSS / INS integrated navigation system. With the emergence of multiple satellite systems, such as my country's BeiDou BDS and the US GPS, UGV-based GNSS / INS integrated navigation can provide good positioning accuracy in open environments. However, in GNSS-denied environments, the navigation and positioning accuracy of UGVs will be severely affected. Therefore, the biggest challenge in UGV research is currently the autonomous navigation and positioning problem in GNSS-denied environments.

[0003] In GNSS-denied environments, vehicle-mounted GNSS / INS integrated navigation and positioning methods can generally be divided into two categories: First, advanced fusion algorithms, such as neural network algorithms, are used to simulate GNSS measurement information, thereby reducing positioning errors to some extent. However, the navigation accuracy of these algorithms cannot meet the needs of practical applications under long-term GNSS signal loss. Second, methods seek to integrate other sensors for navigation and positioning, which generally have a good complementary relationship with inertial measurement units (IMUs). With the rapid development of vision technology, visual navigation has become an indispensable part of UGV navigation in recent years.

[0004] Visual odometry (VO), an important branch of visual navigation, is a significant research hotspot. It utilizes cameras, such as monocular, binocular, and multi-view cameras, to acquire image sequences of the surrounding environment and then estimates the camera's pose and motion based on the geometric relationships between the images. Monocular cameras are widely used due to their low power consumption, cost, size, and low image processing requirements. However, monocular cameras are insufficient for some challenging environments, such as fast-moving, textureless environments, and rapidly changing lighting conditions, and they also suffer from scale uncertainty, making it difficult to recover the scale information of the environment. Monocular cameras and Inertial Navigation Systems (INS) have a good complementary relationship; combining them can form a VIO (Visual-inertial odometry) navigation and positioning system. The monocular camera can reduce errors caused by IMU bias, while the IMU can help the monocular camera eliminate scale uncertainty and provide observations when visual tracking is lost. Given the advantages of VIO integrated navigation systems, such as low power consumption, low cost, and high autonomy, it has gradually become a research hotspot and an important development direction for improving UGVS navigation accuracy in GNSS-denied environments.

[0005] Currently, various filter structures for VIO integrated navigation systems have been studied, among which the Multi-State Constraint Kalman Filter (MSCKF) is quite popular. To avoid estimating the 3D spatial positions of feature points and simplify the measurement update process of MSCKF, thereby reducing the computational complexity of the VIO integrated navigation algorithm, the measurement equations for VIO can be established using epipolar geometry and trifocal tensor geometry constraint models. This approach does not require calculating the 3D spatial coordinates of feature points or reconstructing the environment. Furthermore, since the VIO integrated navigation system is a high-dimensional nonlinear system, some researchers have proposed combining the capillary Kalman filter (CMSKF) with MSCKF to construct a capillary-based MSCKF (CMSCKF) algorithm, achieving good navigation and positioning performance.

[0006] However, these algorithms all fall under the category of standard Kalman filtering, which achieves good filtering accuracy when the statistical characteristics of system noise are precisely known and the system model is accurate. But when these assumptions are not met, such as when the system noise is colored noise, the statistical characteristics are not precisely known, or the system model is inaccurate, significant filtering errors will occur. In actual UGV movement, the VIO navigation system is inevitably affected by uncertainties inherent to itself and the external environment, severely challenging the robustness and reliability of its navigation and positioning results. Therefore, improving the robust adaptive capability of VIO integrated navigation in complex environments is a core challenge that urgently needs to be addressed to improve the navigation and positioning accuracy of UGVs under GNSS rejection conditions. Summary of the Invention

[0007] To improve the navigation and positioning accuracy of UGVs in complex environments without GNSS signals, this invention proposes a robust adaptive VIO navigation and positioning method suitable for GNSS denied environments.

[0008] This method first utilizes an IMU (Integrated Measurement Unit) to capture the acceleration and angular velocity information of UGVs, while simultaneously using a camera to capture information about their surrounding environment. Then, leveraging the inherent relationship between the IMU measurement data and the camera-captured images, a measurement constraint model for VIO (Vehicle Identification and Optimization) is established based on the epipolar geometry and trifocal tensor geometry relationships among multiple images. Next, in the filtering algorithm section, the H∞ criterion is introduced into the CMSCKF (Combined Microsystems and Components Filtering) to improve the robustness of the VIO filtering algorithm. Furthermore, a system uncertainty characterization method based on the constrained memory exponent weighting method is designed. Based on this, a three-stage robust adaptive VIO filtering algorithm is designed. Finally, the standard Kalman filter algorithm and the H∞ filter algorithm are combined to complement each other, improving the overall performance of the filtering algorithm. This results in VIO integrated navigation not only having high filtering accuracy but also good robustness, thereby improving the navigation and positioning performance of UGVs in complex environments.

[0009] The technical solution of this invention is as follows:

[0010] The robust adaptive VIO navigation and positioning method applicable to GNSS denied environments includes the following steps:

[0011] Step 1: Use an inertial measurement unit (IMU) and a camera to acquire acceleration, angular velocity information of UGVs and image information of the surrounding environment.

[0012] In GNSS-denied environments, the positioning accuracy of GNSS / INS integrated navigation commonly used in UGVs will be severely affected due to the inability to receive GNSS signals. Since a camera is a sensor with low power consumption, small size, light weight, and rich information about the surrounding environment, a forward-facing camera is installed in the UGV to capture environmental information in front of the vehicle. Simultaneously, an IMU is used to acquire the UGV's three-axis acceleration and three-axis angular velocity information, combining the IMU and camera to form a VIO (Visual-inertial Odometry) integrated navigation system.

[0013] Step 2: Construct the state model and measurement model of the VIO integrated navigation system.

[0014] First, based on the 15-dimensional error state model of the Inertial Navigation System (INS), and drawing on the ideas of the currently popular and high-performance Multi-state Constraint Kalman Filter (MSCKF) architecture, the state equations for VIO are established. Among them, the state variables... The selections are as follows:

[0015]

[0016]

[0017] Where I represents the IMU coordinate system. In VIO, the IMU is fixed to the vehicle body. In principle, it is desirable for the IMU to be completely coincident with the carrier coordinate system during installation. Therefore, without considering the installation error angle or after compensation, the IMU coordinate system can be considered to be the "right front upper" carrier coordinate system. G is the global coordinate system. In this invention, the IMU coordinate system corresponding to the first frame image of the camera is selected as the global coordinate system G. and These represent the attitude, velocity, and position errors of the INS, respectively. g and b a The gyroscope and accelerometer are respectively zero biased; and These represent the attitude error and position error of the two frames preceding the current INS error state, respectively.

[0018] Then, based on the intrinsic relationship between INS and visual camera measurement information, and the epipolar geometry and triangulation tensor geometry relationships between multiple images, the measurement constraint equations for VIO are constructed.

[0019] Step 3: Based on the state model and measurement model of the VIO integrated navigation system constructed in Step 2, design a suitable filtering algorithm for filtering estimation.

[0020] The VIO model constructed in step 2 is nonlinear. A capacitive MSCKF (CMSKCF) filter is constructed based on the capacitive Kalman filter and the MSCKF algorithm to obtain better nonlinear filtering performance. Meanwhile, to improve the robustness of the algorithm and ensure the navigation performance of the VIO integrated navigation system in complex environments, the H∞ criterion is further introduced into the CMSCKF, forming an HCMSCKF filtering algorithm.

[0021] To effectively utilize the designed CMSCKF and HCMSCKF algorithms and leverage their complementary advantages, this invention employs a three-stage robust adaptive VIO filtering algorithm. When the VIO navigation system is operating normally, the CMSCKF filtering algorithm is used for filtering estimation to obtain high-precision filtering results. However, when the VIO system exhibits uncertainty, the HCMSCKF algorithm is introduced to improve system robustness. This involves two scenarios: one where, when uncertainty is low, both CMSCKF and HCMSCKF are used simultaneously for filtering with weighted optimization; and another where, when uncertainty is high, only HCMSCKF is used for filtering estimation. Ultimately, this improves the overall performance of the VIO filtering algorithm, enabling it to achieve high filtering accuracy in complex environments while also possessing good robust adaptive capabilities, thereby enhancing the navigation and positioning performance of UGVs.

[0022] Furthermore, the VIO state equation established in step 2 is as follows:

[0023]

[0024] in, Angular velocity after deducting gyroscope zero drift antisymmetric matrix, This represents the acceleration after deducting the zero bias acceleration. antisymmetric matrix, Represent the quaternion expression for the rotation matrix from frame I to frame G. express Direction cosine matrix, n bg and n ba Gaussian white noise n for gyroscopes and accelerometers g and n a The first derivative.

[0025] Furthermore, the VIO measurement constraint equations established in step 2

[0026]

[0027]

[0028] Among them, z i (1) and z i (2) is z i The first and second rows of elements specifically represent the epipolar geometric relationships corresponding to the matching feature points between the first and second images, and between the second and third images in three consecutive images; z i (3) is z i The third row of elements specifically represents the trifocal tensor geometric relationship corresponding to the matching feature points between the three consecutive images; p j This represents the pixel position of the feature point in the pixel coordinate system corresponding to the three images; K is the camera's intrinsic parameter matrix, the parameter values ​​of which are provided by the camera manufacturer or obtained through camera calibration techniques; T i For tensor description; R 12 t 12 and R 23 t 23 These represent the rotation and translation matrices between the camera coordinate system corresponding to the first image and the camera coordinate system corresponding to the second image, and between the camera coordinate system corresponding to the second image and the camera coordinate system corresponding to the third image, respectively.

[0029] Furthermore, regarding the uncertainty of the VIO system, drawing on the idea of ​​a sliding window and emphasizing the role of recent data while gradually forgetting the role of old data, a method for characterizing the uncertainty of the VIO navigation system is designed here based on the weighted method of limited memory exponent:

[0030]

[0031]

[0032] Among them, z j These are actual observed values. For predicted values, To predict the residual vector, S is an adjustment factor satisfying S≥1, the specific value of which depends on the actual system. In this patent, S=1.5 is chosen. tr[·] represents finding the trace of the matrix in parentheses, h(·) is the measurement equation for VIO constructed in step 1, and ξ i，j / j-1 To calculate the sampling points using the capacitive Kalman filter algorithm, R j The covariance matrix of the measurement noise, ω j Let be the weighting factors at different times, and satisfy the following condition:

[0033]

[0034] Where b is the forgetting factor, and in this invention b = 0.9 is selected; M is the width of the sliding window, and in this invention M = 10 is selected.

[0035] Furthermore, when using both CMSCKF and HCMSCKF for filtering simultaneously and performing weighted optimization, the weighting factors w corresponding to CMSCKF and HCMSCKF are... CMSCKF and w HCMSCKF They are respectively:

[0036]

[0037] w HCMSCKF =1-w CMSCKF

[0038] Where ρ1 and ρ2 represent two thresholds for the system uncertainty.

[0039] Beneficial effects

[0040] This invention addresses the navigation and positioning problem of UGVs in GNSS-denied environments. To overcome the shortcomings of current VIO integrated navigation algorithms, it proposes a robust adaptive VIO navigation and positioning method suitable for GNSS-denied environments. This method first utilizes the inherent relationship between the IMU and camera images, establishing a measurement constraint model for VIO based on epipolar geometry and trifocal tensor geometry. This avoids the problem of calculating the three-dimensional spatial positions of image feature points, significantly reducing the computational load of the algorithm. Furthermore, the H∞ criterion is introduced into the existing CMSCKF filtering algorithm, improving the robustness of the VIO algorithm. Simultaneously, addressing the limitation of the H∞ filtering algorithm in sacrificing accuracy for robustness, a system uncertainty characterization method based on the constrained memory exponent weighting method is further designed. Drawing inspiration from the IGG III model, a three-stage robust adaptive VIO filtering algorithm is proposed. This enables the VIO navigation system to possess both high positioning accuracy and high robustness in complex environments, improving the navigation and positioning performance of UGVs in GNSS-denied environments.

[0041] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0042] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which:

[0043] Figure 1 : Flowchart of the present invention;

[0044] Figure 2 Positional errors of different methods;

[0045] The curves corresponding to the star-shaped, rhombus-shaped, circular, and square markers are respectively the position error diagrams of pure INS navigation and positioning (Method 1), VIO navigation and positioning based on CMSCKF (Method 2), VIO navigation and positioning based on HCMSCKF (Method 3), and the three-segment robust adaptive VIO navigation filtering algorithm based on system uncertainty designed in this invention (Method 4).

[0046] Figure 3 Attitude errors of different methods;

[0047] The curves corresponding to the star, rhombus, circle, and square markers represent the attitude errors when using methods 1, 2, 3, and 4 for navigation and positioning. Detailed Implementation

[0048] The embodiments of the present invention are described in detail below. These embodiments are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0049] This invention is a robust adaptive VIO navigation and positioning method applicable to GNSS denied environments, and its flowchart is as follows. Figure 1 As shown, it mainly includes three parts: the construction of the state equation and measurement equation of the VIO integrated navigation system, the prediction update module of the designed VIO filtering algorithm, and the measurement update module of the designed VIO filtering algorithm.

[0050] (1) Construction of state equations and measurement equations of VIO integrated navigation system

[0051] First, a forward-facing camera is installed on the UGVs to capture environmental information in front of the vehicles. Simultaneously, an IMU is used to acquire the UGVs' three-axis acceleration and three-axis angular velocity information, combining the IMU and camera into a VIO integrated navigation system. Then, drawing inspiration from the MSCKF algorithm, the selection of the VIO state variables is as follows:

[0052]

[0053]

[0054] Where I represents the IMU coordinate system, the IMU is fixed to the vehicle body, and without considering the installation error angle or after compensation, the IMU coordinate system can be considered as the "right front upper" carrier coordinate system; G is the global coordinate system, and in this invention, the IMU coordinate system corresponding to the first frame image of the camera is selected as the global coordinate system. b g and b a These are the attitude, velocity, and position errors of the INS, as well as the zero bias of the gyroscope and accelerometer; and Let represent the pose error and position error of the two preceding frames corresponding to the current INS error state, respectively. Based on the INS error model, and considering the fact that the pose of the two preceding frames at corresponding moments is a parameter from the past moment and does not change during the filtering and prediction process, the VIO state equation is established as follows:

[0055]

[0056] in, Angular velocity after deducting gyroscope zero drift antisymmetric matrix, This represents the acceleration after deducting the zero bias acceleration. antisymmetric matrix, Represent the quaternion expression for the rotation matrix from frame I to frame G. express Direction cosine matrix, n bg and n ba Gaussian white noise n for gyroscopes and accelerometers g and n a The first derivative.

[0057] Based on the aforementioned VIO state equations, and further considering the intrinsic relationship between IMU and camera measurement information, and the epipolar geometry and trifocal tensor geometry relationships between multiple images, the VIO measurement constraint equations are constructed as follows:

[0058]

[0059]

[0060] Among them, z i (1) and z i (2) is z i The first and second rows of elements specifically represent the epipolar geometric relationships corresponding to the matching feature points between the first and second images, and between the second and third images in three consecutive images; z i (3) is z i The third row of elements specifically represents the trifocal tensor geometric relationship corresponding to the matching feature points between the three consecutive images; p j This represents the pixel position of the feature point in the pixel coordinate system corresponding to the three images; K is the camera's intrinsic parameter matrix, the parameter values ​​of which are provided by the camera manufacturer or obtained through camera calibration techniques; T i For tensor description; R 1，2 t 1，2 and R 2，3 t 2，3These represent the rotation and translation matrices between the camera coordinate system corresponding to the first image and the camera coordinate system corresponding to the second image, and between the camera coordinate system corresponding to the second image and the camera coordinate system corresponding to the third image, respectively.

[0061] (2) Prediction update module of the designed VIO filtering algorithm

[0062] To better illustrate the designed VIO filtering algorithm, the VIO state equation and measurement equation established by formulas (3) and (4) in Part (1) are expressed as a nonlinear discrete stochastic system as shown below:

[0063] x k =f(x) k-1 )+w k-1 (6)

[0064] z k =h(x k )+v k (7)

[0065] Where, x k Let z be the state variable to be estimated in the system; zk be the quantity to be measured in the system; w k-1 and v k These are the system's process noise and measurement noise, respectively.

[0066] Thus, through the prediction update step of the proposed VIO filtering algorithm, the predicted value of the state vector is obtained. and its corresponding error covariance matrix P k / k-1 This can be accomplished by the following formula:

[0067]

[0068]

[0069] X i，k / k-1 =f(ξ i，k-1 ), i = 1, 2, ..., 2n (10)

[0070] Where n represents the dimension of the state vector; ξ i and w i Let i and n represent the i-th sampling point and its corresponding weight, respectively. This value is obtained using the third-order spherical radial law shown in the following formula:

[0071]

[0072] Among them, e i It is a unit vector whose i-th element is 1. and P x These are the state estimates obtained at the previous time step. and the corresponding error covariance matrix P k-1 .

[0073] (3) Measurement update module of the designed VIO filtering algorithm

[0074] For VIO navigation in practical applications, the environment is constantly changing, and therefore the system noise is also constantly changing. In this case, the importance of recent data should be emphasized, while older data should be gradually forgotten. Therefore, this invention proposes a more accurate method for judging the uncertainty of filters based on the limited memory exponential weighting method, which is accomplished by the following formula:

[0075]

[0076]

[0077] Among them, z j These are actual observed values; This is a predicted value; To predict the residual vector; S is the adjustment factor, satisfying S≥1, its specific value depends on the actual system, here S=1.5; tr[·] indicates finding the trace of the matrix in parentheses, R j The covariance matrix of the measurement noise; ω j The weighting factors at different times satisfy the following condition:

[0078]

[0079] Where b is the forgetting factor, and in this embodiment b = 0.9 is selected; M is the window width, and here M = 10 is selected.

[0080] After calculating the uncertainty of the VIO system using equation (12), drawing on the idea of ​​the IGG III model commonly used in the field of GNSS navigation and positioning, the uncertainty is calculated... The magnitude of the uncertainty is categorized into three cases: no uncertainty, small uncertainty, and large uncertainty, corresponding to... as well as ρ1 and ρ2 represent two threshold values ​​for system uncertainty; in this invention, ρ1 = 1.5 and ρ2 = 30 are chosen. This results in three different measurement update steps:

[0081] 1) When the VIO navigation system has no uncertainty, i.e., is operating normally, only the existing CMSCKF filtering algorithm is used for filtering estimation to obtain high-precision filtering results. At this time, w can be considered as... CMSCKF =1, w HCMSCKF =0, the update steps are as follows:

[0082] make Px =P k / k-1 Substitute into equation (11) to solve for the corresponding sampling point ξ i，k / k-1 and weight w i Thus, the state estimate is obtained. and its corresponding error covariance matrix P k / k as follows:

[0083]

[0084]

[0085]

[0086]

[0087]

[0088]

[0089] 2) When the uncertainty of the VIO system is large, the H∞ criterion is introduced to construct the HCMSCKF for filtering estimation to improve the robustness of the system. In this case, w can be considered as CMSCKF =0, w HCMSCKF =1. Based on equations (15)-(19), equation (20) is calculated using the following formula to obtain the measurement update steps for HCMSCKF after introducing the H∞ criterion:

[0090]

[0091]

[0092] Where γ is the adjustment parameter of the H∞ filter, and its values ​​are as follows:

[0093]

[0094] 3) When the uncertainty of the VIO system is small, the joint filtering estimation using CMSCKF and HCMSCKF is employed. The weights for both are calculated as follows:

[0095]

[0096] w HCMSCKF =1-w CMSCKF (25)

[0097] Using the weights calculated by equations (24) and (25), the filtered estimates obtained by the CMSCKF and HCMSCKF algorithms are weighted and optimized respectively.

[0098] Finally, by combining the advantages of the CMSCKF and HCMSCKF algorithms through the designed three-stage robust adaptive VIO algorithm, the overall performance of the filtering algorithm is improved, so that the system has high-precision filtering performance and high robust adaptive capability.

[0099] In this embodiment, a set of vehicle motion data collected in a residential area is used to verify the effectiveness of the invention by using uncertainties in the VIO navigation system model or noise caused by the navigation system itself, motion state and external environment, such as trees, houses, changes in lighting and motion blur. Here, the following four algorithms will be compared for position and attitude errors: (1) pure INS navigation positioning, denoted as Method 1; (2) VIO navigation positioning based on CMSCKF algorithm, denoted as Method 2; (3) VIO navigation positioning based on HCMSCKF algorithm, denoted as Method 3; (4) the three-stage robust adaptive VIO navigation positioning based on system uncertainty designed in this invention, denoted as Method 4.

[0100] Figure 2 and Figure 3 The position and attitude errors of different navigation and positioning methods are shown. It can be seen that the position and attitude errors of method 1 are small in the short term because it only uses INS. However, these errors will increase rapidly over time, with the maximum position and attitude errors reaching 2486.910m and 14.351°, respectively. The other three methods, which incorporate a monocular camera to form a VIO navigation system, have better suppression of position and attitude errors. Due to the inherent uncertainties in the VIO system, Method 2 achieves maximum position and attitude errors of 18.550m and 2.499°, respectively. Method 3, by introducing the H∞ criterion, enhances the algorithm's robustness, achieving maximum position and attitude errors of 11.482m and 1.538°, respectively. However, as mentioned earlier, it has certain limitations and drawbacks. Method 4, designed in this invention, is a three-stage robust adaptive VIO algorithm that can adapt to the degree of uncertainty in the VIO system. It comprehensively utilizes the advantages of Methods 2 and 3, complementing each other to improve the overall performance of the filtering algorithm. This results in VIO integrated navigation possessing not only high filtering accuracy but also good robustness in complex environments, thereby improving the navigation and positioning performance of UGVs. Method 4 achieves the best navigation performance, with maximum position and attitude errors of 8.076m and 0.984°, respectively.

[0101] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. A robust adaptive VIO navigation and positioning method suitable for GNSS denied environments, characterized in that: Includes the following steps: Step 1: Use an inertial measurement unit (IMU) and a camera to acquire the vehicle's acceleration, angular velocity information, and image information of the surrounding environment; the IMU and camera form a VIO integrated navigation system; Step 2: Construct the state model and measurement model of the VIO integrated navigation system: Based on the error state model of the inertial navigation system, a multi-state constrained Kalman filter architecture is adopted to establish the state model of VIO. In the VIO state model, state variables The selections are as follows: in, Indicates the IMU coordinate system. It is a global coordinate system. , and These are the attitude, velocity, and position errors of the INS, respectively. and The gyroscope and accelerometer are respectively zero biased; and These represent the attitude error and position error of the two frames preceding the current INS error state, respectively. The established VIO state model is as follows: in, Angular velocity after deducting gyroscope zero drift antisymmetric matrix, This represents the acceleration after deducting the zero bias acceleration. antisymmetric matrix, Represent the quaternion expression for the rotation matrix from frame I to frame G. express The direction cosine matrix, and Gaussian white noise for gyroscopes and accelerometers and The first derivative; Based on INS and camera measurement information, a measurement model for VIO is constructed based on the epipolar geometry and triangulation tensor geometry relationships between multiple images. The established VIO measurement model is as follows: in, and yes The first and second row elements specifically represent the epipolar geometric relationships corresponding to the matching feature points between the first and second images, and between the second and third images in three consecutive images; yes The third row of elements specifically represents the geometric relationship of the trifocal tensor corresponding to the matching feature points between the three consecutive images; This indicates the pixel position of the feature point in the pixel coordinate system corresponding to the three images; This is the intrinsic parameter matrix of the camera; Described by tensors; , and , These represent the rotation and translation matrices between the camera coordinate system corresponding to the first image and the camera coordinate system corresponding to the second image, and between the camera coordinate system corresponding to the second image and the camera coordinate system corresponding to the third image, respectively. Step 3: Based on the state model and measurement model of the integrated navigation system constructed in Step 2, a three-segment robust adaptive VIO filtering algorithm based on the CMSCKF algorithm and the HCMSCKF algorithm is used for filtering estimation to obtain the final positioning result.

2. The robust adaptive VIO navigation and positioning method for GNSS denied environments as described in claim 1, characterized in that: Choose the IMU coordinate system corresponding to the first frame image from the camera as the global coordinate system. Tie.

3. The robust adaptive VIO navigation and positioning method for GNSS denied environments as described in claim 1, characterized in that: Step 3, the prediction update process of the VIO filtering algorithm is as follows: Rewrite the VIO state model and measurement model expressions as a nonlinear discrete stochastic system: in, The state variables of the system to be estimated; For system measurement; and These are the system's process and measurement noise, respectively. Predicted value of state vector and its corresponding error covariance matrix This can be accomplished by the following formula: in, The dimension of the state vector; and They represent the first Each sampling point and its corresponding weight are obtained using the third-order spherical radial law shown in the following formula: in, It is the first A unit vector with each element equal to 1. and These are the state estimates obtained at the previous time step. and the corresponding error covariance matrix .

4. The robust adaptive VIO navigation and positioning method for GNSS denied environments as described in claim 3, characterized in that: Step 3, the measurement update process of the VIO filtering algorithm is as follows: Based on the uncertainty of the VIO system The uncertainty is divided into three cases: no uncertainty, small uncertainty, and large uncertainty, which correspond to... , as well as , and Two thresholds representing system uncertainty; When the VIO navigation system has no uncertainty, the CMSCKF filtering algorithm is used for filtering estimation. make , Substitute these values ​​into the third-order spherical radial law to solve for the corresponding sampling points. and weight Thus, the state estimate is obtained. and its corresponding error covariance matrix as follows: When the uncertainty of the VIO system is large, the H∞ criterion is introduced to construct the HCMSCKF for filtering estimation: Obtain the state estimate and its corresponding error covariance matrix as follows: in, The adjustment parameters for the H∞ filter are as follows: When the uncertainty of the VIO system is small, the joint filtering estimation using CMSCKF and HCMSCKF is performed, and the corresponding weights are calculated as follows: Using the calculated weights, the filtered estimates obtained by the CMSCKF and HCMSCKF algorithms are weighted and optimized to obtain the localization result.

5. The robust adaptive VIO navigation and positioning method for GNSS denied environments according to claim 4, characterized in that: and .

6. The robust adaptive VIO navigation and positioning method for GNSS denied environments according to claim 4, characterized in that: Uncertainty of VIO system Calculated using the following formula: in, These are actual observed values. For predicted values, To predict the residual vector, As a regulating factor, satisfying , This indicates finding the trace of the matrix within the parentheses. For VIO measurement models, To calculate the sampling points using the capacitive Kalman filter algorithm, The covariance matrix of the measurement noise, Let be the weighting factors at different times, and satisfy the following condition: in, Forgetting factor, The width of the sliding window.

7. The robust adaptive VIO navigation and positioning method for GNSS denied environments as described in claim 6, characterized in that: ， ， 。

Images