Method for constructing local semantic passable probability grid map of autonomous mobile robot

Abstract

The present invention relates to a method for constructing a local semantic passable probability grid map of an autonomous mobile robot, belonging to the field of simultaneous localization and mapping of mobile robots. The method includes: S1: constructing an occupancy grid map according to the measured values of laser data; S2: performing semantic segmentation using a real-time semantic segmentation model, extracting semantic labels, and performing 2D semantic segmentation; S3: after semantic segmentation, according to the coordinate relationship, projecting the semantic information using the depth map, that is, constructing a semantic grid map according to the semantic information; S4: fusing the semantic grid map probability with a multi-layer cost map: performing probability conversion on the semantic information through distance mapping to form a semantic layer cost map, and then fusing it with the obstacle layer cost map of the laser to obtain the final local semantic passable probability grid map. The present invention can solve the problem that the grid map cannot represent unstructured environments and realize the navigation of mobile robots in unstructured environments.

Description

Technical Field

[0001] The present invention belongs to the field of simultaneous localization and mapping of mobile robots, and relates to a method for constructing a local semantic traversable probability grid map for an autonomous mobile robot in an outdoor unstructured environment. Background Art

[0002] Nowadays, the rapid development of artificial intelligence has promoted the breakthrough and implementation of mobile robot technology. In particular, the research on the navigation methods and applications of mobile robots in outdoor environments has received much attention. Mobile robots are increasingly appearing in industrial parks, hospitals, campuses and other places. How to ensure that mobile robots have autonomous navigation capabilities in unknown unstructured environments is an important research issue.

[0003] J. Borenstein et al. developed a mobile robot. The user pushes the towing bar in front of the body and feels an obvious traction force through the handle as a steering command. In this way, it is possible to guide easily and safely without interaction information such as sound and vibration. Siagian C et al. from the California Institute of Technology developed a wheeled mobile robot that represents roads with a set of lines in the captured image and determines the direction of the road by determining its vanishing point to achieve autonomous navigation. In 2018, Tzu-Kuan Chuang et al. developed a mobile robot that can identify yellow and blue stripe trajectories and the "Free Trajectory" in Boston, USA. The system can train the deep CNN model in two ways. Among them, the "behavioral reflection" method directly maps the input image into three motion commands of "turn left", "go straight" and "turn right"; the "direct perception" method collects the lateral distance of the track from the center of the image and the heading of the robot, and guides the robot to follow the track through an improved filtering algorithm.

[0004] Current literature research focuses on the mapping and navigation problems of mobile robots in structured environments. For outdoor unstructured environments, the terrain structure is complex and there are many types of obstacles. There are still problems with the mapping and navigation of mobile robots. Problem 1: Traditional lasers cannot obtain the semantic information of the road in front of the mobile robot. For semantic information such as fruit peels, grasslands, and puddles, the mobile robot cannot avoid them and cannot accurately identify the traversability of the environment. Problem 2: The occupancy grid map constructed by lidar only has obstacle information on a single plane, lacks environmental information, and the traditional cost map lacks the judgment of the traversable probability of grid cells. Problem 3: The traditional single-layer cost map cannot be used for mobile robot navigation when it contains semantic information.

[0005] Therefore, there is an urgent need for a new method for constructing a local semantic traversable probability grid map for mobile robots to solve the above problems. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a method for constructing a local semantic traversable probability grid map for a mobile robot. By calculating the grid traversable probability and constructing a grid map with traversable probability evaluation, and then through multi-layer cost map fusion, the problem that the grid map cannot represent unstructured environments is solved, and navigation of the mobile robot in unstructured environments is realized.

[0007] To achieve the above object, the present invention provides the following technical solutions:

[0008] A method for constructing a local semantic traversable probability grid map for an autonomous mobile robot, specifically including the following steps:

[0009] S1: Construct an occupancy grid map according to the measurement values of laser data;

[0010] S2: Use a real-time semantic segmentation model to perform semantic segmentation, extract semantic labels, and perform 2D semantic segmentation;

[0011] S3: After semantic segmentation, project the semantic information using the depth map according to the coordinate relationship, that is, construct a semantic grid map according to the semantic information;

[0012] S4: Probability conversion of the semantic grid map and fusion with multi-layer cost maps: Perform probability conversion on the semantic information through distance mapping to form a semantic layer cost map, and then fuse it with the obstacle layer cost map of the laser to obtain the final local semantic traversable probability grid map.

[0013] Further, in step S1, constructing the occupancy grid map specifically includes the following steps:

[0014] S11: Set the resolution of the grid map, that is, an appropriate grid size;

[0015] S12: Update the occupancy probability of any grid map based on the pose state and observation values of the robot at the current moment and the existing grid map probability at the previous moment.

[0016] Further, in step S12, the occupancy probability update rule of the grid map is as follows:

[0017] (1) Obtain measurement values through sensors to determine that the corresponding grid is occupied \(m\) i \( = 1\) or is free \(m\) i \( = 0\);

[0018] (2) According to Bayes' formula, perform conversion and then take the logarithm to obtain the state update rule of the grid cell;

[0019] (3) Finally, obtain the posterior probability of any grid \(m\) i \(.

[0020] Further, in step S2, the semantic segmentation is specifically performed by a semantic segmentation model based on a convolutional neural network, which assigns a semantic label to each pixel in the image at the output layer. As a result, the passable area is segmented out for the robot to construct a semantic grid map.

[0021] Further, in step S2, the 2D semantic segmentation specifically includes the following steps:

[0022] S21: Input the sensor (camera) image;

[0023] S22: The first part is the spatial path, which mainly extracts the spatial information in the image; after passing through three convolutional layers, a feature map is output;

[0024] S23: The second part is the semantic path, which mainly extracts the context information, rapidly increases the receptive field and reduces the size of the feature map through multi-layer convolution, and outputs a feature map;

[0025] S24: The feature maps obtained from the previous two parts are fused through a feature fusion module, and the fused features are upsampled and then the final segmentation result is output, that is, other areas such as roads and grasslands are distinguished by different colors.

[0026] Further, in step S3, the semantic information is projected using the depth map, specifically including: obtaining an RGB image and a depth image through a binocular camera, the semantic segmentation image is obtained from the RGB image through a semantic segmentation model, the position information of the object or the passable area can be obtained, and the actual distance information is obtained by combining the depth map information with coordinate transformation. Finally, the semantic information is projected onto the local grid map.

[0027] Further, step S3 specifically includes the following steps:

[0028] S31: Obtain the position information of the object or the passable area in the pixel coordinates in the semantic segmentation image;

[0029] S32: Map the pixels in the RGB coordinate plane to the depth image pixel plane to obtain the depth value d of each pixel;

[0030] S33: Perform coordinate transformation through the camera internal parameter formula to convert the target from the pixel coordinate system to the camera reference system;

[0031] S34: Convert the target from the camera coordinate system to the robot coordinate system through rotation and translation;

[0032] S35: Add a layer of grid map, with the robot as the center, project the target position onto the grid map to form a semantic grid map.

[0033] Further, step S4 specifically includes the following steps:

[0034] S41: Perform probability conversion on the semantic grid map to obtain the traversability probability p of each grid cell t ;

[0035] S42: Multilayer cost map fusion: On the original local cost map, add a semantic layer cost map, and finally fuse the laser obstacle layer, dilation layer, and semantic layer, and publish them to the main cost map layer to finally obtain the local semantic grid map

[0036] Furthermore, step S41 specifically includes: Further process the semantic grid map to distinguish the traversable areas in the grid map; specifically, perform distance conversion on the grid cells marked as traversable in the semantic grid map, and map the distance from the traversable area grid cells to the nearest non-traversable cell to a real number distance value to represent the distance from each traversable grid cell to the non-traversable cell; after distance transformation, the central part of the traversable area will obtain a higher mapping value, on the contrary, the mapping values in the edge areas and areas close to obstacles are lower

[0037] After distance conversion, further transform the distance through Gaussian distribution, set the mean of the Gaussian distribution to the maximum value of the distance transformation, and the variance to the standard deviation of the squares of all distances; obtain the traversability probability p of each grid cell D ; Then preset the prior traversability probability p according to different semantic labels l Finally, obtain the traversability probability p of each grid cell t p t = p D p l .

[0038] Furthermore, step S42 specifically includes:

[0039] In the laser obstacle layer, the cost value of the grid cell of the fatal obstacle is set to 254, and a collision with the obstacle is inevitable within this cost value; the cost value of the free movement space is set to 0; the cost value range of the minimum non-free space is from 1 to 127, and the robot will not collide within this range; the cost value of the dilation layer is from 128 to 253, and the cost value of the unknown area is 255

[0040] The cost value of the semantic layer is set as follows:

[0041] C s = c i (2 - p t )

[0042] where c i is the traversal cost value preset by the semantic label, p tis the passable probability of the semantic grid cell; the preset pass cost value of the difficult-to-pass semantic label is higher than that of the easy-to-pass preset pass cost value, and finally the cost value of each grid cell in the semantic layer can be obtained;

[0043] Finally, it is fused into the main cost map in the order of the laser obstacle layer, the semantic layer, and the dilation layer. The fusion method uses the maximum value comparison method, and the formula is as follows:

[0044]

[0045] Among them, represents the cost value of the index-th grid cell in the main cost map, represents the cost value of the index-th grid cell in the current hierarchical cost map; when updating the map, the cost value of each grid cell in the current sub-layer is compared with the cost value at the corresponding position in the main cost map.

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

[0047] 1) On the basis of the original laser obstacle cost map, the present invention adds a semantic layer cost map, provides semantic information for the mobile robot, and can obtain the passability of the local environment and the cost value of the grid cell.

[0048] 2) When processing the semantic grid map, the present invention first distinguishes the passable area in front of the mobile robot, then obtains the grid cell distance value through distance transformation, and then converts it into a passable probability using Gaussian distribution, so as to obtain the passable probability of different points, which can better represent the environmental information in front of the mobile robot.

[0049] 3) When fusing multiple hierarchical cost maps, the present invention converts the probability value of the semantic grid map into the cost value of the corresponding semantic layer cost map according to the preset cost value of the semantic label, and finally uses the maximum value comparison method to fuse it into the main cost map in turn. It can be used for subsequent path planning of the mobile robot for semantic information, improving the intelligence of the mobile robot.

[0050] Other advantages, objectives and features of the present invention will be described to some extent in the subsequent description, and to some extent, will be obvious to those skilled in the art based on the study of the following text, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be described in detail preferably with reference to the accompanying drawings, where:

[0052] Figure 1Schematic diagram of occupancy grid map update;

[0053] Figure 2 Schematic diagram of semantic segmentation network;

[0054] Figure 3 Schematic diagram of the construction process of semantic grid map;

[0055] Figure 4 Schematic diagram of the overall framework of local semantic passable probability grid map. Detailed implementation manners

[0056] The following uses specific examples to illustrate the implementation manners of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation manners. Various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the diagrams provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner. Without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0057] Please refer to Figures 1 to 4 , the present invention provides a method for constructing a local semantic grid map of a mobile robot. As Figure 4 shown, it specifically includes the following steps:

[0058] S1: Construct an occupancy grid map according to the measurement values of laser data;

[0059] S2: Use a real-time semantic segmentation model to perform semantic segmentation, extract semantic labels, and perform 2D semantic segmentation;

[0060] S3: After semantic segmentation, project the semantic information using the depth map according to the coordinate relationship, that is, construct a semantic grid map according to the semantic information;

[0061] S4: After probability conversion of the semantic grid map, fuse it with a multi-layer cost map. Perform probability conversion on the semantic information to form a semantic layer cost map, and then fuse it with the obstacle layer cost map of the laser to obtain the final local semantic passable probability grid map. Among them, the local semantic grid map includes a laser obstacle cost map, a semantic layer cost map, and a dilation layer cost map.

[0062] As Figure 1 shown, the present invention provides an embodiment of a method for constructing a laser obstacle cost map, that is, the above step S1 specifically includes the following steps:

[0063] S11: Set the resolution of the grid map, that is, a suitable grid size.

[0064] S12: Obtain measurement values through a laser sensor, determine whether the corresponding grid is occupied or free, calculate the posterior probability of the grid cell according to the state update rule of the grid cell, and update the laser obstacle cost map.

[0065] The state update rule of grid cell i is as follows:

[0066]

[0067] In the formula, the states of grid i before and after obtaining the laser measurement values are respectively and

[0068] As Figure 2 shown, the present invention provides an embodiment of a semantic segmentation method, that is, the above-mentioned step S2 specifically includes the following steps:

[0069] S21: Input the sensor (camera) image.

[0070] S22: The first part is the spatial path, which mainly extracts the spatial information in the image. After passing through three convolutional layers, the feature map is output.

[0071] S23: The second part is the semantic path, which mainly extracts the context information, quickly increases the receptive field and reduces the size of the feature map through multi-layer convolution, and outputs the feature map.

[0072] S24: Fuse the feature maps obtained in the previous two parts through a feature fusion module, and output the final segmentation result after upsampling the fused features, that is, other areas such as roads and grasslands are distinguished by different colors.

[0073] As Figure 3 shown, the present invention provides an embodiment of a semantic information projection method, that is, the above-mentioned step S3 specifically includes the following steps:

[0074] S31: Obtain the position information of the object or passable area in the semantic segmentation image under the pixel coordinates.

[0075] S32: Map the pixels in the RGB coordinate plane to the depth image pixel plane to obtain the depth value d of each pixel.

[0076] S33: Through the camera internal parameter formula, perform coordinate transformation to convert the target from the pixel coordinate system to the camera reference system;

[0077] The coordinate transformation formula is as follows:

[0078]

[0079] In the formula, (u, v, 1) is the homogeneous coordinate of the target point in the pixel coordinate system, cx and c y are the number of horizontal and vertical pixels that differ between the central pixel coordinates of the image and the origin pixel coordinates of the image. f x and f y is the focal length; obtained through camera calibration. Z c is the depth information of this point, [X c Y c Z c T are the coordinates of the target point in the camera coordinate system.

[0080] S34: Then, through rotation and translation, the target is transformed from the camera coordinate system to the robot coordinate system.

[0081] The coordinate transformation formula is as follows:

[0082]

[0083] In the formula, R and T are the rotation and translation matrices of the camera coordinate system relative to the robot coordinate system, [X r Y r Z r T are the coordinates of the target point in the robot coordinate system.

[0084] S35: Add a layer of grid map, with the robot as the center, project the target position onto the grid map to form a semantic grid map.

[0085] As Figure 4 shown, the present invention provides an embodiment of a semantic probability conversion and multi-cost map fusion method, that is, the above step S4 specifically includes the following steps:

[0086] S41: Perform probability conversion on the semantic grid map to obtain the traversable probability p of each grid cell t .

[0087] Further process the semantic grid map to distinguish the traversable area in the grid map. Specifically, perform distance conversion on the grid cells marked as traversable in the semantic grid map, map the distance from the traversable area grid cell to the nearest non-traversable cell to a real number distance value, so as to represent the distance from each traversable grid cell to the non-traversable cell. After distance transformation, a relatively high value will be obtained in the central part of the traversable area, and on the contrary, the values in the edge area and the area close to the obstacle are lower.

[0088] The distance transformation formula is as follows:

[0089] D(p) = min(||m - n|| 2 ), m ∈ T r ​​, n ∈ N t

[0090] Wherein, the set of passable areas in the semantic grid map is T r , and the set of impassable areas is N t .

[0091] After distance transformation, the distance is further transformed by Gaussian distribution. The mean of the Gaussian distribution is set to the maximum value of the distance transformation, and the variance is the standard deviation of the squares of all distances. Finally, the passing probability p of each grid cell is obtained D . Finally, according to different semantic labels, the prior passable probability p is preset l , and finally the passable probability p of each grid cell is obtained t .

[0092] The formula for the passable probability of a grid cell is: p t = p D p l .

[0093] S42: On the original local cost map, add a semantic layer cost map. Finally, fuse the laser obstacle layer, the dilation layer, and the semantic layer, and publish them to the main cost map layer to finally obtain the local semantic grid map

[0094] Multi-layer cost map fusion:

[0095] In the laser obstacle layer, the cost value of a grid cell with a fatal obstacle is set to 254, and a collision with an obstacle is inevitable within this cost value; the cost value of a free movement space is set to 0; the cost value range of the minimum non-free space is from 1 to 127, and the robot will not collide within this range; the cost value of the dilation layer is from 128 to 253, and the cost value of an unknown area is 255

[0096] The cost values of the semantic layer are set as follows:

[0097] C s = c i (2 - p t )

[0098] Wherein, c i is the preset passing cost value of the semantic label, and p t is the passable probability of the semantic grid cell; the preset passing cost value of a difficult-to-pass semantic label is higher than that of an easy-to-pass one, and finally the cost value of each grid cell in the semantic layer can be obtained

[0099] Finally, fuse them into the fused main cost map in the order of the laser obstacle layer, the semantic layer, and the dilation layer. The fusion method uses the maximum value comparison method, and the formula is as follows:

[0100]

[0101] Among them, represents the cost value of the index-th grid cell in the main cost map, represents the cost value of the index-th grid cell in the current hierarchical cost map. When updating the map, the cost value of each grid cell in the current sub-layer is compared with the cost value at the corresponding position in the main cost map.

[0102] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the purpose and scope of the present technical solution, and they should all be covered by the scope of the claims of the present invention.

Claims

1. A method for constructing a local semantic passable probability grid map of an autonomous mobile robot, characterized in that The method specifically includes the following steps: S1: Construct an occupancy grid map based on the measured values of the laser data; S2: Use a real-time semantic segmentation model to perform semantic segmentation, extract semantic labels, and perform 2D semantic segmentation; S3: After semantic segmentation, project the semantic information using the depth map according to the coordinate relationship, that is, construct a semantic grid map based on the semantic information; S4: After the probability conversion of the semantic grid map, it is fused with the multi-layer cost map: the semantic information is probabilistically converted through distance mapping to form a semantic layer cost map, and then fused with the obstacle layer cost map of the laser to obtain the final local semantic traversable probability grid map, which specifically includes the following steps: S41: Perform probability conversion on the semantic grid map to obtain the traversable probability p of each grid cell t , specifically including: performing distance conversion on the grid cells marked as traversable in the semantic grid map, mapping the distance from the traversable area grid cells to the nearest non-traversable cell to a real distance value, so as to represent the distance from each traversable grid cell to the non-traversable cell; after distance transformation, the central part of the traversable area will obtain a high mapping value, on the contrary, the mapping values of the edge area and the area close to the obstacle are low: The distance transformation formula is as follows: D(p) = min(||m - n|| 2 ), m ∈ T r , n ∈ N t Among them, the set of passable areas in the semantic grid map is T r , and the set of impassable areas is N t ; After distance transformation, the distance is further transformed by Gaussian distribution. The mean of the Gaussian distribution is set to the maximum value of the distance transformation, and the variance is the standard deviation of the squares of all distances; the traversability probability p of each grid cell is obtained D ; then, the prior traversability probability p l is preset according to different semantic labels, and finally the traversability probability p t of each grid cell is obtained, where p t = p D p l ; S42: Multi-layer cost map fusion: On the original local cost map, add a semantic layer cost map, and finally fuse the laser obstacle layer, dilation layer, and semantic layer and publish them to the main cost map layer to finally obtain the local semantic grid map; Specifically include: In the laser obstacle layer, the cost value of the grid cell's fatal obstacle is set to 254, and within this cost value, a collision with the obstacle will definitely occur; the cost value of the free movement space is set to 0; the cost value range of the minimum non-free space is between 1 and 127, and the robot will not collide within this range; the cost value of the dilation layer is between 128 and 253, and the cost value of the unknown area is 255; The cost values of the semantic layer are set as follows: C s = c i (2 - p t ) Among them, c i is the preset passing cost value of the semantic label, and p t is the passing probability of the semantic grid cell; the preset passing cost value of the semantic label with difficult passage is higher than that of the preset passing cost value with easy passage, and finally the cost value of each grid cell in the semantic layer can be obtained; Finally, fuse them into the fused main cost map in the order of the laser obstacle layer, semantic layer, and dilation layer. The fusion method uses the maximum value comparison method, and the formula is as follows: Among them, represents the cost value of the index-th grid cell in the main cost map, represents the cost value of the index-th grid cell in the current hierarchical cost map; when updating the map, the cost value of each grid cell in the current sub-layer is compared with the cost value at the corresponding position in the main cost map.

2. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 1, characterized in that In step S1, to construct the occupancy grid map, it specifically includes the following steps: S11: Set the resolution of the grid map, that is, the appropriate grid size; S12: Based on the pose state and observation values of the robot at the current moment, update the occupancy probability of any grid map on the basis of the grid map probability at the previous moment.

3. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 2, wherein In step S12, the occupancy probability update rule of the grid map is: (1) Obtain measurement values through sensors and determine that the corresponding grid is occupied, where m i = 1 or is free, where m i = 0; (2) According to the Bayesian formula, after conversion and taking the logarithm, find the state update rule of the grid cell; (3)Finally, the posterior probability of any grid m is obtained. i is obtained.

4. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 1, characterized in that, In step S2, the semantic segmentation is specifically through a semantic segmentation model based on a convolutional neural network, and a semantic label is assigned to each pixel in the image at the output layer. As a result, the traversable area is segmented out for the robot to construct a semantic grid map.

5. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 1 or 4, characterized in that In step S2, the 2D semantic segmentation specifically includes the following steps: S21: Sensor image input; S22: The first part is the spatial path, which extracts the spatial information in the image; after passing through three convolutional layers, the feature map is output; S23: The second part is the semantic path, which extracts the context information, rapidly increases the receptive field and reduces the size of the feature map through multi-layer convolution, and outputs the feature map; S24: Fuse the feature maps obtained in the previous two parts through a feature fusion module, and the fused features are upsampled and then the final segmentation result is output.

6. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 1, characterized in that, In step S3, the semantic information is projected using the depth map, which specifically includes: obtaining an RGB image and a depth image through a binocular camera, obtaining the semantic segmentation image from the RGB image through a semantic segmentation model, obtaining the position information of an object or a passable area, obtaining the actual distance information through the depth map information combined with coordinate transformation, and finally projecting the semantic information onto a local grid map.

7. The method for constructing a local semantic passable probability grid map of an autonomous mobile robot according to claim 1 or 6, characterized in that Step S3 specifically includes the following steps: S31: Obtain the position information of an object or a passable area in the pixel coordinates in the semantic segmentation image; S32: Map the pixels in the RGB coordinate plane to the depth image pixel plane to obtain the depth value d of each pixel; S33: Perform coordinate transformation through the camera intrinsic parameter formula to transform the target from the pixel coordinate system to the camera reference system; S34: Transform the target from the camera coordinate system to the robot coordinate system through rotation and translation; S35: Add a layer of grid map, with the robot as the center, and project the target position onto the grid map to form a semantic grid map.

Images