CN113759977B - Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles - Google Patents
Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles Download PDFInfo
- Publication number
- CN113759977B CN113759977B CN202111103700.XA CN202111103700A CN113759977B CN 113759977 B CN113759977 B CN 113759977B CN 202111103700 A CN202111103700 A CN 202111103700A CN 113759977 B CN113759977 B CN 113759977B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- obstacle
- representing
- constraint
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000005457 optimization Methods 0.000 claims abstract description 39
- 230000009977 dual effect Effects 0.000 claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims abstract description 20
- 230000008878 coupling Effects 0.000 claims description 14
- 238000010168 coupling process Methods 0.000 claims description 14
- 238000005859 coupling reaction Methods 0.000 claims description 14
- 230000002265 prevention Effects 0.000 claims description 13
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 8
- 206010048669 Terminal state Diseases 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract 1
- 239000000725 suspension Substances 0.000 description 6
- 230000000694 effects Effects 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000053208 Porcellio laevis Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003795 chemical substances by application Substances 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
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
-
- 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
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 an obstacle avoidance track planning method based on optimized coordinated carrying of multiple tethered unmanned aerial vehicles, and belongs to the field of coordinated control of rigid-flexible coupled multiple robots; establishing a constraint model, deducing constraint conditions, constructing an optimized track plan, and finally solving; the method can solve the track planning problem of the multi-robot cooperative operation system and the real-time obstacle avoidance motion planning problem of the robots. The method for explicitly representing the collision avoidance constraint between objects based on the set distance simultaneously adopts the strong dual property in convex optimization, and the equivalent of the collision avoidance constraint can be miniaturized and smoothed, so that the proposed collision avoidance track planning problem can be solved by adopting the traditional optimization method based on the gradient and the black plug matrix, the calculation complexity can be remarkably reduced, the algorithm instantaneity can be improved, and the method is simultaneously suitable for the problem of obstacle avoidance of convex and non-convex obstacles formed by polyhedrons and has wide application range. The method can solve the track planning problem of a multi-robot cooperative operation system and the real-time obstacle avoidance motion planning problem of robots.
Description
Technical Field
The invention belongs to the field of cooperative control of rigid-flexible coupled multiple robots, and particularly relates to an obstacle avoidance track planning method based on cooperative transportation of optimized tethered multiple unmanned aerial vehicles.
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. With the complexity and diversity of operation tasks, the extreme and uncertainty of the operation environment and the redundancy brought by the safety operation requirements, the single robot system is difficult or even impossible to meet the requirements, and the co-operation of multiple robots is an irreplaceable choice. Different from the single-robot track planning problem, the multi-robot cooperative operation system has the advantages of high dimensionality, multiple optimization variables, complex constraint, large calculated amount and poor timeliness by adopting the single-robot track planning method, and the aim of online autonomous obstacle avoidance track planning is difficult to realize. Therefore, how to implement real-time trajectory planning of a multi-robot system is a urgent problem to be solved.
Along with the upsizing, heavy weight and precision of the carrying materials, urgent demands for a platform capable of realizing oversized overweight load carrying and stable posture are generated. The tether suspension type multi-unmanned aerial vehicle cooperative operation system serves as a platform for effectively solving the problems, and is a research hot spot due to low cost and control flexibility. The system is a complex rigid-flexible coupling system with high nonlinearity and under actuation. The tether connection brings kinematic constraint to the unmanned aerial vehicle, so that the problem of system track planning is non-convex; meanwhile, the system has high degree of freedom and complex constraint conditions, so that the real-time obstacle avoidance track planning of the tethered multi-unmanned aerial vehicle system is a very challenging problem. Obstacle avoidance trajectory planning of a rope-based multi-unmanned-aerial-vehicle cooperative operation system needs to consider the kinematics and dynamics constraint of unmanned aerial vehicles, the kinematics constraint among unmanned aerial vehicles caused by rope connection, the collision avoidance constraint among unmanned aerial vehicles and between unmanned aerial vehicles and heavy objects, and the collision avoidance constraint among unmanned aerial vehicles, environmental barriers and heavy objects and environmental barriers.
In the existing obstacle avoidance trajectory planning research, a great number of research results approximate the obstacle avoidance constraint by using the fact that the Euclidean distance between the centers of objects is smaller than the safety distance, and the processing method simplifies two objects into balls or cylinders for processing, so that the method has no need of being worry about solving the obstacle avoidance between non-ball objects and the planning of the aggressive motion trajectory although the calculated amount is small. For example, in the invention patent 'time-optimal rapid three-dimensional obstacle avoidance path planning method' (authorized publication number: CN 109828600B), an obstacle in the flight space of an unmanned aerial vehicle is described as a three-dimensional sphere and a cylinder, collision detection is performed by calculating the center distance between the unmanned aerial vehicle and the obstacle, and the simple obstacle avoidance constraint processing method is too conservative for polyhedral or non-convex obstacles. Similarly, in the invention patent 'an energy-saving unmanned aerial vehicle path planning obstacle avoidance method' (application publication number: CN 109343528A), an obstacle in the flight space of an unmanned aerial vehicle is described as a three-dimensional ball, and meanwhile, the obstacle avoidance method by adopting an artificial potential field method is adopted, so that the track planning is easy to fall into a local optimal solution.
Disclosure of Invention
The technical problems to be solved are as follows:
in order to avoid the defects of the prior art, the invention provides an obstacle avoidance track planning method based on the collaborative transportation of an optimized tethered multi-unmanned aerial vehicle, which is characterized in that a constraint model is established, constraint conditions are deduced, an optimized track plan is constructed, and finally, the track plan optimization problem is solved; the method can solve the track planning problem of the multi-robot cooperative operation system and the real-time obstacle avoidance motion planning problem of the robots.
The technical scheme of the invention is as follows: an obstacle avoidance track planning method based on optimized rope multi-unmanned aerial vehicle cooperative transportation is characterized by comprising the following specific steps:
step one: establishing an obstacle avoidance constraint model of a rope coupling multi-unmanned aerial vehicle cooperative operation system;
first, a geometric model set of weights is established asThe geometrical model set of unmanned plane i is +.>The set of geometrical models of obstacle j in the environment is +.>
Wherein,,the state of the weight at the moment k; i epsilon {1,2, …, N }, N being the number of unmanned aerial vehicles in the operating system, { I }>The state of the unmanned aerial vehicle i at the moment k; j is {1,2, …, M }, M being the number of obstacles in the environment;
then, a barrier avoidance constraint model is obtained through coupling: the collision prevention constraint between unmanned aerial vehicles is expressed as:
wherein the symbol ∈ represents the intersection of the sets,representing an empty set;
the collision prevention constraint between the unmanned aerial vehicle and the obstacle is expressed as:
the collision prevention constraint between the weight and the unmanned aerial vehicle is expressed as:
the collision prevention constraint between the weight and the obstacle is expressed as:
step two: the pushing rope is coupled with the optimization constraint condition of the multi-unmanned aerial vehicle cooperative operation system;
firstly, converting the obstacle avoidance constraint model obtained in the first step to obtain the effective distance between unmanned aerial vehicles, wherein the effective distance is as follows:
wherein dist represents the distance between the two sets;
the effective distance between unmanned aerial vehicle and the barrier is:
the effective distance between the weight and the unmanned aerial vehicle is:
the effective distance between the weight and the obstacle is as follows:
then, based on the dual transformation principle, the optimized constraint conditions are obtained:
wherein lambda is ij ，μ ij ，z ij Representing the dual variable; | x I * Representing the dual norms;representation->Is a dual cone of (2); />Representation->Is a dual cone of (2); />And->The dual variables respectively representing the problem (6); />Representation->Is a dual cone of (2); lambda (lambda) i0 ，μ i0 And z i0 The dual variables respectively representing the problem (7); />And->Dual variables respectively representing the problem (8); d, d 1 Represents the minimum safety distance of collision avoidance between unmanned aerial vehicles, d 2 Represents the minimum safety distance of collision between the unmanned aerial vehicle and the obstacle, d 3 Representing the minimum safety distance of collision between a heavy object and an unmanned aerial vehicle, d 4 Representing the minimum safety distance between the heavy object and the obstacle;
step three: constructing a rope coupling multi-unmanned aerial vehicle cooperative operation system based on an optimized track planning problem;
and (3) sorting the various constraint conditions obtained in the second step to obtain the track planning problem of the system based on optimization as follows:
wherein,,representing a phase objective function; n (N) T Representing the time domain; x (0) and x (N) T ) Respectively representing an initial state and a terminal state;
step four: and solving the track planning optimization problem.
The invention further adopts the technical scheme that: in the first step, the weightIs set of geometric models of (a)Is that
Wherein,,representing the orthogonal rotation matrix of the weight at time k; />Representing a displacement vector of the weight; />A set of geometric models representing the initial moment of the weight; matrix A 0 Sum vector b 0 Constitution->Determined by the shape of the heavy object; />Representing a normal cone defining a generalized inequality, determined by the shape of the obstacle, if the obstacle is a polyhedral shape +.>Is a non-negative limit, if the obstacle is ellipsoidal in shape, +.>Is a second order cone; y is belonging to the set->Is an arbitrary element of (c).
The invention further adopts the technical scheme that: in the first step, the geometric model set of the unmanned plane iIs that
Wherein,,the state of the unmanned aerial vehicle i at the moment k; />An orthogonal rotation matrix of the unmanned plane i at the time k is represented;a displacement vector representing the unmanned plane i; />A geometric model set representing the initial moment of the unmanned plane i; matrix A i Sum vector b i Constitution->Is determined by the shape of the unmanned aerial vehicle i; />The normal cone representing the generalized inequality is defined, as determined by the shape of the drone.
The invention further adopts the technical scheme that: in the first step, the geometric model set of the obstacle j in the environmentIs that
Wherein matrix G j Sum vector g j Constitution ofIs determined by the shape of the obstacle j; gamma represents the set +.>Any element in (a); />The normal cone, representing the generalized inequality, is defined by the shape of the obstacle.
The invention further adopts the technical scheme that: in the second step, in order to realize collision avoidance between unmanned aerial vehicles, the distance between the required sets satisfies the following relationship:
wherein d 1 Representing the minimum safe distance between unmanned aerial vehicles for collision avoidance.
The invention further adopts the technical scheme that: in the second step, in order to realize collision avoidance between the unmanned aerial vehicle and the obstacle, a distance is requiredThe following relationships are satisfied:
wherein d 2 And the minimum safety distance for collision avoidance between the unmanned aerial vehicle and the obstacle is represented.
The invention further adopts the technical scheme that: in the second step, in order to realize collision avoidance between the heavy object and the unmanned aerial vehicle, a distance is requiredThe following relationships are satisfied:
wherein d 3 Representing the minimum safety distance between the heavy object and the unmanned aerial vehicle.
The invention further adopts the technical scheme that: in the second step, in order to realize collision avoidance between the heavy object and the obstacle, a distance is requiredThe following relationships are satisfied:
wherein d 4 Indicating the minimum safe distance for collision between the weight and the obstacle.
The invention further adopts the technical scheme that: in the second step, the tethered multi-unmanned aerial vehicle system also has the constraint problems of limited state, limited actuator, limited tethered connection and a dynamics equation;
the unmanned plane i state and actuator constraints are expressed as follows:
wherein,,the control input of the unmanned aerial vehicle i at the moment k is provided;
the tether connection constraints are expressed as follows:
wherein,,installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; l (L) i0 Tensioning length of tether for connecting unmanned aerial vehicle i with weightA degree; | x I 2 Representing the Euclid norm of the vector;
the system discrete dynamics equation is:
x k+1 ＝f(x k ,u k ) (23)
wherein,,is a compact vector formed by a weight at the moment k and the state of the unmanned aerial vehicle;
is a compact vector formed by the control inputs of the unmanned aerial vehicle at the moment k; x is x k+1 Is the state of the system at time k+1.
The invention further adopts the technical scheme that: in the fourth step, an initial track is searched by adopting an A-method and is used as an initial guess for solving the optimization problem in the third step; in the third step, the optimization problem is a non-convex optimization problem, and a heuristic method is adopted to solve the optimization problem to obtain a reference track of the system.
Advantageous effects
The invention has the beneficial effects that: the invention provides an obstacle avoidance trajectory planning method based on an optimized tethered multi-agent system, which is a method for explicitly representing collision avoidance constraints among objects based on set distances, and simultaneously adopts strong dual properties in convex optimization to lead the obstacle avoidance constraints to be equivalent to microminiaturization and smoothness.
1) The method can be used for solving the track planning problem of the multi-robot cooperative operation system;
2) The robot real-time obstacle avoidance motion planning method can be used for solving the problem of robot real-time obstacle avoidance motion planning.
1-3, the obstacle avoidance track planning method of the rope-based multi-unmanned-aerial-vehicle collaborative handling system provided by the invention has a good effect, realizes obstacle avoidance of unmanned aerial vehicles and heavy objects, prevents collision between unmanned aerial vehicles and heavy objects, and meets the constraint condition of unmanned aerial vehicle dynamic performance. Meanwhile, the invention is a patent of a first application on a obstacle avoidance track planning method of a multi-unmanned plane cooperative transportation system, and fills the blank of technical research in the aspect.
Drawings
FIG. 1 illustrates obstacle avoidance flight trajectories of a three-unmanned-aerial-vehicle tether suspension type cooperative handling system;
the moment when the three unmanned aerial vehicle tether suspension type cooperative conveying system passes through a window in figure 2;
fig. 3 obstacle avoidance trajectory curves for drone and weight planning.
Detailed Description
The embodiments described below by referring to the drawings are illustrative and intended to explain the present invention and should not be construed as limiting the invention.
The invention proposes the following execution steps:
step 1, establishing an obstacle avoidance constraint model of a rope coupling multi-unmanned aerial vehicle cooperative operation system;
step 2, deriving optimization constraint conditions of the rope coupling multi-unmanned aerial vehicle cooperative operation system;
step 3, constructing a rope coupling multi-unmanned aerial vehicle cooperative operation system based on an optimized track planning problem;
and 4, solving the track planning optimization problem.
1. Establishing obstacle avoidance constraint model of rope coupling multi-unmanned aerial vehicle cooperative operation system
The geometric model of the weight is formed by the combination of the motion spaceWherein->Is the state of the weight at time k. The set of geometrical models of unmanned plane i is +.>Wherein i is {1,2, …, N }, N is the number of unmanned aerial vehicles in the operating system, +.>The state of the unmanned plane i at the time k. The set of geometrical models of obstacles j in the environment is +.>Where j ε {1,2, …, M } 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 microminiaturization of the optimization problem, the geometric model set of the weight is expressed as follows:
wherein,,representing the orthogonal rotation matrix of the weight at time k; />Representing a displacement vector of the weight; />A set of geometric models representing the initial moment of the weight; matrix A 0 Sum vector b 0 Constitution->Determined by the shape of the heavy object; />Representing a normal cone defining a generalized inequality, determined by the shape of the obstacle, if the obstacle is a polyhedral shape +.>As a non-negative image limit of the image,if the obstacle is ellipsoidal in shape +.>Is a second order cone; y is belonging to the set->Is an arbitrary element of (c). The set of geometric models of unmanned aerial vehicle i is represented as:
wherein,,an orthogonal rotation matrix of the unmanned plane i at the time k is represented; />A displacement vector representing the unmanned plane i; />A geometric model set representing the initial moment of the unmanned plane i; matrix A i Sum vector b i Constitution->Is determined by the shape of the unmanned aerial vehicle i; />The normal cone representing the generalized inequality is defined, as determined by the shape of the drone. The set of geometric models of the obstacle j in the environment is expressed as:
wherein matrix G j Sum vector g j Constitution ofIs determined by the shape of the obstacle j; gamma represents the set +.>Any element in (a); />The normal cone, representing the generalized inequality, is defined by the shape of the obstacle.
In the tethered coupling multi-unmanned aerial vehicle co-operation system, collision risks exist among unmanned aerial vehicles, among unmanned aerial vehicles and obstacles, among weights and unmanned aerial vehicles and among weights and obstacles. The collision prevention constraint between unmanned aerial vehicles is expressed as:
wherein the symbol ∈ represents the intersection of the sets,representing an empty set. The collision prevention constraint between the unmanned aerial vehicle and the obstacle is expressed as:
the collision prevention constraint between the weight and the unmanned aerial vehicle is expressed as:
the collision prevention constraint between the weight and the obstacle is expressed as:
2. optimization constraint condition of push rope coupling multi-unmanned aerial vehicle cooperative operation system
The collision avoidance constraint in step 1 is usually non-convex and non-micromanipulation, which can cause great trouble to the optimization problem solving method based on the gradient and black plug matrix. Therefore, in step 2, we first need to re-express the collision avoidance constraint in step 1 equivalently with a new mathematical expression method. The effective distance between unmanned aerial vehicle is:
wherein dist represents the distance between the two sets. In order to realize collision avoidance between unmanned aerial vehicles, the distance between the required sets satisfies the following relationship:
wherein d 1 Representing the minimum safe distance between unmanned aerial vehicles for collision avoidance. The effective distance between unmanned aerial vehicle and the barrier is:
in order to realize collision avoidance between the unmanned aerial vehicle and the obstacle, a distance is requiredThe following relationships are satisfied:
wherein d 2 And the minimum safety distance for collision avoidance between the unmanned aerial vehicle and the obstacle is represented. The effective distance between the weight and the unmanned aerial vehicle is:
in order to realize collision prevention between heavy objects and unmanned aerial vehicle, a required distance is requiredSeparation ofThe following relationships are satisfied:
wherein d 3 Representing the minimum safety distance between the heavy object and the unmanned aerial vehicle. The effective distance between the weight and the obstacle is as follows:
in order to realize collision avoidance between the heavy objects and the barriers, the distance is requiredThe following relationships are satisfied:
wherein d 4 Indicating the minimum safe distance for collision between the weight and the obstacle.
The equivalent form of the optimization problem (31) is:
corresponding dual function g (lambda ij ,μ ij ,z ij ) The method comprises the following steps:
wherein lambda is ij ，μ ij ，z ij Representing the dual variable; | x I * Representing the dual norms. Thus, the dual problem of the optimization problem (39) is:
wherein,,representation->Is a dual cone of (c). Likewise, we give the dual problem of the optimization problem (33) as:
wherein,,representation->Is a dual cone of (2); />And->The dual variables of the problem (33) are represented respectively. The dual problem of the optimization problem (35) is:
wherein,,representation->Is a dual cone of (2); lambda (lambda) i0 ，μ i0 And z i0 The dual variables of the problem (35) are represented respectively. Optimization problem (3)7) The dual problem of (2) is:
wherein,,and->The dual variables of the problem (37) are represented respectively. From the question (39), we know that the question is a convex question, and +.>With a non-empty relative interior point. Therefore, if the Slater condition of the problem (39) is satisfied, the dual problem (41) satisfies the strong dual. Likewise, the optimization problems (42), (43), (44) are all satisfied with strong duality. Then, the collision avoidance constraints (32), (34), (36), (38) are equivalent to the following constraints:
in addition to the collision avoidance constraints discussed above, tethered multi-unmanned aerial vehicle systems also have constraints such as limited state, limited actuator, limited tether connection, and kinetic equations. The unmanned plane i state and actuator constraints are expressed as follows:
wherein,,the control input of the unmanned aerial vehicle i at the moment k is provided. The tether connection constraints are expressed as follows:
wherein,,installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; l (L) i0 The tensioning length of the tether for connecting the unmanned aerial vehicle i with the weight; | x I 2 Representing the Euclid norm of the vector. The system discrete dynamics equation is:
x k+1 ＝f(x k ,u k ) (51)
wherein,,is a compact vector formed by a weight at the moment k and the state of the unmanned aerial vehicle;is a compact vector formed by the control inputs of the unmanned aerial vehicle at the moment k; x is x k+1 Is the state of the system at time k+1.
3. Construction of a tethered coupling multi-unmanned aerial vehicle collaborative operation system based on an optimized track planning problem
Step 2, deducing various constraint conditions existing in the system, and finishing to obtain the track planning problem of the system based on optimization as follows:
wherein,,representing a phase objective function; n (N) T Representing the time domain; x (0) and x (N) T ) Representing an initial state and a terminal state, respectively.
4. Solving the problem of trajectory planning optimization
The trajectory optimization problem obtained in step 3 has a plurality of variables and constraints, but most of the constraints are in the form of linear equations and inequalities, the constraints are relatively easy to process, and all the constraints are continuously and slightly variable. 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. In the step 3, the optimization problem 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.
Examples:
the invention verifies the advancement and superiority of the invention in the aspect of processing the obstacle avoidance track planning problem of the multi-robot cooperative operation by simulating and analyzing the obstacle avoidance track planning problem of the tether suspension cooperative transport system consisting of three rotor unmanned aerial vehicles. 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.01.0; 0.0082 0; 00 0.0148]kgm 2 Unmanned aerial vehicle radius 0.2m, roll direction unmanned aerial vehicle motor interaxial distance 0.2136m, pitch direction unmanned aerial vehicle motor interaxial distance 0.1758m, motor thrust-torque constant 81.0363N/Nm, motor force drift-0.2046N, motor force constant 2.0784 ×10 -8 N/RPM 2 An offset of angular velocity from force of 1004.5RPM, a voltage to angular velocity offset of 2132.6RPM, an effective motor speed constant of 1295.4RPM/V, a maximum voltage of 12.6V per motor, a weight mass of 0.3kg, a weight radius of 0.1m, a length of 1m per rope, a starting position of the weight [3.5;5, a step of; 0.634]m, end position of weight [9;3.5;1.134]m. The obstacle is a wall having a window through which the unmanned aerial vehicle co-handling system is expected to travel from one side of the wallMove to the other side. The system motion space is [2,10]m×[0,10]m×[0,5]m, the wall coverage range is [7,7.5 ]]m×[0,10]m×[0,5]m, wall Window coverage is [7,7.5 ]]m×[4.5,5.5]m×[2,3]m is a window of size 1m x 1 m.
Fig. 1 shows a three-dimensional effect diagram of obstacle avoidance flight trajectories of three unmanned aerial vehicle tether suspension type cooperative conveyance systems, fig. 2 shows the moment when three unmanned aerial vehicle tether suspension type cooperative conveyance systems pass through windows, 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 track planning method of the rope-based multi-unmanned-aerial-vehicle collaborative handling system provided by the invention has a good effect, realizes obstacle avoidance of unmanned aerial vehicles and heavy objects, prevents collision between unmanned aerial vehicles and heavy objects, and meets the constraint condition of unmanned aerial vehicle dynamic performance. Meanwhile, the invention is a patent of a first application on a obstacle avoidance track planning method of a multi-unmanned plane cooperative transportation system, and fills the blank of technical research in the aspect.
Although embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives, and variations may be made in the above embodiments by those skilled in the art without departing from the spirit and principles of the invention.
Claims (10)
1. An obstacle avoidance track planning method based on optimized rope multi-unmanned aerial vehicle cooperative transportation is characterized by comprising the following specific steps:
step one: establishing an obstacle avoidance constraint model of a rope coupling multi-unmanned aerial vehicle cooperative operation system;
first, a geometric model set of weights is established asThe geometrical model set of unmanned plane i is +.>Environment (environment)The set of geometrical models of obstacle j in +.>
Wherein,,the state of the weight at the moment k; i epsilon {1,2, …, N }, N being the number of unmanned aerial vehicles in the operating system, { I }>The state of the unmanned aerial vehicle i at the moment k; j is {1,2, …, M }, M being the number of obstacles in the environment;
then, a barrier avoidance constraint model is obtained through coupling: the collision prevention constraint between unmanned aerial vehicles is expressed as:
wherein the symbol ∈ represents the intersection of the sets,representing an empty set;
the collision prevention constraint between the unmanned aerial vehicle and the obstacle is expressed as:
the collision prevention constraint between the weight and the unmanned aerial vehicle is expressed as:
the collision prevention constraint between the weight and the obstacle is expressed as:
step two: the pushing rope is coupled with the optimization constraint condition of the multi-unmanned aerial vehicle cooperative operation system;
firstly, converting the obstacle avoidance constraint model obtained in the first step to obtain the effective distance between unmanned aerial vehicles, wherein the effective distance is as follows:
wherein dist represents the distance between the two sets;
the effective distance between unmanned aerial vehicle and the barrier is:
the effective distance between the weight and the unmanned aerial vehicle is:
the effective distance between the weight and the obstacle is as follows:
then, based on the dual transformation principle, the optimized constraint conditions are obtained:
wherein lambda is ij ，μ ij ，z ij Representing the dual variable; | x I * Representing the dual norms;representation->Is a dual cone of (2); />Representation->Is a dual cone of (2); />And->The dual variables respectively representing the problem (6); />Representation->Is a dual cone of (2); lambda (lambda) i0 ，μ i0 And z i0 The dual variables respectively representing the problem (7); />And->Dual variables respectively representing the problem (8); d, d 1 Represents the minimum safety distance of collision avoidance between unmanned aerial vehicles, d 2 Represents the minimum safety distance of collision between the unmanned aerial vehicle and the obstacle, d 3 Representing the minimum safety distance of collision between a heavy object and an unmanned aerial vehicle, d 4 Representing the minimum safety distance between the heavy object and the obstacle;
step three: constructing a rope coupling multi-unmanned aerial vehicle cooperative operation system based on an optimized track planning problem;
and (3) sorting the various constraint conditions obtained in the second step to obtain the track planning problem of the system based on optimization as follows:
for i,j＝1,…,N，i＜j，m＝1,…,M (13)
wherein l (x k ,u k ) Representing a phase objective function; n (N) T Representing the time domain; x (0) and x (N) T ) Respectively representing an initial state and a terminal state;representing the orthogonal rotation matrix of the weight at time k; />An orthogonal rotation matrix of the unmanned plane i at the time k is represented; />Representing a displacement vector of the weight; />A displacement vector representing the unmanned plane i;
step four: and solving the track planning optimization problem.
2. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the first step, the geometric model set of the weight is obtainedIs that
Wherein,,representing the orthogonal rotation matrix of the weight at time k; />Representing a displacement vector of the weight; />A set of geometric models representing the initial moment of the weight; matrix A 0 Sum vector b 0 Constitution->Determined by the shape of the heavy object; />Representing a normal cone defining a generalized inequality, determined by the shape of the obstacle, if the obstacle is a polyhedral shape +.>Is a non-negative limit, if the obstacle is ellipsoidal in shape, +.>Is a second order cone; y is belonging to the set->Is an arbitrary element of (c).
3. According to claimThe obstacle avoidance track planning method based on the collaborative handling of the optimized tethered multiple unmanned aerial vehicles is characterized by comprising the following steps of: in the first step, the geometric model set of the unmanned plane iIs that
Wherein,,the state of the unmanned aerial vehicle i at the moment k; />An orthogonal rotation matrix of the unmanned plane i at the time k is represented;a displacement vector representing the unmanned plane i; />A geometric model set representing the initial moment of the unmanned plane i; matrix A i Sum vector b i Constitution->Is determined by the shape of the unmanned aerial vehicle i; />The normal cone representing the generalized inequality is defined, as determined by the shape of the drone.
4. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the first step, the geometric model set of the obstacle j in the environmentIs that
Wherein matrix G j Sum vector g j Constitution ofIs determined by the shape of the obstacle j; gamma represents the set +.>Any element in (a); />The normal cone, representing the generalized inequality, is defined by the shape of the obstacle.
5. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the second step, in order to realize collision avoidance between unmanned aerial vehicles, the distance between the required sets satisfies the following relationship:
wherein d 1 Representing the minimum safe distance between unmanned aerial vehicles for collision avoidance.
6. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the second step, in order to realize collision avoidance between the unmanned aerial vehicle and the obstacle, a distance is requiredThe following relationships are satisfied:
wherein d 2 And the minimum safety distance for collision avoidance between the unmanned aerial vehicle and the obstacle is represented.
7. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the second step, in order to realize collision avoidance between the heavy object and the unmanned aerial vehicle, a distance is requiredThe following relationships are satisfied:
wherein d 3 Representing the minimum safety distance between the heavy object and the unmanned aerial vehicle.
8. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the second step, in order to realize collision avoidance between the heavy object and the obstacle, a distance is requiredThe following relationships are satisfied:
wherein d 4 Indicating the minimum safe distance for collision between the weight and the obstacle.
9. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the second step, the tethered multi-unmanned aerial vehicle system also has the constraint problems of limited state, limited actuator, limited tethered connection and a dynamics equation;
the unmanned plane i state and actuator constraints are expressed as follows:
wherein,,the control input of the unmanned aerial vehicle i at the moment k is provided;
the tether connection constraints are expressed as follows:
wherein,,installing the positions of the nodes on the unmanned aerial vehicle and the heavy object for the tether; l (L) i0 The tensioning length of the tether for connecting the unmanned aerial vehicle i with the weight; | x I 2 Representing the Euclid norm of the vector;
the system discrete dynamics equation is:
x k+1 ＝f(x k ,u k ) (23)
wherein,,is a compact vector formed by a weight at the moment k and the state of the unmanned aerial vehicle;is a compact vector formed by the control inputs of the unmanned aerial vehicle at the moment k; x is x k+1 Is the state of the system at time k+1.
10. The obstacle avoidance trajectory planning method based on optimized tethered multi-unmanned aerial vehicle collaborative handling of claim 1, wherein the method is characterized by: in the fourth step, an initial track is searched by adopting an A-method and is used as an initial guess for solving the optimization problem in the third step; in the third step, the optimization problem is a non-convex optimization problem, and a heuristic method is adopted to solve the optimization problem to obtain a reference track of the system.
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 CN113759977A (en) | 2021-12-07 |
| CN113759977B true 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) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114167732B (en) * | 2021-12-13 | 2024-08-09 | 西北工业大学 | Coupling constraint multi-agent distributed robust nonlinear model prediction control method |
| CN114089780B (en) * | 2022-01-21 | 2022-04-12 | 中国电子科技集团公司第二十八研究所 | Urban space-oriented multi-rotor unmanned aerial vehicle path planning method |
| CN117466161B (en) * | 2023-09-08 | 2024-06-11 | 兰州交通大学 | Obstacle avoidance track planning method for multi-machine suspension system |
| 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)
| 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 |
-
2021
- 2021-09-18 CN CN202111103700.XA patent/CN113759977B/en active Active
Patent Citations (4)
| 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)
| Title |
|---|
| 一种基于六次多项式轨迹规划的机械臂避障算法;马宇豪;梁雁冰;;西北工业大学学报(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113759977A (en) | 2021-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113759977B (en) | Obstacle avoidance track planning method based on optimized coordinated transportation of multiple tethered unmanned aerial vehicles | |
| Ollero et al. | Past, present, and future of aerial robotic manipulators | |
| Douthwaite et al. | Velocity obstacle approaches for multi-agent collision avoidance | |
| CN107490965A (en) | A kind of multiple constraint method for planning track of the free floating devices arm in space | |
| Zhang et al. | An obstacle avoidance algorithm for robot manipulators based on decision-making force | |
| CN108393886B (en) | Distributed multi-mobile manipulator cooperative carrying method capable of optimizing energy and operation degree | |
| Hong et al. | Quadrotor path planning using A* search algorithm and minimum snap trajectory generation | |
| Huang et al. | Distributed flocking control of quad-rotor UAVs with obstacle avoidance under the parallel-triggered scheme | |
| Tanha et al. | Control a mobile robot in Social environments by considering human as a moving obstacle | |
| Lori et al. | Transportation of an unknown cable-suspended payload by a quadrotor in windy environment under aerodynamics effects | |
| Borysov et al. | Parameters for Mobile Robot Kinematic Model Development Determination | |
| Li et al. | A model predictive obstacle avoidance method based on dynamic motion primitives and a Kalman filter | |
| Lippiello et al. | Visual coordinated landing of a UAV on a mobile robot manipulator | |
| Ruggiero et al. | Introduction to the special issue on aerial manipulation | |
| Helble et al. | 3-d path planning and target trajectory prediction for the oxford aerial tracking system | |
| Choi et al. | Collision avoidance of unmanned aerial vehicles using fuzzy inference system-aided enhanced potential field | |
| Touil et al. | Intelligent image-based-visual servoing for quadrotor air vehicle | |
| Kotani et al. | Cooperative transport control by a multicopter system | |
| CN114518709A (en) | Full-attitude four-axis turntable frame angle instruction resolving method, equipment and medium | |
| Xu et al. | Research on the strategies for collision avoidance of multi‐UAV with three dimensional formation in combination of consensus algorithm and uniform flow | |
| Patel et al. | Artificial neural network-assisted controller for fast and agile UAV flight: Onboard implementation and experimental results | |
| Hota et al. | Waypoint-based trajectory planning of fixed-wing MAVs in 3D space | |
| Li et al. | An improved APF-based path planning algorithm for a quadrotor intercepting autonomous ground robots | |
| Wehbe et al. | Novel three-dimensional optimal path planning method for vehicles with constrained pitch and yaw | |
| Aghaeeyan et al. | UAV guidance for tracking control of mobile robots in presence of obstacles |
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 |