Method and system for constructing local obstacle grid map based on octree structure

Abstract

The present invention proposes a method and system for constructing a local obstacle grid map based on an octree structure, including obtaining local obstacle map prior information based on a preloaded map, and then generating an actual local obstacle map based on collected data, and using the fusion of the two to provide an accurate and reliable obstacle information around the robot. Compared with previous map construction methods, the present invention has advantages in scenes with large human traffic and frequently changing environments: point clouds with a certain slope are converted to the xOy plane through ground detection, which is convenient for height threshold judgment and obstacle point cloud processing; the octree structure can dynamically update the local three-dimensional point cloud through the point cloud hit and pass probability, and has strong adaptability to scenes with a large number of people and dynamically changing environments; since only the local grid map around the robot is saved, the memory usage overhead is reduced, ensuring that resource consumption does not increase linearly with running time; compared with the laser radar solution, the present invention has the advantage of low cost.

Description

Technical Field

[0001] The present invention relates to the field of ground robot positioning, mapping and navigation, and in particular to a method for generating a local obstacle grid map based on an octree structure in the field of ground robot mapping and navigation. Background Art

[0002] With the increasing technological level, people are gradually using robots to replace humans to complete some highly repeatable tasks, such as sweeping robots, food delivery robots, patrol robots and storage robots, etc. Robots have the advantages of low long-term investment costs, no need for rest time, high work efficiency, and easy management. In recent years, they have been widely used in restaurant delivery, cargo handling, factory inspection and other areas, and the market is broad.

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

[0004] The existing local grid map construction methods based on octree structure generally have the following two processes:

[0005] 1) Through point cloud matching, the currently collected point cloud is fused with the point cloud of the previous frame. After the point cloud of the area of ​​interest (the local area centered on the unmanned vehicle) is cropped, the fused point cloud is downsampled and denoised, and the processed fused point cloud is inserted into the octree map and projected as a local grid map.

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

[0007] However, for the first method mentioned above, the point clouds fused at different times are inserted into the octree map at the same time. When the point clouds are matched, the point clouds of dynamic objects such as people and cars will be repeatedly superimposed. When a person passes by the robot, the obstacle point cloud will cover the passable area in front, resulting in path planning failure. Figure 6 As shown in the figure, the robot is in the lower left corner, the wall is in the upper right corner, and the circle in the middle represents a moving object, such as a person. The present 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 for high dynamic scenes; and uses the pre-built priori grid map as a supplement, focusing on real-time acquisition of the dynamic changes of obstacles in the area around the robot.

[0008] For the second method mentioned above, there will be a problem of information loss in the field of ground robots, such as Figure 7 As shown in the figure, without the assistance of previous information, when the ground robot continues to move forward, the previously existing obstacle information cannot be transferred to the current state, which will cause a collision accident. Summary of the invention

[0009] The purpose of the present invention is to overcome the problem in the above-mentioned prior art that when a ground robot is running in a high-dynamic scene, the traversable area ahead is often blocked by passing dynamic objects, and not utilizing previous point cloud information will cause the ground robot to collide with obstacles. A local obstacle grid map construction method based on an octree structure is proposed.

[0010] In view of the shortcomings of the prior art, the present invention proposes a method for constructing a local obstacle grid map based on an octree structure, which includes:

[0011] Step 1: Obtain a pre-built scene obstacle grid map and initialize the octree map; the ground robot collects depth information of the current environment through a camera, and the sensor inside the ground robot collects the posture information of the ground robot, and removes obstacle points in the octree map that exceed the set range according to the posture information to obtain a simplified octree map;

[0012] 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 posture information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into the map coordinate system;

[0013] Step 3: Use octree ray detection to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map, and project the insertion result onto a two-dimensional plane to obtain an actual local obstacle map; according to the posture information, only retain the map around the ground robot in the scene obstacle grid map to obtain a local obstacle prior map, and fuse the actual local obstacle map and the local obstacle prior map to obtain a local obstacle grid map to guide the ground machine to complete obstacle avoidance.

[0014] The local obstacle grid map construction method based on the octree structure, wherein the initialization information of the octree map initialized in step 1 includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the resolution size of the local grid map and the storage range threshold of the local grid map;

[0015] The specific process of generating the simplified octave map in step 1 includes:

[0016] Traverse all nodes in the octree map and obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to get the distance, and set the nodes whose distance exceeds the preset threshold as empty nodes.

[0017] In the method for constructing a local obstacle grid map based on an octree structure, in step 2, the three-dimensional point cloud is transformed into a map coordinate system by the following formula:

[0018]

[0019] Where X, Y, Z are the coordinates of the 3D point cloud after transformation to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, and R m,c Represents the rotation matrix of the camera coordinate system relative to the map coordinate system; 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;

[0020] 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:

[0021] T m,c =T M,r *T r,c

[0022] Where T m,c represents the coordinate transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

[0023] 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:

[0024] Traverse all points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula:

[0025]

[0026] Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of passes.

[0027] In step 3, the specific process of projecting the insertion result onto a two-dimensional plane to obtain the actual local obstacle map includes:

[0028] Set the map coordinates of the ground robot as the origin of the actual local obstacle map;

[0029] According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1;

[0030] All minimum voxel leaf nodes in the inserted result octree map are traversed. If the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinate of the point meets the set obstacle projection maximum height threshold, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

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

[0032] 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 inside the ground robot collects the posture information of the ground robot, and removes the obstacle points in the octree map that exceed the set range according to the posture information to obtain a simplified octree map;

[0033] A transformation module is used to convert the depth information into a three-dimensional point cloud, obtain a relative transformation of the camera relative to a map coordinate system according to the posture information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into the map coordinate system;

[0034] A construction module is used to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map by using octree ray detection, and project the insertion result onto a two-dimensional plane to obtain an actual local obstacle map; according to the posture information, only the map around the ground robot in the scene obstacle grid map is retained 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 to guide the ground machine to complete obstacle avoidance.

[0035] The local obstacle grid map construction system based on the octree structure, wherein the initialization information of the octree map initialized in the initial module includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the resolution size of the local grid map and the local grid map storage range threshold;

[0036] The specific process of generating the simplified octave map in the initial module includes:

[0037] Traverse all nodes in the octree map and obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to get the distance, and set the nodes whose distance exceeds the preset threshold as empty nodes.

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

[0039]

[0040] Where X, Y, Z are the coordinates of the 3D point cloud after transformation to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, and R m,c Represents the rotation matrix of the camera coordinate system relative to the map coordinate system; 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;

[0041] 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:

[0042] T m,c =T m, RT r,c

[0043] Where T m,c represents the coordinate transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

[0044] The local obstacle grid map construction system based on the octree structure, wherein the construction 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:

[0045] Traverse all points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula:

[0046]

[0047] Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of passes.

[0048] The specific process of projecting the insertion result onto a two-dimensional plane to obtain the actual local obstacle map in this construction module includes:

[0049] Set the map coordinates of the ground robot as the origin of the actual local obstacle map;

[0050] According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1;

[0051] All minimum voxel leaf nodes in the inserted result octree map are traversed. If the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinate of the point meets the set obstacle projection maximum height threshold, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

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

[0053] The present invention also proposes a client for use in any one of the local obstacle grid map construction systems based on an octree structure.

[0054] It can be seen from the above scheme that the advantages of the present invention are:

[0055] The present invention provides a local obstacle grid map construction method based on an octree structure, which obtains local obstacle map prior information according to a preloaded map, and then generates an actual local obstacle map according to the collected data, and uses the fusion of the two 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 human traffic 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 point cloud processing; secondly, the octree structure can dynamically update the local three-dimensional point cloud through the point cloud hit and pass probability, and has strong adaptability to scenes with a large number of people and dynamically changing environments; then, since only the local grid map around the robot is saved, the memory usage overhead is reduced, ensuring that resource consumption will not increase linearly with running time; in addition, compared with the solution using laser radar, the present invention has the advantage of low cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 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;

[0057] Figure 2 A schematic diagram of projecting a fitting plane provided by an embodiment of the present invention onto an xOy plane;

[0058] Figure 3 A schematic diagram of an octree map structure provided by an embodiment of the present invention;

[0059] Figure 4 A schematic diagram of an octree leaf node update method provided in an embodiment of the present invention;

[0060] Figure 5 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;

[0061] Figure 6 A schematic diagram is provided to illustrate the problems that may occur in high dynamic scenes in the prior art;

[0062] Figure 7 A schematic diagram is presented to illustrate possible problems in the prior art that do not utilize prior information. DETAILED DESCRIPTION

[0063] When the inventor was conducting research on the construction of local grid maps based on the octree structure, he found that in the prior art, when ground robots were operating in highly dynamic scenes, they often encountered the problem that the traversable area ahead was completely blocked by dynamic objects passing by. This was caused by first fusing the point cloud and then inserting it into the octree map. Not using the previous point cloud information would cause the ground robot to crash into obstacles. After research, the inventor found that the octree map can be updated using the connection between the ground robot's own position and the obstacle point cloud each time the point cloud information is obtained. This method will not lose the obstacle data information after losing the field of view, and can dynamically update the obstacle data information that has left the original position; thus perfectly solving the problems existing in the prior art.

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

[0065] S1, real-time acquisition of the depth information of the target environment and the position information of the robot collected by the RGB-D camera;

[0066] S2, load the pre-built scene obstacle grid map, initialize the octree structure, and the octree is an empty data structure at this time; wherein the pre-built scene obstacle grid map can be obtained by running other SLAM simultaneous positioning and mapping methods when there are no dynamic obstacles in the scene;

[0067] S3. According to the position information of the robot, removing obstacle points in the octree map that have exceeded a set range;

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

[0069] S5. Calculate the relative transformation of the camera relative to the map (scene obstacle grid map) coordinate system according to the posture information of the robot and the external parameters of the robot chassis relative to the camera, and transform the three-dimensional point cloud into the map coordinate system;

[0070] S6. Pre-acquire a priori obstacle grid map around the robot according to the position information of the robot;

[0071] S7, inserting the three-dimensional point cloud transformed into the map coordinate system into the octree map using octree ray detection;

[0072] S8, projecting the octree map onto a two-dimensional plane;

[0073] S9: Generate a local obstacle grid map that integrates prior scene information and current scene information.

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

[0075] 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 step S2 includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the local grid map resolution size and the local grid map storage range threshold.

[0076] In the method for constructing a local obstacle grid map based on an octree structure, the step S3 removes obstacle point clouds that are beyond a set range in the octree map by using the following method:

[0077] Traverse all occupied nodes in the octree map and obtain the map coordinates of the nodes; subtract the map coordinates of the node from the map coordinates of the camera. If the distance exceeds the preset local grid map storage range threshold, it is considered that the node is too far away from the robot, and the node is set to an empty node, and the occupation probability and map coordinates contained in the node are cleared.

[0078] The step S4 further comprises:

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

[0080] Downsample and denoise 3D point clouds;

[0081] The ground equation is fitted by random sampling consensus method RANSAC, and the point cloud is transformed to the xOy plane;

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

[0083]

[0084] Where X, Y, Z are the coordinates of the 3D point cloud after transformation 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 Represents the rotation matrix of the RGB-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; 0 represents a 1x3 zero matrix vector.

[0085] The step S5 uses the following formula to calculate the coordinate system transformation of the RGB-D camera coordinate system relative to the map coordinate system:

[0086] T m,c =T m,r *T r,c

[0087] Where T m,c represents the coordinate transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

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

[0089] Traverse all points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula:

[0090]

[0091] Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of passes.

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

[0093] Set the map coordinates of the robot as the origin of the local obstacle grid map;

[0094] According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1;

[0095] 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, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it means that the grid is a passable area.

[0096] The downsampling and denoising of the 3D point cloud include:

[0097] Downsample the point cloud using a voxel filter, where the size of the voxel filter is no larger than half the resolution of the smallest voxel leaf node of the octree map;

[0098] Use statistical filtering to remove outliers from point clouds.

[0099] In order to make the above features and effects of the present invention more clearly understood, embodiments are given below and described in detail with reference to the accompanying drawings.

[0100] The present invention provides a local obstacle grid map construction method based on an octree structure, which obtains local obstacle map prior information according to a preloaded map, and then generates an actual local obstacle map according to the collected data, and uses the fusion of the two 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 human traffic 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 point cloud processing; secondly, the octree structure can dynamically update the local three-dimensional point cloud through the point cloud hit and pass probability, and has strong adaptability to scenes with a large number of people and dynamically changing environments; then, since only the local grid map around the robot is saved, the memory usage overhead is reduced, ensuring that resource consumption will not increase linearly with running time; in addition, compared with the solution using laser radar, the present invention has the advantage of low cost.

[0101] Figure 1 A flowchart of a method for constructing a local obstacle grid map based on an octree structure provided in an embodiment of the present invention is provided below in conjunction with Figure 1 The technical solution of the present invention is described in detail.

[0102] Step 110, acquiring depth information of the target environment and position information of the robot collected by the RGB-D camera in real time;

[0103] Specifically, the RGB-D camera provides the depth map data of the target environment in real time, and the robot's position information can be obtained through the wheel speed meter data carried by the robot itself, or can be obtained based on global positioning sensors such as the global positioning system GPS, the ultra-wideband system UWB, or through the joint optimization of multiple sensor data. Because this method mainly uses the local obstacle map, it can still work normally when the positioning is slightly offset, and the RGB-D camera sensor can be one or more.

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

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

[0106] The resolution size of the smallest voxel leaf node;

[0107] The maximum height threshold of obstacle projection;

[0108] The farthest update threshold for ray detection;

[0109] Local grid map resolution size;

[0110] The local raster map save range threshold.

[0111] Figure 2 The figure shows a schematic diagram of the octree structure used in this method. After the point cloud is inserted, the octree will extend downward layer by layer until it reaches the set minimum voxel node. Figure 2 The large cube in the left image represents the root node, which is divided into 8 sub-cube spaces; the blank cube represents the leaf node with no point cloud inserted in the area, the black cube represents the smallest voxel leaf node, and the gray cube represents the intermediate node. The abstract structure of the octree is as follows Figure 2 As shown in the right figure, the hollow node represents the root node. Starting from the root node, eight child nodes extend downward, among which the gray nodes represent intermediate nodes, indicating that there is a minimum voxel leaf node in it; each intermediate node continues to divide downward into child nodes containing the minimum voxel leaf node until it is divided into the minimum voxel node; for each minimum voxel leaf node, it contains the occupancy probability of the leaf node and the corresponding map coordinates.

[0112] Step 130, removing obstacle points in the octree map that are beyond a set range according to the position and posture information of the robot;

[0113] Specifically, firstly, all occupied nodes in the octree map are traversed to obtain the map coordinates of the nodes; the map coordinates of the nodes are subtracted from the map coordinates of the camera. If the distance exceeds a preset threshold, it is considered that the node is too far away from the robot, and the node is set to an empty node, and the occupation probability and map coordinates and other information contained in the node are cleared.

[0114] Step 140, converting the target environment depth information into a three-dimensional point cloud, and preprocessing the three-dimensional point cloud;

[0115] Specifically, the depth information of the target environment is converted into a three-dimensional point cloud by the following formula:

[0116]

[0117] The X, Y, and Z on the left represent the coordinate information of the three-dimensional point cloud in the camera coordinate system. x 、f y 、c x 、c y is the intrinsic parameter of the camera, u and v represent the pixel coordinate information, and d represents the depth value corresponding to the pixel coordinate of the point.

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

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

[0120] Downsample and denoise 3D point clouds;

[0121] The ground equation is fitted by random sampling consensus method RANSAC and the point cloud is transformed to the xOy plane.

[0122] Among them, the point cloud is downsampled using a voxel filter, and the size of the voxel filter is not greater than half of the resolution size of the minimum voxel leaf node of the octree map; statistical filtering is used to remove outliers in the point cloud.

[0123] In fitting the ground, assuming that the road surface on which the robot is running has a certain slope (the slope is less than 45 degrees), three points are selected through the random sampling consensus method RANSAC to form a plane, and the number of points on the plane (that is, the points whose vertical distance to the plane is less than a certain threshold) is calculated. This operation is repeated continuously, and the model with the most points on the plane is selected as the final result. After the random sampling consensus method RANSAC obtains the plane equation of the ground, the plane's normal vector n 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:

[0124]

[0125]

[0126] Figure 2 What is shown is a schematic diagram of projecting the fitting plane onto the xOy plane, where plane π is the ground plane fitted by the random sampling consensus method RANSAC, the rotation axis n' is the intersection of plane π and the xOy plane; the rotation angle θ is the angle between plane π and the xOy plane; rotating plane π around the rotation axis n' by θ degrees can achieve the desired effect.

[0127] After obtaining the rotation axis n' and the rotation angle θ, the rotation vector is θn'. The rotation vector is converted into a rotation matrix, and all points are rotated using the following formula:

[0128]

[0129] Among them, X, Y, and Z are the coordinates of the three-dimensional point cloud after transforming the ground to the xOy plane, and x, y, and z are the coordinates of the original three-dimensional point cloud in the camera coordinate system.

[0130] Step 150, calculating the relative transformation of the camera relative to the map coordinate system according to the posture information of the robot and the external parameters of the robot chassis relative to the camera, and transforming the three-dimensional point cloud into the map coordinate system;

[0131] The relative transformation of the camera with respect to the map coordinate system is calculated as follows:

[0132] T m,c =T m,r *T r,c

[0133] Where T m,c Indicates the coordinate transformation of the RGB-D camera coordinate system relative to the map coordinate system; T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration; T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information read in step 110.

[0134] After calculating the relative transformation T of the camera relative to the map coordinate system m,c Finally, the 3D point cloud is transformed from the map coordinate system to the world coordinate system using the following formula:

[0135]

[0136] Where X, Y, and Z are the coordinates of the three-dimensional point cloud after the ground is transformed into the map coordinate system, x, y, and z are the coordinates of the three-dimensional point cloud in the camera after the three-dimensional point cloud is transformed to the xOy plane in step 140, and R m,cRepresents the rotation matrix of the RGB-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.

[0137] Step 160, pre-acquiring a priori obstacle grid map around the robot according to the position and posture information of the robot;

[0138] Specifically, firstly, the map coordinates of the robot are obtained through the robot posture information;

[0139] The map coordinates of the robot are used as the origin of the local obstacle grid map. A blank obstacle map is created according to the set grid map resolution and local map size. Each point on the map is initialized to -1.

[0140] Overlay the grid map values ​​corresponding to the pre-built scene map onto the blank local obstacle grid map to obtain a priori grid map;

[0141] Step 170, inserting the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map by using octree ray detection;

[0142] Specifically, the method of inserting a 3D point cloud using octree ray detection is as follows:

[0143] Traverse all points of the three-dimensional point cloud converted to the map coordinate system, calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; for each octree node passed, 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 passes is increased by one, and the occupancy probability is updated using the following formula:

[0144]

[0145] Preferably, if P occ If the value of is greater than 0.6, the point is considered to be an obstacle point, otherwise it is considered to be a passable point.

[0146] Figure 3 The above method is used to represent the update method of the leaf nodes in the octree structure. The actual update is performed on the three-dimensional space points. For the convenience of explanation, it is displayed in two-dimensional space. The black square in the upper right corner represents the point cloud point to be inserted. The point P in the lower left corner is the current position of the robot. The line connecting the robot position and the point cloud point to be inserted is calculated. The octree nodes that the line passes through are represented by white squares. For the octree nodes corresponding to the white squares, the number of times they pass through is C. miss Add one and update the corresponding occupancy probability P occ; For the octree node corresponding to the black square, its hit count is increased by 1, and the corresponding occupancy probability P is updated occ If the node was originally hit 2 times and passed through 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.

[0147] Step 180, projecting the octree map onto a two-dimensional plane;

[0148] Specifically, the octree map is projected onto a two-dimensional plane in the following way:

[0149] Set the map coordinates of the robot as the origin of the local obstacle grid map;

[0150] 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;

[0151] 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 coordinate of the point meets the set height threshold, the value of the corresponding grid map coordinate is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

[0152] Step 190, generating a local obstacle grid map integrating prior scene information and current scene information;

[0153] 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, that is, the unknown area, it is replaced with the map value corresponding to the prior grid map to obtain more comprehensive obstacle information around the robot.

[0154] Figure 4 The process of fusing the current grid map and the prior grid map is shown; the upper left corner is the current grid map generated by step 180, which describes the scene information currently seen by the robot, where black pixels represent obstacles, white pixels represent passable areas, and gray pixels represent unknown areas. It can be seen that there are large unknown areas 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 arrangement of tables and chairs at that time is somewhat different from that of now; 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 a more comprehensive distribution of obstacles around the robot, and can be updated and corrected in real time.

[0155] The present invention provides a method for constructing a local obstacle grid map based on an octree structure. It obtains local obstacle map prior information based on a preloaded map, and then generates an actual local obstacle map based on collected data, and uses the fusion of the two to provide an accurate and reliable obstacle information around the robot. Compared with previous map construction methods, the present invention has great advantages in scenes with large human traffic and frequently changing environments: first, the octree structure can dynamically update the local three-dimensional point cloud through the point cloud hit and pass probability, and has strong adaptability to scenes with a large number of people and dynamically changing environments; second, because only the local grid map around the robot is saved, the memory usage overhead is reduced, ensuring that resource consumption does not increase linearly with running time.

[0156] The following is a system embodiment corresponding to the above method embodiment. This embodiment can be implemented in conjunction with the above embodiment. The relevant technical details mentioned in the above embodiment are still valid in this embodiment. In order to reduce repetition, they are not repeated here. Accordingly, the relevant technical details mentioned in this embodiment can also be applied in the above embodiment.

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

[0158] 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 inside the ground robot collects the posture information of the ground robot, and removes the obstacle points in the octree map that exceed the set range according to the posture information to obtain a simplified octree map;

[0159] A transformation module is used to convert the depth information into a three-dimensional point cloud, obtain a relative transformation of the camera relative to a map coordinate system according to the posture information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into the map coordinate system;

[0160] A construction module is used to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map by using octree ray detection, and project the insertion result onto a two-dimensional plane to obtain an actual local obstacle map; according to the posture information, only the map around the ground robot in the scene obstacle grid map is retained 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 to guide the ground machine to complete obstacle avoidance.

[0161] The local obstacle grid map construction system based on the octree structure, wherein the initialization information of the octree map initialized in the initial module includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the resolution size of the local grid map and the local grid map storage range threshold;

[0162] The specific process of generating the simplified octave map in the initial module includes:

[0163] Traverse all nodes in the octree map and obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to get the distance, and set the nodes whose distance exceeds the preset threshold as empty nodes.

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

[0165]

[0166] Where X, Y, Z are the coordinates of the 3D point cloud after transformation to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, and R m,c Represents the rotation matrix of the camera coordinate system relative to the map coordinate system; 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;

[0167] 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:

[0168] T m,c =T m,r *T r,c

[0169] Where T m,c represents the coordinate transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

[0170] The local obstacle grid map construction system based on the octree structure, wherein the construction 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:

[0171] Traverse all points of the three-dimensional point cloud converted to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula:

[0172]

[0173] Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of passes.

[0174] The specific process of projecting the insertion result onto a two-dimensional plane to obtain the actual local obstacle map in this construction module includes:

[0175] Set the map coordinates of the ground robot as the origin of the actual local obstacle map;

[0176] According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1;

[0177] All minimum voxel leaf nodes in the inserted result octree map are traversed. If the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinate of the point meets the set obstacle projection maximum height threshold, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

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

[0179] The present invention also proposes a client for use in any one of the local obstacle grid map construction systems based on an octree structure.

[0180] It should be further appreciated by professionals 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 the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and implementation steps of each example are described in detail in the above description according to the generality of functions. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present invention.

[0181] The steps of the method or algorithm described in conjunction with the embodiments disclosed herein may be implemented using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

[0182] The above specific implementation methods further illustrate the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above are only specific implementation methods of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for constructing a local obstacle grid map based on an octree structure, characterized in that: include: Step 1: Get the pre-built scene obstacle grid map and initialize the octree map; The ground robot collects depth information of the current environment through a camera, and the sensors inside the ground robot collect the posture information of the ground robot, and according to the posture information, removes obstacle points in the octree map that exceed the set range to obtain 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 posture information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into the 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 octree map, and project the insertion result onto a two-dimensional plane to obtain an actual local obstacle map; according to the posture information, only the map around the ground robot in the scene obstacle grid map is retained 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 to guide the ground machine to complete obstacle avoidance; In step 3, the three-dimensional point cloud transformed into the map coordinate system is inserted into the simplified octave map in the following manner: Traverse all points of the three-dimensional point cloud transformed to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula: Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of times it has been passed through; In step 3, the specific process of projecting the insertion result onto a two-dimensional plane to obtain the actual local obstacle map includes: Set the map coordinates of the ground robot as the origin of the actual local obstacle map; According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1; All minimum voxel leaf nodes in the inserted result octree map are traversed. If the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinate of the point meets the set obstacle projection maximum height threshold, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

2. The method for constructing a local obstacle grid map based on an octree structure according to claim 1, characterized in that: The initialization information of the octree map initialized in step 1 includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the resolution size of the local grid map and the storage range threshold of the local grid map; The specific process of generating the simplified octave map in step 1 includes: Traverse all nodes in the octree map and obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to get the distance, and set the nodes whose distance exceeds the preset threshold as empty nodes.

3. The method for constructing a local obstacle grid map based on an octree structure according to claim 1, characterized in that: In step 2, the 3D point cloud is transformed into the map coordinate system using the following formula: Where X, Y, Z are the coordinates of the 3D point cloud after transformation to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, and R m,c Represents the rotation matrix of the camera coordinate system relative to the map coordinate system; 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 transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

4. A local obstacle grid map construction system based on an octree structure, characterized in that: include: The initialization module is used to obtain the pre-built scene obstacle grid map and initialize the octree map; The ground robot collects depth information of the current environment through a camera, and the sensors inside the ground robot collect the posture information of the ground robot, and according to the posture information, removes obstacle points in the octree map that exceed the set range to obtain a simplified octree map; A transformation module is used to convert the depth information into a three-dimensional point cloud, obtain a relative transformation of the camera relative to a map coordinate system according to the posture information and the external parameters of the ground robot chassis relative to the camera, and transform the three-dimensional point cloud into the map coordinate system; A construction module is used to insert the three-dimensional point cloud transformed into the map coordinate system into the simplified octree map by using octree ray detection, and project the insertion result onto a two-dimensional plane to obtain an actual local obstacle map; according to the posture information, only the map around the ground robot in the scene obstacle grid map is retained 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 to guide the ground machine to complete obstacle avoidance; This building block is used to insert the 3D point cloud transformed to the map coordinate system into the simplified octave map in the following way: Traverse all points of the three-dimensional point cloud transformed to the map coordinate system, and calculate the octree nodes passed by the line connecting the map coordinates of the camera and the map coordinates of each point; if the line length is greater than the set ray detection farthest update threshold, only the line length is less than or equal to the update threshold and the end node is considered to be a passable node, otherwise it is considered to be an obstacle node; for each octree node passed, the number of times the node is passed is increased by one, if the end node of the line is an obstacle node, the number of hits is increased by one, otherwise the number of passes is increased by one, and the occupancy probability is updated using the following formula: Where P occ represents the occupancy probability, C hits Indicates the number of hits, C miss Indicates the number of times it has been passed through; The specific process of projecting the insertion result onto a two-dimensional plane to obtain the actual local obstacle map in this construction module includes: Set the map coordinates of the ground robot as the origin of the actual local obstacle map; According to the set grid map resolution and local grid map preservation range threshold, a blank obstacle grid map is created, and each grid point on the map is assigned an initial value of -1; All minimum voxel leaf nodes in the inserted result octree map are traversed. If the occupancy probability of the minimum voxel leaf node is greater than the set probability value and the height value of the map coordinate of the point meets the set obstacle projection maximum height threshold, the value of the corresponding grid map point is recorded as 100, indicating that the grid is occupied; otherwise, it indicates that the grid is a passable area.

5. The local obstacle grid map construction system based on octree structure as claimed in claim 4, characterized in that: The initialization information of the octree map in the initialization module includes: the resolution size of the minimum voxel leaf node, the maximum height threshold of the obstacle projection, the farthest update threshold of the ray detection, the resolution size of the local grid map and the storage range threshold of the local grid map; The specific process of generating the simplified octave map in the initial module includes: Traverse all nodes in the octree map and obtain the node map coordinates; subtract the node map coordinates from the camera's map coordinates to get the distance, and set the nodes whose distance exceeds the preset threshold as empty nodes.

6. The local obstacle grid map construction system based on octree structure as claimed in claim 4, characterized in that: In the transformation module, the 3D point cloud is transformed to the map coordinate system using the following formula: Where X, Y, Z are the coordinates of the 3D point cloud after transformation to the map coordinate system, x, y, z are the coordinates of the 3D point cloud in the camera coordinate system, and R m,c Represents the rotation matrix of the camera coordinate system relative to the map coordinate system; 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 transformation of the RGB-D camera coordinate system relative to the map coordinate system, T r,c represents the coordinate transformation of the camera relative to the robot chassis, obtained through offline calibration, T m,r The coordinate system transformation of the robot chassis relative to the map coordinate system is obtained through the robot posture information obtained in step S1.

7. A storage medium for storing a program for executing any one of the methods for constructing a local obstacle grid map based on an octree structure as claimed in claims 1 to 3.

8. A client, used in any one of the local obstacle grid map construction systems based on an octree structure as claimed in claims 4 to 6.

Images