A multi-source tightly coupled SLAM method based on global information in dynamic scenes

Abstract

The present invention discloses a multi-source tightly coupled SLAM method based on global information in dynamic scenes, which belongs to the field of mobile robots. The present invention realizes efficient positioning and mapping through multi-source sensor data fusion and algorithm optimization. First, combining binocular camera, single-line laser radar, IMU and odometer data, using point cloud filtering, dynamic target detection and motion distortion correction technology, eliminates invalid point cloud and data distortion, and fuses camera and laser radar observation data; secondly, the IMU and odometer data are fused through unscented Kalman filtering (UKF) to output high-precision initial pose estimation; finally, the Gmapping algorithm is improved, and the grid map update module is optimized by using subgraph fusion and breadth-first search strategy to enhance the environmental feature retention capability of the SLAM module. In dynamic and complex scenes, this method has better robustness, and the constructed map is more in line with the real environment, while effectively reducing the pseudo obstacle features in the map.

Description

Technical Field

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

[0002] SLAM (Simultaneous Localization and Mapping) technology, a key research area in robotics, has driven advancements in mobile robot navigation, autonomous driving, and other technologies. Traditional SLAM technologies, such as laser SLAM and visual SLAM, typically use a single sensor to collect environmental data. While they offer good localization and mapping performance in specific environments, they are susceptible to degradation in complex and dynamic environments. Multi-sensor fusion SLAM systems offer even better performance. For example, binocular cameras provide rich 3D point cloud data, offering a wider vertical field of view and enabling target recognition and localization using image information. Single-line lidar offers a wider horizontal range and high-precision distance information. Inertial measurement units (IMUs) and odometry are independent of the external environment and can provide a measure of the robot's motion state through time pre-integration. Fusion of data from different sensors allows for more accurate robot state and environmental information, eliminating interference from moving obstacles and achieving more precise localization and mapping. The SLAM method using a single-line laser radar cannot correctly present some obstacle information in the environment in the map. Secondly, it is often unable to accurately identify mobile obstacles in the environment, such as pedestrians, other mobile robots, etc., which leads to the appearance of pseudo-obstacle features in the constructed map. The implementation cost of the SLAM system using a multi-line laser radar is relatively high, and the effect of identifying mobile obstacles only through the multi-line laser radar 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 effect of the SLAM system. In order to overcome the above shortcomings, the present invention realizes the robot to build a more accurate environment map in complex dynamic scenes through the application of multi-sensor fusion, target detection algorithm and improvement of SLAM algorithm. Summary of the Invention

[0003] In response to 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 dynamic scenes, which has a reasonable design, overcomes the shortcomings 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 clouds and data distortion, and fuse the observation data of the binocular camera and lidar; Step 2: pose estimation; unifying the IMU data into the odometer coordinate system through spatiotemporal alignment, and then using unscented Kalman filtering to fuse the odometer and IMU, and outputting initial pose estimation data; Step 3: SLAM mapping; improving the Gmapping algorithm, and using subgraph fusion and breadth-first search strategy to optimize the grid map update module.

[0005] Preferably, step 1 specifically includes the following steps: Step 1.1: filtering out invalid point clouds from the 3D point cloud output by the binocular camera, including invalid point clouds from the ground, walls and dynamic obstacles, and generating a 2D point cloud through dynamic projection; Step 1.2: performing motion distortion correction on the 2D point cloud of the lidar; calculating the translation and rotation offsets of each point in the point cloud based on the odometer and IMU data, and generating a corrected lidar point cloud; Step 1.3: performing spatiotemporal registration on the 2D point cloud of the binocular camera and the 2D point cloud of the lidar, and converting 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 the particle filter; Step 3.2: use subgraph 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: generating a sub-grid map for each frame of fused laser data, and updating the sub-map according to the occupancy probability rule; step 3.2.2: fusing each sub-map with the global map, and adopting the weighted probability superposition rule for the same grid; step 3.2.3: using breadth-first search to fill the closed free area in the global map with an unknown state, and eliminate the pseudo obstacle features.

[0008] Preferably, in step 1.1, filtering out invalid point clouds includes 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 through-pass 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 located: ; Step 1.1.3: Calculate point p in sequence i (x i ,y i ,z i ) to the plane distance d i : ; d i Less than the set threshold d max The points are regarded as inliers in the plane. 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 inlier annotations are cancelled 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 3D point cloud information of obstacles 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 in combination with 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.

[0009] Preferably, step 1.1.5 specifically includes the following steps: step 1.1.5.1: collecting image data sets and training models for specific moving targets; collecting image data sets for specific moving targets in advance, and then training the model in a configured YOLO environment, so as to identify specific moving targets through the YOLO target detection algorithm on the binocular camera; step 1.1.5.2: calculating the spatial distance Z of the dynamic target according to the binocular parallax ranging principle; obtaining 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 left and right camera imaging planes relative to the center of the imaging plane.

[0010] According to the theory of similar triangles, 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 in the frame; First, convert the center pixel coordinates and pixel size of the target recognition frame into actual coordinates in the image coordinate system (x tru ,y tru ) and actual size size_x tru 、size_y tru ; ; Then calculate the four vertex 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 frame in the image coordinate system are p(x tru-i ,y tru-i ), the coordinate in the camera coordinate system is 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 set to the target space distance Z value provided by binocular parallax ranging. The coordinates x and y of the point cloud are within the target recognition frame in the camera coordinate system, and the Z value is less than Z c-i The point cloud of specific moving targets is deleted to eliminate the influence of dynamic objects in the environment on the map.

[0011] Preferably, in step 1.2, the distortion correction of the lidar point cloud is performed using the odometer and IMU data, including the following steps: Step 1.2.1: Based on the posture changes of the odometer and IMU during the lidar acquisition cycle, the translation offset of each laser point is calculated: Assume that the time when the lidar 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 acquisition range of the odometer and IMU should be t s-1 to t e+1 , to ensure that the translation and rotation within one acquisition cycle of the lidar can be fully calculated; assuming t s-1 The odometer displacement and rotation matrix at the moment is T start , t e+1 The displacement and rotation matrix at the moment is T end , then from t s-1 to t e+1The calculation formula of the wheel speed meter coordinate transformation matrix T is: ; Among them, R and trs are respectively s to t e The matrix corresponding to the rotation and displacement that occurs within a cycle; assuming that the corresponding moment of a laser point in an acquisition 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 within one acquisition cycle, respectively. Then the offsets of p relative to the first laser point in the X and Y directions, posX and posY, are as follows: ; Step 1.2.2: Calculate the rotation offset around the Z axis based on the IMU angular velocity integral; Assume that the angle of the lidar when the first acquisition point is emitted is θ s , the angle of rotation of the kth frame IMU data relative to the first frame 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 angles 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 relative to the first data point around the Z axis is: ; Step 1.2.3: Through the coordinate transformation matrix T cur Correct 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 p of point p corr for: ; Each time the laser radar completes data acquisition, it performs steps 1.2.1 and 1.2.2 on each point in the point cloud to determine the translation and rotation offset of each point relative to the first point. Then, after performing coordinate transformation according to the formula described in step 1.2.3, the laser radar point cloud after motion distortion correction is obtained.

[0012] Preferably, step 1.3 includes the following steps: Step 1.3.1: Obtain the extrinsic parameter matrices (R, T) of the binocular camera and the lidar through the Autoware calibration tool, and convert the binocular camera 2D point cloud to the lidar coordinate system; specifically as follows: Under the condition of known camera intrinsic parameters, use the calibration tool in Autoware to jointly calibrate the binocular camera and the lidar to obtain the extrinsic parameter matrices of the two coordinate systems, let R be the rotation matrix, T be the translation matrix, p c is the 2D point cloud coordinate of the binocular camera after processing in step 1.1, p cl is the coordinate of the binocular camera 2D point cloud in the laser radar coordinate system. The binocular camera 2D point cloud and the laser radar 2D point cloud are spatially registered according to the following formula to obtain the coordinate of the binocular camera 2D point cloud in the laser radar coordinate system. ; Step 1.3.2: Union and fuse the binocular camera 2D point cloud with the lidar 2D point cloud with timestamp alignment, and convert them into laser data format through open source tools; specifically: assuming that the lidar 2D point cloud set after distortion correction is A, and the binocular camera 2D point cloud set in the lidar coordinate system after spatial registration is B, fuse the timestamp-aligned A and B through a union operation to obtain the fused point cloud data; then convert the fused point cloud data into laser data format.

[0013] Preferably, step 2 includes the following steps: Step 2.1: Determine the position (x, y, yaw) output by the wheel speed meter, the linear velocity v in the x direction and the y direction x 、v y And the x-direction and y-direction acceleration a output by the IMU x 、a y and 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 state transfer function f(X 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: According to the state vector, state transfer function and measurement model, the fusion pose estimation is performed through UKF; specifically including the following steps: Step 2.3.1: First, the initial value of the system state and the covariance matrix P are initialized; Step 2.3.2: Sigma point sampling is performed according to the initial value of the state and the covariance matrix P; Step 2.3.3: According to the state transfer function, the sample points are nonlinearly transformed, and the Kalman gain is calculated in combination with the measurement model, and the state vector is updated according to the Kalman gain to obtain the fusion estimated pose.

[0014] Preferably, step 3 includes the following steps: step 3.1: initializing a sub-grid map for each frame of fused laser data, and defining the corresponding grid state as free, occupied, or unknown based on the occupancy probability; step 3.2: fusing the sub-grid map updated by each frame of fused laser data with the previous global grid map; step 3.3: traversing the free grid through a breadth-first search strategy, and filling the closed area grid with an unknown state.

[0015] The beneficial technical effects brought about by the present invention are as follows: the present invention enables the robot to build a more accurate environmental map in complex dynamic scenes through the application of multi-sensor fusion, target detection algorithm and improvement of SLAM algorithm; in dynamic and complex scenes, the method has better robustness, and the constructed map is more in line with the real environment, while it can effectively reduce the pseudo-obstacle features in the map; the method is suitable for fields such as mobile robot navigation and autonomous driving that require precise positioning and mapping in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is the system framework diagram; Figure 2 This is the principle diagram of binocular parallax ranging; Figure 3 Schematic diagram of the conversion relationship between camera coordinates and image coordinates; Figure 4 This is the effect diagram of the fusion of camera and lidar data; Figure 5 This is a schematic diagram of BFS processing on a raster map; Figure 6 Schematic diagram of the simulation environment; Figure 7 This is a schematic diagram of the simulation results of the traditional method; Figure 8 This is a schematic diagram of the simulation mapping results of the method of the present invention; Figure 9 This is a schematic diagram of the actual mapping effect of the traditional method; Figure 10 This is a schematic diagram of the actual mapping effect of the method described in the present invention. DETAILED DESCRIPTION

[0017] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments: 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 odometry and IMU data fusion. S1: First, dynamic obstacles and invalid point clouds such as the ground and walls in the 3D point cloud output by the binocular camera are eliminated, and then a 2D point cloud is obtained through dynamic projection; at the same time, motion distortion correction is performed on the lidar 2D point cloud to reduce the matching error between the lidar data and the real environment; then the camera 2D point cloud and the lidar 2D point cloud are fused through spatiotemporal registration, and the fused point cloud is converted into laser data to obtain fused laser observation data. S2: On the other hand, the IMU data is unified into the odometry coordinate system through spatiotemporal registration, and then the unscented Kalman filter (UKF) is used to fuse the odometry 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) Invalid point cloud filtering of the binocular camera 3D point cloud; including the following steps: (1) First, the 3D point cloud output by the binocular camera is downsampled by voxel filtering, and the point cloud above the laser radar scanning plane and the ground point cloud are filtered out by straight-through filtering to obtain a point cloud set P; then the point cloud set P = {p i ,i=1,2…,k}, calculate the equation of the plane L where the three points p1, p2, and p3 are located: ; Then calculate the point p in turn i (x i ,y i ,z i ) to the distance d from the plane L i : ; d i Less than the set threshold d max The points are regarded as inliers in the plane. 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 marked and random sampling and fitting are performed again until the iteration is completed. 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) According to the robot working environment, image datasets are collected in advance for specific mobile targets such as workers and other mobile devices, and then the model is trained in the configured YOLO environment so that the specific mobile targets can be identified by the YOLO target detection algorithm on the binocular camera. The image dataset required for model training can be obtained by downloading the public dataset for screening. If there is no image dataset corresponding to the mobile target in the public dataset, at least 500 to 1000 images of each type of target can be taken by yourself and annotated by the LabelImg software, and then a YOLO format annotation file is generated to obtain the dataset. According to the YOLO 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 yolo on the left and right cameras, the spatial distance Z is calculated. The principle is as follows Figure 2 As shown: where b is the baseline of the two cameras, f is the focal length, which is a fixed parameter and can be obtained through calibration. The imaging points of the moving target center point P on the left and right cameras are P respectively. L 、P R , L is twice the distance from the optical center of the left and right cameras to the imaging surface, x L 、x R Respectively represent the horizontal offset of the imaging position of point P on the left and right camera imaging planes relative to the center of the imaging plane. Then the two imaging points P L and P R The distance between them is: ; According to the theory of similar triangles, the spatial distance Z of P can be calculated: ; Parallax x L -x R Obtained through binocular image matching.

[0020] First, the center pixel coordinates and pixel size of the recognition frame are converted into actual coordinates in the image coordinate system (x tru ,y tru ,) and actual size size_x tru 、size_y tru ; ; Then calculate the real coordinates of the four vertices of the target recognition box in the image coordinate system: ; Calculate the coordinates of the target recognition frame vertices in the camera coordinate system. The relationship between the camera coordinate system and the image coordinate system is as follows: Figure 3 shown.

[0021] Assume that the four vertex coordinates of the target recognition frame in the image coordinate system are p(x tru-i ,y tru-i), the coordinate in the camera coordinate system is P(X c-i ,Y c-i ,Z c-i ), then according to Figure 3 Similar relationships: ; According to the similarity relationship, the four vertices p can be calculated 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 can be obtained by calculating the four vertex coordinates of the target recognition frame in the camera coordinate system based on the above formula for p1~p4. The point cloud midpoint coordinates x and y are within the target recognition frame in the camera coordinate system, and the z value is less than Z c-i The point deletion is performed to remove the specific moving target point cloud, so as to eliminate the influence of dynamic objects in the environment on the map construction.

[0022] (3) Project the point cloud processed in steps (1) and (2) above onto the horizontal coordinate plane of the camera coordinate system in real time to obtain the obstacle 2D point cloud.

[0023] 2) LiDAR point cloud distortion correction: The LiDAR point cloud is distorted and corrected using odometry and IMU data to reduce matching errors.

[0024] (1) 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 acquisition range of the odometer and IMU should be t s-1 to t e+1 , ensuring that the translation and rotation within one acquisition cycle of the lidar can be fully calculated. Assuming t s-1 The odometer displacement and rotation matrix at the moment is T start , t e+1 The displacement and rotation matrix at the moment is T end , then from t s-1 to t e+1 The calculation formula of the wheel speed meter coordinate transformation matrix T is: ; R and trs are respectively s to t e The matrix corresponding to the rotation and displacement that occurs within one cycle.

[0025] (2) Let the corresponding time of a laser point in a collection cycle be t p , the radar coordinate system at this time is f p , the laser point is at f pThe coordinate in the coordinate system is p, trs-x and trs-y are the total offsets in the X and Y directions within one acquisition cycle, respectively. Then the offsets of p relative to the first laser point in the X and Y directions, posX and posY, are as follows: ; (3) 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 IMU data relative to the first frame 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: ;(4)The calculation of the rotation offset only needs to consider the rotation offset around the Z axis, let θ front ,θ back t p The rotation angles 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 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 , the corrected point is p corr , based on the translation and rotation offset of each point relative to the first point, the coordinate system f of the current point p can be determined p To the coordinate system f of the first acquisition point s The coordinate transformation matrix T between cur , then the corrected coordinates of point p can be calculated corr for: ; After performing the above operations (2) to (5) on each point in the point cloud, the laser radar point cloud after motion distortion correction can be obtained.

[0026] 3) Spatiotemporal 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. Timestamp alignment indicates that the two sensors were in the same position when collecting data, with no spatial misalignment. Point clouds with aligned timestamps are fused.

[0027] (1) Under the condition of known camera intrinsic parameters, the binocular camera and lidar are calibrated together through the calibration tool in Autoware to obtain the external parameter matrix of the coordinate system of the two. Let R be the rotation matrix, T be the translation matrix, and p c is the camera 2D point cloud coordinate after preprocessing in step 1), p cl is the coordinate of the camera 2D point cloud in the LiDAR coordinate system, the camera 2D point cloud and the LiDAR 2D point cloud can be spatially registered according to the following formula to obtain the coordinate of the camera 2D point cloud in the LiDAR coordinate system: ; (2) Assuming that the 2D point cloud set of the laser radar after distortion correction is A, through the above steps The camera's 2D point cloud set in the spatially registered lidar coordinate system is B. The timestamp-aligned points A and B are fused using a union operation, i.e., A ∪ B, to obtain the fused point cloud data. The fused point cloud data is then converted to a laser data format using the open-source function package "pointcloud_to_laserscan," based on lidar parameters such as angular resolution and maximum range.

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

[0029] 4) Pose estimation using the fusion of the wheel speed meter and the IMU: The TF listener obtains the extrinsic matrix between the IMU, odometry, and the robot's coordinate system using the TF listener. The odometry pose is then transformed to the robot's coordinate system using a coordinate transformation based on the extrinsic matrix. The IMU data is calibrated to ensure that the direction of the IMU's measured acceleration and other data aligns with the robot's motion, achieving spatial unification of the odometry and IMU data. The odometry and IMU are then fused using an unscented Kalman filter (UKF) for pose estimation, and the fused estimated pose is used as the prior pose for SLAM.

[0030] (1) First determine the position (x, y, yaw) output by the wheel speed meter, the linear velocity v in the x and y directions x 、v y And the x-direction and y-direction acceleration a output by the IMU x 、a y and yaw angular velocity v yaw For the measurement term, the state vector is defined as: ; 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: ;h(u t ) is the sensor measurement function, where 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 ), μ, R kAll are determined by the sensor's own performance parameters.

[0031] (2) Based on the above state vector, state transfer function and measurement model, the fused pose estimation is performed through UKF. First, the initial state value and covariance matrix P of the system are initialized. Then, Sigma points are sampled according to the initial state value and covariance matrix P. Then, the sample points are nonlinearly transformed according to the state transfer function, and the Kalman gain is calculated in combination with the measurement model. The state vector is updated according to the Kalman gain to obtain the fused estimated pose.

[0032] 5) Design of the probabilistic grid map update module for the Gmapping algorithm. The SLAM component uses the Gmapping algorithm, using the fused data obtained from the two processes described in 3) and 4) as input. Gmapping is a particle-filter-based 2D lidar SLAM algorithm. It first initializes a set of particles carrying independent grid maps, each representing a robot pose hypothesis within the observation range of the fused laser data. Particle poses are then predicted by fusing the odometry with the pose estimation data output by the IMU. The fused laser observation data is then used to calculate the likelihood of matching the laser point cloud with the map for each particle pose to update the particle weights. Adaptive resampling is then performed based on the weights, eliminating low-weight particles and replicating high-weight particles. Finally, the grid map is updated based on the poses of the existing particles and the current laser data, using the map update method described in this invention. Because the camera's horizontal field of view is limited to 60°, only one-sixth of the range in the fused lidar data is covered by the combined camera and lidar scans. To better preserve environmental features, the probabilistic grid map update module of the Gmapping algorithm was improved based on the fused laser data observation model.

[0033] (1) The Gmapping algorithm is improved to initialize the grid map only for the first frame of laser data, so that each frame of fused laser data received initializes a corresponding sub-grid map. 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, it has not been penetrated or hit by the laser. After the improvement, the probability of the grid being occupied in the grid map corresponding to each frame of laser is only the following three cases: ; (2) The grid map updated by each frame of fused laser data is fused with the previous grid map. The fusion rules for the same index grid are shown in Table 1.

[0034] Table 1: Grid status and fusion rules

[0035] .

[0036] (3) Fill the closed area grid formed by the occupied grid in the map with an unknown state through the breadth-first search (BFS). After the map is built, the global map is processed. The principle is as follows Figure 5 shown.

[0037] Traverse the grids in the grid map that are in the "free" state in turn, and Figure 5 The steps a to d below correspond to Figure 5 a~d in.

[0038] a. During the traversal process, the grid index that is free and has not been visited is stored in queue q as the starting point, and then b~d are repeated if queue q is not empty; b. The grid indexes that are dequeued are dequeued from queue q in sequence, and the dequeued grid indexes are added to the index queue f of the closed area. At the same time, all adjacent grids of the grid (eight directions in total: up, down, left, right, and diagonal) are traversed to check whether they are legal grids (within the map range and the status is not unknown); c. If the adjacent grid has not been visited and is a free point, its index is added to queue q and marked as visited. If one of the adjacent points is illegal, all the free grid indexes in the area connected to the grid are dequeued, and the grid status is not modified; d. If the grid corresponding to the index in queue f is in the closed area of ​​the occupied grid, the indexes in queue f are dequeued in sequence, and the grid status corresponding to the index is filled with unknown.

[0039] The process ends when all grids in the grid map with a status of "free" have been visited.

[0040] This is the entire content of the present invention. The comparison of the mapping experimental results based on the above content is as follows: 6) Comparison of mapping effects: The mapping results of the method described in the present invention are compared with the mapping results of a single laser radar in the same simulation environment. Figure 6-8 shown.

[0041] Comparison of the mapping results of the method described in the present invention and the single laser radar mapping results under the same indoor environment Figure 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 technicians in this technical field within the essential scope of the present invention should also fall within the scope of protection of the present invention.

Claims

1. A multi-source tightly coupled SLAM method based on global information in dynamic scenes, characterized by: The following steps are involved: Step 1: Sensor data fusion processing; Combining binocular camera, lidar, IMU and odometry 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 odometry coordinate system through spatiotemporal registration, and then the odometry and IMU are fused using an unscented Kalman filter to output the initial pose estimation data. Step 3: SLAM mapping; Improve the Gmapping algorithm and use subgraph fusion and breadth-first search strategies to optimize the grid map update module; 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 such as the ground, walls, and dynamic obstacles, and generate a 2D point cloud through dynamic projection; Step 1.2: Perform motion distortion correction on the LiDAR 2D point cloud. Calculate the translation and rotation offsets of each point in the point cloud based on the odometry 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; 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 the particle filter; Step 3.2: Update the grid map using sub-map fusion and breadth-first search strategy to generate a global map with accurate environmental features; 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 submap with the global map, using a 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; 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 lidar 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 located: Ax + By + Cz + D = 0; 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 inliers in the plane. 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 marked and random sampling and fitting are performed again until the iteration is completed. 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 3D point cloud information of obstacles 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 onto the horizontal coordinate plane of the camera coordinate system in real time to obtain the obstacle 2D point cloud; 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 datasets for specific moving targets in advance, and then train the model in the configured YOLO environment so that specific moving targets can be identified using 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 in 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, L is twice the distance from the optical center of the binocular camera to the imaging surface, and x L 、x R 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 x L -x R Acquired through binocular image matching; Step 1.1.5.3: Convert the pixel coordinates of the target recognition box to 3D coordinates in the camera coordinate system and delete the point cloud within the box; First, according to the pixel resolution size_p, the center pixel coordinates and pixel size of the target recognition frame are converted into the 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 frame in the image coordinate system are p(x tru-i ,y tru-i ), the coordinate in the camera coordinate system is 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 the coordinates x and y less than Z. c-i Point deletion, remove specific moving target point cloud; In step 1.2, distortion correction is performed on the lidar point cloud using odometry and IMU data, which includes the following steps: Step 1.2.1: Calculate the translation offset of each laser point based on the changes in the odometry and IMU pose 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 acquisition range of the odometer and IMU should be t s-1 to t e+1 , to ensure that the translation and rotation within one acquisition cycle of the lidar can be fully calculated; assuming t s-1 The odometer displacement and rotation matrix at the moment is T start , t e+1 The displacement and rotation matrix at the moment is T end , then from t s-1 to t e+1 The calculation formula of the wheel speed meter coordinate transformation matrix T is: Among them, R and trs are respectively s to t e The matrix corresponding to the rotation and displacement that occurs within one cycle of ; 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 within one acquisition cycle, respectively. Then the offsets of p relative to the first laser point in the X and Y directions, posX and posY, are 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 IMU data relative to the first frame 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 angles 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 relative to the first data point around the Z axis 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 p of point p corr for: p corr =T s -1 ×T cur ×p; After each LiDAR data acquisition, perform steps 1.2.1 and 1.2.2 for each point in the point cloud to determine the translation and rotation offset of each point relative to the first point. Then, perform coordinate transformation according to the formula described in step 1.2.3 to obtain the LiDAR point cloud after motion distortion correction. Step 1.3 includes the following steps: Step 1.3.1: Use the Autoware calibration tool to obtain the extrinsic parameter matrices (R, T) of the binocular camera and lidar, and convert the binocular camera 2D point cloud to the lidar coordinate system; the details are as follows: Under the condition of known camera intrinsic parameters, the binocular camera and lidar are calibrated together through the calibration tool in Autoware to obtain the external parameter matrix of the coordinate system of the two. Let R be the rotation matrix, T be the translation matrix, and p c is the 2D point cloud coordinate of the binocular camera after processing in step 1.1, p cl is the coordinate of the binocular camera 2D point cloud in the laser radar coordinate system. The binocular camera 2D point cloud and the laser radar 2D point cloud are spatially registered according to the following formula to obtain the coordinate of the binocular camera 2D point cloud 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. The timestamp-aligned A and B are fused through a union operation to obtain the fused point cloud data; then the fused point cloud data is converted into laser data format.

2. The multi-source tightly coupled SLAM method based on global information in a dynamic scene according to claim 1, characterized in that Step 2 includes the following steps: Step 2.1: Determine the position (x, y, yaw) and the x- and y-direction linear velocities v 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 yaw angular velocity v yaw For the measurement term, the state vector is defined as: X=[x,y,yaw,v x ,v y ,v yaw ,a x ,a y ] T ; Step 2.2: Construct the state transfer function and measurement model; Define the state transfer function f(X t ,z t ): Among them, Δt 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: z=h(u t )+δ; 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 system state initial value and 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.

Images