Passable plane extraction method in multilayer structured environment

Abstract

The invention provides a passable plane extraction method in a multi-layer structured environment, which comprises the following steps of: obtaining a global point cloud of the multi-layer structured environment, and carrying out sampling and filtering processing to obtain a plane structure chart; projecting each point capable of penetrating through the plane to the plane, and calculating a minimum convex polygon containing the points to obtain a plurality of layers of communicated planes; performing gridding processing on the connected multi-layer plane and updating the Euclidean signed distance field to obtain a vertex set, a path and a corresponding weight, and storing the vertex set, the path and the corresponding weight as a multi-layer passable plane undirected cost graph; and obtaining a starting position coordinate and a final position coordinate, and obtaining a feasible path by using the multilayer passable plane undirected cost graph. According to the method, the point cloud information is processed to obtain the passable planes in the structured environment and the types of the passable planes, the connectivity between the planes is judged based on the passable planes to generate the navigation map, and global path search is accelerated.

Description

Technical Field

[0001] The invention belongs to the field of robot autonomous navigation, and in particular relates to a method for extracting a traversable plane in a multi-layer structured environment. Background Art

[0002] Robots are expected to assist in all aspects of human life, especially in built spaces where people spend most of their time, such as homes, factories, offices, shopping malls, and complex buildings. Recent advances in motion control for tracked or legged robots have significantly improved their ability to navigate obstacles such as stairs and slopes. In addition, the development of vision- and lidar-based SLAM technology provides stable and accurate localization information. In order to apply these robots as fully autonomous systems in large 3D complex multi-story built spaces, it is necessary to provide these robots with a method to extract navigable planes in these structured environments, thereby laying the foundation for the robot's autonomous navigation.

[0003] Most current maps only focus on single-layer environments and cannot be applied to complex multi-layer structured environments.

[0004] Since no feasible map structure is extracted, directly using point cloud maps is complex and time-consuming in planning, especially when searching for feasible paths in large-scale maps.

[0005] Due to the lack of semantic information, the robot cannot know what plane it is on, which may lead to dangerous operations when the robot goes up and down stairs. Summary of the invention

[0006] In view of the problems existing in the prior art, the present invention provides a method for extracting traversable planes in a multi-layer structured environment, processes point cloud information to obtain traversable planes and their types in a structured environment, judges the connectivity between planes based on these traversable planes to generate a navigation map, and speeds up the global path search.

[0007] The technical solution of the present invention is as follows:

[0008] A method for extracting traversable planes in a multi-layer structured environment comprises the following steps:

[0009] Obtain a global point cloud of a multi-layer structured environment, perform sampling and filtering, and then obtain a normal map by estimating normals;

[0010] Using the normal map to extract all planes through a region growing method;

[0011] Based on the covariance matrix, all planes are divided into traversable planes and perpendicular planes, and the coplanar and adjacent traversable planes are merged to obtain a plane structure diagram;

[0012] Project each point that can pass through the plane onto the plane, and calculate the smallest convex polygon containing these points to obtain a connected multi-layer plane;

[0013] Meshing the connected multi-layer planes and marking the meshes as unknown meshes, safe meshes, overlapping meshes, boundary meshes or occupied meshes;

[0014] The unknown grid, the occupied grid and the boundary grid are used as obstacles to update the Euclidean signed distance field, obtain the vertex set, the path and the corresponding weight and store them as a multi-layer traversable planar undirected cost graph;

[0015] The coordinates of the starting position and the end position are obtained, and a feasible path is obtained using the multi-layer traversable planar undirected cost graph.

[0016] Preferably, in the method for extracting traversable planes in a multi-layer structured environment, if there are two parallel planes of the same size and close distance, the lower plane is removed.

[0017] Preferably, in the method for extracting a traversable plane in a multi-layer structured environment,

[0018] The grid map is initialized to an unknown state;

[0019] If there is at least one projected point in the mesh, it is set to a safe state;

[0020] Meshes corresponding to intersections between planes are marked as connected, while overlapping non-traversable regions are marked as overlapping;

[0021] Overlapping meshes adjacent to the safety mesh are defined as boundary meshes;

[0022] A vertical plane may be located above as an obstacle. Alpha Shapes is used to find the boundary points of the vertical plane. If the distance to the boundary point is less than the safe distance, the corresponding grid will be set to occupied state.

[0023] Preferably, in the method for extracting a traversable plane in a multi-layer structured environment,

[0024] Use an undirected cost graph G = (V, E, W) to describe these planes;

[0025] Define the vertex set V as the intersection line l i，j The point on

[0026] Assume that the i，j The midpoint of the point; use ESDF to check whether the point is feasible on two adjacent planes; if not feasible, the algorithm checks the points on both sides until the first feasible point is found;

[0027] After obtaining all V, the algorithm traverses all planes and uses A* to determine whether there is a path connecting two vertices, recording the path as an edge E in the plane at the cost of the corresponding weight W.

[0028] Preferably, in the method for extracting a traversable plane in a multi-layer structured environment,

[0029] Getting the starting position w After p0, the algorithm first projects the point onto the nearest plane P1 and uses its projected point 1 p0 is the starting node;

[0030] from 1 Search paths from p0 to all vertices on P1;

[0031] Add feasible paths and their costs to G;

[0032] Final Position w p f It is also treated similarly as the final node;

[0033] Do a breadth-first search within G, connecting edges to find a path from the start node to the final node.

[0034] The present invention has the following beneficial effects:

[0035] 1. Extract the traversable plane from the global point cloud and determine the plane type based on the plane features;

[0036] 2. Use plane information to determine the connectivity between planes, thereby establishing a navigation map between planes;

[0037] 3. Using the navigation map, the global path search is accelerated.

[0038] Other advantages, objectives and features of the present invention will be embodied in part through the following description, and in part will be understood by those skilled in the art through study and practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A process diagram of extracting traversable planes according to an embodiment of the traversable plane extraction method in a multi-layer structured environment provided by the present invention;

[0040] Figure 2 A schematic diagram of grid states of an embodiment of a method for extracting traversable planes in a multi-layer structured environment provided by the present invention;

[0041] Figure 3 A process diagram of an embodiment of a method for extracting traversable planes in a multi-layer structured environment provided by the present invention. DETAILED DESCRIPTION

[0042] The present invention is further described in detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

[0043]

[0044]

[0045] The algorithm takes a global point cloud as input and outputs a traversable plane and its parameters, including the coordinate system Ψ w To the plane coordinate system Ψ p The transformation matrix T p , Euclidean signed distance field (ESDF), indices of adjacent planes and lines. Used in the following text * p indicates that the point belongs to the coordinate system Ψ * .

[0046] Plane extraction and merging (lines 1 to 3): The algorithm first downsamples and filters the input point cloud, and then estimates the normals to obtain the normal map ( Figure 1 (a)). Through region growing [1], the algorithm extracts the plane PL from the point cloud and normal map ( Figure 1 (b)). Inspired by

[22] , the algorithm merges planes belonging to stairs. The inclination is calculated by the covariance matrix and the plane is classified as a traversable plane PL t and the vertical plane PL v The algorithm merges coplanar and adjacent PL t To obtain a complete planar structure. If two parallel planes are of the same size and close to each other, the algorithm will remove the lower plane ( Figure 1 (c)).

[0047] Determine plane connectivity (lines 4 to 9): The algorithm converts each PL t The points are projected onto the plane and the smallest convex polygon containing these points is calculated. Since points near the border may be misclassified, the algorithm slightly expands the border points of the polygon outward. If the intersection point l of the two polygons is found i，j , the algorithm saves two endpoints and the index of the intersecting plane ( Figure 1 (d)).

[0048] Meshing (Lines 10 to 15): The algorithm uses the centroid c of the point cloud i ∈R 3 As a i The origin of the plane, the rising direction of the plane as the x-axis, the direction perpendicular to the plane as the z-axis, so the algorithm can get the transformation matrix T i The rotation matrix Ri∈R3×3 and the translation vector c i ∈R 3 The grid map is initialized to an unknown state. If there is at least one projection point in the grid, it is set to a safe state. The grid corresponding to the intersection between the planes is marked as Interline, and the overlapping non-traversable area is marked as Overlap. The Overlap grid adjacent to the safe grid is defined as Boundary. Considering the vertical plane PL v May be located at PL as an obstacle t Above, the algorithm uses Alpha Shapes[2] to find PL v If the boundary point to PL t If the distance is less than the safe distance, the corresponding grid will be set to Occupied ( Figure 1 (e)). Finally, the algorithm updates the ESDF by considering unknown, occupied, and boundary grids as obstacles ( Figure 1 (f)). So far, the algorithm represents the feasible region in the 3D environment as traversable planes, which keep the structure of the feasible region simple and the data size small as much as possible. The algorithm uses an undirected cost graph G = (V, E, W) to describe these planes. The algorithm defines the vertex set V as the intersection line l i，j Initially, these points are assumed to be located on l i，j The algorithm uses ESDF to check whether the point is feasible on two adjacent planes; if not, the algorithm checks the points on both sides until the first feasible point is found. After obtaining all V, the algorithm traverses all planes and uses A* to determine whether there is a path connecting the two vertices, recording the path as an edge E in the plane at the cost of the corresponding weight W.

[0049] Getting the starting position w After p0, the algorithm first projects the point onto the nearest plane P1 and uses its projected point 1 p0 is the starting node. 1 Search for paths from all vertices on p0 to p1. Add feasible paths and their costs to G. Final position w p f Perform a similar process as the final node. Perform a breadth-first search in G, connecting the edges to find a path from the starting node to the final node.

[0050] 【1】T.Westfechtel, K.Ohno, B.Mertsching, R.Hamada, D.Nickchen, S.Kojima, and S.Tadokoro, "Robust stairway-detection and localization method for mobilerobots using a graph-based model and competing initializations," TheInternational Journal of Robotics Research, vol.37, no.12, pp.1463–1483, 2018.

[0051] 【2】H.Edelsbrunner and EPM¨ucke, "Three-dimensional alpha shapes," ACMTransactions On Graphics (TOG), vol.13, no.1, pp.43–72, 1994.

[0052] The algorithm was tested in a simulation environment. Figure 3 As shown in the figure, the first row shows the point cloud of the environment: plane (a1), platform (b1). The global point cloud can be obtained in advance by radar, handheld scanning devices, etc. The second row shows the traversable planes, the intersection lines between the planes, the plan view, and the searched path to obtain the navigation map. The third row shows the ESDF and the generated trajectory. The table shows the trajectory length L and the trajectory optimization time To. The global trajectory is obtained using the navigation map after the starting point and the end point are given. The proposed method is able to generate feasible and safe trajectories in complex building spaces.

[0053] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and the implementation modes. They can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to specific details.

Claims

1. A method for extracting traversable planes in a multi-layer structured environment, characterized in that: The following steps are involved: Obtain a global point cloud of a multi-layer structured environment, perform sampling and filtering, and then obtain a normal map by estimating normals; Using the normal map to extract all planes through a region growing method; Based on the covariance matrix, all planes are divided into traversable planes and perpendicular planes, and the coplanar and adjacent traversable planes are merged to obtain a plane structure diagram; Project each point that can pass through the plane onto the plane, and calculate the smallest convex polygon containing these points to obtain a connected multi-layer plane; Meshing the connected multi-layer planes and marking the meshes as unknown meshes, safe meshes, overlapping meshes, boundary meshes or occupied meshes; The unknown grid, the occupied grid and the boundary grid are used as obstacles to update the Euclidean signed distance field, obtain the vertex set, the path and the corresponding weight and store them as a multi-layer traversable planar undirected cost graph; The coordinates of the starting position and the end position are obtained, and a feasible path is obtained using the multi-layer traversable planar undirected cost graph.

2. The method for extracting traversable planes in a multi-layer structured environment according to claim 1, characterized in that: If there are two parallel planes of equal size and close proximity, remove the lower plane.

3. The method for extracting traversable planes in a multi-layer structured environment according to claim 2, characterized in that: The grid map is initialized to an unknown state; If there is at least one projected point in the mesh, it is set to a safe state; Meshes corresponding to intersections between planes are marked as connected, while overlapping non-traversable regions are marked as overlapping; Overlapping meshes adjacent to the safety mesh are defined as boundary meshes; A vertical plane may be located above as an obstacle. Alpha Shapes is used to find the boundary points of the vertical plane. If the distance to the boundary point is less than the safe distance, the corresponding grid will be set to occupied state.

4. The method for extracting traversable planes in a multi-layer structured environment according to claim 3, characterized in that: Use an undirected cost graph G = (V, E, W) to describe these planes; Define the vertex set V as the intersection line l i，j The point on Assume that the i，j The midpoint of the point; use ESDF to check whether the point is feasible on the two adjacent planes; if not feasible, the algorithm checks the points on both sides until the first feasible point is found; After obtaining all V, the algorithm traverses all planes and uses A* to determine whether there is a path connecting two vertices, recording the path as an edge E in the plane at the cost of the corresponding weight W.

5. The method for extracting traversable planes in a multi-layer structured environment according to claim 4, characterized in that: Getting the starting position w After p0, the algorithm first projects the point onto the nearest plane P1 and uses its projected point 1 p0 is the starting node; from 1 Search paths from p0 to all vertices on P1; Add the feasible paths and their costs to G; Final Position w p f It is also treated similarly as the final node; Do a breadth-first search within G, connecting edges to find a path from the start node to the final node.

Images