Joint calibration optimization method applied to underground space

Abstract

A joint calibration optimization method applied to underground space mainly comprises the following steps: establishing an IMU pre-integration model, and deducing continuous and discrete forms of the IMU pre-integration model for IMU measurement preprocessing; an IMU error parameter residual error is added in bundle adjustment based on the mahalanobis distance to serve as an optimization item, and a Jacobian matrix of the optimization item is deduced; in order to solve the problem that the covariance matrix is irreversible due to the increase of IMU error parameters, a method for regularizing the covariance matrix is adopted, so that the excessive singularity of the matrix can be prevented, and the reversibility of the matrix is improved; visual point feature reprojection errors and visual line feature reprojection errors serve as error factors to be introduced into an optimization function, more constraints and information can be provided to help optimization of a sensor, and through utilization of gray information, a calibration system can have higher robustness in an underground space region with poor texture. By constructing the joint optimization function, joint optimization of a plurality of error factors can be realized, and the calibration result is optimized.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor joint calibration optimization, and specifically relates to a joint calibration optimization method applied to underground space. Background Art

[0002] With the advancement of the urbanization process of human society and the growth of urban population, the available space on the surface area is increasingly reduced. Therefore, the collaborative development and utilization of urban underground space will become the future trend of urban development. The underground space is also considered as a new field for human life and combat after land, sea, air, and space. The underground space environment is a special environment with characteristics such as satellite denial, poor lighting conditions, and weak texture. Studying the autonomous navigation and positioning technology of unmanned aerial vehicles (UAVs) in underground space has important practical significance to complete tasks such as detection, search and rescue, and combat.

[0003] The prerequisite for a UAV to achieve automated operation is to accurately estimate its own position and attitude and perceive the surrounding environment. After accurately perceiving its own pose and environment, the UAV can make autonomous decisions, plans, and controls to complete automated operations. Various sensors can provide global or local measurements for positioning and mapping.

[0004] The inertial navigation system can provide high-frequency measurements, and its measurements are not affected by external environments such as light, texture, and weather. However, its measurements have time-varying biases and require sensors that perceive the external environment to provide effective constraints to reduce measurement errors; cameras can perceive rich texture information in the environment, but visual SLAM (Simultaneous Localization and Mapping) and inertial / visual SLAM technologies cannot successfully track visual features in weak texture areas or under poor lighting conditions, resulting in performance degradation or even failure; lidar is good at measuring distances to capture structural information in the environment, and its measurement values are hardly affected by light. However, the point cloud measurement of lidar is not as dense as visual images and has lower discrimination. Therefore, in a structurally degraded scene, the positioning and mapping method based on lidar degrades in performance due to the inability to obtain sufficient constraints. To make up for the deficiencies of single sensors in perception ability and robustness, multiple sensors can be fused to improve the accuracy and reliability of UAV positioning and mapping in underground space.

[0005] Calibration is the prerequisite for accurate multi-sensor measurement. The internal and external parameters obtained by offline calibration will deviate from the actual system parameters due to mechanical deformation and environmental changes during use. Therefore, parameter optimization is required to reduce the impact of bad external parameters. Summary of the Invention

[0006] Since the internal and external parameters obtained by offline calibration will deviate from the actual system parameters due to mechanical deformation and environmental changes during use, the present invention proposes a joint calibration optimization method for underground spaces to optimize the calibration results.

[0007] To achieve the above object, the technical solution adopted by the present invention is:

[0008] A joint calibration optimization method for underground spaces, comprising the following steps:

[0009] Step 1: Establish an IMU pre-integration model and derive its continuous and discrete forms for IMU measurement preprocessing;

[0010] Step 2: Add the IMU error parameter residual as an optimization term in the bundle adjustment based on Mahalanobis distance and derive its Jacobian matrix;

[0011] Step 3: To solve the problem of the non-invertibility of the covariance matrix caused by adding IMU error parameters, a method of regularizing the covariance matrix is adopted;

[0012] Step 4: Introduce the visual point feature reprojection error and the visual line feature reprojection error as error factors into the optimization function to achieve the joint optimization of multiple error factors and obtain an improved joint optimization function.

[0013] As a further improvement of the present invention, the establishment of the pre-integration model in Step 1 specifically includes: establishing an IMU pre-integration model and deriving its continuous and discrete forms for IMU measurement preprocessing;

[0014] The IMU measurement pre-integration between the k-th frame and the (k + 1)-th frame is:

[0015]

[0016] The IMU predicted item between two adjacent frames is

[0017]

[0018] After discretization, the integration from the i-th IMU moment to the (i + 1)-th IMU moment is:

[0019]

[0020] The relationship between the i-th IMU measurement and the (i + 1)-th IMU measurement is:

[0021]

[0022] As a further improvement of the present invention, the addition of the IMU error parameter residual as an optimization term in Step 2 and the derivation of its Jacobian matrix are specifically as follows:

[0023] In two adjacent frames b of the sliding window k , b k+1 , the IMU measurement residual is

[0024]

[0025] The problem of calibrating the IMU error parameters can be transformed into the problem of minimizing the objective function in the above formula. The variables to be optimized are the positions, velocities, attitudes, and IMU error parameters of the k-th frame and the (k + 1)-th frame, that is:

[0026]

[0027] The variables to be optimized constitute the IMU constraint between two adjacent frames. While realizing pose estimation, the IMU error parameters are also estimated. The solution method can adopt various iterative optimization algorithms;

[0028] To solve the optimization problem using an iterative algorithm, it is necessary to derive the Jacobian matrix that determines the iterative direction. Since there are many parameters to be optimized and the dimension of the Jacobian matrix is large, the derivation process adopts block processing and is divided into 4 vector blocks;

[0029]

[0030] Then, the Jacobian matrices of the four parameter vectors to be optimized are derived respectively.

[0031] As a further improvement of the present invention, in step 3, to solve the problem that the covariance matrix is irreversible due to the increase of IMU error parameters, a method of regularizing the covariance matrix is adopted. The specific steps are as follows:

[0032] The Mahalanobis distance of the IMU measurement residual is:

[0033]

[0034] Where P imu is the covariance of the IMU measurement residual. Due to the increase of the IMU scale factor and the installation error estimation parameters, the noise driving matrix V is a row rank-deficient matrix, making the covariance matrix P imu of the IMU measurement residual irreversible, so the Mahalanobis distance cannot be solved;

[0035] Regularize the covariance matrix, introduce a regularization term when calculating the covariance matrix, and the corrected Mahalanobis distance formed by this method is:

[0036]

[0037] Beneficial effects:

[0038] A joint calibration optimization method applied to underground spaces mainly includes: establishing an IMU pre-integration model and deriving its continuous and discrete forms for IMU measurement preprocessing; adding the IMU error parameter residual as an optimization term in the bundle adjustment based on Mahalanobis distance and deriving its Jacobian matrix; to solve the problem of the non-invertibility of the covariance matrix caused by adding IMU error parameters, a method of regularizing the covariance matrix is used, which can prevent the matrix from being overly singular and improve the invertibility of the matrix; taking the reprojection error of visual point features and the reprojection error of visual line features as error factors and introducing them into the optimization function can provide more constraints and information to help optimize the sensor. By utilizing the gray information, the calibration system can be more robust in areas with poor texture in underground spaces. The present invention can realize the joint optimization of multiple error factors and optimize the calibration results by constructing a joint optimization function. Description of the Drawings

[0039] Figure 1 is the overall flowchart of the method of the present invention. Detailed Embodiments

[0040] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0041] Illustrate by way of example the content included in the claims

[0042] Next, the embodiments of the present invention will be described in detail in conjunction with the drawings;

[0043] As Figure 1 shown, the present invention provides a joint optimization method applied to underground spaces, and the method includes the following steps for implementation:

[0044] Step 1: Establish an IMU pre-integration model and derive its continuous and discrete forms for IMU measurement preprocessing;

[0045] 1.1 Add the measurement of the lidar to the VINS based on sliding window optimization to form an inertial / visual / LiDAR measurement system. The state variables to be optimized and estimated in the sliding window are:

[0046]

[0047] where x k is the IMU state variable obtained from the k-th frame of the image, which includes the position, velocity, attitude, accelerometer, and gyro zero bias b a of the IMU in the world coordinate system ω, b g , scale factor, and installation error t a , t g . is the transformation matrix between the camera and the IMU, is the transformation matrix between the lidar and the IMU, and n + 1 is the total number of state variables within the window.

[0048] 1.2 Derive the continuous pre-integration model

[0049] According to the IMU pre-integration algorithm, the pre-integration result of the IMU measurements between the k-th frame and the (k + 1)-th frame can be obtained as:

[0050]

[0051] where Δt k is the time difference of the integration interval [t k , t k+1 . It can be seen that the pre-integration result of the (k + 1)-th frame depends on the state variables of the k-th frame. During the sliding window optimization process, the state variables of the k-th frame are continuously iteratively updated, resulting in the need to re-integrate after each iteration to obtain the state variables of the (k + 1)-th frame. To avoid the problem of recalculating the integral, multiply both sides by or to convert the pre-integration in the world coordinate system to the pre-integration between two adjacent frame coordinate systems, and we can get:

[0052]

[0053] In the above formula

[0054]

[0055] and are the IMU pre-integration terms between two adjacent frames, representing the velocity, position, and attitude increments that only depend on the IMU measurements and the IMU parameters, and are independent of the state variables to be optimized in the k-th frame. Since the constraints between two adjacent frames are determined, after the state variables are optimized and changed, there is no need to re-integrate the pre-integration terms, which can effectively reduce the computational amount.

[0056] 1.3 Derive the discrete pre-integration model

[0057] Discretize the continuous IMU pre-integration model. First, use the median method to achieve discretization. The integration from the i-th IMU time to the (i + 1)-th IMU time is:

[0058]

[0059] Then, discretize the IMU pre-integration terms between two adjacent frames using the median method. Specifically, for the relationship between the i-th IMU measurement and the (i + 1)-th IMU measurement, we can get:

[0060]

[0061] where the average acceleration of the IMU pre-integration term and the average angular velocity are:

[0062]

[0063] Step 2: Add the IMU error parameter residual as an optimization term in the bundle adjustment based on Mahalanobis distance, and derive its Jacobian matrix;

[0064] 2.1 In two adjacent frames b k , b k+1 of the sliding window, the IMU measurement residual is

[0065]

[0066] where [·] xyz is the vector part of the quaternion. The IMU bias, scale factor, and installation error are added to the residual term as optimization terms. Since the time between two adjacent frames is short, theoretically, the IMU error parameters will not change during this period. Therefore, the difference in their changes between the two frames can be used to form a cost function.

[0067] The problem of calibrating the IMU error parameters can be transformed into the problem of minimizing the objective function in the above formula. The variables to be optimized are the position, velocity, attitude, and IMU error parameters of the k-th frame and the (k + 1)-th frame, that is:

[0068]

[0069] The variables to be optimized form the IMU constraint between two adjacent frames, and while realizing pose estimation, the IMU error parameters are also estimated online. The solution method can adopt various iterative optimization algorithms.

[0070] 2.2 To solve the optimization problem using an iterative algorithm, it is necessary to derive the Jacobian matrix that determines the iterative direction. Since there are many parameters to be optimized and the dimension of the Jacobian matrix is large, the derivation process uses block processing and is divided into 4 vector blocks.

[0071]

[0072] The Jacobian matrices of the IMU measurement residual with respect to the four vector blocks of the parameters to be optimized are respectively:

[0073]

[0074] Step 3: To solve the problem that the covariance matrix becomes non-invertible due to the addition of IMU error parameters, a method of regularizing the covariance matrix is adopted, which can prevent the matrix from being overly singular and improve the invertibility of the matrix:

[0075] The Mahalanobis distance of the IMU measurement residual is as follows:

[0076]

[0077] Where P imu is the covariance of the IMU measurement residual. Due to the increase of the IMU scale factor and the installation error estimation parameters, the noise driving matrix V is a row-deficient rank matrix, making the covariance matrix P of the IMU measurement residual imu non-invertible, and thus the Mahalanobis distance cannot be solved.

[0078] When solving the Mahalanobis distance, it is necessary to ensure that the covariance is invertible so as to describe the variance and covariance relationship between data, thereby providing an accurate measure when calculating the Mahalanobis distance. A reversible covariance matrix is a prerequisite for solving the Mahalanobis distance. However, due to the existence of the IMU scale factor and installation error in the quantity to be optimized, the covariance matrix of the IMU measurement residual is non-invertible, and the Mahalanobis distance cannot be solved.

[0079] To solve the problem of the non-invertible covariance matrix, the proposed solution is to regularize the covariance matrix. By introducing a regularization term when calculating the covariance matrix, it is possible to prevent the matrix from being overly singular and improve the invertibility of the matrix. The corrected Mahalanobis distance formed by this method is as follows:

[0080]

[0081] Where the covariance matrix P imu The proportionality factor λ of the added identity matrix I is determined according to the minimum eigenvalue λ of the covariance matrix min as follows:

[0082]

[0083] Step 4: Introduce the visual point feature reprojection error and the visual line feature reprojection error as error factors into the optimization function to achieve the joint optimization of multiple error factors and obtain the final joint optimization function.

[0084] The visual line feature reprojection error based on the Mahalanobis distance is as follows:

[0085] Where

[0086] The finally constructed inertial / visual / LiDAR joint optimization function is as follows:

[0087]

[0088] Where Denote the Mahalanobis distance of the vector. The five residual terms are the marginalized prior information, the IMU measurement residual, the visual point feature reprojection residual, the visual line feature reprojection error, and the lidar point cloud measurement residual, is the covariance matrix of the IMU pre-integration for the k-th and (k + 1)-th frames, P C is the covariance matrix of the visual reprojection, P L is the covariance matrix of the lidar residual term.

[0089] As described above, it is only the preferred embodiment of the present invention, and it is not a limitation of the present invention in any other form. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. A joint calibration optimization method applied to underground space, characterized by: The following steps are involved: Step 1: Establish an IMU pre-integration model and derive its continuous and discrete forms for IMU measurement preprocessing; Step 2: Add the IMU error parameter residual as an optimization item in the bundle adjustment based on Mahalanobis distance, and derive its Jacobian matrix; Step 3: In order to solve the irreversible problem of the covariance matrix caused by adding the IMU error parameters, a method of regularizing the covariance matrix is ​​adopted; Step 4: Introduce the visual point feature reprojection error and the visual line feature reprojection error into the optimization function as error factors to achieve joint optimization of multiple error factors and obtain an improved joint optimization function.

2. The joint calibration optimization method for underground space according to claim 1, characterized in that: The step 1 establishes a pre-integration model, specifically including: establishing an IMU pre-integration model, deriving its continuous and discrete forms for IMU measurement preprocessing; The IMU measurement pre-integration between the kth frame and the (k+1th frame is: The estimated IMU items between two adjacent frames are After discretization, the integral from the i-th IMU moment to the (i+1)-th IMU moment is: The relationship between the i-th IMU measurement and the (i+1)-th IMU measurement is:

3. The joint calibration optimization method for underground space according to claim 1, characterized in that: In step 2, the IMU error parameter residual is added as an optimization item, and its Jacobian matrix is ​​derived. The specific method is as follows: In the two adjacent frames b of the sliding window k , b k+1 In the above example, the IMU measurement residual is The problem of calibrating the IMU error parameters can be transformed into the problem of minimizing the objective function in the above formula. The variables to be optimized are the position, velocity, attitude and IMU error parameters of the kth frame and the (k+1)th frame, that is: The variables to be optimized constitute the IMU constraints between two adjacent frames. While realizing the pose estimation, the IMU error parameters are also estimated. The solution method can adopt various iterative optimization algorithms. In order to solve the optimization problem using iterative algorithms, it is necessary to derive the Jacobian matrix that determines the iteration direction. Since there are many parameters to be optimized and the Jacobian matrix has a large dimension, the derivation process is divided into four vector blocks using block processing; Then the Jacobian matrices of the four parameter vector blocks to be optimized are derived respectively.

4. The joint calibration optimization method for underground space according to claim 1, characterized in that: In step 3, in order to solve the irreversible problem of the covariance matrix caused by adding the IMU error parameter, a method of regularizing the covariance matrix is ​​adopted. The specific steps are as follows: The Mahalanobis distance of the IMU measurement residual is: Where P imu is the covariance of the IMU measurement residual. Due to the increase of the IMU scale factor and the installation error estimation parameters, the noise driving matrix V is a row-saturated rank matrix, making the covariance matrix P of the IMU measurement residual imu If it is not reversible, the Mahalanobis distance cannot be solved; The covariance matrix is ​​regularized, and the regularization term is introduced when calculating the covariance matrix. The corrected Mahalanobis distance constructed by this method is:

Images