A three-dimensional dense mapping method based on ORB-SLAM2 and monocular depth camera

Abstract

This invention provides a 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera, comprising: estimating the camera's state using ORB-SLAM2 to obtain the position and related pose information of the monocular depth camera; simultaneously reading the monocular depth image from the monocular depth camera and publishing topics on both; using the position and pose of the depth camera, the monocular depth image, and the monocular color image as input, and employing a posterior probability-based method to utilize the point cloud information generated from the depth image to construct a 3D dense map; recursively expanding the grid of the 3D dense map as the root node to construct an octree map; for idle obstacle grids, using the Euclidean distance algorithm to calculate the distance between the idle grid and the nearest obstacle grid, further constructing a signed Euclidean distance map; and finally displaying it using an NVIDIA edge intelligence development board and a monitor.

Description

Technical Field

[0001] This invention relates to the field of robot simultaneous localization and mapping technology, and more particularly to a method for dense 3D mapping based on ORB-SLAM2 and a monocular depth camera. Background Technology

[0002] With the rapid development of simultaneous localization and mapping (SLT / AR) technology, its application in robotics has become particularly prominent in recent years. Simultaneously, the emergence of increasingly sophisticated edge devices, including embedded cameras, inertial measurement units (IMUs), vision-based SLT / AR, and vision-and-inertial combined SLT / AR, are continuously improving and developing perception technologies in the robotics field. Whether at the fundamental or experimental levels, visual estimation of robot states is crucial and widely used in autonomous robot navigation. By acquiring visual information and depth information from the surrounding environment through a front-end camera, SLT / AR algorithms estimate the robot's pose, and local optimization methods are used to optimize the pose. Keyframes in the visual information are selected, output, and used to construct navigation map information. Frame matching provides a reference transformation matrix for the robot. Therefore, in unknown environments, robots using vision-based SLT / AR can complete path planning and construct and save maps with environmental visual features, even at low speeds, which is of great significance. To meet different robot navigation needs, various types of maps can be constructed, such as octree maps with expanded obstacles or Euclidean distance maps with symbols. This provides more information for autonomous robot navigation, further optimizing and refining robot path planning techniques.

[0003] Most existing ORB-SLAM2 algorithms rely on outputting keyframes to construct sparse point cloud maps. However, current map-building algorithms suffer from the following problems:

[0004] Sparse point cloud maps constructed based on keyframe information cannot meet the requirements for autonomous navigation of robots or autonomous driving of vehicles in unknown and complex environments. The constructed obstacle information is missing, which easily leads to collision hazards.

[0005] The information used to build the map has not been further improved or perfected, such as expanding obstacles or constructing a distance field map to provide more information about the environment, such as distance or gradient, which cannot meet the needs of the backend path planning algorithm that optimizes based on gradient.

[0006] Third, for algorithms that perform dense 3D mapping based on visual information, most rely on monocular cameras and combined inertial navigation systems, or on binocular cameras and inertial navigation systems, which are costly and require joint calibration, making debugging difficult. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera. The invention utilizes ORB-SLAM2 for pose estimation via a front-end odometry thread, a local map thread, and a loop closure detection thread. Taking the depth camera's position and pose, monocular depth image, and monocular color image as input, it constructs a global map in three map formats using a fast incremental algorithm. This is achieved through a 3D dense map constructed based on posterior probability estimation, a signed Euclidean distance field constructed based on a trilinear interpolation algorithm, and an octree map for obstacle dilation.

[0008] The technical means employed in this invention are as follows:

[0009] A method for dense 3D mapping based on ORB-SLAM2 and a monocular depth camera includes:

[0010] The camera's state is estimated using ORB-SLAM2 to obtain the position and related pose information of the monocular depth camera. Simultaneously, the monocular depth image of the monocular depth camera is read, and both are published as topics.

[0011] Using the position and pose of the depth camera, monocular depth images, and monocular color images as input, a posterior probability-based method is used to construct a 3D dense map using the point cloud information generated by the transformation of the depth images.

[0012] An octree map is constructed by recursively expanding the raster of a 3D dense map as the root node.

[0013] For idle obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the idle grid and the nearest obstacle grid. A signed Euclidean distance map is then constructed, and finally displayed using NVIDIA's edge intelligence development board and a monitor.

[0014] Furthermore, the step of using the position and pose of the depth camera, the monocular depth image, and the monocular color image as input, and employing a posterior probability-based method to utilize the point cloud information generated from the depth image to construct a dense 3D map includes:

[0015] The obtained depth image is transformed into a point cloud, and the transformed point cloud is divided into a grid. Each grid has three states: occupied, idle, and unknown.

[0016] Each grid cell is independent and has the same distribution; suppose the overall map is composed of an infinite number of grid cells obtained from the division, expressed in the form m = {m0, m1, ..., m}. n For the state of a grid, the probability of whether it is occupied follows a normal distribution with the number of measurements.

[0017] Given a sufficiently large number of measurements, the mean of the normal distribution is assumed to be the true value; the variance of the normal distribution depends on the properties of the depth camera itself.

[0018] Let p(m) be the probability that each grid cell is occupied. i =1), the probability of being idle is p(m) i =0), the probability of an unknown grid cell is set to 0.5;

[0019] Let x be the camera pose sequence and observation sequence obtained from ORB-SLAM2. 1:t and z 1:t The posterior probability of solving the map is p(m|x). 1:t ,z 1:t );

[0020] Assuming the predicted map grid state is static, and each grid has two possible states, a static binary Bayesian filter and Markov assumption method are used to estimate the grid state and calculate the map posterior probability. The calculation formula is shown below:

[0021]

[0022] Similarly, the probability formula for an empty grid can be obtained as follows:

[0023]

[0024] Let m be the grid i The observation result of the t-th depth camera is denoted as l. t (m i The observation results of the depth camera in the first t-1 iterations are denoted as l. t-1 (m i Then, the above equation can be rearranged to obtain the following equation:

[0025]

[0026] Given that for the monocular depth camera used, based on the continuously updated depth image information, the t-th observation is performed on the basis of the (t-1)-th observation, thus obtaining a dense three-dimensional point cloud map.

[0027] Furthermore, the recursive expansion of the octree map using the grid of the 3D dense map as the root node includes:

[0028] Set the maximum recursion depth of the octree map, find the maximum size of the scene, and build the first cube with the maximum size. Then, put the grid elements in the dense point cloud map into the cube without child nodes in sequence.

[0029] If the maximum recursion depth is not reached, the cube is subdivided into eight equal parts, and the dense point cloud contained in the cube is then distributed to the eight sub-cubes in turn.

[0030] If the number of dense point clouds assigned to a sub-cube is not zero and is the same as that of the parent cube, then the sub-cube is stopped from being subdivided.

[0031] Repeat the operation on the grid elements until the set recursion depth is reached;

[0032] When all child nodes of a node are "occupied", "not occupied", or "undetermined", prune that node. When most grid cells are idle, use a larger square to represent the map to reduce storage space.

[0033] Assume discrete time T = {1, 2, 3, ..., t}, and observed data z = {z0, z1, ..., z} n The calculation of the nth leaf node is as follows:

[0034]

[0035] Meanwhile, the probability of the root node of the octree map can be calculated from the corresponding values ​​of the leaf nodes.

[0036] Furthermore, for idle obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the idle grid and the nearest obstacle grid, and a signed Euclidean distance map is further constructed, including:

[0037] In the grid map that has been divided, calculate the Euclidean distance from each unoccupied grid to each occupied grid in the x-axis direction. Compare each distance and take the minimum value as the distance value of the free grid in the x-axis direction. At the same time, take the distance of the row of free grids without obstacles in the x-axis direction as an infinite value.

[0038] Based on the existing obstacle grid information and distance information, calculate the distance of the grid with distance value or the obstacle grid in the y-axis direction for the free grid in turn. Similarly, take the distance of the free grid without obstacles in the y-axis direction as an infinite value.

[0039] The distance along the z-axis is calculated using the same method as above to obtain the distance field of the free grid in the three-dimensional plane. To ensure the safety of path planning during the path planning process, it is usually required that grids with obstacles provide a reverse distance field.

[0040] Calculate the distance of the obstacle grid to the nearest free grid in the 3D plane using the method described above, take its negative value, and superimpose it with the previous distance field to obtain a signed Euclidean distance map. The calculation formula is as follows:

[0041] D(p)=min{(po) 2 +f(o)}

[0042] Where p is an empty grid, o is an occupied grid, and f(o) is the existing distance value in the empty grid. Finally, a fixed plane is selected to display the Euclidean distance field.

[0043] Compared with the prior art, the present invention has the following advantages:

[0044] 1. Compared with existing ORB-SLAM2-based simultaneous localization and real-time mapping methods, this method constructs a dense point cloud map, which is more suitable for the needs of autonomous robot navigation. At the same time, the dense point cloud map provides richer obstacle information.

[0045] 2. Compared with existing mapping methods based on binocular vision or a combination of monocular vision and inertial measurement unit, this invention only requires a monocular depth camera to map unknown environments, eliminating the need for joint calibration of the monocular camera and inertial measurement unit.

[0046] 3. Compared with existing dense point cloud mapping algorithms, this invention further constructs a Euclidean distance map and an octree map, providing more usable information for path planning, such as distance and gradient. This creates conditions for constructing complex optimization problems in complex environments.

[0047] Based on the above reasons, this invention can be widely applied in fields such as robot synchronous localization and map building. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0049] Figure 1 This is a flowchart of the method of the present invention.

[0050] Figure 2 This is a schematic diagram of the environment constructed by a monocular depth camera provided in an embodiment of the present invention.

[0051] Figure 3This is a schematic diagram of the three-dimensional dense grid map constructed in this invention.

[0052] Figure 4 This is a schematic diagram of the octree map constructed in this invention.

[0053] Figure 5 This is a velocity diagram of the Euclidean distance map constructed in this invention. Detailed Implementation

[0054] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0055] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0057] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0058] In the description of this invention, it should be understood that the orientation or positional relationship indicated by directional terms such as "front, back, up, down, left, right", "horizontal, vertical, horizontal" and "top, bottom" is generally based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing this invention and simplifying the description. Unless otherwise stated, these directional terms do not indicate or imply that the device or element referred to must have a specific orientation or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the scope of protection of this invention. The directional terms "inner" and "outer" refer to the inner and outer contours relative to the outline of each component itself.

[0059] For ease of description, spatial relative terms such as "above," "over," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation besides the orientation of the device as described in the figures. For example, if the device in the figures is inverted, a device described as "above" or "above" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein will be interpreted accordingly.

[0060] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore should not be construed as limiting the scope of protection of this invention.

[0061] like Figure 1 As shown, this invention provides a 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera, including:

[0062] Based on ORB-SLAM2, the camera's state is estimated to obtain the position and related pose information of the monocular depth camera. Simultaneously, the monocular depth image from the monocular depth camera is read, and topic publication is performed on both. Figure 2 The diagram illustrates the environment constructed by a monocular depth camera. In this embodiment, the ORB-SLAM2 algorithm first utilizes image pyramid construction, feature point extraction, feature point homogenization, feature point principal orientation calculation, Gaussian blurring, descriptor calculation, image distortion correction, calculation of the image boundary after distortion correction, and determination of the image grid containing the feature points to estimate the position and camera pose. Topics are then published based on the visual information acquired by the monocular depth camera each time and the camera's position and pose.

[0063] Using the position and pose of the depth camera, monocular depth images, and monocular color images as input, a posterior probability-based method is employed to construct a dense 3D map using point cloud information derived from the depth images; for example... Figure 3 The image shows the constructed 3D dense grid map. In this embodiment, a method based on posterior probability estimation is used. According to the Markov assumption, the observation results obtained from each observation are independent. The 3D dense map is updated based on the depth images acquired by the monocular depth camera each time, by inserting and deleting obstacles within the camera's range. Specifically, in a preferred embodiment of the present invention, the step of using the depth camera's position and pose, monocular depth images, and monocular color images as inputs, and using the point cloud information formed by the depth images based on posterior probability to construct a 3D dense map includes:

[0064] The obtained depth image is transformed into a point cloud, and the transformed point cloud is divided into a grid. Each grid has three states: occupied, idle, and unknown.

[0065] Each grid cell is independent and has the same distribution; suppose the overall map is composed of an infinite number of grid cells obtained from the division, expressed in the form m = {m0, m1, ..., m}. n For the state of a grid, the probability of whether it is occupied follows a normal distribution with the number of measurements.

[0066] Given a sufficiently large number of measurements, the mean of the normal distribution is assumed to be the true value; the variance of the normal distribution depends on the properties of the depth camera itself.

[0067] Let p(m) be the probability that each grid cell is occupied. i =1), the probability of being idle is p(m) i =0), the probability of an unknown grid cell is set to 0.5;

[0068] Let x be the camera pose sequence and observation sequence obtained from ORB-SLAM2. 1:t and z 1:t The posterior probability of solving the map is p(m|x). 1:t ,z 1:t );

[0069] Assuming the predicted map grid state is static, and each grid has two possible states, a static binary Bayesian filter and Markov assumption method are used to estimate the grid state and calculate the map posterior probability. The calculation formula is shown below:

[0070]

[0071] Similarly, the probability formula for an empty grid can be obtained as follows:

[0072]

[0073] Let m be the grid i The observation result of the t-th depth camera is denoted as l. t (m i The observation results of the depth camera in the first t-1 iterations are denoted as l. t-1 (m i Then, the above equation can be rearranged to obtain the following equation:

[0074]

[0075] Given that for the monocular depth camera used, based on the continuously updated depth image information, the t-th observation is performed on the basis of the (t-1)-th observation, thus obtaining a dense three-dimensional point cloud map.

[0076] An octree map is constructed by recursively expanding the raster of a dense 3D map using the root node; for example... Figure 4 The diagram shows the constructed octree map. In this embodiment, two independent update queues are introduced, and an index data structure and a doubly linked list are used for map maintenance. The octree map is constructed using densely occupied graticules as root nodes and unoccupied graticules as leaf nodes. Specifically, in a preferred embodiment of this invention, the recursive expansion of the 3D dense map's graticules as root nodes to construct the octree map includes:

[0077] An octree is defined as a tree with eight child nodes. In practice, the octree data structure is a structure where the root node expands continuously, each expansion producing eight leaf nodes, where the leaf nodes represent the highest resolution. The construction steps are as follows: Set the maximum recursion depth of the octree map, find the maximum size of the scene, and build the first cube with the maximum size. Then, sequentially place the raster elements from the dense point cloud map into the cubes without child nodes.

[0078] If the maximum recursion depth is not reached, the cube is subdivided into eight equal parts, and the dense point cloud contained in the cube is then distributed to the eight sub-cubes in turn.

[0079] If the number of dense point clouds assigned to a sub-cube is not zero and is the same as that of the parent cube, then the sub-cube is stopped from being subdivided.

[0080] Repeat the operation on the grid elements until the set recursion depth is reached;

[0081] When all child nodes of a node are "occupied", "not occupied", or "undetermined", prune that node. When most grid cells are idle, use a larger square to represent the map to reduce storage space.

[0082] Assume discrete time T = {1, 2, 3, ..., t}, and observed data z = {z0, z1, ..., z} n The calculation of the nth leaf node is as follows:

[0083]

[0084] Meanwhile, the probability of the root node in the octree map can be calculated from the corresponding values ​​of the leaf nodes. This process constitutes the construction of the octree map.

[0085] For idle obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the idle grid and the nearest obstacle grid. This is then used to construct a signed Euclidean distance map, which is finally displayed using an NVIDIA edge intelligence development board and a monitor. Figure 5 The image shows the constructed Euclidean distance map. In a preferred embodiment of this invention, for empty obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the empty grid and the nearest obstacle grid, further constructing a signed Euclidean distance map, including:

[0086] Based on the dense point cloud map, another thread, operating in parallel with the octree map construction, is the signed Euclidean distance map construction. In the pre-divided grid map, the Euclidean distance from each unoccupied grid to each occupied grid is calculated sequentially along the x-axis. Each distance is compared, and the minimum value is taken as the distance value along the x-axis for the corresponding free grid. Simultaneously, the distance of the row of free grids without obstacles along the x-axis is set to infinity.

[0087] Based on the existing obstacle grid information and distance information, calculate the distance of the grid with distance value or the obstacle grid in the y-axis direction for the free grid in turn. Similarly, take the distance of the free grid without obstacles in the y-axis direction as an infinite value.

[0088] The distance along the z-axis is calculated using the same method as above to obtain the distance field of the free grid in the three-dimensional plane. To ensure the safety of path planning during the path planning process, it is usually required that grids with obstacles provide a reverse distance field.

[0089] Calculate the distance of the obstacle grid to the nearest free grid in the 3D plane using the method described above, take its negative value, and superimpose it with the previous distance field to obtain a signed Euclidean distance map. The calculation formula is as follows:

[0090] D(p)=min{(po) 2 +f(o)}

[0091] Where p is an empty grid, o is an occupied grid, and f(o) is the existing distance value in the empty grid. Finally, a fixed plane is selected to display the Euclidean distance field.

[0092] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for dense 3D mapping based on ORB-SLAM2 and a monocular depth camera, characterized in that, include: The camera's state is estimated using ORB-SLAM2 to obtain the position and related pose information of the monocular depth camera. Simultaneously, the monocular depth image of the monocular depth camera is read, and both are published as topics. Using the position and pose of the depth camera, monocular depth images, and monocular color images as input, a posterior probability-based method is used to construct a 3D dense map using the point cloud information generated by the transformation of the depth images. An octree map is constructed by recursively expanding the raster of a 3D dense map as the root node. For idle obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the idle grid and the nearest obstacle grid. A signed Euclidean distance map is then constructed, and finally displayed using NVIDIA's edge intelligence development board and a monitor.

2. The 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera according to claim 1, characterized in that, The process involves taking the position and pose of the depth camera, monocular depth images, and monocular color images as input, and using a posterior probability-based method to construct a dense 3D map from the point cloud information generated by the depth image transformation, including: The obtained depth image is transformed into a point cloud, and the transformed point cloud is divided into a grid. Each grid has three states: occupied, idle, and unknown. Each grid cell is independent and has the same distribution; let the overall map be composed of an infinite number of grid cells obtained from the division, expressed in the form m={m0,m1,...,m...} n For the state of a grid, the probability of whether it is occupied follows a normal distribution with the number of measurements. Given a sufficiently large number of measurements, the mean of the normal distribution is assumed to be the true value; the variance of the normal distribution depends on the properties of the depth camera itself. Let p(m) be the probability that each grid cell is occupied. i =1), the probability of being idle is p(m) i =0), the probability of an unknown grid cell is set to 0.5; Let x be the camera pose sequence and observation sequence obtained from ORB-SLAM2. 1:t and z 1:t The posterior probability of solving the map is p(m|x). 1:t ,z 1:t ); Assuming the predicted map grid state is static, and each grid has two possible states, a static binary Bayesian filter and Markov assumption method are used to estimate the grid state and calculate the map posterior probability. The calculation formula is as follows: Similarly, the probability formula for an empty grid can be obtained as follows: Let m be the grid i The observation result of the t-th depth camera is denoted as l. t (m i The observation results of the depth camera in the first t-1 iterations are denoted as l. t-1 (m i Then, the above equation can be rearranged to obtain the following equation: Given that for the monocular depth camera used, based on the continuously updated depth image information, the t-th observation is performed on the basis of the (t-1)-th observation, thus obtaining a dense three-dimensional point cloud map.

3. The 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera according to claim 1, characterized in that, The recursive expansion of the octree map, using the grid of the 3D dense map as the root node, to construct the octree map includes: Set the maximum recursion depth of the octree map, find the maximum size of the scene, and build the first cube with the maximum size. Then, put the grid elements in the dense point cloud map into the cube without child nodes in sequence. If the maximum recursion depth is not reached, the cube is subdivided into eight equal parts, and the dense point cloud contained in the cube is then distributed to the eight sub-cubes in turn. If the number of dense point clouds assigned to a sub-cube is not zero and is the same as that of the parent cube, then the sub-cube is stopped from being subdivided. Repeat the operation on the grid elements until the set recursion depth is reached; When all child nodes of a node are "occupied", "not occupied", or "undetermined", prune that node. When most grid cells are idle, use a larger square to represent the map to reduce storage space. Assume discrete time T = {1, 2, 3, ..., t}, and observed data z = {z0, z1, ..., z} n The calculation of the nth leaf node is as follows: Meanwhile, the probability of the root node of the octree map can be calculated from the corresponding values ​​of the leaf nodes.

4. The 3D dense mapping method based on ORB-SLAM2 and a monocular depth camera according to claim 1, characterized in that, For idle obstacle grids, the Euclidean distance algorithm is used to calculate the distance between the idle grid and the nearest obstacle grid, and a signed Euclidean distance map is further constructed, including: In the grid map that has been divided, calculate the Euclidean distance from each unoccupied grid to each occupied grid in the x-axis direction. Compare each distance and take the minimum value as the distance value of the free grid in the x-axis direction. At the same time, take the distance of the row of free grids without obstacles in the x-axis direction as an infinite value. Based on the existing obstacle grid information and distance information, calculate the distance of the grid with distance value or the obstacle grid in the y-axis direction for the free grid in turn. Similarly, take the distance of the free grid without obstacles in the y-axis direction as an infinite value. The distance along the z-axis is calculated using the same method as above to obtain the distance field of the free grid in the three-dimensional plane. To ensure the safety of path planning during the path planning process, it is usually required that grids with obstacles provide a reverse distance field. Calculate the distance of the obstacle grid to the nearest free grid in the 3D plane using the method described above, take its negative value, and superimpose it with the previous distance field to obtain a signed Euclidean distance map. The calculation formula is as follows: D(p)=min{(po) 2 +f(o)} Where p is an empty grid, o is an occupied grid, and f(o) is the existing distance value in the empty grid. Finally, a fixed plane is selected to display the Euclidean distance field.

Images