Robot multi-floor composite navigation method and equipment

Abstract

The invention discloses a robot multi-floor composite navigation method and equipment, and belongs to the technical field of robot navigation. According to an existing robot navigation scheme, in the multi-floor navigation process, the information storage degree of a constructed grid map is low, and an optimal planning path cannot be obtained. According to the robot multi-floor composite navigation method, the multi-floor navigation characteristics are fully considered, the three-dimensional point cloud map is segmented according to the floors to obtain the three-dimensional point cloud maps of different floors, so that the information storage degree can be greatly improved, the three-dimensional point cloud map of each floor is subjected to projection and rasterization processing, and the navigation efficiency is improved. Generating a plurality of two-dimensional same-layer maps; analyzing the connection relationship between the floors and the structural characteristics between the floors to obtain an interlayer connection map; and based on the inter-floor connection map and the two-dimensional same-floor map, topological path search is carried out to obtain an optimal planning path, so that accurate navigation of the robot in the same-building multi-floor environment can be realized.

Description

Technical Field

[0001] The present invention relates to a robot multi-floor composite navigation method and equipment, belonging to the technical field of robot navigation. Background Art

[0002] A Chinese patent application (publication number: CN116718189A) provides a mobile robot and a cross-building path planning method for the mobile robot. 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 structure; constructing a topological map of the cross-building structure, wherein the topological map of the cross-building structure includes the distances and connectivity relationships between various stations in the cross-building structure; and constructing a two-dimensional grid map of one or more floors of one or more buildings in the cross-building structure and the topological map of the cross-building structure.

[0003] The above solution directly constructs a two-dimensional grid map of buildings across buildings based on point cloud data. Small-scale details such as the specific structures on the floors may be swallowed up, resulting in a relatively low level of floor information preservation. In addition, the solution does not disclose how to extract floors and inter-floor structures other than elevators from the three-dimensional point cloud, which affects the accuracy of the floor map construction.

[0004] Furthermore, the above scheme is mainly used for path planning across buildings. Although it mentions mapping the elevator car points in the topological map of each floor to the topological map of the cross-building building as the connection point between nodes on different floors, there is relatively little content involving cross-floor scenes in the same building, and the robot may not be able to take the elevator to go up and down the stairs. Even if it can take the elevator, in some cases, it is not the optimal path choice. Therefore, this scheme cannot obtain the optimal planning path during the cross-floor navigation process in the same building, and cannot achieve accurate navigation of the robot in a multi-floor environment. Furthermore, the above scheme mentions that the A* algorithm can be used to find the shortest path from a specified starting point to a specified end point, but no specific implementation process is given, resulting in the inability to implement the multi-floor navigation scheme, affecting the promotion and use of the multi-floor navigation scheme. Summary of the Invention

[0005] In response to the above problem or one of the above problems, an object of the present invention is to provide a robot multi-floor composite navigation method and device, which divides the three-dimensional point cloud map covering the entire world by floor to obtain three-dimensional point cloud maps of different floors, and projects and rasterizes the three-dimensional point cloud map of each floor separately to generate multiple two-dimensional same-floor maps, which can greatly improve the degree of information preservation and effectively avoid the risk of small-scale details such as specific structures in the floor being swallowed up; then, based on the connection relationship between floors and the structural characteristics between floors, the node information and topological edges are determined, and then the multiple topological edges are connected to form a graph structure to obtain an inter-layer connection map, thereby realizing the extraction of various structures between floors and between floors, so that the floor map can be accurately constructed; and then, based on the inter-layer connection map and the two-dimensional same-floor map, a topological path search is performed to obtain the optimal planning path, thereby realizing accurate navigation of the robot in a multi-floor environment, which is particularly suitable for multi-floor robot navigation in the same building scene.

[0006] In response to the above problem or one of the above problems, the second purpose of the present invention is to provide a robot multi-floor composite navigation method and equipment, which represents the connection relationship between floors through an inter-layer connection map, realizes accurate navigation of the robot in a multi-floor environment, and solves the problem that traditional navigation methods cannot effectively handle vertical movement and floor switching; and performs global positioning through point cloud maps and local positioning in combination with two-dimensional same-floor maps, thereby ensuring high-precision positioning during the navigation process.

[0007] In response to the above problem or one of the above problems, the third object of the present invention is to provide a robot multi-floor composite navigation method and device, which can adapt to the complex environment in multi-floor buildings, including various connection structures such as stairs, elevators, and corridors, and through hierarchical processing and topological simplification, improve the real-time performance of path planning and navigation execution, and can quickly respond to environmental changes and navigation needs. The solution is detailed and provides a practical cross-floor path navigation solution, which facilitates the promotion and use of multi-floor navigation solutions.

[0008] To achieve one of the above purposes, the first technical solution of the present invention is: A robot multi-floor composite navigation method, comprising the following contents: Collect multi-floor information to be navigated; Based on the multi-floor information, determine the navigation area and obtain a three-dimensional point cloud map of the navigation area; Identify the 3D point cloud maps of different floors, project and rasterize the 3D point cloud maps of each floor, and generate multiple 2D maps of the same floor; Analyze the connection relationship between floors to obtain an inter-floor connection map; it includes the following contents: Step 11: Determine a number of node information based on the connection relationship between floors and the structural characteristics between floors; Step 12: Establish connection relationships between nodes to form multiple topological edges for representing same-layer connection paths and inter-layer connection paths; Step 13: Connect multiple topological edges to form a graph structure; Step 14: Eliminate redundant edges, calculate costs, and check connectivity of the graph structure to obtain an 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 planning path and realize multi-floor composite navigation.

[0009] The present invention fully considers the characteristics of multi-floor navigation, divides the global three-dimensional point cloud map by floor, obtains three-dimensional point cloud maps of different floors, and projects and rasterizes the three-dimensional point cloud map of each floor to generate multiple two-dimensional same-floor maps; then, based on the connection relationship between floors and the structural characteristics between floors, node information and topological edges are determined, and multiple topological edges are connected to form a graph structure to obtain an inter-layer connection map; then, based on the inter-layer connection map and the two-dimensional same-floor map, a topological path search is performed to obtain the optimal planning path; therefore, by first identifying the floor and then processing the raster map, the degree of information preservation can be greatly improved, and the risk of small-scale details such as the specific structure in the floor being swallowed up can be effectively avoided, and various structures on the floor and between floors can be extracted, so that the floor map can be accurately constructed, thereby realizing accurate navigation of the robot in a multi-floor environment, which is particularly suitable for multi-floor robot navigation in the same building scenario. The scheme is detailed and feasible.

[0010] Furthermore, the present invention decomposes the three-dimensional point cloud map into several two-dimensional same-layer maps and inter-layer connection maps, which greatly reduces the demand for computing resources, reduces computational complexity, improves navigation efficiency, and reduces data storage. Therefore, it can meet real-time navigation needs, effectively solve multi-floor navigation problems, and is suitable for real-time operation on embedded systems.

[0011] The present invention also uses an inter-floor connection map to represent the connections between floors, enabling accurate robot navigation in multi-floor environments. This addresses the inability of traditional navigation methods to effectively handle vertical movement and floor switching. Furthermore, the present invention uses a point cloud map for global positioning, combined with a two-dimensional same-floor map for local positioning, ensuring high-precision positioning during navigation.

[0012] As preferred technical measures: The method for determining the navigation area based on multi-floor information and obtaining the 3D point cloud map of the navigation area is as follows: Based on multi-floor information, define the navigation area; Set the point cloud resolution based on 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 is subjected to downsampling, outlier removal and smoothing to obtain an optimized point cloud; The octree subdivision algorithm is used to split the optimized point cloud into several sub-point clouds; Through the simultaneous positioning and mapping algorithm, several sub-point clouds are processed to obtain a three-dimensional point cloud map.

[0013] As preferred technical measures: The method for identifying the 3D point cloud maps of different floors, projecting and rasterizing the 3D point cloud maps of each floor, and generating multiple 2D maps of the same floor is as follows: Obtain a 3D point cloud map, identify the 3D point cloud maps located on different floors, and segment the 3D point cloud map by floor to obtain point cloud data for each floor; Project the point cloud data of each floor, that is, delete the ground point cloud in the point cloud data, unify the vertical coordinate values ​​of the remaining point cloud data to zero, perform compression operation, and generate a two-dimensional map; The projected two-dimensional map is rasterized to generate a two-dimensional same-layer map; A two-dimensional same-layer map is a map composed of binary grids, which has only two attributes, 0 and 1. Each grid has a point cloud value of 1 and a point cloud value of 0.

[0014] As preferred technical measures: The method for analyzing the connection relationship between floors and obtaining the inter-layer connection map is as follows: Step 11: Determine a number of node information based on the connection relationship between floors and the structural characteristics between floors; Step 12: Establish connection relationships between nodes to form multiple topological edges for representing same-layer connection paths and inter-layer connection paths; Step 13: Connect multiple topological edges to form a graph structure; In step 14, redundant edges are eliminated, costs are calculated, and connectivity is checked on the graph structure to obtain an inter-layer connection map, which is used to represent the connection relationship and navigation path between floors.

[0015] As preferred technical measures: Step 11: Based on the connection relationship between floors and the structural characteristics between floors, a method for determining information of several nodes is as follows: According to the connection relationship between floors and the structural characteristics between floors, the entrances and exits are identified to obtain entrance and exit information; the entrances include elevator entrances and stair entrances; the exits include elevator exits and stair exits; Process the entrance and exit information and construct key topological points; Combine the task points and specific points, select relevant topological points, and obtain the default topological points; Default topological points include aisles, corridor intersections, corner points, and rest areas; Node numbers and coordinate values ​​are calculated for key topological points and default topological points respectively to obtain a number of node information; the node information at least includes the node number, the floor to which the node belongs, and the node position coordinates.

[0016] As preferred technical measures: Step 14: Eliminate redundant edges, calculate costs, and check connectivity of the graph structure to obtain an inter-layer connection map as follows: For unused key topological points and default topological points, delete the points and reset the topological edges; Eliminate redundant edges on the deleted topological points and reset topological edges to obtain new topological edges; Calculate the Euclidean distance between topological points; Based on the Euclidean distance, the cost of the topological edge is calculated to obtain the cost value of each topological edge; The cost value is assigned to the graph structure, and the connectivity of the graph structure is checked to obtain the inter-layer connection map.

[0017] As preferred technical measures: Based on the inter-layer connection map and the two-dimensional same-layer map, the topological path search is performed to obtain the optimal planning path as follows: Get the starting point and target point to be navigated; Unify the coordinate system of the starting point and the coordinate system of the target point so that the two coordinate systems 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 relations include floor information and accessibility information; Build a comprehensive navigation strategy with the shortest path and minimum cost as the goal; According to the inter-layer connection map and the two-dimensional same-layer map, and based on the attribute relationship and comprehensive navigation strategy, a topological path search is performed to obtain the optimal planned path.

[0018] As preferred technical measures: The method of constructing a comprehensive navigation strategy with the shortest path and minimum cost as the goal is as follows: Obtain several groups of navigation routes, each group of navigation routes includes several mission points; Calculate the Euclidean distance between two task points to obtain the distance information of each set of navigation routes; Based on the distance information, filter out the navigation route corresponding to the shortest Euclidean distance; Connect the task points to obtain connecting edges, and determine the cost value of each connecting edge based on the Euclidean distance; According to the cost value information, filter out the navigation route corresponding to the minimum cost; Based on the navigation route with the shortest distance and the navigation route with the lowest cost, the optimal target calculation formula is constructed; Solve the optimal target calculation formula to generate a navigation route that satisfies the minimum comprehensive cost and realize the construction of a comprehensive navigation strategy.

[0019] As preferred technical measures: Based on the inter-layer connection map and the two-dimensional same-layer map, and based on the attribute relationship and the comprehensive navigation strategy, the topological path search is performed to obtain the optimal planning path as follows: Step 21, based on the attribute relationship between the starting point and the target point, a path planning algorithm is used to select several groups of grid points from the two-dimensional same-layer map and several groups of topological points from the inter-layer connection map to complete the topological path search; Step 22: Arrange and combine several groups of topological points and several groups of grid points to form multiple navigation routes; Step 23: Calculate the Euclidean distance and cost value of each navigation route according to the comprehensive navigation strategy to obtain the navigation route with the minimum comprehensive cost; Step 24: The navigation route with the minimum comprehensive cost is taken as the optimal planning path.

[0020] As preferred technical measures: The path planning algorithm includes the following: Read in the 2D same-layer map and inter-layer connection map; Couple the two-dimensional same-layer map and the inter-layer connection map to obtain a dynamic navigation map; Based on the 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 participated in the search in the dynamic navigation map; the closed list includes several explored nodes, which are nodes that have participated in the search in the dynamic navigation map; Selecting a node with the smallest comprehensive cost from the open list for expansion to obtain a plurality of nodes; the nodes are grid points or topological points; The comprehensive 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.

[0021] The present invention can adapt to the complex environment of multi-story buildings, including various connection structures such as stairs, elevators, and corridors. Through layered processing and topological simplification, it improves the real-time performance of path planning and navigation execution, and can quickly respond to environmental changes and navigation needs. At the same time, the scheme is detailed and provides a practical cross-floor path navigation scheme, which facilitates the promotion and use of multi-floor navigation schemes.

[0022] To achieve one of the above purposes, the second technical solution of the present invention is: A device comprising: one or more processors; a 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 the above-mentioned robot multi-floor composite navigation method.

[0023] Compared with the existing technical solutions, the present invention has the following beneficial effects: The present invention fully considers the characteristics of multi-floor navigation, divides the global three-dimensional point cloud map by floor, obtains three-dimensional point cloud maps of different floors, and projects and rasterizes the three-dimensional point cloud map of each floor respectively to generate multiple two-dimensional same-floor maps; then, based on the connection relationship between floors and the structural characteristics between floors, node information and topological edges are determined, and multiple topological edges are connected to form a graph structure to obtain an inter-layer connection map; then, based on the inter-layer connection map and the two-dimensional same-floor map, a topological path search is performed to obtain the optimal planned path, thereby realizing accurate navigation of the robot in a multi-floor environment, and is particularly suitable for multi-floor robot navigation in the same building scenario. The scheme is scientific, reasonable and feasible.

[0024] At the same time, the present invention adopts an overall re-layering approach, which can greatly improve the degree of information preservation compared to the existing technology of constructing a two-dimensional map layer by layer and then connecting elevator nodes. It effectively avoids the risk of small-scale details such as the specific structure on the floor being swallowed up, and facilitates the extraction of floors and various structures between floors from the three-dimensional point cloud, so that the floor map can be accurately constructed.

[0025] Furthermore, the present invention decomposes the three-dimensional point cloud map into several two-dimensional same-layer maps and inter-layer connection maps, which greatly reduces the demand for computing resources, reduces computational complexity, improves navigation efficiency, and reduces data storage. Therefore, it can meet real-time navigation needs, effectively solve multi-floor navigation problems, and is suitable for real-time operation on embedded systems.

[0026] The present invention also uses an inter-floor connection map to represent the connections between floors, enabling accurate robot navigation in multi-floor environments. This addresses the inability of traditional navigation methods to effectively handle vertical movement and floor switching. Furthermore, the present invention uses a point cloud map for global positioning, combined with a two-dimensional same-floor map for local positioning, ensuring high-precision positioning during navigation.

[0027] Furthermore, the present invention adopts a modular design, independently configuring point cloud maps, two-dimensional same-floor maps, and inter-floor connection maps, facilitating maintenance and expansion of map modules. Furthermore, the present invention can adapt to the complex environments of multi-story buildings, including various connecting structures such as stairs, elevators, and hallways. Through layered processing and topological simplification, it improves the real-time performance of path planning and navigation execution, enabling rapid response to environmental changes and navigation needs. Furthermore, the detailed solution provides a practical cross-floor path navigation solution, facilitating the widespread use of multi-floor navigation solutions.

[0028] Furthermore, the present invention addresses the unique challenges of floor switching by designing a specialized processing mechanism, improving the stability and reliability of floor switching. Consequently, the present invention organically combines point cloud maps, two-dimensional same-floor maps, and inter-floor connection maps to form a highly integrated navigation system, enabling each map to work collaboratively to maximize efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 A schematic diagram of a flow chart of the multi-floor composite navigation method of the robot according to the present invention; Figure 2 A schematic diagram of a structure of a multi-story building to be navigated; Figure 3 A navigation route diagram generated by applying the present invention; Figure 4 A schematic diagram for selecting the optimal planning path for applying the present invention. DETAILED DESCRIPTION

[0030] In order 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 in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only embodiments of a part of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work should fall within the scope of protection of this application. The present invention covers any substitution, modification, equivalent method and scheme made on the essence and scope of the present invention as defined by the claims.

[0031] like Figure 1As shown, the first specific embodiment of the robot multi-floor composite navigation method of the present invention is: A robot multi-floor composite navigation method, comprising the following contents: Collect multi-floor information to be navigated; Based on the multi-floor information, determine the navigation area and obtain a three-dimensional point cloud map of the navigation area; Identify the 3D point cloud maps of different floors, project and rasterize the 3D point cloud maps of each floor, and generate multiple 2D maps of the same floor; Analyze the connection relationship between floors and obtain the inter-floor connection map; Based on the inter-layer connection map and the two-dimensional same-layer map, a topological path search is performed to obtain the optimal planning path and realize multi-floor composite navigation.

[0032] The second specific embodiment of the robot multi-floor composite navigation method of the present invention is as follows: A robot multi-floor composite navigation method includes a global path planning mechanism, a topological path search algorithm, and same-layer and adjacent-layer planning methods. It represents the multi-floor environment by constructing an inter-layer connection map, and realizes efficient multi-floor navigation by combining a point cloud map and a two-dimensional same-layer map.

[0033] In this embodiment, the point cloud map is a three-dimensional point cloud collection covering the entire navigation space, which is used for global positioning and environmental perception. The point cloud map contains the following elements: Point cloud data structure: Each point contains position coordinates (x, y, z) and optional attributes such as color and reflectivity.

[0034] Point cloud organization method: Use the octree structure to organize point cloud data to improve query and retrieval efficiency.

[0035] Point cloud resolution: Depending on the complexity of the environment and positioning accuracy, an appropriate point cloud resolution is required, usually 1-5 cm.

[0036] Point cloud filtering and optimization: downsample, remove outliers, and smooth the original point cloud to improve quality and efficiency.

[0037] Point cloud map construction is the basis of multi-floor navigation. The collected point cloud data is processed by the simultaneous positioning and mapping algorithm SLAM to construct a global point cloud map.

[0038] In this embodiment, there are multiple 2D same-floor maps, where multiple refers to more than two, and the value is the same as the number of floors in the building. Each 2D same-floor map corresponds to a floor or area and is used for path planning and obstacle avoidance within a single floor. The main features of the 2D same-floor map include: Grid resolution: The actual physical size of each grid cell, usually 5-10cm.

[0039] Grid state: Each grid has three possible states: occupied, free, and unknown.

[0040] Grid probability representation: Use probability values ​​(0-100) to represent the possibility of grid occupation, improving the robustness of the map.

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

[0042] The construction process of a two-dimensional same-layer map is the process of converting 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 by floor, and the point cloud data of different floors are identified, including steps such as height clustering, plane detection, area segmentation and floor marking. The point cloud data of each floor is projected and rasterized, and then a two-dimensional same-layer map can be generated.

[0043] Projection operations include the following: Delete the ground point cloud of the point cloud and unify the z values ​​of other point clouds to zero for compression.

[0044] Rasterization processing, including the following: The projected map is converted into a two-dimensional same-layer map. The two-dimensional same-layer map is a map composed of binary grids, which has only two attributes, 0 and 1. If there is a point cloud at the grid, it is 1, and if there is no point cloud, it is 0.

[0045] In this embodiment, the inter-layer connection map is a graph structure composed of nodes (topological points) and edges (topological edges), which is used to represent the connection relationship and navigation path between floors. The inter-layer connection map mainly consists of two elements: nodes and edges.

[0046] A topological edge has two nodes. It represents the connections between floors in the map. Paths across floors are represented by solid lines, while paths within the same floor are represented by dashed lines. Solid lines are included in the a priori map, while dashed lines are automatically generated as needed during navigation. A node represents a specific location, including its node number, floor, location coordinates, node type, and edge list.

[0047] The node number is a unique identifier for a node, used to distinguish different nodes. The floor is the number of the 2D floor map where the node is located. The location coordinates are the 2D coordinates (x, y) of the node in the corresponding floor coordinate system. Node types include common nodes, elevator entrances, and stair entrances. The edge list contains the IDs of all edges connected to the node.

[0048] A connection edge connects two nodes and represents the navigation path between them. It includes the index number, coordinate sequence, starting and ending numbers, the floors to which the starting and ending floors belong, the type of the connection edge, the width and height of the connection edge, the cost of the connection edge, the slope of the connection edge, and the direction of the connection edge.

[0049] The index number is a unique identifier for the connecting edge, used to distinguish different edges. The coordinate sequence is a sequence of points used to describe the geometric shape of the edge, representing the actual path from the start point to the end point. The starting point number and the end point number represent the starting node number and the ending node number of the connecting edge. The floor to which the starting point belongs and the floor to which the end point belongs represent the floor numbers of the starting point and the end point. The category of the connecting edge represents 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 channel, which are used to determine whether the robot can pass through. The cost of the connecting edge represents the navigation cost of passing through the edge and is used for path planning. The slope of the connecting edge is the inclination angle of the channel, which is used to determine whether the robot can pass through. The direction of the connecting edge represents the direction of passage of the edge, 0 represents one-way (opposite to the storage direction), 1 represents one-way (same as the storage direction), and 2 represents two-way.

[0050] The process of constructing the inter-layer connection map is to represent the connection relationship of the multi-floor environment as a graph structure, which mainly includes the following steps: Step 1, node generation includes the following: First, default topological points are selected, typically in conjunction with task points, with specific locations being manually or robotically surveyed. Next, nodes for the interlayer connection map are generated at key locations. This involves steps such as entry / exit identification, key point extraction, node attribute setting, and node optimization. Entry / exit identification and key point extraction are input and configured manually or robotically, based on pre-defined protocol attributes.

[0051] Step 2, edge generation includes the following: Establishing connections between nodes to form edges involves steps such as same-layer connections, inter-layer connections, and attribute settings. Same-layer connections are typically automatically set by default using program policies, while inter-layer connections are directly determined based on corresponding attributes input by humans or robots to ensure the accuracy of generated edges.

[0052] Step 3, optimization of the inter-layer connection map, includes the following: Optimize the constructed inter-layer connection map 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 survey process for various reasons and are not used later, these points will be manually deleted and edges reset. Then, based on the deleted points and reset edges, redundant edge elimination, cost calculation, connectivity checking, and graph structure optimization will be automatically performed.

[0053] The cost is calculated using Euclidean distance, and connectivity is checked based on attribute connection configuration. The data structure of two points and one side automatically handles connectivity checks. The cost of Euclidean distance is directly converted from Euclidean distance. For example, if the distance between two points is 1 meter, the cost of that side is 1 meter.

[0054] 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. The main steps include coordinate conversion, floor determination, accessibility analysis, and planning strategy selection. Specifically, the following are included: First, determine whether the coordinate systems of the starting point and the target point are unified. If not, perform coordinate conversion. Once this is done, detect the positional relationship between the starting point and the target to obtain the attribute relationship between the starting point and the target point. Based on the attribute relationship, determine which floor the starting point and the target point are located on, whether they can be reached directly, and whether there are any special planning strategies, i.e., whether it is necessary to cross floors.

[0055] This embodiment mainly involves the following coordinate systems: World coordinate system {W}: A global coordinate system that remains stationary relative to the environment, with its xy coordinate axes aligned with the horizontal plane and the z axis pointing vertically upward.

[0056] Robot Coordinate System {R}: A coordinate system fixed to the robot body, typically 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.

[0057] Floor coordinate system {F_i}: Each floor has its own independent two-dimensional coordinate system, used to represent the floor map. The xy plane of the floor coordinate system is parallel to the xy plane of the world coordinate system, but the origin is at a different location.

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

[0059] The transformation relationships between coordinate systems are represented by homogeneous transformation matrices, which include the rotation matrix R and the translation vector t. These transformation relationships are the basis for multi-floor navigation and ensure the consistency and coherence of data in different coordinate systems.

[0060] In this embodiment, the topological path search algorithm is the core of multi-floor navigation, which includes the following steps: Step 1: Determine the basic rules for topology search, which include the following: Determine the current point ID, search for the same ID in the ID pool, determine the edge to which the ID belongs, and then continue searching from the other node of the corresponding edge until the target point is found. The ID pool consists of all topological path points, which can be collected in advance by humans or robots.

[0061] Step 2: Select a navigation strategy based on user needs or environmental conditions, 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, the general settings of the comprehensive navigation strategy are as follows: support stair access, and take the final generated planning trajectory to meet the shortest path G and minimum cost, where the shortest path is the Euclidean distance, the minimum cost The minimum cost is determined by the topological properties of each edge. The value of can be directly determined according to the Euclidean distance, and can be modified by adding relevant weight coefficients as needed to form a comprehensive cost RET.

[0062] The calculation formula for the comprehensive cost is as follows:

[0063] Among them, the Euclidean distance The formula is:

[0064] in, is the X-axis coordinate of the topological point or grid point, The Y-axis coordinate of the topological point or grid point.

[0065] The minimum cost is a compensation value and can also be determined based on experience.

[0066] Step 3: Traverse all possible route combinations and calculate the total cost of each route based on the calculation formula selected by the above strategy, including route generation, cost calculation, route sorting, and optimal route selection. Due to the diversity of the inter-layer connection map, multiple paths may be calculated, and then the comprehensive cost of each route is calculated. , and finally the route with the lowest comprehensive cost is taken as the optimal planning path.

[0067] In this embodiment, the same-layer and adjacent-layer planning method includes a same-layer planning mechanism and an adjacent-layer planning mechanism.

[0068] When the starting point and the destination point are on the same floor, a same-floor planning mechanism is constructed in combination with the path planning algorithm, which includes the following: 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 with the estimated residual cost. In this scenario, the path planning algorithm reads the entire 2D same-layer map and searches for suitable trajectories within it. The path planning algorithm maintains an open list and a closed list.

[0069] The open list includes several nodes to be explored, which are nodes that have not participated in the search in the two-dimensional same-layer map; the closed list includes several explored nodes, which are nodes that have participated in the search in the two-dimensional same-layer map.

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

[0071] The heuristic function h(n) is usually constructed using Manhattan distance or Euclidean distance. The function optimizes the path by dynamically updating the node value g(n) and the parent node pointer 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:

[0072] Euclidean distance The calculation formula is as follows:

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

[0074] Adjacency layer planning mechanism, including the following: When the starting point and the destination point are on adjacent floors, the adjacent layer planning method is used. This method includes two steps: edge identification and three-segment path planning. Edge identification is mainly determined by topological properties. The three-segment path planning includes the following: 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 floor F2 and floor F3, the trajectory is the fixed path route2 corresponding to the edge β; when the position P3 and the target position Ps both belong to floor F3, the trajectory route3 is generated by the path planning algorithm.

[0075] In this scenario, the path planning algorithm reads the 2D same-layer map and the inter-layer connection map, and searches for a suitable trajectory in the 2D same-layer map and the inter-layer connection map.

[0076] like Figure 2 As shown, a specific embodiment of applying the method of the present invention to navigate a robot: It is known that the starting point of a robot, start, belongs to floor F1, and the target destination, goal, belongs to floor F3.1. Table 1 below is the attribute table of the topological edges in the planning space.

[0077] Table 1

[0078] Based on the method of the present invention, the robot is navigated, which includes the following steps: S1: Set the selection strategy, which includes taking stairs / elevator, taking stairs only, and taking elevator only. In this embodiment, a total of 4 feasible routes to the destination are obtained, which can be seen in Figure 3 .

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

[0080] The calculation process of the comprehensive cost of line 1 is as follows: Get 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 end point goal, and then calculate the sum of the costs of line 1, that is, the comprehensive cost.

[0081] The calculation process of the comprehensive cost of line 2 is as follows: Get 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 end point goal, and then calculate the cost sum of route 2. The comprehensive cost calculation process for routes 3 and 4 is similar to that for routes 1 and 2. S3: Select the route with the smallest cost sum as the planned route, which is expressed as follows: start—A—B—E—F—goal.

[0082] S4: Based on the same-layer planning mechanism, the path planned 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 path planned 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 path planned is from position F to the goal point goal, and then the robot is controlled to reach the end point, which can be seen in Figure 4 .

[0083] Therefore, the robot multi-floor composite navigation method proposed in the present invention uses an inter-floor connection map to represent the connection relationship between floors, thereby achieving accurate navigation of the robot in a multi-floor environment and solving the difficulties of traditional navigation methods in dealing with vertical movement and floor switching. Compared with constructing a complete three-dimensional environmental map, the present invention decomposes the three-dimensional navigation problem into a two-dimensional navigation problem and an inter-floor topological connection problem, greatly reducing the computing resource requirements and being suitable for real-time operation on embedded systems. At the same time, it has a flexible navigation strategy and supports multiple navigation strategies (such as taking stairs / elevator, taking stairs only, taking elevator only), and can flexibly select the optimal path according to actual needs and environmental conditions. In addition, global positioning is performed through a point cloud map, combined with local positioning through a two-dimensional same-floor map, to ensure high-precision positioning during the navigation process.

[0084] Furthermore, the present invention adopts a modular design, and each module map (point cloud map, two-dimensional same-layer map, and inter-layer connection map) is relatively independent, which is convenient for maintenance and expansion. Furthermore, the present invention can adapt to the complex environment in multi-story buildings, including various connection structures such as stairs, elevators, and corridors. The method of the present invention completely breaks away from the limitations of the navigation framework ROS and adopts a self-developed architecture with stronger scalability, which is convenient for customization and optimization according to specific needs. Through hierarchical processing and topology simplification, the real-time performance of path planning and navigation execution is improved, and it can quickly respond to environmental changes and navigation needs. In response to the special challenges in the floor switching process, a special processing mechanism is designed to improve the stability and reliability of floor switching. Therefore, the present invention organically combines the point cloud map, the two-dimensional same-layer map and the inter-layer connection map to form a highly integrated navigation system, so that the modules can work together to maximize their efficiency.

[0085] An embodiment of a device applying the method of the present invention: An electronic device comprising: one or more processors; a 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 the above-mentioned robot multi-floor composite navigation method.

[0086] A computer medium embodiment of the method of the present invention: A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the above-mentioned robot multi-floor composite navigation method.

[0087] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, optical storage, etc.) containing computer-usable program code.

[0088] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. 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 above-described embodiments, those skilled in the art should understand that any person skilled in the art can modify the technical solutions described in the above-described embodiments within the technical scope disclosed by the present invention, or replace some of the technical features therein with equivalents. However, these modifications or replacements will not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and any modifications or equivalent replacements that do not deviate from the spirit and scope of the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A robot multi-floor composite navigation method, characterized by: Includes the following: Collect multi-floor information to be navigated; Based on the multi-floor information, determine the navigation area and obtain a three-dimensional point cloud map of the navigation area; Identify the 3D point cloud maps of different floors, project and rasterize the 3D point cloud maps of each floor, and generate multiple 2D maps of the same floor; Analyze the connection relationship between floors and obtain the inter-floor connection map; It includes the following: Step 11: Determine a number of node information based on the connection relationship between floors and the structural characteristics between floors; Step 12: Establish connection relationships between nodes to form multiple topological edges for representing same-layer connection paths and inter-layer connection paths; Step 13: Connect multiple topological edges to form a graph structure; Step 14: Eliminate redundant edges, calculate costs, and check connectivity of the graph structure to obtain an 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 planning path and realize multi-floor composite navigation.

2. A robot multi-floor composite navigation method according to claim 1, characterized in that: The method for determining the navigation area based on multi-floor information and obtaining the 3D point cloud map of the navigation area is as follows: Based on multi-floor information, define the navigation area; Set the point cloud resolution based on 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 is subjected to downsampling, outlier removal and smoothing to obtain an optimized point cloud; The octree subdivision algorithm is used to split the optimized point cloud into several sub-point clouds; Through the simultaneous positioning and mapping algorithm, several sub-point clouds are processed to obtain a three-dimensional point cloud map.

3. A robot multi-floor composite navigation method according to claim 1, characterized in that: The method for identifying the 3D point cloud maps of different floors, projecting and rasterizing the 3D point cloud maps of each floor, and generating multiple 2D maps of the same floor is as follows: Obtain a 3D point cloud map, identify the 3D point cloud maps located on different floors, and segment the 3D point cloud map by floor to obtain point cloud data for each floor; Project the point cloud data of each floor, that is, delete the ground point cloud in the point cloud data, unify the vertical coordinate values ​​of the remaining point cloud data to zero, perform compression operation, and generate a two-dimensional map; The projected two-dimensional map is rasterized to generate a two-dimensional same-layer map; A two-dimensional same-layer map is a map composed of binary grids, which has only two attributes, 0 and 1. Each grid has a point cloud value of 1 and a point cloud value of 0.

4. A robot multi-floor composite navigation method as claimed in claim 1, characterized in that: Step 11: Based on the connection relationship between floors and the structural characteristics between floors, a method for determining information of several nodes is as follows: According to the connection relationship between floors and the structural characteristics between floors, the entrances and exits are identified to obtain entrance and exit information; the entrances include elevator entrances and stair entrances; the exits include elevator exits and stair exits; Process the entrance and exit information and construct key topological points; Combine the task points and specific points, select relevant topological points, and obtain the default topological points; Default topological points include aisles, corridor intersections, corner points, and rest areas; Node numbers and coordinate values ​​are calculated for key topological points and default topological points respectively to obtain a number of node information; the node information at least includes the node number, the floor to which the node belongs, and the node position coordinates.

5. The multi-floor composite navigation method of a robot according to claim 1, characterized in that: Step 14: Eliminate redundant edges, calculate costs, and check connectivity of the graph structure to obtain an inter-layer connection map as follows: For unused key topological points and default topological points, delete the points and reset the topological edges; Eliminate redundant edges on the deleted topological points and reset topological edges to obtain new topological edges; Calculate the Euclidean distance between topological points; Based on the Euclidean distance, the cost of the topological edge is calculated to obtain the cost value of each topological edge; The cost value is assigned to the graph structure, and the connectivity of the graph structure is checked to obtain the inter-layer connection map.

6. A robot multi-floor composite navigation method as claimed in claim 1, characterized in that: Based on the inter-layer connection map and the two-dimensional same-layer map, the topological path search is performed to obtain the optimal planning path as follows: Get the starting point and target point to be navigated; Unify the coordinate system of the starting point and the coordinate system of the target point so that the two coordinate systems 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 relations include floor information and accessibility information; Build a comprehensive navigation strategy with the shortest path and minimum cost as the goal; According to the inter-layer connection map and the two-dimensional same-layer map, and based on the attribute relationship and comprehensive navigation strategy, a topological path search is performed to obtain the optimal planned path.

7. A robot multi-floor composite navigation method as claimed in claim 6, characterized in that: The method of constructing a comprehensive navigation strategy with the shortest path and minimum cost as the goal is as follows: Obtain several groups of navigation routes, each group of navigation routes includes several mission points; Calculate the Euclidean distance between two task points to obtain the distance information of each set of navigation routes; Based on the distance information, filter out the navigation route corresponding to the shortest Euclidean distance; Connect the task points to obtain connecting edges, and determine the cost value of each connecting edge based on the Euclidean distance; According to the cost value information, filter out the navigation route corresponding to the minimum cost; Based on the navigation route with the shortest distance and the navigation route with the lowest cost, the optimal target calculation formula is constructed; Solve the optimal target calculation formula to generate a navigation route that satisfies the minimum comprehensive cost and realize the construction of a comprehensive navigation strategy.

8. A robot multi-floor composite navigation method as claimed in claim 7, characterized in that: Based on the inter-layer connection map and the two-dimensional same-layer map, and based on the attribute relationship and the comprehensive navigation strategy, the topological path search is performed to obtain the optimal planning path as follows: Step 21, based on the attribute relationship between the starting point and the target point, a path planning algorithm is used to select several groups of grid points from the two-dimensional same-layer map and several groups of topological points from the inter-layer connection map to complete the topological path search; Step 22: Arrange and combine several groups of topological points and several groups of grid points to form multiple navigation routes; Step 23: Calculate the Euclidean distance and cost value of each navigation route according to the comprehensive navigation strategy to obtain the navigation route with the minimum comprehensive cost; Step 24: The navigation route with the minimum comprehensive cost is taken as the optimal planning path.

9. A robot multi-floor composite navigation method as claimed in claim 8, characterized in that: The path planning algorithm includes the following: Read in the 2D same-layer map and inter-layer connection map; Couple the two-dimensional same-layer map and the inter-layer connection map to obtain a dynamic navigation map; Based on the 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 participated in the search in the dynamic navigation map; the closed list includes several explored nodes, which are nodes that have participated in the search in the dynamic navigation map; Selecting a node with the smallest comprehensive cost from the open list for expansion to obtain a plurality of nodes; the nodes are grid points or topological points; The comprehensive 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; a 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 the robot multi-floor composite navigation method according to any one of claims 1 to 9.

Images