CN113759977A - Obstacle avoidance trajectory planning method based on optimized tether multi-unmanned aerial vehicle cooperative transportation - Google Patents

Obstacle avoidance trajectory planning method based on optimized tether multi-unmanned aerial vehicle cooperative transportation Download PDF

Info

Publication number
CN113759977A
CN113759977A CN202111103700.XA CN202111103700A CN113759977A CN 113759977 A CN113759977 A CN 113759977A CN 202111103700 A CN202111103700 A CN 202111103700A CN 113759977 A CN113759977 A CN 113759977A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
obstacle
representing
obstacle avoidance
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
CN202111103700.XA
Other languages
Chinese (zh)
Other versions
CN113759977B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202111103700.XA priority Critical patent/CN113759977B/en
Publication of CN113759977A publication Critical patent/CN113759977A/en
Application granted granted Critical
Publication of CN113759977B publication Critical patent/CN113759977B/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention discloses an optimized rope system multi-unmanned aerial vehicle cooperative transportation-based obstacle avoidance track planning method, belonging to the field of rigid-flexible coupling multi-robot cooperative control; establishing a constraint model, deducing constraint conditions, constructing an optimized track plan and finally solving; the method can solve the problem of trajectory planning of a multi-robot cooperative operation system and the problem of planning of real-time obstacle avoidance motion of the robot. The method for explicitly representing the obstacle avoidance constraint between the objects based on the set distance simultaneously adopts the strong dual property in convex optimization to enable the obstacle avoidance constraint to be equivalent and microminiaturized and smooth, and the processing enables the proposed obstacle avoidance trajectory planning problem to be solved by adopting the traditional optimization method based on the gradient and the black plug matrix, so that the calculation complexity can be obviously reduced, the algorithm real-time performance can be improved, and the method is suitable for the obstacle avoidance problem of convex and non-convex obstacles formed by polyhedrons and has wide application range. The method can solve the problem of trajectory planning of a multi-robot cooperative operation system and the problem of real-time obstacle avoidance motion planning of the robot.

Description

Obstacle avoidance trajectory planning method based on optimized tether multi-unmanned aerial vehicle cooperative transportation
Technical Field
The invention belongs to the field of rigid-flexible coupling multi-robot cooperative control, and particularly relates to an optimized rope-based multi-unmanned aerial vehicle cooperative transportation obstacle avoidance trajectory planning method.
Background
Obstacle avoidance trajectory planning is a key technology for realizing autonomous intelligence of a robot and is a precondition for executing safe, stable and efficient motion control. Along with the complexity and diversity of operation tasks, the extreme and uncertain operation environment and the redundancy brought by safe operation requirements, a single robot system is difficult to even cannot meet the requirements, and the common task completed by the cooperative operation of multiple robots becomes an irreplaceable choice. Different from the problem of single-robot track planning, the multi-robot cooperative operation system has high dimensionality, more optimized variables and complex constraint, and the single-robot track planning method has large calculated amount and poor timeliness, so that the purpose of online autonomous obstacle avoidance track planning is difficult to achieve. Therefore, how to implement the real-time trajectory planning of the multi-robot system is an urgent problem to be solved.
Along with the large-scale, heavy and precise material carrying, the urgent need for a platform capable of realizing the carrying of ultra-large and ultra-heavy loads and stable posture control is generated. Tether many unmanned aerial vehicle of suspension type collaborative operation system is as an effective platform of solving above-mentioned problem to its low cost with control the flexibility and become the research focus. The system is a high-nonlinearity and under-actuated complex rigid-flexible coupling system. The tether connection brings kinematic constraint between unmanned aerial vehicles, so that the problem of system trajectory planning is non-convex; meanwhile, the system has high degree of freedom and complex constraint conditions, so that the real-time obstacle avoidance trajectory planning of the tethered multi-unmanned aerial vehicle system is a very challenging problem. Obstacle avoidance trajectory planning of the rope-system multi-unmanned aerial vehicle cooperative operation system needs to consider kinematics and dynamics constraints of the unmanned aerial vehicles, kinematics constraints between the unmanned aerial vehicles caused by connection of the ropes, collision avoidance constraints between the unmanned aerial vehicles and the heavy object, and collision avoidance constraints between the unmanned aerial vehicles and the environmental barrier and between the heavy object and the environmental barrier.
In the existing obstacle avoidance trajectory planning research, a large number of research results approximate collision avoidance constraints by using the Euclidean distance between the centers of objects to be smaller than the safe distance, the processing method simplifies the two objects into a ball or a cylinder for processing, and although the calculation amount is small, the method is not free from the attention when collision avoidance between non-ball objects and planning of an aggressive motion trajectory are solved. For example, in the invention patent "time-optimal fast three-dimensional obstacle avoidance path planning method" (granted publication number: CN109828600B), the obstacles in the flight space of the unmanned aerial vehicle are described as three-dimensional spheres and cylinders, and collision detection is performed by calculating the distance between the unmanned aerial vehicle and the center of the obstacle, so that the simple obstacle avoidance constraint processing method is too conservative for polyhedral or non-convex obstacles. Similarly, in the invention patent of an energy-saving unmanned aerial vehicle path planning obstacle avoidance method (application publication number: CN109343528A), obstacles in the flight space of the unmanned aerial vehicle are described as three-dimensional balls, and meanwhile, the obstacles are avoided by adopting an artificial potential field method, so that the trajectory planning is easy to fall into a local optimal solution.
Disclosure of Invention
The technical problem to be solved is as follows:
in order to avoid the defects of the prior art, the invention provides an optimized rope system multi-unmanned aerial vehicle cooperative transportation-based obstacle avoidance track planning method, which comprises the steps of establishing a constraint model, deriving constraint conditions, constructing an optimized track plan and finally solving a track plan optimization problem; the method can solve the problem of trajectory planning of a multi-robot cooperative operation system and the problem of planning of real-time obstacle avoidance motion of the robot.
The technical scheme of the invention is as follows: an optimized obstacle avoidance trajectory planning method based on rope system multi-unmanned aerial vehicle cooperative transportation is characterized by comprising the following specific steps:
the method comprises the following steps: establishing an obstacle avoidance constraint model of a rope system coupled multi-unmanned aerial vehicle cooperative operation system;
first, a geometric model set of the weight is established as
Figure BDA0003269619620000021
The set of geometric models of drone i is
Figure BDA0003269619620000022
The set of geometric models of the obstacle j in the environment is
Figure BDA0003269619620000023
Wherein the content of the first and second substances,
Figure BDA0003269619620000024
the state of the weight at the moment k; i ∈ {1,2, …, N }, where N is the number of drones in the operating system,
Figure BDA0003269619620000025
the state of the unmanned aerial vehicle i at the moment k is shown; j belongs to {1,2, …, M }, wherein M is the number of obstacles in the environment;
then, obtaining an obstacle avoidance constraint model through coupling: the collision avoidance constraints between drones are expressed as:
Figure BDA0003269619620000026
wherein the symbol n represents the intersection of the sets,
Figure BDA0003269619620000027
representing an empty set;
the collision avoidance constraint between the drone and the obstacle is expressed as:
Figure BDA0003269619620000028
the collision avoidance constraint between the weight and the drone is expressed as:
Figure BDA0003269619620000029
the collision avoidance constraint between the weight and the obstacle is expressed as:
Figure BDA0003269619620000031
step two: deducing optimization constraint conditions of the rope system coupling multi-unmanned aerial vehicle cooperative operation system;
firstly, transforming the obstacle avoidance constraint model obtained in the step one to obtain the effective distance between the unmanned aerial vehicles as follows:
Figure BDA0003269619620000032
where dist (, x) represents the distance between the two sets;
the effective distance between unmanned aerial vehicle and the barrier does:
Figure BDA0003269619620000033
the effective distance between heavy object and unmanned aerial vehicle does:
Figure BDA0003269619620000034
the effective distance between the weight and the barrier is as follows:
Figure BDA0003269619620000035
then, based on the dual transformation principle, obtaining an optimized constraint condition:
Figure BDA0003269619620000036
Figure BDA0003269619620000037
Figure BDA0003269619620000038
Figure BDA0003269619620000039
wherein λ isij，μij，zijRepresenting dual variables; l |. electrically ventilated margin*Representing a dual norm;
Figure BDA00032696196200000310
to represent
Figure BDA00032696196200000311
The dual cone of (2);
Figure BDA00032696196200000312
to represent
Figure BDA00032696196200000313
The dual cone of (2);
Figure BDA00032696196200000314
and
Figure BDA00032696196200000315
dual variables respectively representing the problem (6);
Figure BDA00032696196200000316
to represent
Figure BDA00032696196200000317
The dual cone of (2); lambda [ alpha ]i0，μi0And zi0Dual variables respectively representing the problem (7);
Figure BDA00032696196200000318
and
Figure BDA00032696196200000319
dual variables respectively representing the questions (8); d1Minimum safe distance, d, representing collision avoidance between drones2Minimum safe distance, d, representing collision avoidance between unmanned aerial vehicle and obstacle3Represents the minimum safe distance, d, of collision avoidance between the heavy object and the unmanned aerial vehicle4Represents the minimum safe distance between the heavy object and the barrier to avoid collision;
step three: constructing a track planning problem of a rope system coupled multi-unmanned aerial vehicle cooperative operation system based on optimization;
and sorting the various constraint conditions obtained by the derivation in the step two to obtain the following optimized-based track planning problem of the system:
Figure BDA0003269619620000041
wherein the content of the first and second substances,
Figure BDA00032696196200000414
representing a phase objective function; n is a radical ofTRepresents the time domain; x (0) and x (N)T) Respectively representing an initial state and a terminal state;
step four: and solving the optimization problem of the trajectory planning.
The further technical scheme of the invention is as follows: in the first step, the geometric model set of the weight is
Figure BDA0003269619620000042
Is composed of
Figure BDA0003269619620000043
Wherein the content of the first and second substances,
Figure BDA0003269619620000044
an orthogonal rotation matrix representing the weight at time k;
Figure BDA0003269619620000045
representing a displacement vector of the weight;
Figure BDA0003269619620000046
a set of geometric models representing an initial moment of the weight; matrix A0And vector b0Form a
Figure BDA0003269619620000047
Determined by the shape of the weight;
Figure BDA0003269619620000048
represents a normal cone defining a generalized inequality, consisting ofThe shape of the obstacle is determined, if the obstacle is polyhedral, then
Figure BDA0003269619620000049
For non-negative image limitation, if the obstacle is ellipsoidal in shape
Figure BDA00032696196200000410
Is a second order cone; y is belonging to the set
Figure BDA00032696196200000411
Any of (1).
The further technical scheme of the invention is as follows: in the step one, the geometric model set of the unmanned aerial vehicle i
Figure BDA00032696196200000412
Is composed of
Figure BDA00032696196200000413
Wherein the content of the first and second substances,
Figure BDA0003269619620000051
the state of the unmanned aerial vehicle i at the moment k is shown;
Figure BDA0003269619620000052
an orthogonal rotation matrix representing unmanned aerial vehicle i at time k;
Figure BDA0003269619620000053
a displacement vector representing drone i;
Figure BDA0003269619620000054
a set of geometric models representing an initial moment of the unmanned aerial vehicle i; matrix AiAnd vector biForm a
Figure BDA0003269619620000055
Determined by the shape of drone i;
Figure BDA0003269619620000056
the normal cone defining the generalized inequality is represented, determined by the shape of the drone.
The further technical scheme of the invention is as follows: in the first step, a geometric model set of an obstacle j in the environment
Figure BDA0003269619620000057
Is composed of
Figure BDA0003269619620000058
Wherein, the matrix GjSum vector gjForm a
Figure BDA0003269619620000059
Determined by the shape of the obstacle j; gamma denotes a set
Figure BDA00032696196200000510
Any of (1);
Figure BDA00032696196200000511
the normal cone defining the generalized inequality is represented, determined by the shape of the obstacle.
The further technical scheme of the invention is as follows: in the second step, in order to realize collision avoidance between the unmanned aerial vehicles, the distance between the requirement sets meets the following relation:
Figure BDA00032696196200000512
wherein d is1Representing the minimum safe distance to avoid collision between drones.
The further technical scheme of the invention is as follows: in the second step, the distance is required for realizing collision avoidance between the unmanned aerial vehicle and the barrier
Figure BDA00032696196200000513
Satisfies the relationship:
Figure BDA00032696196200000514
wherein d is2Representing the minimum safe distance for collision between the drone and the obstacle.
The further technical scheme of the invention is as follows: in the second step, the distance is required for avoiding collision between the heavy object and the unmanned aerial vehicle
Figure BDA00032696196200000515
Satisfies the relationship:
Figure BDA00032696196200000516
wherein d is3The minimum safe distance of collision between the heavy object and the unmanned aerial vehicle is shown.
The further technical scheme of the invention is as follows: in the second step, the distance is required for avoiding collision between the heavy object and the barrier
Figure BDA00032696196200000517
Satisfies the relationship:
Figure BDA00032696196200000518
wherein d is4Indicating the minimum safe distance between the weight and the obstacle to avoid collision.
The further technical scheme of the invention is as follows: in the second step, the tethered multi-unmanned aerial vehicle system also has the problems of limited state, limited actuators, limited tethered connections and constraint of a dynamic equation;
drone i state and actuator constraints are expressed as follows:
Figure BDA0003269619620000061
wherein the content of the first and second substances,
Figure BDA0003269619620000062
control input of the unmanned aerial vehicle i at the moment k;
tether connection constraints are expressed as follows:
Figure BDA0003269619620000063
wherein the content of the first and second substances,
Figure BDA0003269619620000064
installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; li0Connecting the tensioning length of a tether for the unmanned aerial vehicle i and the weight; l |. electrically ventilated margin2The Euclid norm representing the vector;
the system discrete kinetic equation is:
xk+1＝f(xk,uk) (23)
wherein the content of the first and second substances,
Figure BDA0003269619620000065
is a compact vector composed of the weight and the unmanned aerial vehicle state at the moment k;
Figure BDA0003269619620000066
is a compact vector formed by unmanned aerial vehicle control input at the moment k; x is the number ofk+1Is the state of the system at time k + 1.
The further technical scheme of the invention is as follows: in the fourth step, an initial track is searched by adopting an A-method and used as an initial guess for solving the optimization problem in the third step; and (3) solving the optimization problem in the third step to obtain a reference track of the system by adopting a heuristic method, wherein the optimization problem is a non-convex optimization problem.
Advantageous effects
The invention has the beneficial effects that: the invention provides an optimized rope system constraint multi-agent system-based obstacle avoidance trajectory planning method, which is a method for explicitly representing the obstacle avoidance constraint between objects based on collective distance, simultaneously adopts strong dual property in convex optimization to equivalently miniaturize and smooth the obstacle avoidance constraint, so that the proposed obstacle avoidance trajectory planning problem can be solved by adopting a traditional optimization method based on gradient and black plug matrix, the calculation complexity can be obviously reduced, the algorithm real-time performance is improved, the method is suitable for the obstacle avoidance problem of convex and non-convex obstacles formed by polyhedrons, and the application range is wide.
1) The method can be used for solving the problem of trajectory planning of the multi-robot cooperative operation system;
2) the method can be used for solving the problem of planning the real-time obstacle avoidance movement of the robot.
Referring to simulation results of fig. 1-3, the obstacle avoidance trajectory planning method of the rope system multi-unmanned aerial vehicle cooperative transportation system provided by the invention has a good effect, realizes obstacle avoidance of the unmanned aerial vehicles and the heavy objects, collision avoidance between the unmanned aerial vehicles, collision avoidance of the unmanned aerial vehicles and the heavy objects, and satisfies constraint conditions of dynamic performance of the unmanned aerial vehicles. Meanwhile, the invention relates to a patent of a first application on an obstacle avoidance track planning method of a multi-unmanned aerial vehicle cooperative transportation system, and fills the gap of technical research in the aspect.
Drawings
Fig. 1 illustrates obstacle avoidance flight trajectories of three unmanned aerial vehicle tether suspension type cooperative transportation systems;
FIG. 2 is a schematic view of the three unmanned aerial vehicle tether suspended type cooperative handling system passing through a window at the instant;
fig. 3 illustrates an obstacle avoidance trajectory curve for unmanned aerial vehicle and weight planning.
Detailed Description
The embodiments described below with reference to the drawings are illustrative and intended to be illustrative of the invention and are not to be construed as limiting the invention.
The invention proposes the following execution steps:
step 1, establishing an obstacle avoidance constraint model of a rope system coupled multi-unmanned aerial vehicle cooperative operation system;
step 2, deducing optimization constraint conditions of the rope system coupling multi-unmanned aerial vehicle cooperative operation system;
step 3, constructing a track planning problem of a rope system coupling multi-unmanned aerial vehicle cooperative operation system based on optimization;
and 4, solving the optimization problem of the trajectory planning.
1. Obstacle avoidance constraint model for establishing rope system coupling multi-unmanned aerial vehicle cooperative operation system
The geometric model of the weight is formed into a set in the motion space as
Figure BDA0003269619620000071
Wherein
Figure BDA0003269619620000072
The state of the weight at time k. The set of geometric models of drone i is
Figure BDA0003269619620000073
Wherein i ∈ {1,2, …, N }, N is the number of drones in the operating system,
Figure BDA0003269619620000074
the state of the unmanned plane i at the moment k. The set of geometrical models of the obstacle j in the environment is
Figure BDA0003269619620000075
Where j ∈ {1,2, …, M }, where M is the number of obstacles in the environment. Considering the requirements of the optimization method based on the gradient and the black plug matrix on the continuity and the differentiability of the optimization problem, the geometric model set of the weight is expressed as follows:
Figure BDA0003269619620000076
wherein the content of the first and second substances,
Figure BDA0003269619620000077
an orthogonal rotation matrix representing the weight at time k;
Figure BDA0003269619620000078
representing a displacement vector of the weight;
Figure BDA0003269619620000079
a set of geometric models representing an initial moment of the weight; matrix A0And vector b0Form a
Figure BDA0003269619620000081
Determined by the shape of the weight;
Figure BDA0003269619620000082
representing a normal cone defining a generalized inequality, determined by the shape of the obstacle, if the obstacle is polyhedral in shape
Figure BDA0003269619620000083
For non-negative image limitation, if the obstacle is ellipsoidal in shape
Figure BDA0003269619620000084
Is a second order cone; y is belonging to the set
Figure BDA0003269619620000085
Any of (1). The set of geometric models for drone i is represented as:
Figure BDA0003269619620000086
wherein the content of the first and second substances,
Figure BDA0003269619620000087
an orthogonal rotation matrix representing unmanned aerial vehicle i at time k;
Figure BDA0003269619620000088
a displacement vector representing drone i;
Figure BDA0003269619620000089
a set of geometric models representing an initial moment of the unmanned aerial vehicle i; matrix AiAnd vector biForm a
Figure BDA00032696196200000810
Determined by the shape of drone i;
Figure BDA00032696196200000811
the normal cone defining the generalized inequality is represented, determined by the shape of the drone. The set of geometric models for an obstacle j in the environment is represented as:
Figure BDA00032696196200000812
wherein, the matrix GjSum vector gjForm a
Figure BDA00032696196200000813
Determined by the shape of the obstacle j; gamma denotes a set
Figure BDA00032696196200000814
Any of (1);
Figure BDA00032696196200000815
the normal cone defining the generalized inequality is represented, determined by the shape of the obstacle.
In the many unmanned aerial vehicle of tether coupling cooperative operation system, there is the collision risk between the unmanned aerial vehicle, between unmanned aerial vehicle and the barrier, between heavy object and the unmanned aerial vehicle and between heavy object and the barrier. The collision avoidance constraints between drones are expressed as:
Figure BDA00032696196200000816
wherein the symbol n represents the intersection of the sets,
Figure BDA00032696196200000817
indicating an empty set. The collision avoidance constraint between the drone and the obstacle is expressed as:
Figure BDA00032696196200000818
the collision avoidance constraint between the weight and the drone is expressed as:
Figure BDA00032696196200000819
the collision avoidance constraint between the weight and the obstacle is expressed as:
Figure BDA00032696196200000820
2. deducing optimal constraint conditions of rope system coupling multi-unmanned aerial vehicle cooperative operation system
The collision avoidance constraint conditions in step 1 are usually non-convex and non-differentiable, which will bring great troubles to the optimization problem solving method based on the gradient and the black plug matrix. Therefore, in step 2, we first need to re-equivalently express the collision avoidance constraint in step 1 by using a new mathematical expression method. The effective distance between the unmanned aerial vehicles is:
Figure BDA0003269619620000091
where dist (, x) denotes the distance between the two sets. For realizing collision avoidance between unmanned aerial vehicles, the distance between the sets is required to satisfy the following relation:
Figure BDA0003269619620000092
wherein d is1Representing the minimum safe distance to avoid collision between drones. The effective distance between unmanned aerial vehicle and the barrier does:
Figure BDA0003269619620000093
for realizing collision avoidance between unmanned aerial vehicle and barrier, required distance
Figure BDA0003269619620000094
Satisfies the relationship:
Figure BDA0003269619620000095
wherein d is2Representing the minimum safe distance for collision between the drone and the obstacle. The effective distance between heavy object and unmanned aerial vehicle does:
Figure BDA0003269619620000096
for realizing collision prevention between heavy objects and unmanned aerial vehicle, the required distance
Figure BDA0003269619620000097
Satisfies the relationship:
Figure BDA0003269619620000098
wherein d is3The minimum safe distance of collision between the heavy object and the unmanned aerial vehicle is shown. The effective distance between the weight and the barrier is as follows:
Figure BDA0003269619620000099
the distance is required for avoiding collision between heavy objects and barriers
Figure BDA00032696196200000910
Satisfies the relationship:
Figure BDA00032696196200000911
wherein d is4Indicating the minimum safe distance between the weight and the obstacle to avoid collision.
The equivalent of the optimization problem (31) is:
Figure BDA0003269619620000101
corresponding dual function g (lambda)ij,μij,zij) Comprises the following steps:
Figure BDA0003269619620000102
wherein λ isij，μij，zijRepresenting dual variables; l |. electrically ventilated margin*Representing a dual norm. Therefore, the dual problem of the optimization problem (39) is:
Figure BDA0003269619620000103
wherein the content of the first and second substances,
Figure BDA0003269619620000104
to represent
Figure BDA0003269619620000105
The dual cone of (2). Likewise, we present the dual problem of the optimization problem (33) as:
Figure BDA0003269619620000106
wherein the content of the first and second substances,
Figure BDA0003269619620000107
to represent
Figure BDA0003269619620000108
The dual cone of (2);
Figure BDA0003269619620000109
and
Figure BDA00032696196200001010
respectively, the dual variables of the problem (33). The dual problem of the optimization problem (35) is:
Figure BDA0003269619620000111
wherein the content of the first and second substances,
Figure BDA0003269619620000112
to represent
Figure BDA0003269619620000113
The dual cone of (2); lambda [ alpha ]i0，μi0And zi0Respectively, the dual variables of the problem (35). The dual problem of the optimization problem (37) is:
Figure BDA0003269619620000114
wherein the content of the first and second substances,
Figure BDA0003269619620000115
and
Figure BDA0003269619620000116
respectively, represent dual variables of the problem (37). From problem (39), we know that this problem is a convex one, and
Figure BDA0003269619620000117
with non-empty relative interior points. Therefore, if the Slater condition of the problem (39) is satisfied, the dual problem (41) satisfies strong duality. Likewise, the optimization problems (42), (43), (44) all satisfy strong duality. Then, the collision avoidance constraints (32), (34), (36), (38) are equivalent to the following constraints:
Figure BDA0003269619620000118
Figure BDA0003269619620000119
Figure BDA00032696196200001110
Figure BDA00032696196200001111
besides the collision avoidance constraints discussed above, the tethered multi-drone system also has the constraint problems of state limitation, actuator limitation, tethered connection limitation, kinetic equations and the like. Drone i state and actuator constraints are expressed as follows:
Figure BDA0003269619620000121
wherein the content of the first and second substances,
Figure BDA0003269619620000122
and (4) controlling input of the unmanned aerial vehicle i at the moment k. Tether connection constraints are expressed as follows:
Figure BDA0003269619620000123
wherein the content of the first and second substances,
Figure BDA0003269619620000124
installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; li0Connecting the tensioning length of a tether for the unmanned aerial vehicle i and the weight; l |. electrically ventilated margin2Representing the euclidd norm of the vector. The system discrete kinetic equation is:
xk+1＝f(xk,uk) (51)
wherein the content of the first and second substances,
Figure BDA0003269619620000125
is a compact vector composed of the weight and the unmanned aerial vehicle state at the moment k;
Figure BDA0003269619620000126
is a compact vector formed by unmanned aerial vehicle control input at the moment k; x is the number ofk+1Is the state of the system at time k + 1.
3. Optimization-based trajectory planning problem for constructing rope system coupled multi-unmanned aerial vehicle cooperative operation system
Step 2, deducing various constraint conditions existing in the system, and sorting to obtain the following optimized trajectory planning problem of the system:
Figure BDA0003269619620000127
wherein the content of the first and second substances,
Figure BDA0003269619620000128
representing a phase objective function; n is a radical ofTRepresents the time domain; x (0) and x (N)T) Respectively representing an initial state and a terminal state.
4. Solving a trajectory planning optimization problem
Although the track optimization problem obtained in the step 3 has a plurality of variables and a plurality of constraint conditions, most of the constraint conditions are in the form of linear equations and inequalities, the constraint conditions are relatively easy to process, and all the constraint conditions are continuous and differentiable. Initial guessing is an important step in order to get the optimal solution to the optimization problem faster. The invention adopts an A method to search an initial track as an initial guess for solving the optimization problem in the step 3. The optimization problem in step 3 is a non-convex optimization problem, and a heuristic method can be adopted to solve the optimization problem to obtain a reference track of the system.
Example (b):
by analyzing the problem of planning the obstacle avoidance track of the tether suspended type cooperative transportation system consisting of the three rotor unmanned aerial vehicles through simulation, the advancement and superiority of the method in the aspect of processing the problem of planning the obstacle avoidance track of multi-robot cooperative operation are verified. The system parameters in the simulation case are as follows, the mass of the unmanned aerial vehicle is 1.121kg, and the rotational inertia of the unmanned aerial vehicle is [ 0.0100; 00.00820, respectively; 000.0148]kgm2Radius of unmanned plane is 0.2m, distance between shafts of unmanned plane motors in rolling direction is 0.2136m, and pitching angle isThe inter-shaft distance between the motors of the directional unmanned aerial vehicle is 0.1758m, the thrust-torque constant of the motor is 81.0363N/Nm, the motor force drift is-0.2046N, and the motor force constant is 2.0784 multiplied by 10-8N/RPM2Angular velocity to force offset 1004.5RPM, voltage to angular velocity offset 2132.6RPM, effective motor speed constant 1295.4RPM/V, maximum voltage per motor 12.6V, weight mass 0.3kg, weight radius 0.1m, length of each tether 1m, initial position of weight [ 3.5; 5; 0.634]m, end position of weight [ 9; 3.5; 1.134]And m is selected. The barrier is a wall with a window, and it is desirable that the unmanned aerial vehicle coordinated handling system moves through the window from one side of the wall to the other. The system motion space is [2,10 ]]m×[0,10]m×[0,5]m, the wall coverage is [7,7.5 ]]m×[0,10]m×[0,5]m, the window coverage on the wall is [7,7.5 ]]m×[4.5,5.5]m×[2,3]m, is a window of 1m × 1m size.
Fig. 1 shows a three-dimensional effect diagram of obstacle avoidance flight trajectories of three unmanned aerial vehicle tether suspended type cooperative transportation systems, fig. 2 shows the moment when three unmanned aerial vehicle tether suspended type cooperative transportation systems pass through a window, and fig. 3 depicts components of obstacle avoidance trajectory curves planned by unmanned aerial vehicles and heavy objects in the x, y and z directions. From the simulation results, the obstacle avoidance trajectory planning method of the rope system multi-unmanned aerial vehicle cooperative transportation system has a good effect, realizes obstacle avoidance of the unmanned aerial vehicles and the heavy objects, collision avoidance between the unmanned aerial vehicles, collision avoidance of the unmanned aerial vehicles and the heavy objects, and meets the constraint conditions of the dynamic performance of the unmanned aerial vehicles. Meanwhile, the invention relates to a patent of a first application on an obstacle avoidance track planning method of a multi-unmanned aerial vehicle cooperative transportation system, and fills the gap of technical research in the aspect.
Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made in the above embodiments by those of ordinary skill in the art without departing from the principle and spirit of the present invention.

Claims (10)

1. An optimized obstacle avoidance trajectory planning method based on rope system multi-unmanned aerial vehicle cooperative transportation is characterized by comprising the following specific steps:
the method comprises the following steps: establishing an obstacle avoidance constraint model of a rope system coupled multi-unmanned aerial vehicle cooperative operation system;
first, a geometric model set of the weight is established as
Figure FDA0003269619610000011
The set of geometric models of drone i is
Figure FDA0003269619610000012
The set of geometric models of the obstacle j in the environment is
Figure FDA0003269619610000013
Wherein the content of the first and second substances,
Figure FDA0003269619610000014
the state of the weight at the moment k; i ∈ {1,2, …, N }, where N is the number of drones in the operating system,
Figure FDA0003269619610000015
the state of the unmanned aerial vehicle i at the moment k is shown; j belongs to {1,2, …, M }, wherein M is the number of obstacles in the environment;
then, obtaining an obstacle avoidance constraint model through coupling: the collision avoidance constraints between drones are expressed as:
Figure FDA0003269619610000016
wherein the symbol n represents the intersection of the sets,
Figure FDA0003269619610000017
representing an empty set;
the collision avoidance constraint between the drone and the obstacle is expressed as:
Figure FDA0003269619610000018
the collision avoidance constraint between the weight and the drone is expressed as:
Figure FDA0003269619610000019
the collision avoidance constraint between the weight and the obstacle is expressed as:
Figure FDA00032696196100000110
step two: deducing optimization constraint conditions of the rope system coupling multi-unmanned aerial vehicle cooperative operation system;
firstly, transforming the obstacle avoidance constraint model obtained in the step one to obtain the effective distance between the unmanned aerial vehicles as follows:
Figure FDA00032696196100000111
where dist (, x) represents the distance between the two sets;
the effective distance between unmanned aerial vehicle and the barrier does:
Figure FDA00032696196100000112
the effective distance between heavy object and unmanned aerial vehicle does:
Figure FDA00032696196100000113
the effective distance between the weight and the barrier is as follows:
Figure FDA0003269619610000021
then, based on the dual transformation principle, obtaining an optimized constraint condition:
Figure FDA0003269619610000022
Figure FDA0003269619610000023
Figure FDA0003269619610000024
Figure FDA0003269619610000025
wherein λ isij，μij，zijRepresenting dual variables; l |. electrically ventilated margin*Representing a dual norm;
Figure FDA0003269619610000026
to represent
Figure FDA0003269619610000027
The dual cone of (2);
Figure FDA0003269619610000028
to represent
Figure FDA0003269619610000029
The dual cone of (2);
Figure FDA00032696196100000210
and
Figure FDA00032696196100000211
dual variables respectively representing the problem (6);
Figure FDA00032696196100000212
to represent
Figure FDA00032696196100000213
The dual cone of (2); lambda [ alpha ]i0，μi0And zi0Dual variables respectively representing the problem (7);
Figure FDA00032696196100000214
and
Figure FDA00032696196100000215
dual variables respectively representing the questions (8); d1Minimum safe distance, d, representing collision avoidance between drones2Minimum safe distance, d, representing collision avoidance between unmanned aerial vehicle and obstacle3Represents the minimum safe distance, d, of collision avoidance between the heavy object and the unmanned aerial vehicle4Represents the minimum safe distance between the heavy object and the barrier to avoid collision;
step three: constructing a track planning problem of a rope system coupled multi-unmanned aerial vehicle cooperative operation system based on optimization;
and sorting the various constraint conditions obtained by the derivation in the step two to obtain the following optimized-based track planning problem of the system:
Figure FDA0003269619610000031
s.t.x0＝x(0)，
Figure FDA0003269619610000032
xk+1＝f(xk,uk)，
Figure FDA0003269619610000033
Figure FDA0003269619610000034
Figure FDA0003269619610000035
Figure FDA0003269619610000036
Figure FDA0003269619610000037
Figure FDA0003269619610000038
for i,j＝1,…,N，i＜j，m＝1,…,M (13)
wherein, l (x)k,uk) Representing a phase objective function; n is a radical ofTRepresents the time domain; x (0) and x (N)T) Respectively representing an initial state and a terminal state;
step four: and solving the optimization problem of the trajectory planning.
2. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the first step, the geometric model set of the weight is
Figure FDA0003269619610000039
Is composed of
Figure FDA00032696196100000310
Wherein the content of the first and second substances,
Figure FDA00032696196100000311
means of weightAn orthogonal rotation matrix of the object at time k;
Figure FDA00032696196100000312
representing a displacement vector of the weight;
Figure FDA00032696196100000313
a set of geometric models representing an initial moment of the weight; matrix A0And vector b0Form a
Figure FDA00032696196100000314
Determined by the shape of the weight;
Figure FDA00032696196100000315
representing a normal cone defining a generalized inequality, determined by the shape of the obstacle, if the obstacle is polyhedral in shape
Figure FDA00032696196100000316
For non-negative image limitation, if the obstacle is ellipsoidal in shape
Figure FDA00032696196100000317
Is a second order cone; y is belonging to the set
Figure FDA00032696196100000318
Any of (1).
3. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the step one, the geometric model set of the unmanned aerial vehicle i
Figure FDA00032696196100000319
Is composed of
Figure FDA00032696196100000320
Wherein the content of the first and second substances,
Figure FDA00032696196100000321
the state of the unmanned aerial vehicle i at the moment k is shown;
Figure FDA00032696196100000322
an orthogonal rotation matrix representing unmanned aerial vehicle i at time k;
Figure FDA00032696196100000323
a displacement vector representing drone i;
Figure FDA00032696196100000324
a set of geometric models representing an initial moment of the unmanned aerial vehicle i; matrix AiAnd vector biForm a
Figure FDA00032696196100000325
Determined by the shape of drone i;
Figure FDA00032696196100000326
the normal cone defining the generalized inequality is represented, determined by the shape of the drone.
4. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the first step, a geometric model set of an obstacle j in the environment
Figure FDA0003269619610000041
Is composed of
Figure FDA0003269619610000042
Wherein, the matrix GjSum vector gjForm a
Figure FDA0003269619610000043
Determined by the shape of the obstacle j; gamma denotes a set
Figure FDA0003269619610000044
Any of (1);
Figure FDA0003269619610000045
the normal cone defining the generalized inequality is represented, determined by the shape of the obstacle.
5. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the second step, in order to realize collision avoidance between the unmanned aerial vehicles, the distance between the requirement sets meets the following relation:
Figure FDA0003269619610000046
wherein d is1Representing the minimum safe distance to avoid collision between drones.
6. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the second step, the distance is required for realizing collision avoidance between the unmanned aerial vehicle and the barrier
Figure FDA0003269619610000047
Satisfies the relationship:
Figure FDA0003269619610000048
wherein d is2Representing the minimum safe distance for collision between the drone and the obstacle.
7. The obstacle avoidance trajectory planning method based on optimized tethered multi-drone coordinated handling of claim 1, wherein the method is applied to the planning of obstacle avoidance trajectoriesIs characterized in that: in the second step, the distance is required for avoiding collision between the heavy object and the unmanned aerial vehicle
Figure FDA0003269619610000049
Satisfies the relationship:
Figure FDA00032696196100000410
wherein d is3The minimum safe distance of collision between the heavy object and the unmanned aerial vehicle is shown.
8. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the second step, the distance is required for avoiding collision between the heavy object and the barrier
Figure FDA00032696196100000411
Satisfies the relationship:
Figure FDA00032696196100000412
wherein d is4Indicating the minimum safe distance between the weight and the obstacle to avoid collision.
9. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the second step, the tethered multi-unmanned aerial vehicle system also has the problems of limited state, limited actuators, limited tethered connections and constraint of a dynamic equation;
drone i state and actuator constraints are expressed as follows:
Figure FDA0003269619610000051
wherein the content of the first and second substances,
Figure FDA0003269619610000052
control input of the unmanned aerial vehicle i at the moment k;
tether connection constraints are expressed as follows:
Figure FDA0003269619610000053
wherein the content of the first and second substances,
Figure FDA0003269619610000054
installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; li0Connecting the tensioning length of a tether for the unmanned aerial vehicle i and the weight; l |. electrically ventilated margin2The Euclid norm representing the vector;
the system discrete kinetic equation is:
xk+1＝f(xk,uk) (23)
wherein the content of the first and second substances,
Figure FDA0003269619610000055
is a compact vector composed of the weight and the unmanned aerial vehicle state at the moment k;
Figure FDA0003269619610000056
is a compact vector formed by unmanned aerial vehicle control input at the moment k; x is the number ofk+1Is the state of the system at time k + 1.
10. The obstacle avoidance trajectory planning method based on the optimized tether multi-unmanned aerial vehicle cooperative transportation is characterized in that: in the fourth step, an initial track is searched by adopting an A-method and used as an initial guess for solving the optimization problem in the third step; and (3) solving the optimization problem in the third step to obtain a reference track of the system by adopting a heuristic method, wherein the optimization problem is a non-convex optimization problem.
CN202111103700.XA 2021-09-18 2021-09-18 Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles Active CN113759977B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111103700.XA CN113759977B (en) 2021-09-18 2021-09-18 Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111103700.XA CN113759977B (en) 2021-09-18 2021-09-18 Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles

Publications (2)

Publication Number Publication Date
CN113759977A true CN113759977A (en) 2021-12-07
CN113759977B CN113759977B (en) 2023-07-28

Family

ID=78796567

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111103700.XA Active CN113759977B (en) 2021-09-18 2021-09-18 Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles

Country Status (1)

Country Link
CN (1) CN113759977B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114089780A (en) * 2022-01-21 2022-02-25 中国电子科技集团公司第二十八研究所 Urban space-oriented multi-rotor unmanned aerial vehicle path planning method
CN117466161A (en) * 2023-09-08 2024-01-30 兰州交通大学 Obstacle avoidance track planning method for multi-machine suspension system
CN117798918A (en) * 2024-01-11 2024-04-02 兰州交通大学 Space obstacle avoidance planning method and system for multi-machine suspension system based on collaborative optimization
CN117798918B (en) * 2024-01-11 2024-05-14 兰州交通大学 Space obstacle avoidance planning method and system for multi-machine suspension system based on collaborative optimization

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018103242A1 (en) * 2016-12-09 2018-06-14 武汉科技大学 Electric tower inspection method of using four-rotor unmanned aerial vehicle based on motor learning
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112923925A (en) * 2021-01-07 2021-06-08 天津大学 Dual-mode multi-unmanned aerial vehicle collaborative track planning method for hovering and tracking ground target
CN113156966A (en) * 2021-04-30 2021-07-23 湖南大学 Multi-incomplete robot vehicle collaborative trajectory planning method, device and medium based on adaptive scale constraint optimization

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018103242A1 (en) * 2016-12-09 2018-06-14 武汉科技大学 Electric tower inspection method of using four-rotor unmanned aerial vehicle based on motor learning
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112923925A (en) * 2021-01-07 2021-06-08 天津大学 Dual-mode multi-unmanned aerial vehicle collaborative track planning method for hovering and tracking ground target
CN113156966A (en) * 2021-04-30 2021-07-23 湖南大学 Multi-incomplete robot vehicle collaborative trajectory planning method, device and medium based on adaptive scale constraint optimization

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
马宇豪;梁雁冰;: "一种基于六次多项式轨迹规划的机械臂避障算法", 西北工业大学学报, no. 02 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114089780A (en) * 2022-01-21 2022-02-25 中国电子科技集团公司第二十八研究所 Urban space-oriented multi-rotor unmanned aerial vehicle path planning method
CN114089780B (en) * 2022-01-21 2022-04-12 中国电子科技集团公司第二十八研究所 Urban space-oriented multi-rotor unmanned aerial vehicle path planning method
CN117466161A (en) * 2023-09-08 2024-01-30 兰州交通大学 Obstacle avoidance track planning method for multi-machine suspension system
CN117798918A (en) * 2024-01-11 2024-04-02 兰州交通大学 Space obstacle avoidance planning method and system for multi-machine suspension system based on collaborative optimization
CN117798918B (en) * 2024-01-11 2024-05-14 兰州交通大学 Space obstacle avoidance planning method and system for multi-machine suspension system based on collaborative optimization

Also Published As

Publication number Publication date
CN113759977B (en) 2023-07-28

Similar Documents

Publication Publication Date Title
Ollero et al. Past, present, and future of aerial robotic manipulators
CN109625333B (en) Spatial non-cooperative target capturing method based on deep reinforcement learning
Tan et al. Three dimensional collision avoidance for multi unmanned aerial vehicles using velocity obstacle
CN113759977A (en) Obstacle avoidance trajectory planning method based on optimized tether multi-unmanned aerial vehicle cooperative transportation
Aghdam et al. Cooperative load transport with movable load center of mass using multiple quadrotor UAVs
CN111027143B (en) Shipboard aircraft approach guiding method based on deep reinforcement learning
Zhang et al. An obstacle avoidance algorithm for robot manipulators based on decision-making force
Tran et al. Switching formation strategy with the directed dynamic topology for collision avoidance of a multi‐robot system in uncertain environments
Gómez-Avila et al. Control of Quadrotor using a Neural Network based PID
Lin et al. Experiments on human-in-the-loop coordination for multirobot system with task abstraction
CN111309002A (en) Wheel type mobile robot obstacle avoidance method and system based on vector
Gkountas et al. Leader/follower force control of aerial manipulators
Watanabe et al. Image-based visual PID control of a micro helicopter using a stationary camera
Rao et al. Path planning for dual UAVs cooperative suspension transport based on artificial potential field-A* algorithm
Liu et al. Dynamic obstacle avoidance for cable-driven parallel robots with mobile bases via sim-to-real reinforcement learning
Zhang et al. Coordinated transport by multiple biomimetic robotic fish in underwater environment
Omar et al. Robust active force control of a quadcopter
CN116714780A (en) Rotor flying mechanical arm and planning and control method for rapid aerial grabbing
Guney et al. Formation control of a group of micro aerial vehicles (MAVs)
Bisig et al. Genetic fuzzy systems for decentralized, multi-uav cargo handling
Iswanto et al. Path planning of decentralized multi-quadrotor based on fuzzy-cell decomposition algorithm
Zhuang et al. Dynamic Path Planning of USV Based on Improved Artificial Potential Field Method in Harsh Environment
Patel et al. Artificial neural network-assisted controller for fast and agile UAV flight: Onboard implementation and experimental results
Li et al. Designing, modeling, and control allocation strategy of a novel omnidirectional aerial robot
CN114518709B (en) Method, equipment and medium for resolving frame angle instruction of full-attitude four-axis turntable

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