CN112526870B - Road cleaning formation planning method based on inverse clustering algorithm - Google Patents

Road cleaning formation planning method based on inverse clustering algorithm Download PDF

Info

Publication number
CN112526870B
CN112526870B CN202011401608.7A CN202011401608A CN112526870B CN 112526870 B CN112526870 B CN 112526870B CN 202011401608 A CN202011401608 A CN 202011401608A CN 112526870 B CN112526870 B CN 112526870B
Authority
CN
China
Prior art keywords
cleaning
cleaning machine
variable
alpha
state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011401608.7A
Other languages
Chinese (zh)
Other versions
CN112526870A (en
Inventor
王刚
王方霄
杨雯文
邹鹏钰
孙美琪
吕幼新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202011401608.7A priority Critical patent/CN112526870B/en
Publication of CN112526870A publication Critical patent/CN112526870A/en
Application granted granted Critical
Publication of CN112526870B publication Critical patent/CN112526870B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P.I., P.I.D.
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0291—Fleet control
    • G05D1/0293—Convoy travelling

Abstract

The invention discloses a road cleaning formation planning method based on an inverse clustering algorithm, which comprises the following steps: s1, determining a to-be-detected area of a cleaning machine cluster, a detection radius of an unmanned cleaning machine, positions of all obstacles, the number N of the unmanned cleaning machines and initial positions of the N unmanned cleaning machines; s2, garbage detection is carried out, information of all cleaning machines is collected in real time, collected information is obtained and fed back to each cleaning machine, and the collected information comprises: counting the detected garbage position, the state and the position of each cleaning machine and the percentage of the current cleaning area in the total area; and S3, for any cleaning machine, determining the working state of the cleaning machine according to whether the garbage is detected and the state information of other cleaning machines, and planning a path according to the working state of the cleaning machine. The invention provides a route planning algorithm of a cleaner cluster based on an inverse cluster algorithm, which realizes cleaning tasks with higher efficiency and shorter time in cleaning fields with the same area size.

Description

Road cleaning formation planning method based on inverse clustering algorithm
Technical Field
The invention relates to the field of road cleaning, in particular to a road cleaning formation planning method based on an inverse clustering algorithm.
Background
Path planning of the cleaning robot may be classified into path planning with an environment model and path planning without an environment model according to the presence or absence of the environment model. The method for planning the full-area coverage path without the environmental model usually adopts an S-shaped round-trip traversal or a return-word traversal. For the situation of building an environment model, a map model is usually required to be built, such as: grid maps, visual images, topological graphs and the like, and then traversing and other specific algorithms are realized.
Algorithms applied to path planning of cleaning robots are gradually flexible and quick, but when the cleaning requirements of large-area environments are met, the working efficiency of a single machine is far less efficient than that of cluster work. The application of the current algorithm for cleaning robot cluster sweeping is very limited, which causes the road cleaning efficiency to be still deficient.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a path planning algorithm of a cleaner cluster based on an inverse cluster algorithm, so that cleaning tasks with higher efficiency and shorter time are realized in cleaning fields with the same area size.
The purpose of the invention is realized by the following technical scheme: a road cleaning formation planning method based on an inverse clustering algorithm comprises the following steps:
s1, determining a to-be-detected area of a cleaning machine cluster, a detection radius of an unmanned cleaning machine, positions of all obstacles, the number N of the unmanned cleaning machines and initial positions of the N unmanned cleaning machines;
s2, garbage detection is carried out, information of all cleaning machines is collected in real time, collected information is obtained and fed back to each cleaning machine, and the collected information comprises: counting the detected garbage position, the state and the position of each cleaning machine and the percentage of the current cleaning area in the total area;
and S3, for any cleaning machine, determining the working state of the cleaning machine according to whether the garbage is detected and the state information of other cleaning machines, and planning a path according to the working state of the cleaning machine.
Wherein, the operating condition of descaling machine includes exploring state, execution state and clearance state, wherein:
the exploration state refers to a state that the cleaning machine does not find garbage in a detection range or finds that the garbage is being executed by other cleaning machines in the cluster to perform cleaning tasks;
the execution state refers to a state that the cleaning machine finds the garbage in the detection range and no other cleaning machine is executing the cleaning task of the garbage;
the cleaning state refers to a state that the cleaning machine reaches a garbage finding position after the cleaning machine is in an execution state, and the garbage is cleaned.
In step S3, the working state of the cleaning machine is switched as follows:
after initialization, all cleaning machines enter an exploration state, and for each cleaning machine, when the cleaning machine finds garbage in a detection range and the garbage does not have other cleaning machines executing cleaning tasks, the state of the cleaning machine is converted into an execution state;
when the cleaning machine enters the execution state, the cleaning machine gradually moves to the searched garbage position, and when the cleaning machine reaches a garbage place, the cleaning machine enters the cleaning state and keeps still cleaning the garbage;
after the garbage is cleaned by the cleaning machine after entering the cleaning state, the cleaning machine judges whether the range explored by the cleaning machine in the cluster is completely overlapped with the area to be detected of the whole field, when the range explored by the cleaning machine in the cluster is completely overlapped, namely the exploration rate reaches 100%, the work is finished, otherwise, the cleaning machine returns to the exploration state to continue exploring the area to be detected.
In the step S2, during the movement of the cleaning machine, each cleaning machine performs path planning according to its own state, and during the path planning, the anti-flooding algorithm is used to implement the obstacle avoidance function:
let the moving individual be the virtual alpha variable:
Figure BDA0002812628940000021
qi(t)，pi(t)，ui(t)∈R2the position, velocity and acceleration, respectively, control the input of variable i in time,
Figure BDA0002812628940000022
denotes qi(t) derivative of;
with respect to alphaiThe alpha neighborhood of a variable is defined in the time dimension as:
Figure BDA0002812628940000023
wherein | | · | | is at R2Euclidean norm r ofsPreset parameters, and rs＞0，rcIs the range of communication of each node, i.e., the detection range; over time, the alpha variable will change according to equation (1), resulting in
Figure BDA0002812628940000024
A change in (b);
vα＝{1，2，...，N}
defining a function of potential possibilities for non-negative repulsive pairings
Figure BDA0002812628940000025
Wherein, KpIs a positive constant; Ψ (z, d) reaches its maximum value as z approaches 0, a smooth approach 0 as z approaches d, and continues to be 0 beyond d;
distributed information maps: let miIs alphaiInformation map of variables, each individual in a cluster at miIs defined as m in timei(x) X is the center coordinate of each individual; at the very beginning of the process, the first,each individual in the information map is initialized to their default address, and as the α variable continues to explore the target area, their information map is updated:
mi(x)＝t (3)
if it is not
||x-qi(t)||＜rs (4)
For all i ∈ vαThe time t is more than or equal to 0;
when the reverse cluster without obstacle is made in the air, the input alpha is controllediThe variable is composed of two parts
Figure BDA0002812628940000031
Wherein
Figure BDA0002812628940000032
And
Figure BDA0002812628940000033
are off-center terms and self-center terms, off-center terms
Figure BDA0002812628940000034
The aim is to adjust the distance between the alpha variables, which term also indirectly avoids collisions between the alpha variables; in this process, the off-center of the α neighborhood has been implemented for application to the virtual potential field:
Figure BDA0002812628940000035
wherein
Figure BDA0002812628940000036
Is qiThe gradient operator of (a) is selected,
Figure BDA0002812628940000037
is a collective latent function based on alphaiThe relative distance of a variable from its neighborhood, rejected by equation (2)The likelihood function of a pair defines:
Figure BDA0002812628940000038
dαis the smallest advisable distance in the alpha variable (0 < d)β≤2rs) Selecting dα＞2rsCoverage holes may result;
the self-centering term in equation (5) is related to the search area maximization for each alpha variable, defining static virtual variables as gamma variables, the gamma variables controlling the alpha variables toward their self-centering targets, each alpha variable having a corresponding gamma variable; if α isiThe position of the gamma variable of the variable at time t is
Figure BDA0002812628940000039
Then
Figure BDA00028126289400000310
Can be defined as:
Figure BDA00028126289400000311
κsand kappavAre both positive constants, with equations (6) and (7), the control input in equation (5) is rewritten as:
Figure BDA00028126289400000312
calculation of the position of the gamma variable: the location of the gamma variable should be chosen to maximize cumulative coverage, minimizing coverage between each individual; the above-mentioned information map enables efficient determination of the gamma variable, in determining alphaiInformation map m with variable at time t > 0iWhen, an effective function xi is giveni(x, t) miThe value of (c):
ξi(x，t)＝(t-mi(x))(ρ+(1-ρ)λi(x)) (10)
(t-mi(x) The term) is the time span after the update of the position x, ρ is a constant, where we take ρ ═ 0.2, for the function λi(x)
Figure BDA00028126289400000313
Wherein sigma1And σ2Is a positive constant; in the formula (9), ξiMay be considered an alternative to
Figure BDA00028126289400000314
A quantitative measure of; thus selecting
Figure BDA00028126289400000315
To maximize xii(x，t)：
Figure BDA00028126289400000316
Wherein
Figure BDA0002812628940000041
Open-ground obstacle-free inverse cluster analysis: to analyze the collision avoidance capability of the target algorithm, an energy function is defined for a set of alpha and gamma variables, and the control functions in equation (8) are provided as their sum of potential and dynamic energy:
Figure BDA0002812628940000042
wherein potential energy Ui(q)
Figure BDA0002812628940000043
Dynamic energy Ki(p)
Figure BDA0002812628940000044
Adding an anti-flooding algorithm for avoiding barriers: let it be assumed that for all alpha variables, the location of the obstacle is known before the search is expanded;
the obstacle is represented by a virtual variable, beta, a series of beta variables denoted as Vβ1 ', 2 ', N '; they are another type of static variable, located at the surface of each obstacle in the environment; a series ofiThe beta neighborhood of a variable is defined at time t as
Figure BDA0002812628940000045
Wherein
Figure BDA0002812628940000046
Is betakPosition of variable, dβIs a definite constant, the neighborhood relation of alpha variable and beta variable is determined by a series of edge functions
Figure BDA0002812628940000047
Wherein
Figure BDA0002812628940000048
Because the beta variables are not related to each other, none of them have any edge function;
αithe control input of the variables is composed of three parts in this algorithm
Figure BDA0002812628940000049
In this connection, it is possible to use,
Figure BDA00028126289400000410
and
Figure BDA00028126289400000411
has the same definition and function as the space anti-flooding,
Figure BDA00028126289400000412
the item is newly added to avoid the obstacle; in the course of passing through
Figure BDA00028126289400000413
Defining as a virtual potential field to realize alphaiCollision avoidance between a variable and its beta neighborhood;
Figure BDA00028126289400000414
Figure BDA00028126289400000415
is a collision avoidance function, which is related to alphaiThe relative distance of a variable and its beta neighborhood; is defined by the repulsive pairwise likelihood function in equation (2)
Figure BDA00028126289400000416
Wherein d isβ，(0＜dβ≤rs) Is the minimum expected distance between the alpha variable and the beta variable, d is selectedβ＞rsAreas that may result in search coverage are at the edge of obstacles;
in the formula (16), hiIs a binary function that determines when to retire alpha variables from their beta neighborhood; this repulsion causes an oscillating behavior of the alpha variable when moving towards the boundary of the obstacle if and only if it occurs when a moving individual approaches towards the obstacle, otherwise when its corresponding gamma variable is located opposite the obstacle; to avoid such undesirable behavior of the alpha variable, h is definedi(t)
Figure BDA0002812628940000051
According to equation (18), there is no repulsion between the α variable and its β neighborhood as it moves parallel or away toward the barrier edge.
For the cleaning machine in an execution state, a PID control algorithm with an obstacle avoidance function is adopted to plan the path of the cleaning locomotive:
the PID control algorithm is described as:
Figure BDA0002812628940000052
where error (k) represents the deviation of the target position from the current position at time k, kiIs a predetermined non-negative constant, kp、kdIs a preset normal number;
because the anti-flooding algorithm has established the purpose of avoiding barriers
Figure BDA0002812628940000053
It is introduced into the PID algorithm:
Figure BDA0002812628940000054
the finally obtained u is the acceleration of the cleaning machine in an execution state capable of realizing the obstacle avoidance function, wherein cfiIs a preset normal number.
The invention has the beneficial effects that: the invention utilizes an anti-flooding algorithm, utilizes a plurality of cleaning machines, is different from the single machine work in the prior art, and the plurality of cleaning machines are communicated with each other to carry out work division and cooperation, thereby accelerating the scanning of the cleaning area of the same area. The cluster is divided into work and cooperates to simulate the cleaning mode of cleaners, each unmanned cleaning machine works simultaneously, the working areas are not overlapped, the number of the unmanned cleaning machines is increased or reduced at any time, and the task completion quality is not influenced.
Drawings
FIG. 1 is a flow chart of a method of the present invention;
FIG. 2 is a state transition diagram of an individual descaling machine in the method according to an embodiment of the invention;
FIG. 3 is a schematic diagram of the calculation idea of the PID algorithm designed by the individual in the execution state in the method according to the embodiment of the invention;
Detailed Description
The technical solutions of the present invention are further described in detail below with reference to the accompanying drawings, but the scope of the present invention is not limited to the following.
As shown in fig. 1, a road cleaning formation planning method based on an inverse clustering algorithm includes the following steps:
s1, determining a to-be-detected area of a cleaning machine cluster, a detection radius of an unmanned cleaning machine, positions of all obstacles, the number N of the unmanned cleaning machines and initial positions of the N unmanned cleaning machines;
s2, garbage detection is carried out, information of all cleaning machines is collected in real time, collected information is obtained and fed back to each cleaning machine, and the collected information comprises: counting the detected garbage position, the state and the position of each cleaning machine and the percentage of the current cleaning area in the total area;
and S3, for any cleaning machine, determining the working state of the cleaning machine according to whether the garbage is detected and the state information of other cleaning machines, and planning a path according to the working state of the cleaning machine.
Wherein, the operating condition of descaling machine includes exploring state, execution state and clearance state, wherein:
the exploration state refers to a state that the cleaning machine does not find garbage in a detection range or finds that the garbage is being executed by other cleaning machines in the cluster to perform cleaning tasks;
the execution state refers to a state that the cleaning machine finds the garbage in the detection range and no other cleaning machine is executing the cleaning task of the garbage;
the cleaning state refers to a state that the cleaning machine reaches a garbage finding position after the cleaning machine is in an execution state, and the garbage is cleaned.
As shown in fig. 2, in step S3, the operating state of the cleaning machine is switched as follows:
after initialization, all cleaning machines enter an exploration state, and for each cleaning machine, when the cleaning machine finds garbage in a detection range and the garbage does not have other cleaning machines executing cleaning tasks, the state of the cleaning machine is converted into an execution state;
when the cleaning machine enters the execution state, the cleaning machine gradually moves to the searched garbage position, and when the cleaning machine reaches a garbage place, the cleaning machine enters the cleaning state and keeps still cleaning the garbage;
after the garbage is cleaned by the cleaning machine after entering the cleaning state, the cleaning machine judges whether the range explored by the cleaning machine in the cluster is completely overlapped with the area to be detected of the whole field, when the range explored by the cleaning machine in the cluster is completely overlapped, namely the exploration rate reaches 100%, the work is finished, otherwise, the cleaning machine returns to the exploration state to continue exploring the area to be detected.
In the step S2, during the movement of the cleaning machine, each cleaning machine performs path planning according to its own state, and during the path planning, the anti-flooding algorithm is used to implement the obstacle avoidance function:
let the moving individual be the virtual alpha variable:
Figure BDA0002812628940000061
qi(t)，pi(t)，ui(t)∈R2the position, velocity and acceleration, respectively, control the input of variable i in time,
Figure BDA0002812628940000062
denotes qi(t) derivative of;
with respect to alphaiThe alpha neighborhood of a variable is defined in the time dimension as:
Figure BDA0002812628940000071
wherein | | · | | is at R2Euclidean norm r ofsPreset parameters, and rs＞0，rcIs the range of communication of each node, i.e., the detection range; over time, the alpha variable will change according to equation (1), resulting in
Figure BDA0002812628940000072
A change in (b);
vα＝{1，2，...，N}
defining a function of potential possibilities for non-negative repulsive pairings
Figure BDA0002812628940000073
Wherein, KpIs a positive constant; Ψ (z, d) reaches its maximum value as z approaches 0, a smooth approach 0 as z approaches d, and continues to be 0 beyond d;
distributed information maps: let miIs alphaiInformation map of variables, each individual in a cluster at miIs defined as m in timei(x) X is the center coordinate of each individual; initially, each individual in the information map is initialized to their default address, and as the α variable continues to explore the target area, their information map is updated:
mi(x)＝t (3)
if it is not
||x-qi(t)||＜rs (4)
For all i ∈ vαThe time t is more than or equal to 0;
when the reverse cluster without obstacle is made in the air, the input alpha is controllediThe variable is composed of two parts
Figure BDA0002812628940000074
Wherein
Figure BDA0002812628940000075
And
Figure BDA0002812628940000076
are off-center terms and self-center terms, off-center terms
Figure BDA0002812628940000077
The aim is to adjust the distance between the alpha variables, which term also indirectly avoids collisions between the alpha variables; in this process, the off-center of the α neighborhood has been implemented for application to the virtual potential field:
Figure BDA0002812628940000078
wherein
Figure BDA0002812628940000079
Is qiThe gradient operator of (a) is selected,
Figure BDA00028126289400000710
is a collective latent function based on alphaiThe relative distance of a variable from its neighborhood is defined by the exclusive paired likelihood function in equation (2):
Figure BDA00028126289400000711
dαis the smallest advisable distance in the alpha variable (0 < d)β≤2rs) Selecting dα＞2rsCoverage holes may result;
the self-centering term in equation (5) is related to the search area maximization for each alpha variable, defining static virtual variables as gamma variables, the gamma variables controlling the alpha variables toward their self-centering targets, each alpha variable having a corresponding gamma variable; if α isiThe position of the gamma variable of the variable at time t is
Figure BDA0002812628940000081
Then
Figure BDA0002812628940000082
Can be defined as:
Figure BDA0002812628940000083
κsand kappavAre both positive constants, with equations (6) and (7), the control input in equation (5) is rewritten as:
Figure BDA0002812628940000084
calculation of the position of the gamma variable: the location of the gamma variable should be chosen to maximize cumulative coverage, minimizing coverage between each individual; the above-mentioned information map enables efficient determination of the gamma variable, in determining alphaiInformation map m with variable at time t > 0iWhen, an effective function xi is giveni(x, t) miThe value of (c):
ξi(x，t)＝(t-mi(x))(ρ+(1-ρ)λi(x)) (10)
(t-mi(x) The term) is the time span after the update of the position x, ρ is a constant, where we take ρ ═ 0.2, for the function λi(x)
Figure BDA0002812628940000085
Wherein sigma1And σ2Is a positive constant; in the formula (9), ξiMay be considered an alternative to
Figure BDA0002812628940000086
A quantitative measure of; thus selecting
Figure BDA0002812628940000087
To maximize xii(x，t)：
Figure BDA0002812628940000088
Wherein
Figure BDA0002812628940000089
Open-ground obstacle-free inverse cluster analysis: to analyze the collision avoidance capability of the target algorithm, an energy function is defined for a set of alpha and gamma variables, and the control functions in equation (8) are provided as their sum of potential and dynamic energy:
Figure BDA00028126289400000810
wherein potential energy Ui(q)
Figure BDA00028126289400000811
Dynamic energy Ki(p)
Figure BDA00028126289400000812
Adding an anti-flooding algorithm for avoiding barriers: let it be assumed that for all alpha variables, the location of the obstacle is known before the search is expanded;
the obstacle is represented by a virtual variable, beta, a series of beta variables denoted as Vβ1 ', 2 ', N '; they are another type of static variable, located at the surface of each obstacle in the environment; a series ofiThe beta neighborhood of a variable is defined at time t as
Figure BDA0002812628940000091
Wherein
Figure BDA0002812628940000092
Is betakPosition of variable, dβIs a definite constant, the neighborhood relation of alpha variable and beta variable is determined by a series of edge functions
Figure BDA0002812628940000093
Wherein
Figure BDA0002812628940000094
Because the beta variables are not related to each other, none of them have any edge function;
αithe control input of the variables is composed of three parts in this algorithm
Figure BDA0002812628940000095
In this connection, it is possible to use,
Figure BDA0002812628940000096
and
Figure BDA0002812628940000097
has the same definition and function as the space anti-flooding,
Figure BDA0002812628940000098
the item is newly added to avoid the obstacle; in the course of passing through
Figure BDA0002812628940000099
Defining as a virtual potential field to realize alphaiCollision avoidance between a variable and its beta neighborhood;
Figure BDA00028126289400000910
Figure BDA00028126289400000911
is a collision avoidance function, which is related to alphaiThe relative distance of a variable and its beta neighborhood; is defined by the repulsive pairwise likelihood function in equation (2)
Figure BDA00028126289400000912
Wherein d isβ，(0＜dβ≤rs) Is the minimum expected distance between the alpha variable and the beta variable, d is selectedβ＞rsAreas that may result in search coverage are at the edge of obstacles;
in the formula (16), hiIs a binary function that determines when to retire alpha variables from their beta neighborhood; this repulsion causes an oscillating behavior of the alpha variable when moving towards the boundary of the obstacle if and only if it occurs when a moving individual approaches towards the obstacle, otherwise when its corresponding gamma variable is located opposite the obstacle; to avoid such undesirable behavior of the alpha variable, h is definedi(t)
Figure BDA00028126289400000913
According to equation (18), there is no repulsion between the α variable and its β neighborhood as it moves parallel or away toward the barrier edge.
For the cleaning machine in an execution state, a PID control algorithm with an obstacle avoidance function is adopted to plan the path of the cleaning locomotive:
as shown in fig. 3, the PID control algorithm is to linearly combine three algorithms of proportion, integral and differential of deviation to form a control quantity to control the controlled object, wherein:
the proportional control algorithm is as follows:
u＝kp*error(t)
and (3) an integral control algorithm:
u＝ki*error(t)
and (3) a differential control algorithm:
u＝kd(error(t)-error(t-1))
wherein k isiIs a predetermined non-negative constant, kp、kdThe error (t) is a preset normal number and represents the difference value between the target position and the current position at the moment t;
the PID control algorithm is described as:
Figure BDA0002812628940000101
PID is the simplest of the closed loop control algorithms. PID is an abbreviation for proportional, integral, derivative, which represents the three control algorithms, respectively. The deviation of the controlled object can be effectively corrected by the combination of the three algorithms, so that the controlled object reaches a stable state.
Because the anti-flooding algorithm has established the purpose of avoiding barriers
Figure BDA0002812628940000102
It is introduced into the PID algorithm:
Figure BDA0002812628940000103
the finally obtained u is the acceleration of the cleaning machine in an execution state capable of realizing the obstacle avoidance function, wherein cfiIs a preset normal number.
In the embodiment of the application, the unmanned cleaning machine firstly explores the whole field (anti-flooding algorithm), and when detecting the garbage, namely when the garbage enters the detection range of the unmanned cleaning machine, the information map is updated, and the garbage points appear on the map. And then the unmanned cleaning machine starts to use a PID algorithm to approach the garbage and finally reaches the place where the garbage is located to clean the garbage. And after the garbage is cleaned, the information map is updated, and the point is deleted in the map. And the unmanned cleaning machine continuously executes the anti-flooding algorithm to detect the whole area, and repeats the previous action when finding the next garbage. When all the garbage in the area is cleaned up, the algorithm stops.
The foregoing is illustrative of the preferred embodiments of this invention, and it is to be understood that the invention is not limited to the precise form disclosed herein and that various other combinations, modifications, and environments may be resorted to, falling within the scope of the concept as disclosed herein, either as described above or as apparent to those skilled in the relevant art. And that modifications and variations may be effected by those skilled in the art without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (4)

1. A road cleaning formation planning method based on an inverse clustering algorithm is characterized by comprising the following steps: the method comprises the following steps:
s1, determining a to-be-detected area of a cleaning machine cluster, a detection radius of an unmanned cleaning machine, positions of all obstacles, the number N of the unmanned cleaning machines and initial positions of the N unmanned cleaning machines;
s2, garbage detection is carried out, information of all cleaning machines is collected in real time, collected information is obtained and fed back to each cleaning machine, and the collected information comprises: counting the detected garbage position, the state and the position of each cleaning machine and the percentage of the current cleaning area in the total area;
s3, for any cleaning machine, determining the working state of the cleaning machine according to whether the garbage is detected and the state information of other cleaning machines, and planning a path according to the working state of the cleaning machine;
in the step S3, during the movement of the cleaning machine, each cleaning machine performs path planning according to its own state, and during the path planning, the anti-flooding algorithm is used to implement the obstacle avoidance function:
let the moving individual be the virtual alpha variable:
Figure FDA0003389471260000011
qi(t)，pi(t)，ui(t)∈R2the position, velocity and acceleration, respectively, control the input of variable i in time,
Figure FDA0003389471260000012
denotes qi(t) derivative of;
with respect to alphaiThe alpha neighborhood of a variable is defined in the time dimension as:
Figure FDA0003389471260000013
wherein | | · | | is at R2Euclidean norm r ofsPreset parameters, and rs＞0，rcIs the range of communication of each node, i.e., the detection range; over time, the alpha variable will change according to equation (1), resulting in
Figure FDA0003389471260000014
A change in (b);
vα＝{1，2，...，N}
defining a function of potential possibilities for non-negative repulsive pairings
Figure FDA0003389471260000015
Wherein, KpIs a positive constant; Ψ (z, d) reaches its maximum value as z approaches 0, a smooth approach 0 as z approaches d, and continues to be 0 beyond d;
distributed information maps: let miIs alphaiInformation map of variables, each individual in a cluster at miIs defined as m in timei(x) X is the center coordinate of each individual; initially, each individual in the information map is initialized to their default address, and as the α variable continues to explore the target area, their information map is updated:
mi(x)＝t (3)
if it is not
||x-qi(t)||＜rs (4)
For all i e vαThe time t is more than or equal to 0;
when the reverse cluster without obstacle is made in the air, the input alpha is controllediThe variable is composed of two parts
Figure FDA0003389471260000021
Wherein
Figure FDA0003389471260000022
And
Figure FDA0003389471260000023
are off-center terms and self-center terms, off-center terms
Figure FDA0003389471260000024
The aim is to adjust the distance between the alpha variables, which term also indirectly avoids collisions between the alpha variables; in this process, the off-center of the α neighborhood has been implemented for application to the virtual potential field:
Figure FDA0003389471260000025
wherein
Figure FDA0003389471260000026
Is qiThe gradient operator of (a) is selected,
Figure FDA0003389471260000027
is a collective latent function based on alphaiThe relative distance of a variable from its neighborhood is defined by the exclusive paired likelihood function in equation (2):
Figure FDA0003389471260000028
dαis the smallest advisable distance in the alpha variable (0 < d)β≤2rs) Selecting dα＞2rsCoverage holes may result;
the self-centering term in equation (5) is related to the search area maximization for each alpha variable, defining static virtual variables as gamma variables, the gamma variables controlling the alpha variables toward their self-centering targets, each alpha variable having a corresponding gamma variable; if α isiThe position of the gamma variable of the variable at time t is
Figure FDA0003389471260000029
Then
Figure FDA00033894712600000210
Can be defined as:
Figure FDA00033894712600000211
ksand kappavAre both positive constants, with equations (6) and (7), the control input in equation (5) is rewritten as:
Figure FDA00033894712600000212
calculation of the position of the gamma variable: the location of the gamma variable should be chosen to maximize cumulative coverage, minimizing coverage between each individual; the above-mentioned information map enables efficient determination of the gamma variable, in determining alphaiInformation map m with variable at time t > 0iWhen, an effective function xi is giveni(x, t) miThe value of (c):
ξi(x，t)＝(t-mi(x))(ρ+(1-ρ)λi(x)) (10)
(t-mi(x) The term) is the time span after the update of the position x, ρ is a constant, with respect to the function λi(x)
Figure FDA00033894712600000213
Wherein sigma1And σ2Is a positive constant; in the formula (9), ξiMay be considered an alternative to
Figure FDA00033894712600000214
qiA quantitative measure of; thus selecting
Figure FDA00033894712600000215
To maximize xii(x，t)：
Figure FDA00033894712600000216
Wherein
Figure FDA0003389471260000031
Open-ground obstacle-free inverse cluster analysis: to analyze the collision avoidance capability of the target algorithm, an energy function is defined for a set of alpha and gamma variables, and the control functions in equation (8) are provided as their sum of potential and dynamic energy:
Figure FDA0003389471260000032
wherein potential energy Ui(q)
Figure FDA0003389471260000033
Dynamic energy Ki(p)
Figure FDA0003389471260000034
Adding an anti-flooding algorithm for avoiding barriers: let it be assumed that for all alpha variables, the location of the obstacle is known before the search is expanded;
the obstacle is represented by a virtual variable, beta, a series of beta variables denoted as Vβ1 ', 2 ', N '; they are another type of static variable, located at the surface of each obstacle in the environment; a series ofiThe beta neighborhood of a variable is defined at time t as
Figure FDA0003389471260000035
Wherein
Figure FDA0003389471260000036
Is betakPosition of variable, dβIs a definite constant, the neighborhood relation of alpha variable and beta variable is determined by a series of edge functions
Figure FDA0003389471260000037
Wherein
Figure FDA0003389471260000038
Because the beta variables are not related to each other, none of them have any edge function;
αithe control input of the variables is composed of three parts in this algorithm
Figure FDA0003389471260000039
In this connection, it is possible to use,
Figure FDA00033894712600000310
and
Figure FDA00033894712600000311
has the same definition and function as the space anti-flooding,
Figure FDA00033894712600000312
the item is newly added to avoid the obstacle; in the course of passing through
Figure FDA00033894712600000313
Defining as a virtual potential field to realize alphaiCollision avoidance between a variable and its beta neighborhood;
Figure FDA00033894712600000314
Figure FDA00033894712600000315
is a collision avoidance function, which is related to alphaiThe relative distance of a variable and its beta neighborhood; is defined by the repulsive pairwise likelihood function in equation (2)
Figure FDA00033894712600000316
Wherein d isβ，(0＜dβ≤rs) Is the minimum expected distance between the alpha variable and the beta variable, d is selectedβ＞rsAreas that may result in search coverage are at the edge of obstacles;
in the formula (16), hiIs a binary function that determines when to retire alpha variables from their beta neighborhood; this repulsion occurs if and only if a moving individual approaches towards an obstacle, otherwise when it corresponds to a gamma variableWhen the position is opposite to the obstacle, it causes an oscillatory behavior of the alpha variable when moving towards the obstacle boundary; to avoid such undesirable behavior of the alpha variable, h is definedi(t)
Figure FDA0003389471260000041
According to equation (18), there is no repulsion between the α variable and its β neighborhood as it moves parallel or away toward the barrier edge.
2. The method for planning a clean formation of roads based on an inverse clustering algorithm as claimed in claim 1, wherein: the working state of the cleaning machine comprises an exploration state, an execution state and a cleaning state, wherein:
the exploration state refers to a state that the cleaning machine does not find garbage in a detection range or finds that the garbage is being executed by other cleaning machines in the cluster to perform cleaning tasks;
the execution state refers to a state that the cleaning machine finds the garbage in the detection range and no other cleaning machine is executing the cleaning task of the garbage;
the cleaning state refers to a state that the cleaning machine reaches a garbage finding position after the cleaning machine is in an execution state, and the garbage is cleaned.
3. The method for planning a clean formation of roads based on an inverse clustering algorithm as claimed in claim 1, wherein: in step S3, the working state of the cleaning machine is switched as follows:
after initialization, all cleaning machines enter an exploration state, and for each cleaning machine, when the cleaning machine finds garbage in a detection range and the garbage does not have other cleaning machines executing cleaning tasks, the state of the cleaning machine is converted into an execution state;
when the cleaning machine enters the execution state, the cleaning machine gradually moves to the searched garbage position, and when the cleaning machine reaches a garbage place, the cleaning machine enters the cleaning state and keeps still cleaning the garbage;
after the garbage is cleaned by the cleaning machine after entering the cleaning state, the cleaning machine judges whether the range explored by the cleaning machine in the cluster is completely overlapped with the area to be detected of the whole field, when the range explored by the cleaning machine in the cluster is completely overlapped, namely the exploration rate reaches 100%, the work is finished, otherwise, the cleaning machine returns to the exploration state to continue exploring the area to be detected.
4. The method for planning a clean formation of roads based on an inverse clustering algorithm as claimed in claim 2, wherein: for the cleaning machine in an execution state, a PID control algorithm with an obstacle avoidance function is adopted to plan the path of the cleaning locomotive:
the PID control algorithm is described as:
Figure FDA0003389471260000051
where error (k) represents the deviation of the target position from the current position at time k, kiIs a predetermined non-negative constant, kp、kdIs a preset normal number;
because the anti-flooding algorithm has established the purpose of avoiding barriers
Figure FDA0003389471260000052
It is introduced into the PID algorithm:
Figure FDA0003389471260000053
the finally obtained u is the acceleration of the cleaning machine in an execution state capable of realizing the obstacle avoidance function, wherein cfiIs a preset normal number.
CN202011401608.7A 2020-12-02 2020-12-02 Road cleaning formation planning method based on inverse clustering algorithm Active CN112526870B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011401608.7A CN112526870B (en) 2020-12-02 2020-12-02 Road cleaning formation planning method based on inverse clustering algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011401608.7A CN112526870B (en) 2020-12-02 2020-12-02 Road cleaning formation planning method based on inverse clustering algorithm

Publications (2)

Publication Number Publication Date
CN112526870A CN112526870A (en) 2021-03-19
CN112526870B true CN112526870B (en) 2022-02-01

Family

ID=74996682

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011401608.7A Active CN112526870B (en) 2020-12-02 2020-12-02 Road cleaning formation planning method based on inverse clustering algorithm

Country Status (1)

Country Link
CN (1) CN112526870B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101301186A (en) * 2008-04-23 2008-11-12 上海中为智能机器人有限公司 4-segment type sweeping robot
CN102818568A (en) * 2012-08-24 2012-12-12 中国科学院深圳先进技术研究院 Positioning and navigation system and method of indoor robot
US8372166B2 (en) * 2010-11-15 2013-02-12 Adaptivearc, Inc. Plasma assisted gasification system
CN104155998A (en) * 2014-08-27 2014-11-19 电子科技大学 Route planning method based on potential field method
CN106843242A (en) * 2017-03-21 2017-06-13 天津海运职业学院 A kind of multi-robots system of under-water body cleaning
CN108058790A (en) * 2017-12-28 2018-05-22 重庆邮电大学 A kind of rubbish cleaning unmanned boat waterborne
CN109375633A (en) * 2018-12-18 2019-02-22 河海大学常州校区 River course clear up path planning system and method based on global state information
CN109567676A (en) * 2017-09-29 2019-04-05 松下知识产权经营株式会社 Autonomous scavenging machine, cleaning method and program
CN110641881A (en) * 2019-09-29 2020-01-03 北京智行者科技有限公司 Driverless garbage classification cleaning method
CN111469131A (en) * 2020-05-10 2020-07-31 上海大学 Unmanned ship water surface garbage cleaning control system and method with mechanical arm

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101301186A (en) * 2008-04-23 2008-11-12 上海中为智能机器人有限公司 4-segment type sweeping robot
US8372166B2 (en) * 2010-11-15 2013-02-12 Adaptivearc, Inc. Plasma assisted gasification system
CN102818568A (en) * 2012-08-24 2012-12-12 中国科学院深圳先进技术研究院 Positioning and navigation system and method of indoor robot
CN104155998A (en) * 2014-08-27 2014-11-19 电子科技大学 Route planning method based on potential field method
CN106843242A (en) * 2017-03-21 2017-06-13 天津海运职业学院 A kind of multi-robots system of under-water body cleaning
CN109567676A (en) * 2017-09-29 2019-04-05 松下知识产权经营株式会社 Autonomous scavenging machine, cleaning method and program
CN108058790A (en) * 2017-12-28 2018-05-22 重庆邮电大学 A kind of rubbish cleaning unmanned boat waterborne
CN109375633A (en) * 2018-12-18 2019-02-22 河海大学常州校区 River course clear up path planning system and method based on global state information
CN110641881A (en) * 2019-09-29 2020-01-03 北京智行者科技有限公司 Driverless garbage classification cleaning method
CN111469131A (en) * 2020-05-10 2020-07-31 上海大学 Unmanned ship water surface garbage cleaning control system and method with mechanical arm

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
"复杂空域环境下多飞行器协同机动理论与方法"2013年度报告;葛树志，等;《科技咨讯》;20161231(第15期);第173页 *
A New Decentralized Planning Strategy for Flocking of Swarm Robots;Ximing Liang, Xiang Li;《JOURNAL OF COMPUTERS》;20100630;第5卷(第6期);第914-921页 *
Behavioural response of European starlings exposed to video playback of conspecific flocks: Effect of social context and predator threat;Francesca Zorattoa，等;《Behavioural Processes》;20141231;第103卷;第269–277页 *
Route Planning for Anti-ship Missile Position Setting;Xingang Wang，Xiaofang Xie;《2011 Fourth International Symposium on Computational Intelligence and Design》;20111231;第155-158页 *
无人机编队协同任务规划仿真系统研究;王国强，等;《系统仿真学报》;20140831;第26卷(第8期);第1856-1862页 *

Also Published As

Publication number Publication date
CN112526870A (en) 2021-03-19

Similar Documents

Publication Publication Date Title
KR101339480B1 (en) Trajectory planning method for mobile robot using dual tree structure based on rrt
Guo et al. Revising motion planning under linear temporal logic specifications in partially known workspaces
Marthaler et al. Tracking environmental level sets with autonomous vehicles
Dong et al. Real-time avoidance strategy of dynamic obstacles via half model-free detection and tracking with 2d lidar for mobile robots
Chaimowicz et al. A paradigm for dynamic coordination of multiple robots
Ardiyanto et al. Real-time navigation using randomized kinodynamic planning with arrival time field
Masehian et al. Cooperative mapping of unknown environments by multiple heterogeneous mobile robots with limited sensing
Pradhan et al. Robot crowd navigation using predictive position fields in the potential function framework
Pimentel et al. Information-driven rapidly-exploring random tree for efficient environment exploration
KR20050024840A (en) Path planning method for the autonomous mobile robot
CN111578926A (en) Map generation and navigation obstacle avoidance method based on automatic driving platform
Missura et al. Minimal construct: Efficient shortest path finding for mobile robots in polygonal maps
CN112526870B (en) Road cleaning formation planning method based on inverse clustering algorithm
Tao et al. Motion planning for slam based on frontier exploration
Belavadi et al. Frontier exploration technique for 3d autonomous slam using k-means based divisive clustering
Westbrook et al. Anytime kinodynamic motion planning using region-guided search
Alsaab et al. Improving velocity obstacle approach for obstacle avoidance in indoor environments
Arul et al. DS-MPEPC: Safe and Deadlock-Avoiding Robot Navigation in Cluttered Dynamic Scenes
Rappel et al. Stigmergy-based, dual-layer coverage of unknown regions
Cocaud et al. Environment mapping using probabilistic quadtree for the guidance and control of autonomous mobile robots
Vestli et al. Mops, a system for mail distribution in office type buildings
Yoshimura et al. Iterative transportation planning of multiple objects by cooperative mobile robots
Adams et al. Control and localisation of a post distributing mobile robot
Deghat et al. Safe formation control with obstacle avoidance
Vandorpe et al. A reflexive navigation algorithm for an autonomous mobile robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant