Multi-source tight coupling SLAM method based on global information in dynamic scene

Abstract

The invention discloses a multi-source tight coupling SLAM method based on global information in a dynamic scene, and belongs to the field of mobile robots. According to the method, efficient positioning and mapping are realized through multi-source sensor data fusion and algorithm optimization. The method comprises the following steps of: firstly, eliminating invalid point cloud and data distortion by combining a binocular camera, a single-line laser radar, an IMU (Inertial Measurement Unit) and odometer data and utilizing point cloud filtering, dynamic target detection and motion distortion correction technologies, and fusing observation data of the camera and the laser radar; secondly, IMU and odometer data are fused through unscented Kalman filtering (UKF), and high-precision initial pose estimation is output; and finally, improving a Gmapping algorithm, optimizing a grid map updating module by adopting a subgraph fusion and breadth-first search strategy, and enhancing the environmental feature retention capability of the SLAM module. In a dynamic and complex scene, the method has better robustness, the constructed map better conforms to a real environment, and meanwhile, the pseudo obstacle characteristics in the map can be effectively reduced.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robots, and particularly relates to a multi-source tightly coupled SLAM method based on global information in a dynamic scenario. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) technology, as an important research direction in the field of robotics, has promoted the progress of technologies such as mobile robot navigation and autonomous driving through its research and development. Traditional SLAM technologies such as laser SLAM and visual SLAM usually collect environmental data with a single sensor and have good positioning and mapping effects in specific environments. However, in complex and dynamic environments, they are easily affected, while multi-sensor fusion SLAM systems have better effects. For example, a binocular camera can provide rich three-dimensional point cloud data, has a larger observation field of view in the vertical direction, and can use image information for specific target recognition and positioning; a single-line lidar has a larger sensing range in the horizontal direction and can obtain high-precision distance information; an IMU and an odometer do not depend on the external environment and can give the motion state of the robot itself through pre-integration of time. By fusing data from different sensors, more accurate robot state information and environmental information can be obtained, and the interference of moving obstacles in the environment can be eliminated to achieve more accurate positioning and mapping. For the SLAM method using a single-line lidar, some obstacle information in the environment cannot be correctly presented in the map. Secondly, it often cannot accurately identify moving obstacles in the environment, such as pedestrians and other mobile robots, resulting in pseudo-obstacle features in the constructed map. The implementation cost of the SLAM system using a multi-line lidar is relatively high, and the effect of identifying moving obstacles only through the multi-line lidar point cloud is poor. At the same time, the distortion correction of sensor data and the adaptability of the SALM algorithm to the multi-sensor fusion data observation model are also one of the key points affecting the positioning and mapping effects of the SLAM system. To overcome the above shortcomings, the present invention realizes the construction of a more accurate environmental map by the robot in a complex dynamic scenario through multi-sensor fusion, the application of target detection algorithms, and the improvement of the SLAM algorithm. Summary of the Invention

[0003] Aiming at the above technical problems existing in the prior art, the present invention proposes a multi-source tightly coupled SLAM method based on global information in a dynamic scenario, which is reasonably designed, overcomes the deficiencies of the prior art, and has good effects.

[0004] In order to achieve the above-mentioned purpose, the present invention adopts the following technical scheme: a multi-source tightly coupled SLAM method based on global information in a dynamic scene, comprising the following steps: Step 1: sensor data fusion processing; combining binocular camera, lidar, IMU and odometer data, using point cloud filtering, dynamic target detection and motion distortion correction technology to eliminate invalid point cloud and data distortion, and fuse the observation data of binocular camera and lidar; Step 2: pose estimation; unifying the IMU data into the odometer coordinate system through spatiotemporal registration, and then using unscented Kalman filtering to fuse the odometer and IMU, and outputting the initial pose estimation data; Step 3: SLAM mapping; improving the Gmapping algorithm, and optimizing the grid map update module by using subgraph fusion and breadth-first search strategy.

[0005] Preferably, step 1 specifically includes the following steps: Step 1.1: Filter out invalid point clouds from the 3D point cloud output by the binocular camera, including invalid point clouds including the ground, walls and dynamic obstacles, and generate a 2D point cloud through dynamic projection; Step 1.2: Correct the motion distortion of the 2D point cloud of the lidar; Calculate the translation and rotation offset of each point in the point cloud based on the odometer and IMU data, and generate a corrected lidar point cloud; Step 1.3: Perform spatiotemporal registration of the 2D point cloud of the binocular camera and the 2D point cloud of the lidar, and convert them into a laser data format after fusion to obtain fused laser observation data.

[0006] Preferably, step 3 specifically includes the following steps: step 3.1: input the fused laser observation data and the initial pose estimation data into the improved Gmapping algorithm, and determine the final pose through a particle filter; step 3.2: use sub-graph fusion and breadth-first search strategy to update the grid map to generate a global map with accurate environmental features.

[0007] Preferably, step 3.2 specifically includes the following steps: step 3.2.1: generate a sub-grid map for each frame of fused laser data, and update the sub-map according to the occupancy probability rule; step 3.2.2: fuse each sub-map with the global map, and use the weighted probability superposition rule for the same grid; step 3.2.3: use breadth-first search to fill the closed free area in the global map with an unknown state to eliminate pseudo-obstacle features.

[0008] Preferably, in step 1.1, filtering out invalid point clouds comprises the following steps: step 1.1.1: downsampling the 3D point cloud output by the binocular camera by voxel filtering, and filtering out the point cloud above the laser radar scanning plane and the ground point cloud by pass-through filtering to obtain a point cloud set P; step 1.1.2: randomly selecting a point cloud set P = {p i ,i=1,2…,k}, calculate the equation of the plane where the three points are: ; Step 1.1.3: Calculate the distance d from point p i (x i , y i , z i ) to the plane: i : ; Consider the points with d i less than the set threshold d max as in-plane points. If the number of in-plane points meets the preset minimum number of in-plane points, use the in-plane points to continue fitting; if the number of in-plane points is insufficient, cancel the original in-plane point annotation and re-perform random sampling fitting until the iteration ends. Delete the in-plane points from the point cloud set P to achieve invalid point cloud filtering; Step 1.1.4: Filter out discrete points in the remaining point cloud set through statistical filtering, and extract relatively independent obstacle 3D point cloud information from the 3D point cloud output by the binocular camera; Step 1.1.5: Identify dynamic obstacles based on the yolo object detection algorithm, calculate the target space distance in combination with binocular disparity ranging, and delete the point cloud within the target recognition frame; Step 1.1.6: Project the processed point cloud onto the horizontal coordinate plane of the camera coordinate system in real time, that is, obtain the obstacle 2D point cloud.

[0009] Preferably, in Step 1.1.5, it specifically includes the following steps: Step 1.1.5.1: Collect image data sets and train models for specific moving targets; Collect image data sets for specific moving targets in advance, and then train the model in the configured yolo environment to identify specific moving targets through the yolo object detection algorithm on the binocular camera; Step 1.1.5.2: Calculate the spatial distance Z of the dynamic target according to the binocular disparity ranging principle; Obtain the central pixel coordinates (x p , y p ) and pixel sizes size_x p , size_y p of the target recognition frame of the target in the image coordinate system according to the yolo object detection algorithm; Calculate the spatial distance Z according to the imaging disparity of the moving target recognized by the yolo object detection algorithm on the binocular camera: specifically as follows: The imaging points of the center point P of the moving target on the binocular camera are P L , P R respectively. The distance between the two imaging points P L and P R is: ; where b is the baseline of the binocular camera, f is the focal length, L is twice the distance from the optical center of the binocular camera to the imaging plane, , respectively represent the horizontal offsets of the imaging positions of point P on the imaging planes of the left and right cameras relative to the center of the imaging plane.

[0010] According to the similar triangle theory, calculate the spatial distance Z of P: ; Parallax Obtained through binocular image matching; Step 1.1.5.3: Convert the pixel coordinates of the target recognition frame into three-dimensional coordinates in the camera coordinate system and delete the point cloud within the frame; First, convert the center pixel coordinates and pixel size of the target recognition frame into actual coordinates (x tru , y tru ) and actual size size_x tru , size_y tru in the image coordinate system according to the pixel resolution size_p; ; Then calculate the true coordinates of the four vertices p i of the target recognition frame in the image coordinate system: ; Assume that the coordinates of the target recognition frame in the image coordinate system are p(x tru-i , y tru-i ), and the coordinates in the camera coordinate system are P(X c-i , Y c-i , Z c-i ); According to the similarity relationship, calculate the coordinates (X i , Y c-i , Z c-i , Z c-i ) of the four vertices p of the target recognition frame in the camera coordinate system: c-i ; where f is the focal length, Z c-i is the target space distance Z value provided by binocular parallax ranging. Delete the points with x and y coordinates of the point cloud within the target recognition frame in the camera coordinate system and Z value less than Z

[0011] Preferably, in Step 1.2, the distortion correction of the lidar point cloud is performed through odometer and IMU data, including the following steps: Step 1.2.1: Calculate the translational offset of each laser point based on the pose changes of the odometer and IMU during the lidar acquisition period; Assume that the time when the lidar emits the first laser point is t s , the lidar coordinate system is f s , and the time when the last laser point is emitted is t e , then the data acquisition range of the odometer and IMU should be from t s-1 to t e+1 to ensure that the translation and rotation within one acquisition period of the lidar can be fully calculated; Assume that the odometer displacement and rotation matrix at time t s-1 is T start , and the displacement and rotation matrix at time t e+1 is T end , then from t s-1 to t e+1The calculation formula for the wheel speedometer coordinate transformation matrix T is as follows: ; where R and trs are the matrices corresponding to the rotation and displacement occurring within a period from t s to t e respectively; assuming that the corresponding time of a certain laser point within a collection period is t p , the radar coordinate system at this time is f p , the coordinate of the laser point in the f p coordinate system is p, trs-x and trs-y are the total offsets in the X direction and Y direction within a collection period respectively, then the offsets posX and posY of p relative to the first laser point in the X and Y directions are as follows: ; Step 1.2.2: Calculate the rotational offset around the Z axis according to the integration of the IMU angular velocity; assume that the angle of the lidar at the time of emitting the first collection point is θ s , the angle value of the k-th frame of IMU data relative to the first frame of IMU data at the corresponding time within the collection period is θ k , ω(τ) is the angular velocity at time t, then the calculation formula for θ k is: ; Let θ front , θ back be the rotational angles of the IMU data at the previous moment and the next moment relative to the first frame of IMU data at t p respectively, then the rotational offset rotZ of p relative to the first data point around the Z axis is: ; Step 1.2.3: Correct each laser point through the coordinate transformation matrix T cur ; assume that the transformation matrix of the first collection point is T s , the corrected point is p corr , determine the coordinate transformation matrix T p between the coordinate system f s of the current point p and the coordinate system f cur of the first collection point according to the translation and rotational offsets of each point relative to the first point, and calculate the corrected coordinate p corr of point p as: ; Every time the lidar completes a data collection, perform the operations of Step 1.2.1 and Step 1.2.2 on each point in the point cloud respectively to determine the translation and rotational offsets of each point relative to the first point, and then after coordinate transformation according to the formula described in Step 1.2.3, the lidar point cloud after motion distortion correction is obtained.

[0012] Preferably, in step 1.3, the following steps are included: Step 1.3.1: Obtain the extrinsic parameter matrix (R, T) of the binocular camera and the lidar through the Autoware calibration tool, and convert the 2D point cloud of the binocular camera to the lidar coordinate system; specifically as follows: Under the condition of known camera internal parameters, use the calibration tool in Autoware to jointly calibrate the binocular camera and the lidar to obtain the extrinsic parameter matrix of their coordinate systems. Let R be the rotation matrix and T be the translation matrix, and p c is the 2D point cloud coordinate of the binocular camera after being processed by step 1.1, and p cl is the coordinate of the 2D point cloud of the binocular camera in the lidar coordinate system. Perform spatial registration of the 2D point cloud of the binocular camera and the 2D point cloud of the lidar according to the following formula to obtain the coordinate of the 2D point cloud of the binocular camera in the lidar coordinate system; ; Step 1.3.2: Perform union fusion on the 2D point cloud of the binocular camera and the 2D point cloud of the lidar with aligned timestamps, and convert them into the lidar data format through an open-source tool; specifically as follows: Assume that the set of 2D point clouds of the lidar after distortion correction is A, and the set of 2D point clouds of the binocular camera in the lidar coordinate system after spatial registration is B. Perform fusion on A and B with aligned timestamps through the union operation to obtain the fused point cloud data; then convert the fused point cloud data into the lidar data format.

[0013] Preferably, in step 2, the following steps are included: Step 2.1: Determine the pose (x, y, yaw), the linear velocities v x in the x direction and v y in the y direction output by the wheel speedometer, and the accelerations a x in the x direction and a y in the y direction and the yaw angular velocity v yaw output by the IMU as measurement items, and define the state vector as: ; Step 2.2: Construct the state transition function and the measurement model; Define the state transition function f(X t , z t ) according to the state vector and the kinematic model: ; where, is the time interval between two consecutive data acquisitions, X t corresponds to the state vector, z t is the value of the measurement item in the state vector output by the sensor, and is obtained according to the measurement model z: ; where, h(u t ) is the sensor measurement function, u t is the direct measurement value of the sensor measurement item, δ is the sensor random error, satisfying the normal distribution with μ as the mean and the measurement noise variance matrix R k corresponding to the variance of the measurement item noise as the variance, that is, δ~N(μ, R k); Step 2.3: Based on the state vector, state transition function, and measurement model, perform fused pose estimation through UKF; specifically including the following steps: Step 2.3.1: First, initialize the initial value of the system state and the covariance matrix P; Step 2.3.2: Perform Sigma point sampling according to the initial state value and the covariance matrix P; Step 2.3.3: Perform a non-linear transformation on the sample points according to the state transition function, calculate the Kalman gain in combination with the measurement model, and update the state vector according to the Kalman gain to obtain the fused estimated pose.

[0014] Preferably, in Step 3, it includes the following steps: Step 3.1: Initialize a sub-grid map for each frame of fused laser data, and define the corresponding grid state as free, occupied, or unknown according to the occupancy probability; Step 3.2: Fuse the updated sub-grid map of each frame of fused laser data with the previous global grid map; Step 3.3: Traverse the free grids through the breadth-first search strategy and fill the grids in the closed area with the unknown state.

[0015] The beneficial technical effects brought by the present invention: Through multi-sensor fusion, the application of target detection algorithms, and the improvement of SLAM algorithms, the present invention enables the robot to construct a more accurate environmental map in complex dynamic scenarios; this method has better robustness in dynamic and complex scenarios, the constructed map is more in line with the real environment, and at the same time can effectively reduce the pseudo-obstacle features in the map; this method is applicable to fields such as mobile robot navigation and autonomous driving that require precise positioning and mapping in dynamic environments. Description of the Drawings

[0016] Figure 1 is the system framework diagram; Figure 2 is the schematic diagram of binocular parallax ranging principle; Figure 3 is the schematic diagram of the conversion relationship between camera coordinates and image coordinates; Figure 4 is the effect diagram of camera and lidar data fusion; Figure 5 is the schematic diagram of grid map BFS processing; Figure 6 is the schematic diagram of the simulation environment; Figure 7 is the schematic diagram of the simulation mapping result of the traditional method; Figure 8 is the schematic diagram of the simulation mapping result of the method described in the present invention; Figure 9 is the schematic diagram of the real mapping effect of the traditional method; Figure 10 is the schematic diagram of the real mapping effect of the method described in the present invention. Detailed Embodiments

[0017] The following further elaborates the present invention in detail in conjunction with the drawings and specific embodiments: The framework of this method is as Figure 1As shown in the figure, it mainly consists of two parts: sensor data fusion and SLAM. The sensor data fusion part includes two parts: binocular camera and lidar data fusion, and odometer and IMU data fusion. S1: First, eliminate dynamic obstacles and invalid point clouds such as ground and wall in the 3D point cloud output by the binocular camera, and then obtain 2D point cloud through dynamic projection; at the same time, perform motion distortion correction on the lidar 2D point cloud to reduce the matching error between the lidar data and the real environment; then fuse the camera 2D point cloud and the lidar 2D point cloud through spatiotemporal registration, and convert the fused point cloud into laser data to obtain fused laser observation data. S2: On the other hand, the IMU data is unified to the odometer coordinate system through spatiotemporal registration, and then the unscented Kalman filter (UKF) is used to fuse the odometer and IMU to output the initial pose estimation data. The SLAM part is implemented based on the Gmapping algorithm. The fused laser observation data and initial pose estimation data described in S1 and S2 are used as the input data of SLAM. The final pose is determined by the particle filter of the Gmapping algorithm. At the same time, the map generation module based on subgraph fusion and breadth-first search described in the present invention generates a raster map with more accurate environmental features according to the final pose and fused laser observation data.

[0018] 1) Filtering out invalid point clouds from the 3D point cloud of the stereo camera; including the following steps: (1) first downsample the 3D point cloud output by the stereo camera through voxel filtering, and filter out the point clouds above the laser radar scanning plane and the ground point clouds through pass filtering to obtain a point cloud set P; then randomly select the point cloud set P = {p i ,i=1,2…,k}, calculate the equation of the plane L where the three points p1, p2, p3 are located: ; Then calculate the point p in turn i (x i ,y i ,z i ) to the plane L i : ; d i Less than the set threshold d max The points are regarded as plane inliers. If the number of inliers meets the preset minimum number of inliers, the inliers are used to continue fitting. If the number of inliers is insufficient, the original inliers are annotated and random sampling and fitting are performed again until the iteration ends. The inliers are deleted from the point cloud set P to filter out invalid point clouds such as walls. Finally, the discrete points in the remaining point cloud set are filtered out by statistical filtering. At this point, relatively independent 3D point cloud information of obstacles can be extracted from the 3D point cloud output by the camera.

[0019] (2) Collect the image data set of specific moving targets such as workers and other moving devices in advance according to the robot's working environment, and then train the model in the configured yolo environment so as to identify specific moving targets through the yolo target detection algorithm on the binocular camera. The image data set required for model training can be obtained by screening and downloading the public data set. If there is no corresponding image data set of the moving target in the public data set, at least 500-1000 images can be taken for each type of target and annotated through the LabelImg software, and then the annotation file in yolo format can be generated to obtain the data set. According to the yolo algorithm, the central pixel coordinates (x p , y p ) and pixel size size_x p , size_y p of the target recognition box in the image coordinate system are obtained. According to the imaging parallax of the moving target recognized by yolo on the left and right cameras, calculate the spatial distance Z, and the principle is as Figure 2 shown: where b is the baseline of the two cameras, f is the focal length, which are fixed parameters and can be obtained through calibration. The imaging points of the center point P of the moving target on the left and right cameras are P L , P R respectively. L is twice the distance from the optical centers of the left and right cameras to the imaging plane. x L , x R respectively represent the horizontal offset of the imaging position of point P on the imaging planes of the left and right cameras relative to the center of the imaging plane. Then the distance between the two imaging points P L and P R is: ; According to the similarity triangle theory, the spatial distance Z of P can be calculated: ; The parallax x L - x R is obtained through binocular image matching.

[0020] First, convert the central pixel coordinates and pixel size of the recognition box into the actual coordinates (x tru , y tru , ) and actual size size_x tru , size_y tru in the image coordinate system according to the pixel resolution size_p; ; Then calculate the true coordinates of the four vertex points of the target recognition box in the image coordinate system respectively: ; Calculate the coordinates of the target recognition box vertex in the camera coordinate system, and the relationship between the camera coordinate system and the image coordinate system is as Figure 3 shown.

[0021] Assume that the four vertex coordinates of the target recognition box in the image coordinate system are p(x tru-i , y tru-i), and its coordinates in the camera coordinate system are P(X c-i , Y c-i , Z c-i ). Then, according to Figure 3 there is a similarity relationship: ; based on the similarity relationship, the coordinates of the four vertices p i in the camera coordinate system (X c-i , Y c-i , Z c-i ) can be calculated as follows: ; where f is the known focal length, and Z c-i is the target space distance Z value provided by binocular parallax ranging. After calculating p1 - p4 according to the above formula, the coordinates of the four vertices of the target recognition frame in the camera coordinate system can be obtained. Delete the points with coordinates x and y of the point cloud within the target recognition frame in the camera coordinate system and z value less than Z c-i to achieve the purpose of removing the point cloud of specific moving targets and eliminating the influence of dynamic objects in the environment on mapping.

[0022] (3) Project the point cloud after the above two steps (1) and (2) to the horizontal coordinate plane of the camera coordinate system in real time, and the obstacle 2D point cloud can be obtained.

[0023] 2) Distortion correction of lidar point cloud; correct the lidar point cloud through odometer and IMU data to reduce the matching error.

[0024] (1) Assume that the time when the lidar emits the first laser point is t s , the radar coordinate system is f s , and the time when the last laser point is emitted is t e . Then, the data acquisition range of the odometer and IMU should be from t s-1 to t e+1 to ensure that the translation and rotation within one acquisition cycle of the lidar can be calculated completely. Assume that the odometer displacement and rotation matrix at time t s-1 is T start , and the displacement and rotation matrix at time t e+1 is T end . Then, the calculation formula for the coordinate transformation matrix T of the wheel speedometer from t s-1 to t e+1 is: ; where R and trs are the matrices corresponding to the rotation and displacement that occur within one cycle from t s to t e .

[0025] (2) Let the time corresponding to a certain laser point within one acquisition cycle be t p , and the radar coordinate system at this time is f p , and the laser point is at f pThe coordinate in the coordinate system is p, and trs-x and trs-y are the total offsets in the X and Y directions within one acquisition period respectively. Then, the offsets posX and posY of p relative to the first laser point in the X and Y directions are as follows: ; (3) Assume that the angle of the lidar when the first acquisition point is emitted is θ s , and the angle value of the k-th frame of IMU data relative to the corresponding moment of the first frame of IMU data during the acquisition period is θ k , ω(τ) is the angular velocity at time t, then θ k is calculated by the following formula: ; (4) For the calculation of the rotational offset, only the rotational offset around the Z-axis needs to be considered. Let θ front , θ back be the rotational angles of the IMU data at the previous moment and the next moment respectively relative to the first frame of IMU data at time t p . Then, the rotational offset rotZ of p relative to the first data point around the Z-axis is: ; (5) Assume that the transformation matrix of the first acquisition point is T s , and the corrected point is p corr . According to the translational and rotational offsets of each point relative to the first point, the coordinate transformation matrix T p between the coordinate system f s of the current point p and the coordinate system f cur of the first acquisition point can be determined. Then, the corrected coordinate p corr of point p can be calculated as: ; After performing the above operations (2) to (5) on each point in the point cloud, the lidar point cloud after motion distortion correction can be obtained.

[0026] 3) Spatial and temporal registration of the binocular camera point cloud and the lidar point cloud; Using the system time as the timestamp for sensor sampling, each frame of the lidar 2D point cloud and the 2D point cloud of obstacles detected by the binocular camera has a corresponding timestamp. Aligning the timestamps indicates that the two sensors are at the same position when collecting data and there is no spatial misalignment. Then, fuse the point clouds with aligned timestamps.

[0027] (1) Under the condition of known camera internal parameters, jointly calibrate the binocular camera and the lidar through the calibration tool in Autoware to obtain the external parameter matrix of their coordinate systems. Let R be the rotation matrix and T be the translation matrix. p c is the coordinate of the camera 2D point cloud after preprocessing in step 1), and p cl is the coordinate of the camera 2D point cloud in the lidar coordinate system. Then, the spatial registration of the camera 2D point cloud and the lidar 2D point cloud can be performed according to the following formula to obtain the coordinate of the camera 2D point cloud in the lidar coordinate system: ; (2) Assume that the lidar 2D point cloud set after distortion correction is A. Through the above steps The camera 2D point cloud set in the lidar coordinate system after spatial registration is B. Align the timestamps of A and B and fuse them through the union operation, that is, A ∪ B to obtain the fused point cloud data. Then, convert the fused point cloud data into the lidar data format through the open-source functional package "pointcloud_to_laserscan" according to relevant parameters such as the lidar angular resolution and maximum ranging.

[0028] The camera 3D point cloud, lidar 2D point cloud, and fused point cloud effects in the same indoor environment are as Figure 4 shown.

[0029] 4) Pose estimation by fusing the wheel speed sensor and IMU; Use the TF listener to monitor the TF tree to obtain the extrinsic parameter matrix between the IMU, odometer, and the robot body coordinate system. Then, according to the extrinsic parameter matrix, transform the odometer pose to the robot body coordinate system through coordinate transformation, and calibrate the IMU data at the same time to ensure that the direction of the acceleration and other data measured by the IMU is consistent with the robot body movement direction, realizing the spatial unification of the odometer and IMU data. Then, fuse the odometer and IMU through the unscented Kalman filter (UKF) for pose estimation, and use the fused estimated pose as the prior pose of SLAM.

[0030] (1) First, determine the pose (x, y, yaw), linear velocities v x in the x and y directions, v y output by the wheel speed sensor, and the accelerations a x in the x and y directions and the yaw angular velocity v y output by the IMU as the measurement items, and define the state vector as: yaw ; Define the state transition function f(X , z t , z t ) according to the state vector and the kinematic model: ; where is the time interval between two consecutive data acquisitions, X t corresponds to the state vector, z t is the value of the measurement item in the state vector output by the sensor, and is obtained according to the measurement model z: ; h(u t ) is the sensor measurement function, where u t is the direct measurement value of the sensor measurement item, δ is the sensor random error, satisfying the normal distribution with μ as the mean and the measurement noise variance matrix R k corresponding to the measurement item noise variance as the variance, that is, δ ~ N(μ, R k ), μ, R kAll are determined by the performance parameters of the sensor itself.

[0031] (2) Perform fused pose estimation through UKF based on the above state vector, state transition function, and measurement model. First, initialize the initial value of the system state and the covariance matrix P. Secondly, perform Sigma point sampling according to the initial state value and the covariance matrix P. Then, perform a non-linear transformation on the sample points according to the state transition function, and calculate the Kalman gain in combination with the measurement model. Update the state vector according to the Kalman gain to obtain the fused estimated pose.

[0032] 5) Design of the probability grid map update module of the Gmapping algorithm; The SLAM part uses the Gmapping algorithm and takes the fused data obtained from the two processes described in 3) and 4) as input data. Gmapping is a SLAM algorithm for 2D lidar based on particle filter. First, initialize a group of particles carrying independent grid maps, and each particle represents a robot pose hypothesis within the observation range of the fused lidar data. Then, predict the particle pose by fusing the pose estimation data output by the odometer and the IMU. Subsequently, use the fused lidar observation data to calculate the matching likelihood value between the laser point cloud and the map at each particle pose to update the particle weights. Then, perform adaptive resampling according to the weights, eliminate low-weight particles and duplicate high-weight particles. Finally, update the grid map based on the poses of the existing particles and the current lidar data, and through the map update method described in the present invention. Since the horizontal viewing angle of the camera is only 60°, and only 1 / 6 of the range in the fused data with the lidar is the range jointly scanned by the camera and the lidar, in order to better preserve the environmental features, the probability grid map update module of the Gmapping algorithm is improved for the fused lidar data observation model.

[0033] (1) Improve the initialization of the grid map in the Gmapping algorithm from only the first frame of lidar data to initializing a corresponding sub-grid map for each frame of fused lidar data received. Assume that p(s = 0) represents the probability that the grid state is free, p(s = 1) represents the probability that the grid state is occupied, and S = -1 represents that the grid is unknown, that is, not penetrated or hit by the laser. Then, there are only the following three cases for the probability that the grid is occupied in the grid map corresponding to each frame of laser: ; (2) Fuse the grid map updated by each frame of fused lidar data with the previous grid map, and the fusion rules for grids with the same index are shown in Table 1.

[0034] Table 1: Grid State and Fusion Rules

[0035] .

[0036] (3) Fill the closed area grids formed by the occupied grids in the map with the unknown state by means of breadth-first search (BFS). After the mapping is completed, the global map is processed, and the principle is as Figure 5 shown.

[0037] Traverse the grids with the state of "free" in the grid map in turn, and process them according to the Figure 5 steps described below. The following steps a to d correspond to Figure 5 a to d in

[0038] a. During the traversal, the grid index that is free and not visited is stored as the starting point in the queue q, and then b to d are repeatedly executed while the queue q is not empty; b. Dequeue from the queue q in turn, and add the dequeued grid index to the index queue f of the closed area. At the same time, traverse all adjacent grids of this grid (a total of eight directions: up, down, left, right, and diagonal), and check whether they are legal grids (within the map range and the state is not unknown); c. If the adjacent grid is not visited and is a free point, add its index to the queue q and mark it as visited. If one of the adjacent points is illegal, all the free grid indexes in the connected area where this grid is located are dequeued, and the grid states are not modified; d. If the grid corresponding to the index in the queue f is in the closed area of the occupied grid, dequeue the indexes in the queue f in turn, and fill the state of the grid corresponding to the index with the unknown state.

[0039] Until all the grids with the state of "free" in the grid map are visited, the processing ends.

[0040] So far, the above is all the content of the present invention. The comparison of the mapping experiment results according to the above content is as follows: 6) Comparison of mapping effects; The mapping results of the method described in the present invention and the mapping results of a single lidar in the same simulation environment are compared as Figures 6 - 8 shown.

[0041] The mapping results of the method described in the present invention and the mapping results of a single lidar in the same indoor environment are compared as Figures 9 - 10 shown.

[0042] Of course, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by those skilled in the art within the essence of the present invention should also fall within the protection scope of the present invention.

Claims

1. A multi-source tightly coupled SLAM method based on global information in dynamic scenes, characterized in that: The following steps are involved: Step 1: Sensor data fusion processing; Combining binocular camera, lidar, IMU and odometer data, using point cloud filtering, dynamic target detection and motion distortion correction technology to eliminate invalid point clouds and data distortion, and fuse the observation data of binocular camera and lidar; Step 2: Pose estimation; The IMU data is unified into the odometer coordinate system through spatiotemporal registration, and then the odometer and IMU are fused using unscented Kalman filtering to output the initial pose estimation data; Step 3: SLAM map building; The Gmapping algorithm is improved, and the sub-graph fusion and breadth-first search strategy are used to optimize the grid map update module.

2. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 1, characterized in that: In step 1, the following steps are specifically included: Step 1.1: Filter out invalid point clouds from the 3D point cloud output by the binocular camera, including invalid point clouds on the ground, walls and dynamic obstacles, and generate 2D point clouds through dynamic projection; Step 1.2: Perform motion distortion correction on the 2D point cloud of the LiDAR; calculate the translation and rotation offset of each point in the point cloud based on the odometer and IMU data to generate a corrected LiDAR point cloud; Step 1.3: Perform spatiotemporal registration on the 2D point cloud of the binocular camera and the 2D point cloud of the lidar, and convert them into laser data format after fusion to obtain fused laser observation data.

3. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 2, characterized in that: In step 3, the following steps are specifically included: Step 3.1: Input the fused laser observation data and the initial pose estimation data into the improved Gmapping algorithm, and determine the final pose through the particle filter; Step 3.2: Use sub-map fusion and breadth-first search strategy to update the grid map and generate a global map with accurate environmental features.

4. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 3, characterized in that: Step 3.2 specifically includes the following steps: Step 3.2.1: Generate a sub-grid map for each frame of fused laser data and update the sub-map according to the occupancy probability rule; Step 3.2.2: Fuse each sub-map with the global map, and use the weighted probability overlay rule for the same grid; Step 3.2.3: Use breadth-first search to fill the closed free areas in the global map with unknown states to eliminate pseudo-obstacle features.

5. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 4, characterized in that: In step 1.1, invalid point cloud filtering includes the following steps: Step 1.1.1: Downsample the 3D point cloud output by the binocular camera through voxel filtering, and filter out the point cloud above the laser radar scanning plane and the ground point cloud through pass filtering to obtain the point cloud set P; Step 1.1.2: Randomly select a point cloud set P = {p i ,i=1,2…,k}, calculate the equation of the plane where the three points are: ; Step 1.1.3: Calculate the points p one by one i (x i ,y i ,z i ) to the plane i : ； D i Less than the set threshold d max The points are regarded as plane inliers. If the number of inliers meets the preset minimum number of inliers, the inliers are used to continue fitting. If the number of inliers is insufficient, the original inliers are annotated and random sampling and fitting are performed again until the iteration ends. The inliers are deleted from the point cloud set P to filter out invalid point clouds. Step 1.1.4: Filter out the discrete points in the remaining point cloud set through statistical filtering, and extract relatively independent obstacle 3D point cloud information from the 3D point cloud output by the binocular camera; Step 1.1.5: Identify dynamic obstacles based on the YOLO target detection algorithm, calculate the target space distance by combining binocular parallax ranging, and delete the point cloud within the target recognition box; Step 1.1.6: Project the processed point cloud to the horizontal coordinate plane of the camera coordinate system in real time to obtain the obstacle 2D point cloud.

6. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 5, characterized in that: Step 1.1.5 specifically includes the following steps: Step 1.1.5.1: Collect image datasets and train models for specific mobile targets; Collect image data sets for specific moving targets in advance, and then train the model in the configured YOLO environment so that specific moving targets can be identified through the YOLO target detection algorithm on the binocular camera; Step 1.1.5.2: Calculate the spatial distance Z of the dynamic target based on the binocular parallax ranging principle; According to the YOLO target detection algorithm, the center pixel coordinates (x p ,y p ) and pixel size size_x p 、size_y p ; According to the imaging parallax of the moving target identified by the yolo target detection algorithm on the binocular camera, the spatial distance Z is calculated as follows: The imaging points of the moving target center point P on the binocular camera are P L , P R , two imaging points P L and P R The distance between them is: ； Where b is the baseline of the binocular camera, f is the focal length, and L is twice the distance from the optical center of the binocular camera to the imaging surface. , Respectively represent the horizontal offset of the imaging position of point P on the imaging plane of the left and right cameras relative to the center of the imaging plane; According to the theory of similar triangles, calculate the spatial distance Z of P: ； Parallax Acquired through binocular image matching; Step 1.1.5.3: Convert the pixel coordinates of the target recognition box into three-dimensional coordinates in the camera coordinate system and delete the point cloud in the box; First, according to the pixel resolution size_p, the center pixel coordinates and pixel size of the target recognition box are converted into actual coordinates (x tru ,y tru ) and actual size size_x tru 、size_y tru ; ； Then calculate the four points p of the target recognition box respectively i The real coordinates in the image coordinate system: ； Assume that the coordinates of the target recognition box in the image coordinate system are p(x tru-i ,y tru-i ), the coordinates in the camera coordinate system are P(X c-i ,Y c-i ,Z c-i ); According to the similarity relationship, calculate the four vertices p of the target recognition box i The coordinates in the camera coordinate system (X c-i ,Y c-i ,Z c-i ): ； Where f is the focal length, Z c-i The target space distance Z value provided by binocular parallax ranging is placed in the target recognition frame range of the point cloud with coordinates x and y less than Z. c-i Delete the points and remove the specific moving target point cloud.

7. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 6, characterized in that: In step 1.2, the laser radar point cloud is distorted by using the odometer and IMU data, including the following steps: Step 1.2.1: Calculate the translation offset of each laser point based on the position changes of the odometer and IMU during the lidar acquisition cycle; Assume that the time when the laser radar emits the first laser point is t s , the radar coordinate system is f s The time when the last laser point is emitted is t e , then the data collection range of the odometer and IMU should be t s-1 to e+1 , ensuring that the translation and rotation of the laser radar within one acquisition cycle can be fully calculated; assuming t s-1 The odometer displacement and rotation matrix at time T start , t e+1 The displacement and rotation matrix at the moment is T end , then from t s-1 to e+1 The calculation formula of the wheel speed meter coordinate transformation matrix T is: ； Among them, R and trs are respectively s to e The matrix corresponding to the rotation and displacement that occurs within one cycle; Assume that the corresponding time of a laser point in a collection cycle is t p , the radar coordinate system at this time is f p , the laser point is at f p The coordinate in the coordinate system is p, trs-x and trs-y are the total offsets in the X and Y directions in one acquisition cycle, respectively. Then the offsets of p relative to the first laser point in the X and Y directions are posX and posY as follows: ； Step 1.2.2: Calculate the rotation offset around the Z axis based on the IMU angular velocity integration; Assume that the angle of the laser radar when the first acquisition point is emitted is θ s , the angle of rotation of the kth frame of IMU data relative to the first frame of IMU data at the corresponding moment during the acquisition period is θ k , ω(τ) is the angular velocity at time t, then θ k The calculation formula is: ； Let θ front ,θ back t p The rotation angle of the IMU data at the previous moment and the next moment relative to the first frame of IMU data, then the rotation offset rotZ of p around the Z axis relative to the first data point is: ； Step 1.2.3: Use the coordinate transformation matrix T cur Calibrate each laser point; Assume that the transformation matrix of the first acquisition point is T s , the corrected point is p corr , determine the coordinate system f of the current point p based on the translation and rotation offset of each point relative to the first point p To the coordinate system f of the first acquisition point s The coordinate transformation matrix T between cur , calculate the corrected coordinates of point p corr for: ； Each time the laser radar completes data collection, steps 1.2.1 and 1.2.2 are performed on each point in the point cloud to determine the translation and rotation offset of each point relative to the first point. Then, after coordinate transformation according to the formula described in step 1.2.3, the laser radar point cloud after motion distortion correction is obtained.

8. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 7, characterized in that: Step 1.3 includes the following steps: Step 1.3.1: Use the Autoware calibration tool to obtain the external parameter matrix (R, T) of the stereo camera and the lidar, and convert the 2D point cloud of the stereo camera to the lidar coordinate system; the details are as follows: Under the condition of known camera intrinsic parameters, the stereo camera and lidar are calibrated jointly through the calibration tool in Autoware to obtain the external parameter matrices of the coordinate systems of the two. Let R be the rotation matrix, T be the translation matrix, and p c is the 2D point cloud coordinates of the binocular camera after processing in step 1.1, p cl is the coordinate of the 2D point cloud of the binocular camera in the laser radar coordinate system. The spatial registration of the 2D point cloud of the binocular camera and the 2D point cloud of the laser radar is performed according to the following formula to obtain the coordinate of the 2D point cloud of the binocular camera in the laser radar coordinate system; ； Step 1.3.2: Union and fuse the timestamp-aligned binocular camera 2D point cloud with the lidar 2D point cloud, and convert them into laser data format using open source tools; the details are as follows: Assume that the 2D point cloud set of the laser radar after distortion correction is A, and the 2D point cloud set of the binocular camera in the laser radar coordinate system after spatial registration is B, and fuse A and B with aligned timestamps through a union operation to obtain the fused point cloud data; then convert the fused point cloud data into laser data format.

9. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 8, characterized in that: Step 2 includes the following steps: Step 2.1: Determine the position (x, y, yaw) and the linear velocity v in the x and y directions output by the wheel speed meter x 、v y And the x-direction and y-direction acceleration a output by the IMU x 、a y and the yaw angular velocity v yaw For the measurement term, the state vector is defined as: ; Step 2.2: Construct state transfer function and measurement model; Define the state transfer function f(X) according to the state vector and kinematic model t ,z t ): ； in, is the time interval between two consecutive data collections, X t The corresponding state vector, z t is the value of the measurement item in the state vector output by the sensor, obtained according to the measurement model z: ； Among them, h(u t ) is the sensor measurement function, u t is the direct measurement value of the sensor measurement item, δ is the random error of the sensor, μ is the mean, and the measurement noise variance matrix R k The corresponding measurement item noise variance is the normal distribution of variance, that is, δ~N(μ,R k ); Step 2.3: Based on the state vector, state transfer function and measurement model, perform fusion pose estimation through UKF; specifically, the following steps are included: Step 2.3.1: First, initialize the initial value of the system state and the covariance matrix P; Step 2.3.2: Perform Sigma point sampling based on the initial state value and covariance matrix P; Step 2.3.3: Perform nonlinear transformation on the sample points according to the state transfer function, calculate the Kalman gain based on the measurement model, and update the state vector according to the Kalman gain to obtain the fused estimated pose.

10. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 9, characterized in that: Step 3 includes the following steps: Step 3.1: Initialize a sub-grid map for each frame of fused laser data, and define the corresponding grid state as free, occupied or unknown according to the occupancy probability; Step 3.2: Fuse the sub-grid map updated by each frame of fused laser data with the previous global grid map; Step 3.3: Traverse the free grid using a breadth-first search strategy to fill the closed area grid with an unknown state.

Images