Unmanned ship full-coverage garbage cleaning path planning method

Abstract

The invention relates to the technical field of unmanned ship automatic control, in particular to an unmanned ship full-coverage garbage cleaning path planning method, which comprises the following steps of: dynamically adjusting control parameters and penalty coefficients according to a current path and an obstacle distribution condition by introducing a self-adaptive adjustment mechanism so as to ensure that an unmanned ship can obtain an optimal path in different environments; and the coverage efficiency and safety of path planning are considered.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic control of unmanned boats, and in particular to a method for planning a full-coverage garbage cleaning path for an unmanned boat. Background Art

[0002] In recent years, unmanned driving technology has been widely used in many fields. Due to its advantages of low manufacturing cost, short manufacturing cycle, strong environmental adaptability and low labor cost, unmanned boats have shown broad application prospects in marine resource survey, environmental monitoring, water cleaning, etc. However, in complex water surface environments, how to efficiently clean up garbage and avoid obstacles is one of the main challenges facing the autonomous development of unmanned boats.

[0003] The unit decomposition method is a classic path planning algorithm that is suitable for full coverage path planning in a known environment. This method generates a coverage path by dividing the entire working area into several small units and traversing these units one by one. When dealing with complex or irregular obstacles, the traditional unit decomposition method may generate too many units, resulting in path redundancy and unnecessary turns. The improved unit decomposition method optimizes the efficiency and accuracy of path planning by merging adjacent path segments and reducing the number of initial units.

[0004] When the existing unit decomposition method is used to deal with full coverage path planning in a static known environment, if the obstacles are irregularly distributed or there are many concave obstacles, it will lead to a lot of redundancy and unnecessary turns in path planning. In addition, it is difficult for traditional obstacle avoidance algorithms to simultaneously take into account the coverage efficiency and safety of path planning. Summary of the invention

[0005] In view of this, the purpose of the present invention is to propose a method for planning a full-coverage garbage cleaning path for an unmanned boat, so as to solve the problem that it is difficult for existing methods to simultaneously take into account both the coverage efficiency and safety of path planning.

[0006] Based on the above purpose, the present invention provides a method for planning a path for full-coverage garbage cleaning by an unmanned boat, comprising:

[0007] S1, divide the water surface area into a grid map and generate larger units by merging adjacent path segments;

[0008] S2, based on greedy algorithm and topological Figure 3 Second, solve the traversal order between units, gradually merge units and optimize local paths;

[0009] S3, using circular and convex quadrilateral envelope surfaces to handle irregular obstacles, transforming the obstacle avoidance problem into a state inequality constraint, i.e., an obstacle constraint;

[0010] S4, transforming the time optimal control problem of path planning into an optimal parameter selection problem, and obtaining the objective function of time optimal control;

[0011] S5. Construct obstacle constraints, control quantity constraints, kinematic equation constraints, initial state constraints and terminal state constraints as constraint penalty functions, add them to the objective function, transform them into an optimal parameter selection problem without state constraints, and construct an optimization objective function;

[0012] S6. Solve the optimization objective function to obtain the garbage cleaning path that is fully covered by the unmanned boat.

[0013] Preferably, in step S1, generating a larger unit by merging adjacent path segments comprises:

[0014] Decompose the grid map into several path segments, each of which consists of grids in the same row and adjacent to each other on the left and right.

[0015] Preliminary units are generated based on the path segments and these units are merged to reduce the number of units and reduce path redundancy.

[0016] Preferably, in step S3, the processing of irregular obstacles using circular and convex quadrilateral envelope surfaces includes:

[0017] For the circular envelope, let the coordinates of the obstacle center be (x i ,y i ), the envelope radius is R i , the obstacle avoidance constraint is expressed as:

[0018] (x(t)-x i ) 2 +(y(t)-y i ) 2 ≥R i 2 ;

[0019] Where x(t) and y(t) represent the position coordinates of the unmanned boat at time t;

[0020] For a convex quadrilateral envelope, assume that the envelope consists of four boundary lines, and the equation of each boundary line is:

[0021] l j,z :y(t)-k j,z x(t)-b j,z ≤0.

[0022] Where j represents the obstacle number, that is, the number of the convex quadrilateral envelope surface established, z is the number of the four sides of the established quadrilateral, l j,z represents the zth side of the jth convex quadrilateral, k j,zrepresents the slope of the zth side of the jth convex quadrilateral, b j,z represents the intercept of the zth side of the jth convex quadrilateral.

[0023] Preferably, step S4 specifically includes:

[0024] Transform the variable time domain [0, T] to the fixed domain [0, 1], the transformation formula is:

[0025] dt / ds=T, where s is the converted time scale;

[0026] In the process of control parameterization, the continuous control quantity is discretized into control parameters in several intervals, and the time domain [0, T] is divided into N equally spaced intervals. Then the value of the control quantity u(t) in the i-th interval is a constant u i ,Right now:

[0027]

[0028] For each interval, the control quantity u i As the optimization variable, combined with the time scale transformation, the original time optimal control problem is transformed into a discrete parameter optimization problem, and the objective function of the time optimal control is obtained as follows:

[0029] Among them, J is the performance index, T is the total time, i is the penalty coefficient of the control quantity, which is used to smooth the change of the control quantity.

[0030] Preferably, the constraints of the objective function of time optimal control include:

[0031]

[0032] Where P1 is the minimum objective function to be solved in path planning, S1 is the control quantity constraint, S2 is the kinematic equation constraint, S3 is the initial state and terminal state constraints, S4 and S5 are obstacle constraints, γ is the angular acceleration; a is the linear acceleration, is the kinematic equation, X(0)=X 0 , X(T) = X T are the initial state and the terminal state respectively, N i and N j is a natural number.

[0033] Preferably, the optimization objective function constructed in step S5 is:

[0034]

[0035] Among them, α, β, λ are constants; ε is the penalty parameter, ε a , εβ , ε γ is the penalty factor, Wj is a positive real number parameter, and δ, W are weights.

[0036] Preferably, step S6 further includes:

[0037] The optimization objective function is processed by Hamiltonian function to obtain the Hamiltonian equation:

[0038]

[0039] The Hamiltonian equation is solved to obtain the planned garbage cleaning path with full coverage by the unmanned boat.

[0040] Beneficial effects of the present invention:

[0041] The present invention is based on the improved unit decomposition method and the precise penalty function optimal control. By converting the constraint conditions into penalty terms and adding them to the objective function, the optimization problem can be solved by conventional nonlinear optimization techniques. The precise penalty function method has significant advantages in dealing with continuous state inequality constraints and terminal constraints, and can optimize the objective function while ensuring that the constraint conditions are met. Combined with control parameterization and time scale transformation, the precise penalty function method effectively solves the obstacle avoidance trajectory planning problem of unmanned boats in complex waters;

[0042] The present invention introduces an adaptive adjustment mechanism to dynamically adjust control parameters and penalty coefficients according to the current path and obstacle distribution, ensuring that the unmanned boat can obtain the optimal path in different environments, while taking into account the coverage efficiency and safety of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only for the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0044] Figure 1 A schematic diagram of an improved unit decomposition method according to an embodiment of the present invention;

[0045] Figure 2 A schematic diagram of a flow chart of an improved unit decomposition method according to an embodiment of the present invention;

[0046] Figure 3 A schematic diagram of modeling a quadrilateral obstacle envelope surface according to an embodiment of the present invention;

[0047] Figure 4 Schematic diagram of modeling a circular obstacle envelope surface according to an embodiment of the present invention. DETAILED DESCRIPTION

[0048] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments.

[0049] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0050] like Figure 1-Figure 4 As shown, the embodiment of this specification provides a method for planning a path for full-coverage garbage cleaning by an unmanned boat, including:

[0051] Grid map initialization: The water surface area is divided into a grid map, where each grid represents a unit or obstacle to be traversed. In this way, the entire water surface area is subdivided into multiple small grid units, so that fine path planning and coverage control can be achieved.

[0052] Path segment generation and merging: The grid map is decomposed into several path segments, each of which consists of grids in the same row and adjacent to each other. Figure 1 shown.

[0053] Preliminary units are generated based on the path segments and these units are merged to reduce the number of units and reduce path redundancy.

[0054] Path optimization: Based on greedy algorithm and topological Figure 3 The traversal order between units is solved, and the units are gradually merged and the local paths are optimized. Specifically, the path optimization steps are as follows: Figure 2 shown.

[0055] Obstacle handling: Use circular and convex quadrilateral envelope surfaces to handle irregular obstacles and transform the obstacle avoidance problem into a state inequality constraint.

[0056] For the circular envelope, let the coordinates of the obstacle center be (x i ,y i ), the envelope radius is R i.like Figure 4 As shown, the obstacle avoidance constraint can be expressed as:

[0057] (x(t)-x i ) 2 +(y(t)-y i ) 2 ≥R i 2

[0058] For a convex quadrilateral envelope, assume that the envelope consists of four boundary lines, such as Figure 3 As shown, the equation of each boundary line is:

[0059] l j,z :y(t)-k j,z x(t)-b j,z ≤0

[0060] Control parameterization and time scale transformation: Convert the time optimal control problem into an optimal parameter selection problem, and simplify the problem solving through control parameterization and time scale transformation.

[0061] The time scale transformation transforms the variable time domain [0, T] into the fixed domain [0, 1]. The specific transformation formula is:

[0062] dt / ds=T, where s is the time scale after conversion.

[0063] In the process of control parameterization, the continuous control quantity is discretized into control parameters in several intervals to facilitate optimization. Assuming that the time domain [0, T] is divided into N equally spaced intervals, the value of the control quantity u(t) in the i-th interval is a constant u i ,Right now:

[0064]

[0065] For each interval, the control quantity u i As the optimization variable, combined with the time scale transformation, the original time optimal control problem is transformed into a discrete parameter optimization problem. The objective function is:

[0066] Among them, T is the total time, i is the penalty coefficient of the control quantity, which is used to smooth the change of the control quantity.

[0067] Constructing the optimization problem: Constructing the optimization model of the time optimal control problem, the objective function is the movement time T of the unmanned boat, and the constraints include control quantity constraints, kinematic equation constraints, initial state and terminal state constraints, and obstacle constraints. The optimization problem expression is as follows:

[0068]

[0069] S1:γ(s)|≤γ,|a(s)|≤a max ,s∈(0,1)

[0070]

[0071] S3:X(0)=X 0 ,X(T)=X T

[0072]

[0073] Where P1 is the minimum objective function to be solved in path planning, S1 is the control quantity constraint, S2 is the kinematic equation constraint, S3 is the initial state and terminal state constraints, S4 and S5 are obstacle constraints, γ is the angular acceleration; a is the linear acceleration, is the kinematic equation, X(0)=X 0 , X(T) = X T are the initial state and the terminal state respectively, N i and N j is a natural number.

[0074] The exact penalty function method is used to process multiple continuous state inequality constraints, and these constraints are converted into penalty terms in the objective function. The optimization objective function is constructed as:

[0075]

[0076] Among them, α, β, λ are constants; ε is the penalty parameter, ε a , ε β , ε γ is the penalty factor, Wj is a positive real number parameter, and δ, W are weights.

[0077] The Hamiltonian function is used to process the optimization objective function to obtain the Hamiltonian equation:

[0078]

[0079] H(X(s),u(s),λ(s),T,ε)=φ(X(s),u(s),T,ε)+λ T (s)f(s,X(s),u(s))·T

[0080] Solve the Hamiltonian equation to obtain the planned garbage cleaning path with full coverage by the unmanned boat.

[0081] Those skilled in the art should understand that the discussion of any of the above embodiments is only exemplary and is not intended to imply that the scope of the present invention is limited to these examples; under the concept of the present invention, the technical features in the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other changes in different aspects of the present invention as described above, which are not provided in detail for the sake of simplicity. Any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for planning a path for full-coverage garbage cleaning by an unmanned boat, characterized in that: The method comprises: S1, divide the water surface area into a grid map and generate larger units by merging adjacent path segments; S2, based on the greedy algorithm and topological map, solve the traversal order between units three times, gradually merge units and optimize local paths; S3, using circular and convex quadrilateral envelope surfaces to handle irregular obstacles, transforming the obstacle avoidance problem into a state inequality constraint, i.e., an obstacle constraint; S4, transforming the time optimal control problem of path planning into an optimal parameter selection problem, and obtaining the objective function of time optimal control; S5. Construct obstacle constraints, control quantity constraints, kinematic equation constraints, initial state constraints and terminal state constraints as constraint penalty functions, add them to the objective function, transform them into an optimal parameter selection problem without state constraints, and construct an optimization objective function; S6. Solve the optimization objective function to obtain the garbage cleaning path that is fully covered by the unmanned boat.

2. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 1, characterized in that: In step S1, generating a larger unit by merging adjacent path segments includes: Decompose the grid map into several path segments, each of which consists of grids in the same row and adjacent to each other on the left and right. Preliminary units are generated based on the path segments and these units are merged to reduce the number of units and reduce path redundancy.

3. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 1, characterized in that: In step S3, the use of circular and convex quadrilateral envelope surfaces to process irregular obstacles includes: For the circular envelope, let the coordinates of the obstacle center be (x i ,y i ), the envelope radius is R i , the obstacle avoidance constraint is expressed as: (x(t)-x i ) 2 +(y(t)-y i ) 2 ≥R i 2 ; Where x(t) and y(t) represent the position coordinates of the unmanned boat at time t; For a convex quadrilateral envelope, assume that the envelope consists of four boundary lines, and the equation of each boundary line is: l j,z :y(t)-k j,z x(t)-b j,z ≤0。 Where j represents the obstacle number, that is, the number of the convex quadrilateral envelope surface established, z is the number of the four sides of the established quadrilateral, l j,z represents the zth side of the jth convex quadrilateral, k j,z represents the slope of the zth side of the jth convex quadrilateral, b j,z represents the intercept of the zth side of the jth convex quadrilateral.

4. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 3 is characterized in that: Step S4 specifically includes: Transform the variable time domain [0, T] to the fixed domain [0, 1], the transformation formula is: dt / ds=T, where s is the converted time scale; In the process of control parameterization, the continuous control quantity is discretized into control parameters in several intervals, and the time domain [0, T] is divided into N equally spaced intervals. Then the value of the control quantity u(t) in the i-th interval is a constant u i ,Right now: For each interval, the control quantity u i As the optimization variable, combined with the time scale transformation, the original time optimal control problem is transformed into a discrete parameter optimization problem, and the objective function of the time optimal control is obtained as follows: Among them, J is the performance index, T is the total time, i is the penalty coefficient of the control quantity, which is used to smooth the change of the control quantity.

5. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 4, characterized in that: The constraints of the objective function of the time optimal control include: Where P1 is the minimum objective function to be solved in path planning, S1 is the control quantity constraint, S2 is the kinematic equation constraint, S3 is the initial state and terminal state constraints, S4 and S5 are obstacle constraints, γ is the angular acceleration; a is the linear acceleration, is the kinematic equation, X(0) = X0, X(T) = X T are the initial state and the terminal state respectively, N i and N j is a natural number.

6. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 1, characterized in that: The optimization objective function constructed in step S5 is: Among them, α, β, λ are constants; ε is the penalty parameter, ε a , ε β , ε γ is the penalty factor, Wj is a positive real number parameter, and δ, W are weights.

7. The method for planning a path for full-coverage garbage cleaning by an unmanned boat according to claim 5, characterized in that: Step S6 also includes: The optimization objective function is processed by Hamiltonian function to obtain the Hamiltonian equation: The Hamiltonian equation is solved to obtain the planned garbage cleaning path with full coverage by the unmanned boat.

Images