CN113625780B - 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
CN113625780B
CN113625780B CN202111184687.5A CN202111184687A CN113625780B CN 113625780 B CN113625780 B CN 113625780B CN 202111184687 A CN202111184687 A CN 202111184687A CN 113625780 B CN113625780 B CN 113625780B
Authority
CN
China
Prior art keywords
constraint
distributed unmanned
planning
unmanned cluster
distributed
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
CN202111184687.5A
Other languages
Chinese (zh)
Other versions
CN113625780A (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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

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, an optimization problem and a model function of the distributed unmanned cluster cooperative movement path planning are established by defining constraints, targets and variables, an infinite view is defined by adding terminal constraints, then an obstacle avoidance constraint is established 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, so that the problems of cluster system cooperative movement path planning with complex dynamics, deadlock and avoidance feasibility are solved.

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 constraints, targets and variables, specifically, firstly, an infinite view is introduced by adding terminal constraints to realize the feasibility of an optimization problem solution in model prediction control, so that the feasibility of a movement path planning algorithm is ensured, secondly, a buffer Weino unit containing an early warning zone is used for constructing an obstacle avoidance constraint, and a penalty function related to the early warning zone is introduced into a target function to avoid deadlock; 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 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 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,
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
Time of dayThe actual speed of the vehicle, obtained from the 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,
Figure 707592DEST_PATH_IMAGE050
is the minimum distance between two objects;
the early warning band constraint is expressed as:
Figure 950355DEST_PATH_IMAGE051
wherein,
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,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,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,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,
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,
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 oftTime of dayAnd at the actual position of
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,
Figure 697992DEST_PATH_IMAGE050
is the minimum distance between two objects;
the early warning band constraint is expressed as:
Figure 829896DEST_PATH_IMAGE126
wherein,
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,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, using a buffer Voronoi unit with an early warning band to be empty based on a preset pathThe segmentation is carried out, so that the preset path used in the optimization process can be proved to meet all the limitations 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 right-handed force can be demonstrated by KKT (Karush-Kuhn-Tucker) condition of the optimal solution of the convex optimization problem and further explained
Figure 993920DEST_PATH_IMAGE142
For breaking the balance of forceBy 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; object at initial time t0From an initial position p0To respective target positions ptargetAnd 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 variableDefining an infinite visual field by adding terminal constraint, constructing obstacle avoidance constraint by using a buffer Vono unit containing an early warning zone, and introducing a penalty function related to the early warning zone 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;
recording a preset path of any object i in the distributed unmanned cluster as
Figure FDA0003367043610000011
p0Is an initial position; t is t0Is the initial time; k, where K is the FOV length;
2) i, communicating 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 rhoij＝ρ0exp(sinθij)；θijAn avoidance angle parameter for object i with respect to object j; rho0Is 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 FDA0003367043610000018
And location information
Figure FDA0003367043610000019
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 object i in the distributed unmanned cluster is from the initial position
Figure FDA0003367043610000012
Move to the target movement position
Figure FDA0003367043610000013
Taking the view length K epsilon Z of each object at each time interval h+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 FDA0003367043610000014
Figure FDA0003367043610000015
Figure FDA0003367043610000016
eij＜ε
Figure FDA0003367043610000017
Figure FDA0003367043610000021
wherein,
Figure FDA0003367043610000022
is a control input at FOV;
Figure FDA0003367043610000023
obtaining state quantity of the view k needing to be planned;
Figure FDA0003367043610000024
Figure FDA0003367043610000025
representing a sequence of positions at FOV;
Figure FDA0003367043610000026
showing a velocity sequence at the view field k; c, planning an objective function of the optimization model for the distributed unmanned cluster cooperative motion path; a ismaxIs the maximum acceleration; v. ofmaxIs the maximum speed;
52) defining optimization variables, including control inputs
Figure FDA0003367043610000027
Quantity of state
Figure FDA0003367043610000028
And the early warning band variable eij(ii) a Self-defining early warning zone variables;
initial position specification in planning variables
Figure FDA0003367043610000029
And an initial speed is defined as
Figure FDA00033670436100000210
Wherein p isi(t) and
Figure FDA00033670436100000211
is the actual position of the object at time t and
Figure FDA00033670436100000212
actual speed of time of dayDegrees, obtained from the measurements;
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 FDA00033670436100000213
wherein:
Figure FDA00033670436100000214
wherein A is1＝Id， A2＝hId， A3＝Id，IdAn identity matrix representing a dimension d; also:
Figure FDA00033670436100000215
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 FDA00033670436100000216
wherein eijE is R as an early warning zone variable; parameter(s)
Figure FDA00033670436100000217
And
Figure FDA00033670436100000218
the definition is as follows:
Figure FDA0003367043610000031
Figure FDA0003367043610000032
Figure FDA0003367043610000033
the planning result at the view k at the last moment, namely t-h, is also called as a preset path; first planning time taking
Figure FDA0003367043610000034
Wherein p is0Is an initial position; extend a minimum distance of
Figure FDA0003367043610000035
Wherein r isminIs the minimum distance between two objects;
the early warning band constraint is expressed as:
eij＜∈
wherein epsilon R represents the width of the buffer zone;
the input constraints are:
Figure FDA0003367043610000036
the speed constraint is:
Figure FDA0003367043610000037
the terminal constraints are:
Figure FDA0003367043610000038
an objective function C of the constructed distributed unmanned cluster collaborative motion path planning optimization model is expressed as formula (2):
Figure FDA0003367043610000039
wherein C is an objective function; qke.R is a weight constant at the view k;
Figure FDA00033670436100000310
is a target position of the object; rhoij＝ρ0exp(sinθij) Is the avoidance coefficient of object i with respect to object j, where p0e.R is a constant, θijIs an avoidance angle parameter of object i with respect to object j;
54) solving the cluster motion path planning problem, wherein the obtained result comprises input information
Figure FDA00033670436100000311
And location information
Figure FDA00033670436100000312
55) The solution result is obtained
Figure FDA00033670436100000313
As input u at time ti(t) inputting the motion data into a lower-layer motion control system, thereby realizing motion control and achieving the purpose of motion obstacle avoidance;
6) location information results obtained from optimization problems
Figure FDA00033670436100000314
Obtaining a preset path
Figure FDA00033670436100000315
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 method for planning the cooperative motion path of the distributed unmanned cluster for deadlock avoidance according to claim 1, wherein individual objects in the distributed unmanned cluster move in the same dimension d ═ 2, 3} dimensional space.
4. The deadlock avoidance distributed unmanned cluster cooperative motion path planning method according to claim 1, wherein for any object with number i in the distributed unmanned cluster, the process of moving from the initial position to the target motion position is represented as:
Figure FDA0003367043610000041
wherein
Figure FDA0003367043610000042
Is a derivative of the state information;
Figure FDA0003367043610000043
and
Figure FDA0003367043610000044
as kinematic matrix:
Figure FDA0003367043610000045
Figure FDA0003367043610000046
Figure FDA0003367043610000047
indicating the system state, pi，vi，ui∈RnRespectively representing the position, velocity and control input of the object. And the following speed and input constraints need to be satisfied:
||ui(t)||2≤amax
||vi||2≤vmax.
wherein, amaxIs the maximum acceleration, vmaxIs the maximum speed; | | non-woven hair2Representing the modulus of the vector. The following conditions are required to be met for obstacle avoidance between any two objects i and j:
||pi-pj||2≥rmin
wherein r isminIs the minimum separation between two objects i, j, below which they will 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 CN113625780A (en) 2021-11-09
CN113625780B true 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)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115237157B (en) * 2022-08-08 2024-01-23 南京理工大学 Air-ground unmanned cluster multi-task point path planning method under road network constraint
CN115328161B (en) * 2022-09-15 2024-04-26 安徽工程大学 Welding robot path planning method based on K vision ant colony algorithm
CN115774455B (en) * 2023-02-13 2023-04-21 北京大学 Method for planning track of distributed unmanned cluster for avoiding deadlock in complex obstacle environment

Citations (5)

* 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
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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11429118B2 (en) * 2018-07-18 2022-08-30 The Trustees Of The University Of Pennsylvania Control of multi-drone fleets with temporal logic objectives

Patent Citations (5)

* 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
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
Distributed sliding mode control for leader-follower formation flight of fixed-wing unmanned aerial vehicles subject to velocity constraints;Xiangke Wang 等;《INTERNATIONAL JOURNAL OF ROBUST AND NONLINEAR CONTROL》;20210430;第31卷(第6期);2110-2125 *
不确定通讯下的异构多智能体网络鲁棒编队控制;刘淼 等;《空间控制技术与应用》;20181031;第44卷(第5期);47-54 *

Also Published As

Publication number Publication date
CN113625780A (en) 2021-11-09

Similar Documents

Publication Publication Date Title
CN113625780B (en) Distributed unmanned cluster cooperative motion path planning method capable of avoiding deadlock
Yang et al. Collision free 4D path planning for multiple UAVs based on spatial refined voting mechanism and PSO approach
CN114048889B (en) Aircraft trajectory prediction method based on long-term and short-term memory network
Grigorescu et al. Neurotrajectory: A neuroevolutionary approach to local state trajectory learning for autonomous vehicles
Oh et al. Rendezvous and standoff target tracking guidance using differential geometry
CN109871031B (en) Trajectory planning method for fixed-wing unmanned aerial vehicle
Zhang et al. Pigeon-inspired optimization approach to multiple UAVs formation reconfiguration controller design
EP0960413A1 (en) Cooperative resolution of air traffic conflicts
Huang et al. Recoat: A deep learning-based framework for multi-modal motion prediction in autonomous driving application
Kant et al. Long short-term memory auto-encoder-based position prediction model for fixed-wing UAV during communication failure
Ali et al. Feature selection-based decision model for UAV path planning on rough terrains
Ariola et al. Model predictive control for a swarm of fixed wing uavs
Chronis et al. Dynamic navigation in unconstrained environments using reinforcement learning algorithms
Sanders et al. Optimal offline path planning of a fixed wing unmanned aerial vehicle (UAV) using an evolutionary algorithm
Selje et al. Gps-denied three dimensional leader-follower formation control using deep reinforcement learning
CN112258896A (en) Unmanned aerial vehicle fusion airspace operation method based on flight path
Spatharis et al. Apprenticeship learning of flight trajectories prediction with inverse reinforcement learning
Valeriy et al. Method of the multi-uav formation flight control
CN112161626B (en) High-flyability route planning method based on route tracking mapping network
Khachumov Controlling the motion of a group of unmanned flight vehicles in a perturbed environment based on the rules
Skyrda Decentralized autonomous unmanned aerial vehicle swarm formation and flight control
Zardashti et al. Constrained optimal terrain following/threat avoidance trajectory planning using network flow
Gan et al. Research on vehicle trajectory prediction method for intersections without signal lights
Bertoncini et al. Fixed-Wing UAV Path Planning and Collision Avoidance using Nonlinear Model Predictive Control and Sensor-based Cloud Detection
Zhang et al. Multi-loss function for distance-to-collision estimation

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