SLAM method based on binocular vision and IMU fusion

Abstract

The invention belongs to the technical field of binocular vision, and provides an SLAM (Simultaneous Localization and Mapping) method based on binocular vision and IMU (Inertial Measurement Unit) fusion. Comprising a front-end visual odometer module, a rear-end nonlinear optimization module and a loopback detection module, the front-end visual odometer module comprises an image feature extraction part and an initialization part, in the image feature extraction part, depth information is obtained, then IMU pre-integration is carried out, an initial value of initialization is provided, and in the initialization process, a loose coupling method is adopted to carry out state estimation on visual information and an IMU measurement value; after initialization is completed, entering a back-end nonlinear optimization module, and in the back-end nonlinear optimization process, adopting a sliding window tight coupling nonlinear optimization method to minimize all error terms; and finally, eliminating accumulated errors through loopback optimization. Compared with the prior art, the positioning accuracy can be effectively improved, so that the accuracy and robustness of the system are remarkably improved.

Description

Technical Field

[0001] The invention belongs to the field of binocular vision technology, and specifically relates to a SLAM method based on the fusion of binocular vision and IMU. Background Art

[0002] Simulaneous localization and mapping (SLAM) generally refers to the process in which a carrier uses its own sensors to estimate its position and obtain its own motion trajectory. With the development of artificial intelligence, SLAM systems have shined in many fields. Purely visual SLAM systems have high requirements for the surrounding environment and cannot process some dynamic scenes and scenes with missing texture details, which has great limitations. The Inertial Measurement Unit (IMU) has the feature of measuring the acceleration and angular velocity of the carrier in real time, which can effectively alleviate the limitations of pure visual SLAM. Summary of the invention

[0003] In view of the above defects, the purpose of the present invention is to propose a SLAM method based on binocular vision and IMU fusion, which can effectively improve the accuracy of positioning.

[0004] The technical scheme adopted by the present invention to achieve its purpose is: a SLAM method based on binocular vision and IMU fusion, comprising a front-end visual odometer module, a back-end nonlinear optimization module and a loop detection module; the front-end visual odometer module comprises two parts, image feature extraction and initialization, in which ORB feature points are extracted in the image feature extraction part, feature matching is then performed to obtain depth information, and then IMU pre-integration is performed to obtain displacement, speed and rotation at the current moment, and initial values ​​for initialization are provided, and in the initialization process, a loosely coupled method is used to perform state estimation on visual information and IMU measurement values ​​respectively; after initialization is completed, the back-end nonlinear optimization module is entered, and in the process of back-end nonlinear optimization, a sliding window tightly coupled nonlinear optimization method is used to minimize all error terms; the loop detection module uses a loop detection method based on a bag-of-words model to realize closed-loop detection, and finally loop optimization is used to eliminate accumulated errors.

[0005] Furthermore, the IMU pre-integration process includes establishing an IMU noise model and a measurement model to complete the IMU pre-integration process.

[0006] Furthermore, the loop detection module performs relocation, uses a dictionary tree to calculate the similarity between images, and corrects the accumulated error with the help of loop constraints.

[0007] In the present invention, the front-end visual odometer module includes two parts: image feature extraction and initialization. In the feature extraction process, the main direction of the feature point is mainly calculated to increase the rotation invariance, and the feature matching is implemented using the fast nearest neighbor. This method can reduce the computational complexity and is more efficient. Secondly, the IMU noise model and measurement model are established, and the IMU pre-integration process is completed to provide inertial measurement information for system initialization. System initialization is a very important part of the system, which has a direct impact on the system performance and can provide the system with more accurate initial values ​​and parameters. The present invention uses pnp to estimate the motion of the camera in binocular vision initialization, specifically estimating the camera's posture through the projection positions of n 3D space points and n points; the back-end nonlinear optimization uses a sliding window to control the scale of optimization, the back-end receives the camera posture of the visual mileage calculation at different times, and the information of loop detection, optimizes them, and obtains a globally consistent trajectory and map; loop detection is repositioning, and a method based on a bag-of-words model is used for loop detection, a trained dictionary tree is pre-loaded, and the dictionary tree is used to calculate the similarity between images to detect loops, and loop constraints are used to correct accumulated errors. After the loop frame is identified, the repositioning function is realized by means of a tight coupling method, and the current trajectory is loop optimized, thereby significantly improving the accuracy and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 It is the overall framework diagram of the present invention. DETAILED DESCRIPTION

[0009] See also Figure 1 The present invention proposes a SLAM method based on binocular vision and IMU fusion, including a front-end visual odometer module, a back-end nonlinear optimization module and a loop detection module; the front-end visual odometer module includes two parts: image feature extraction and initialization. In the image feature extraction part, ORB feature points are extracted, and then feature matching is performed to obtain depth information. Then, IMU pre-integration is performed to obtain the displacement, speed and rotation at the current moment, and the initial value of initialization is provided. During the initialization process, a loosely coupled method is used to perform state estimation on the visual information and the IMU measurement value respectively; after the initialization is completed, the back-end nonlinear optimization module is entered. During the back-end nonlinear optimization process, a sliding window tightly coupled nonlinear optimization method is used to minimize all error terms; the loop detection module uses a loop detection method based on a bag-of-words model to realize closed-loop detection, and finally the accumulated error is eliminated through loop optimization.

[0010] Furthermore, the IMU pre-integration process includes establishing an IMU noise model and a measurement model to complete the IMU pre-integration process.

[0011] Furthermore, the loop detection module performs relocation, uses a dictionary tree to calculate the similarity between images, and corrects the accumulated error with the help of loop constraints.

[0012] In the present invention, the front-end visual odometer module includes two parts: image feature extraction and initialization. In the feature extraction process, the main direction of the feature point is mainly calculated to increase the rotation invariance. Feature matching is implemented using fast nearest neighbor, which can reduce the computational complexity and is more efficient. Secondly, the IMU noise model and measurement model are established, and the IMU pre-integration process is completed to provide inertial measurement information for system initialization.

[0013] Specific implementation steps of feature extraction and matching: Use ORB feature point method to extract and match features. The extraction steps are as follows:

[0014] Step 1: Select a point P from the image, draw a circle with P as the center and 3 pixels as the radius, and set a threshold based on the pixel value of point P. If the following two conditions are met, P is considered a feature point.

[0015] The two conditions are: (1) First, detect the grayscale values ​​at the four positions 1, 5, 9, and 13. There must be 3 or more pixel values ​​greater than (or less than) the set threshold;

[0016] (2) There are n consecutive pixels on the circumference whose grayscale values ​​are greater (or smaller) than the threshold value of point P, where n is generally 9, 11 or 12.

[0017] Step 2: In order to keep the scale unchanged, ORB features will build a Gaussian pyramid, reduce and enlarge the original image according to a certain scale factor, and then extract ORB feature points in each layer of the image.

[0018] Step 3: Perform corner detection on each layer of the constructed pyramid to make the scale invariant. Use the intensity centroid method to verify the invariance. The specific implementation process is as follows:

[0019] 1. Take a small image block, the moment of the image block is:

[0020] m pq =Σ x,y∈B x p y p I(x,y)p,q={0,1}

[0021] 2. Solve the centroid of the image block based on the moment of the image block:

[0022]

[0023] 3. The direction vector of the feature point can be defined as:

[0024]

[0025] Step 4: Perform feature matching on the two corrected images, use the corresponding algorithm to eliminate the mismatched feature points in the image, and finally use triangulation to calculate the depth of the feature points based on the matching results.

[0026] Step 5: Establish IMU noise model and measurement model, angular velocity measurement value of IMU noise model and acceleration measurements Respectively expressed as:

[0027]

[0028] Among them, ω (t) Represents the actual measurement value of the IMU gyroscope, is the bias of the IMU gyroscope, which follows a Gaussian distribution. is the noise of the IMU gyroscope, a (t) Represents the actual measurement value of the IMU accelerometer, is the bias of the IMU accelerometer, which follows a Gaussian distribution. is the noise of the IMU accelerometer.

[0029] The IMU motion model is as follows:

[0030]

[0031] B represents the IMU body coordinate system, W represents the world coordinate system, and WB as a subscript represents the conversion from the IMU coordinate system to the world coordinate system. w is the acceleration due to gravity.

[0032] According to the dynamic model, the angular velocity and acceleration of the IMU are integrated to obtain the data of posture and velocity.

[0033]

[0034] Next, let the time interval between the two frames of image k and k+1 be Δt, and integrate the above equation:

[0035]

[0036] Because the frame rate of the IMU is very high, the corresponding time interval Δt is very short, and it can be considered that there is no change in the interval, thus simplifying the above formula to:

[0037] R WB(t+Δt) =R WB(t) exp(w B(t) Δt)

[0038] v W(t+Δt) =v W(t) +a w(t) Δt

[0039]

[0040] Next, substitute the IMU measurement value into the above formula.

[0041] Step 6: After the model is established, IMU pre-integration is performed to complete the image preprocessing part.

[0042] System initialization is a very important part of the system. It has a direct impact on system performance and can provide the system with relatively accurate initial values ​​and parameters. In binocular vision initialization, pnp is used to estimate the motion of the camera. Specifically, the camera's position is estimated by n 3D space points and the projection positions of n points. The main task of visual inertial navigation joint initialization is to align the IMU with the trajectory of the camera. The specific implementation steps of system initialization are as follows:

[0043] Step 1: Use the pnp method to estimate camera motion, calculate the pixel and spatial point position of a spatial point P and its projection P′, spatial point P i =[X i ,Y i ,Z i ] T , the projected pixel coordinate u i =[u i ,v i ] T .

[0044]

[0045] Step 2: Minimize the error.

[0046]

[0047] Step 3: Construct an unconstrained optimization problem and solve the Jacobian matrix.

[0048] e(x+Δx)≈e(x)+JΔx

[0049]

[0050] The backend nonlinear optimization uses a sliding window to control the scale of optimization. The backend receives the camera pose of the visual mileage calculation at different times, as well as the information of loop detection, and optimizes them to obtain a globally consistent trajectory and map.

[0051] Step 1: Use the sliding window tightly coupled nonlinear optimization method to minimize all error terms and add new variables into the least squares system optimization;

[0052] Step 2: Maintain the number of window key frames. When the number reaches a certain dimension, remove the old variables. Repeat these two steps to integrate all state quantities in the sliding window together and minimize the loss function by optimizing the state vector.

[0053] Step 3: Joint optimization to obtain the best estimate of the camera pose and 3D landmark points, and introduce inertial measurement to create constraints between the camera's continuous motion poses.

[0054]

[0055] The state vector contains the states of all n+1 cameras in the sliding window: position, orientation, acceleration, and gyroscope bias, as well as the external parameters from the camera to the IMU and the inverse depth of m+1 3D points. Then, the objective function of the optimization problem is constructed:

[0056]

[0057] Among them, the three error terms are marginalized prior information, IMU measurement error and visual reprojection residual. The minimum value of the objective function is then solved through the optimization method to complete the optimization.

[0058] Loop detection is a relocation process. It uses a bag-of-words model-based method to perform loop detection. The trained dictionary tree is pre-loaded, and the dictionary tree is used to calculate the similarity between images to detect loops. Loop constraints are used to correct accumulated errors. After the loop frame is identified, the relocation function is achieved through tight coupling, and the current trajectory is loop optimized, thereby significantly improving the accuracy and robustness of the system.

[0059] Step 1: When the system first starts running, loop closure is not detected. Key frames are regularly selected during the preprocessing process. The first 100 input key frames are trained to generate the dictionary tree of the bag-of-words model.

[0060] Step 2: Traverse the key frame database, perform practical consistency checks, and calculate the similarity score of each group. The key frame with the highest score in each group is the candidate closed-loop frame and is placed in the closed-loop candidate frame queue.

[0061] Step 3: Check the geometric consistency of the candidate key frames and remove geometric outliers.

[0062] The fourth step is to set the feature point set matched between the current frame and the loop frame as L, with v representing the key frame of the closed loop and l representing the sequence number of the feature point observed in the loop frame. The objective function of relocalization is:

[0063]

[0064] The fifth step is to perform closed-loop optimization after relocalization is completed. The constraint relationship between the two key frames i and j in the closed-loop optimization is assumed to be:

[0065]

[0066] In the above formula, Indicates relative position. Represents the yaw angle, and defines the residual between two frames as:

[0067]

[0068] In the sixth step, let the sets of all closed-loop edges and sequence edges be L and S respectively, and perform closed-loop optimization on the loss functions of the two sets.

[0069]

Claims

1. A SLAM method based on binocular vision and IMU fusion, characterized in that: The method comprises a front-end visual odometer module, a back-end nonlinear optimization module and a loop detection module; the front-end visual odometer module comprises two parts, namely, image feature extraction and initialization. In the image feature extraction part, ORB feature points are extracted, and then feature matching is performed to obtain depth information; then IMU pre-integration is performed to obtain the displacement, speed and rotation at the current moment, and the initial value of initialization is provided. During the initialization process, a loosely coupled method is used to perform state estimation on the visual information and the IMU measurement value respectively; after the initialization is completed, the back-end nonlinear optimization module is entered. During the back-end nonlinear optimization process, a sliding window tightly coupled nonlinear optimization method is used to minimize all error terms; the loop detection module uses a loop detection method based on a bag-of-words model to realize closed-loop detection, and finally loop optimization is used to eliminate the accumulated error.

2. A SLAM method based on binocular vision and IMU fusion as claimed in claim 1, characterized in that, The IMU pre-integration process includes establishing an IMU noise model and a measurement model to complete the IMU pre-integration process.

3. A SLAM method based on binocular vision and IMU fusion as claimed in claim 1, characterized in that, The loop detection module performs relocation, uses a dictionary tree to calculate the similarity between images, and corrects the accumulated error with the help of loop constraints.

Images