A SLAM algorithm based on asynchronous fusion of laser and vision

Abstract

The present invention discloses a SLAM algorithm based on asynchronous fusion of laser and vision. First, the data of the sensor is divided into m sampling intervals according to a faster sensor sampling frequency. There are two situations in each sampling interval: 1) only the pose estimation of the camera is available; 2) the pose estimation from the laser sensor and the visual sensor is available at the same time. By combining the Kalman filter and the method of asynchronous fusion of multiple sensors, an improved pose estimation algorithm based on asynchronous fusion of laser and vision is obtained. In order to effectively utilize the different characteristics of each sensor, an angle-based weighting coefficient is introduced according to the running state of the robot to improve the confidence of the measurement. The method can better utilize the redundant data in the visual sensor and improve the tracking accuracy of the algorithm. At the same time, the pose estimation of the visual sensor is used as the prior of the pose estimation of the laser sensor, which reduces the number of iterations and improves the efficiency of the algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning and navigation, and in particular to a SLAM algorithm based on asynchronous fusion of laser and vision. Background Art

[0002] The autonomous positioning and navigation capabilities of mobile robots are the key to walking-assistance robots, and accurate maps are the prerequisite for the autonomous positioning and navigation of walking-assistance robots.

[0003] Over the past few decades, mobile robot SLAM has experienced rapid development. Laser sensors and visual sensors are the two main sensors used in SLAM applications. SLAM algorithms based on laser sensors include the Gmapping algorithm, the Hector algorithm, and the Cartographer algorithm. While mapping can be achieved using only 2D laser sensors, since 2D laser sensors only provide depth information and scan a flat surface, the algorithm will fail if the depth information returned by the sensor does not change over time while the robot moves through a corridor. Cameras, as another inexpensive sensor, offer significant advantages in this situation. They can capture significant corners or edges in three-dimensional space. Furthermore, cameras are widely used due to their higher sampling rates.

[0004] Because each sensor has its own limitations, researchers have recently developed numerous SLAM mapping algorithms based on multi-sensor fusion. Hu Lihe et al. disclosed a recycling robot using multi-sensor fusion SLAM technology. By designing a robot control system consisting of a controller and a SLAM system, and integrating environmental data collected by ultrasonic obstacle avoidance sensors, infrared sensors, and radar sensors, the robot achieves automatic obstacle avoidance. The camera component scans positioning markers in real time, enabling precise positioning of the recycling robot. Wu Yelan et al. presented a SLAM positioning and navigation method and system based on multi-sensor fusion. By integrating information from a depth camera, lidar, and omnidirectional wheel odometry for navigation, and by improving the Bayesian data fusion algorithm, the system ultimately achieves multi-sensor-assisted automatic localization, global two-dimensional grid mapping, and obstacle avoidance for the mobile robot, thereby enabling autonomous navigation. Despite this, algorithms that combine vision and laser systems are very rare. Therefore, integrating these two sensor systems, which have strong complementary advantages, to achieve efficient and accurate mapping and navigation is a highly significant task. Summary of the Invention

[0005] In order to overcome the shortcomings of the existing technology, the present invention proposes a weighted asynchronous fusion algorithm based on 2D laser and binocular camera. The traditional fusion method is to fuse the two sensors after obtaining the optimal estimate of the two sensors, but the speed of the two sensors is usually not the same. If the timestamp alignment method is used, it will cause a lot of data from the faster sensor to not be well utilized. Therefore, the present invention adopts an asynchronous fusion method to fuse the data from the two sensors. This algorithm takes into account the acquisition frequency characteristics of different sensors and enables different sensors to have different confidence levels under different operating conditions. At the same time, the visual sensor posture estimation is used as the prior for the laser sensor posture estimation, reducing the number of iterations and improving the efficiency of the algorithm. Finally, the effectiveness of the algorithm is also demonstrated through simulation and experiments. The experiments show that the algorithm proposed in the present invention has good accuracy and robustness.

[0006] The technical solutions of the present invention are as follows:

[0007] A SLAM algorithm based on asynchronous fusion of laser and vision includes the following steps:

[0008] Step 1) For the position increment obtained by the visual sensor feedback, according to the camera pinhole imaging principle and the rigid body transformation formula, it is assumed that the homogeneous coordinates of the spatial point P in the world coordinate system are P = (X W ,Y W ,Z W ,1) T . Then its coordinates in the camera's pixel coordinate system are:

[0009]

[0010] Where (u, v) is the coordinate value of point p in the pixel coordinate system; s is the depth value of point p in the camera coordinate system; K is the intrinsic parameter matrix of the camera; R is the rotation matrix; and t is the translation matrix.

[0011] Step 2) The feature points of the image in the current frame can be obtained through the ORB feature extraction algorithm. The descriptors of the feature points in the current frame obtained by the rBRIEF descriptor are matched with the feature points in the image at the previous moment to establish a corresponding matching relationship. The matching points at the previous moment are projected into the current frame image through the camera's intrinsic parameter matrix and initial posture. Since the camera's posture usually has a certain deviation, there is usually a certain deviation between the projected point and the matching point on the current map. Assume that the coordinates of the feature point P in the current frame image are P = (u1, v1) T , the coordinates of the matching point in the current frame corresponding to the matching point in the previous frame are P'=(u2,v2) T Usually, the coordinates of these two points are not the same due to the deviation of the camera pose, so the error function is defined as:

[0012]

[0013] Where e is the error value, (u, v) is the pixel coordinate of the feature point in the current frame, and P is the matching point corresponding to the feature point in the current frame. Rewrite it in Lie algebra form:

[0014]

[0015] Where exp(δ) is the Lie algebra expression of the camera coordinate system, and e is the projection error between the spatial point P projected into the current image and the matching point.

[0016] Usually there are several matching points in a frame of image, and the projection error function can be obtained based on all the matching points:

[0017]

[0018] where u i represents the i-th feature point in the current frame; P i Indicates the world coordinates of the corresponding matching point; s i Represents the depth of the point; K represents the intrinsic parameter matrix of the camera; exp(δ) represents the Lie algebra expression of the camera coordinate system.

[0019] Step 3) Solve the optimization equation using the Gauss-Newton method or the Levenberg-Marquardt method to obtain the pose estimate:

[0020]

[0021] where z1(k1) is the optimal estimate of the vision sensor at time k1, R is the rotation matrix, and t is the translation matrix. v1(k1) is the camera estimation noise and is assumed to follow a Gaussian distribution.

[0022] Step 4) Assume that the laser sensor data obtained at time k2 is Let S i (δ2) is the world coordinate, M(S i (δ2)) is the coordinate point S i The occupancy probability of (δ2), M(S i (δ2)). M(S i (δ2)) and The value of the world coordinate S i (δ2) is obtained by bilinear interpolation. Then the robot's posture increment from the previous moment to the current moment can be given by the following formula:

[0023]

[0024] in

[0025]

[0026] Step 5) Based on the pose increment, the coordinates of the robot under the laser sensor pose estimation can be obtained as:

[0027] z2(k2)=z2(k2-1)+Δδ2+v2(k2) (8)

[0028] Where z2(k2) is the optimal pose estimate of the laser sensor at time k2; z2(k2-1) is the optimal pose estimate at the previous moment; v2(k2) is the observation noise and is assumed to obey a Gaussian distribution, satisfying the following relationship:

[0029]

[0030] Step 6) Traditional fusion methods fuse the data from two sensors after obtaining the optimal estimate. However, the two sensors typically have different speeds. If timestamp alignment is used, much of the data from the faster sensor will not be fully utilized. Therefore, the present invention uses an asynchronous fusion method to fuse the data from the two sensors.

[0031] First, the sensor data is divided into m sampling intervals according to the sampling frequency of the faster sensor (simulated camera sensor). There are two situations in each sampling interval:

[0032] 1) Only have the camera pose estimation;

[0033] 2) Positive and negative pose estimation from both laser sensors and visual sensors.

[0034] Combining the Kalman filter and multi-sensor asynchronous fusion method, the improved pose estimation algorithm based on asynchronous fusion of laser and vision is obtained as follows:

[0035]

[0036] in is the predicted pose obtained based on the optimal estimate of the robot pose at the previous moment; P0(k|k) is the covariance matrix of the predicted pose; z i (k i ) is the optimal pose estimate from the visual or laser sensor at time k.

[0037] When there is only camera data in the sampling interval, i=1, and equation (10) becomes:

[0038]

[0039] At this time, the estimation model is transformed into a Kalman filter that estimates the pose of a single sensor data at a faster sensor sampling frequency.

[0040] When the camera data and the pose estimation results of the laser sensor exist simultaneously in the sampling interval, Equation (10) becomes Equation (12) and (13):

[0041]

[0042]

[0043] The estimation model now becomes a Kalman filter algorithm that fuses the laser sensor and vision sensor at a slower sensor (lidar) sampling rate. The algorithm uses the best estimate at the previous moment as the predictor, first fusing the pose estimated by sensor 1 (camera) at the current moment, then fusing the pose estimated by sensor 2 (lidar), and finally using the two fused sensor estimates as the final pose estimate.

[0044] Step 7) To effectively utilize the different characteristics of each sensor, the concept of confidence is introduced based on these characteristics. This is to reduce the impact of laser sensor data deviation when the robot walks in the corridor and to reduce the problem of pose drift caused by camera drift when the robot turns.

[0045] The confidence w is used to represent the confidence of the pose estimation of the two sensors, and Equation (10) is changed to:

[0046]

[0047] where w i Represents the confidence of the pose estimation of the i-th sensor.

[0048] Step 8) The characteristics of the sensor are usually related to the robot's movement process. When the robot walks in a straight line, the laser sensor often has positioning problems, and when turning, the camera often drifts. Therefore, w i It should be related to the robot's motion state. When the robot moves in a straight line, the algorithm increases the confidence in the vision; when the robot rotates, the algorithm increases the confidence in the laser.

[0049] At each fusion, the algorithm determines whether the robot has rotated during its current motion based on the latest pose estimate of the laser sensor. This is obtained from Equation (6) and is denoted as Δα2.

[0050] Substitute the angle change into w i get:

[0051]

[0052] Where Δα2 is the estimated robot rotation angle from the previous moment to the current moment. According to the current robot operation performance, the robot rotation angle does not exceed 0.25 rad in each sampling period, so Equation (15) is modified as follows:

[0053]

[0054] Substituting formula (16) into formula (14) yields:

[0055]

[0056]

[0057] The beneficial effects of the present invention are as follows:

[0058] This method firstly improves the data utilization rate of the visual sensor, and secondly takes advantage of the different characteristics of different sensors. The algorithm has better utilization rate of different sensors under different conditions, making the system tracking effect more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 This is a schematic diagram of the gazebo simulation environment of the present invention;

[0060] Figure 2 This is a schematic diagram of the mapping result obtained by the asynchronous Kalman filtering and weighting algorithm of the present invention;

[0061] Figure 3 This is a schematic diagram comparing the fusion pose estimation obtained by the weighted asynchronous fusion Kalman filtering algorithm of the present invention with the actual robot motion trajectory;

[0062] Figure 4 This is a schematic diagram of a real motion scene of the present invention;

[0063] Figure 5 This is a schematic diagram of the mapping result obtained by the weighted asynchronous fusion algorithm of the present invention;

[0064] Figure 6 This is a schematic diagram of the trajectory results obtained by the weighted asynchronous fusion algorithm of the present invention;

[0065] Figure 7 Flowchart of the present invention. DETAILED DESCRIPTION

[0066] The present invention will be described in further detail below with reference to the accompanying drawings.

[0067] Reference Figure 1-7 , a SLAM algorithm based on asynchronous fusion of laser and vision. The algorithm implementation process of this patent is as follows:

[0068] Step 1) To verify the effectiveness of the algorithm, experiments were conducted in the gazebo simulation environment and the real environment. Figure 1 The figure shows a randomly constructed Gazebo simulation environment. The simulation environment consists of a 16m×16m circular space. Most of the space is a normal, mappable environment, but a long corridor at the bottom is a degraded environment. In this corridor, the laser will obtain less feature information. A simulated lidar and a binocular camera are mounted above the robot model. The lidar uses the libgazebo_ros_laser.so plugin, with a maximum scanning distance of 6m and a maximum scanning angle of 4 rad (approximately 230°). The simulated noise is Gaussian with a mean of 0 and a variance of 0.01. The camera model uses the libgazebo_ros_multicamera.so plugin, with a horizontal field of view of 1.39 rad and an image size of 800×800 square. The simulated noise is Gaussian with a mean of 0 and a variance of 0.007.

[0069] Step 2) Use the visual pose estimation and laser pose estimation results to perform 2D SLAM mapping in the simulation map using asynchronous Kalman filtering and weighted algorithm, and obtain the mapping result as shown below: Figure 2 .

[0070] Step 3) Figure 3 The fusion pose estimation obtained by using the weighted asynchronous fusion Kalman filter algorithm for visual pose estimation and laser pose estimation is compared with the actual robot motion trajectory. Figure 3 It can be seen that the pose estimation curve of the algorithm has a very high fit with the actual robot motion curve, and the robot's positioning accuracy is more accurate.

[0071] Step 4) Figure 4The scene shown is a real-world mapping scenario, consisting of a typical first floor of a trapezoidal office building. The robot moves and maps within this degraded scene, with some areas in the center. The robot controller is a Jetson TX2, running Ubuntu 16.04 and the Kinetic version of ROS. A HOKUYO URG-04LX-UG012D laser rangefinder is mounted above the robot. It measures a range of 5.6 meters, an angle of 240 degrees, and an update rate of 10 Hz. A Kinect v1 RGB-D camera is mounted directly above the laser sensor. The camera has a color resolution of 640 × 480, a depth map resolution of 320 × 240, and an update rate of 30 Hz. The horizontal and vertical fields of view are 57° and 43°, respectively. The robot chassis is equipped with a pair of differential-drive wheels. Communication with the motor driver is accomplished using the ROS socket_can node, and robot motion is controlled via the keyboard control node.

[0072] Step 5) Use the weighted asynchronous Kalman filter algorithm to map Figure 5 As shown, the robot motion trajectory is Figure 6 The mapping results demonstrate that the algorithm works very well. The pose estimation trajectory shows that the starting and ending points of the trajectory estimation using the weighted asynchronous Kalman filter algorithm are essentially identical, demonstrating that the positioning accuracy error of the proposed algorithm is extremely small. This demonstrates the effectiveness of the proposed weighted asynchronous multi-rate fusion algorithm.

[0073] The above embodiment represents only one embodiment of the present invention and should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art can make several variations without departing from the scope of the present invention, and these variations fall within the scope of protection of the present invention.

Claims

1. A SLAM algorithm based on asynchronous fusion of laser and vision, characterized in that: The following steps are involved: Step 1) For the position increment obtained by the visual sensor feedback, according to the camera pinhole imaging principle and the rigid body transformation formula, it is assumed that the homogeneous coordinates of the spatial point P in the world coordinate system are P = (X W ,Y W ,Z W ,1) T ; then its coordinates in the camera's pixel coordinate system are: Where (u, v) is the coordinate value of point p in the pixel coordinate system; s is the depth value of point p in the camera coordinate system; K is the intrinsic parameter matrix of the camera; R is the rotation matrix; t is the translation matrix; Step 2) Obtain feature points of the image in the current frame through the ORB feature extraction algorithm, match the descriptors of the feature points in the current frame obtained by the rBRIEF descriptor with the feature points in the image at the previous moment, and establish a corresponding matching relationship; project the matching points at the previous moment into the current frame image through the camera's intrinsic parameter matrix and initial posture; since the camera's posture usually has deviations, there are usually deviations between the projection points and the matching points on the current map; assume that the coordinates of the feature point P in the current frame image are P = (u1, v1) T , the coordinates of the matching point in the previous frame in the current frame are P'=(u2,v2) T ; Usually due to the deviation of the camera pose, the coordinates of these two points are not the same, so the error function is defined: Where e is the error value, (u, v) is the pixel coordinates of the feature point in the current frame, and P is the matching point corresponding to the feature point in the current frame; Rewriting this into Lie algebraic form: Where exp(δ) is the Lie algebra expression of the camera coordinate system, and e is the projection error between the spatial point P projected into the current image and the matching point; Usually there are several matching points in a frame of image, and the projection error function is obtained based on all the matching points: where u i represents the i-th feature point in the current frame; P i Indicates the world coordinates of the corresponding matching point; s i represents the depth of the point; K represents the intrinsic parameter matrix of the camera; exp(δ) represents the Lie algebraic expression of the camera coordinate system; Step 3) Use the Gauss-Newton method or the Levenberg-Marquardt method to solve formula (4) and obtain the pose estimate: Where z1(k1) is the optimal estimate of the visual sensor at time k1, R is the rotation matrix, and t is the translation matrix; v1(k1) is the camera estimation noise and is assumed to obey a Gaussian distribution; Step 4) Assume that the laser sensor data obtained at time k2 is Let S i (δ2) is the world coordinate, M(S i (δ2)) is the coordinate point S i The occupancy probability of (δ2), M(S i (δ2)) derivative; M(S i (δ2)) and The value of the world coordinate S i (δ2) is obtained by bilinear interpolation; then the robot's position increment from the previous moment to the current moment can be given by the following formula: in Step 5) According to the pose increment, the coordinates of the robot under the pose estimation of the laser sensor are obtained as follows: z2(k2)=z2(k2-1)+Δδ2+v2(k2) (8) Where z2(k2) is the optimal pose estimate of the laser sensor at time k2; z2(k2-1) is the optimal pose estimate at the previous moment; v2(k2) is the observation noise and is assumed to obey a Gaussian distribution, satisfying the following relationship: where v i (k i ), v j T (k j ) is the measurement noise, E represents the mean, δ ij , is the Kronecker function, R i (k i ) is the covariance matrix from the visual or laser sensor at time k; Step 6) An asynchronous fusion method is used to fuse the data from the two sensors; First, the sensor data is divided into m sampling intervals according to the faster sensor sampling frequency. There are two situations in each sampling interval: 1) Only have the camera pose estimation; 2) Having pose estimation from both laser sensors and visual sensors; Combining the Kalman filter and multi-sensor asynchronous fusion method, the improved pose estimation algorithm based on asynchronous fusion of laser and vision is obtained as follows: in is the predicted posture obtained based on the optimal estimate of the robot posture at the previous moment; P0(k|k) is the covariance matrix of the predicted posture; z i (k i ) is the optimal pose estimate from the visual or laser sensor at time k; R i (k) is the covariance matrix from the visual or laser sensor at time k, and the subscript i represents the number of sensor data; When there is only camera data in the sampling interval, i=1, and equation (10) becomes: At this time, the estimation model is transformed into a Kalman filter that estimates the pose of a single sensor data at a faster sensor sampling frequency; When the camera data and the pose estimation results of the laser sensor exist in the sampling interval at the same time, equation (10) becomes equation (12) and (13): At this time, the estimation model becomes a Kalman filter algorithm that performs fusion estimation of the laser sensor and the visual sensor at a slower sensor sampling rate. The algorithm uses the optimal estimate at the previous moment as the prediction quantity, first fuses the pose estimated by sensor one at the current moment, then fuses the pose estimated by sensor two, and finally uses the sensor estimation values ​​after the two fusions as the final pose estimation result. Step 7) In order to effectively utilize the different characteristics of each sensor, the concept of confidence is introduced; The confidence w is used to represent the confidence of the pose estimation of the two sensors, and equation (10) is changed to: where w i Represents the confidence of the pose estimation of the i-th sensor; Step 8) The characteristics of the sensor are usually related to the robot motion process, so w i It should be related to the robot's motion state; when the robot moves in a straight line, the algorithm increases the confidence in vision; When the robot rotates, the algorithm increases the confidence in the laser; Each time fusion occurs, the algorithm determines whether the robot rotates during the current motion according to the latest pose estimation of the laser sensor; this is obtained by equation (6), which is denoted as Δα2 here; Substitute the angle change into w i get: Where Δα2 is the estimated robot rotation angle from the previous moment to the current moment; according to the current robot operation effect, the robot rotation angle does not exceed 0.25 rad in each sampling period, so formula (15) is modified as follows: Substituting formula (16) into formula (14), we obtain:

Images