Three-dimensional topological graph construction method and system, robot and readable storage medium

Abstract

The invention discloses a three-dimensional topological graph construction method, system and device and a readable storage medium. The method comprises the steps that three-dimensional point cloud data in a robot working space is acquired, the three-dimensional point cloud data comprises position information and a label field, and the label field is used for indicating that a corresponding point is a passable point or an impassable point; dividing the workspace into a plurality of subspace nodes, and screening a first space node from the subspace nodes based on the number of point clouds in each subspace node and a tag field; generating a footprint bounding box based on the size of the robot, and placing the footprint bounding box at a first reference position of the first space node; and determining whether an impassable point exists in a preset range around the footprint bounding box, and if not, marking the first space node as a second space node to construct a three-dimensional topological graph. By means of the topology construction mode, the drawing problem of the three-dimensional topological graph can be simplified, and higher drawing precision is achieved.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology, specifically relating to a method, system, robot, and readable storage medium for constructing a three-dimensional topology map. Background Technology

[0002] Robot path planning is one of the core problems in the field of mobile robotics. Its goal is to find a safe and efficient movement path for a robot from a starting point to a destination in a complex environment. Traditional path planning methods, such as algorithms based on two-dimensional grid maps, typically compress the environment into a two-dimensional plane for planning.

[0003] While these methods are computationally efficient, they struggle to handle real-world environments with rich three-dimensional structural features, such as slopes, steps, and overhangs. This can easily lead to planned paths that are impassable in practice, or overly conservative approaches that limit the robot's range of motion.

[0004] With the popularization of 3D sensing technology, environment modeling and planning based on 3D point clouds has become a research hotspot. One direct approach is to plan directly on the 3D point cloud. However, point cloud data is usually characterized by massive volume, unordered nature, and unstructured structure, resulting in huge computational overhead for planning algorithms, making it difficult to meet the real-time requirements of robots. At the same time, the simple binarization in existing technologies fails to fully consider the physical dimensions and motion performance of the robot body, which may lead to the planning results being unexecutable in practice.

[0005] Therefore, in order to address the above-mentioned technical problems, it is necessary to provide a method, system, device, and readable storage medium for constructing a three-dimensional topology map.

[0006] The information disclosed in this background section is intended only to enhance the understanding of the overall background of the invention and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0007] The purpose of this invention is to provide a method, system, device, and readable storage medium for constructing a three-dimensional topology graph, which solves the problem that the topology graph construction in the prior art cannot meet the requirements of three-dimensional space.

[0008] To achieve the above objectives, a specific embodiment of the present invention provides the following technical solution:

[0009] In a first aspect, the present invention provides a method for constructing a three-dimensional topology graph, applied to robot path planning, comprising:

[0010] Acquire three-dimensional point cloud data within the robot's workspace. The three-dimensional point cloud data includes location information and a label field. The label field is used to indicate whether the corresponding point is a passable point or an impassable point.

[0011] The workspace is divided into multiple subspace nodes, and a first space node is selected from the subspace nodes based on the number of point clouds and the label field in each subspace node.

[0012] Based on the robot's dimensions, a footprint bounding box is generated, and the footprint bounding box is placed at the first reference position of the first spatial node;

[0013] Determine whether there are any impassable points within a preset range around the footprint bounding box. If not, mark the first spatial node as the second spatial node to construct a three-dimensional topology map.

[0014] In one or more embodiments of the present invention, determining whether there are impassable points within a preset range around the footprint enclosure includes:

[0015] A sampling point grid is generated within the footprint bounding box with a preset step size;

[0016] Perform a radius search on the sampling points within the first spatial node to determine whether there are any impassable points within the preset first radius neighborhood of the sampling points.

[0017] In this implementation, the accuracy and robustness of the inspection are improved by using a sampling point grid and radius search to check for impassable points. The systematic sampling and search ensures safety within a preset range around the footprint bounding box, avoiding misjudgments caused by sparse point cloud data or noise, thereby enhancing the robot's collision avoidance capabilities.

[0018] In one or more embodiments of the present invention, the method further includes:

[0019] Establish edge connections for each of the second spatial nodes to form a connected group; the connected group includes the second spatial nodes connected based on the edge connections;

[0020] Based on the number of second spatial nodes in each of the connected groups, filter the third spatial nodes;

[0021] Output the topology graph formed based on the third spatial node.

[0022] This implementation further optimizes the structure of the topology graph by establishing edge connections between nodes in the second spatial region and forming connected groups. This ensures connectivity between nodes, facilitating the path planning algorithm in finding feasible paths. Third spatial nodes are selected based on the number of nodes in the connected groups, filtering out isolated or small regions while retaining the main connected regions, thus improving the practicality of the topology graph and the efficiency of path planning.

[0023] In one or more embodiments of the present invention, establishing edge connections for each of the second spatial nodes includes:

[0024] Define a traversability score for each of the second spatial nodes;

[0025] Using at least one of the following constraints—the maximum connection distance, the height difference between nodes, and the node's traversability score—edge connections are established for each of the second spatial nodes.

[0026] This implementation introduces a drivability score and multiple constraints to establish edge connections, ensuring that connections are established only between safe and reliable nodes, avoiding unstable connections, and improving the reliability of the topology graph and the robot's navigation safety. The drivability score provides a quantitative evaluation standard, making connection decisions more scientific.

[0027] In one or more embodiments of the present invention, establishing edge connections for each of the second spatial nodes is based on at least one of the following constraints: a preset maximum connection distance, the height difference between nodes, and the drivability score of the nodes. This includes:

[0028] Acquire two second spatial nodes whose distance is less than the preset maximum connection distance;

[0029] If the height difference between the two second spatial nodes is less than or equal to the preset climbing distance, and the accessibility score of the two second spatial nodes is greater than the preset safety value, then an edge connection is established between the two second spatial nodes.

[0030] This implementation specifies the constraints on edge connections, further refining the connection criteria through a comprehensive judgment of distance, height difference, and drivability score. This avoids establishing connections between nodes with steep slopes or poor drivability, reduces risks in path planning, and improves the robot's adaptability in complex terrain.

[0031] In one or more embodiments of the present invention, the step of filtering the third spatial node based on the number of second spatial nodes in each of the connected groups includes:

[0032] Calculate the number of second spatial nodes in each of the connected groups, and determine the reference connected groups whose number of nodes is greater than or equal to a preset first threshold;

[0033] Determine whether the number of the reference connected groups is greater than a preset threshold; if so,

[0034] Remove connected groups whose number of nodes is less than a preset second threshold, where the second threshold is less than or equal to the first threshold.

[0035] This implementation calculates the number of nodes in connected groups and sets a threshold to filter out important connected groups as third-space nodes, while removing connected groups with too few nodes. This helps to focus on the main working area, reduce redundancy in the topology graph, and improve the efficiency and accuracy of path planning.

[0036] In one or more embodiments of the present invention, the step of filtering the third spatial node based on the number of second spatial nodes in each of the connected groups includes:

[0037] Calculate the number of second spatial nodes in each of the connected groups, and determine the reference connected groups whose number of nodes is greater than or equal to a preset first threshold;

[0038] Determine whether the number of the reference connected groups is greater than a preset threshold; if not,

[0039] Mark all second-space nodes as third-space nodes.

[0040] This implementation provides a flexible filtering strategy that retains all second-space nodes as third-space nodes when the number of reference connected groups is small. This ensures that over-filtering is not performed when the number of connected regions is small, maintains the integrity of the topology graph, and avoids path planning failures due to over-filtering.

[0041] In one or more embodiments of the present invention, filtering a first spatial node from the subspace nodes based on the number of point clouds and the label field within each subspace node includes:

[0042] Count the number of point clouds in each subspace node and filter out subspace nodes whose internal point cloud count is less than a preset point cloud count threshold.

[0043] For a subspace node whose point cloud count is greater than or equal to a preset point cloud count threshold, a radius search is performed at the second reference position coordinates in the subspace node to count the number of passable points within the preset second radius.

[0044] The subspace nodes whose number of passable points within a preset second radius is greater than a preset statistical value are marked as first space nodes.

[0045] This implementation method filters the first spatial nodes based on the number of point clouds and the statistics of traversable points, ensuring that the selected nodes have sufficient point cloud data support and a high density of traversable points. This improves the representativeness and reliability of the nodes, reduces false selections due to insufficient data, and makes the topology map more closely reflect the actual environment.

[0046] In one or more embodiments of the present invention, the workspace is divided into multiple subspace nodes, including:

[0047] The robot's workspace is divided into n first spatial node units;

[0048] The first spatial node unit is recursively divided into n second spatial node units until the size of the smallest spatial node unit formed by the division is less than or equal to the preset size value in each spatial dimension, and / or the number of point clouds contained in the smallest spatial node formed by the division is less than the preset threshold.

[0049] The smallest spatial node unit is determined as the subspace node.

[0050] This implementation recursively divides the workspace until size or point cloud quantity conditions are met, creating subspace nodes of uniform size that adapt to environmental characteristics. This partitioning method improves the accuracy and efficiency of spatial partitioning, allowing the topology graph to better reflect the structure of the workspace.

[0051] In one or more embodiments of the present invention, the preset size value is the size of the footprint bounding box in the corresponding spatial dimension.

[0052] This implementation sets the preset size value to the size of the footprint bounding box, ensuring that the subspace nodes match the actual size of the robot, making the topology map more in line with the robot's movement needs.

[0053] In one or more embodiments of the present invention, generating a footprint bounding box based on the robot's dimensions includes:

[0054] Obtain the robot in Axial direction, Axial direction and The maximum dimensions in the axial direction are respectively , as well as ;

[0055] Configuration Axial direction, Axial direction and The safety margin dimensions in the axial direction are respectively ;

[0056] The footprint enclosure is calculated based on the safety edge dimensions. Axial direction, Axial direction and The dimensions in the axial direction are respectively , as well as .

[0057] This implementation defines the robot's safe operating area by generating a footprint bounding box that takes into account robot size and safety edges. The addition of safety edges provides a buffer distance, enhances the reliability of collision avoidance, makes the topology safer, and is suitable for practical robot deployment.

[0058] In one or more embodiments of the present invention, forming a connected group includes:

[0059] Create a corresponding Boolean vector for each of the second spatial nodes. The initial value of the Boolean vector is a first identifier, which indicates that the corresponding second spatial node has not been visited.

[0060] Traverse the second space nodes, and recursively visit other second space nodes connected to an unvisited second space node as the first node of a connected group.

[0061] Each connected second spatial node is determined as the connected group, and the Boolean vector corresponding to the second spatial node in the connected group is updated to a second identifier, which indicates that the corresponding second spatial node has been visited.

[0062] This implementation uses Boolean vectors and a recursive access algorithm to efficiently form connected groups, reducing computational complexity and memory usage, and improving the speed of topology graph construction. This method ensures the accuracy of connected group identification and is suitable for large-scale point cloud data processing.

[0063] In one or more embodiments of the present invention, the impassable point includes: an aerial reference point and an obstacle point; and / or,

[0064] The method further includes:

[0065] Obtain the intensity value of each point in the 3D point cloud data within the robot's workspace;

[0066] Based on the intensity value, the label field of the corresponding point is determined.

[0067] This implementation method clarifies the types of impassable points and defines a technical means of determining the label field through intensity values, thereby improving the accuracy and flexibility of point cloud data classification.

[0068] Secondly, the present invention provides a three-dimensional topology graph construction system, comprising:

[0069] The acquisition module is used to acquire three-dimensional point cloud data within the robot's workspace. The three-dimensional point cloud data includes location information and a label field. The label field is used to indicate whether the corresponding point is a passable point or an impassable point.

[0070] The first filtering module is used to divide the workspace into multiple subspace nodes, and filter the first space node from the subspace nodes based on the number of point clouds and the label field in each subspace node.

[0071] The generation module is used to generate a footprint bounding box based on the size of the robot and place the footprint bounding box at a first reference position of the first spatial node;

[0072] The second filtering module is used to mark the first spatial node as the second spatial node when there are no impassable points within a preset range around the footprint bounding box, so as to construct a three-dimensional topology map.

[0073] Thirdly, the present invention provides a robot comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the three-dimensional topology map construction method by executing the computer instructions.

[0074] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to execute the three-dimensional topology graph construction method.

[0075] Compared with existing technologies, the 3D topology map construction method provided by this invention utilizes the label field in point cloud data to encode drivability semantic information, rather than relying solely on geometric height. Based on a multi-level filtering structure, the early removal of leaf nodes with insufficient point cloud data greatly reduces the scale of data requiring subsequent processing, achieving an optimal balance between computational efficiency and model accuracy, and meeting the real-time requirements of robot systems.

[0076] This invention does not plan on abstract spatial points, but rather closely integrates the robot's actual physical dimensions and motion performance. When establishing edge connections, a dual verification of maximum climbing distance and node drivability score is introduced, ensuring that the generated topology and subsequent paths strictly conform to the robot's kinematic and dynamic constraints, resulting in accurate and more feasible planned paths.

[0077] Furthermore, by directly querying the intensity value during the radius search, it is possible to accurately distinguish between truly passable areas and obstacles, effectively avoiding misjudging geometrically flat but semantically impassable areas such as thin ceilings and the tops of low obstacles as nodes. This fundamentally improves the safety and reliability of the topology graph and significantly reduces the robot's collision risk.

[0078] Furthermore, this invention introduces a connected component analysis and filtering mechanism to identify and eliminate small, isolated connected components. This effectively filters out "topological islands," thereby improving the success rate and efficiency of global path planning. Attached Figure Description

[0079] 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 only some embodiments recorded in the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0080] Figure 1 This is a flowchart of a three-dimensional topology graph construction method in one embodiment of the present invention;

[0081] Figure 2 This is a structural block diagram of a three-dimensional topology graph construction system according to another embodiment of the present invention;

[0082] Figure 3 This is a structural block diagram of a robot according to one embodiment of the present invention;

[0083] Figure 4 This is a schematic diagram of the footprint bounding box coordinate system in a specific embodiment of the present invention;

[0084] Figure 5 This is a schematic diagram of traversing sampling points in a footprint bounding box in a specific embodiment of the present invention;

[0085] Figure 6 This is a visualization pattern of three-dimensional point cloud intensity values ​​in one embodiment of the present invention;

[0086] Figure 7 This is a schematic diagram of a fragmented topology under one embodiment of the present invention;

[0087] Figure 8 This is a schematic diagram of a non-fragmented topology graph according to one embodiment of the present invention. Detailed Implementation

[0088] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0089] Unless otherwise expressly stated, throughout the specification and claims, the term "comprising" or its variations such as "including" or "comprises" shall be understood to include the stated elements or components without excluding other elements or other components.

[0090] Please refer to Figure 1 The diagram shown illustrates a process for constructing a three-dimensional topology graph according to an embodiment of the present invention. This three-dimensional topology graph construction method specifically includes the following steps:

[0091] S101: Acquire three-dimensional point cloud data within the robot's workspace, wherein the three-dimensional point cloud data includes location information and a label field;

[0092] The robot workspace of this invention can refer to all the spatial range that the robot's end effector or body can reach, naturally including obstacles, boundaries, and free areas within that range. It is understood that for a three-dimensional region, considering potential obstacles, excessively high step differences, and other scenarios unsuitable for robot passage, these scenarios need to be excluded to obtain the complete three-dimensional area within which the robot can physically move and manipulate.

[0093] In embodiments of the present invention, any three-dimensional region can be used as the robot's workspace, and a three-dimensional topological map that allows the robot to move and operate can be constructed on this basis. However, robots cannot intuitively understand continuous workspaces. Therefore, a digital and sparse representation of the shape of objects or environmental surfaces in the aforementioned workspace can be formed based on discrete data points in space. These discrete data points are the three-dimensional point cloud in the present invention.

[0094] Specifically, point cloud data can be a single frame of point cloud obtained by a sensor (such as a lidar) at a certain moment, or it can be a collection of multiple frames of point cloud obtained by the lidar periodically scanning within a set period. The three-dimensional topology map construction method provided in various embodiments of the present invention can be based on a single frame of point cloud or a collection of multiple frames of point cloud. In some scenarios, the lidar can be installed on a suitable acquisition device. When the acquisition device is stationary or in motion, the lidar can scan an area within a certain range to obtain point cloud data for the corresponding area. In the embodiments of this application, the point cloud data for the corresponding area can be regarded as three-dimensional point cloud data within the robot's workspace. The present invention does not limit the range of the area.

[0095] Furthermore, the 3D point cloud data includes location information. This location information represents the 3D coordinates of the point cloud in a preset coordinate system. The preset coordinate system can be a world coordinate system with the robot's initial position as the origin, providing a fixed global reference for subsequent path planning. In other implementations, the preset reference system can also be configured as a sensor coordinate system with the optical center of the sensor (such as LiDAR) as the origin; a robot coordinate system with the robot's torso center as the origin; or a geographic coordinate system formed by registering the point cloud information to a real latitude, longitude, and elevation coordinate system, etc. This embodiment of the invention does not impose limitations on this and can be dynamically adjusted based on the actual application scenario.

[0096] In addition to location information, 3D point cloud data also includes a label field, which indicates the drivability of points. Within a robot's workspace, different point clouds may exhibit varying attributes. To avoid wasting computational resources, this invention focuses on robot path planning, dividing the point cloud into drivable and impassable points based on drivability.

[0097] In one embodiment of the present invention, impassable points can be further divided into aerial reference points and obstacle points. A traversable point refers to a surface point in an area where the robot can safely and stably step; an obstacle point refers to a surface point in three-dimensional space where the robot's torso or limbs cannot collide with it in any movement posture; an aerial reference point is a type of auxiliary, non-stepping point, mainly used to provide spatial structure information, especially to distinguish different floors or indicate spatial height.

[0098] It should be noted that, in the embodiments of the present invention, the inherent fields in the three-dimensional point cloud data can be used to store the accessibility information, without the need to store an additional independent field. This directly utilizes the existing data structure of the point cloud, making the implementation of the present invention more efficient.

[0099] In one specific embodiment, the label field can be stored using the inherent intensity value field of the LiDAR point cloud. By semantically redefining the intensity value, it serves as a carrier for a accessibility label. Different accessibility categories correspond to an intensity value or range agreed upon during upstream SLAM mapping, as shown in Table 1 below.

[0100] Table 1

[0101]

[0102] In another specific embodiment without an aerial reference point, its visualized point cloud image is as follows: Figure 6 As shown.

[0103] It is understood that, in the embodiments of the present invention, the source of the 3D point cloud may include, but is not limited to, 3D LiDAR, depth camera, and photogrammetry. For point clouds from other sources, color channels or custom scalar fields can also be used to achieve the same functionality.

[0104] For example, for 3D point clouds acquired using a depth camera, a depth map is typically generated using infrared structured light or time-of-flight methods, and then converted into a 3D point cloud. Since laser echo intensity detection is not involved, these point clouds usually do not contain a physically meaningful "intensity" field. However, they often contain color information (RGB). Semantic tags can be encoded into a specific channel of the RGB color. All points marked as passable are defined with a channel value set to 50. This is equivalent to the function of "intensity value = 50" in a LiDAR point cloud. Other sources of 3D point clouds will not be described in detail here.

[0105] S102: Divide the workspace into multiple subspace nodes, and select a first space node from the subspace nodes based on the number of point clouds and the label field in each subspace node;

[0106] In fact, directly performing global calculations and processing millions or even tens of millions of 3D point cloud data in real time would result in path planning computation times reaching the second level, failing to meet the robot's requirements for real-time navigation and decision-making. Furthermore, using a high-resolution 3D grid map to discretize the entire workspace would generate a massive number of grid cells, leading to enormous memory consumption and a cubic increase in computational complexity, severely limiting its feasibility for use on embedded mobile computing platforms.

[0107] Therefore, in this embodiment of the invention, the workspace is divided into multiple sub-space nodes, which may have the same or different sizes and shapes. Taking the division of the workspace into multiple identical sub-space nodes as an example, it may specifically include: dividing the robot workspace into n first space node units; recursively dividing the first space node units into n second space node units, until the smallest space node unit formed by the division has a size in each spatial dimension that is less than or equal to a preset size value, and / or the number of point clouds contained in the smallest space node formed by the division is less than a preset threshold, and the smallest space node unit is the sub-space node.

[0108] Recursively partitioning spatial nodes organizes the space into a hierarchical tree structure, transforming some global operations into efficient local node traversal operations. This significantly reduces algorithm complexity from being oriented towards the entire point cloud to being oriented towards the tree structure or related to local node density, fundamentally improving processing speed. Dividing the workspace into multiple identical sub-space nodes also enables efficient management, compression, and structured processing of massive, continuous 3D point cloud data, transforming robot path planning into rapid optimization among a finite number of ordered nodes. This discretization and structuring transformation is fundamental to solving key challenges in 3D path planning, such as poor real-time performance and high memory consumption.

[0109] Furthermore, to ensure the accuracy of path planning, in this embodiment of the invention, the granularity of the smallest segmented spatial node can be controlled to be smaller than the footprint bounding box formed based on the robot's dimensions. That is, the aforementioned preset size value can be the size of the footprint bounding box in the corresponding spatial dimension. The process of obtaining and calculating the footprint bounding box will be described later in S103.

[0110] Furthermore, in an exemplary embodiment of the present invention, the first spatial node is filtered based on the number of point clouds in each subspace node and the label field, including: counting the number of point clouds in each subspace node and filtering out subspace nodes whose internal point cloud number is less than a preset point cloud number threshold; for subspace nodes whose point cloud number is greater than or equal to the preset point cloud number threshold, performing a radius search at the second reference position coordinates in the subspace node and counting the number of passable points within a preset second radius; and marking the subspace nodes whose number of passable points within the preset second radius is greater than a preset statistical value as the first spatial node.

[0111] During 3D environmental scanning, the acquired point cloud data inevitably contains a large number of noise points due to factors such as sensor measurement errors, interference from airborne particles, differences in surface reflection characteristics, and multiple reflections. These noise points typically exhibit a random, isolated spatial distribution, floating discretely around the surface of real objects, unable to form dense clusters with structural consistency. In contrast, the point cloud distribution of regions representing the surface of real, stable objects shows continuous and clustered characteristics. Therefore, when the number of points in a node is below a preset threshold, it can be determined that the node region is likely to be noise or invalid measurement, rather than a physical structure with actual supporting significance. By filtering out such unreliable regions, not only can the generation of path nodes in unstable areas be avoided, but it also helps to suppress noise and simplify point cloud data, improving the accuracy and efficiency of subsequent processing.

[0112] It should be further explained that in the multi-level screening process constructed in this invention, the screening based on the number of point clouds within a node is a coarse screening at the first level. This screening mechanism has good flexibility and can be implemented at different stages: it can be performed uniformly after the space is divided to the target size, or it can be executed immediately after each round of recursive division, that is, dynamically removing nodes with sparse point clouds based on the point cloud number threshold set for the smallest spatial node at the current level. However, it should be noted that, regardless of the implementation method, the subspace nodes that continue to participate in the subsequent first spatial node screening are all spatial nodes obtained from the last point cloud number-based screening.

[0113] In comparison, the first implementation only requires setting a threshold for the number of point clouds for spatial nodes of the target size, which helps reduce the number of custom parameters and simplifies system configuration. The second implementation, however, can quickly identify and remove invalid regions in the early stages of the segmentation process, avoiding redundant subdivision of obviously unsuitable subspaces, thereby significantly reducing the computational burden in subsequent processing stages and improving the efficiency of overall topology graph generation. The above processing logic reflects the present invention's synergistic optimization of algorithm real-time performance and computational resource efficiency while ensuring path safety and reliability, demonstrating a comprehensive consideration of performance and availability in the system design.

[0114] Furthermore, after filtering out subspace nodes whose internal point cloud count is less than a preset point cloud count threshold, for subspace nodes whose point cloud count is greater than or equal to the preset point cloud count threshold, their second reference position coordinates can be calculated, and a radius search can be performed at the second reference position coordinates to count the number of passable points within a preset second radius; subspace nodes whose number of passable points within a preset second radius is greater than a preset statistical value are marked as first space nodes.

[0115] The second reference position coordinates can be the spatial geometric center of the subspace. In practical applications, it can be calculated based on the following formula:

[0116]

[0117] in, These are the coordinates of the spatial center (second reference position coordinates). as well as These are the two diagonal vertices of the space in the world coordinate system, and these two diagonal vertices are the minimum and maximum values ​​of the subspace on the X, Y, and Z coordinate axes, respectively.

[0118] For example, in a subspace with a side length of 1.0 meter and a corner point located at the world origin (0,0,0), the diagonal vertices opposite to the minimum and maximum values ​​on the X, Y, and Z coordinate axes are respectively...

[0119]

[0120]

[0121] The center point of the subspace can then be calculated as:

[0122]

[0123] It is understood that the specific settings of the second reference position, the threshold of the number of point clouds, and the specific size of the preset statistical value are not limited in the embodiments of the present invention, and can be dynamically adjusted based on the actual use scenario.

[0124] In the multi-level filtering process constructed in this invention, filtering based on the presence or absence of impassable points within a node belongs to the second level of filtering. This second-level filtering can be applied to subspace nodes that have reached the target size and passed the first-level filtering. Through this filtering, spatial nodes with impassable points are filtered out based on the point cloud's label field, forming the first spatial nodes. This achieves the removal of most invalid nodes with relatively low computational resource consumption.

[0125] S103: Based on the size of the robot, generate a footprint bounding box and place the footprint bounding box at the first reference position of the first spatial node;

[0126] It should be noted that, in this invention, the footprint bounding box can refer to a geometric model that approximates the maximum physical space occupied by the robot in three-dimensional space using a regular three-dimensional graphic, with the robot's torso center as the reference. By applying the footprint bounding box, the robot's complex shape can be simplified into a regular geometric shape, facilitating efficient and uniform collision detection and drivability verification in the future; effectively avoiding the problem of collisions caused by treating the robot as a point mass and ignoring its actual volume.

[0127] The aforementioned regular 3D shape can be a cuboid, which has a simple structure, is easy to calculate, and is suitable for most conventionally shaped robots. However, in special implementation scenarios, such as when the robot itself has a specific shape and structure that would introduce a lot of unusable space into the cuboid footprint bounding box, other more suitable 3D shapes can be selected as its footprint bounding box based on the robot's actual shape. This invention does not limit the shape of the footprint bounding box.

[0128] For example, when planning the path of a police spherical robot, using a cuboid as the bounding box for its footprints would result in a large amount of unnecessary space being occupied in the corner areas, thus reducing planning efficiency. In this case, based on the robot's structural parameters, its circumscribed sphere can be selected as the bounding box for the footprints, making the bounded area more closely match the actual shape and helping to improve the quality and performance of motion planning.

[0129] Furthermore, considering the inevitable risks of sensor noise, positioning errors, control errors, and dynamic environmental changes during practical applications, additional safety space should be provided for the robot's gait and leg swing when generating the footprint bounding box to prevent the legs or joints from scraping against obstacles.

[0130] In one exemplary embodiment, forming a cuboid footprint bounding box for a conventional robot specifically includes: acquiring the robot's footprint bounding box... Axial direction, Axial direction and Maximum dimension in the axial direction, and configuration Axial direction, Axial direction and Safety margin dimensions in the axial direction; calculation of the footprint bounding box based on the safety margin dimensions. Axial direction, Axial direction and The dimension of the axis. The function form can be expressed as:

[0131]

[0132]

[0133]

[0134] in, , as well as The robots are respectively in Axial direction, Axial direction and Maximum dimension in the axial direction; , as well as They are respectively Axial direction, Axial direction and Safety margin dimensions in the axial direction; , as well as These represent the footprint bounding box in Axial direction, Axial direction and Dimensions in the axial direction.

[0135] It should also be noted that the robot is in Axial direction, Axial direction and The maximum dimension along the axis refers to the maximum dimension that the robot can achieve in the corresponding coordinate axis direction under various possible motion configurations, rather than the measurement value under an upright, a specific static, or a single motion configuration. Meanwhile, the configuration of the safety margin dimension is related to system error values ​​and environmental changes; this invention does not limit its specific value and it can be dynamically adjusted based on the actual usage scenario.

[0136] S104: Determine whether there are any impassable points within a preset range around the footprint bounding box. If not, mark the first spatial node as the second spatial node to construct a three-dimensional topology map.

[0137] It should be noted that the preset range around the footprint enclosure refers to the space that can completely surround the footprint enclosure. For example, it can refer to the enclosed space area formed by offsetting all the outer boundaries of the footprint enclosure outward by a preset safety distance.

[0138] In one embodiment of the present invention, determining whether there are impassable points within a preset range around the footprint bounding box may include: generating a sampling point grid within the footprint bounding box with a preset step size; performing a radius search on the sampling points within the first spatial node to determine whether there are impassable points within the preset first radius neighborhood of the sampling points.

[0139] Specifically, a sampling point grid is generated within the footprint bounding box based on a preset step size. This can be achieved by first establishing a grid like... Figure 4 The footprint bounding box coordinate system shown has its origin o as the geometric center of the bounding box ground. Then, as... Figure 5 As shown, in the x-axis direction, from arrive / 2, traversing with a preset first step length; in the y-axis direction, from arrive Traverse the system with a preset second step size; in the z-axis direction, from 0 to... The process is repeated with a preset third step length to form a 3D sampling point grid. In this embodiment, Figure 5 The sampling points formed by the traversal are the corner points of the small rectangles divided based on the first to third step lengths.

[0140] It should be noted that, based on this implementation method, the coordinates of the sampling points need to be transformed to the world coordinate system of the corresponding first spatial node before the radius search. That is, the reference point for performing the radius search on the sampling points within the first spatial node can be the coordinates of the sampling points in the footprint bounding box coordinate system. The summation of the coordinates of the spatial center of the corresponding first spatial node. The calculation of the spatial center coordinates of the spatial node is described in S102 of this specification, and will not be repeated here.

[0141] In the multi-level screening process constructed in this invention, screening for second spatial nodes from first spatial nodes belongs to the third level of screening. This level of screening strategy focuses on the robot itself. By taking into account the robot's motion posture and volume, it further filters out nodes that may cause robot collisions, providing a safety guarantee for subsequent path planning.

[0142] It should also be noted that, in an exemplary embodiment of the present invention, a fourth-level filtering mechanism is also provided. Specifically, this includes: establishing edge connections for each second spatial node to form a connected group; the connected group contains second spatial nodes connected by edge connections; filtering third spatial nodes based on the number of second spatial nodes in each connected group; and outputting a topology graph formed based on the third spatial nodes.

[0143] It should be noted that since this invention focuses on path planning in three-dimensional space, the spatial advantages established in the early stages of this invention can be further utilized when establishing edge connections. The robot's climbing distance is included in the node selection process by considering the distance between candidate second-space nodes and the z-axis component of that distance. It is also understood that the cost of direct passage between nodes with greater distances is higher, and the ease of passage varies between different nodes. For example, taking a staircase as an example, directly traversing five steps is more costly than traversing each step one by one. Furthermore, even at the edges of the staircase, the robot can still move without collisions; therefore, using the center of the staircase as the robot's landing point is clearly safer than using the edges.

[0144] Based on this, establishing edge connections for each second space node may include: defining a traversability score for each second space node; and establishing edge connections for each second space node using at least one of the following constraints: a preset maximum connection distance, the height difference between nodes, and the traversability score of the node.

[0145] In an exemplary embodiment, this can be specifically manifested as follows: acquiring two second spatial nodes whose distance is less than a preset maximum connection distance; if the height difference between the two second spatial nodes is less than or equal to a preset climbing distance, and the traversability scores of the two second spatial nodes are both greater than a preset safety value, then establishing an edge connection between the two second spatial nodes. The specific values ​​of the preset climbing distance, the preset maximum connection distance, and the preset safety value are not limited in this embodiment of the invention.

[0146] It is understandable that, based on edge connections, a topology graph based on second-space nodes has already been formed. This invention continues with a fourth level of filtering, namely, island clearing based on the number of connected nodes. Specifically, this may include: creating a corresponding Boolean vector for each second-space node, with the initial value of the Boolean vector being a first identifier, where the first identifier indicates that the corresponding second-space node has not been visited; traversing the second-space nodes, and recursively visiting other second-space nodes connected to an unvisited second-space node as the first node of a connected group; determining the obtained connected second-space nodes as a connected group, and updating the Boolean vector corresponding to the second-space nodes within the connected group to a second identifier, where the second identifier indicates that the corresponding second-space node has been visited.

[0147] Furthermore, the number of second spatial nodes in each connected group is calculated, and reference connected groups with a number of nodes greater than or equal to a preset first threshold are identified; it is determined whether the number of reference connected groups is greater than a preset number threshold; if so, connected groups with a number of nodes less than a preset second threshold are removed, wherein the second threshold is less than or equal to the first threshold; if not, all second spatial nodes are marked as third spatial nodes.

[0148] like Figure 7 In the illustrated embodiment, when the maximum number of nodes in the entire map's connected components is less than a preset first threshold, it indicates that the current environment is highly fragmented. Forcibly deleting small connected components could lead to the deletion of most passable areas in the environment, resulting in complete failure of path planning because no area can satisfy the robot's movement needs. Therefore, in this case, a conservative strategy is preferred, retaining all nodes, treating them as third-space nodes, and directly outputting the topology map. While noise and isolated areas exist in the map, it at least provides the robot with the possibility of moving within fragmented spaces. The robot may not be able to perform long-distance navigation, but it can still explore or perform tasks within its local area. This ensures the basic functional availability of the system under extremely harsh conditions.

[0149] Conversely, such as Figure 8In the illustrated embodiment, if the largest number of nodes in a connected group is greater than or equal to a preset first threshold, it indicates the presence of a significant large connected component, resulting in a high-quality map and a relatively clear main structure. At this point, some isolated connected groups are highly likely to be genuine noise and isolated islands, such as indoor chandeliers, windowsills, and tables; or outdoor balconies and low-lying decorative roofs. Retaining these nodes could lead to physically infeasible path planning schemes, and useless nodes and edges would inevitably increase the space and time complexity of graph search. Therefore, the optimization strategy here is better than retaining everything. Based on a second threshold less than or equal to the first threshold, isolated islands in the red-highlighted areas of the graph can be filtered out, and the retained nodes can be used as third-space nodes to output the topology graph.

[0150] For example, in one specific embodiment, the topology graph contains 8,000 original nodes, of which the main connected components account for as much as 94.3% (7,540 nodes), and the remaining 460 nodes are scattered in 11 isolated small areas. At this time, the system determines that the map quality is high and the main structure is clear, so it executes an optimization strategy to remove all isolated areas, reducing the search space by 5.7% and effectively improving the search efficiency of subsequent path planning.

[0151] Conversely, in another specific embodiment, due to missing point cloud annotations, the largest connected component contains only 120 nodes (24% of the total 500 nodes), below the preset threshold of 50%. Based on this, the system judges the map quality to be poor and the structure highly fragmented, thus activating a conservative strategy to retain all nodes. Although this approach does not simplify the map, it successfully avoids the risk of losing critical paths due to the accidental deletion of secondary connected components (such as areas with 90, 80, or 70 nodes), ensuring that usable paths can still be planned even under less than ideal data conditions.

[0152] The adaptive connectivity cleanup mechanism of this invention fully demonstrates the powerful robustness of this method in making an adaptive balance between "efficiency optimization" and "functionality preservation" when facing data of different quality environments.

[0153] Please refer to Figure 2 As shown, based on the same inventive concept as the aforementioned three-dimensional topology graph construction method, one embodiment of the present invention provides a three-dimensional topology graph construction system 200, including: an acquisition module 201, a first filtering module 202, a generation module 203, and a second filtering module 204.

[0154] Specifically, the acquisition module 201 is used to acquire 3D point cloud data within the robot's workspace. The 3D point cloud data includes location information and a label field, which indicates whether the corresponding point is passable or impassable. The first filtering module 202 is used to divide the workspace into multiple sub-space nodes and filter the first space node from the sub-space nodes based on the number of point clouds in each sub-space node and the label field. The generation module 203 is used to generate a footprint bounding box based on the robot's size and place the footprint bounding box at the first reference position of the first space node. The second filtering module 204 is used to mark the first space node as a second space node when there are no impassable points within a preset range around the footprint bounding box.

[0155] Please refer to Figure 3 As shown, embodiments of the present invention also provide a robot 300, which includes at least one processor 301, a memory 302 (e.g., non-volatile memory), a memory 303, and a communication interface 304, and the at least one processor 301, memory 302, memory 303, and communication interface 304 are connected together via an internal bus 305. At least one processor 301 is used to invoke at least one program instruction stored or encoded in the memory 302, so that the at least one processor 301 performs various operations and functions of the three-dimensional topology map construction method described in the various embodiments of this specification.

[0156] In the embodiments of this specification, the robot 300 can be configured with a functional terminal to carry the above-mentioned hardware structure. The terminal may include, but is not limited to: personal computer, server computer, workstation, desktop computer, laptop computer, notebook computer, mobile electronic device, smartphone, tablet computer, cellular phone, personal digital assistant (PDA), handheld device, messaging device, wearable electronic device, consumer electronic device, etc.

[0157] This invention also provides a computer-readable medium carrying computer-executable instructions. When executed by a processor, these instructions can be used to implement various operations and functions of the three-dimensional topology graph construction method described in the various embodiments of this specification.

[0158] The computer-readable medium in this invention can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0159] In this invention, the computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. This propagated data signal may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wireless, wireline, optical fiber, RF, etc., or any suitable combination thereof.

[0160] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0161] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus, systems, and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0162] The foregoing description of specific exemplary embodiments of the invention is for illustrative and explanatory purposes. These descriptions are not intended to limit the invention to the precise forms disclosed, and it will be apparent that many changes and variations can be made in accordance with the foregoing teachings. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling those skilled in the art to implement and utilize various different exemplary embodiments of the invention, as well as various different choices and variations. The scope of the invention is intended to be defined by the claims and their equivalents.

[0163] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0164] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for constructing a three-dimensional topology graph, applied to robot path planning, characterized in that, include: Acquire three-dimensional point cloud data within the robot's workspace. The three-dimensional point cloud data includes location information and a label field. The label field is used to indicate whether the corresponding point is a passable point or an impassable point. The workspace is divided into multiple subspace nodes, and a first space node is selected from the subspace nodes based on the number of point clouds and the label field in each subspace node. Based on the robot's dimensions, a footprint bounding box is generated, and the footprint bounding box is placed at the first reference position of the first spatial node; Determine whether there are any impassable points within a preset range around the footprint bounding box. If not, mark the first spatial node as the second spatial node to construct a three-dimensional topology map.

2. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, Determining whether there are impassable points within a preset range around the footprint enclosure includes: A sampling point grid is generated within the footprint bounding box with a preset step size; Perform a radius search on the sampling points within the first spatial node to determine whether there are any impassable points within the preset first radius neighborhood of the sampling points.

3. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, The method further includes: Establish edge connections for each of the second spatial nodes to form a connected group; the connected group includes the second spatial nodes connected based on the edge connections; Based on the number of second spatial nodes in each of the connected groups, filter the third spatial nodes; Output the topology graph formed based on the third spatial node.

4. The method for constructing a three-dimensional topology graph according to claim 3, characterized in that, Establishing edge connections for each of the second spatial nodes includes: Define a traversability score for each of the second spatial nodes; Using at least one of the following constraints—the maximum connection distance, the height difference between nodes, and the node's traversability score—edge connections are established for each of the second spatial nodes.

5. The method for constructing a three-dimensional topology graph according to claim 4, characterized in that, Using at least one of the following constraints—preset maximum connection distance, height difference between nodes, and node traversability score—edge connections are established for each node in the second space, including: Acquire two second spatial nodes whose distance is less than the preset maximum connection distance; If the height difference between the two second spatial nodes is less than or equal to the preset climbing distance, and the accessibility score of the two second spatial nodes is greater than the preset safety value, then an edge connection is established between the two second spatial nodes.

6. The method for constructing a three-dimensional topology graph according to claim 3, characterized in that, The step of filtering third spatial nodes based on the number of second spatial nodes in each of the connected groups includes: Calculate the number of second spatial nodes in each of the connected groups, and determine the reference connected groups whose number of nodes is greater than or equal to a preset first threshold; Determine whether the number of the reference connected groups is greater than a preset threshold; if so, Remove connected groups whose number of nodes is less than a preset second threshold, where the second threshold is less than or equal to the first threshold.

7. The method for constructing a three-dimensional topology graph according to claim 3, characterized in that, The step of filtering third spatial nodes based on the number of second spatial nodes in each of the connected groups includes: Calculate the number of second spatial nodes in each of the connected groups, and determine the reference connected groups whose number of nodes is greater than or equal to a preset first threshold; Determine whether the number of the reference connected groups is greater than a preset threshold; if not, Mark all second-space nodes as third-space nodes.

8. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, Based on the number of point clouds and the label field within each of the subspace nodes, a first space node is selected from the subspace nodes, including: Count the number of point clouds in each subspace node and filter out subspace nodes whose internal point cloud count is less than a preset point cloud count threshold. For a subspace node whose point cloud count is greater than or equal to a preset point cloud count threshold, a radius search is performed at the second reference position coordinates in the subspace node to count the number of passable points within the preset second radius. The subspace nodes whose number of passable points within a preset second radius is greater than a preset statistical value are marked as first space nodes.

9. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, The workspace is divided into multiple subspace nodes, including: The robot's workspace is divided into n first spatial node units; The first spatial node unit is recursively divided into n second spatial node units until the size of the smallest spatial node unit formed by the division is less than or equal to the preset size value in each spatial dimension, and / or the number of point clouds contained in the smallest spatial node formed by the division is less than the preset threshold. The smallest spatial node unit is determined as the subspace node.

10. The method for constructing a three-dimensional topology graph according to claim 9, characterized in that, The preset size value is the size of the footprint bounding box in the corresponding spatial dimension.

11. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, The generation of footprint bounding boxes based on the robot's dimensions includes: Obtain the robot in Axial direction, Axial direction and The maximum dimensions in the axial direction are respectively , as well as ; Configuration Axial direction, Axial direction and The safety margin dimensions in the axial direction are respectively ; The footprint enclosure is calculated based on the safety edge dimensions. Axial direction, Axial direction and The dimensions in the axial direction are respectively , as well as .

12. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, The formation of the connected group includes: Create a corresponding Boolean vector for each of the second spatial nodes. The initial value of the Boolean vector is a first identifier, which indicates that the corresponding second spatial node has not been visited. Traverse the second space nodes, and recursively visit other second space nodes connected to an unvisited second space node as the first node of a connected group. Each connected second spatial node is determined as the connected group, and the Boolean vector corresponding to the second spatial node in the connected group is updated to a second identifier, which indicates that the corresponding second spatial node has been visited.

13. The method for constructing a three-dimensional topology graph according to claim 1, characterized in that, The impassable points include: aerial reference points and obstacle points; and / or, The method further includes: Obtain the intensity value of each point in the 3D point cloud data within the robot's workspace; Based on the intensity value, the label field of the corresponding point is determined.

14. A three-dimensional topology graph construction system, characterized in that, include: The acquisition module is used to acquire three-dimensional point cloud data within the robot's workspace. The three-dimensional point cloud data includes location information and a label field. The label field is used to indicate whether the corresponding point is a passable point or an impassable point. The first filtering module is used to divide the workspace into multiple subspace nodes, and filter the first space node from the subspace nodes based on the number of point clouds and the label field in each subspace node. The generation module is used to generate a footprint bounding box based on the size of the robot and place the footprint bounding box at a first reference position of the first spatial node; The second filtering module is used to mark the first spatial node as the second spatial node when there are no impassable points within a preset range around the footprint bounding box, so as to construct a three-dimensional topology map.

15. A robot, characterized in that, include: A memory and a processor are communicatively connected, the memory stores computer instructions, and the processor executes the computer instructions to perform the three-dimensional topology map construction method according to any one of claims 1-13.

16. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing a computer to perform the three-dimensional topology map construction method according to any one of claims 1-13.

Images