A three-dimensional full-coverage path planning method for the cleaning of secondary water supply tanks

Abstract

The present invention relates to the field of path planning, and discloses a three-dimensional full-coverage path planning method for the cleaning of secondary water supply tanks. According to the three-dimensional geometric characteristics and task characteristics of the secondary water supply tanks, a three-dimensional task space graph model is constructed, and node and connection attributes are defined; the transition cost between two nodes is designed according to the full-coverage contribution rate, the number of turns and the operation loss between the graph nodes; a virtual end point is added, and the full-coverage path planning problem is transformed into a minimum Hamiltonian circuit problem; the Hamiltonian cost function is iteratively optimized to generate the optimal connection of the path end points until no iterative update is possible; path tracing is performed, the virtual end point information is deleted, and an optimized three-dimensional full-coverage path sequence is output. The present invention designs three-dimensional full-coverage path planning in combination with task requirements, has a fast response time and low computational resource requirements, saves the hardware cost of the robot controller while reducing the movement loss of the robot, and efficiently completes the cleaning task of the secondary water supply tank by the robot.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and more specifically, to a three-dimensional full-coverage path planning technology. Background Art

[0002] In order to make up for the insufficient pressure of municipal water supply pipelines and ensure the water use of residents in high-rise buildings, secondary water supply has become the main water supply method in the current urban construction water supply work. Compared with raw water supply, the water quality of secondary water supply is more easily polluted. The cleaning of secondary water supply facilities is related to the water quality, water pressure and water supply safety of secondary water supply, and is closely related to the normal and stable life of the people. Generally, it is necessary to fully cover and clean the inner walls of six water tanks, as well as three-dimensional parts such as horizontal ribs and vertical ribs, and at the same time meet the requirements of operation robot control and energy consumption. The main objectives of the existing full-coverage path planning for secondary water supply tanks are: (1) full coverage, to fully cover and clean the three-dimensional surfaces of the six inner walls, horizontal ribs and vertical ribs of the water tank on the premise of effectively avoiding all obstacles; (2) low repetition rate, when performing full-coverage path planning, the repetition rate is an important indicator to evaluate the quality of the path. A low repetition rate can effectively reduce the energy consumption of the robot and enable it to complete the established tasks more efficiently; (3) low turning times, the energy of the robot itself is limited. The less the number of turns, the lower the energy consumption, and the less the number of turns reduces the probability of the cleaning robot's water pipe entanglement and the control difficulty. If the path planning must achieve full coverage and require a low number of turns, the repetition rate will inevitably increase. It is necessary to design a path planning algorithm with a low repetition rate and a low number of turns under the premise of full coverage. Based on these challenges, a path planning algorithm that can balance the cleaning task requirements and the robot motion loss is crucial. This algorithm framework should enable the water tank autonomous cleaning robot to significantly improve the operation efficiency while completing the work tasks.

[0003] There are three basic walking methods for the existing full-coverage path planning, namely the reciprocating method and the inner and outer spiral method. The reciprocating method is also called the ox plowing method. It walks along a straight line to the boundary of the area to be fully covered and then turns to continue walking along a parallel line. The interval between the two paths is determined by the working range of the robot. The inner spiral method is to walk along a straight line to the boundary of the area to be fully covered and then turn to continue walking to the boundary and then turn again. The outer spiral is the opposite. Both methods are "offline" and are widely used in the autonomous operations of intelligent machines such as plant protection UAVs and cleaning robots. The reciprocating method is simple and convenient, but it also has disadvantages. It has a good effect on simple maps, but when the obstacles are complex, too many decomposition units will have the opposite effect and make the path become complex.

[0004] In most of the prior art research, the focus is on the full coverage of 2D maps, which can solve the problem of full coverage of irregular two-dimensional maps. However, 2D full coverage path planning is not applicable to the path planning of 3D full coverage tasks. Currently, most of the 3D full coverage path planning research inherits the zigzag path pattern of the 2D mode, only changes the cost function, and does not break the traditional straight-line path setting. In addition, the path planning for avoiding obstacles on the 3D surface depends on the 2D terrain passability analysis. Since it is necessary to plan the full coverage path within a separate sub-section, it will bring additional complex computational work and result in unreasonable disconnection of the path at the boundary of the sub-section. Therefore, it is desirable to have a technical solution to overcome or at least mitigate at least one of the above-mentioned defects of the prior art.

[0005] In response to the problems in the related art, no effective solution has been proposed yet. Summary of the Invention

[0006] Objective of the Invention: In response to the problems in the related art, the present invention proposes a three-dimensional full coverage path planning method for the cleaning of secondary water supply water tanks to overcome the above-mentioned technical problems existing in the prior related art.

[0007] Technical Solution: A three-dimensional full coverage path planning method for the cleaning of secondary water supply water tanks according to the present invention includes the following steps:

[0008] Step 1: Divide the working area according to the three-dimensional geometric characteristics and task requirements of the secondary water supply water tank, construct a three-dimensional task space graph model, define the node and connection attributes, and then obtain a three-dimensional water tank cleaning graph model G = <N, E>;

[0009] Among them, N = {v i = (p ix , p iy )} is the graph node, i = 1... N, N represents the information for performing the cleaning task, including position information and operation requirements, and (p ix , p iy ) is the center position of the divided grid; is the connection relationship between nodes, including moving adjacent, operation adjacent, and non-adjacent;

[0010] Step 2: Design the transition cost w between two nodes according to the position full coverage contribution rate the number of turns and the operation loss ij ;

[0011]

[0012] Step 3: Add a virtual end point to transform the full-coverage path planning problem into a minimum Hamiltonian cycle problem; use iterative optimization of the Hamiltonian cost function to generate the best connection of the path endpoints; repeat Step 3 until no iterative update is possible;

[0013] The specific process of the said Step 3 is as follows:

[0014] Step 31: Expand the vertex set V to V η by adding a virtual vertex v E such that the transition cost, W η,0 = 0,

[0015] Subsequently, a minimum Hamiltonian cycle weight matrix of size (η + 1)×(η - 1) is given

[0016]

[0017] where η is the transition cost after adding the virtual node v

[0018] Step 32: The set of all solutions of the cycle associated with the weight matrix W E is denoted as where is the set of all Hamiltonian cycles; each Hamiltonian cycle

[0019]

[0020] in the set provides the order in which the vertices are visited and has the following form: where u(λ) is the vertex visited at step λ, and for we have υ(λ) ≠ υ(λ′); λ, λ′ = 0, 1, … η; η is the set of nodes including v

[0021] S33: According to the above definition, the cost of a Hamiltonian cycle C is as follows:

[0022]

[0023] S34: Use a minimum-nearest neighbor sampling solver to obtain the optimal Hamiltonian cycle which is:

[0024]

[0025] Step 4: Conduct path tracing, delete the virtual end point information, and output the optimized three-dimensional full-coverage path sequence.

[0026] Further, the detailed process of the said Step 1 includes the following steps:

[0027] Step 11: Construct an x-y plane in the working area, take the lower left corner point of the three-dimensional map as the coordinate origin A, establish a three-dimensional coordinate system, and take the maximum lengths AB, AD, and AA' of the three-dimensional water tank along the x-axis, y-axis, and z-axis directions respectively with A as the vertex to construct a three-dimensional water tank cube area ABCD-A'B'C'D.

[0028] Step 12: Divide the three-dimensional water tank cube area into grids, and use the spacing L of the horizontal ribs or vertical ribs in the water tank structure h , L v to divide the three-dimensional water tank into N equal parts, where N>1, so that the spatial grid is a cube of (L h , L v ).

[0029] Step 13: Classify the connection relationships of each grid block in the cube obtained in Step 12. The connection relationships are divided into three categories in total:

[0030] A): If two grid blocks can be reached by the mobile robot moving at two adjacent moments, then the two grid blocks are called mobile adjacent; for example, two grid blocks connected at the bottom layer; the robot can move to the grid and directly control the cleaning operation through the action of the nozzle, and the operation loss is T l ;

[0031] B): If two grid blocks can be continuously reached by the action of the robot operating mechanism, then the two grid blocks are called operation adjacent. For example, the robot needs to reach the movable area of its projection, rise through the Z-axis lifting mechanism to the upper layer of the same projection area, and lower the z-axis after completing the cleaning operation, so that the robot can continue to move. The operation loss between the time when the robot reaches the projection position and the time when it restores this state is T h ;

[0032] C): Not adjacent;

[0033] Step 14: Establish a three-dimensional water tank cleaning graph model G = <N, E> according to the three-dimensional divided grid.

[0034] Further, the cost of the robot full-coverage contribution rate in the said Step 2 is:

[0035] N e , N u respectively represent the explored nodes and unexplored nodes of the robot;

[0036] The number of turns of the said robot The calculation formula is as follows:

[0037]

[0038] In the above formula, the x-axis is the traveling direction of the robot. If the distance between two nodes on the x-axis is less than the width of a grid, it means that there is no movement in the x-axis direction between the two nodes. If the distance between the two nodes in the y-axis direction is greater than or equal to the width of a grid, it means that a turn occurs in the y-axis direction;

[0039] The described operation loss is calculated as follows:

[0040]

[0041] The above formula means that if the robot moves in the x-axis or y-axis, the operation loss it represents is T l , and the operation loss at the high level is T h ;

[0042] Finally, when the robot transitions from the current node to the next node, it selects the smallest possible full-coverage contribution rate, number of turns, and operation loss, and can balance different cost attributes.

[0043] Furthermore, the specific method of step 4 is as follows:

[0044] Step S41: Based on the optimal Hamiltonian circuit C, obtain the endpoint connection sequence with the minimum transfer cost;

[0045] Step S42: Delete the virtual end point and output the three-dimensional full-coverage minimum-cost path planning solution for the secondary water supply tank.

[0046] Advantageous effects: The present invention designs a three-dimensional full-coverage path planning algorithm in combination with task requirements. The designed algorithm can efficiently complete the cleaning task of the robot for the secondary water supply tank; can autonomously generate the full-coverage path of the secondary water supply tank and adapt to water tanks with different structures; the algorithm has a fast response time and low computational resource requirements, saving the hardware cost of the robot controller; reduces the computational amount, and the calculated optimal path connection greatly reduces the total path turning times and operation time cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 is the overall flowchart of the present invention;

[0048] Figure 2 is the model diagram of the secondary water supply tank in the embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0049] As Figure 1As shown in the figure, a three-dimensional full-coverage path planning method for the cleaning of secondary water supply tanks according to the present invention includes the following steps:

[0050] Step 1: Divide the working area according to the three-dimensional geometric characteristics and task requirements of the secondary water supply tank, construct a three-dimensional task space graph model, define node and connection attributes, and then obtain a three-dimensional water tank cleaning graph model G = <N, E>;

[0051] Among them, N = {v i : = (p ix , p iy )}, i = 1…N} are graph nodes, N represents the information for performing cleaning tasks, including position information and operation requirements, (p ix , p iy ) is the center position of the divided grid; is the connection relationship between nodes, including moving adjacent, operating adjacent, and not adjacent;

[0052] Step 2: Design the transition cost w turning times and operation loss between two nodes according to the position full-coverage contribution rate ij ;

[0053]

[0054] Step 3: Add a virtual end point to transform the full-coverage path planning problem into a minimum Hamiltonian cycle problem; use iterative optimization of the Hamiltonian cost function to generate the best connection of the path endpoints; repeat Step 3 until no iterative update is possible;

[0055] The specific process of the said Step 3 is as follows:

[0056] Step 31: Expand the vertex set V to v η by adding a virtual vertex v E , so that the transition cost,

[0057] Consequently, a minimum Hamiltonian cycle weight matrix of size (η + 1) × (η - 1) is given

[0058]

[0059] is the transition cost after adding the virtual node v η ;

[0060] Step 32: With the weight matrix The set of all solutions of the associated circuit is represented as is the set of all Hamiltonian circuits; each Hamiltonian circuit in the set provides the order in which the vertices are visited, in the following form:

[0061]

[0062] υ(λ) is the vertex visited in step λ, and for there is υ(λ)≠υ(λ′); λ, λ′ = 0, 1, … η; represents the set of nodes including v η , λ and λ′ represent two different vertices in the visit;

[0063] S33. According to the above definition, the cost of a Hamiltonian circuit is as follows:

[0064]

[0065] S34. Using the minimum proximity sampling solver, obtain the optimal Hamiltonian circuit which is:

[0066]

[0067] Step 4. Perform path tracing, delete the virtual end point information, and output the optimized three-dimensional full coverage path sequence.

[0068] As Figure 2 shown, the detailed process of this implementation step 1 includes the following steps:

[0069] Step 11. Construct an x - y plane in the working area, take the lower left corner point of the three-dimensional map as the coordinate origin A, establish a three-dimensional coordinate system, and take the maximum lengths AB, AD, AA′ of the three-dimensional water tank along the x-axis, y-axis, and z-axis directions respectively with A as the vertex to construct a three-dimensional water tank cube region ABCD - A′B′C′D;

[0070] Step 12. Divide the three-dimensional water tank cube region into grids, and use the spacing L h , L v to divide the three-dimensional water tank into N equal parts, N > 1, so that the spatial grid is a cube of (L h , L v );

[0071] Step 13. Classify the connection relationships of each grid block in the cube obtained in step 12. The connection relationships are divided into three categories in total:

[0072] A), If two grid blocks can be reached by the mobile robot at two adjacent times, the two grid blocks are said to be mobile adjacent; for example, two grid blocks connected at the bottom layer; the robot can move to the grid and directly control the cleaning operation through the action of the nozzle, and the operation loss is T l ;

[0073] B), If two grid blocks can be continuously reached by the action of the robot operating mechanism, the two grid blocks are said to be operation adjacent. For example, the robot needs to reach the movable area of its projection, rise through the Z-axis lifting mechanism to the upper layer of the same projection area, and lower the Z-axis after the cleaning operation is completed, so that the robot can continue to move. The operation loss between the time when the robot reaches the projection position and the time when it returns to this state is T h ;

[0074] C), Not adjacent;

[0075] Step 14: Divide the grid in three dimensions and establish a three-dimensional water tank cleaning graph model G = <N, E>.

[0076] In step 2 of this embodiment, the cost of the full coverage contribution rate of the robot is:

[0077] N e , N u respectively represent the explored nodes and unexplored nodes of the robot;

[0078] The number of turns of the robot The calculation formula is as follows:

[0079]

[0080] In the above formula, the x-axis is the traveling direction of the robot. If the distance between two nodes on the x-axis is less than the width of one grid, it means that there is no movement between the two nodes on the x-axis. If the distance between the two nodes in the y-axis direction is greater than or equal to the width of one grid, then a turn occurs in the y direction;

[0081] The calculation method of the operation loss is as follows:

[0082]

[0083] The above formula means that if the robot moves on the x-axis or y-axis, the operation loss it represents is T l , and the operation loss of the upper layer is T h ; Finally, when the robot transitions from the current node to the next node, it selects the one with the smallest full coverage contribution rate, number of turns, and operation loss, and can balance different cost attributes.

[0084] The specific method of step 4 of this embodiment is:

[0085] Step S41: Based on the optimal Hamiltonian circuit C, obtain the endpoint connection sequence with the minimum transfer cost;

[0086] Step S42: Delete the virtual end point and output the three-dimensional full-coverage minimum-cost path planning solution for the secondary water supply tank.

[0087] As can be seen from the above embodiments, the present invention can autonomously generate a full-coverage path for the secondary water supply tank to adapt to water tanks with different structures; the generated full-coverage path enables the robot to avoid immovable areas; reduces the amount of calculation, and the calculated optimal path connection greatly reduces the total number of path turns and the operation time cost.

Claims

1. A three-dimensional full coverage path planning method for cleaning a secondary water supply tank, characterized in that: The following steps are involved: Step 1: Divide the work area according to the three-dimensional geometric features of the secondary water supply tank and the task requirements, construct a three-dimensional task space graph model, define the node and connection attributes, and then obtain the three-dimensional water tank cleaning graph model G=<V,E> ; in, is a graph node, N represents the information of executing cleaning tasks, is the center position of the divided grid; The connection relationship between nodes, including move-adjacent, operation-adjacent, and non-adjacent; Step 2: Full coverage contribution rate based on the position between graph nodes , Number of turns and operating losses Design the transition cost between two nodes ; ； Step 3: Add a virtual endpoint to transform the full coverage path planning problem into a minimum Hamiltonian circuit problem; use iterative optimization of the Hamiltonian cost function to generate the best connection of the path endpoints; repeat step 3 until it cannot be iteratively updated; The specific process of step 3 is: Step 31: Add a virtual endpoint To collect vertices Expand to , making the transition cost , , ; Then, the size is given by The minimum Hamiltonian circuit weight matrix : ； Add virtual endpoints Transition costs after Step 32, and the weight matrix The set of all solutions of the associated loop is represented as , is the set of all Hamiltonian circuits; the set Each Hamiltonian circuit hi in provides an order to visit all vertices and satisfies the constraints that each vertex is visited exactly once and the circuit must return to the starting point. Specifically, it has the following form: It is in step The vertices visited in , and for ,have ; ; and Indicates two different vertices being visited; S33. According to the above definition, a Hamiltonian circuit The costs are as follows: ； S34, using the minimum adjacent sampling solver to obtain the optimal Hamiltonian loop for: ； Step 4: Perform path tracing, delete virtual endpoint information, and output an optimized three-dimensional full coverage path sequence.

2. The three-dimensional full coverage path planning method for secondary water supply tank cleaning according to claim 1 is characterized in that: The detailed process of step 1 includes the following steps: Step 11, construct an xyz coordinate system in the working area, take the lower left corner of the three-dimensional map as the coordinate origin A, establish a three-dimensional coordinate system, take A as the vertex, take the maximum length AB, AD, AA' of the three-dimensional water tank along the x-axis, y-axis and z-axis directions respectively to construct the three-dimensional water tank cube area ABCD-A'B'C'D; Step 12: Divide the three-dimensional water tank cube area into grids, and use the horizontal or vertical reinforcement spacing L in the water tank structure to divide the water tank cube area into grids. h , L v Divide the three-dimensional water tank into N equal parts, N>1, so that the spatial grid is (L h , L h , L v ) cube; Step 13: Classify the connection relationships of the grid blocks in the cube obtained in step 12. The connection relationships are divided into three categories: A) If two grid blocks can be reached by a mobile robot at two adjacent moments, the two grid blocks are called mobile adjacent, and the corresponding operation loss is ; B) If two grid blocks can be reached continuously through the action of the robot operating mechanism, the two grid blocks are called operation adjacent, and the corresponding operation loss is ; C) not adjacent; Step 14: Based on the three-dimensional grid division, establish a three-dimensional water tank cleaning model G=<V,E> .

3. The three-dimensional full coverage path planning method for secondary water supply tank cleaning according to claim 1 is characterized in that: Step 2: Robot full coverage contribution cost for: ; They represent the nodes that have been explored and the nodes that have not been explored by the robot respectively; The number of turns of the robot The calculation formula is as follows: ； In the above formula, The axis is the direction of travel of the robot. If the distance between the two nodes on the x-axis is less than the width of a grid, it means that the two nodes have not moved on the x-axis. If the distance between the two nodes on the y-axis is greater than or equal to the width of a grid, a turn occurs in the y direction. The operating loss The calculation method is as follows: ； The above formula indicates that if the robot moves on the x-axis or y-axis, the work loss it represents is , the high-level operation loss is .

4. The three-dimensional full coverage path planning method for secondary water supply tank cleaning according to claim 1 is characterized in that: The specific method of step 4 is: Step S41, based on the optimal Hamiltonian circuit C, obtaining an endpoint connection sequence with a minimum transfer cost; Step S42, delete the virtual end point and output the three-dimensional full coverage minimum cost path planning solution for the secondary water supply tank.

Images