CN113625780A - Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock - Google Patents

Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock Download PDF

Info

Publication number
CN113625780A
CN113625780A CN202111184687.5A CN202111184687A CN113625780A CN 113625780 A CN113625780 A CN 113625780A CN 202111184687 A CN202111184687 A CN 202111184687A CN 113625780 A CN113625780 A CN 113625780A
Authority
CN
China
Prior art keywords
constraint
planning
distributed unmanned
distributed
path planning
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.)
Granted
Application number
CN202111184687.5A
Other languages
Chinese (zh)
Other versions
CN113625780B (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.)
Peking University
Original Assignee
Peking University
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 Peking University filed Critical Peking University
Priority to CN202111184687.5A priority Critical patent/CN113625780B/en
Publication of CN113625780A publication Critical patent/CN113625780A/en
Application granted granted Critical
Publication of CN113625780B publication Critical patent/CN113625780B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying

Abstract

The invention discloses a deadlock-free distributed unmanned cluster cooperative motion path planning method, wherein N objects in a distributed unmanned cluster move in the same dimension space; the object is at the initial moment
Figure 850308DEST_PATH_IMAGE001
From the initial position
Figure 924705DEST_PATH_IMAGE002
Move to respective target positions
Figure 235601DEST_PATH_IMAGE003
And do not collide with each other during the movement; the method is characterized in that aiming at the distributed unmanned cluster movement path planning, the optimization problem and the model function of the distributed unmanned cluster cooperative movement path planning are constructed by defining constraint, target and variable, an infinite view is defined by adding terminal constraint, and then an early warning band is usedThe buffer Vono unit constructs obstacle avoidance constraints, and by introducing a penalty function related to an early warning zone into a target function, the problem of planning the cooperative motion path of the cluster system with complex dynamics is solved, the deadlock problem is avoided, and the feasibility is ensured.

Description

Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock
Technical Field
The invention relates to an unmanned intelligent cluster cooperative control method, in particular to a distributed unmanned cluster cooperative motion path planning method which is based on distributed model prediction control, avoids deadlock and ensures feasibility.
Background
Distributed unmanned cluster cooperative control is a technical problem of a complex system, individuals in a cluster can be in various forms such as unmanned aerial vehicles, unmanned vehicles and unmanned boats, and key technologies to be solved mainly include an information interaction mechanism, a multi-machine cooperative environment perception technology, a cooperative decision, a cluster cooperative motion path planning technology and the like. And the cluster coordinated movement path planning is a core part of the cluster coordinated movement path planning.
Aiming at a cluster cooperative motion path planning algorithm, the current relatively mature cluster distributed cooperative motion path planning algorithm mainly comprises the following steps: dispute-resolution (contentions-resolution), artificial potential field (posititialflies), distributed model predictive control (distributed model predictive control), and geometry (geometry). The dispute resolution method and the artificial potential field method are two commonly used distributed collaborative motion path planning algorithms at present, only play a limited mutual avoidance role under more conditions, cannot ensure the obstacle avoidance between clusters, cannot consider the motion model of a controlled object by a geometric rule, and are only suitable for a few application scenes such as a simple speed motion control system. In the distributed model predictive control method, through constructing a convex optimization problem containing position constraint and dynamic constraint, mutual obstacle avoidance can be ensured, and a complex kinematic model of a controlled object is considered. But the existing methods can not process the deadlock problem caused by the lack of a central coordinator in the distributed unmanned cluster. Deadlock problems, which are more likely to occur in dense clustering situations, refer to multiple objects blocking each other so that no parties can reach the destination. The prior art lacks a distributed unmanned cluster collaborative motion path planning method which can avoid deadlock and ensure feasibility.
Disclosure of Invention
The invention aims to provide a distributed unmanned cluster cooperative motion path planning method capable of ensuring feasibility and avoiding deadlock, which is used for solving the problem of cluster system cooperative motion path planning with complex dynamics, avoiding the deadlock problem and ensuring the feasibility.
Based on distributed model predictive control, aiming at distributed unmanned cluster motion path planning, an optimization problem and a model function of the distributed unmanned cluster collaborative motion path planning are constructed by defining constraints, targets and variables, specifically, firstly, a concept of infinite view is introduced by adding terminal constraints, so that feasibility of an optimization problem solution in the model predictive control is realized, and therefore, feasibility of a motion path planning algorithm is ensured, secondly, a buffer Weino unit (warnebufferdVoronoi cell) with an early warning band is used for constructing an obstacle avoidance constraint, and a penalty function related to the early warning band is introduced into a target function, so that feasibility of the motion path planning algorithm is ensured, and a deadlock problem is thoroughly avoided.
The technical scheme provided by the invention is as follows:
a method for planning cooperative motion paths of a distributed unmanned cluster to avoid deadlock is disclosed, wherein individual objects in the distributed unmanned cluster comprise unmanned planes, unmanned vehicles and unmanned boats in various forms; the distributed unmanned cluster has N objects moving in the same dimension space;Nthe objects are identical
Figure 234005DEST_PATH_IMAGE001
Moving in space at an initial moment
Figure 682304DEST_PATH_IMAGE002
From the initial position
Figure 196462DEST_PATH_IMAGE003
Move to respective target positions
Figure 624295DEST_PATH_IMAGE004
And do not collide with each other during the movement; aiming at the distributed unmanned cluster movement path planning, an optimization problem and a model function of the distributed unmanned cluster cooperative movement path planning are constructed by defining constraint, target and variable, specifically, firstly, an infinite view is introduced by adding terminal constraint to realize the feasibility of an optimization problem solution in model prediction control, thereby ensuring the feasibility of a movement path planning algorithm, and secondly, the method uses a solution containing a target and a variableConstructing obstacle avoidance constraints by buffer Vono units of the early warning zone, and avoiding deadlock by introducing penalty functions related to the early warning zone into the objective function; the distributed unmanned cluster cooperative motion path planning method comprises the following steps:
1) initializing a preset path in a distributed unmanned cluster to an objectiFor example, the predetermined path is recorded as
Figure 969825DEST_PATH_IMAGE005
，
Figure 589025DEST_PATH_IMAGE003
In order to be the initial position of the device,ibeing an object in a distributed unmanned cluster;
Figure 387217DEST_PATH_IMAGE006
is the initial time; horizon of the eye
Figure 320538DEST_PATH_IMAGE007
WhereinKIs the length of the visual field;
2) any one object in distributed unmanned clusteriCommunicating respective preset path information with other objects;
3) acquiring state information including position information and speed information through a real-time positioning system;
4) obtaining an avoidance coefficient
Figure 520575DEST_PATH_IMAGE008
；
Figure 310677DEST_PATH_IMAGE009
As an objectiAbout an objectjThe avoidance angle parameter of (1);
Figure 799427DEST_PATH_IMAGE010
is a constant;
5) according to the obtained state information and the avoidance coefficient, an infinite view field is defined by adding terminal constraint, the avoidance constraint is constructed by using a buffer Voronoi unit containing the early warning zone, a penalty function related to the early warning zone is introduced into a target function, and distribution is constructedOptimizing problems and model functions of the formula unmanned cluster collaborative motion path planning; and solving to obtain input information
Figure 67597DEST_PATH_IMAGE011
And information
Figure 387720DEST_PATH_IMAGE012
Namely, the planning of the distributed unmanned cluster cooperative motion path for avoiding deadlock is realized; the specific process is as follows:
51) constructing a distributed unmanned cluster cooperative motion path planning optimization problem (model function), and realizing motion path planning by adopting a distributed model prediction control method; the method comprises the following steps:
any one object in distributed unmanned clusteriFrom an initial position
Figure 551985DEST_PATH_IMAGE013
Move to the target movement position
Figure 826234DEST_PATH_IMAGE014
It has the dynamics of a second order system;
each object in the distributed unmanned cluster at each elapsed time intervalhLength of field of view
Figure 898095DEST_PATH_IMAGE015
Constructing and solving the convex-down optimization problem for re-planning; the constructed distributed unmanned cluster cooperative motion optimization path planning model function is expressed as follows:
Figure 275987DEST_PATH_IMAGE016
Figure 142312DEST_PATH_IMAGE017
Figure 667971DEST_PATH_IMAGE018
formula (1)
Wherein the content of the first and second substances,
Figure 481206DEST_PATH_IMAGE019
is the field of visionkA control input of;
Figure 775921DEST_PATH_IMAGE020
view obtained for planningkThe state quantity of (c);
Figure 78727DEST_PATH_IMAGE021
Figure 763786DEST_PATH_IMAGE022
；
Figure 443029DEST_PATH_IMAGE023
indicating the horizonkA sequence of positions;
Figure 326671DEST_PATH_IMAGE024
indicating the horizonkA sequence of velocities;Cplanning an objective function of an optimization model for the distributed unmanned cluster cooperative motion path;
52) defining optimization variables, including control inputs
Figure 3640DEST_PATH_IMAGE025
State quantity of
Figure 739777DEST_PATH_IMAGE026
And early warning belt variables
Figure 691553DEST_PATH_IMAGE027
(ii) a Self-defining early warning zone variables;
initial position specification in planning variables
Figure 898543DEST_PATH_IMAGE028
And an initial speed is defined as
Figure 808730DEST_PATH_IMAGE029
Wherein
Figure 265120DEST_PATH_IMAGE030
And
Figure 223848DEST_PATH_IMAGE031
is an object oftActual position of time and in
Figure 82083DEST_PATH_IMAGE032
The actual speed at the moment is obtained by measurement;
53) defining constraints comprising: the method comprises the following steps of dynamic constraint, obstacle avoidance constraint, early warning zone constraint, input constraint, speed constraint and terminal constraint; self-defining obstacle avoidance constraint, early warning zone constraint and terminal constraint;
the kinetic constraint, expressed as:
Figure 897592DEST_PATH_IMAGE033
wherein:
Figure 841277DEST_PATH_IMAGE034
it is composed of
Figure 603697DEST_PATH_IMAGE035
,
Figure 582017DEST_PATH_IMAGE036
,
Figure 792595DEST_PATH_IMAGE037
，
Figure 426838DEST_PATH_IMAGE038
The dimension of expression isdThe identity matrix of (1); also:
Figure 789687DEST_PATH_IMAGE039
obstacle avoidance and restraint: the following linear constraints are obtained by dividing the space through a buffer voronoi unit with an early warning zone:
Figure 622513DEST_PATH_IMAGE040
wherein
Figure 717508DEST_PATH_IMAGE041
Is a warning tape variable; parameter(s)
Figure 901365DEST_PATH_IMAGE042
And
Figure 802325DEST_PATH_IMAGE043
the definition is as follows:
Figure 427341DEST_PATH_IMAGE044
Figure 21134DEST_PATH_IMAGE045
is the last moment
Figure 161128DEST_PATH_IMAGE046
When in the visual fieldkThe planning result is also called as a preset path; first planning time taking
Figure 69041DEST_PATH_IMAGE047
Wherein
Figure 610881DEST_PATH_IMAGE048
Is an initial position; extend minimum distance
Figure 877040DEST_PATH_IMAGE049
(ii) a Wherein the content of the first and second substances,
Figure 707592DEST_PATH_IMAGE050
is the minimum distance between two objects;
the early warning band constraint is expressed as:
Figure 950355DEST_PATH_IMAGE051
wherein the content of the first and second substances,
Figure 612280DEST_PATH_IMAGE052
the width of the buffer zone is indicated and is set manually.
The input constraints are:
Figure 282296DEST_PATH_IMAGE053
the speed constraint is:
Figure 600145DEST_PATH_IMAGE054
the terminal constraints are:
Figure 381019DEST_PATH_IMAGE055
target function of constructed distributed unmanned cluster cooperative motion path planning optimization model
Figure 163030DEST_PATH_IMAGE056
Represented by formula (2):
Figure 207210DEST_PATH_IMAGE057
formula (2)
Wherein the content of the first and second substances,Cis an objective function;
Figure 74672DEST_PATH_IMAGE058
is the visual fieldkThe weight constant of (1);
Figure 659237DEST_PATH_IMAGE059
is a target position of the object;
Figure 531640DEST_PATH_IMAGE060
Figure 543458DEST_PATH_IMAGE061
is an objectiAbout an objectjOf wherein
Figure 570320DEST_PATH_IMAGE062
Is a constant number of times that the number of the first,
Figure 958576DEST_PATH_IMAGE063
is an objectiAbout an objectjThe avoidance angle parameter;
54) solving the cluster motion path planning problem, wherein the obtained result comprises input information
Figure 449600DEST_PATH_IMAGE064
And location information
Figure 835582DEST_PATH_IMAGE065
；
55) The solution result is obtained
Figure 412057DEST_PATH_IMAGE066
AstInput of time of day
Figure 135163DEST_PATH_IMAGE067
The motion is input into a lower-layer motion control system, so that the motion control is realized, and the purpose of motion obstacle avoidance is achieved;
6) location information results obtained from optimization problems
Figure 418376DEST_PATH_IMAGE068
Obtaining a predetermined path
Figure 303156DEST_PATH_IMAGE069
(ii) a Wherein h is a set time interval;
namely: generating a position information result
Figure 602812DEST_PATH_IMAGE068
Set as a preset path
Figure 536133DEST_PATH_IMAGE069
(ii) a Wherein the content of the first and second substances,kfirst fingerkAt each FOV;his a set time interval;tis the time;ifirst fingeriAn object.
Through the steps, the planning of the distributed unmanned cluster cooperative motion path for avoiding deadlock is realized.
The invention is implemented by adopting a crazyfiles unmanned aerial vehicle system to realize the process of the method. In specific implementation, the optimization model is solved by the convex optimization function CVXPY and a solver MOSEK matched with the CVXPY in python language, so that an accurate global optimal solution can be obtained in a short time, and the reliability is high.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides an unmanned cluster cooperative motion path planning method which ensures feasibility and avoids deadlock. Based on the distributed model predictive control, the concept of infinite view is introduced by adding terminal constraint, so that the feasibility of optimizing the problem solution is realized, and the feasibility of planning the motion path is ensured. Secondly, a buffer Voronoi cell (a warning buffered Voronoi cell) with an early warning band is introduced to divide the motion space of different objects, and a penalty function related to the early warning band is introduced into a target function to avoid the deadlock problem. The method can ensure the feasibility of the unmanned cluster in motion path planning and avoid deadlock, and is particularly suitable for motion path planning in a dense cluster environment in a dynamic environment, complex dynamics requirements and high maneuverability. The method can be applied to the situations such as multi-missile cooperative strike, multi-robot cooperative transportation, multi-unmanned aerial vehicle cooperative reconnaissance and the like.
Drawings
Fig. 1 is a block diagram of a platform module according to an embodiment of the present invention, which includes a sensing layer, a planning layer, and an execution layer.
FIG. 2 is a block flow diagram of the method of the present invention.
FIG. 3 is a block diagram of a process for implementing the method of the present invention based on optitrack and crazyflies according to an embodiment of the present invention.
FIG. 4 is a graph of obtaining an avoidance angle parameter
Figure 736170DEST_PATH_IMAGE070
Schematic representation of (a).
Wherein the content of the first and second substances,xOya reference plane that is artificially defined,
Figure 260693DEST_PATH_IMAGE071
as an objectiIn the visual fieldKIs located at a predetermined position in the vertical direction,
Figure 811760DEST_PATH_IMAGE072
as an objectjIn the visual fieldKIs located at a predetermined position in the vertical direction,
Figure 283192DEST_PATH_IMAGE070
is an objectiAbout an objectjThe avoidance angle of (2).
Detailed Description
The invention will be further described by way of examples, without in any way limiting the scope of the invention, with reference to the accompanying drawings.
The invention provides a distributed unmanned cluster cooperative motion path planning method capable of guaranteeing feasibility and avoiding deadlock, which can solve the problem of cluster system cooperative motion path planning with complex dynamics, avoid the deadlock problem and guarantee the feasibility.
Fig. 1 shows a structure of an experimental platform for implementing the method of the present invention, and the overall flow of the method of the present invention is shown in fig. 2. The following is a specific process of each object in the distributed unmanned cluster collaborative motion path planning to avoid deadlock and guarantee feasibility:
1) initializing a preset path in a distributed unmanned cluster to an objectiFor example, the initialized default path is recorded as
Figure 603315DEST_PATH_IMAGE073
Figure 564318DEST_PATH_IMAGE074
，
Figure 274785DEST_PATH_IMAGE074
Is the initial position of the preset path,ibeing an object in a distributed unmanned cluster;
Figure 346646DEST_PATH_IMAGE075
is the initial time;kis the view serial number; then entering a main cycle;
2) any one object in distributed unmanned cluster
Figure 786855DEST_PATH_IMAGE076
Communicating respective preset path information with other objects;
3) acquiring state information including position information and speed information through a real-time positioning system (such as Vicon, Optitrack or GPS);
4) obtaining an avoidance coefficient
Figure 856442DEST_PATH_IMAGE077
；
Figure 617987DEST_PATH_IMAGE009
As an objectiAbout an objectjThe avoidance angle parameter of (1);
Figure 493539DEST_PATH_IMAGE078
is a constant;
5) after state information and avoidance coefficients are obtained, a CVXPY is utilized to construct an optimization problem, a MOSEK solver is used for solving in the CVXPY, and the obtained result comprises input information
Figure 725937DEST_PATH_IMAGE079
And location information
Figure 763163DEST_PATH_IMAGE080
(ii) a Input information in the obtained result
Figure 776119DEST_PATH_IMAGE079
Setting as a control input;
6) and then using the location information in the results of the above optimization problem
Figure 393045DEST_PATH_IMAGE081
Obtaining a new preset path
Figure 276687DEST_PATH_IMAGE082
；
7) Finally inputting the control quantity
Figure 15973DEST_PATH_IMAGE079
To the underlying motion control systems (crazyfiles). The motion process of the objects in the distributed unmanned cluster is realized through control input.
In the field of distributed unmanned cluster cooperative control technology, the most common problem of cluster motion path planning is described as follows:
assuming cluster existenceNAn object. Need thisNThe objects are identical
Figure 188329DEST_PATH_IMAGE083
The motion in the dimensional space is, at an initial moment,
Figure 140104DEST_PATH_IMAGE075
from the initial position
Figure 409411DEST_PATH_IMAGE084
Move to respective target positions
Figure 289905DEST_PATH_IMAGE085
And do not collide with each other during movement.
Is numbered for any one of themiThe object of (2), which needs to move from an initial position to a target motion position, has the dynamics of a second order system, approximating as much as possible the actual situation:
Figure 949556DEST_PATH_IMAGE086
wherein
Figure 970602DEST_PATH_IMAGE087
Is a derivative of the state information;
Figure 32099DEST_PATH_IMAGE088
and
Figure 847608DEST_PATH_IMAGE089
as kinematic matrix:
Figure 56873DEST_PATH_IMAGE090
Figure 350451DEST_PATH_IMAGE091
the status of the system is indicated,
Figure 266454DEST_PATH_IMAGE092
respectively, representing the position, velocity and control input of the object. And the following speed and input constraints need to be satisfied:
Figure 518444DEST_PATH_IMAGE093
wherein the content of the first and second substances,
Figure 152688DEST_PATH_IMAGE094
the acceleration is the maximum acceleration, and the acceleration is the maximum acceleration,
Figure 249957DEST_PATH_IMAGE095
is the maximum speed;
Figure 82783DEST_PATH_IMAGE096
representing the modulus of the vector. Any two objectsiAndjthe following conditions are required to be met for avoiding the obstacle:
Figure 443358DEST_PATH_IMAGE097
wherein
Figure 863100DEST_PATH_IMAGE098
Is two objectsi、jAnd a minimum distance therebetween, less than which the two would collide.
Aiming at the problem of cluster motion path planning, a distributed model predictive control method is adopted to realize motion path planning. Distributed model predictive control entails having each object at each elapsed time intervalhLength of field of view
Figure 764060DEST_PATH_IMAGE099
And constructing and solving the following optimization problem for re-planning.
Figure 654655DEST_PATH_IMAGE100
Figure 982869DEST_PATH_IMAGE101
Figure 388442DEST_PATH_IMAGE102
Wherein the content of the first and second substances,
Figure 93093DEST_PATH_IMAGE103
the time of this rescheduled time is indicated. The solution result is obtained
Figure 838195DEST_PATH_IMAGE104
Can be used as a system intInput of time of day
Figure 602889DEST_PATH_IMAGE105
And the motion is input into a lower-layer motion control system, so that the motion control is realized, and the purpose of avoiding obstacles by motion is achieved.
The motion path planning problem with the obstacle avoidance requirement is a typical non-convex optimization problem or a nonlinear programming problem after being converted into an optimization problem, and has the characteristics of low solving speed and no global optimal solution. Therefore, how to convert the convex optimization problem into a convex optimization problem is a core problem in model predictive control, and therefore, the construction of the convex optimization problem needs to be described in detail.
In the following, the FOV
Figure 230179DEST_PATH_IMAGE106
WhereinKIs the length of the field of view.
Optimizing variables including control inputs
Figure 410625DEST_PATH_IMAGE107
State quantity of
Figure 338129DEST_PATH_IMAGE026
And an early warning band variable, wherein the invention defines the early warning band variable.
Figure 211408DEST_PATH_IMAGE108
Is the field of visionkThe control input of (a) is received,
Figure 827459DEST_PATH_IMAGE109
to the view field to be plannedkA state quantity of (b), wherein
Figure 873912DEST_PATH_IMAGE110
Indicating the horizonkThe sequence of positions of (a) and (b),
Figure 593607DEST_PATH_IMAGE111
indicating the horizonkA velocity sequence of (a). Furthermore, the initial position in the planning variables is specified as
Figure 434524DEST_PATH_IMAGE112
And an initial speed is defined as
Figure 36406DEST_PATH_IMAGE113
Wherein
Figure 886551DEST_PATH_IMAGE030
And
Figure 460751DEST_PATH_IMAGE114
is an object oftActual position of time and in
Figure 472570DEST_PATH_IMAGE115
The actual speed at the moment is obtained by measurement;
Figure 561749DEST_PATH_IMAGE116
is a warning tape variable.
The constraint includes: the method comprises the following steps of dynamic constraint, obstacle avoidance constraint, early warning zone constraint, input constraint, speed constraint and terminal constraint; the invention defines obstacle avoidance constraint, early warning zone constraint and terminal constraint.
Figure 153267DEST_PATH_IMAGE117
Figure 378712DEST_PATH_IMAGE118
Wherein:
Figure 340194DEST_PATH_IMAGE119
wherein
Figure 916669DEST_PATH_IMAGE035
,
Figure 46299DEST_PATH_IMAGE036
,
Figure 391830DEST_PATH_IMAGE037
，
Figure 11030DEST_PATH_IMAGE038
The dimension of expression isdThe identity matrix of (1); also:
Figure 12484DEST_PATH_IMAGE120
obstacle avoidance and restraint:
the following linear constraints can be obtained by dividing the space by the buffer voronoi unit with the early warning zone:
Figure 742543DEST_PATH_IMAGE121
wherein the parameters
Figure 145842DEST_PATH_IMAGE122
And
Figure 935944DEST_PATH_IMAGE123
the definition is as follows:
Figure 221432DEST_PATH_IMAGE124
Figure 755181DEST_PATH_IMAGE045
is the last moment
Figure 12987DEST_PATH_IMAGE046
When in the visual fieldkThe planning result is also called as a preset path; first planning time taking
Figure 475455DEST_PATH_IMAGE047
Wherein
Figure 248239DEST_PATH_IMAGE048
Is an initial position; extend minimum distance
Figure 523362DEST_PATH_IMAGE125
(ii) a Wherein the content of the first and second substances,
Figure 697992DEST_PATH_IMAGE050
is the minimum distance between two objects;
the early warning band constraint is expressed as:
Figure 829896DEST_PATH_IMAGE126
wherein the content of the first and second substances,
Figure 27659DEST_PATH_IMAGE052
the width of the buffer zone is indicated and is set manually.
Inputting constraints:
Figure 168790DEST_PATH_IMAGE127
speed constraint:
Figure 197926DEST_PATH_IMAGE128
and (4) terminal constraint:
Figure 438414DEST_PATH_IMAGE129
the existence of the terminal constraint ensures the existence of the optimization problem solution.
Target function of constructed distributed unmanned cluster cooperative motion path planning optimization modelCRepresented by formula (3):
Figure 451370DEST_PATH_IMAGE130
formula (3)
Wherein the content of the first and second substances,Cis an objective function;
Figure 865034DEST_PATH_IMAGE131
is the visual fieldkThe weight constant of (1);
Figure 951938DEST_PATH_IMAGE059
is a target position of the object;
Figure 661531DEST_PATH_IMAGE060
Figure 896203DEST_PATH_IMAGE132
is an objectiAbout an objectjOf wherein
Figure 316820DEST_PATH_IMAGE062
Is a constant number of times that the number of the first,
Figure 54969DEST_PATH_IMAGE133
is an objectiAbout an objectjThe avoidance angle parameter of (1), which is obtained as shown in fig. 4. In the context of figure 4, it is shown,xOya reference plane that is artificially defined,
Figure 965156DEST_PATH_IMAGE134
as an objectiIn the visual fieldKIs located at a predetermined position in the vertical direction,
Figure 624807DEST_PATH_IMAGE135
as an objectjIn the visual fieldKAt a preset position.
Figure 380274DEST_PATH_IMAGE136
Is composed of
Figure 504088DEST_PATH_IMAGE137
And
Figure 319597DEST_PATH_IMAGE138
the included angle therebetween.
Fig. 3 shows a flow of implementing the method of the present invention using a crazyfiles unmanned aerial vehicle system in the embodiment of the present invention.
When the method is implemented specifically, the optimization model is solved by the convex optimization function CVXPY and the solver MOSEK matched with the CVXPY in the python language, so that an accurate global optimal solution can be obtained in a short time, and the method has high reliability.
According to the method, terminal constraints are added into the optimization problem model, the view length in model prediction control can be equivalent to infinity, and further, the concept of infinite view is introduced. Then, the space is divided by using the buffer Voronoi unit containing the early warning zone based on the preset path, so that the preset path used in the optimization process can be proved to meet all the limits in the convex optimization, the feasible region of the convex optimization is guaranteed to be non-empty, and the recursion feasibility of a plurality of continuous convex optimization problems (namely sequence convex optimization) at different moments is realized. The invention uses and expands the minimum distance
Figure 200965DEST_PATH_IMAGE125
Alternative minimum distances
Figure 25702DEST_PATH_IMAGE050
Forming obstacle avoidance constraints (equations (11) - (12)) compared with the original minimum distance
Figure 239908DEST_PATH_IMAGE050
The obstacle avoidance condition at the sampling point can only be considered, and the extended distance is minimum
Figure 429581DEST_PATH_IMAGE139
The obstacle avoidance situation in the time period between sampling points can be considered, so that the obstacle avoidance feasibility in the whole process is further ensured. Therefore, the method ensures the obstacle avoidance feasibility.
The invention also adds the variable of the early warning zone into the obstacle avoidance constraint
Figure 126141DEST_PATH_IMAGE140
So that a plurality of objects can generate interaction force in the process of mutual blocking, and then the avoidance coefficient is set
Figure 223410DEST_PATH_IMAGE141
The magnitude of the interaction force is controlled to generate a right-handed force, so that the deadlock state caused by balanced force among multiple objects is broken. This dextrorotatory acting force can be optimally solved by the convex optimization problemKKT (Karush-Kuhn-Tucker) conditions of (1) were demonstrated and further elucidated
Figure 993920DEST_PATH_IMAGE142
For the purpose of breaking the force balance, by setting
Figure 151232DEST_PATH_IMAGE141
And deadlock states among multiple objects due to balanced acting force are broken.
To sum up, the invention realizes the feasibility of optimizing the problem solution by adding the concept of introducing infinite view by terminal constraint based on the distributed model predictive control, thereby ensuring the feasibility of motion path planning. And dividing motion spaces of different objects by introducing a buffering Voronoi cell (warning buffered Voronoi cell) containing an early warning band, and avoiding a deadlock problem by introducing a penalty function related to the early warning band into a target function. The specific implementation shows that the method can ensure the feasibility of the unmanned cluster in motion path planning and avoid deadlock, and is particularly suitable for motion path planning in a dense cluster environment in a dynamic environment, complex dynamics requirements and high maneuverability. The method can be applied to the situations such as multi-missile cooperative strike, multi-robot cooperative transportation, multi-unmanned aerial vehicle cooperative reconnaissance and the like.
It is noted that the disclosed embodiments are intended to aid in further understanding of the invention, but those skilled in the art will appreciate that: various alternatives and modifications are possible without departing from the invention and scope of the appended claims. Therefore, the invention should not be limited to the embodiments disclosed, but the scope of the invention is defined by the appended claims.

Claims (6)

1. A method for planning cooperative motion paths of a distributed unmanned cluster to avoid deadlock is disclosed, wherein N objects in the distributed unmanned cluster move in the same dimensional space; the object is at the initial moment
Figure 478384DEST_PATH_IMAGE001
From the initial position
Figure 582606DEST_PATH_IMAGE002
Move to respective target positions
Figure 269939DEST_PATH_IMAGE003
And do not collide with each other during the movement; the method is characterized in that aiming at the distributed unmanned cluster movement path planning, an optimization problem and a model function of the distributed unmanned cluster cooperative movement path planning are constructed by defining constraints, targets and variables, an infinite view is defined by adding terminal constraints, then an obstacle avoidance constraint is constructed by using a buffer Weinuo unit containing an early warning zone, and a penalty function related to the early warning zone is introduced into a target function;
the method for planning the distributed unmanned cluster cooperative motion path for avoiding deadlock comprises the following steps:
1) initializing a preset path in the distributed unmanned cluster;
distributing any one object in unmanned clusteriIs recorded as
Figure 863732DEST_PATH_IMAGE004
，
Figure 206988DEST_PATH_IMAGE005
Is an initial position;
Figure 646060DEST_PATH_IMAGE006
is the initial time; horizon of the eye
Figure 453479DEST_PATH_IMAGE007
WhereinKIs the length of the visual field;
2） icommunicating respective preset path information with other objects;
3) acquiring state information including position information and speed information through a real-time positioning system;
4) obtainTaking avoidance coefficient
Figure 155856DEST_PATH_IMAGE008
；
Figure 783146DEST_PATH_IMAGE009
As an objectiAbout an objectjThe avoidance angle parameter of (1);
Figure 792953DEST_PATH_IMAGE010
is a constant;
5) according to the obtained state information and avoidance coefficients, an infinite view field is defined by adding terminal constraints, an obstacle avoidance constraint is constructed by using a buffer Vono unit containing an early warning zone, penalty functions related to the early warning zone are introduced into a target function, and an optimization problem and a model function of the distributed unmanned cluster cooperative motion path planning are constructed; and solving to obtain input information
Figure 658141DEST_PATH_IMAGE011
And location information
Figure 328156DEST_PATH_IMAGE012
Namely, the planning of the distributed unmanned cluster cooperative motion path for avoiding deadlock is realized; the specific process is as follows:
51) constructing a model function of a distributed unmanned cluster cooperative motion path planning optimization problem, and realizing motion path planning by adopting a distributed model prediction control method; the method comprises the following steps:
any one object in distributed unmanned clusteriFrom an initial position
Figure 442743DEST_PATH_IMAGE013
Move to the target movement position
Figure 692459DEST_PATH_IMAGE014
Each object at each elapsed time intervalhLength of field of view
Figure 208891DEST_PATH_IMAGE015
Constructing and solving the convex-down optimization problem for re-planning;
the model function of the constructed distributed unmanned cluster cooperative motion optimization path planning optimization problem is represented as follows:
Figure 315387DEST_PATH_IMAGE016
Figure 854953DEST_PATH_IMAGE017
Figure 705097DEST_PATH_IMAGE018
formula (1)
Wherein the content of the first and second substances,
Figure 76035DEST_PATH_IMAGE019
is the field of visionkA control input of;
Figure 291116DEST_PATH_IMAGE020
view obtained for planningkThe state quantity of (c);
Figure 645874DEST_PATH_IMAGE021
Figure 270016DEST_PATH_IMAGE022
；
Figure 964302DEST_PATH_IMAGE023
indicating the horizonkA sequence of positions;
Figure 412601DEST_PATH_IMAGE024
indicating the horizonkA sequence of velocities;Cas a distributed unmanned collectionPlanning an objective function of an optimization model by using the group collaborative motion path;
52) defining optimization variables, including control inputs
Figure 926759DEST_PATH_IMAGE025
State quantity of
Figure 853127DEST_PATH_IMAGE026
And early warning belt variables
Figure 464237DEST_PATH_IMAGE027
(ii) a Self-defining early warning zone variables;
initial position specification in planning variables
Figure 21120DEST_PATH_IMAGE028
And an initial speed is defined as
Figure 84891DEST_PATH_IMAGE029
Wherein
Figure 814950DEST_PATH_IMAGE030
And
Figure 218249DEST_PATH_IMAGE031
is an object oftActual position of time and in
Figure 8350DEST_PATH_IMAGE032
The actual speed of the moment is obtained by measurement;
53) defining constraints comprising: the method comprises the following steps of dynamic constraint, obstacle avoidance constraint, early warning zone constraint, input constraint, speed constraint and terminal constraint; self-defining obstacle avoidance constraint, early warning zone constraint and terminal constraint;
the kinetic constraint, expressed as:
Figure 293838DEST_PATH_IMAGE033
wherein:
Figure 30850DEST_PATH_IMAGE034
wherein
Figure 586859DEST_PATH_IMAGE035
,
Figure 547861DEST_PATH_IMAGE036
,
Figure 523908DEST_PATH_IMAGE037
，
Figure 595769DEST_PATH_IMAGE038
The dimension of expression isdThe identity matrix of (1); also:
Figure 504819DEST_PATH_IMAGE039
the obstacle avoidance constraint is a linear constraint obtained by dividing the space through a buffer voronoi unit with an early warning zone, and is expressed as follows:
Figure 902302DEST_PATH_IMAGE040
wherein
Figure 100066DEST_PATH_IMAGE041
Is a warning tape variable; parameter(s)
Figure 241197DEST_PATH_IMAGE042
And
Figure 4754DEST_PATH_IMAGE043
the definition is as follows:
Figure 510821DEST_PATH_IMAGE044
Figure 258197DEST_PATH_IMAGE045
is the last moment
Figure 173326DEST_PATH_IMAGE046
When in the visual fieldkThe planning result is also called as a preset path; first planning time taking
Figure 260231DEST_PATH_IMAGE047
Wherein
Figure 999517DEST_PATH_IMAGE048
Is an initial position; extend a minimum distance of
Figure 171872DEST_PATH_IMAGE049
(ii) a Wherein the content of the first and second substances,
Figure 389227DEST_PATH_IMAGE050
is the minimum distance between two objects;
the early warning band constraint is expressed as:
Figure 127376DEST_PATH_IMAGE051
wherein the content of the first and second substances,
Figure 37563DEST_PATH_IMAGE052
indicating the width of the buffer zone;
the input constraints are:
Figure 697214DEST_PATH_IMAGE053
the speed constraint is:
Figure 452681DEST_PATH_IMAGE054
the terminal constraints are:
Figure 576494DEST_PATH_IMAGE055
target function of constructed distributed unmanned cluster cooperative motion path planning optimization modelCRepresented by formula (2):
Figure 126424DEST_PATH_IMAGE056
formula (2)
Wherein the content of the first and second substances,Cis an objective function;
Figure 273372DEST_PATH_IMAGE057
is the visual fieldkThe weight constant of (1);
Figure 599573DEST_PATH_IMAGE058
is a target position of the object;
Figure 312314DEST_PATH_IMAGE059
Figure 501987DEST_PATH_IMAGE060
is an objectiAbout an objectjOf wherein
Figure 932969DEST_PATH_IMAGE061
Is a constant number of times that the number of the first,
Figure 30238DEST_PATH_IMAGE062
is an objectiAbout an objectjThe avoidance angle parameter of (1);
54) solving the cluster motion path planning problem, wherein the obtained result comprises input information
Figure 66327DEST_PATH_IMAGE063
And location information
Figure 489218DEST_PATH_IMAGE064
；
55) The solution result is obtained
Figure 141916DEST_PATH_IMAGE065
AstInput of time of day
Figure 246138DEST_PATH_IMAGE066
The motion is input into a lower-layer motion control system, so that the motion control is realized, and the purpose of motion obstacle avoidance is achieved;
6) location information results obtained from optimization problems
Figure 199051DEST_PATH_IMAGE067
Obtaining a predetermined path
Figure 527264DEST_PATH_IMAGE068
(ii) a Wherein h is a set time interval; through the steps, the planning of the distributed unmanned cluster cooperative motion path for avoiding deadlock is realized.
2. The deadlock avoidance distributed unmanned cluster cooperative motion path planning method of claim 1, wherein the individual objects in the distributed unmanned cluster include unmanned vehicles, unmanned vehicles and unmanned boats.
3. The deadlock avoidance distributed unmanned cluster collaborative motion path planning method of claim 1, wherein individual objects in the distributed unmanned cluster are in the same dimension
Figure 870521DEST_PATH_IMAGE069
And (4) moving in a dimensional space.
4. The deadlock avoidance distributed unmanned cluster cooperative motion path planning method of claim 1, wherein any one of the distributed unmanned clusters is numbered asiThe process of moving from the initial position to the target moving position is represented as:
Figure 76636DEST_PATH_IMAGE070
wherein
Figure 618476DEST_PATH_IMAGE071
Is a derivative of the state information;
Figure 320853DEST_PATH_IMAGE072
and
Figure 213723DEST_PATH_IMAGE073
as kinematic matrix:
Figure 456485DEST_PATH_IMAGE074
Figure 56094DEST_PATH_IMAGE075
the status of the system is indicated,
Figure 726109DEST_PATH_IMAGE076
the position, velocity and control inputs of the object are represented separately, and the following velocity and input constraints need to be satisfied:
Figure 106275DEST_PATH_IMAGE077
wherein the content of the first and second substances,
Figure 355991DEST_PATH_IMAGE078
the acceleration is the maximum acceleration, and the acceleration is the maximum acceleration,
Figure 606844DEST_PATH_IMAGE079
is the maximum speed;
Figure 713340DEST_PATH_IMAGE080
modulo representing a vector, any two objectsiAndjthe following conditions are required to be met for avoiding the obstacle:
Figure 518485DEST_PATH_IMAGE081
wherein
Figure 592796DEST_PATH_IMAGE082
Is two objectsi、jAnd a minimum distance therebetween, less than which the two would collide.
5. The deadlock avoidance distributed unmanned cluster cooperative motion path planning method of claim 1, wherein the real time positioning system specifically employs Vicon, Optitrack or GPS.
6. The deadlock avoidance distributed unmanned cluster cooperative motion path planning method of claim 1, wherein the lower layer motion control system specifically employs crazyfiles.
CN202111184687.5A 2021-10-12 2021-10-12 Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock Active CN113625780B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111184687.5A CN113625780B (en) 2021-10-12 2021-10-12 Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111184687.5A CN113625780B (en) 2021-10-12 2021-10-12 Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock

Publications (2)

Publication Number Publication Date
CN113625780A true CN113625780A (en) 2021-11-09
CN113625780B CN113625780B (en) 2022-01-28

Family

ID=78391012

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111184687.5A Active CN113625780B (en) 2021-10-12 2021-10-12 Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock

Country Status (1)

Country Link
CN (1) CN113625780B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115237157A (en) * 2022-08-08 2022-10-25 南京理工大学 Road network constraint-based air-to-ground unmanned cluster multi-task point path planning method
CN115328161A (en) * 2022-09-15 2022-11-11 安徽工程大学 Welding robot path planning method based on K-view ant colony algorithm
CN115774455A (en) * 2023-02-13 2023-03-10 北京大学 Distributed unmanned cluster trajectory planning method for avoiding deadlock in complex obstacle environment
CN115328161B (en) * 2022-09-15 2024-04-26 安徽工程大学 Welding robot path planning method based on K vision ant colony algorithm

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109582027A (en) * 2019-01-14 2019-04-05 哈尔滨工程大学 A kind of USV cluster collision-avoidance planning method based on Modified particle swarm optimization algorithm
CN111291984A (en) * 2020-01-21 2020-06-16 北京大学 Multi-unmanned aerial vehicle distributed task selection and trajectory design method and device
CN111811511A (en) * 2020-06-23 2020-10-23 北京理工大学 Unmanned aerial vehicle cluster real-time track generation method based on dimension reduction decoupling mechanism
US20200348696A1 (en) * 2018-07-18 2020-11-05 The Trustees Of The University Of Pennsylvania Control of multi-drone fleets with temporal logic objectives
CN112000131A (en) * 2020-09-09 2020-11-27 中国人民解放军国防科技大学 Unmanned aerial vehicle cluster path planning method and system based on artificial potential field method
CN113253764A (en) * 2021-07-14 2021-08-13 北京大学 Unmanned cluster affine formation control method based on dimensionality reduction observer

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200348696A1 (en) * 2018-07-18 2020-11-05 The Trustees Of The University Of Pennsylvania Control of multi-drone fleets with temporal logic objectives
CN109582027A (en) * 2019-01-14 2019-04-05 哈尔滨工程大学 A kind of USV cluster collision-avoidance planning method based on Modified particle swarm optimization algorithm
CN111291984A (en) * 2020-01-21 2020-06-16 北京大学 Multi-unmanned aerial vehicle distributed task selection and trajectory design method and device
CN111811511A (en) * 2020-06-23 2020-10-23 北京理工大学 Unmanned aerial vehicle cluster real-time track generation method based on dimension reduction decoupling mechanism
CN112000131A (en) * 2020-09-09 2020-11-27 中国人民解放军国防科技大学 Unmanned aerial vehicle cluster path planning method and system based on artificial potential field method
CN113253764A (en) * 2021-07-14 2021-08-13 北京大学 Unmanned cluster affine formation control method based on dimensionality reduction observer

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
XIANGKE WANG 等: "Distributed sliding mode control for leader-follower formation flight of fixed-wing unmanned aerial vehicles subject to velocity constraints", 《INTERNATIONAL JOURNAL OF ROBUST AND NONLINEAR CONTROL》 *
刘淼 等: "不确定通讯下的异构多智能体网络鲁棒编队控制", 《空间控制技术与应用》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115237157A (en) * 2022-08-08 2022-10-25 南京理工大学 Road network constraint-based air-to-ground unmanned cluster multi-task point path planning method
CN115237157B (en) * 2022-08-08 2024-01-23 南京理工大学 Air-ground unmanned cluster multi-task point path planning method under road network constraint
CN115328161A (en) * 2022-09-15 2022-11-11 安徽工程大学 Welding robot path planning method based on K-view ant colony algorithm
CN115328161B (en) * 2022-09-15 2024-04-26 安徽工程大学 Welding robot path planning method based on K vision ant colony algorithm
CN115774455A (en) * 2023-02-13 2023-03-10 北京大学 Distributed unmanned cluster trajectory planning method for avoiding deadlock in complex obstacle environment

Also Published As

Publication number Publication date
CN113625780B (en) 2022-01-28

Similar Documents

Publication Publication Date Title
Yang et al. Collision free 4D path planning for multiple UAVs based on spatial refined voting mechanism and PSO approach
Singla et al. Memory-based deep reinforcement learning for obstacle avoidance in UAV with limited environment knowledge
Chen et al. Cyber-physical system enabled nearby traffic flow modelling for autonomous vehicles
Hong et al. Rules of the road: Predicting driving behavior with a convolutional model of semantic interactions
CN109032168B (en) DQN-based multi-unmanned aerial vehicle collaborative area monitoring airway planning method
Grigorescu et al. Neurotrajectory: A neuroevolutionary approach to local state trajectory learning for autonomous vehicles
Zheng et al. Evolutionary route planner for unmanned air vehicles
CN113625780B (en) Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock
Oh et al. Rendezvous and standoff target tracking guidance using differential geometry
Zhang et al. Pigeon-inspired optimization approach to multiple UAVs formation reconfiguration controller design
Hou et al. Time-coordinated control for unmanned aerial vehicle swarm cooperative attack on ground-moving target
US20210325891A1 (en) Graph construction and execution ml techniques
Brittain et al. Scalable autonomous separation assurance with heterogeneous multi-agent reinforcement learning
Li et al. A deep reinforcement learning based approach for autonomous overtaking
Huang et al. Recoat: A deep learning-based framework for multi-modal motion prediction in autonomous driving application
Al-Sharman et al. Self-learned autonomous driving at unsignalized intersections: A hierarchical reinforced learning approach for feasible decision-making
Ali et al. Feature selection-based decision model for UAV path planning on rough terrains
Sanders et al. Optimal offline path planning of a fixed wing unmanned aerial vehicle (UAV) using an evolutionary algorithm
CN112034880A (en) Novel multi-unmanned aerial vehicle collaborative route planning method
Regier et al. Improving navigation with the social force model by learning a neural network controller in pedestrian crowds
CN112258896A (en) Unmanned aerial vehicle fusion airspace operation method based on flight path
CN116339321A (en) Global information driven distributed multi-robot reinforcement learning formation surrounding method based on 5G communication
Chronis et al. Dynamic Navigation in Unconstrained Environments Using Reinforcement Learning Algorithms
Skyrda Decentralized autonomous unmanned aerial vehicle swarm formation and flight control
CN112161626B (en) High-flyability route planning method based on route tracking mapping network

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