A multi-sensor fusion SLAM method for monotonous and textureless large scenes

Abstract

The present invention relates to a multi-sensor fusion SLAM method for monotonous, textureless, large scenes, comprising the following steps: S1, acquiring information from a 3D laser radar, an IMU sensor, a depth camera, and a wheel encoder; S2, extracting ORB feature points from each frame of image captured by the depth camera through an image pyramid, matching the feature points, and obtaining an estimated pose value of the depth camera based on the matching results; S3, monitoring the information of the IMU and wheel encoder for pose fusion, obtaining an updated pose matrix and covariance matrix; S4, further fusing the pose fused from the IMU and wheel encoder with the pose estimate of the depth camera; S5, classifying and matching the point cloud scanned by the 3D laser radar to obtain a dedistorted point cloud; S6, further classifying and matching the dedistorted point cloud, and constructing a global map. Compared with the existing technology, the present invention has the advantages of improved system robustness, increased computing speed, and simple deployment.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robots, and in particular to a multi-sensor fusion SLAM method for monotonous and textureless large scenes. Background Art

[0002] With the modernization, automation and intelligent development of industrial technology, more and more scenarios require the participation of robots and unmanned vehicles. At the same time, localization and mapping (SLAM) has become a key technology for autonomous navigation and positioning of unmanned vehicles, allowing robots to create environmental maps based on their sensors (such as lidar, depth cameras, etc.) during movement in unknown environments, thereby realizing autonomous positioning and navigation of robots.

[0003] Robot navigation technology based on lidar is inseparable from the implementation of positioning technology. The premise of positioning is a clear environmental map. The existing SLAM technology can perform high-precision mapping and positioning in small-scale scenes, but in large-scale scenes with weak textures, problems such as unclear mapping and inaccurate positioning will occur, especially for SLAM systems with cameras as the main sensors. Therefore, it is necessary to propose a multi-sensor fusion SLAM method for monotonous and textureless large scene environments. Summary of the Invention

[0004] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a multi-sensor fusion SLAM method for monotonous and textureless large scenes.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] A multi-sensor fusion SLAM method for monotonous and textureless large scenes is used to reduce the drift caused by radar positioning in monotonous and textureless large scenes, including the following steps:

[0007] S1, obtain information from 3D lidar, IMU sensor, depth camera and wheel encoder;

[0008] S2. Extract ORB feature points from each frame of image captured by the depth camera through the image pyramid, match the feature points, and obtain the pose estimation value of the depth camera based on the matching results;

[0009] S3, monitors the information of IMU and wheel encoder, performs pose fusion through extended Kalman filtering, and obtains the updated pose matrix and covariance matrix;

[0010] S4, further fuse the pose after the fusion of IMU and wheel encoder with the pose estimation value of the depth camera, and determine the dynamic weight of the fusion according to the number of ORB feature points extracted by the visual odometry;

[0011] S5. Classify and match the point cloud scanned by the 3D laser radar, and then calculate the current position of the radar through least squares to obtain the dedistorted point cloud;

[0012] S6. Further classify and match the dedistorted point cloud to obtain accurate pose and build a global map.

[0013] The step S1 is specifically as follows:

[0014] According to the actual parameters and application requirements of the 3D lidar, the laser point cloud is obtained and filtered, and the pixel information and depth information of the depth camera are divided into two channels for extraction.

[0015] The step S2 is specifically as follows:

[0016] For the FAST corner points in ORB feature points, an image pyramid is first used to extract the corner points to ensure scale characteristics. During the extraction process, the feature points are evenly distributed on each layer of the pyramid to improve the robustness and stability of the system. The direction of the corner points is then obtained using the grayscale centroid method. The BRIEF descriptor of the feature points is calculated and matched using the Hamming distance. Finally, the bundle adjustment method is used to construct the least squares problem of the camera pose with respect to the reprojection error, and the camera pose is iteratively solved using the Levenberg-Marquardt method.

[0017] The calculation formula for extracting corner points using image pyramid is:

[0018]

[0019] Among them, N m is the number of feature points to be extracted at the mth level of the pyramid, N is the total number of feature points to be extracted, s is the scaling factor between adjacent pyramid levels, and M is the total number of pyramid levels.

[0020] In order to overcome the difficulty in extracting feature points in textureless environments, the index δ is proposed to measure the accuracy of visual odometry. The expression of the index δ is:

[0021]

[0022] Among them, n_featurepoints is the maximum number of feature points extracted, and n is the number of feature points extracted.

[0023] The step S3 is specifically as follows:

[0024] First, start the ROS node and listen to the information released by the wheel encoder and IMU, build an extended Kalman filter, perform state updates, and use the obtained wheel encoder information as the observation quantity and covariance matrix to update the state, and send the updated state variables and covariance matrix; then obtain the IMU information as the observation quantity and covariance matrix and update the state according to the state variables and covariance matrix sent by the wheel encoder; finally, publish the updated state variables and covariance matrix as the fused IMU odometer information.

[0025] In step S4, when the visual odometry is integrated with the robot's own motion odometry, the dynamic weight of the integration is determined according to the number of ORB feature points, and the following is obtained:

[0026] When the number of feature points extracted by the visual odometry is n>80:

[0027]

[0028] When the number of feature points extracted by the visual odometry is n < 30:

[0029]

[0030] When the number of feature points extracted by the visual odometry is 30<n<80:

[0031]

[0032] Among them, t is the fused position vector, q is the fused attitude quaternion, t camera is the camera position vector, q camera is the camera's attitude quaternion, t imu is the position vector of the IMU odometer, q imu is the attitude quaternion of the odometry, and then outputs the fused pose according to the dynamic weight.

[0033] In step S5, the plane smoothness of each feature point is calculated to classify it into plane points and edge points. The edge points are matched by calculating the point-to-line distance, and the plane points are matched by calculating the point-to-plane distance. The radar's least squares function with respect to distance is obtained, and iterative optimization is performed to obtain the radar pose. The point cloud data is reprojected to the initial moment to eliminate the point cloud distortion caused by the radar motion.

[0034] The expression of the plane smoothness c is:

[0035]

[0036] The radar coordinate system is set as L, L k represents the radar observation coordinate system at the kth period, X L(k,i) is the i-th point cloud scanned in the k-th cycle, S is the set where the current point is located,

[0037] In step S6, the further classification and matching process of the feature points is the same as step S5. By storing the point cloud on the map in a cubic space, the feature points intersecting with the current point cloud are extracted, the covariance matrix of the set around the feature point is calculated, and the eigenvalues ​​and eigenvectors of the covariance matrix are calculated. According to the eigenvalues ​​and eigenvectors, the point is judged to be a plane point or an edge point. The method of calculating the point-to-plane distance for plane points and the point-to-line distance for edge points is used. The nonlinear optimization is optimized using the Levenberg-Marquardt method. Finally, the map is optimized using a pixel grid filter to make the point cloud distribution more uniform.

[0038] Compared with the prior art, the present invention has the following advantages:

[0039] 1. The present invention integrates visual odometry and IMU odometry, which can solve the problem of inaccurate positioning of lidar in open environments such as long corridors and underground garages due to the inability to scan enough feature points. The integration of multiple sensors also improves the robustness of the system.

[0040] 2. The radar pose estimation method is simple and efficient. The present invention avoids processing all points in all point clouds by classifying point clouds, thereby improving the operation speed of the SLAM method and saving more operation space.

[0041] 3. The deployment of the present invention is relatively simple, and the relevant parameters of the fusion can be adjusted, which can be flexibly adjusted to cope with various actual scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is the overall framework diagram of the SLAM method of the present invention.

[0043] Figure 2 This is the operating process of the visual odometry.

[0044] Figure 3 Flowchart of IMU fusion wheel encoder. DETAILED DESCRIPTION

[0045] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0046] like Figure 1 As shown, the present invention provides a multi-sensor fusion SLAM method for monotonous and textureless large scenes, which includes four parts:

[0047] (1) Front-end: This includes the interaction between sensors and the environment. The lidar obtains point cloud data by scanning the surrounding environment. The depth camera obtains the depth map and pixel coordinates by reading the surrounding environment. The IMU calculates the current acceleration and angular velocity of the car through the acceleration and gyroscope. At the same time, by constructing a visual odometry and fusing the IMU with the wheel encoder, the position of the camera and IMU can be estimated respectively. The two are then fused according to the following formula:

[0048] When the number of feature points extracted by the visual odometry is n>80, there are:

[0049]

[0050] When n<30:

[0051]

[0052] When 30<n<80, there are:

[0053]

[0054] Among them, t is the fused position vector, q is the fused attitude quaternion, t camera is the camera position vector, q camera is the camera's attitude quaternion, t imu is the position vector of the IMU odometer, q imi is the attitude quaternion of the odometry, and then outputs the fused pose according to the dynamic weight.

[0055] (2) Radar odometer: The motion estimation model of the radar in the present invention is a uniform speed and uniform angular velocity motion in a scanning cycle, t is the current timestamp, t+1 is the start of the next scanning cycle, T k+1 L For radar in [t k+1 ,t] time period, T k+1 L Including the rigid motion of the radar in 6 degrees of freedom, and T k+1 L =[t x ,t y ,t z ,θ x ,θ y ,θ z ] T ,t x ,t y ,t z is the displacement of the radar coordinate system along the x, y, and z axes, θ x ,θ y ,θ zis the rotation angle in the right-hand coordinate system, for P k+1 Point i in the i is the current timestamp, T (k+1,i) L for [t k+1 ,t i ] coordinate transformation, T (k+1,i) L Can be T k+1 L Linear interpolation yields:

[0056]

[0057] ε k+1 and H k+1 P k+1 The set of edge points and plane points, and For the tth k+1 The point set after reprojection in the second scan is used to solve the radar motion. k+1 and and H k+1 and The geometric relationship is deduced as follows:

[0058]

[0059] X L (k+1,i) is ε k+1 and The coordinates of the midpoint i, yes and The coordinates of the points that meet the conditions in T (k+1,i) L (a:b) is T (k+1,i) L The element from a to b in , R is the rotation matrix calculated by Rodriguez formula:

[0060]

[0061] Get in ε k+1 The geometric relationship between edge points and feature lines is obtained:

[0062] fε(X L (k+1,i) ,T k+1 L )=dε,i∈ε k+1

[0063] Get H k+1 The geometric relationship between the midplane point and the feature plane:

[0064] fH(X L (k+1,i) ,T k+1 L )=dH,i∈H k+1

[0065] Finally, solving for the radar motion using the Levenberg-Marquardt method yields the nonlinear function:

[0066] f(T k+1 L )=d

[0067] Each row of f corresponds to a feature point, and d includes the corresponding distance. The present invention calculates the relationship between f and T k+1 L The Jacob matrix is ​​denoted as J, and the algorithm is This problem is then transformed into a nonlinear least squares problem solved by minimizing d:

[0068] T k+1 L ←T k+1 L -(J T J+λdiag(J T J)) -1 J T d

[0069] Where λ is the factor in the LM method.

[0070] (3) Radar mapping: storing point cloud Q on the map k To a 10m cube space, the cube The intersecting feature points will be extracted and stored in the 3D kd-tree. Let S′ be Q k The set of points surrounding the feature point in S′ is then calculated, denoted as M, and the eigenvalues ​​and eigenvectors of M are denoted as V and E. If one of the eigenvalues ​​contained in V is significantly larger than the other two, then the set of S′ is a boundary line, and the eigenvector E corresponding to this eigenvalue also represents the direction of the boundary line. If V contains two large eigenvalues ​​and the other is very small, then the set of S′ is a plane, and the E corresponding to the smallest eigenvalue also represents the direction of this plane.

[0071] (4) Posture conversion: By integrating the posture output by the radar odometry and radar mapping, the accurate posture is finally output and the posture is optimized using an iterative method.

[0072] Figure 2 The construction process for visual odometry includes the following steps:

[0073] First, the camera is initialized. The visual odometry node receives the camera's internal parameters and obtains the translation amount and focal length information of the pixel coordinate system. Then, feature points are extracted. ORB feature points are extracted under the current camera image. A threshold N is set. When the number of feature points is less than N, it means that the environment lacks feature points and the positioning is inaccurate. Here, N=30 is taken. When N<30, the camera is reinitialized and the current pose is set to 0. When the number of feature points is greater than N, as the robot continues to move forward, the visual odometry matches the previously extracted feature points with the feature points extracted in the current frame, and estimates the camera pose through the matched feature point pairs. Finally, the point cloud data is published to provide a basis for map construction based on the visual odometry.

[0074] The extraction and matching of feature points can be divided into the following steps:

[0075] (1) Construct an image pyramid for the current image;

[0076] (2) Extract FAST corner points at each level of the pyramid;

[0077] (3) Calculate the angle of the feature point according to the grayscale centroid method;

[0078] (4) Calculate the BRIEF descriptor;

[0079] (5) Match the BRIEF descriptors of the two frames of images.

[0080] Camera motion estimation uses bundle adjustment (BA), a method that extracts optimal model and camera parameters through visual reconstruction. This method sums the difference between the camera's actual projected position and its observed position, converting the error into a function related to the camera's pose. This function is then iteratively optimized using the least-squares method to obtain the optimal estimate of the camera's pose.

[0081] Figure 3 The process of integrating wheel encoders into the IMU includes the following steps:

[0082] (1) Start the ROS node and monitor the information sent by the wheel encoder and IMU

[0083] (2) Constructing an Extended Kalman Filter

[0084] (3) Add white noise and update the status

[0085] (4) Obtain the information of the wheel encoder as the observation quantity and covariance matrix and update the state, and then send the updated state variables and covariance matrix

[0086] (5) Obtain IMU information as observation quantity and covariance matrix and perform state update based on the state variables and covariance matrix sent by the wheel encoder

[0087] (6) Publish the last updated state variables and covariance matrix as the fused odometry information to the topic / odom_combined.

Claims

1. A multi-sensor fusion SLAM method for monotonous and textureless large scenes, used to reduce the drift caused by radar positioning in monotonous and textureless large scenes, characterized by: It includes the following steps: S1. Obtain the information of the 3D lidar, IMU sensor, depth camera, and wheel encoder; S2. Extract ORB feature points from each frame of the image collected by the depth camera through an image pyramid, implement the matching between feature points, and obtain the pose estimation value of the depth camera according to the matching result; S3. Listen to the information of the IMU and wheel encoder, perform pose fusion through an extended Kalman filter, and obtain the updated pose matrix and covariance matrix; S4. Further fuse the pose after the fusion of the IMU and wheel encoder with the pose estimation value of the depth camera, and determine the dynamic weight of the fusion according to the number of ORB feature points extracted by the visual odometer; S5. Classify and match the point cloud scanned by the 3D lidar, and then calculate the current pose of the lidar through least squares and obtain the undistorted point cloud; S6. Further classify and match the undistorted point cloud to obtain an accurate pose and construct a global map; The specific step S2 is as follows: For the FAST corner points in the ORB feature points, first use an image pyramid to extract the corner points to ensure scale characteristics, and ensure that the feature points are evenly distributed on each layer of the pyramid during the extraction process to improve the robustness and stability of the system. Then obtain the corner point direction through the gray centroid method, calculate the BRIEF descriptor of the feature points and perform matching through the Hamming distance. Finally, use the bundle adjustment method to construct a least squares problem of the camera pose with respect to the reprojection error, and use the Levenberg-Marquardt method to iteratively solve the camera pose; The calculation formula for extracting corner points using an image pyramid is: Among them, N m is the number of feature points to be extracted at the mth level of the pyramid, N is the total number of feature points to be extracted, s is the scaling factor between adjacent levels of the pyramid, and M is the total number of levels of the pyramid; To overcome the influence of difficulty in extracting feature points in a textureless environment, an index δ is proposed to measure the accuracy of the visual odometer. The expression of the index δ is: where, n_featurepoints is the maximum number of feature points extracted, and n is the number of feature points extracted.

2. A multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 1, characterized in that: The specific step S1 is as follows: Obtain the lidar point cloud according to the actual parameters and application requirements of the 3D lidar and perform filtering. Extract the pixel information and depth information of the depth camera into two channels.

3. A multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 1, characterized in that: The specific step S3 is as follows: First, start the ROS node and listen to the information published by the wheel encoder and IMU, construct an extended Kalman filter, perform state updates, use the obtained wheel encoder information as the observation quantity and covariance matrix for state updates, and send the updated state variables and covariance matrix; then obtain the IMU information as the observation quantity and covariance matrix and perform state updates according to the state variables and covariance matrix sent by the wheel encoder; finally, publish the updated state variables and covariance matrix as the fused IMU odometer information.

4. A multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 1, characterized in that: In step S4, when fusing the visual odometer with the robot's own motion odometer, determine the dynamic weight of the fusion according to the number of ORB feature points. Then there is: When the number of feature points n extracted by the visual odometer > 80: When the number of feature points n extracted by the visual odometer < 30: When the number of feature points n extracted by the visual odometer 30 < n < 80: Among them, t is the fused position vector, q is the fused attitude quaternion, t camera is the camera position vector, q camera is the camera's attitude quaternion, t imu is the position vector of the IMU odometer, q imu is the attitude quaternion of the odometry, and then outputs the fused pose according to the dynamic weight.

5. The multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 1, characterized in that: In step S5, the plane smoothness of each feature point is calculated to classify it into plane points and edge points. The edge points are matched by calculating the point-to-line distance, and the plane points are matched by calculating the point-to-plane distance. The radar's least squares function with respect to distance is obtained, and iterative optimization is performed to obtain the radar pose. The point cloud data is reprojected to the initial moment to eliminate the point cloud distortion caused by the radar motion.

6. A multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 5, characterized in that: The expression of the plane smoothness c is: The radar coordinate system is set as L, L k represents the radar observation coordinate system at the kth period, X L (k,i) is the i-th point cloud scanned in the k-th cycle, and S is the set where the current point is located.

7. The multi-sensor fusion SLAM method for monotonous and textureless large scenes according to claim 1, characterized in that: In step S6, the further classification and matching process of the feature points is the same as step S5. By storing the point cloud on the map in a cubic space, the feature points intersecting with the current point cloud are extracted, the covariance matrix of the set around the feature point is calculated, and the eigenvalues ​​and eigenvectors of the covariance matrix are calculated. According to the eigenvalues ​​and eigenvectors, the point is judged to be a plane point or an edge point. The method of calculating the point-to-plane distance for plane points and the point-to-line distance for edge points is used. The nonlinear optimization is optimized using the Levenberg-Marquardt method. Finally, the map is optimized using a pixel grid filter to make the point cloud distribution more uniform.

Images