CN114758063A - Construction method and system of local obstacle grid map based on octree structure - Google Patents

Abstract

The invention provides a method and a system for building a grid map of a local obstacle based on an octree structure. Compared with the prior map construction method, the method has the advantages in the scenes with large pedestrian volume and frequently changed environment: the point cloud with a certain gradient is converted to an xOy plane through ground detection, so that judgment of a height threshold value and point cloud processing of an obstacle are facilitated; the octree structure can dynamically update the local three-dimensional point cloud through the point cloud hit and penetration probability, and has strong adaptability to scenes with more personnel and dynamic environment changes; only a local grid map around the robot is stored, so that the memory occupation cost is reduced, and the resource consumption is ensured not to be linearly increased along with the running time; compared with the scheme of laser radar, the invention has the advantage of low cost.

Description

Construction method and system of local obstacle grid map based on octree structure

technical field

The invention relates to the field of ground robot mapping and navigation, in particular to a method for generating a grid map of local obstacles based on an octree structure in the field of ground robot mapping and navigation.

Background technique

With the increasing level of science and technology, people gradually use robots to replace humans to complete some highly repeatable tasks, such as sweeping robots, food delivery robots, patrol robots and warehouse robots. Robots have the advantages of low long-term investment cost, no need for rest time, high work efficiency, and easy management.

For robots, real-time positioning and map construction technology is one of its core technologies. In order to realize automatic operation in a dynamic environment, it is necessary to provide a method that can simply and efficiently construct a scene map around the robot's location, so as to realize upper-level tasks such as path planning and real-time obstacle avoidance.

The general process of the existing local grid map construction method based on octree structure is divided into the following two types:

1). The currently collected point cloud is fused with the point cloud of the previous frame through point cloud matching, and the point cloud of the area of interest (the local area centered on the unmanned vehicle) is cut out, and then the fusion point cloud is down-sampled and summed. Noise reduction processing, insert the processed fused point cloud into the octree map, and project it as a local grid map.

2). The current point cloud information is collected in real time through the depth point cloud sensor. After downsampling and noise reduction processing, the point cloud is directly inserted into the octree map and projected as a local grid map, without using the previously collected point cloud. information, so as to realize the real-time update of the obstacle point cloud.

However, for the first method above, the point clouds fused together at different times are inserted into the octree map at the same time. When the point clouds are matched, dynamic objects such as people and cars will have overlapping point clouds. When people pass the robot, obstacles will occur. The object point cloud will cover the passable area in front of it, causing the path planning to fail. As shown in Figure 6, the lower left corner is the robot, the upper right corner is the wall, and the circle in the middle represents moving objects, such as people. The invention inserts the point cloud into the octree map through ray detection and updates the node occupancy probability, emphasizing the adaptability and real-time update of the high dynamic scene; and uses the pre-built prior grid map as a supplement, and the focus is to obtain the robot in real time. Dynamic changes of obstacles in the surrounding area.

For the second method above, there will be a problem of information loss in the field of ground robots, as shown in Figure 7. Without the assistance of previous information, when the ground robot continues to move forward, the previously existing obstacle information cannot be transmitted to the current state, which will lead to a collision accident.

SUMMARY OF THE INVENTION

The purpose of the present invention is to overcome the above-mentioned existing ground robot in the prior art that often encounters that the passable area in front is blocked by all passing dynamic objects when running in a high dynamic scene, and the ground robot will crash without using the previous point cloud information. To solve the problem of obstacles, a method for constructing local obstacle grid map based on octree structure is proposed.

In view of the deficiencies of the prior art, the present invention proposes a method for constructing a grid map of local obstacles based on an octree structure, including:

Step 1. Obtain the pre-built scene obstacle grid map, and initialize the octree map; the ground robot collects the depth information of the current environment through the camera, and the sensor in the ground robot collects the pose information of the ground robot, and according to the position Attitude information, remove the obstacle points beyond the set range in the octree map, and get a simplified octree map;

Step 2. Convert the depth information into a three-dimensional point cloud, obtain the relative transformation of the camera relative to the map coordinate system according to the pose information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into map coordinate system;

Step 3. Use octree ray detection to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octomap, and project the insertion result to the two-dimensional plane to obtain the actual local obstacle map; according to the pose information, only Retain the map around the ground robot in the obstacle grid map of the scene, obtain a local obstacle prior map, fuse the actual local obstacle map and the local obstacle prior map, and obtain a local obstacle grid map to guide The ground machine completes obstacle avoidance.

The method for constructing a local obstacle grid map based on an octree structure, wherein the initialization information for initializing the octree map in step 1 includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, Ray detection farthest update threshold, local grid map resolution size and local grid map save range threshold;

The specific process of generating the simplified octagonal map in step 1 includes:

Traverse all nodes in the octree map to obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to obtain the distance, and set the nodes whose distances exceed the preset threshold as empty nodes.

The described local obstacle grid map construction method based on octree structure, wherein in step 2, this three-dimensional point cloud is transformed to the map coordinate system by the following formula:

where X, Y, Z are the coordinates of the 3D point cloud transformed to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, R _{m, c} represent the camera coordinate system relative to the map coordinate system Rotation matrix; t _m,c represents the translation matrix of the camera coordinate system relative to the map coordinate system; 0 represents a 1x3 zero matrix vector;

In step 2, the following formula is used to obtain the coordinate system transformation of the camera coordinate system relative to the map coordinate system:

T _m,c =T _M,r *T _r,c

Where T _m,c represents the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system, T _r,c represents the coordinate system transformation of the camera relative to the robot chassis, obtained through offline calibration, T _{m, r} represents the robot chassis relative to The coordinate system transformation based on the map coordinate system is obtained through the robot pose information obtained in step S1.

The method for constructing a local obstacle grid map based on an octree structure, wherein step 3 inserts the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map in the following manner:

Traverse all the points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes through which the line connecting the map coordinates of the camera and the map coordinates of each point passes; if the length of the line is greater than the set ray Detect the farthest update threshold, only calculate the connection whose length is less than or equal to the update threshold and the end node is considered as a passable node, otherwise it is considered as an obstacle node; for each passing octree node, the node passing through The number of times is increased by one. If the end node of the connection is an obstacle node, the number of hits is increased by one, otherwise the number of crossings is increased by one, and the occupancy probability is updated using the following formula:

where P _occ represents the probability of occupancy, C _hits represents the number of hits, and C _miss represents the number of passes.

In step 3, the insertion result is projected to a two-dimensional plane, and the specific process of obtaining the actual local obstacle map includes:

Set the map coordinates where the ground robot is located as the origin of the actual local obstacle map;

Create a blank obstacle grid map according to the set grid map resolution and local grid map saving range threshold, and assign an initial value of -1 to each grid point on the map;

Traverse all the minimum voxel leaf nodes in the insertion result octree map, if the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinates of the point satisfies the set obstacle projection maximum If the height threshold is set, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, the grid is a passable area.

The present invention also proposes a local obstacle grid map construction system based on the octree structure, which includes:

The initial module is used to obtain the pre-built scene obstacle grid map and initialize the octree map; the ground robot collects the depth information of the current environment through the camera, and the sensor in the ground robot collects the pose information of the ground robot, and according to the The pose information, remove the obstacle points beyond the set range in the octree map, and get a simplified octree map;

The transformation module is used to convert the depth information into a three-dimensional point cloud. According to the pose information and the external parameters of the ground robot chassis relative to the camera, the relative transformation of the camera relative to the map coordinate system is obtained, and the three-dimensional point cloud is obtained. Transform to the map coordinate system;

The building module is used to insert the 3D point cloud transformed into the map coordinate system into the simplified octree map using octree ray detection, and project the insertion result to the 2D plane to obtain the actual local obstacle map; according to the pose information , retain only the map around the ground robot in the scene obstacle grid map, obtain the local obstacle prior map, fuse the actual local obstacle map and the local obstacle prior map, and obtain the local obstacle grid map, to guide the ground machine to complete obstacle avoidance.

The described local obstacle grid map construction system based on the octree structure, wherein the initialization information for initializing the octree map in the initial module includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, Ray detection farthest update threshold, local grid map resolution size and local grid map save range threshold;

The specific process of generating the simplified octagonal map in the initial module includes:

Traverse all nodes in the octree map to obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to obtain the distance, and set the nodes whose distances exceed the preset threshold as empty nodes.

The described local obstacle grid map construction system based on the octree structure, wherein in the transformation module, the three-dimensional point cloud is transformed into the map coordinate system by the following formula:

where X, Y, Z are the coordinates of the 3D point cloud transformed to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, R _{m, c} represent the camera coordinate system relative to the map coordinate system Rotation matrix; t _m,c represents the translation matrix of the camera coordinate system relative to the map coordinate system; 0 represents a 1x3 zero matrix vector;

The following formula is used in the transformation module to obtain the coordinate system transformation of the camera coordinate system relative to the map coordinate system:

T _m,c =T _m, r*T _r,c

Where T _m,c represents the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system, T _r,c represents the coordinate system transformation of the camera relative to the robot chassis, obtained through offline calibration, T _{m, r} represents the robot chassis relative to The coordinate system transformation based on the map coordinate system is obtained through the robot pose information obtained in step S1.

The described local obstacle grid map construction system based on the octree structure, wherein the building module is used to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map in the following manner:

Traverse all the points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes through which the line connecting the map coordinates of the camera and the map coordinates of each point passes; if the length of the line is greater than the set ray Detect the farthest update threshold, only calculate the connection whose length is less than or equal to the update threshold and the end node is considered as a passable node, otherwise it is considered as an obstacle node; for each passing octree node, the node passing through The number of times is increased by one. If the end node of the connection is an obstacle node, the number of hits is increased by one, otherwise the number of crossings is increased by one, and the occupancy probability is updated using the following formula:

where P _occ represents the probability of occupancy, C _hits represents the number of hits, and C _miss represents the number of passes.

In this building block, the insertion result is projected to a two-dimensional plane, and the specific process of obtaining the actual local obstacle map includes:

Set the map coordinates where the ground robot is located as the origin of the actual local obstacle map;

Create a blank obstacle grid map according to the set grid map resolution and local grid map saving range threshold, and assign an initial value of -1 to each grid point on the map;

Traverse all the minimum voxel leaf nodes in the insertion result octree map, if the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinates of the point satisfies the set obstacle projection maximum If the height threshold is set, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, the grid is a passable area.

The present invention also provides a storage medium for storing a program for executing any of the methods for constructing a local obstacle grid map based on an octree structure.

The present invention also provides a client, which is used for any of the local obstacle grid map construction systems based on the octree structure.

As can be seen from the above scheme, the advantages of the present invention are:

The invention provides a method for constructing a grid map of local obstacles based on an octree structure. The prior information of the local obstacle map is obtained according to the preloaded map, and then the actual local obstacle map is generated according to the collected data, and the fusion of the two is used to provide An accurate and reliable obstacle information around the robot. Compared with the previous map construction method, the present invention has great advantages in scenes with large flow of people and frequently changing environments: first, the point cloud of a certain slope is converted to the xOy plane through ground detection, which is convenient for height threshold judgment and obstacle detection. Point cloud processing; secondly, the octree structure can dynamically update the local 3D point cloud through the point cloud hit and pass probability, which has strong adaptability to scenes with many people and dynamic changes in the environment; then, because only the robot is saved The surrounding local grid map reduces the memory occupation overhead and ensures that the resource consumption will not increase linearly with the running time; in addition, compared with the scheme using the lidar, the present invention has the advantage of low cost.

Description of drawings

1 is a flowchart of a method for constructing a local obstacle grid map based on an octree structure provided by an embodiment of the present invention;

2 is a schematic diagram of a fitting plane projected to an xOy plane provided by an embodiment of the present invention;

3 is a schematic structural diagram of an octree map provided by an embodiment of the present invention;

FIG. 4 is a schematic diagram of a method for updating an octree child node according to an embodiment of the present invention;

5 is a schematic diagram of a priori splicing of a local obstacle grid map and a local grid map in a preloaded map provided by an embodiment of the present invention;

6 is a schematic diagram of the introduction of problems that may occur in a high dynamic scene in the prior art;

FIG. 7 is a schematic diagram illustrating a possible problem in the prior art that does not utilize previous information.

Detailed ways

While conducting research on the construction of a local grid map based on an octree structure, the inventor found that there is a problem in the prior art that the ground robot often encounters the problem that the passable area in front is blocked by all passing dynamic objects when running in a high dynamic scene. , which is caused by first fusing the point cloud and then inserting the octree map, and not using the previous point cloud information will cause the ground robot to crash into the obstacle. After research, the inventor found that the octree map can be updated by using the connection between the ground robot's own position and the obstacle point cloud every time the point cloud information is obtained, and the method will not lose the obstacle data after losing the field of view. information, and can dynamically update the obstacle data information that has left the original position; thus perfectly solving the problems existing in the prior art.

Specifically, the present invention proposes a method for constructing a local obstacle grid map based on an octree structure, which includes the following steps:

S1. Obtain the depth information of the target environment and the pose information of the robot collected by the RGB-D camera in real time;

S2. Load the pre-built scene obstacle grid map, and initialize the octree structure. At this time, the octree is an empty data structure; wherein, the pre-built scene obstacle grid map can pass through no dynamic obstacles in the scene. It can be obtained by running other SLAM simultaneous positioning and mapping methods at the same time;

S3, according to the position and attitude information of the robot, remove the obstacle points that have exceeded the set range in the octree map;

S4, converting the target environment depth information into a three-dimensional point cloud, and preprocessing the three-dimensional point cloud;

S5, according to the position and attitude information of the robot and the external parameters of the robot chassis relative to the camera, calculate the relative transformation of the camera relative to the map (scene obstacle grid map) coordinate system, and transform the three-dimensional point cloud to the map coordinate system;

S6. Pre-obtain a priori obstacle grid map around the robot according to the pose information of the robot;

S7, using octree ray detection to insert the three-dimensional point cloud transformed into the map coordinate system into the octree map;

S8. Project the octree map to a two-dimensional plane;

S9. Generate a local obstacle grid map that fuses the prior scene information and the current scene information.

The core invention points of the present invention are removing the obstacle points in the octree map that have exceeded the set range in S3, and fitting the ground equation through the random sampling consensus method RANSAC in the preprocessing part of S4 and transforming the point cloud to xOy Plane and real-time ray detection updates in S7.

A method for constructing a local obstacle grid map based on an octree structure according to claim 1, wherein the initialization information of the octree structure in the step S2 includes: the resolution size of the minimum voxel leaf node, the obstacle Object projection maximum height threshold, ray detection farthest update threshold, local raster map resolution size and local raster map save range threshold.

In the described method for constructing a local obstacle grid map based on an octree structure, the step S3 uses the following method to remove the obstacle point cloud in the octree map beyond the set range:

Traverse all occupied nodes in the octree map to obtain the map coordinates of the node; subtract the map coordinates of the node from the map coordinates of the camera, if the distance exceeds the preset local grid map preservation range threshold, the node is considered to be If the distance from the robot is too far, set the node as an empty node, and clear the occupancy probability and map coordinates contained in the node.

The step S4 also includes:

Offline calibration of the coordinate system transformation of the robot chassis relative to the camera;

Downsampling and denoising of 3D point clouds;

Fit the ground equation by random sampling consensus method RANSAC, and transform the point cloud to the xOy plane;

According to a method for constructing a local obstacle grid map based on an octree structure according to claim 1, the three-dimensional point cloud in the step S5 is transformed into the map coordinate system by the following formula:

Among them, X, Y, Z are the coordinates of the 3D point cloud transformed to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system in step S3, and R _{m, c} represent the RGB-D camera coordinate system The rotation matrix relative to the map coordinate system; t _m,c represents the translation matrix of the RGB-D camera coordinate system relative to the map coordinate system, which is also the map coordinate of the camera; 0 represents a 1x3 zero matrix vector.

The step S5 adopts the following formula to calculate the coordinate system conversion of the RGB-D camera coordinate system relative to the map coordinate system:

T _m,c =T _m,r *T _r,c

Where T _m,c represents the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system, T _r,c represents the coordinate system transformation of the camera relative to the robot chassis, obtained through offline calibration, T _{m, r} represents the robot chassis relative to The coordinate system transformation based on the map coordinate system is obtained through the robot pose information obtained in step S1.

The step S7 inserts the three-dimensional point cloud transformed into the map coordinate system into the octree map in the following manner:

Traverse all the points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes through which the line connecting the map coordinates of the camera and the map coordinates of each point passes; if the length of the line is greater than the set ray Detect the farthest update threshold, only calculate the connection whose length is less than or equal to the update threshold and the end node is considered as a passable node, otherwise it is considered as an obstacle node; for each passing octree node, the node passing through The number of times is increased by one. If the end node of the connection is an obstacle node, the number of hits is increased by one, otherwise the number of crossings is increased by one, and the occupancy probability is updated using the following formula:

where P _occ represents the probability of occupancy, C _hits represents the number of hits, and C _miss represents the number of passes.

The step S8 uses the following method to project the local obstacle grid map:

Set the map coordinates where the robot is located as the origin of the local obstacle grid map;

Create a blank obstacle grid map according to the set grid map resolution and local grid map saving range threshold, and assign an initial value of -1 to each grid point on the map;

Traverse all the minimum voxel leaf nodes in the octree map after step 6, if the occupancy probability of the point is greater than the set probability value and the height value of the map coordinates of the point meets the set obstacle projection maximum height threshold, Then mark the value of the corresponding grid map point as 100, indicating that the grid has been occupied; otherwise, it indicates that the grid is a passable area.

The downsampling and noise reduction of the 3D point cloud includes:

Downsample the point cloud with a voxel filter, where the size of the voxel filter is not greater than half of the resolution size of the minimum voxel leaf node of the set octree map;

Use statistical filtering to remove outliers from point clouds.

In order to make the above-mentioned features and effects of the present invention more clearly and comprehensible, embodiments are given below, and detailed descriptions are given below in conjunction with the accompanying drawings.

The invention provides a method for constructing a grid map of local obstacles based on an octree structure. The prior information of the local obstacle map is obtained according to the preloaded map, and then the actual local obstacle map is generated according to the collected data. An accurate and reliable obstacle information around the robot. Compared with the previous map construction method, the present invention has great advantages in scenes with large flow of people and frequently changing environments: first, the point cloud of a certain slope is converted to the xOy plane through ground detection, which is convenient for height threshold judgment and obstacle detection. Point cloud processing; secondly, the octree structure can dynamically update the local 3D point cloud through the point cloud hit and pass probability, which has strong adaptability to scenes with many people and dynamic changes in the environment; then, because only the robot is saved The surrounding local grid map reduces the memory occupation overhead and ensures that the resource consumption will not increase linearly with the running time; in addition, compared with the scheme using the lidar, the present invention has the advantage of low cost.

FIG. 1 is a flowchart of a method for constructing a local obstacle grid map based on an octree structure provided by an embodiment of the present invention. The technical solution of the present invention will be described in detail below with reference to FIG. 1 .

Step 110, obtain the depth information of the target environment collected by the RGB-D camera and the pose information of the robot in real time;

Specifically, the RGB-D camera gives the depth map data of the target environment in real time. The pose information of the robot can be obtained through the data of the wheel speedometer carried by the robot itself, or it can be obtained according to the global positioning sensor such as the global positioning system GPS and the ultra-wideband system UWB. given, or obtained by jointly optimizing multiple sensor data. Because this method mainly uses the local obstacle map, it can still work normally when the positioning is slightly offset, in which the RGB-D camera sensor can be one or more.

Step 120, load the pre-built scene obstacle grid map, and initialize the octree structure;

Specifically, the pre-built scene obstacle grid map is constructed in a static scene, and the construction should not contain dynamic objects. For example, when the restaurant is closed or the exhibition hall is closed, the scene is mapped once and can be reused later. The initialization parameters of the octree structure include:

The resolution size of the minimum voxel leaf node;

Obstacle projection maximum height threshold;

The farthest update threshold of ray detection;

Local grid map resolution size;

The local raster map holds the extent threshold.

Figure 2 shows a schematic diagram of the octree structure used in this method. After inserting a point cloud, the octree will extend down layer by layer until it reaches a set minimum voxel node. The large square in the left picture of Figure 2 represents the root node, which is divided into 8 sub-square spaces; the blank square represents the leaf node without point cloud insertion in the area, the black square represents the smallest voxel leaf node, and the gray square represents the intermediate node. The abstract structure of the octree is shown in the right figure of Figure 2. The hollow node represents the root node. It starts from the root node and extends down to eight child nodes. The gray node represents the intermediate node, which means that there is a minimum voxel leaf node; each intermediate node The nodes continue to divide down the child nodes containing the minimum voxel leaf node until the minimum voxel node is divided; for each minimum voxel leaf node, the occupancy probability of the leaf node and the corresponding map coordinates are included.

Step 130, according to the position and attitude information of the robot, remove the obstacle points that have exceeded the set range in the octree map;

Specifically, first traverse all occupied nodes in the octree map to obtain the map coordinates of the nodes; subtract the map coordinates of the nodes from the map coordinates of the camera, and if the distance exceeds a preset threshold, it is considered that the node is connected to the robot If the distance is too far, set the node as an empty node, and clear the occupancy probability and map coordinates contained in the node.

Step 140, converting the target environment depth information into a 3D point cloud, and preprocessing the 3D point cloud;

Specifically, the depth information of the target environment is converted into a 3D point cloud by the following formula:

Among them, X, Y, and Z on the left represent the coordinate information of the 3D point cloud in the camera coordinate system, f _x , f _y , c _x , and _cy are the internal parameters of the camera, u and v represent the pixel coordinate information, and d represents the pixel of the point. The depth value corresponding to the coordinate.

Specifically, the preprocessing of the 3D point cloud includes the following steps:

Offline calibration of the coordinate system transformation of the robot chassis relative to the camera;

Downsampling and denoising of 3D point clouds;

The ground equations are fitted by the random sampling consensus method RANSAC, and the point cloud is transformed to the xOy plane.

Among them, the voxel filter is used to downsample the point cloud, and the size of the voxel filter is not greater than half of the resolution size of the minimum voxel leaf node of the set octree map; statistical filtering is used to remove the distance in the point cloud. group point.

In fitting the ground, it is assumed that the robot's running road has a certain slope (the slope is less than 45 degrees), and the random sampling consensus method RANSAC selects three points to form a plane, and calculates the points on the plane (that is, the distance with the plane). The number of points whose vertical distance is less than a certain threshold), repeat this operation continuously, and select the model with the most points on the plane as the final result. After the random sampling consensus method RANSAC obtains the plane equation of the ground, the normal vector n of the plane is used to calculate the rotation transformation that rotates the ground to the xOy plane, where the rotation axis n' and the rotation angle θ are calculated as follows:

Figure 2 shows a schematic diagram of the fitting plane projected onto the xOy plane, where the plane π is the ground plane fitted by the random sampling consensus method RANSAC, the rotation axis n' is the intersection of the plane π and the xOy plane; the rotation angle θ is The angle between the plane π and the xOy plane; the desired effect can be obtained by rotating the plane π around the rotation axis n' by θ degrees.

After obtaining the rotation axis n' and the rotation angle θ, the rotation vector is θn', convert the rotation vector into a rotation matrix, and rotate all points by the following formula:

Where X, Y, Z are the coordinates of the 3D point cloud after transforming the ground to the xOy plane, and x, y, z are the coordinates of the original 3D point cloud in the camera coordinate system.

Step 150, according to the position and attitude information of the robot and the external parameters of the robot chassis relative to the camera, calculate the relative transformation of the camera relative to the map coordinate system, and transform the three-dimensional point cloud into the map coordinate system;

The relative transformation of the camera with respect to the map coordinate system is calculated by:

T _m,c =T _m,r *T _r,c

Among them, T _m,c represents the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system; T _r,c represents the coordinate system transformation of the camera relative to the robot chassis, obtained through offline calibration; T _m,r represents the robot chassis relative to The coordinate system conversion to the map coordinate system is obtained through the robot pose information read in step 110 .

After calculating the relative transformation T _m,c of the camera relative to the map coordinate system, the 3D point cloud is transformed to the map coordinate system to the world coordinate system by the following formula:

Among them, X, Y, Z are the coordinates of the three-dimensional point cloud after transforming the ground to the map coordinate system, x, y, z are the coordinates in the camera after the three-dimensional point cloud is transformed to the xOy plane in step 140, and R _{m, c} represent RGB The rotation matrix of the -D camera coordinate system relative to the map coordinate system; t _m,c represents the translation matrix of the RGB-D camera coordinate system relative to the map coordinate system, which is also the map coordinate of the camera.

Step 160: Pre-acquire a priori obstacle grid map around the robot according to the position and attitude information of the robot;

Specifically, first, the map coordinates where the robot is located are obtained through the robot pose information;

Use the map coordinates where the robot is located as the origin of the local obstacle grid map, create a blank obstacle map according to the set grid map resolution and local map size, and assign an initial value of -1 to each point on the map ;

Overlay the grid map values corresponding to the pre-built scene map on the blank local obstacle grid map to obtain a priori grid map;

Step 170, using octree ray detection to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map;

Specifically, the method for inserting a three-dimensional point cloud using octree ray detection is as follows:

Traverse all the points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes passed by the connection line between the map coordinates of the camera and the map coordinates of each point; for each passing octree node , update the occupancy probability of the node. If it is the end node of the connection, the number of hits is increased by one, otherwise the number of crossings is increased by one, and the occupancy probability is updated using the following formula:

Preferably, if the value of P _occ is greater than 0.6, the point is considered as an obstacle point, otherwise, the point is considered as a passable point.

则将该点标记为障碍物点。FIG. 3 shows the update manner of the leaf nodes in the octree structure represented by the above manner, and the update is actually performed as a point in a three-dimensional space, which is shown here in a two-dimensional space for the convenience of description. The black square in the upper right corner represents the point cloud point to be inserted currently, and the P point in the lower left corner is the current position of the robot. Nodes are represented by white squares. For the octree node corresponding to the white square, the number of passes C _miss is incremented by one, and the corresponding occupation probability P _occ is updated; for the octree node corresponding to the black square, the number of hits is incremented by 1, and the corresponding occupation is updated. Probability P _occ . If the number of hits in this section is 2 times and the number of times of passing through is 2 times, it is a passable point; after this update, the number of hits is 3 times,

then mark the point as an obstacle point.

Step 180, project the octree map to a two-dimensional plane;

Specifically, the octree map is projected to a two-dimensional plane in the following way:

Set the map coordinates where the robot is located as the origin of the local obstacle grid map;

Create a blank obstacle grid map according to the set grid map resolution and local map size, and assign an initial value of -1 to each point on the map;

Traverse all the minimum voxel leaf nodes in the octree map. If the occupancy probability of the point is greater than the set probability value and the height value of the map coordinates of the point meets the set height threshold, the corresponding grid map will be traversed. The value of the coordinate is recorded as 100, indicating that the grid is occupied; otherwise, the grid is a passable area.

Step 190, generating a local obstacle grid map that fuses the prior scene information and the current scene information;

Specifically, the local grid map value of the current scene information is overlaid on the prior grid map; that is, if the local grid map value of the current scene information is -1, which is an unknown area, the map corresponding to the prior grid map is used. The value is replaced to obtain more comprehensive information about obstacles around the robot.

Figure 4 shows the process of fusing the current grid map and the prior grid map; the upper left corner is the current grid map generated by step 180, which describes the scene information currently seen by the robot, in which black pixels represent obstacles and white pixels Indicates the passable area, and the gray pixels represent the unknown area. It can be seen that there is a large area of unknown area in the current grid map; the pre-built scene map is loaded on the right, and the scene information is more comprehensive, but it can be seen that the tables and chairs were placed at that time. There are some differences from the present; for the gray unknown area of the map in the upper left corner, the pixel value of the corresponding area on the right is used to replace it, and the fused local obstacle grid map is obtained, as shown in the lower left corner; the map in the lower left corner has more The comprehensive distribution of obstacles around the robot can be updated and corrected in real time.

A method for constructing a local obstacle grid map based on an octree structure of the present invention obtains the prior information of the local obstacle map according to the preloaded map, then generates the actual local obstacle map according to the collected data, and uses the fusion of the two to provide a local obstacle map. Accurate and reliable information about obstacles around the robot. Compared with the previous map construction methods, the present invention has great advantages in scenes with large traffic flow and frequently changing environments: First, the octree structure can dynamically update the local 3D point cloud through the point cloud hitting and passing probability. Scenarios with many people and dynamic changes in the environment have strong adaptability; secondly, because only the local grid map around the robot is saved, the memory usage overhead is reduced, and resource consumption will not increase linearly with running time.

The following are system embodiments corresponding to the foregoing method embodiments, and this implementation manner may be implemented in cooperation with the foregoing implementation manners. The related technical details mentioned in the foregoing embodiment are still valid in this embodiment, and are not repeated here in order to reduce repetition. Correspondingly, the relevant technical details mentioned in this embodiment can also be applied to the above-mentioned embodiments.

The present invention also proposes a local obstacle grid map construction system based on the octree structure, which includes:

The initial module is used to obtain the pre-built scene obstacle grid map and initialize the octree map; the ground robot collects the depth information of the current environment through the camera, and the sensor in the ground robot collects the pose information of the ground robot, and according to the The pose information, remove the obstacle points beyond the set range in the octree map, and get a simplified octree map;

The transformation module is used to convert the depth information into a three-dimensional point cloud. According to the pose information and the external parameters of the ground robot chassis relative to the camera, the relative transformation of the camera relative to the map coordinate system is obtained, and the three-dimensional point cloud is obtained. Transform to the map coordinate system;

The building module is used to insert the 3D point cloud transformed into the map coordinate system into the simplified octree map using octree ray detection, and project the insertion result to the 2D plane to obtain the actual local obstacle map; according to the pose information , retain only the map around the ground robot in the scene obstacle grid map, obtain the local obstacle prior map, fuse the actual local obstacle map and the local obstacle prior map, and obtain the local obstacle grid map, to guide the ground machine to complete obstacle avoidance.

The described local obstacle grid map construction system based on the octree structure, wherein the initialization information for initializing the octree map in the initial module includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, Ray detection farthest update threshold, local grid map resolution size and local grid map save range threshold;

The specific process of generating the simplified octagonal map in the initial module includes:

Traverse all nodes in the octree map to obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to obtain the distance, and set the nodes whose distances exceed the preset threshold as empty nodes.

The described local obstacle grid map construction system based on the octree structure, wherein in the transformation module, the three-dimensional point cloud is transformed into the map coordinate system by the following formula:

where X, Y, Z are the coordinates of the 3D point cloud transformed to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, R _{m, c} represent the camera coordinate system relative to the map coordinate system Rotation matrix; t _m,c represents the translation matrix of the camera coordinate system relative to the map coordinate system; 0 represents a 1x3 zero matrix vector;

The following formula is used in the transformation module to obtain the coordinate system transformation of the camera coordinate system relative to the map coordinate system:

T _m,c =T _m,r *T _r,c

Where T _m,c represents the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system, T _r,c represents the coordinate system transformation of the camera relative to the robot chassis, obtained through offline calibration, T _{m, r} represents the robot chassis relative to The coordinate system transformation based on the map coordinate system is obtained through the robot pose information obtained in step S1.

The described local obstacle grid map construction system based on the octree structure, wherein the building module is used to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map in the following manner:

Traverse all the points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes through which the line connecting the map coordinates of the camera and the map coordinates of each point passes; if the length of the line is greater than the set ray Detect the farthest update threshold, only calculate the connection whose length is less than or equal to the update threshold and the end node is considered as a passable node, otherwise it is considered as an obstacle node; for each passing octree node, the node passing through The number of times is increased by one. If the end node of the connection is an obstacle node, the number of hits is increased by one, otherwise the number of crossings is increased by one, and the occupancy probability is updated using the following formula:

where P _occ represents the probability of occupancy, C _hits represents the number of hits, and C _miss represents the number of passes.

In this building block, the insertion result is projected to a two-dimensional plane, and the specific process of obtaining the actual local obstacle map includes:

Set the map coordinates where the ground robot is located as the origin of the actual local obstacle map;

Create a blank obstacle grid map according to the set grid map resolution and local grid map saving range threshold, and assign an initial value of -1 to each grid point on the map;

Traverse all the minimum voxel leaf nodes in the insertion result octree map, if the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinates of the point satisfies the set obstacle projection maximum If the height threshold is set, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, the grid is a passable area.

The present invention also provides a storage medium for storing a program for executing any of the methods for constructing a local obstacle grid map based on an octree structure.

The present invention also provides a client, which is used for any of the local obstacle grid map construction systems based on the octree structure.

Professionals should be further aware that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of both. In order to clearly illustrate the interchangeability of hardware and software, the constitution and implementation steps of each example are generally described in detail in terms of functions in the above description. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of the present invention.

The steps of a method or algorithm described in connection with the embodiments disclosed herein may be implemented in hardware, a software module executed by a processor, or a combination of the two. A software module can be placed in random access memory (RAM), internal memory, read only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other in the technical field. in any other known form of storage medium.

The above specific embodiments further describe the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above are only specific embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Within the spirit and principle of the present invention, any modifications, equivalent replacements, improvements, etc. made should be included within the protection scope of the present invention.

Claims (10)

1. A local obstacle grid map construction method based on an octree structure is characterized by comprising the following steps:

step 1, acquiring a pre-built scene barrier grid map, and initializing an octree map; the ground robot acquires the depth information of the current environment through a camera, simultaneously acquires the pose information of the ground robot through a sensor in the ground robot, and removes the barrier points exceeding the set range in the octree map according to the pose information to obtain a simplified octree map;

step 2, converting the depth information into a three-dimensional point cloud, obtaining relative transformation of the camera relative to a map coordinate system according to the pose information and external reference of the ground robot chassis relative to the camera, and transforming the three-dimensional point cloud into the map coordinate system;

step 3, inserting the three-dimensional point cloud converted into the map coordinate system into the simplified octree map by utilizing octree ray detection, and projecting an insertion result to a two-dimensional plane to obtain an actual local obstacle map; according to the pose information, only a map around the ground robot in the scene obstacle grid map is reserved to obtain a local obstacle prior map, and the actual local obstacle map and the local obstacle prior map are fused to obtain a local obstacle grid map so as to guide the ground robot to complete obstacle avoidance.

2. The octree structure-based local obstacle grid map construction method according to claim 1, wherein the initializing information for initializing the octree map in step 1 comprises: the resolution of a minimum voxel leaf node, a maximum height threshold of barrier projection, a ray detection farthest updating threshold, a local grid map resolution and a local grid map storage range threshold;

the specific process of generating the simplified octree map in the step 1 comprises the following steps:

traversing all nodes in the octree map to obtain a node map coordinate; and subtracting the map coordinates of the node from the map coordinates of the camera to obtain a distance, and setting the node with the distance exceeding a preset threshold value as a null node.

3. The octree-based local obstacle grid mapping method according to claim 1, wherein the three-dimensional point cloud is transformed to the map coordinate system in step 2 by the following formula:

wherein X, Y and Z are coordinates of the three-dimensional point cloud after being transformed into a map coordinate system, X, Y and Z are coordinates of the three-dimensional point cloud in a camera coordinate system, and R_m,cA rotation matrix representing a camera coordinate system relative to a map coordinate system; t is t_m,cA translation matrix representing a camera coordinate system relative to a map coordinate system; 0 represents a zero matrix vector of 1x 3;

In step 2, the coordinate system conversion of the camera coordinate system relative to the map coordinate system is obtained by adopting the following formula:

T_m,c＝T_m,r*T_r,c

wherein T is_m,cRepresenting the transformation of the coordinate system of the RGB-D camera with respect to the coordinate system of the map, T_r,cRepresenting the transformation of the coordinate system of the camera with respect to the robot chassis, obtained by off-line calibration, T_m,rCoordinate system transformation representing the robot chassis relative to the map coordinate system is obtained from the robot pose information acquired in step S1.

4. The octree structure-based local obstacle grid map construction method according to claim 1, wherein said step 3 inserts the three-dimensional point cloud transformed to the map coordinate system into the reduced octree map by:

traversing all points of the three-dimensional point cloud converted into the map coordinate system, and calculating octree nodes through which a connecting line of the map coordinate of the camera and the map coordinate of each point passes; if the length of the connecting line is greater than the set ray detection farthest updating threshold, only calculating the connecting line of which the length is less than or equal to the updating threshold, and considering the tail node as a passable node, otherwise, considering the tail node as an obstacle node; for each passing octree node, the passing times of the node is added with one, if the tail node of the connecting line is the barrier node, the hitting times are added with one, otherwise, the passing times are added with one, and the occupation probability is updated by using the following formula:

Wherein P is_occDenotes the probability of occupation, C_hitsIndicates the number of hits, C_missRepresenting the number of passes.

The specific process of projecting the insertion result to the two-dimensional plane in the step 3 to obtain the actual local obstacle map includes:

setting the map coordinate of the ground robot as the original point of the actual local obstacle map;

creating a blank barrier grid map according to the set grid map resolution and the local grid map storage range threshold, and assigning an initial value of-1 to each grid point on the map;

traversing all minimum voxel leaf nodes in the interpolation result octree map, and recording the value of the corresponding grid map point as 100 to indicate that the grid is occupied if the occupation probability of the minimum voxel leaf nodes is greater than the set probability value and the height value of the point map coordinate meets the set maximum height threshold of the barrier projection; otherwise, the grid is a passable area.

5. An octree structure-based local obstacle grid mapping system, comprising:

the system comprises an initial module, a first search module and a second search module, wherein the initial module is used for acquiring a pre-built scene barrier grid map and initializing an octree map; the ground robot acquires the depth information of the current environment through a camera, simultaneously the sensor in the ground robot acquires the pose information of the ground robot, and barrier points exceeding a set range in the octree map are removed according to the pose information to obtain a simplified octree map;

The transformation module is used for transforming the depth information into a three-dimensional point cloud, obtaining relative transformation of the camera relative to a map coordinate system according to the pose information and external reference of the ground robot chassis relative to the camera, and transforming the three-dimensional point cloud into the map coordinate system;

the building module is used for inserting the three-dimensional point cloud converted into the map coordinate system into the simplified octree map by utilizing octree ray detection, and projecting an insertion result to a two-dimensional plane to obtain an actual local obstacle map; according to the pose information, only the map around the ground robot in the scene obstacle grid map is reserved to obtain a local obstacle prior map, and the actual local obstacle map and the local obstacle prior map are fused to obtain the local obstacle grid map so as to guide the ground robot to finish obstacle avoidance.

6. The octree structure-based local obstacle grid mapping system according to claim 6, wherein the initialization information for initializing the octree map in the initialization module comprises: the resolution of a minimum voxel leaf node, a maximum height threshold of barrier projection, a ray detection farthest updating threshold, the resolution of a local grid map and a local grid map storage range threshold;

The specific process of generating the simplified octree map in the initial module comprises the following steps:

traversing all nodes in the octree map to obtain node map coordinates; and subtracting the map coordinates of the node from the map coordinates of the camera to obtain a distance, and setting the node with the distance exceeding a preset threshold value as a null node.

7. The octree structure based local obstacle grid mapping system according to claim 6, wherein the three-dimensional point cloud in the transformation module is transformed to the map coordinate system by the following formula:

wherein X, Y and Z are coordinates of the three-dimensional point cloud after being transformed into a map coordinate system, X, Y and Z are coordinates of the three-dimensional point cloud in a camera coordinate system, and R is_m,cA rotation matrix representing a camera coordinate system relative to a map coordinate system; t is t_m,cA translation matrix representing a camera coordinate system relative to a map coordinate system; 0 represents a zero matrix vector of 1x 3;

the transformation module obtains the coordinate system transformation of the camera coordinate system relative to the map coordinate system by adopting the following formula:

T_m,c＝T_m,r*T_r,c

wherein T is_m,cRepresenting the transformation of the coordinate system of the RGB-D camera with respect to the coordinate system of the map, T_r,cRepresenting the transformation of the coordinate system of the camera with respect to the robot chassis, obtained by off-line calibration, T_m,rCoordinate system transformation, which indicates the robot chassis relative to the map coordinate system, is obtained from the robot pose information acquired in step S1.

8. The octree structure based local obstacle grid mapping system of claim 5, wherein the construction module is configured to insert the three-dimensional point cloud transformed to the map coordinate system into the reduced octree map by:

traversing all points of the three-dimensional point cloud converted into the map coordinate system, and calculating octree nodes through which a connecting line of the map coordinate of the camera and the map coordinate of each point passes; if the length of the connecting line is greater than the set ray detection farthest updating threshold, only calculating the connecting line of which the length of the connecting line is less than or equal to the updating threshold and considering the tail node as a passable node, otherwise, considering the tail node as an obstacle node; for each passing octree node, the passing times of the node is added with one, if the tail node of the connecting line is an obstacle node, the hitting times are added with one, otherwise, the passing times are added with one, and the occupation probability is updated by using the following formula:

wherein P is_occIndicates the probability of occupation, C_hitsIndicates the number of hits, C_missIndicating the number of passes.

The specific process of projecting the insertion result to a two-dimensional plane by the construction module to obtain the actual local obstacle map comprises the following steps:

setting the map coordinate of the ground robot as the original point of the actual local obstacle map;

Creating a blank barrier grid map according to the set grid map resolution and the local grid map storage range threshold, and assigning an initial value of-1 to each grid point on the map;

traversing all minimum voxel leaf nodes in the interpolation result octree map, and recording the value of the corresponding grid map point as 100 to indicate that the grid is occupied if the occupation probability of the minimum voxel leaf nodes is greater than the set probability value and the height value of the point map coordinate meets the set maximum height threshold of the barrier projection; otherwise, the grid is a passable area.

9. A storage medium storing a program for executing the octree structure-based local obstacle grid mapping method according to any one of claims 1 to 4.

10. A client for the octree structure-based local obstacle grid mapping system of any one of claims 5 to 8.

Images