An improved VINS method for complex lighting environments

Abstract

The present invention discloses an improved VINS method for complex lighting environments. First, the camera response function is estimated through an initialization process. This process can model and calibrate the camera imaging process. Then, the function is used to decompose the optical flow process from the relationship between posture and grayscale to the relationship between posture and exposure parameters and irradiance, so that the influence of exposure parameters on posture estimation can be estimated in this process. Through this method, the changes in camera parameters caused by complex lighting can be integrated into the overall VINS system posture estimation, so that the system can adapt to the environment. Finally, the algorithm of the present invention is compared with the traditional VIINS algorithm through real experiments, which proves the feasibility and effectiveness of the algorithm, thereby completing the implementation of the improved SLAM system in complex lighting environments based on the traditional VINS algorithm.

Description

Technical Field

[0001] In view of the problem of inaccurate pose estimation of the traditional VINS algorithm in complex lighting environments, the present invention designs an improved VINS system based on the traditional VINS system that is more robust to complex lighting environments. This method further decomposes the grayscale error in the traditional optical flow into irradiance error by estimating the camera imaging response function, and at the same time adds the exposure parameters in the process of the camera collecting real scenes in reality into the visual data association process, performs data association and estimates exposure information at the same time, improves the quality of visual signals in complex lighting environments, and thus makes the overall improved VINS system more robust in complex lighting environments. Background Art

[0002] Navigation is the process of correctly guiding the carrier along the predetermined route, with the required accuracy, and to the destination within the specified time. Over the years, inertial navigation units have been very mature and successfully applied to many applications such as missile ships, aerospace, etc., especially when the GPS signal is greatly affected. It is particularly important. The inertial navigation system is an autonomous navigation system that does not rely on external information when working, nor does it radiate any energy to the outside world. It has good concealment and strong anti-interference ability, and can provide complete motion information for the carrier all day and all weather. However, most high-precision inertial navigation systems have high accuracy and occupy a large area and are costly.

[0003] In recent years, with the development of microelectronics technology, low-cost gyroscopes and accelerometer units realized by microelectronics technology have made inertial navigation units more widely used in various fields. However, low-cost inertial units are more susceptible to noise interference during measurement. In order to make up for the loss of accuracy caused by the low cost of inertial navigation units, many solutions choose to use visual measurement modules to compensate and correct the inertial units. The visual measurement module is low-cost and small in size, and can be well integrated with the inertial module. The fusion of monocular vision slam and low-cost inertial slam can well make up for each other's shortcomings.

[0004] The implementation methods of obtaining the posture information of the body through visual sensors are mainly divided into two categories: direct method and feature point method. The direct method assumes that the grayscale of the corresponding points in the image stream obtained by the visual sensor remains unchanged, thereby optimizing the photometric error of the corresponding points to obtain the posture change of the body. This method has the possibility of restoring dense maps, but it usually requires a large amount of calculation and takes a long time. The feature point method selects key points by calculating image gradients and other methods, and obtains the posture change of the body by matching the corresponding key points. However, key points alone are often unable to describe complex image information. For this reason, descriptors are introduced to describe the image features around feature points, such as the famous SIFT, SURF, ORB, and FAST. However, the matching calculation process of the descriptor cannot achieve both accuracy and speed.

[0005] In the current mainstream visual inertial SLAM methods, in order to fully utilize the advantages of the high frame rate of IMU information and to meet the real-time frame rate of the existing image stream, semi-direct methods are often used to deal with image data association problems. This method only performs corner detection and calculates key points without calculating descriptors, and uses optical flow to track the movement of key points. The advantage of this method is that optical flow calculation usually takes less time than descriptor calculation, making the overall system more real-time and being able to put more computing resources on the overall optimization level to improve the overall system pose estimation accuracy. However, the optical flow method has poor robustness to image grayscale changes, and factors such as lighting and camera exposure that affect image grayscale greatly affect the accuracy of visual information. Since the traditional VINS system uses this method for front-end data association, the accuracy and stability of the system will be greatly affected in complex lighting environments. Summary of the invention

[0006] In order to solve the problem that the existing system is not adapted to complex lighting environments, the present invention estimates and compensates for grayscale changes, thereby improving the robustness of the system mounted on the body in the environment. Many researchers have proposed the use of photometric calibration algorithms to solve this problem. However, the current photometric calibration algorithms are mainly designed for direct methods. Since the exposure time and the irradiance of each pixel must be calculated at the same time, the optimization vector is very large, which results in a slightly slower exposure time calculation speed. For a camera with a frame rate of 30H z The VINS algorithm with a length of about 1000 pixels is difficult to meet the requirements of online operation. The compensation scheme proposed in this paper for abnormal grayscale changes in the image during the movement of the body has greatly improved the stability of the body in the environment with changing lighting, and the running speed can meet the camera frame rate requirements.

[0007] 1. Calibrate the camera response function model during system initialization

[0008] The imaging process of a camera is generally divided into the image acquisition part and the image post-processing part. Scene objects absorb light energy and reflect it into the lens according to optical principles, and are converted into a set of image sequences through various sensors and optical devices. The second part, image post-processing, is the main part that will have an adverse effect on pose estimation. The performance and settings of the camera sensor or optical elements in this part will directly affect the image brightness, which conflicts with the grayscale invariance assumption in the direct matching process, greatly affecting the quality of pose estimation.

[0009] In order to correct and estimate the camera imaging process, the algorithm proposed in the present invention needs to calibrate the camera imaging model during the initialization process to improve the calculation speed of online optical compensation during system operation.

[0010] The camera imaging process can be abstracted as follows: (1) Objects in the scene absorb light energy and reflect a portion of it, which is called irradiance B(x) (2) The scene irradiance B(x) is absorbed by the imaging array in the camera through the lens (the photosensitive element of a traditional camera is film, and the photosensitive element of a digital camera is generally a CCD or CMOS sensor) (3) The total amount of energy absorbed by the imaging array is controlled by parameters such as exposure time. Therefore, in order to estimate the impact of the camera on the image, the camera imaging model is introduced:

[0011] I(x)=f(tV(x)B(x)) (1)

[0012] Among them, t is the camera exposure time, and the entire image has the same exposure time. y(x) represents lens distortion, which is related to the lens shape and refractive index on the lens. B(x) represents the irradiance in the scene corresponding to the imaging plane, which is only related to the properties of the specific material in the scene. The irradiance B(x) will not change when the light source remains unchanged. f() represents the mapping process, which is a nonlinear process that includes the differences between different camera individuals or the impact of the camera manufacturing process on the exposure process. I(x) is the output image, the pixel grayscale value of point x.

[0013] The process of calibrating the camera response function model is to estimate f() (or f -1 ()). In the camera model, V(x) is an inherent property of the lens, which is only related to the physical properties of the lens and will not change with the camera exposure parameters. Therefore, in the model design, B(x) is allowed to absorb V(x) to obtain a loose irradiance B′(x). Assuming B′(x)=V(x)B(x), the camera model becomes:

[0014] I(x)=f(tB′(x)) (2)

[0015] For the i-th image, there is a hypothetical f -1 (), we can get the following equation:

[0016]

[0017] That is, through the function f -1 () The grayscale value of the image at position x can be transformed into the product of the exposure time parameter and the loose irradiance. At the same time, due to the error caused by the transformation, it is assumed that there is an error w between the transformed value and the true value i , the error is a noise that follows a Gaussian distribution

[0018] Therefore, a least squares problem can be constructed to estimate the function f -1 ()as follows:

[0019]

[0020] Since the function value range of I(x) is a discrete integer value from 0 to 255, f -1 (I i (x)) corresponds to a finite number of mapping relationships (256 in total), and the unknown variables to be solved are the irradiance of each scene point and the 256 mapping relationships f -1 (). During the calibration of the response function, the camera should be kept still, and the optimal solution can be obtained by taking a series of images with different exposure times. The image stream output in this process has a known exposure time t i , and the camera remains stationary, the scene point corresponding to each pixel x in the image remains unchanged, and the irradiance B′(x) of each scene point in each equation remains unchanged, so the only unknown quantity is f -1 () and the irradiance of the scene point. For actual images, the number of unknown quantities is equal to the number of pixels plus 256. Collecting enough images with different exposure times can construct an overdetermined equation to solve the camera response function f that is closest to the true value. -1 ().

[0021] 2. Estimated exposure time during operation

[0022] The present invention provides an algorithm for synchronously estimating camera exposure time during system operation, and the algorithm is specifically improved based on LK optical flow.

[0023] Optical flow is a method that describes the movement of pixels between images over time. Among them, sparse optical flow, represented by LK optical flow, which only tracks the movement of some pixels, can be well used for feature point tracking. In order for its algorithm to work, LK optical flow needs to meet three assumptions:

[0024] 1 Grayscale invariance assumption, that is, the grayscale value of the pixel used in the optical flow solution process does not change over time.

[0025] 2 Micro-motion assumption, that is, the tracked pixels will not experience drastic position changes between two adjacent frames.

[0026] 3. Spatial consistency assumption, that is, the neighboring pixels of the tracked pixel are still its neighboring pixels in the next frame.

[0027] Under the condition that these three assumptions are met, there are two frames of continuous one-dimensional images:

[0028]

[0029] Among them, F(x) represents the image point x in the previous frame, F(x+h) represents the grayscale of the point x in the next frame, h represents the displacement, x and h represent the position information in the image, and F′(x) represents the image gradient. When there is no drastic change between two adjacent frames, that is, the micro-motion assumption is met, and h is small enough, the above formula holds, allowing the system to establish a connection between the image gradient and the camera body motion.

[0030] When the gray value of the tracked image point x remains unchanged during the tracking process, that is, when the gray value remains unchanged assumption is satisfied, F(x+h)=G(x), and the above formula becomes:

[0031]

[0032] Transforming the equation to highlight the unknown h, we have:

[0033]

[0034] The above method can be used to solve the displacement of corresponding points in two images.

[0035] When the image points around the tracked image point x move consistently, that is, when the spatial consistency assumption is satisfied, we have:

[0036]

[0037]

[0038]

[0039] The most practical h can be solved by overdetermined equations.

[0040] This principle is extended to continuous two-dimensional image sequences. Because the grayscale invariance hypothesis shows that when the scene point corresponding to the point (x, y) on the tth frame of the image sequence has passed through time After the image sequence frame The grayscale values ​​of the two points are still equal. The equation is established as follows:

[0041]

[0042] Because of the nudging assumption, the right side of the above equation can be Taylor expanded:

[0043]

[0044] When the motion is small, higher-order infinitesimals can be ignored, and equation 4 is substituted into equation 3 to establish the following equation:

[0045]

[0046] Divide both sides of the equation by δt, and let have:

[0047] I x u+I y v=-I t (14)

[0048] Where u and v are continuous optical flows. x and I y is the image gradient along the x and y directions at the point. According to the spatial consistency assumption, the optical flow value between the regions around the corresponding pixels on the image is a constant. Assuming that the size of the region is m×m and the number of pixels it contains is n=m×m, the equation is established as follows:

[0049]

[0050] The overdetermined equations are:

[0051]

[0052] The optimal solution can be solved by the above equation, which is the most likely optical flow value in the neighborhood.

[0053] During the operation of the traditional VINS system, since its image sequence comes from the movement of the body, the latter two assumptions are generally easier to meet. However, due to the movement of the body during the operation of the traditional VINS system, the lighting environment often changes during the movement of the body. Therefore, the grayscale invariance assumption is often affected by various factors and cannot be met.

[0054] From 11 formula

[0055]

[0056] The body motion is linked to the image grayscale. At the same time, a satisfying f can be obtained from equations 3 and 4. -1 () makes f -1 (I i (x))≈t i B′(x) (17)

[0057] Using Equation 17 to expand the image flow I(x, y, t), we can get

[0058] f -1 (I(x,y,t))≈eB′(x,y,t) (18)

[0059] In the formula, in order to distinguish the image stream timestamp t, e is used to represent the exposure time. Using formula 18 to expand formula 11

[0060] eB′(x, y, t)=(eΔexp)B′(x+Δx, y+Δy, t+Δt) (19)

[0061] Where Δexp is the change in exposure time between two adjacent frames. e =eB′, Taylor expands the right side of Formula 19 to obtain the following equation:

[0062]

[0063] Substituting form 20 into form 19, we get form 21 which is similar to form 13:

[0064]

[0065] make Simplifying equation 21, we can get the overdetermined equation:

[0066]

[0067] in u and v It's optical flow. τ is directly related to the exposure time. Through the above formula, the change in exposure time and the change in pose under this change can be estimated at the same time. Considering the speed of pose estimation, it can also provide higher quality images for backend optimization to solve problems that may arise when building maps. Among them, due to the state τ It is only related to the change of exposure time. The exposure information during the operation of the camera can be synchronously solved through formula 22. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 : Camera imaging process flow chart;

[0069] Figure 2 : System flow chart;

[0070] Figure 3 : Exposure compensation image stream comparison chart;

[0071] Figure 4 : Comparison chart of experimental results; DETAILED DESCRIPTION

[0072] Step 1: Build an information data collection system

[0073] Based on the information collection and transmission methods between each sensor, a multi-sensor information data collection system equipped with GPS, camera, IMU and other sensors is constructed.

[0074] Step 2: System Initialization

[0075] Start and initialize the system, calibrate basic information such as the carrier camera intrinsic parameters, IMU bias, and the transformation matrix between the IMU coordinate system (body coordinate system) and the camera coordinate system.

[0076] Step 3: Camera Initialization

[0077] Multiple groups of images with different exposure times are continuously collected under the same posture state of the body, and the collected images are processed to establish the cost function of the camera response function and the nonlinear optimization process.

[0078]

[0079] Solve the above equation to find f -1 () and B′(x), respectively solve the partial derivatives of the cost function for the two sets of unknown quantities and set the partial derivatives equal to zero to obtain the local minimum, and we have:

[0080]

[0081]

[0082] Solve for the optimal f -1 () and B′(x)

[0083] Step 4: Exposure parameter estimation during body motion

[0084] Read two adjacent frames of images and import the data into Formula 22

[0085]

[0086] The exposure parameter τ and the compensated optical flow estimate u, u are solved simultaneously.

[0087] Gain Verification

[0088] In order to evaluate the performance of the improved VINS algorithm proposed in this paper, a device was designed to collect actual data near the campus teaching building. The data were processed using the improved VINS algorithm and the traditional VINS algorithm, and then the two were compared to illustrate the advantages and disadvantages of the algorithm proposed in this paper.

[0089] In order to verify the improvement of the algorithm on system accuracy under complex lighting conditions, data was collected around the tallest building of the school in the afternoon. The main buildings around the data set basically correspond to several different light intensities, forming a relatively complex lighting environment. In this environment, the machine ran a total of two laps as experimental data.

[0090] In order to verify the compensation effect of the present invention on the image, this paper selects adjacent images with obvious problems in the image stream for grayscale correction. It can be seen from the visual data of the experiment that the parameter settings of the camera itself do have a great influence on the acquisition of the image stream. There is a significant difference in the grayscale value of two adjacent frames in the image stream, which cannot be estimated by the VINS algorithm. This difference leads to a significant reduction in the credibility of the visual information. After the compensation method proposed in this paper, this difference in the image is significantly eliminated.

[0091] In order to verify the improvement of the pose estimation accuracy of the entire visual-inertial SLAM system by the present invention, the experiment was designed to use both the traditional VINS system and the improved VINS system to process the data set collected in the above method. Since the traditional VINS does not consider the image interference caused by other problems, the visual processing method based on the traditional LK optical flow cannot solve some problems when the light changes sharply. Compared with the traditional method, the algorithm proposed in this paper reduces the drift of pose estimation and improves the accuracy of pose estimation. Compared with the traditional algorithm, the overall pose estimation of the improved algorithm is closer to the actual GPS value. Compared with the traditional algorithm, the average positioning error of the three axes is reduced by 9.3%, 11.9% and 1.6% respectively. The three-axis stability is improved by 10.3%, 9.5% and 2.7% respectively.

Claims

1. An improved VINS method for complex lighting environments, characterized by: In order to estimate the impact of the camera on the image, the camera imaging model is introduced: I(x)=f(tV(x)B(x)) (1) Where t is the camera exposure time, and the entire image has the same exposure time. V(x) represents lens distortion, which is related to the lens shape and refractive index on the lens. B(x) represents the irradiance in the scene corresponding to the imaging plane, which is only related to the properties of the specific material in the scene. The irradiance B(x) will not change when the light source remains unchanged. f() represents the mapping process, and I(x) is the output image, the pixel grayscale value at point x. The process of calibrating the camera response function model is to estimate f() or f -1 () process; in the camera model, V(x) is the inherent property of the lens. In the model design, B(x) is allowed to absorb V(x) to obtain a loose irradiance B'(x). Assuming B'(x) = V(x)B(x), the camera model becomes: I(x)=f(tB(x)) (2) For the i-th image, there is a hypothetical f -1 (), the following equation is obtained: That is, through the function f -1 () The gray value of the image at position x is transformed into the product of the exposure time parameter and the loose irradiance. At the same time, due to the error caused by the transformation, it is assumed that there is an error w between the transformed value and the true value i , the error is a noise that follows a Gaussian distribution Therefore, a least squares problem is constructed to estimate the function f -1 ()as follows: Since the function value range of I(x) is a discrete integer value from 0 to 255, f -1 (I i (x)) corresponds to a finite number of mapping relationships, and the unknown variables to be solved are the irradiance of each scene point and the 256 mapping relationships f -1 (); During the calibration of the response function, the camera should be kept still, and the optimal solution is obtained by taking a series of images with different exposure times; the image stream output during the process has a known exposure time t i , and the camera remains stationary, the scene point corresponding to each pixel x in the image remains unchanged, and the irradiance B'(x) of each scene point in each equation remains unchanged, so the only unknown quantity is f -1 () and the irradiance of the scene point; for the actual image, the number of unknown quantities is equal to the number of pixels plus 256. Collect multiple images with different exposure times to construct an overdetermined equation to solve the camera response function f that is closest to the true value -1 (); Apply the camera response function f estimated during initialization -1 (), an algorithm for synchronously estimating the camera exposure time during system operation is obtained, which is a targeted improvement based on LK optical flow; The optical flow method establishes the equation as follows: Because of the perturbation assumption, the right side of the above equation is Taylor expanded: When the motion is small, higher-order infinitesimals can be ignored, and formula 12 is substituted into formula 11 to establish the following equation: Divide both sides of the equation by δt, and let have: I x u+I y v=-I t (14) where u and ν are continuous optical flows; I x and I y is the image gradient along the x and y directions at the point; according to the spatial consistency assumption, the optical flow value between the areas around the corresponding pixels on the image is a constant; assuming that the size of the area is m×m, and the number of pixels it contains is n=m×m, the equation is established as follows: The overdetermined equations are: The optimal solution can be solved by the above equation, which is the most likely optical flow value in the neighborhood; From formula (11) The body motion is linked to the image grayscale; at the same time, a satisfying f can be obtained from equations 3 and 4. -1 () makes f -1 (I i (x))≈t i B'(x) (17) Using Equation 17 to expand the image flow I(x,y,t), we can get f -1 (I(x,y,t))≈eB'(x,y,t) (18) In the formula, in order to distinguish the image stream timestamp t, e is used to represent the exposure time; Formula 11 is expanded using Formula 18 eB'(x,y,t)=(eΔexp)B'(x+Δx,y+Δy,t+Δt) (19) Where Δexp is the change in exposure time between two adjacent frames; let B' e =eB', Taylor expands the right side of formula 19 to obtain the following equation: Substituting form 20 into form 19, we get form 21 which is similar to form 13: make Simplifying equation 21, we can get the overdetermined equation: Where u and v are optical flows, and τ is directly related to the exposure time. Through the above formula, the change in exposure time and the change in posture under this change are estimated at the same time. The exposure information during the camera operation is obtained synchronously through equation (22).

2. The improved VINS method for complex lighting environments according to claim 1, characterized in that: The specific steps of this method are as follows: Step 1: Build an information data collection system Based on the information collection and transmission methods between sensors, a multi-sensor information data collection system equipped with GPS, camera, and IMU sensors is constructed; Step 2: System Initialization Start and initialize the system, calibrate the carrier camera intrinsic parameters, IMU bias, and basic information of the transformation matrix between the IMU coordinate system and the camera coordinate system; Step 3: Camera Initialization Continuously collect multiple groups of images with different exposure times under the same posture state of the body, process the collected images, establish the cost function of the camera response function, and establish a nonlinear optimization process; Solve the above equation to find f -1 () and B'(x), respectively solve the partial derivatives of the cost function for the two sets of unknown quantities and set the partial derivatives equal to zero to obtain the local minimum, and we have: Solve for the optimal f -1 () and B'(x) Step 4: Exposure parameter estimation during body motion Read two adjacent frames of pictures and import the data into the following formula The exposure parameter τ and the compensated optical flow estimates u, v are solved simultaneously.

Images