A hybrid semantic slam method under dynamic environment

Abstract

This invention discloses a hybrid semantic SLAM method for dynamic environments, comprising the following steps: Step 1: In a vision-based module, semantic information in the dynamic environment is acquired by processing RGB and depth images, decoupling dynamic and static objects in the environment and generating static semantic point cloud information; Step 2: In a 2D radar-based module, geometric information of the robot's motion plane is generated based on radar and IMU information; Step 3: In a semantic fusion module, semantic and geometric information are fused to generate a local semantic octree and a grid map. By continuously updating the occupancy probabilities of voxels and grids, the global hybrid semantic map is constructed. This invention effectively constructs a hybrid semantic map in dynamic environments, which can be directly used for the control and navigation of mobile robots, and can also be used to improve the semantic-based reasoning, human-computer interaction, and intelligent decision-making capabilities of mobile robots.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a hybrid semantic SLAM method in dynamic environments. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology for autonomous navigation of mobile robots and self-driving cars in unknown environments, and it has been widely used in mobile robots to perceive their geometric environment. Since semantic information in the environment is crucial for mobile robots' reasoning, decision-making, human-robot collaboration, and the execution of advanced tasks, research is increasingly focusing on semantic SLAM methods, which are categorized into vision-based, radar-based, and multi-sensor fusion-based approaches.

[0003] Among these methods, vision-based approaches employ deep learning to extract semantic information from images, remove dynamic features from the environment, and then locate the object to create a more accurate geometric map. Some methods also utilize semantic information to construct semantic maps, but these maps lack information about the robot's movement plane and cannot be directly used for robot control and navigation.

[0004] Radar-based methods are divided into 2D and 3D radar-based methods. The former can only construct a geometric map of the environment and lacks semantic information. The latter can obtain a 3D semantic point cloud by extracting, segmenting, and clustering point clouds, thereby constructing a semantic point cloud map. However, this method is much less efficient than vision-based methods and cannot meet real-time requirements.

[0005] Multi-sensor fusion methods can effectively construct high-precision maps by utilizing sensor information such as vision, radar, and inertial measurement units (IMUs). However, these methods only focus on more effectively extracting and processing environmental geometric information, without fully utilizing semantic information in the environment to construct semantic maps, thereby failing to improve the robot's context-based reasoning and intelligent decision-making capabilities. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, this invention provides a hybrid semantic SLAM method for dynamic environments. The method includes: in a vision module, image frames are filtered using the ORB-SLAM3 framework to obtain keyframes; semantics are extracted from these keyframes to decouple dynamic and static objects, and local semantic point cloud information is generated; in a 2D radar module, 2D radar geometric information and IMU information are fused to obtain a more accurate robot pose; in a semantic fusion module, semantic and geometric information are fused to generate a semantic octree map and a grid map, thereby constructing a local map, and updating the map according to the probability of occupying a grid, ultimately generating a global hybrid semantic map. This invention significantly improves the efficiency and accuracy of semantic map construction for mobile robots, provides geometric information for the motion control of mobile robots, and provides crucial semantic information for semantic-based reasoning, human-computer interaction, and intelligent decision-making in mobile robots.

[0007] The technical solution adopted by this invention to solve the technical problem is: a hybrid semantic SLAM method in a dynamic environment, comprising the following steps:

[0008] Step 1: Obtain semantic information from the environment in the vision module to decouple dynamic and static objects in the environment, and generate static semantic point cloud information at the same time.

[0009] Step 2: Obtain the geometric information of the robot's movement plane in the 2D radar module;

[0010] Step 3: In the semantic fusion module, construct a global hybrid semantic map.

[0011] Furthermore, step 1 includes the following sub-steps:

[0012] Step 1.1: Calibrate the camera's mounting position on the robot and obtain the relative pose transformation matrix of the camera coordinate system relative to the robot coordinate system, denoted as T. CR ;

[0013] Step 1.2: In the tracking thread, each frame of RGB image and its corresponding depth image are acquired through the mobile robot platform. ORB feature points are extracted from the RGB image, and depth information is extracted from the corresponding depth image. Then, the pose of the camera is tracked based on semantic information.

[0014] Step 1.3: In the semantic acquisition thread, the acquired keyframe RGB image is subjected to semantic segmentation based on Yolact to generate the category, confidence and color label of the corresponding pixel, and obtain the mask of the dynamic object. Based on the dynamic mask, the corresponding feature points in step 1.2 are removed.

[0015] Step 1.4: In the local mapping thread, generate and optimize the local map;

[0016] Step 1.5: In the semantic generation thread, based on the semantic and depth information obtained in step 1.2, the depth information of the dynamic region is corrected to obtain the corrected depth image, and then semantic point cloud information is generated.

[0017] Furthermore, step 2 includes the following sub-steps:

[0018] Step 2.1: Calibrate the mounting position of the 2D radar on the robot and obtain the relative pose transformation matrix of the radar coordinate system relative to the robot coordinate system, denoted as T. LR ;

[0019] Step 2.2: The information from the 2D radar and IMU is fused using the extended Kalman filter method to achieve robot localization and obtain the geometric information of the robot's motion plane;

[0020] Step 2.3: Real-time tracking of radar pose is completed based on the geometric information of the motion plane.

[0021] Furthermore, step 3 includes the following sub-steps:

[0022] Step 3.1, based on the relative pose transformation matrix T between the camera coordinate system and the robot coordinate system. CR And the relative pose transformation matrix T of the 2D radar coordinate system relative to the robot coordinate system. LR The relative pose transformation matrix of the camera coordinate system relative to the radar coordinate system is obtained, denoted as...

[0023] Step 3.2: Integrate the semantic point cloud information and the geometric information of the robot's motion plane obtained in Step 1 and Step 2 to generate a local semantic octree and a grid map, respectively.

[0024] Step 3.3, based on the obtained relative pose transformation matrix T of the camera coordinate system relative to the radar coordinate system CL Align the semantic octree and the raster map;

[0025] Step 3.4: Based on the octree map and grid map generated in Step 3.3, update the corresponding voxels and grid occupancy probabilities in real time;

[0026] Step 3.5: Based on the updated octree map and raster map obtained in Step 3.4, generate a global hybrid semantic map.

[0027] Furthermore, the voxel update method in the semantic octree map in step 3.4 is as follows:

[0028] At times t = 1, 2, ..., T, the depth values ​​of the voxels are d = d1, d2, ..., d... TThen, at time T, the probability of voxel occupancy is P(n|d). 1：T )for:

[0029]

[0030] Furthermore, the voxel occupancy probability P(n|d) at time T in the semantic octree map 1：T If the value is greater than the threshold, the voxel is currently in an occupied state.

[0031] Furthermore, the method for updating the grid occupancy probability in the grid map in step 3.4 is as follows:

[0032] Let the state of the grid at the previous moment be s, and the current observation result of the lidar be Z ~ {0|1}. Then, the updated occupancy state S′ of a grid after occupancy state s is:

[0033]

[0034] Where s = 0 represents an idle state and s = 1 represents an occupied state.

[0035] Furthermore, when the value of the updated state S′ of a grid occupancy state s is greater than a set threshold, the grid is in an occupied state; if it is less than the threshold, the grid is considered to be in an idle state.

[0036] Invention Effects

[0037] The beneficial effects of this invention are as follows:

[0038] This invention systematically combines SLAM methods based on vision, radar, and multi-sensor fusion. It overcomes the shortcomings of vision-based methods in constructing semantic maps in dynamic environments, which make it difficult to represent the geometric information of the robot's movement plane; it overcomes the defects of maps constructed based on 2D radar methods, which do not contain semantic information and cannot be used by robots to perform advanced tasks; it overcomes the time-consuming and inefficient characteristics of point cloud segmentation required for constructing semantic maps based on 3D radar methods; and it overcomes the limitations of lacking semantic information in maps constructed based on multi-sensor fusion methods.

[0039] This invention effectively constructs a hybrid semantic map in a dynamic environment, which integrates a semantic map constructed based on a vision method and a robot movement plane map constructed based on a 2D radar method. This hybrid semantic map includes both semantic information in the environment and geometric information of the robot's movement plane, and can be directly used for the control and navigation of mobile robots. At the same time, this map can be used to improve the mobile robot's semantic-based reasoning, human-computer interaction, and intelligent decision-making capabilities. Attached Figure Description

[0040] Figure 1This is a framework diagram of the hybrid semantic SLAM method of the present invention.

[0041] Figure 2 This is a schematic diagram illustrating the operation results of the vision-based module in this invention.

[0042] Figure 3 This is a schematic diagram of the depth image correction results of the present invention.

[0043] Figure 4 This is a schematic diagram of the hybrid semantic map constructed by the hybrid semantic SLAM method of this invention. Detailed Implementation

[0044] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0045] This invention provides a hybrid semantic SLAM method for dynamic environments. The method employs three modules: a vision module, a 2D radar module, and a semantic fusion module. The method specifically includes the following steps:

[0046] Step 1: In the vision module, semantic information in the dynamic environment is obtained by processing RGB and depth images, thereby decoupling dynamic and static objects in the environment and generating static semantic point cloud information.

[0047] Step 2: In the 2D radar module, the geometric information of the robot's motion plane is generated by fusing the 2D radar geometric information and the IMU information.

[0048] Step 3: In the semantic fusion module, semantic and geometric information are fused to generate a local semantic octree and a grid map. By continuously updating the occupancy probability of voxels and grids, the global hybrid semantic map is constructed.

[0049] Since information about the robot's motion plane is difficult to accurately detect using vision-based methods in dynamic environments, this invention supplements it with geometric information provided by a 2D radar module. Furthermore, since the 2D radar module does not contain semantic information about the environment, this invention acquires it through a vision module. The method of this invention effectively fuses semantic and geometric information in a semantic fusion module, overcoming the limitations of current mobile robot semantic and geometric map construction in dynamic environments.

[0050] Reference Figures 1-4 The present invention discloses a hybrid semantic SLAM method for dynamic environments, comprising the following steps:

[0051] Step 1: Obtain semantic information from the environment in the vision module to decouple dynamic and static objects in the environment, and generate static semantic point cloud information at the same time.

[0052] In the vision module, image frames are filtered by the ORB-SLAM3 framework to obtain key frames. By extracting semantics from the key frames, dynamic and static objects are decoupled, and local semantic point cloud information is generated.

[0053] Specifically, it includes:

[0054] Step 1.1: Calibrate the camera's mounting position on the robot and obtain the relative pose transformation matrix of the camera coordinate system relative to the robot coordinate system, denoted as T. CR ;

[0055] Step 1.2: In the tracking thread, each frame of RGB image and corresponding depth image is acquired through the mobile robot platform. ORB (Oriented FAST and Rotated BRIEF) feature points are extracted from the RGB image and the depth information of each frame of RGB image is obtained from the depth image. Then, the pose of the camera is tracked based on semantic information.

[0056] Step 1.3: In the semantic acquisition thread, Yolact-based semantic segmentation is applied to the acquired keyframe RGB images to generate the corresponding pixel category, confidence score, and semantic label, obtaining a mask for dynamic objects. Based on this dynamic mask, dynamic feature points located within the dynamic mask from Step 1.2 are removed. An implementation example is attached. Figure 2 As shown;

[0057] Step 1.4: In the local mapping thread, generate and optimize the local map;

[0058] Step 1.5: In the semantic generation thread, based on the semantic and depth information obtained in Step 1.2, the depth information of the dynamic region is corrected to obtain a corrected depth image, which in turn generates semantic point cloud information; the depth information is the relative pose transformation matrix in Step 1.1. A schematic diagram of the corrected result is attached. Figure 3 As shown;

[0059] Step 2: In the 2D radar module, obtain the geometric information of the robot's motion plane, specifically including:

[0060] Step 2.1: Calibrate the mounting position of the 2D radar on the robot and obtain the relative pose transformation matrix of the radar coordinate system relative to the robot coordinate system, denoted as T. LR ;

[0061] Step 2.2: The 2D radar geometric information and IMU information are fused using the extended Kalman filter method to achieve robot localization and obtain the geometric information of the robot's motion plane;

[0062] Step 2.3: Real-time tracking of radar pose is completed based on the geometric information of the robot's motion plane.

[0063] Step 3: In the semantic fusion module, build and update the global hybrid semantic map, specifically including:

[0064] Step 3.1: Based on the relative pose transformation matrix T between the camera coordinate system and the robot coordinate system CR And the relative pose transformation matrix T of the 2D radar coordinate system relative to the robot coordinate system. LR The relative pose transformation matrix T between the camera coordinate system and the radar coordinate system is obtained. CL , recorded as

[0065]

[0066] Step 3.2: Integrate the semantic point cloud information obtained in Step 1 and Step 2 with the geometric information of the robot's motion plane to generate a local semantic octree map and a grid map, respectively.

[0067] Step 3.3: Based on the obtained relative pose transformation matrix T of the camera coordinate system relative to the radar coordinate system CL Any spatial point P(x, y, z) in the camera coordinate system can be represented by the formula

[0068] P′(x′,y′,z′,1)=P(x,y,z,1)*T CL (2)

[0069] Transform to the radar coordinate system, and denote the transformed result as P′(x′, y′, z′, 1). Align the semantic octree map and the raster map.

[0070] Step 3.4: Based on the aligned octree map and grid map processed in Step 3.3, update the voxel and grid occupancy probabilities in the map in real time, as follows:

[0071] The voxel update method in a semantic octree map is as follows:

[0072] At times t = 1, 2, ..., T, the depth values ​​of the voxels are d = d1, d2, ..., d... T Then the probability of occupying the position at time T is P(n|d). 1：T ) can be obtained through formula

[0073]

[0074] The calculation threshold for the occupancy probability is set to 0.85. That is, when the calculated probability is greater than or equal to 0.85, the voxel is considered to be in an occupied state at the current moment. In the semantic octree map, the voxel occupancy probability is expressed by rendering the voxel's transparency.

[0075] The occupancy probability update method for a grid map is as follows: Based on the grid state s from the previous moment and the current lidar observation result Z ~ {0|1}, the updated state S′ of a grid after occupancy s can be calculated using the formula:

[0076]

[0077] Where s = 0 represents an idle state and s = 1 represents an occupied state;

[0078] In this method, when the value of S′ is greater than the set threshold of 0.85, the grid is considered to be occupied and is represented as a black impassable grid on the map; if it is less than the threshold of 0.85, it is considered to be idle and is represented as a white passable grid on the map.

[0079] Step 3.5: Based on the updated octree map and raster map obtained in Step 3.4, generate a global hybrid semantic map. (See attached image) Figure 4 As shown.

[0080] This invention effectively constructs a hybrid semantic map in a dynamic environment, which integrates a semantic map constructed based on a vision method and a robot movement plane map constructed based on a 2D radar method. This hybrid semantic map includes both semantic information in the environment and geometric information of the robot's movement plane, and can be directly used for the control and navigation of mobile robots. At the same time, this map can be used to improve the mobile robot's semantic-based reasoning, human-computer interaction and intelligent decision-making capabilities.

Claims

1. A hybrid semantic SLAM method for dynamic environments, characterized in that, Specifically, the following steps are included: Step 1: In the vision module, semantic information in the dynamic environment is obtained by processing RGB and depth images, thereby decoupling dynamic and static objects in the environment and generating static semantic point cloud information. Step 2: In the 2D radar module, the geometric information of the robot's motion plane is generated by fusing the 2D radar geometric information and IMU information. Step 3: In the semantic fusion module, the static semantic point cloud information generated in Step 1 and the geometric information of the robot's motion plane obtained in Step 2 are fused to generate a local semantic octree and a grid map. The occupancy probabilities of voxels and grids are continuously updated to complete the construction of the global hybrid semantic map. This includes the following sub-steps: Step 3.1: Based on the relative pose transformation matrix of the camera coordinate system relative to the robot coordinate system And the relative pose transformation matrix of the 2D radar coordinate system relative to the robot coordinate system. The relative pose transformation matrix of the camera coordinate system relative to the radar coordinate system is obtained. , = ; Step 3.2: Integrate the static semantic point cloud information generated in Step 1 and the geometric information of the robot motion plane obtained in Step 2 to generate a local semantic octree map and a grid map respectively. Step 3.3: Relative pose transformation matrix of the camera coordinate system relative to the radar coordinate system obtained in Step 3.

1. Align the semantic octree and the raster map; Step 3.4: Based on the octree map and raster map generated in Step 3.3, update the voxel occupancy probability in the local semantic octree map and the raster occupancy probability in the raster map in real time. The specific voxel update method in the semantic octree map is as follows: exist At time 1, the depth values ​​of the voxels are respectively Then in Time voxel occupancy probability for: ； The semantic octree map in Time voxel occupancy probability If the value is greater than the threshold, the voxel is currently in an occupied state. The specific method for updating the grid occupancy probability in a grid map is as follows: Record the state of the grid at the previous time step as The current lidar observation results are as follows: Then a grid cell occupies a state. Updated Occupied Status for: in Indicates an idle state. Indicates an occupied state; When the grid is occupied Updated status When the value is greater than the set threshold, the grid is in an occupied state; if it is less than the threshold, the grid is considered to be in an idle state. Step 3.5: Based on the updated octree map and raster map obtained in Step 3.4, generate a global hybrid semantic map.

2. The hybrid semantic SLAM method in a dynamic environment as described in claim 1, characterized in that, Step 1 specifically includes the following sub-steps: Step 1.1: Calibrate the camera's mounting position on the robot to obtain the relative pose transformation matrix of the camera coordinate system relative to the robot coordinate system, denoted as... ; Step 1.2: In the tracking thread, each frame of RGB image and corresponding depth image is acquired through the mobile robot platform. Oriented FAST and Rotated BRIEF feature points are extracted from the RGB image and the depth information of each frame of RGB image is obtained from the depth image. Then, the pose of the camera is tracked based on semantic information. Step 1.3: In the semantic acquisition thread, the acquired keyframe RGB image is subjected to semantic segmentation based on Yolact to generate the category, confidence and semantic label of the corresponding pixel, and obtain the mask of the dynamic object. Based on the dynamic mask, the dynamic feature points located in the dynamic mask in step 1.2 are removed. Step 1.4: In the local mapping thread, generate and optimize the local map; Step 1.5: In the semantic generation thread, based on the semantic and depth information obtained in step 1.2, the depth information of the dynamic region is corrected to obtain the corrected depth image and generate semantic point cloud information.

3. The hybrid semantic SLAM method in a dynamic environment as described in claim 1, characterized in that, Step 2 specifically includes the following sub-steps: Step 2.1: Calibrate the mounting position of the 2D radar on the robot, and obtain the relative pose transformation matrix of the radar coordinate system relative to the robot coordinate system, denoted as... ; Step 2.2: The 2D radar geometric information and IMU information are fused using the extended Kalman filter method to achieve robot localization and obtain the geometric information of the robot's motion plane; Step 2.3: Real-time tracking of radar pose is completed based on the geometric information of the robot's motion plane.

Images