A combined navigation method and system based on elastic adjustment

Abstract

The present application belongs to the technical field of integrated navigation, and particularly relates to an integrated navigation method and system based on elastic adjustment. The whole method first obtains a VIO positioning result by a tight integrated method, and then obtains a final positioning result of the system by a loose integrated method. In the tight integrated method, an elastic adjustable scheme is used to adjust a covariance factor, and then the visual covariance is changed, so that the accuracy of the sensor is effectively used in an elastic manner, and the global fusion positioning is smoothly and accurately completed. Moreover, a sensor fusion mode based on a factor graph is used to fuse the signals of the visual sensor and the data of the inertial sensor, so that the other sensors can still be normally integrated in the case that one kind of sensor fails and has no data, and the sensor plug and play is achieved.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation technology, specifically relating to an integrated navigation method and system based on elastic adjustment. Background Technology

[0002] In modern warfare, the physical combat space has evolved from land and sea-based surface battlefields to a multi-dimensional, three-dimensional battlefield integrating land, sea, air, space, electronic warfare, and indoor / outdoor environments. Battlefield space management is becoming increasingly complex and precise, placing ever higher demands on the support of operational big data. Navigation has transformed from an auxiliary means into a crucial element of warfare, especially in complex battlefield environments where navigation and positioning have become prerequisites for modern warfare. Secondly, in the civilian sector, single navigation methods cannot cope with all complex real-world scenarios. For example, in tunnels and urban canyons, GNSS signals become very poor or unusable, severely impacting vehicle and pedestrian navigation. Therefore, multi-sensor fusion navigation has become a trend in solving navigation and positioning problems in complex real-world scenarios.

[0003] With the improvement of integrated positioning, navigation, and time (PNT) systems and flexible PNT frameworks, multi-source PNT data fusion technology is constantly developing towards intelligent and adaptive fusion. To prevent defense operations from over-reliance on the Global Positioning System (GPS), foreign countries have begun to plan national integrated PNT architectures. Multi-source PNT systems are widely used on unmanned autonomous platforms such as unmanned underwater vehicles and drones, and represent a key direction for the future development of autonomous navigation in unmanned systems. As a type of multi-source PNT system, the fusion of a monocular camera and a low-cost inertial measurement unit (IMU) in a GNSS / INS / visual integrated navigation system can significantly improve local positioning accuracy and robustness in the event of GNSS failure. When GNSS positioning is effective, it can provide global positioning trajectory correction for visual / INS integrated navigation position calculation, which is of great significance for researching low-cost, high-precision, and high-reliability navigation methods.

[0004] Currently, in many open-source visual-inertial navigation (VIS) integration algorithm frameworks, the visual covariance is fixed at an initial value and remains unchanged after nonlinear optimization, such as the fixed covariance in VINS-Mono. The covariance factor σ in the figure is a fixed value. Where f is the virtual focal length, and the reprojection error is assumed to be at the 1.5 pixel level on the image plane, this ensures that the visual weight is fixed in any scene. 1.5 pixels is a stable empirical threshold that remains constant regardless of sensor equipment and external environment, such as when using different camera sensor models or when the observed scene changes drastically. This will no longer be applicable, which undoubtedly reduces the positioning and attitude determination accuracy of the integrated navigation method. Summary of the Invention

[0005] The purpose of this invention is to provide a combined navigation method and system based on elastic adjustment, in order to solve the problem of low positioning and attitude determination accuracy of combined navigation positioning methods caused by a fixed covariance factor.

[0006] To address the aforementioned technical problems, this invention provides a combined navigation method based on elastic adjustment, comprising the following steps:

[0007] 1) Using the factor graph optimization method, the state variables to be optimized in the visual inertial odometry are used as state nodes, and the visual projection error and IMU pre-integration error are used as factor nodes to construct a compact combination factor graph model. The error equation in the compact combination factor graph model is solved iteratively to achieve the first nonlinear optimization.

[0008] 2) After the first nonlinear optimization, the unit weighted mean square error of the reprojection error is input into the covariance adjustment model to adjust the visual covariance. Then, the compact combination factor graph model is reconstructed using the adjusted visual covariance, and the error equation in the new compact combination factor graph model is iteratively solved to achieve the second nonlinear optimization, thereby obtaining the navigation results of visual inertial odometry. Among them, the larger the unit weighted mean square error, the larger the new covariance factor in the adjusted visual covariance.

[0009] 3) Combine navigation based on the navigation results from visual inertial odometry and GNSS positioning results.

[0010] Its beneficial effects are as follows: Since the unit weighted mean square error of visual reprojection reflects the overall visual quality, the larger the unit weighted mean square error, the greater the influence of environmental or carrier motion on positioning, the worse the visual measurement quality, and the less reliable the measurement results. Therefore, this invention adopts a flexible and adjustable scheme to adjust the covariance factor, thereby changing the visual covariance, realizing flexible and effective utilization of sensor accuracy, and ensuring smooth and accurate completion of global fusion positioning. Moreover, by using a factor map-based sensor fusion method to fuse the signals from the visual sensor and the data from the inertial sensor, it is possible to ensure that other sensors can still be combined normally even if one sensor fails and has no data, achieving plug-and-play sensor functionality.

[0011] Furthermore, the covariance adjustment model is as follows:

[0012]

[0013]

[0014] In the formula, σ′ is the new covariance factor; σ is the unit weight error calculated based on the a posteriori residual; k is the confidence factor; lg(·) is a logarithmic function with base 10; For visual covariance.

[0015] Its beneficial effects are as follows: introducing a logarithmic function and confidence factor to achieve elastic adjustment of the unit weight error as a new covariance factor can greatly improve the sensitivity of the model function to changes, making the response to the visual unit weight error stronger; moreover, the input of the covariance adjustment function is the unit weight error of the apothetical visual reprojection residual, which can reflect the quality of visual measurement to a certain extent. Using it to adjust the visual covariance matrix can truly reflect the weight ratio of visual measurement and inertial measurement, and make more flexible and effective use of the sensor's accuracy.

[0016] Furthermore, the covariance adjustment model is as follows:

[0017]

[0018]

[0019] In the formula, σ′ is the new covariance factor; σ is the unit weight error calculated based on the a posteriori residual; k is the confidence factor; lg(·) is a logarithmic function with base 10; For visual covariance.

[0020] Its beneficial effects are as follows: introducing a confidence factor to achieve flexible adjustment of the unit weight mean square error as a new covariance factor can improve the sensitivity of the model function to a certain extent; moreover, the input of the covariance adjustment function is the unit weight mean square error of the post-approval visual reprojection residual, which can reflect the quality of visual measurement to a certain extent. Using it to adjust the visual covariance matrix can truly reflect the weight ratio of visual measurement and inertial measurement, and make more flexible and effective use of the sensor's accuracy.

[0021] Furthermore, when constructing the compact combination factor graph model in step 1), a sliding window based on keyframes is used, and the factor nodes also include marginalization factors. If the second newest frame in the sliding window is a keyframe, the oldest frame in the sliding window is marginalized; otherwise, the second newest frame is marginalized and the marginalization factor is added to the next construction of the compact combination factor graph model.

[0022] Furthermore, in step 1), the Gauss-Newton method is used for iterative solution, and the incremental equation corresponding to the overall objective function is:

[0023]

[0024] In the formula, Let be the Jacobian matrix of the pre-integral factors formed between the k-th carrier state variable and the (k+1)-th carrier state variable. b is the Jacobian matrix of the reprojection factor with respect to the observations of the landmark point l; p The marginalization constant term; ΔX is the increment of the quantity to be optimized; H p For marginalized prior information; r is the covariance of the pre-integrated noise term formed between the k-th carrier state variable and the (k+1)-th carrier state variable; B and r C These are the IMU pre-integration factor and the visual reprojection factor, respectively.

[0025] Its beneficial effects are: the Gauss-Newton method has a fast calculation speed and high efficiency, which improves the efficiency of integrated navigation and positioning.

[0026] Furthermore, in step 3), the method for combining navigation based on the visual inertial odometry (VIO) results and GNSS positioning results is as follows: VIO factors and GNSS factors are established using the factor graph optimization method, thereby constructing a loose combination factor graph model. The error equations in the loose combination factor graph model are iteratively solved to obtain the combined navigation results of VIO and GNSS.

[0027] Its beneficial effects are as follows: by using a factor graph-based sensor fusion method to fuse the navigation results of visual inertial odometry and the positioning results of GNSS, it is possible to ensure that other sensors can still be combined normally even if one sensor fails and has no data, thus achieving plug-and-play sensor functionality.

[0028] Furthermore, before step 1), the visual inertial odometry needs to be initialized. The initialization process includes: firstly, using visual measurements to recover the initial motion structure, and using the PnP method to solve the pose of the visual sensor in the visual inertial odometry; then, combining IMU pre-integration measurements to perform visual-inertial coarse alignment in order to estimate the initial state of the visual inertial odometry.

[0029] Its beneficial effect is that the above processing method can ensure that the VIO system can operate under a set of excellent initial values.

[0030] Furthermore, the state variables to be optimized within the sliding window are:

[0031]

[0032]

[0033] In the formula, X represents the state variable to be optimized; n and m represent the number of state vectors to be optimized within the sliding window and the number of all observed feature points, respectively. and Let ρ represent the translation vector and rotation quaternion from the camera coordinate system to the carrier coordinate system, respectively; i The inverse depth of the feature point is represented by i = 1, 2, ..., m; and These represent the translation vector and rotation quaternion from the carrier coordinate system to the world coordinate system corresponding to the k-th state vector, respectively. b represents the velocity of the carrier in the world coordinate system corresponding to the k-th state vector; a and b ω These represent the zero bias of the IMU accelerometer and gyroscope, respectively.

[0034] Furthermore, the VIO factor is the rotation and translation residual between two adjacent states, and the GNSS factor is the difference between the variable to be optimized and the GNSS measurement.

[0035] To address the aforementioned technical problems, this invention also provides a flexible adjustment-based integrated navigation system, comprising a visual sensor, an inertial navigation / GNSS receiving module, an algorithm processing core module, and a power supply module. The inertial navigation / GNSS receiving module receives GNSS signals and IMU data. The visual sensor acquires images and sends them to the algorithm processing core module. The power supply module provides power to the algorithm processing core module. The algorithm processing core module executes program instructions to implement the flexible adjustment-based integrated navigation method described above.

[0036] Its beneficial effects are as follows: This elastically adjustable integrated navigation system provides hardware support for realizing the elastically adjustable integrated navigation method of the present invention, ensuring the effective and reliable execution of the method. Attached Figure Description

[0037] Figure 1 This is a schematic diagram of the overall structure of the system prototype of this invention;

[0038] Figure 2 This is a diagram showing the core algorithm components of the elastically adjustable integrated navigation method of this invention;

[0039] Figure 3 This is a framework diagram of the visual-inertial linear compact combination algorithm based on the covariance adjustment model of the present invention;

[0040] Figure 4 This is a schematic diagram of the observation model of the present invention;

[0041] Figure 5 This is the algorithm factor graph model diagram of the present invention;

[0042] Figure 6 This is a diagram of the covariance adjustment function model of the present invention;

[0043] Figure 7 This is a diagram illustrating the mechanism of action of the covariance matrix in this invention;

[0044] Figure 8 This is a diagram of the loose combination algorithm framework of the present invention;

[0045] Figure 9 This is the factor graph of the loose combination algorithm of the present invention;

[0046] Figure 10 This is a schematic diagram of the prototype system of the present invention;

[0047] Figure 11 This is a physical diagram of the prototype system of the present invention.

[0048] Among them, 1-power battery, 2-monocular camera, 3-NVIDIA Jetson Xavier NX development board, 4-XsensMti-7 inertial / GNSS module, 5-ceramic antenna. Detailed Implementation

[0049] This invention primarily utilizes a tightly coupled visual / INS algorithm and a loosely coupled visual inertial odometry (VIO) / GNSS algorithm to design a flexible GNSS / INS / visual integrated navigation method and system, integrating visual navigation, inertial navigation, and satellite navigation technologies. To make the objectives, technical solutions, and advantages of this invention clearer, the following detailed description, in conjunction with the accompanying drawings and embodiments, further illustrates the invention.

[0050] Method Implementation Examples:

[0051] The elastically adjustable integrated navigation method in this embodiment is a visual-inertial navigation linear compact integration method based on a covariance adjustment model. The hardware system implementing this method consists of three parts: an image sensor (monocular camera), an inertial navigation / GNSS receiver module (Xsens Mti-7), and an algorithm processing core module (NVIDIA Jetson Xavier NX development board). Figure 1As shown, the monocular camera is responsible for acquiring images, the Xsens Mti-7 module is responsible for receiving raw IMU data and GNSS positioning information, and the NVIDIA Jetson Xavier NX development board is responsible for data reception and core navigation algorithm processing. The operating system is Ubuntu 18.04, and the entire method runs in a Robot Operating System (ROS) environment. IMU and GNSS information can be synchronized in hardware via the Xsens Mti-7 module, while soft synchronization technology is used to estimate the time delay between camera image information and the IMU.

[0052] It should be emphasized that this method is aimed at the positioning and pose estimation accuracy in real-time navigation scenarios. It only evaluates the forward pose estimation algorithm and does not consider the correction of global pose by loop closure detection. Furthermore, the following uses (·) b This represents the IMU coordinate system, also known as the carrier coordinate system; (·) w This represents the world coordinate system, which is formed by adjusting the initial carrier coordinate system to horizontal; (·) c Indicates the camera coordinate system, with superscripts and subscripts indicating directions, for example... Indicates a transformation from the carrier coordinate system to the world coordinate system; superscript indicates the transformation. The measured value is represented by the rotation matrix R and the rotation quaternion q. The nonlinear optimization algorithm optimizes the quaternion.

[0053] This method comprises a visual / INS elastic tight combination method based on a covariance adjustment model and a loose combination method of GNSS and VIO, specifically as follows: Figure 2 As shown, the VIO positioning result is first obtained using the tight combination method, and then the final positioning result of the system is obtained using the loose combination method. This will be explained in detail below.

[0054] Step one involves implementing a visual-inertial linear compact combination method based on a covariance adjustment model. The specific framework of this method is as follows: Figure 3 As shown, the process comprises five parts: data acquisition and processing, data management, VIO initialization, backend, and re-optimization. The implementation process is as follows: Using the factor graph optimization method, the state variables to be optimized in the visual inertial odometry (VIO) are used as state nodes, and the visual projection error and IMU pre-integration error are used as factor nodes to construct a compact combination factor graph model. The error equations in the compact combination factor graph model are iteratively solved to achieve the first nonlinear optimization. Then, the unit weighted mean square error of the reprojection error is input into the covariance adjustment model to adjust the visual covariance. The adjusted visual covariance is used to reconstruct the compact combination factor graph model, and the error equations in the new compact combination factor graph model are iteratively solved to achieve the second nonlinear optimization, thus obtaining the navigation results from the visual inertial odometry. This will be described in detail below.

[0055] 1) The data acquisition and processing section is primarily responsible for receiving and preprocessing measurement data from the camera and IMU sensors. The vision front end is consistent with VINS-Mono, using images acquired by the camera and angular velocity measurements from the gyroscope and accelerometer measurements from the IMU as raw data input. The vision front end employs a combination of good feature to track (GFTT) and LK optical flow tracking, a strategy characterized by high real-time performance and high mobility. For processing IMU measurements, an IMU pre-integration model is used.

[0056] 2) The data management section manages IMU pre-integration and image feature point data information by using a pre-integration stack and feature point container.

[0057] 3) The VIO initialization section is responsible for estimating parameters such as the IMU's gyroscope bias, gravity direction, and scale factor to ensure normal system operation. Specifically, VIO initialization first utilizes visual measurements for short-term initial structure from motion (SfM), mainly by triangulating spatial points detected in the image over a period of time to determine their three-dimensional coordinates in the camera coordinate system, and using the perspicitve-n-point (PnP) method to solve for the camera pose. Then, combined with IMU pre-integration measurements, short-term visual-inertial coarse alignment is performed to estimate the initial states of the system, such as gyroscope bias, velocity, gravity, and scale, ensuring that the VIO system can operate with a set of excellent initial values.

[0058] 4) The backend is responsible for non-linear optimization of the sliding window composed of multiple image frames. It will determine the key frame for the next newest frame. If the next newest frame is a key frame, the oldest frame in the sliding window will be marginalized. Otherwise, the next newest frame will be marginalized and the marginalization factor will be added to the construction of the next sliding window factor graph model.

[0059] Specifically, after system initialization, a factor graph model is constructed using a keyframe-based sliding window for nonlinear optimization. The factor graph model consists of state nodes of the sliding window and factor nodes such as visual reprojection factors, IMU pre-integration factors, and marginalization factors. Each sensor factor represents the residual of the error equation established by that type of sensor. The marginalization factor is a constraint imposed by fixing some old state variables during sliding window updates; it is obtained by performing Schur complement operations on the previous sensor factors. The nonlinear optimization in the backend involves iteratively solving the error equations in the factor graph to achieve the first nonlinear optimization. The following sections will detail the construction of the compactly combined factor graph model, the visual reprojection factors, and the IMU pre-integration factors.

[0060] ① Construction of a compact combination factor graph model.

[0061] This compact combination method improves real-time performance by using a specified sliding window length and avoids redundant optimization. The image frame within the sliding window consists of the current frame and previous keyframes. The algorithm determines the state vector to be optimized based on the keyframes and the current frame, and defines all state variables to be optimized within the sliding window as follows:

[0062]

[0063] In the formula, n and m represent the number of state vectors to be optimized within the sliding window and the number of all observed feature points, respectively; and Let ρ represent the translation vector and rotation quaternion from the camera coordinate system to the carrier coordinate system, respectively; i (i = 1, 2, ..., m) represents the inverse depth of the feature point, which characterizes the spatial scale of the feature point; and These represent the translation vector and rotation quaternion from the carrier coordinate system to the world coordinate system corresponding to the k-th state vector, respectively. b represents the velocity of the carrier in the world coordinate system corresponding to the k-th state vector; a and b ω These represent the zero bias of the IMU accelerometer and gyroscope, respectively.

[0064] Observation model such as Figure 4 As shown, within the sliding window, the camera sampling frequency represents the carrier motion. There is a certain degree of image overlap between adjacent frames, and the same feature point may be observed in multiple images. As the carrier moves, new image frames are continuously selected as keyframes x. k If the newest frame is identified as a keyframe, the oldest keyframe x1 within the sliding window will be marginalized. Simultaneously, there is a period of IMU measurements between adjacent frames. Taking a sliding window with n=11 (i.e., a window length of 11) as an example, the specific algorithm factor graph model is as follows: Figure 5 As shown, there is a pre-integration factor b constructed by IMU measurements among the state variables within the sliding window. k Image frames that observe the same feature points constitute the visual reprojection factor c. j When the sliding window moves and a new keyframe enters the sliding window, the old state variables are marginalized, that is, their state vectors are fixed and no longer added to the sliding window optimization. However, the state vectors in the new sliding window are constrained by the state variables of the keyframe. These constraints include the IMU pre-integration constraint between the marginalized frame and the oldest frame in the new sliding window, the reprojection error constraint with the marginalized frame as the first observation frame, and the previous marginalization constraint. These constraints are added to the nonlinear constraints as the marginalization factors of the sliding window.

[0065] Construct a nonlinear optimization overall objective cost function using a sliding window factor graph model:

[0066]

[0067] In the formula, r p and H p The prior information for marginalization is the marginalized residual and the marginalized Hessian matrix; For IMU pre-integration factor; For visual reprojection factor; B represents the measured value; C represents the set of IMU pre-integration sequences; and D represents the set of image frame sequences and all feature points. This represents the covariance of the pre-integrated noise term formed between the k-th carrier state variable and the (k+1)-th carrier state variable; This represents the noise covariance of visual observations.

[0068] When using the Gauss-Newton method for iterative optimization, the incremental equation corresponding to the overall objective function of equation (2) is:

[0069]

[0070] In the formula, Let be the Jacobian matrix of the pre-integral factors formed between the k-th carrier state variable and the (k+1)-th carrier state variable. Let b be the Jacobian matrix of the reprojection factor with respect to the observations of the landmark point l. p ΔX is the marginalization constant term; ΔX is the increment of the quantity to be optimized.

[0071] As can be seen from the incremental equation, the covariance matrix affects the iterative optimization process. When the visual reprojection covariance... The larger the information matrix, the better. The smaller the value, the smaller the impact on the incremental equation. Therefore, the iterative process will rely more on the IMU pre-integrated observations, and vice versa. The relative magnitude of the covariance between the visual and IMU values ​​affects the final optimization result.

[0072] ②Visual reprojection factor.

[0073] Defining the reprojection error of the visual front end on a unit sphere has the advantage that it can be modeled as unit rays connecting the unit sphere for wide-angle cameras, fisheye cameras, etc. For feature point l, considering the first frame image sequence observing l as i, then when feature point l is observed again in image sequence j, the projection error is defined as:

[0074]

[0075] In the formula, It is the image plane coordinate of feature l on image sequence i when it is first observed, in pixels; These are the image plane coordinates observed on image sequence j; and These are the rotation matrix and translation vector between the camera coordinate system and the vehicle coordinate system, respectively, with subscripts and superscripts indicating directions; and π represents the rotation matrix and translation vector between the carrier coordinate system and the world coordinate system, with superscripts and subscripts indicating the direction; c -1 (·) represents the transformation function from the pixel plane to a unit vector; ρ l The inverse depth of the feature point indicates the scale relationship between the unit spherical vector and the coordinates of the actual point in space.

[0076] For pinhole cameras, scaling from the normalized plane to the unit sphere can be implicitly contained in the inverse depth λ of the feature points. l In the estimation, thus π c -1 (·) can be directly expressed as an intrinsic parameter transformation formula from pixel coordinates to normalized planar coordinates:

[0077]

[0078] In the formula, [u,v] represents the pixel coordinates of the point, and [u0,v0] represents the principal point coordinates f. x and f y These represent the focal lengths in the x and y directions of the image coordinate system, respectively.

[0079] The visual reprojection error factor is expressed as:

[0080]

[0081] In the formula, e1 and e2 are a set of orthogonal bases on the tangent plane of the unit sphere.

[0082] ③IMU pre-integration factor.

[0083] The carrier coordinate system is consistent with the IMU measurement coordinate system, and the measurement model of the IMU gyroscope and accelerometer can be expressed by the following formula:

[0084]

[0085] In the formula, b ω and ω represents the measured values ​​from the gyroscope and accelerometer; a represents the actual values ​​from the gyroscope and accelerometer. g is the rotation matrix from the world coordinate system to the carrier coordinate system. w n represents the gravitational acceleration in the world coordinate system. ω and na The additive noise from the gyroscope and accelerometer is processed using a Gaussian white noise model. b ω and b a Given a gyroscope and zero bias acceleration, it is a random walk model with derivatives of Gaussian white noise.

[0086] Considering continuous-time IMU pre-integration, taking two time points i and j, based on time point i, the position, velocity, and attitude at time point j can be expressed as:

[0087]

[0088] In the formula, This represents quaternion multiplication.

[0089] Define the pre-integral quantities for position, velocity, and attitude:

[0090]

[0091] The IMU predicted score is discretized using the median method in the algorithm, and is represented by pre-integration propagation. Combining the above two equations, the IMU pre-integration factor and the IMU pre-integration error are defined as follows:

[0092]

[0093] In the formula, r represents the residual; [·] xyz This represents a three-dimensional vector consisting only of the imaginary parts of the quaternions.

[0094] 5) The re-optimization section utilizes a covariance adjustment model to re-optimize the compact combination factor graph model constructed in the backend, achieving more accurate navigation results. Specifically, after the first nonlinear optimization in the backend, the unit weighted mean square error of the reprojection error is used as the input to the covariance adjustment model function to adjust the visual covariance. In the subsequent re-optimization stage, the factor graph model is reconstructed using the new visual covariance matrix, undergoing a second nonlinear optimization to finally obtain the combined navigation result. The following sections will elaborate on the covariance adjustment model based on the unit weighted mean square error and the re-optimization process.

[0095] ① Covariance adjustment model based on unit weight mean square error.

[0096] Considering the factor graph problem as a nonlinear least squares problem, assuming there are m sets of visual measurements, the visual residual after the first joint optimization of visual and inertial measurements is:

[0097]

[0098] Calculate the unit weighted mean square error of the visual reprojection residual separately:

[0099]

[0100] In the formula, P -1 The matrix is ​​a weight matrix, with unit weights; n d is the total visual reprojection factor dimension, with one observation point providing two constraint equations; t is the number of variables to be optimized, including m inverse depths and 6 degrees of freedom poses.

[0101] The unit weighted mean square error of visual reprojection reflects the overall visual quality. A larger unit weighted mean square error indicates a greater influence of environmental or vehicle motion on localization, resulting in poorer visual measurement quality and less reliable measurement results. Therefore, a covariance adjustment model function based on the unit weighted mean square error is proposed:

[0102]

[0103] In the formula, σ′ is the new covariance factor; σ is the unit weighted error calculated based on the a posteriori residuals; k is the confidence factor; lg(·) is a logarithmic function with base 10.

[0104] The purpose of adding the logarithmic function is to improve the sensitivity of the model function to changes. When k=2, the confidence probability is 95.44%, and the function graph is as follows. Figure 6 As shown, the curve with the added sensitivity function has a steeper slope than the original curve, making the re-optimization stage more responsive to the unit weight error of the visual measurement. Furthermore, the input to the covariance adjustment function is the unit weight error of the post-amplitude visual reprojection residual. This value can reflect the quality of visual measurement to a certain extent. Adjusting the visual covariance matrix with it can accurately reflect the weighting ratio between visual and inertial measurements, allowing for more flexible and effective utilization of sensor accuracy.

[0105] Of course, the new covariance factor can also be set to At this point, the sensitivity of the model function to changes is not as high as that of the model function when an additional logarithmic function is added, but it still has a preliminary adjustment effect. That is, the larger the unit weight error σ is, the larger the new covariance factor σ′ in the adjusted visual covariance is.

[0106] ② Re-optimize.

[0107] Substituting the unit weighted mean square error into the covariance adjustment model function yields a new visual covariance matrix, which reflects the noise level of visual measurements after the initial nonlinear optimization. In the re-optimization phase, the visual noise level is used to determine a new information matrix, which is then re-substituted into the factor graph model for a second nonlinear optimization. The new information matrix signifies the optimized weighting ratio between visual and inertial measurements, altering the system's dependence on these measurements.

[0108] During the re-optimization process, the weights of the marginalization factors were not actively changed, such as... Figure 7 As shown, the algorithm applies the new visual covariance matrix to the marginalization process. Therefore, the marginalization factor optimized by the current sliding window is itself passed from the marginalization process optimized in the previous frame. The covariance matrix of the reprojection residual block has been changed by the algorithm in the previous frame. Therefore, when the marginalization factor is passed to the nonlinear optimization process of this frame, it is not necessary to reweight the visual reprojection factor of the marginalization.

[0109] Step two involves performing loosely combined VIO / GNSS navigation and positioning. This step fuses the local positioning results obtained from the loosely compact combination of visual-inertial navigation (VII) based on the covariance adjustment model in Step one with the GNSS positioning results using a factor map. The framework is as follows: Figure 8 As shown, the VIO and GNSS positioning results are first aligned using ROS time. Then, the aligned positioning results are added to the loosely combined factor graph by constructing VIO and GNSS factors. The final integrated navigation and positioning result is obtained through nonlinear optimization. The following sections will introduce the three parts: construction of the loosely combined factor graph, VIO factors, and GNSS factors.

[0110] 1) Construction of the pine combination factor diagram.

[0111] The loose combination algorithm's factor graph consists of residual equations formed by both VIO and GNSS factors, which are then used for nonlinear optimization to solve for the final positioning result. Assuming there are n local VIO positioning data points (local VIO coordinate system) and m GNSS positioning data points at a certain time point, the variables to be optimized in the loose combination algorithm are n global positioning results (WGS-84 coordinate system):

[0112]

[0113] The system factor diagram is composed as follows Figure 9 As shown, there are a total of n-1 VIO factors and m GNSS factors.

[0114] Construct a nonlinear optimization overall objective cost function using a factor graph model:

[0115]

[0116] In the formula, VIO factor; GNSS factor; Represents the measured value; o represents the set of VIO positioning result sequences; g represents the set of GNSS positioning results; P o Represents the covariance of the VIO term; Let be the covariance expressed in terms of GNSS positioning error.

[0117] 2) VIO factor.

[0118] The VIO factor is the sum of two adjacent state variables. and The rotational and translational residuals formed between them can be expressed as:

[0119]

[0120] In the formula, the superscript G represents the global coordinate system (WGS-84 coordinate system), and the superscript w represents the local VIO coordinate system.

[0121] It is evident that the VIO factor is related to the relative rotation and translation between the two time points, but not to the absolute value.

[0122] 3) GNSS factor.

[0123] The GNSS factor can be directly defined as the difference between the variable to be optimized and the GNSS measurement value, and is defined as follows:

[0124]

[0125] After performing nonlinear optimization on the factor graph composed of all GNSS factors and VIO factors, the final positioning results for all times can be obtained, with the last one being the positioning result for the current time.

[0126] This concludes the introduction to the principle and implementation process of this method. The following section will demonstrate its application in a specific example, with the implementation process divided into the following steps:

[0127] Step 1: System Setup. The system connections and structure are as follows: Figure 10 As shown, the NVIDIA Jetson Xavier NX development board 3 is powered by a DUPLI battery 1. The monocular camera 2 and the Xsens Mti-7 inertial / GNSS module 4 are connected and communicate with the NVIDIA Jetson Xavier NX development board 3 via a USB interface. The Xsens Mti-7 inertial / GNSS module 4 requires a ceramic antenna 5 to receive GNSS signals. The physical system configuration is as follows. Figure 11 As shown. After the system is set up, power it on, and the camera, IMU, and GNSS receiver modules are in operation.

[0128] Step 2: System calibration. Use the camera checkerboard calibration board and the Aprilgrid calibration board to calibrate the camera intrinsic parameters, camera and IMU extrinsic parameters, and time delay parameters respectively. Input the calibration results as parameters into the program parameter file.

[0129] Step 3: Start the ROS driver for the camera and Xsens module to receive images, inertial navigation and GNSS data.

[0130] Step 4: Start the GNSS / INS / visual integrated navigation algorithm based on flexible adjustment. The algorithm will start working after a few seconds of initialization.

[0131] Step 5: Real-time output of global positioning results. The loose combination algorithm will fuse GNSS positioning results on the basis of visual / INS tight combination and output the global fused positioning results in real time.

[0132] In summary, this invention combines satellite navigation, inertial navigation, and visual navigation technologies to propose a GNSS / INS / visual integrated navigation method based on elastic adjustment. In particular, the visual-inertial linear compact combination method based on the covariance adjustment model achieves smooth and accurate global fusion positioning. Moreover, this method adopts a sensor data fusion method based on factor graphs, which can ensure that other sensors can still be combined normally even when one sensor fails and has no data, achieving plug-and-play sensor functionality.

[0133] System Implementation Example:

[0134] The present invention discloses a combined navigation system based on elastic adjustment, the structural diagram and physical diagram of which are shown below. Figure 10 and Figure 11 As shown, the system consists of four parts: an image sensor (monocular camera), an inertial navigation / GNSS receiver module (Xsens Mti-7), an algorithm processing core module (NVIDIA Jetson Xavier NX development board), and a power supply battery. The power supply battery powers the algorithm processing core module. The monocular camera is responsible for image acquisition. The Xsens Mti-7 module receives raw IMU data and GNSS positioning information. The NVIDIA Jetson Xavier NX development board handles data reception and core navigation algorithm processing. The operating system is Ubuntu 18.04, and the entire method runs in a Robot Operating System (ROS) environment. The implemented method is a flexible adjustment-based integrated navigation method described in this embodiment. Further details of this method will not be elaborated upon in this embodiment.

Claims

1. A combined navigation method based on elastic adjustment, characterized in that, Includes the following steps: 1) Using the factor graph optimization method, the state variables to be optimized in the visual inertial odometry are used as state nodes, and the visual projection error and IMU pre-integration error are used as factor nodes to construct a compact combination factor graph model. The error equation in the compact combination factor graph model is solved iteratively to achieve the first nonlinear optimization. 2) After the first nonlinear optimization, the unit weighted mean square error of the reprojection error is input into the covariance adjustment model to adjust the visual covariance. Then, the adjusted visual covariance is used to reconstruct the compact combination factor graph model, and the error equation in the new compact combination factor graph model is iteratively solved to achieve the second nonlinear optimization, thereby obtaining the navigation results of the visual inertial odometry. The larger the unit weighted mean square error, the larger the new covariance factor in the adjusted visual covariance. The covariance adjustment model is as follows: or In the formula, For the new covariance factor; This is the unit weighted mean square error calculated based on the post-hoc residuals; Confidence factor; It is a logarithmic function with base 10; For visual covariance; 3) Combine navigation based on the navigation results from visual inertial odometry and GNSS positioning results.

2. The integrated navigation method based on elastic adjustment according to claim 1, characterized in that, In step 1), when constructing the compact combination factor graph model, a sliding window based on keyframes is required, and the factor nodes also include marginalization factors. If the second newest frame in the sliding window is a keyframe, then the oldest frame in the sliding window is marginalized; otherwise, the second newest frame is marginalized and the marginalization factor is added to the next construction of the compact combination factor graph model.

3. The integrated navigation method based on elastic adjustment according to claim 1, characterized in that, In step 1), the Gauss-Newton method is used for iterative solution, and the incremental equation corresponding to the overall objective function is: In the formula, For the first The carrier state quantity and the first The Jacobian matrix of pre-integral factors formed by the state variables of each carrier. For the reprojection factor about the landmark point The observed Jacobian matrix; For marginalization constant terms; It is the increment of the quantity to be optimized; For marginalized prior information; For the first The carrier state quantity and the first The covariance of the pre-integrated noise term formed by the state variables of each carrier; and These are the IMU pre-integration factor and the visual reprojection factor, respectively.

4. The integrated navigation method based on elastic adjustment according to claim 1, characterized in that, In step 3), the method for combining navigation results from visual inertial odometry and GNSS positioning results is as follows: VIO factors and GNSS factors are established using the factor graph optimization method, thereby constructing a loosely combined factor graph model. The error equations in the loosely combined factor graph model are iteratively solved to obtain the combined navigation results of visual inertial odometry and GNSS.

5. The integrated navigation method based on elastic adjustment according to claim 1, characterized in that, Before step 1), the visual inertial odometry needs to be initialized. The initialization process includes: first, using visual measurements to recover the initial motion structure, and using the PnP method to solve the pose of the visual sensor in the visual inertial odometry; then, combining IMU pre-integration measurements to perform visual-inertial coarse alignment in order to estimate the initial state of the visual inertial odometry.

6. The integrated navigation method based on elastic adjustment according to claim 2, characterized in that, The state variables to be optimized within the sliding window are: In the formula, This represents the state variable to be optimized; and These represent the number of state vectors to be optimized within the sliding window and the total number of observed feature points, respectively. and These represent the translation vector and rotation quaternion from the camera coordinate system to the carrier coordinate system, respectively; Represents the inverse depth of the feature points. ; and They represent the first The translation vector and rotation quaternion from the carrier coordinate system to the world coordinate system corresponding to each state vector; Indicates the first The velocity of the carrier in the world coordinate system corresponding to each state vector; and These represent the zero bias of the IMU accelerometer and gyroscope, respectively.

7. The integrated navigation method based on elastic adjustment according to claim 4, characterized in that, The VIO factor is the rotation and translation residual between two adjacent states.

8. The integrated navigation method based on elastic adjustment according to claim 4, characterized in that, The GNSS factor is the difference between the variable to be optimized and the GNSS measurement value.

9. A combined navigation system based on elastic adjustment, characterized in that, The system includes a visual sensor, an inertial navigation / GNSS receiver module, an algorithm processing core module, and a power supply module. The inertial navigation / GNSS receiver module is used to receive GNSS signals and IMU data. The visual sensor is used to acquire images and send them to the algorithm processing core module. The power supply module is used to supply power to the algorithm processing core module. The algorithm processing core module is used to execute program instructions to implement the flexible adjustment-based integrated navigation method as described in any one of claims 1 to 8.

Images