A robotic multi-floor compound navigation method and apparatus

Abstract

This invention discloses a multi-floor composite navigation method and device for robots, belonging to the field of robot navigation technology. Existing robot navigation schemes, during multi-floor navigation, suffer from low information preservation of the constructed grid map, making it impossible to obtain the optimal planned path. The multi-floor composite navigation method of this invention fully considers the characteristics of multi-floor navigation, dividing the 3D point cloud map by floor to obtain 3D point cloud maps for different floors, thereby greatly improving information preservation. Each floor's 3D point cloud map is then projected and rasterized to generate multiple 2D maps of the same floor. Next, the connection relationships and structural features between floors are analyzed to obtain an inter-floor connection map. Finally, based on the inter-floor connection map and the 2D maps of the same floor, a topological path search is performed to obtain the optimal planned path, thus enabling accurate robot navigation in a multi-floor environment within the same building.

Description

Technical Field

[0001] This invention relates to a multi-floor composite navigation method and device for robots, belonging to the field of robot navigation technology. Background Technology

[0002] Chinese patent application (publication number: CN116718189A) discloses a mobile robot and a cross-building path planning method for it. The cross-building path planning method for the mobile robot includes constructing a two-dimensional grid map of each floor of each building in a cross-building complex; constructing a topology map of the cross-building complex, wherein the topology map of the cross-building complex includes the distances and connectivity relationships between various stations in the cross-building complex; and based on the two-dimensional grid map of one or more floors of one or more buildings in the cross-building complex and the topology map of the cross-building complex.

[0003] The above solution directly constructs a two-dimensional raster map across buildings based on point cloud data. However, small-scale details such as the specific structure within a floor may be lost, resulting in a relatively low level of information preservation for the floor. Furthermore, the solution does not disclose how to extract floors other than elevators and the inter-floor structure from the three-dimensional point cloud, which affects the accuracy of the floor map construction.

[0004] Furthermore, the aforementioned scheme is primarily used for path planning across buildings. While it mentions mapping elevator car points from the topology map of each floor to the topology map of the entire building, serving as connection points between nodes on different floors, it handles relatively few scenarios involving cross-floor navigation within the same building. Moreover, robots may not always be able to use elevators to go up or down floors, and even if they can, it may not be the optimal path choice in some cases. Therefore, this scheme cannot obtain the optimal planned path during cross-floor navigation within the same building, failing to achieve accurate robot navigation in multi-floor environments. Moreover, the scheme mentions using the A* algorithm to find the shortest path from a specified starting point to a specified ending point, but it does not provide a specific implementation process, making multi-floor navigation unfeasible and hindering its widespread adoption. Summary of the Invention

[0005] To address the aforementioned problems, or one of them, the present invention aims to provide a multi-floor composite navigation method and device for robots. This method involves segmenting a globally covering 3D point cloud map by floor to obtain 3D point cloud maps for different floors. Each floor's 3D point cloud map is then projected and rasterized to generate multiple 2D maps of the same floor, significantly improving information preservation and effectively preventing the loss of small-scale details such as specific structures within each floor. Based on the connections and structural features between floors, node information and topological edges are determined. These edges are then connected to form a graph structure, resulting in an inter-floor connection map. This allows for the extraction of various structures within and between floors, enabling accurate construction of floor maps. Furthermore, based on the inter-floor connection map and the 2D maps of the same floor, a topological path search is performed to obtain the optimal planned path. This enables accurate robot navigation in multi-floor environments, making it particularly suitable for multi-floor robot navigation within a single building.

[0006] To address the aforementioned problems or one of them, the second objective of this invention is to provide a multi-floor composite navigation method and device for robots. By representing the connection relationship between floors through an inter-floor connection map, accurate navigation of the robot in a multi-floor environment is achieved, solving the problem that traditional navigation methods cannot effectively handle vertical movement and floor switching. Furthermore, global positioning is performed through a point cloud map, combined with local positioning through a two-dimensional same-floor map, ensuring high-precision positioning during the navigation process.

[0007] To address the aforementioned problems or one of the aforementioned problems, the third objective of this invention is to provide a multi-floor composite navigation method and device for robots, which can adapt to the complex environment in multi-story buildings, including various connecting structures such as stairs, elevators, and corridors. Through layered processing and topology simplification, it improves the real-time performance of path planning and navigation execution, enabling rapid response to environmental changes and navigation needs. Furthermore, the solution is detailed and provides a practical cross-floor path navigation scheme, facilitating the promotion and use of multi-floor navigation solutions.

[0008] To achieve one of the above objectives, the first technical solution of the present invention is as follows:

[0009] A multi-floor composite navigation method for robots includes the following:

[0010] Collect multi-floor information for navigation purposes;

[0011] Based on multi-floor information, the navigation area is determined, and a 3D point cloud map of the navigation area is obtained;

[0012] The three-dimensional point cloud maps of different floors are identified, and the three-dimensional point cloud maps of each floor are projected and rasterized to generate multiple two-dimensional maps of the same floor.

[0013] The connection relationships between floors are analyzed to obtain an inter-floor connection map, which includes the following:

[0014] Step 11: Based on the connection relationships between floors and the structural characteristics between floors, determine the information of several nodes;

[0015] Step 12: Establish the connection relationship between nodes to form multiple topological edges, which are used to represent the connection paths within the same layer and the connection paths between layers;

[0016] Step 13: Connect multiple topological edges to form a graph structure;

[0017] Step 14: Perform redundant edge elimination, cost calculation and connectivity check on the graph structure to obtain the inter-layer connection map, which is used to represent the connection relationship and navigation path between floors;

[0018] Based on the inter-layer connection map and the two-dimensional same-layer map, a topological path search is performed to obtain the optimal planned path, thereby realizing multi-layer composite navigation.

[0019] This invention fully considers the characteristics of multi-floor navigation. It segments the globally covering 3D point cloud map by floor, obtaining 3D point cloud maps for different floors. Each floor's 3D point cloud map is then projected and rasterized to generate multiple 2D maps of the same floor. Based on the connections and structural features between floors, node information and topological edges are determined. These multiple topological edges are then connected to form a graph structure, resulting in an inter-floor connection map. Finally, based on the inter-floor connection map and the 2D maps of the same floor, a topological path search is performed to obtain the optimal planned path. Therefore, by identifying floors before processing the raster map, the information preservation level can be greatly improved, effectively avoiding the risk of small-scale details such as the specific structure within a floor being lost. Furthermore, various structures within and between floors are extracted, allowing for accurate construction of floor maps. This enables accurate robot navigation in multi-floor environments, making it particularly suitable for multi-floor robot navigation in the same building. The solution is detailed and feasible.

[0020] Furthermore, this invention decomposes the 3D point cloud map into several 2D same-layer maps and inter-layer connected maps, which greatly reduces the computational resource requirements, reduces computational complexity, improves navigation efficiency, and reduces data storage. Therefore, it can meet the real-time navigation requirements, effectively solve the multi-layer navigation problem, and is suitable for real-time operation on embedded systems.

[0021] Meanwhile, this invention uses an inter-floor connection map to represent the connections between floors, enabling accurate navigation of the robot in multi-floor environments and solving the problem that traditional navigation methods cannot effectively handle vertical movement and floor switching. Furthermore, this invention uses a point cloud map for global positioning and combines it with a two-dimensional map of the same floor for local positioning, ensuring high-precision positioning during navigation.

[0022] As a preferred technical measure:

[0023] The method for determining the navigation area and obtaining a 3D point cloud map of the navigation area based on multi-floor information is as follows:

[0024] Based on multi-floor information, the navigation area is defined;

[0025] Set the point cloud resolution according to the environmental complexity and positioning accuracy requirements of the navigation area;

[0026] Based on the point cloud resolution, obtain the original point cloud covering the entire navigation area;

[0027] The original point cloud was downsampled, outlier removed, and smoothed to obtain an optimized point cloud.

[0028] An octree subdivision algorithm is used to split the optimized point cloud into several sub-point clouds;

[0029] By using a simultaneous localization and mapping (SMR) algorithm, several sub-point clouds are processed to obtain a 3D point cloud map.

[0030] As a preferred technical measure:

[0031] The method for identifying 3D point cloud maps of different floors, projecting and rasterizing the 3D point cloud map of each floor separately, and generating multiple 2D maps of the same floor is as follows:

[0032] Acquire a 3D point cloud map and identify the 3D point cloud maps located on different floors. Divide the 3D point cloud map by floor to obtain the point cloud data for each floor.

[0033] Projection processing is performed on the point cloud data of each floor, which involves deleting the ground point cloud from the point cloud data, unifying the vertical coordinate values ​​of the remaining point cloud data to zero, performing compression operations, and generating a two-dimensional map.

[0034] The projected 2D map is rasterized to generate a 2D same-layer map.

[0035] A two-dimensional same-layer map is a map composed of binary graticules, which have only two attributes: 0 and 1. Each graticule contains point clouds and is 1, while the absence of point clouds is 0.

[0036] As a preferred technical measure:

[0037] The method for analyzing the connections between floors to obtain an inter-floor connection map is as follows:

[0038] Step 11: Based on the connection relationships between floors and the structural characteristics between floors, determine the information of several nodes;

[0039] Step 12: Establish the connection relationship between nodes to form multiple topological edges, which are used to represent the connection paths within the same layer and the connection paths between layers;

[0040] Step 13: Connect multiple topological edges to form a graph structure;

[0041] Step 14: Perform redundant edge elimination, cost calculation, and connectivity check on the graph structure to obtain the inter-floor connection map, which is used to represent the connection relationship and navigation path between floors.

[0042] As a preferred technical measure:

[0043] Step 11, based on the connection relationships and structural characteristics between floors, the method for determining several node information is as follows:

[0044] Based on the connection relationships and structural features between floors, entrances and exits are identified to obtain entrance and exit information; entrances include elevator entrances and stair entrances; exits include elevator exits and stair exits.

[0045] Process entrance and exit information to construct key topology points;

[0046] By combining the task points and specific locations, relevant topology points are selected to obtain the default topology points;

[0047] The default topology points include passageways, corridor intersections, corner points, and rest areas;

[0048] Node numbering and coordinate value calculation are performed on key topology points and default topology points respectively to obtain several node information; the node information includes at least the node number, the floor to which the node belongs, and the coordinates of the node location.

[0049] As a preferred technical measure:

[0050] Step 14 involves performing redundant edge elimination, cost calculation, and connectivity checks on the graph structure to obtain the inter-layer connectivity map. The method is as follows:

[0051] For unused key topological points and default topological points, perform point deletion operations and reset topological edges;

[0052] Redundant edges are eliminated from the deleted topological points and the reset topological edges to obtain new topological edges;

[0053] Calculate the Euclidean distance between topological points;

[0054] Based on Euclidean distance, the cost of each topological edge is calculated.

[0055] The value is assigned to the graph structure, and the connectivity of the graph structure is checked to obtain the inter-layer connection map.

[0056] As a preferred technical measure:

[0057] The method for obtaining the optimal planned path by performing topological path search based on inter-layer connectivity maps and two-dimensional intra-layer maps is as follows:

[0058] Obtain the starting point and destination point for navigation;

[0059] Unify the coordinate system of the starting point and the coordinate system of the target point so that they are the same;

[0060] After unifying the coordinate system, the positional relationship between the starting point and the target point is analyzed to obtain the attribute relationship between the starting point and the target point;

[0061] Attribute relationships include floor information and accessibility information;

[0062] A comprehensive navigation strategy is constructed with the goal of finding the shortest path and minimizing cost.

[0063] Based on the inter-layer connection map and the two-dimensional same-layer map, and based on attribute relationships and comprehensive navigation strategies, a topological path search is performed to obtain the optimal planned path.

[0064] As a preferred technical measure:

[0065] The method for constructing a comprehensive navigation strategy with the goal of finding the shortest path and minimizing cost is as follows:

[0066] Obtain several sets of navigation routes, each set of navigation routes including several task points;

[0067] Calculate the Euclidean distance between two task points to obtain the distance information for each navigation route;

[0068] Based on distance information, select the navigation route corresponding to the shortest Euclidean distance;

[0069] Connect the task points to obtain connecting edges, and determine the cost information of each connecting edge based on Euclidean distance;

[0070] Based on the cost information, select the navigation route corresponding to the minimum cost;

[0071] Based on the shortest and least costly navigation routes, an optimal target calculation formula is constructed.

[0072] Solve the optimal target calculation formula to generate the navigation route that minimizes the overall cost, and realize the construction of the integrated navigation strategy.

[0073] As a preferred technical measure:

[0074] Based on the inter-layer connectivity map and the two-dimensional same-layer map, and based on attribute relationships and comprehensive navigation strategies, the following method is used to perform topological path search to obtain the optimal planned path:

[0075] Step 21: Based on the attribute relationship between the starting point and the target point, use a path planning algorithm to select several sets of grid points from the two-dimensional same-layer map and several sets of topological points from the inter-layer connected map to complete the topological path search;

[0076] Step 22: Arrange and combine several sets of topological points and several sets of grid points to form multiple navigation routes;

[0077] Step 23: Based on the integrated navigation strategy, calculate the Euclidean distance and cost of each navigation route to obtain the navigation route with the minimum overall cost.

[0078] Step 24: Select the navigation route with the lowest overall cost as the optimal planned path.

[0079] As a preferred technical measure:

[0080] Path planning algorithms include the following:

[0081] Read in two-dimensional maps of the same layer and maps connecting different layers;

[0082] By coupling the two-dimensional same-layer map and the inter-layer connected map, a dynamic navigation map is obtained;

[0083] Based on a dynamic navigation map, set up an open list and a closed list.

[0084] The open list includes several nodes to be explored, which are nodes that have not been searched in the dynamic navigation map; the closed list includes several explored nodes, which are nodes that have been searched in the dynamic navigation map.

[0085] The node with the lowest overall cost is selected from the open list and expanded to obtain several nodes; the nodes are grid points or topological points.

[0086] The overall cost is the sum of the Euclidean distance cost and the heuristic cost; the Euclidean distance cost is the cumulative Euclidean distance from the starting point to the current position, and the heuristic cost is the distance from the current position to the target point.

[0087] This invention can adapt to the complex environment of multi-story buildings, including various connecting structures such as stairs, elevators, and corridors. Through layered processing and topology simplification, it improves the real-time performance of path planning and navigation execution, enabling rapid response to environmental changes and navigation needs. At the same time, the solution is detailed and provides a practical cross-floor path navigation scheme, which facilitates the promotion and use of multi-floor navigation solutions.

[0088] To achieve one of the above objectives, the second technical solution of the present invention is as follows:

[0089] An apparatus comprising:

[0090] One or more processors;

[0091] Storage device for storing one or more programs;

[0092] When the one or more programs are executed by the one or more processors, the one or more processors implement the above-described multi-floor composite navigation method for robots.

[0093] Compared with existing technical solutions, the present invention has the following beneficial effects:

[0094] This invention fully considers the characteristics of multi-floor navigation. It divides the global 3D point cloud map into floors to obtain 3D point cloud maps for different floors. Each floor's 3D point cloud map is then projected and rasterized to generate multiple 2D maps of the same floor. Based on the connection relationships and structural features between floors, node information and topological edges are determined. Multiple topological edges are then connected to form a graph structure, resulting in an inter-floor connection map. Finally, based on the inter-floor connection map and the 2D maps of the same floor, a topological path search is performed to obtain the optimal planned path. This enables accurate navigation of the robot in multi-floor environments, and is particularly suitable for multi-floor robot navigation in the same building. The solution is scientific, reasonable, and feasible.

[0095] Meanwhile, the present invention adopts a whole-layer re-layering approach, which, compared with the existing technology of building two-dimensional maps layer by layer and then connecting elevator nodes, can greatly improve the degree of information preservation, effectively avoid the risk of small-scale details such as specific structures in the floors being swallowed up, and facilitate the extraction of floors and various structures between floors from the three-dimensional point cloud, thereby accurately constructing floor maps.

[0096] Furthermore, this invention decomposes the 3D point cloud map into several 2D same-layer maps and inter-layer connected maps, which greatly reduces the computational resource requirements, reduces computational complexity, improves navigation efficiency, and reduces data storage. Therefore, it can meet the real-time navigation requirements, effectively solve the multi-layer navigation problem, and is suitable for real-time operation on embedded systems.

[0097] Meanwhile, this invention uses an inter-floor connection map to represent the connections between floors, enabling accurate navigation of the robot in multi-floor environments and solving the problem that traditional navigation methods cannot effectively handle vertical movement and floor switching. Furthermore, this invention uses a point cloud map for global positioning and combines it with a two-dimensional map of the same floor for local positioning, ensuring high-precision positioning during navigation.

[0098] Furthermore, this invention employs a modular design, independently setting up point cloud maps, 2D same-floor maps, and inter-floor connection maps, facilitating the maintenance and expansion of map modules. Moreover, this invention can adapt to the complex environments of multi-story buildings, including various connecting structures such as stairs, elevators, and corridors. Through layered processing and topology simplification, it improves the real-time performance of path planning and navigation execution, enabling rapid response to environmental changes and navigation needs. The solution is detailed, providing a practical cross-floor path navigation scheme, facilitating the widespread adoption of multi-floor navigation solutions.

[0099] Furthermore, addressing the unique challenges of floor switching, this invention employs a specialized processing mechanism to enhance the stability and reliability of floor switching. Therefore, this invention organically combines point cloud maps, 2D intra-floor maps, and inter-floor connection maps to form a highly integrated navigation system, enabling the maps to work collaboratively and maximize their effectiveness. Attached Figure Description

[0100] Figure 1 This is a flowchart illustrating a multi-floor composite navigation method for robots according to the present invention.

[0101] Figure 2 A structural diagram of a multi-story building to be navigated;

[0102] Figure 3 A schematic diagram of the navigation route generated by applying this invention;

[0103] Figure 4 A schematic diagram illustrating the selection of the optimal planning path for applying this invention. Detailed Implementation

[0104] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, and not all of them. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this application. This invention covers any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of the invention as defined by the claims.

[0105] like Figure 1 As shown, the first specific embodiment of the multi-floor composite navigation method for robots of the present invention is as follows:

[0106] A multi-floor composite navigation method for robots includes the following:

[0107] Collect multi-floor information for navigation purposes;

[0108] Based on multi-floor information, the navigation area is determined, and a 3D point cloud map of the navigation area is obtained;

[0109] The three-dimensional point cloud maps of different floors are identified, and the three-dimensional point cloud maps of each floor are projected and rasterized to generate multiple two-dimensional maps of the same floor.

[0110] The connection relationships between floors are analyzed to obtain an inter-floor connection map;

[0111] Based on the inter-layer connection map and the two-dimensional same-layer map, a topological path search is performed to obtain the optimal planned path, thereby realizing multi-layer composite navigation.

[0112] A second specific embodiment of the multi-floor composite navigation method for robots of the present invention:

[0113] A multi-floor composite navigation method for robots includes a global path planning mechanism, a topological path search algorithm, and a same-floor and adjacent-floor planning method. It represents the multi-floor environment by constructing an inter-floor connection map and combines point cloud maps and two-dimensional same-floor maps to achieve efficient multi-floor navigation.

[0114] In this embodiment, the point cloud map is a collection of three-dimensional point clouds covering the entire navigation space, used for global positioning and environmental awareness. The point cloud map includes the following elements:

[0115] Point cloud data structure: Each point contains location coordinates (x, y, z) and optional attributes such as color and reflectivity.

[0116] Point cloud organization method: The point cloud data is organized using an octree structure to improve query and retrieval efficiency.

[0117] Point cloud resolution: Depending on the complexity of the environment and the positioning accuracy, an appropriate point cloud resolution should be set, usually 1-5cm.

[0118] Point cloud filtering and optimization: Downsampling, outlier removal, and smoothing are performed on the original point cloud to improve quality and efficiency.

[0119] Point cloud map construction is the foundation of multi-floor navigation. It uses the Simultaneous Localization and Mapping Algorithm (SLAM) to process the collected point cloud data and build a global point cloud map.

[0120] In this embodiment, there are multiple two-dimensional same-floor maps, where "multiple" means two or more. The number of maps corresponds to the number of floors in a building. Each two-dimensional same-floor map corresponds to one floor or area and is used for path planning and obstacle avoidance within a single floor. The main characteristics of the two-dimensional same-floor maps include:

[0121] Grid resolution: The actual physical size of each grid cell, typically 5-10 cm.

[0122] Grid status: Each grid cell has three possible states: occupied, free, and unknown.

[0123] Raster probability representation: Using probability values ​​(0-100) to represent the likelihood of a raster being occupied improves the robustness of the map.

[0124] Expansion treatment: The obstacle area is expanded to take into account the actual size of the robot and ensure navigation safety.

[0125] The process of constructing a two-dimensional same-layer map is to convert a three-dimensional point cloud map into several two-dimensional same-layer maps. It mainly includes the following steps: First, the point cloud map is divided into floors, and the point cloud data of different floors is identified, including steps such as height clustering, plane detection, region segmentation and floor marking. The point cloud data of each floor is then subjected to projection processing and rasterization processing, and then a two-dimensional same-layer map can be generated.

[0126] Projection operations include the following:

[0127] Delete the ground point cloud from the point cloud, and unify the z-values ​​of the other point clouds to zero, then perform a compression operation.

[0128] Rasterization includes the following:

[0129] The projected map is converted into a two-dimensional same-layer map. The two-dimensional same-layer map is a map composed of binary graticules, which has only two attributes: 0 and 1. If there are points in the graticule, it is 1; if there are no points, it is 0.

[0130] In this embodiment, the inter-floor connection map is a graph structure composed of nodes (topological points) and edges (topological edges) used to represent the connection relationships and navigation paths between floors. The inter-floor connection map is mainly composed of two types of elements: nodes and edges.

[0131] A topological edge has two nodes. Topological edges represent the floor connections in the map. Paths across floors are represented by solid line segments, while paths within the same floor are represented by dashed line segments. Solid line segments are included in the prior map, while dashed line segments are automatically generated only as needed during navigation. A node represents a specific location point and includes a node number, the floor it belongs to, its location coordinates, node type, and a list of edges.

[0132] The node number is a unique identifier used to distinguish different nodes. The floor number is the number of the 2D map on the same floor where the node is located. The location coordinates are the 2D coordinates (x, y) of the node in the corresponding floor's coordinate system. Node types include ordinary nodes, elevator entrances, stairwell entrances, etc. The edge list includes a list of identifiers (IDs) of all edges connected to this node.

[0133] Connecting edges are used to connect two nodes, representing the navigation path between them. They include index number, coordinate sequence, start and end point numbers, floor number of the start and end points, edge type, width, and height. The cost, slope, and direction of the connecting edge are also considered.

[0134] The index number is a unique identifier for the connecting edge, used to distinguish different edges. The coordinate sequence is a sequence of points describing the geometry of the edge, representing the actual path from the start point to the end point. The start point number and end point number represent the starting and ending node numbers of the connecting edge. The floor number of the start point and the floor number of the end point represent the floor numbers of the start and end points. The category of the connecting edge indicates the type of edge, such as stairs, elevators, corridors, etc. The width and height of the connecting edge represent the width and height of the passage, used to determine whether the robot can pass. The cost of the connecting edge represents the navigation cost of passing through that edge, used for path planning. The slope of the connecting edge is the inclination angle of the passage, used to determine whether the robot can pass. The direction of the connecting edge indicates the direction of travel: 0 indicates unidirectional (opposite to the storage direction), 1 indicates unidirectional (same as the storage direction), and 2 indicates bidirectional.

[0135] The process of constructing an inter-layer connectivity map is the process of representing the connectivity relationships of a multi-layer environment as a graph structure, which mainly includes the following steps:

[0136] Step 1, node generation includes the following:

[0137] First, default topology points are selected, typically by manually or robotically scouting specific locations in conjunction with task points. Then, nodes for the inter-layer connection map are generated at key locations, including steps such as entrance / exit identification, key point extraction, node attribute setting, and node optimization. The identification of entrances / exits and the extraction of key points will be manually or robotically configured based on pre-defined protocol attributes.

[0138] Step 2, edge generation includes the following:

[0139] Establishing connections between nodes to form edges involves steps such as same-level connections, inter-level connections, and attribute settings. Same-level connections are generally determined by the program's automatic default parameters, while inter-level connections are directly determined based on the corresponding attributes input by humans or the robot, ensuring the accuracy of edge generation.

[0140] Step 3, optimization of the inter-layer connection map, includes the following:

[0141] The constructed inter-layer connectivity map is optimized to improve navigation efficiency and robustness, including steps such as redundant edge elimination, cost calculation, connectivity checking, and graph structure optimization. Since various points are temporarily added during the site reconnaissance process for various reasons, and these points are not used later, these points will be manually removed and edges reset. Then, combining the removed points and reset edges, redundant edge elimination, cost calculation, connectivity checking, and graph structure optimization will be performed automatically.

[0142] The cost calculation will be based on the Euclidean distance, while connectivity checks will be performed according to the attribute connection configuration. The data structure of two points and one edge will automatically handle connectivity checks. The cost calculation for Euclidean distance is a direct conversion from Euclidean distance; for example, if the distance between two points is 1m, then the cost of that edge is 1m.

[0143] In this embodiment, the global path planning mechanism analyzes the positional relationship between the starting point and the target point to determine the planning strategy, thereby determining the optimal path from the starting point to the target point. Its main steps include coordinate transformation, floor determination, accessibility analysis, and planning strategy selection, specifically including the following:

[0144] First, determine whether the coordinate systems of the starting point and the target point are consistent. If they are not consistent, perform a coordinate transformation. After determining this, check the positional relationship between the starting point and the target point to obtain their attribute relationship. Based on the attribute relationship, determine which floor the starting point and the target point are located on, whether they are directly accessible, and whether they have any special planning strategies, such as whether they need to cross floors.

[0145] This embodiment mainly involves the following coordinate systems:

[0146] World coordinate system {W}: A global coordinate system that remains relatively stationary with respect to the environment. Its xy coordinate plane is aligned with the horizontal plane, and its z-axis points vertically upward.

[0147] Robot coordinate system {R}: A coordinate system fixed to the robot body, usually located at the robot's center of rotation. The x-axis points forward, the y-axis points to the left, and the z-axis points vertically upward.

[0148] Floor coordinate system {F_i}: An independent two-dimensional coordinate system for each floor, used to represent the floor plan. The xy plane of the floor coordinate system is parallel to the xy plane of the world coordinate system, but the origin is different.

[0149] Sensor coordinate system {S}: A coordinate system fixed to the sensor, used to represent the raw data collected by the sensor.

[0150] The transformation relationships between different coordinate systems are represented by homogeneous transformation matrices, including rotation matrix R and translation vector t. These transformation relationships are the foundation for multi-floor navigation, ensuring the consistency and coherence of data across different coordinate systems.

[0151] In this embodiment, the topology path search algorithm is the core of multi-floor navigation, and it includes the following steps:

[0152] Step 1: Determine the basic rules for topology search, which include the following:

[0153] The process begins by determining the current point's number, searching the number pool for points with the same number, identifying the edge to which the found point belongs, and then continuing the search from the other node of that edge until the target point is found. The number pool consists of all topological path points, which can be pre-collected manually or by a robot.

[0154] Step 2: Based on user needs or environmental conditions, select a navigation strategy, including comprehensive navigation strategy, stair-only strategy, elevator-only strategy, shortest path strategy, and minimum cost strategy. Taking the comprehensive navigation strategy as an example, its general settings are as follows: it supports stair access and ensures the final generated planning trajectory satisfies both the shortest path G and the minimum cost. For Euclidean distance, minimum cost The minimum cost is determined by the topological properties of each edge. The value can be determined directly based on the Euclidean distance. Later, as needed, relevant weight coefficients can be added to modify it, thereby forming the comprehensive cost RET.

[0155] The formula for calculating the overall cost is as follows:

[0156]

[0157] Among them, Euclidean distance The formula is:

[0158]

[0159] in, The X-axis coordinates of the topological point or grid point. The Y-axis coordinate of the topology point or grid point.

[0160] Minimum cost is a compensation value, which can also be determined based on experience.

[0161] Step 3: Iterate through all possible route combinations and, using the strategy selection formula described above, calculate the total cost of each route. This includes route generation, cost calculation, route sorting, and optimal route selection. Due to the diversity of the inter-layer connectivity map, multiple paths may be calculated. Then, calculate the comprehensive cost of each route. Finally, the route with the lowest overall cost is selected as the optimal planning path.

[0162] In this embodiment, the same-layer and adjacent-layer planning methods include the same-layer planning mechanism and the adjacent-layer planning mechanism.

[0163] When the starting point and the destination point are on the same floor, a same-floor planning mechanism is constructed by combining path planning algorithms, which includes the following:

[0164] The path planning algorithm in this embodiment is a heuristic path planning algorithm that efficiently searches for the optimal path by combining the actual path cost and the estimated remaining cost. In this scenario, the path planning algorithm reads in the entire two-dimensional map and searches for a suitable trajectory within it. The algorithm maintains an open list and a closed list.

[0165] The open list includes several nodes to be explored, which are nodes that have not been searched in the 2D same-layer map; the closed list includes several explored nodes, which are nodes that have been searched in the 2D same-layer map.

[0166] Each time, the node with the lowest overall cost is selected from the open list for expansion, gradually approaching the target. The overall cost is the actual cost g(n), which is the cumulative Euclidean distance cost from the starting point to the current position, plus the heuristic estimated cost h(n), where h(n) is the distance from the current position to the target point.

[0167] The heuristic function h(n) is typically constructed using Manhattan distance or Euclidean distance. This function optimizes the path by dynamically updating the node value g(n) and parent node pointers until the target is found or the open list is empty. When the actual cost of the starting point g(S) = 0, the heuristic cost is h(S) = h(S,G), and the total cost is... The calculation formula is as follows:

[0168]

[0169] European distance The calculation formula is as follows:

[0170]

[0171] Manhattan distance The calculation formula is as follows:

[0172]

[0173] The adjacency layer planning mechanism includes the following:

[0174] When the starting point and the target point are on adjacent floors, the adjacency layer planning method is used, which includes two steps: edge identification and three-segment path planning. Edge identification is mainly determined by the properties of the topology. The three-segment path planning includes the following:

[0175] When the starting position Pe and the node position P2 both belong to floor F2, the trajectory route1 is generated by the path planning algorithm; when the positions P2 and P3 belong to the two endpoints of the edge β connecting floors F2 and F3, the trajectory is the fixed path route2 corresponding to edge β; when the positions P3 and the target position Ps both belong to floor F3, the trajectory route3 is generated by the path planning algorithm.

[0176] In this scenario, the path planning algorithm reads in a two-dimensional map of the same layer and a map connecting different layers, and it searches for a suitable trajectory in both maps.

[0177] like Figure 2 As shown, a specific embodiment of navigating a robot using the method of the present invention is as follows:

[0178] Given that the starting point of a robot belongs to floor F1 and the target endpoint belongs to floor F3.1, Table 1 below is the attribute table of the topological edges in the planning space.

[0179] Table 1

[0180]

[0181] Navigating a robot based on the method of this invention includes the following steps:

[0182] S1: Set the selection strategy, which includes taking the stairs / elevator, taking the stairs only, and taking the elevator only. In this embodiment, a total of four feasible routes to the destination are obtained, which can be found in [reference]. Figure 3 .

[0183] S2: Traverse all route combinations and calculate the cost. The cost of each route is the sum of the distances between neighboring points, where the cost of topological edges is a fixed value, and the cost of dashed lines is the Euclidean distance.

[0184] The calculation process for the overall cost of Line 1 is as follows:

[0185] Obtain the cost from the starting point (start) to position A, the cost of edge AB, the cost from position B to position G, the cost of edge GH, and the cost from position H to the destination (goal). Then calculate the sum of the costs for route 1, which is the total cost.

[0186] The calculation process for the overall cost of Route 2 is as follows:

[0187] Obtain the cost from the starting point (start) to position A, the cost of edge AB, the cost from position B to position E, the cost of edge EF, and the cost from position F to the destination (goal). Then calculate the sum of costs for route 2. The calculation process for the combined cost of routes 3 and 4 is similar to that of routes 1 and 2. S3: Select the route with the minimum total cost as the planned route, expressed as follows:

[0188] start—A—B—E—F—goal.

[0189] S4: Based on the same-layer planning mechanism, the planned path is from the starting point (start) to position A, and then the robot is controlled to reach position A; based on the adjacent-layer planning mechanism, the robot is controlled to reach position B along edge AB; based on the same-layer planning mechanism, the planned path is from position B to position E, and then the robot is controlled to reach position E; based on the adjacent-layer planning mechanism, the robot is controlled to reach position F along edge EF; based on the same-layer planning mechanism, the planned path is from position F to the goal point (goal), and then the robot is controlled to reach the destination. (See also...) Figure 4 .

[0190] Therefore, the multi-floor composite navigation method for robots proposed in this invention represents the connection relationships between floors through an inter-floor connectivity map, achieving accurate navigation of the robot in multi-floor environments and solving the difficulties of traditional navigation methods in handling vertical movement and floor switching. Compared to constructing a complete 3D environment map, this invention decomposes the 3D navigation problem into a 2D navigation problem and an inter-floor topological connectivity problem, greatly reducing the computational resource requirements and making it suitable for real-time operation on embedded systems. Simultaneously, it features flexible navigation strategies, supporting multiple navigation strategies (such as taking stairs / elevators, taking stairs only, and taking elevators only), allowing for flexible selection of the optimal path based on actual needs and environmental conditions. Furthermore, it uses a point cloud map for global localization and combines it with a 2D same-floor map for local localization, ensuring high-precision positioning during the navigation process.

[0191] Furthermore, this invention adopts a modular design, with each module map (point cloud map, 2D same-floor map, and inter-floor connection map) being relatively independent, facilitating maintenance and expansion. Consequently, this invention can adapt to the complex environments of multi-story buildings, including various connecting structures such as stairs, elevators, and corridors. The method of this invention completely breaks free from the limitations of the navigation framework ROS, employing a self-developed architecture with stronger scalability, facilitating customization and optimization according to specific needs. Through layered processing and topology simplification, the real-time performance of path planning and navigation execution is improved, enabling rapid response to environmental changes and navigation requirements. A dedicated processing mechanism is designed to address the specific challenges during floor switching, improving the stability and reliability of floor switching. Therefore, this invention organically combines point cloud maps, 2D same-floor maps, and inter-floor connection maps to form a highly integrated navigation system, enabling each module map to work collaboratively and thus maximize efficiency.

[0192] An embodiment of a device applying the method of the present invention:

[0193] An electronic device comprising:

[0194] One or more processors;

[0195] Storage device for storing one or more programs;

[0196] When the one or more programs are executed by the one or more processors, the one or more processors implement the above-described multi-floor composite navigation method for robots.

[0197] An embodiment of a computer medium applying the method of the present invention:

[0198] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described multi-floor composite navigation method for a robot.

[0199] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application 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, optical storage, etc.) containing computer-usable program code.

[0200] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features within the scope of the technology disclosed in the present invention; and these modifications or substitutions will not cause the substance of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. Any modifications or equivalent substitutions that do not deviate from the spirit and scope of the present invention should be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A multi-floor composite navigation method for a robot, characterized in that: Includes the following: Collect multi-floor information for navigation purposes; Based on multi-floor information, the navigation area is determined, and a 3D point cloud map of the navigation area is obtained; The three-dimensional point cloud maps of different floors are identified, and the three-dimensional point cloud maps of each floor are projected and rasterized to generate multiple two-dimensional maps of the same floor. The connection relationships between floors are analyzed to obtain an inter-floor connection map; It includes the following: Step 11: Based on the connection relationships between floors and the structural characteristics between floors, determine the information of several nodes; Step 12: Establish the connection relationship between nodes to form multiple topological edges, which are used to represent the connection paths within the same layer and the connection paths between layers; Step 13: Connect multiple topological edges to form a graph structure; Step 14: Perform redundant edge elimination, cost calculation and connectivity check on the graph structure to obtain the inter-layer connection map, which is used to represent the connection relationship and navigation path between floors; Based on the inter-layer connection map and the two-dimensional same-layer map, a topological path search is performed to obtain the optimal planned path, thereby realizing multi-layer composite navigation.

2. The multi-floor composite navigation method for a robot as described in claim 1, characterized in that: The method for determining the navigation area and obtaining a 3D point cloud map of the navigation area based on multi-floor information is as follows: Based on multi-floor information, the navigation area is defined; Set the point cloud resolution according to the environmental complexity and positioning accuracy requirements of the navigation area; Based on the point cloud resolution, obtain the original point cloud covering the entire navigation area; The original point cloud was downsampled, outlier removed, and smoothed to obtain an optimized point cloud. An octree subdivision algorithm is used to split the optimized point cloud into several sub-point clouds; By using a simultaneous localization and mapping (SMR) algorithm, several sub-point clouds are processed to obtain a 3D point cloud map.

3. The multi-floor composite navigation method for a robot as described in claim 1, characterized in that: The method for identifying 3D point cloud maps of different floors, projecting and rasterizing the 3D point cloud map of each floor separately, and generating multiple 2D maps of the same floor is as follows: Acquire a 3D point cloud map and identify the 3D point cloud maps located on different floors. Divide the 3D point cloud map by floor to obtain the point cloud data for each floor. Projection processing is performed on the point cloud data of each floor, which involves deleting the ground point cloud from the point cloud data, unifying the vertical coordinate values ​​of the remaining point cloud data to zero, performing compression operations, and generating a two-dimensional map. The projected 2D map is rasterized to generate a 2D same-layer map. A two-dimensional same-layer map is a map composed of binary graticules, which have only two attributes: 0 and 1. Each graticule contains point clouds and is 1, while the absence of point clouds is 0.

4. The multi-floor composite navigation method for a robot as described in claim 1, characterized in that: Step 11, based on the connection relationships and structural characteristics between floors, the method for determining several node information is as follows: Based on the connection relationships and structural features between floors, entrances and exits are identified to obtain entrance and exit information; entrances include elevator entrances and stair entrances; exits include elevator exits and stair exits. Process entrance and exit information to construct key topology points; By combining task points and specific locations, relevant topology points are selected to obtain default topology points; The default topology points include passageways, corridor intersections, corner points, and rest areas; Node numbering and coordinate value calculation are performed on key topology points and default topology points respectively to obtain several node information; the node information includes at least the node number, the floor to which the node belongs, and the coordinates of the node location.

5. The multi-floor composite navigation method for a robot as described in claim 4, characterized in that: Step 14 involves performing redundant edge elimination, cost calculation, and connectivity checks on the graph structure to obtain the inter-layer connectivity map. The method is as follows: For unused key topology points and default topology points, perform point deletion operations and reset topology edges; Redundant edges are eliminated from the deleted topological points and the reset topological edges to obtain new topological edges; Calculate the Euclidean distance between topological points; Based on Euclidean distance, the cost of each topological edge is calculated. The value is assigned to the graph structure, and the connectivity of the graph structure is checked to obtain the inter-layer connection map.

6. The multi-floor composite navigation method for a robot as described in claim 1, characterized in that: The method for obtaining the optimal planned path by performing topological path search based on inter-layer connectivity maps and two-dimensional intra-layer maps is as follows: Obtain the starting point and target point for navigation; Unify the coordinate system of the starting point and the coordinate system of the target point so that they are the same; After unifying the coordinate system, the positional relationship between the starting point and the target point is analyzed to obtain the attribute relationship between the starting point and the target point; Attribute relationships include floor information and accessibility information; A comprehensive navigation strategy is constructed with the goal of finding the shortest path and minimizing cost. Based on the inter-layer connection map and the two-dimensional same-layer map, and based on attribute relationships and comprehensive navigation strategies, a topological path search is performed to obtain the optimal planned path.

7. The multi-floor composite navigation method for a robot as described in claim 6, characterized in that: The method for constructing a comprehensive navigation strategy with the goal of finding the shortest path and minimizing cost is as follows: Obtain several sets of navigation routes, each set of navigation routes including several task points; Calculate the Euclidean distance between two task points to obtain the distance information for each navigation route; Based on distance information, select the navigation route corresponding to the shortest Euclidean distance; Connect the task points to obtain connecting edges, and determine the cost information of each connecting edge based on Euclidean distance; Based on the cost information, select the navigation route corresponding to the minimum cost; Based on the shortest and least costly navigation routes, an optimal target calculation formula is constructed. Solve the optimal target calculation formula to generate the navigation route that minimizes the overall cost, and realize the construction of the integrated navigation strategy.

8. The multi-floor composite navigation method for a robot as described in claim 7, characterized in that: Based on the inter-layer connectivity map and the two-dimensional same-layer map, and based on attribute relationships and comprehensive navigation strategies, the following method is used to perform topological path search to obtain the optimal planned path: Step 21: Based on the attribute relationship between the starting point and the target point, use a path planning algorithm to select several sets of grid points from the two-dimensional same-layer map and several sets of topological points from the inter-layer connected map to complete the topological path search; Step 22: Arrange and combine several sets of topological points and several sets of grid points to form multiple navigation routes; Step 23: Based on the integrated navigation strategy, calculate the Euclidean distance and cost of each navigation route to obtain the navigation route with the minimum overall cost. Step 24: Select the navigation route with the lowest overall cost as the optimal planned path.

9. The multi-floor composite navigation method for a robot as described in claim 8, characterized in that: Path planning algorithms include the following: Read in two-dimensional maps of the same layer and maps connecting different layers; By coupling the two-dimensional same-layer map and the inter-layer connected map, a dynamic navigation map is obtained; Based on a dynamic navigation map, set up an open list and a closed list. The open list includes several nodes to be explored, which are nodes that have not been searched in the dynamic navigation map; the closed list includes several explored nodes, which are nodes that have been searched in the dynamic navigation map. The node with the lowest overall cost is selected from the open list and expanded to obtain several nodes; the nodes are grid points or topological points. The overall cost is the sum of the Euclidean distance cost and the heuristic cost; the Euclidean distance cost is the cumulative Euclidean distance from the starting point to the current position, and the heuristic cost is the distance from the current position to the target point.

10. A device, characterized in that: It includes: One or more processors; Storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement a multi-floor composite navigation method for a robot as described in any one of claims 1-9.

Images