Coverage path planning method and device based on topology graph

Abstract

The present invention provides a topological map-based coverage path planning method and device. Based on depth scan points, the visible boundary of a robot's path to its destination is determined. The robot then advances from its current position toward the visible boundary point. If the distance between the current position and the destination is greater than a set travel distance, the robot advances using the significant feature points between the current position and the destination. Otherwise, the robot proceeds directly to the visible boundary point. This process is repeated until the robot reaches the destination. The detection of visible boundaries and significant feature points in the present invention is less affected by high-noise environments, and the coverage path planning method is computationally efficient and scalable.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a coverage path planning method and device based on a topology graph. Background Art

[0002] Covering unknown spaces is a fundamental capability of mobile robots for active exploration. For fully autonomous system coverage approaches, the robot must actively plan its path to achieve full coverage of the environment. Planning a path that passes through defined feature points while avoiding obstacles is called coverage path planning. The goal is to plan an efficient path that covers a specified area.

[0003] Common coverage path planning methods include boundary-based coverage path planning, next best viewpoint (NBV), and sampling-based coverage path planning.

[0004] Boundary-based coverage path planning is one of the early contributions. Boundary-based methods identify the boundary between free known space and unknown space in the map and repeatedly select one such boundary as the target, eventually leading to complete coverage.

[0005] Computing a series of subsequent sensing positions based on data acquired at previous positions is called the Next-Best-View (NBV). The Next-Best-View NBV algorithm guides the robot navigation through selected positions to ensure comprehensive coverage of the environment. Most existing NBV techniques do not address the issue of navigation safety. In addition, in order to apply information gain-driven coverage path planning strategies for exploration, metric grid maps / voxel maps are often used. Processing voxel maps to identify the next best viewpoint is a computationally intensive task. Calculating the next best viewpoint is costly and time-consuming, and to address this issue, the next best viewpoint is randomly sampled.

[0006] Sampling-based coverage path planning consists of sampling robot configurations that collectively give complete coverage of the structure's boundary, which is known as the coverage sampling problem. Sampling-based methods typically sample view configurations in a next-best-viewpoint (NBV) fashion. Sampling-based methods are computationally more efficient than boundary-based methods, and these methods also apply a cost-utility function to evaluate the exploration potential of sampled nodes to obtain a coverage path.

[0007] Boundary-based coverage path planning, next best viewpoint (NBV), and sampling-based coverage path planning all require prior information about the environment, so they are not suitable for cluttered and dynamic environments. In addition, most existing solutions focus on using occupancy grid maps to plan paths. These maps do not scale well, have low computational efficiency, and are not robust under larger sensor range noise values and are not robust in high-noise environments. Summary of the Invention

[0008] In view of the shortcomings in the prior art, the present invention provides a coverage path planning method and device based on a topology graph.

[0009] The present invention achieves the above technical objectives through the following technical means.

[0010] A coverage path planning method based on a topological map: The industrial computer determines the visible boundary of the robot's driving path to the destination based on the depth scanning points; the robot is controlled to move from the current position to the visible boundary point. If the distance between the current position and the visible boundary point is greater than the set moving distance, the robot uses the significant feature points between the current position and the visible boundary point to move forward; otherwise, it goes directly to the visible boundary point; the above process is repeated until the robot reaches the destination.

[0011] In a further technical solution, the process of determining the visible boundary is as follows: from the depth scanning point p1 to p n When searching counterclockwise, detect the starting point of discontinuity and then determine the end point corresponding to the starting point of discontinuity; from the depth scanning point p n When searching clockwise to p1, the starting point of the discontinuity is detected, and then the end point corresponding to the starting point of the discontinuity is determined; the line between the starting point of the discontinuity and the corresponding end point is the visible boundary.

[0012] In a further technical solution, the discontinuity includes:

[0013] Two consecutive depth scanning points p i and p i+1 They are all obstacle points, i and p i+1 The distance between them exceeds f w :||p i -p i+1 ||>f w ;

[0014] Two consecutive depth scanning points p i and p i+1 At least one of them is a non-obstacle point, and the distance between the non-obstacle point and the robot is the maximum scanning range of the sensor r max ;

[0015] where f w is the minimum width of the boundary that the robot can pass through.

[0016] According to a further technical solution, the process of determining the end point corresponding to the discontinuity starting point is as follows:

[0017] Discontinuity starting point p s (f) and any obstacle point p in the list kThe boundary is visible from the robot's current position and satisfies The obstacle points constitute the set of all valid points V(L a ), the set V(L a ) and the discontinuity starting point p s (f) The point with the shortest distance is the end point p e (f); The list is searched counterclockwise / clockwise when in p s (f) The following is composed of m obstacle points.

[0018] A further technical solution is that the set V(L a ) is an empty set, and the end point p e (f) Set to the starting point p s (f) The distance is f w virtual point to determine the boundary; where f w is the minimum width of the boundary that the robot can pass through.

[0019] According to a further technical solution, the process of determining the significant feature points is as follows:

[0020] The centroid of the triangle formed by the starting point of the obstacle surface, the end point of the obstacle surface and the robot coordinates is considered as a candidate point of a significant feature point. If the distance between the candidate point and all depth scanning points is greater than f w , then the significant feature points will be added to the topological map; where f w is the minimum width of the boundary that the robot can pass through.

[0021] A coverage path planning device based on a topology graph, comprising:

[0022] The visible boundary detection module is used to determine the visible boundary of the robot's path to the destination;

[0023] The salient feature detection module is used to determine the salient feature points between the robot's current position and the visible boundary point it is heading to.

[0024] The beneficial effects of the present invention are:

[0025] (1) The depth sensor of the present invention is only used for depth scanning, and uses the depth scanning points to detect visible boundaries and significant features. The detection results are less affected by high noise environments;

[0026] (2) The present invention determines the corresponding visible boundary points and significant feature points at the current position of the robot in real time, constructs a topological map, and realizes coverage path planning, with high computational efficiency and good scalability. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 This is a schematic diagram of discontinuity detection according to the present invention;

[0028] Figure 2 This is a schematic diagram of the visible boundary search according to the present invention;

[0029] Figure 3 This is a schematic diagram of the salient feature detection according to the present invention;

[0030] Figure 4 A schematic diagram for constructing the topology diagram of the present invention;

[0031] Figure 5 This is a schematic diagram of path point selection according to the present invention;

[0032] Figure 6 This is the topological diagram constructed according to the present invention. DETAILED DESCRIPTION

[0033] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

[0034] The basic idea of the present invention's method and device for planning a coverage path based on a topology graph is as follows:

[0035] The robot is placed in an unfamiliar indoor environment, and the robot's own depth sensor is used to detect the visible frontiers of the robot as it moves forward in the current environment. vf and salient features point q sf ,A roadmap is constructed in the form of a topology graph G in the industrial computer.,The visible boundary detection and significant feature detection are introduced below.

[0036] 1. Detection of Visible Boundaries

[0037] The depth sensor is used to perform depth scans (DS) along the robot's clockwise and counterclockwise directions to detect the depth of the robot at its current position p. r The visible boundary that the sensor can scan; the depth scan point list is DS = {p1,...,p n} indicates that the depth scan points are arranged in counterclockwise order relative to the sensor coordinate system; a single depth scan point p i The polar coordinates and Cartesian coordinates of (r i ,θ i )、(x i ,y i ) indicates that r i Denotes the depth scan point p i The polar radius in the robot coordinate system, θ i Denotes the depth scan point p i The polar angle in the robot coordinate system, (x i,y i ) represents the depth scan point p i The horizontal and vertical coordinates in the world coordinate system, the maximum scanning range of the sensor is r max express.

[0038] (1) Discontinuity detection

[0039] The first step of visible boundary detection is depth discontinuity detection. If one of the following conditions is met, then at two consecutive depth scan points (p i ,p i+1 ) a discontinuity occurs in the depth scan between:

[0040] Type 1 discontinuity: p i and p i+1 They are all obstacle points, i and p i+1 The distance between them exceeds f w :||p i -p i+1 ||>f w ;

[0041] Type 2 discontinuity: p i and p i+1 At least one of the points is a non-obstacle point, and the distance between the non-obstacle point and the robot is the maximum scanning range of the sensor r max ;

[0042] where f w is the minimum width of the boundary that the robot can pass through, f w The determination of is based on the shape of the robot, for example, if the robot is round, then f w Set to its diameter + d safe ; For a rectangular robot, f w Set to its width + d safe , where d safe It is a safe distance to ensure the robot passes safely.

[0043] like Figure 1 As shown, p1, p 10 、p 11 are non-obstacle points, and the rest are obstacle points (obstacle points are directly detected by the depth sensor. There are far more depth scanning points around the robot than these points. For the sake of explanation, it is assumed that these points are all depth scanning points). Since p5 and p6 are both obstacle points, if the distance between them is greater than f w , then a type 1 discontinuity occurs between p5 and p6; since p1 is a non-obstruction point and p2 is an obstacle point, a type 2 discontinuity occurs between p1 and p2; since p6 and p7 are both non-obstruction points, a type 2 discontinuity occurs between p6 and p7.

[0044] The two ends of the discontinuity are marked with (r s ,θ s ) and (r e ,θ e ) indicates that when scanning depth counterclockwise, θ e ≥θ s , while the depth scan is clockwise θ s ≥θ e ; Starting point (r s ,θ s ) is also called the reference point p for discontinuity detection ref (f), used to search for discontinuous endpoints (r e ,θ e ).

[0045] (2) Visible Boundary Search

[0046] First, from point p1 to p n Search counterclockwise to detect the starting point of the discontinuity, and then find the end point corresponding to the starting point of the discontinuity; secondly, from point p n Go to p1 and search clockwise to find the starting point of the discontinuity, then find the end point corresponding to the starting point of the discontinuity; the line between the starting point of the discontinuity and the corresponding end point is the visible boundary. Next, we will use the counterclockwise search as an example to explain in detail:

[0047] Check every two consecutive depth scan points (p i ,p i+1 ) is there a type 1 discontinuity between them? If there is a type 1 discontinuity, then the depth scan point p i As the starting point of discontinuity p s (f) = f(r s ,θ s ), create its reference point at the starting point of the discontinuity, and use p ref (f)=(r s ,θ s ) means that next, we need to find the end point p of the discontinuity. e (f)=(r e ,θ e ), as follows:

[0048] Use L a ={p i+1 ,...,p i+m} indicates that the search is in counterclockwise direction. s (f) The list of m obstacle points after that, which satisfies the requirement that the total rotation angle of the m obstacle points searched counterclockwise does not exceed π;

[0049] List L aEach obstacle point (e.g. p k ∈L a ) is marked as valid, first depends on the starting point p s (f) and any obstacle point p in the list k Is the created boundary from the robot's current position p r Visible (see Figure 2 , the boundary AB is at the current position p of the robot r It can be seen that the boundary DE is at the current position p of the robot r invisible);

[0050] In order to make the obstacle point p k Valid, when the robot's current position p is satisfied r If visible, the following conditions must also be met:

[0051]

[0052] in It falls on L a The location of p s (f) After that and the obstacle point p k The visible angles of the previous series of points and each obstacle point p k ∈L a The relevant visible angle is defined as:

[0053]

[0054] where r s is the robot's current position p r To the starting point p s (f) distance, r k is the starting point p s (f) to p k The distance, D k is the robot's current position p r and p k The distance between them.

[0055] The set V(L) of all valid points that satisfy the obstacle points visible from the robot's current position and formula (1) a ), V(L a ) and p s (f) The point with the shortest distance is selected to represent the end point p e (f):

[0056]

[0057] p s (f) and p e (f) The connected line segment represents a boundary. e(f) Start to continue the discontinuity search to find the next boundary.

[0058] In V(L a ) is empty, the end point p e (f) Set to the starting point p s (f) The distance is f w virtual points to determine the boundaries.

[0059] like Figure 2 As shown in the figure, during the counterclockwise search, point A begins to have type 1 discontinuity, so point A is selected as the starting point of the discontinuity. According to formula (1), all valid points are selected. For example, point B is a valid point selected because the visible angle of point B is It is smaller than the visible angle of all other points between AB (points to the right of B in the figure, such as point G), and then according to formula (3), point C is determined to be the end point of the discontinuity. Figure 2 In the figure, AC represents the first visible boundary detected during the counterclockwise search. Since AC is visible from the robot's current position, it is added to the topology map G. Similarly, DE represents another boundary detected during the counterclockwise search, but boundary DE is not added to the topology map G because it is visible from the robot's current position p. r The border cannot be seen.

[0060] The clockwise search works the same way as the counterclockwise search, see Figure 2 EF represents the first visible boundary detected during the clockwise search. Since EF is visible from the robot's current position, it is added to the topology map G. Similarly, AG represents another boundary detected during the clockwise search, but boundary AG is not added to the topology map G because it is visible from the robot's current position p. r The border cannot be seen.

[0061] (3) Using boundary points to represent visible boundaries

[0062] The midpoint of the line segment formed by the start and end points of the i-th visible boundary It is called a frontier point, which can be used to represent the visible boundary. The calculation formula of the frontier point is as follows:

[0063]

[0064] Represents the i-th boundary point q vf i The horizontal and vertical coordinates in the world coordinate system, and Represents the i-th boundary point q vf iThe horizontal and vertical coordinates of the starting point in the world coordinate system, and Represents the i-th boundary point q vf i The horizontal and vertical coordinates of the end point in the world coordinate system.

[0065] 2. Detection of salient feature points

[0066] In addition to visible boundaries, continuous obstacle surfaces can be identified as salient features of the environment. To find salient features, the sensor depth scan is first divided into several equal parts. For continuous obstacle surfaces, the first point (r s ,θ s ) is used to calculate the starting point of the obstacle surface (x s ,y s ), the last point scanned (r e ,θ e ) is used to calculate the end point of the obstacle surface (x e ,y e ), the coordinates of the two points are calculated as follows:

[0067]

[0068]

[0069] Among them, (x robot ,y robot ) represents the horizontal and vertical coordinates of the robot in the world coordinates, Yaw robot Indicates the orientation of the robot in the world coordinate system by adding (x robot ,y robot ) and Yaw robot , the starting point and end point of the obstacle surface are expressed in the robot coordinate system (r s ,θ s )、(r e ,θ e ) is converted to the representation in the world coordinate system (x s ,y s )、(x e ,y e ), the schematic diagram is as follows Figure 3 shown.

[0070] Then the two endpoints (x s ,y s )、(x e ,y e ) and robot coordinates (x robot ,y robot ) is considered as a significant feature point q sf=(x sf ,y sf ) is calculated as follows:

[0071]

[0072] If q sf The distance to all depth scan points is greater than f w , then the significant feature point q sf =(x sf ,y sf ) will be added to the topology graph G.

[0073] 3. Construction of regional coverage topology map

[0074] The detected visible boundaries and retained salient features are stored in the form of a graph to approximate the topology of the environment. Figure 4 As shown, the robot’s current position p r Provides a basis for building a topology map; Figure 4 In, p r Indicates the current position of the robot, the black arrow represents the direction of the robot, and the four boundary points q vf1 ,q vf2 ,q vf3 ,q vf4 Used to represent the detected boundary, only two boundary points q are retained vf2 ,q vf3 , because they are visible from the robot’s current position; line segments a, b, c, d, e, f represent the depth scan around the robot, a and f correspond to the boundary area, while b, c, d, e correspond to the salient feature point q sf1 ,q sf2 ,q sf3 ,q sf4 , and the adjacent significant feature points are connected by line segments, the current position of the robot p r Serves as the root of the topology graph.

[0075] Set the end position that the robot wants to reach. The robot moves from the current position p r Move towards the detected visible boundary point (at Figure 4 In the example, assuming that the given end position is above the right of the robot, the robot will move from the current position p r Go to q vf2 If only boundary points are used to construct a topology map, the topology map will be too sparse, and the robot's coverage path planning effect will be very poor; the addition of significant feature points makes the constructed topology map dense, which can make the robot's coverage path smoother, and for dense map networks, mature algorithms (such as Dijkstra algorithm) can be used to control the robot's motion. Figure 5 As shown, set the moving distance d fit (pre-set), if the robot's current position p r and the boundary point q vf The distance between them is greater than d fit , the robot will not go straight to the boundary point q vf , but choose to go to the nearest significant feature point, such as q sf1 ; Arrive at q sf1 After that, according to the same principle, since the robot’s current position is close to the boundary point q vf The distance between them is greater than d fit , the robot will not go straight to the boundary point q vf , but will choose to go to the nearest significant feature point, such as q sf2 ; Repeat the above process, the robot from p r Start by passing q sf1 ,q sf2 ,q sf3 ,q sf4 , when the robot reaches q sf4 When the robot's current position is at the boundary point q vf The distance between them is less than d fit , the robot will go straight to the boundary point q vf ; If the robot's current position p r and the boundary point q vf The distance between them is less than or equal to d fit , then go straight to the boundary point q vf .according to Figure 5 The topology diagram finally obtained by the method shown is as follows Figure 6 As shown, the coverage path planning is realized.

[0076] A coverage path planning device based on a topology graph, comprising:

[0077] The visible boundary detection module is used to determine the visible boundary of the robot's path to the destination;

[0078] The salient feature detection module is used to determine the salient feature points between the robot's current position and the visible boundary point it is heading to.

[0079] The embodiments described are preferred implementations of the present invention, but the present invention is not limited to the above implementations. Any obvious improvements, substitutions or modifications that can be made by those skilled in the art without departing from the essence of the present invention are within the scope of protection of the present invention.

Claims

1. A coverage path planning method based on a topology graph, characterized by: Based on the depth scan points, determine the visible boundary of the robot's path to the destination; The robot moves from its current position to a visible boundary point. If the distance between the current position and the target visible boundary point is greater than the set moving distance, it uses the significant feature points between the current position and the target visible boundary point to move forward. Otherwise, it moves directly to the target visible boundary point. Repeat the above process until the robot reaches the end position. The process of determining the visible boundary is as follows: from the depth scanning point p1 to p n When searching counterclockwise, detect the starting point of the discontinuity and then determine the end point corresponding to the starting point of the discontinuity; Scan point p from depth n When searching clockwise at p1, the starting point of the discontinuity is detected, and then the end point corresponding to the starting point of the discontinuity is determined; The line between the starting point of a discontinuity and the corresponding end point is the visible boundary; The process of determining the significant feature point is as follows: the centroid of the triangle formed by the starting point of the obstacle surface, the end point of the obstacle surface and the robot coordinates is considered as a candidate point of a significant feature point. If the distance between the candidate point and all depth scanning points is greater than f w , then the significant feature points will be added to the topological map; where f w is the minimum width of the boundary that the robot can pass through.

2. The coverage path planning method based on topology graph according to claim 1, characterized in that: The discontinuities include: Two consecutive depth scanning points p i and p i+1 They are all obstacle points, i and p i+1 The distance between them exceeds f w :||p i -p i+1 ||>f w ; Two consecutive depth scanning points p i and p i+1 At least one of them is a non-obstacle point, and the distance between the non-obstacle point and the robot is the maximum scanning range of the sensor r max ; where f w is the minimum width of the boundary that the robot can pass through.

3. The coverage path planning method based on topology graph according to claim 1, characterized in that: The process of determining the end point corresponding to the discontinuity starting point is as follows: Discontinuity starting point p s (f) and any obstacle point p in the list k The boundary is visible from the robot's current position and satisfies The obstacle points constitute the set of all valid points V(L a ), the set V(L a ) and the discontinuity starting point p s (f) The point with the shortest distance is the end point p e (f); The list is searched counterclockwise / clockwise when in p s (f) is then composed of m obstacle points, For each obstacle point p k ∈L a The relevant visible angle, It falls on L a The location of p s (f) After that and the obstacle point p k The visible angle of the previous series of points, L a When searching counterclockwise, the s (f) List of the following m obstacle points.

4. The coverage path planning method based on topology graph according to claim 3, characterized in that: The set V(L a ) is an empty set, and the end point p e (f) Set to the starting point p s (f) The distance is f w virtual point to determine the boundary; where f w is the minimum width of the boundary that the robot can pass through.

5. A device for implementing the coverage path planning method based on a topology graph according to any one of claims 1 to 4, characterized in that: include: The visible boundary detection module is used to determine the visible boundary of the robot's path to the destination; The salient feature detection module is used to determine the salient feature points between the robot's current position and the visible boundary point it is heading to.

Images