CN113296536A - Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm - Google Patents
Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm Download PDFInfo
- Publication number
- CN113296536A CN113296536A CN202110567745.6A CN202110567745A CN113296536A CN 113296536 A CN113296536 A CN 113296536A CN 202110567745 A CN202110567745 A CN 202110567745A CN 113296536 A CN113296536 A CN 113296536A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- obstacle avoidance
- convex
- algorithm
- 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
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/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
Abstract
The invention discloses an unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on an A-star and convex optimization algorithm, which comprises the following steps: firstly, defining a design task of a track planning algorithm; secondly, obtaining a reference path of the unmanned aerial vehicle through an A-star algorithm; thirdly, providing obstacle avoidance constraint consisting of a series of convex polyhedrons by an iterative region expansion method based on semi-definite programming; and fourthly, aiming at the unmanned aerial vehicle system, providing a three-dimensional obstacle avoidance track planning model of the unmanned aerial vehicle, and obtaining a state sequence and a control sequence of the unmanned aerial vehicle by resolving the model. The algorithm of the invention not only can avoid convex polyhedral obstacles in the field, but also can avoid the possibility that the unmanned aerial vehicle impacts the obstacles between discrete time steps. Compared with the traditional convex optimization obstacle avoidance algorithm, the algorithm reduces the calculation amount, and finally obtains a group of state sequences meeting the requirements and a group of control sequences with the least fuel consumption.
Description
Technical Field
The invention belongs to the field of aerospace, and relates to a three-dimensional obstacle avoidance algorithm for an unmanned aerial vehicle, in particular to an algorithm which is suitable for the unmanned aerial vehicle and can quickly generate a flight track to reach a specified place and avoid convex polyhedral obstacles in the place.
Background
Traditional unmanned aerial vehicle trajectory planning is generally modeled as a discrete linear optimal control problem, which is essentially a mixed integer linear planning problem. The model often causes a large number of integer variables and inequality constraints to appear in the model due to irregular shapes of obstacles and increase of the number of the obstacles, so that the solving time is too long and the real-time performance is lost, even the solving capability of a general resolver is directly exceeded, and in addition, the possibility that the unmanned aerial vehicle collides the obstacles between discrete time steps is possible. The conventional heuristic path planning algorithm such as the a-algorithm also has the problems that the path is not smooth, and the constraint brought by the unmanned aerial vehicle is difficult to consider.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides an unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on an A-star and convex optimization algorithm. The algorithm can quickly give the flight track from the starting point to the target point in the three-dimensional field, avoid convex polyhedral obstacles existing in the field and simultaneously generate a group of control sequences with the least fuel consumption.
The purpose of the invention is realized by the following technical scheme:
an unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A and convex optimization algorithm comprises the following steps:
firstly, defining a design task of a track planning algorithm, wherein the design task of the track planning algorithm is as follows: given a three-dimensional map containing a plurality of convex polyhedral obstacles, marking a starting point x0And target point xfFor a given drone and a specified time of flight tfinalGiving a flight path of the most fuel-saving fuel and a corresponding control sequence;
secondly, obtaining a reference path of the unmanned aerial vehicle through an A-star algorithm, wherein a cost function is designed as follows:
f(n)＝g(n)+h(n)；
wherein f (n) is from the starting point x0Moving to target point x via node x (n)fG (n) is from the starting point x0A movement cost of moving to a designated cell along a path generated to reach the cell, h (n) is a movement from the designated cell to a target point xfThe estimated cost of (2);
thirdly, providing obstacle avoidance constraints composed of a series of convex polyhedrons through an iterative regional expansion method based on semi-definite programming, wherein the obstacle avoidance constraints of the convex regions are expressed as follows:
L＝{x|Aix≤bi}；
wherein, constraint equation matrix Ai∈Rm×n，bi∈Rm，i＝[1,2，...，Nc]，NcThe number of convex regions, m is the number of constraint elements, n is the number of dimensions, and n is 3;
and fourthly, aiming at the unmanned aerial vehicle system, providing a three-dimensional obstacle avoidance track planning model of the unmanned aerial vehicle, and obtaining a state sequence and a control sequence of the unmanned aerial vehicle by resolving the model, wherein the three-dimensional obstacle avoidance track planning model of the unmanned aerial vehicle is as follows:
s.t.Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
AiPi(j)≤bi；
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)；
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu；
wherein N iscDenotes the number of convex regions, Step 100 denotes the number of steps per convex polyhedron region, ui(j, N) represents the nth axial thrust component in the thrust vector of the jth step in the ith convex region, i ═ 1,2c]，j＝[1,2,...,Step]，n＝[1,2,3]- | represents an absolute value, | - | non-woven phosphor2Denotes the 2-norm, Xi(j)＝[pix(j) piy(j) piz(j) vix(j) viy(j) viz(j)]T，Pi(j)＝[pix(j) piy(j) piz(j)]T，pix(j)、piy(j)、piz(j) Respectively the position components of x, y and z axes in the jth step of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, vix(j)、viy(j)、viz(j) Respectively the speed components of the unmanned aerial vehicle on the X, y and z axes in the jth step in the ith obstacle avoidance constraint area, X0、XfRespectively representing the starting state and the target state of the unmanned aerial vehicle, AdSystem matrix representing unmanned aerial vehicles, BdInput matrix, u, representing dronesi(j) Representing the thrust vector u of the j step of the unmanned aerial vehicle in the ith obstacle avoidance constraint areamaxRepresents the maximum thrust provided by the drone,
t0(i) for the start time, t, of each convex regionf(i) For the end time of each convex region, Δ u represents the maximum thrust rate of change.
Compared with the prior art, the invention has the following advantages:
1. the algorithm of the invention not only can avoid convex polyhedral obstacles in the field, but also can avoid the possibility that the unmanned aerial vehicle impacts the obstacles between discrete time steps.
2. Compared with the traditional convex optimization obstacle avoidance algorithm, the algorithm reduces the calculation amount, and finally obtains a group of state sequences meeting the requirements and a group of control sequences with the least fuel consumption.
Drawings
Fig. 1 is a design flow chart of an unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on an a-and-convex optimization algorithm;
fig. 2 is a flow chart of the a algorithm;
FIG. 3 is a flow chart of an iterative regional dilation algorithm based on semi-deterministic programming;
FIG. 4 is a flow diagram of generating a series of convex region constraints;
FIG. 5 is three-dimensional map data, (a) a three-dimensional view, (b) a top view;
FIG. 6 is a reference flight trajectory, (a) three-dimensional view, (b) top view;
FIG. 7 is a convex region constraint map, (a) three-dimensional view, (b) top view;
fig. 8 is a schematic diagram of the generation of a drone position trajectory, (a) three-dimensional view, (b) top view;
fig. 9 is a schematic diagram of generating drone thrust.
Detailed Description
The technical solution of the present invention is further described below with reference to the accompanying drawings, but not limited thereto, and any modification or equivalent replacement of the technical solution of the present invention without departing from the spirit and scope of the technical solution of the present invention shall be covered by the protection scope of the present invention.
The invention provides an unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization, as shown in figure 1, the specific design steps are as follows:
the first step is as follows: and (5) defining the design task of the trajectory planning algorithm.
The design task of the trajectory planning algorithm is as follows: given a three-dimensional map containing a plurality of convex polyhedral obstacles, marking a starting point x0And target point xfFor a given drone and a specified time of flight tfinalThe flight path for the most fuel efficient and the corresponding control sequence are given.
The second step is that: and giving out a reference path of the aircraft through an A-x algorithm.
Firstly, designing a cost function:
f(n)＝g(n)+h(n)；
wherein f (n) is from the starting point x0Moving to target point x via node x (n)fG (n) is from the starting point x0A movement cost for moving to a designated square along a path generated by reaching the square, wherein the movement cost is a linear distance between centers of two squares, and h (n) is a linear distance from the designated square to a target point xfUsing the currently assigned center point of the square to the target point xfThe straight line distance of the center point of the square is used as the estimated cost.
The flow of the a-algorithm is shown in fig. 2, and may be specifically summarized as follows:
1. add the starting point to the open list.
2. The following procedure was repeated:
a. and traversing the open list, searching f (n) the minimum node, and setting the minimum node as the current node.
b. The current node is moved to close list.
c. And judging whether nodes around the current node exist in the list or not.
If it exists in close list, the node is ignored.
If it is not in openlist, it is added to openlist, and the current tile is set as its parent, and the f (n) value of the tile is recorded.
If it is already in openlist, check if this path is more optimal, if so, set its parent to the current tile, and recalculate its f (n) value.
d. The cycle is stopped when the following occurs:
adding an endpoint to openlist when a path has been found, or
Finding the end point fails and openlist is empty, at which point there is no path.
3. The path is saved. From the end point, each square moves along the parent node until the start point, and a reference path is obtained.
The third step: and providing obstacle avoidance constraint consisting of a series of convex polyhedrons by an iterative region expansion method based on semi-definite programming.
The overall flow chart of the algorithm is shown in fig. 3 and fig. 4, wherein fig. 3 shows a method for obtaining a convex region tangent to an obstacle through an iterative region expansion algorithm based on semi-definite programming according to a point, and fig. 4 shows a method for obtaining a series of convex regions according to reference path coordinates. First, a method for obtaining an optimal set of splitting planes by taking an ellipsoid with a coordinate point as a sphere center and tangent to an obstacle by referring to a trajectory coordinate point is described with reference to fig. 3.
The linear constraint of the convex region can be expressed as:
L＝{x|Ax≤b}；
wherein the constraint equation matrix A belongs to Rm×n，b∈RmM is the number of constraint elements, n is the number of dimensions, and n is 3 in the invention.
The initial ellipsoid is obtained by coordinate transformation of a unit circle, and can be expressed as:
wherein: c is belonged to Rn×n，
d is the coordinates in the reference path of the drone.
Solving the optimal linear constraint of the given ellipsoid is to ensure that the ellipsoid is not intersected with the obstacle when the volume of the inscribed ellipsoid is enlarged, and to solve the problem, a point closest to the ellipsoid in the obstacle can be found first.
Define ellipsoid E and convex polyhedral obstacle OjSet of vertices vjpAnd p is 1,2,3, the nearest distance problem can be mapped to a unit sphere according to the ellipsoid definition formula, namely, only the mapped obstacle needs to be found
The closest point to the origin, the problem can be expressed as:
points obtained by the optimization
Through original mapping
The point x closest to the ellipsoid in the obstacle in the original problem can be obtained*。
After finding the point position of the barrier closest to the ellipsoid, an optimal segmentation surface can be obtained by obtaining a perpendicular orthogonal surface passing the point and the ellipsoid, and the step of obtaining the perpendicular orthogonal surface is as follows:
wherein, ajIs line j of A, bjIs the jth element of b.
Thus, an optimal segmentation surface is obtained, in order to reduce the calculation amount, the barrier is traversed once after the optimal segmentation surface is calculated, and whether the vertexes of the barrier meet the requirement or not is judged
If the result is satisfied, the obstacle and the ellipsoid are separated by the dividing surface, and if the result is not satisfied, another dividing surface is calculated to continuously divide the obstacle and the ellipsoid.
After the optimal segmentation surface set of the given ellipsoid is obtained, in order to make the volume of the convex polyhedron as large as possible, searching the ellipsoid with the largest volume in the obtained optimal segmentation surface set, wherein the searching of the ellipsoid with the largest volume in the given segmentation surface set can be expressed as the following problem:
C≥1；
where N is the number of obstacles and detC represents the determinant of the matrix C. Rewriting the above problems to not include
The format of (a) is as follows:
C≥1；
the problem is a convex optimization problem comprising semi-definite optimization and conic quadratic constraint, and can be solved by using a CVX (composite finite variable X) or Mosek solver. And then, repeatedly iterating according to the flow shown in fig. 3 to obtain the optimal segmentation surface set. The specific iteration process is as follows:
1. initializing an ellipsoid, initializing a location and an obstacle, wherein d ═ x0，C＝104×I3×3，I3×3Representing a three-dimensional unit array.
2. And solving the linear constraint formed by the set of the optimal splitting surfaces of the given ellipse.
3. Solving the maximum inscribed ellipsoid of the given linear constraint.
4. Determining whether | detC-detC is satisfiedmax|/detCmaxEpsilon in which C ismaxAnd C, a matrix C of the maximum detC obtained in iteration is represented, epsilon represents a given error, and if the maximum detC enters the loop for the first time, the judgment is skipped to directly bring the obtained C back to the step 2.
a. If yes, the iteration is ended.
b. If not, the obtained C is brought back to the step 2.
Then, a series of convex regions are obtained according to the flow of fig. 4, and the specific steps are as follows:
1. the reference path start point is set to a given point.
2. And obtaining a convex region constraint formed by an optimal segmentation surface set according to a given point and the iteration region expansion method based on the semi-definite programming.
3. Judging whether the target point meets the convex region constraint formed by the optimal segmentation surface set obtained in the step 2:
a. and if the target point meets the convex constraint, ending the cycle.
b. And (3) traversing the reference path point if the target point does not meet the convex constraint, finding the first path point which does not meet all the generated convex constraints at present, setting the first path point as a given point, and returning to the step (2).
Thus obtaining a series of obstacle avoidance constraints consisting of convex areas without obstacles, and setting a constraint equation matrix of the obstacle avoidance constraints as Ai∈Rm×n，bi∈Rm，i＝[1,2，...，Nc]，NcThe number of convex regions.
The fourth step: and (3) planning the three-dimensional obstacle avoidance online track of the unmanned aerial vehicle based on convex optimization.
Firstly, aiming at the problem of unmanned aerial vehicle track generation, an optimal control problem model comprising performance indexes, kinematic constraints, obstacle avoidance constraints, state constraints and thrust constraints is established.
The trajectory generation performance index J of the drone is shown by the following formula, by minimizing the throttle to obtain the most fuel efficient flight trajectory.
Wherein N iscDenotes the number of convex regions, Step 100 denotes the number of steps per convex polyhedron region, ui(j, N) represents the nth axial thrust component in the thrust vector of the jth step in the ith convex region, i ═ 1,2c]，j＝[1,2,...,Step]，n＝[1,2,3]And | represents an absolute value.
The kinematic constraint of the drone is represented by a system of differential equations represented by:
wherein, Xi＝[pix piy piz vix viy viz]T，pix、piy、pizRespectively the position components of the unmanned aerial vehicle on the x, y and z axes in the ith obstacle avoidance constraint area, vix、viy、vizRespectively x, y and y of the unmanned aerial vehicle in the ith obstacle avoidance constraint area,zVelocity component of the shaft, uiRepresenting the thrust vector of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, AdSystem matrix representing unmanned aerial vehicles, BdAn input matrix representing the drone. In order to convert the problem into a convex optimization problem, the problem needs to be discretized, a state transition matrix of the problem is a first-order approximation of a state matrix, and the discretized form is as follows:
Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
wherein, Xi(j)＝[pix(j) piy(j) piz(j) vix(j) viy(j) viz(j)]T，pix(j)、piy(j)、piz(j) Respectively the position components of x, y and z axes in the jth step of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, vix(j)、viy(j)、viz(j) Respectively the speed components u, u and z of the unmanned aerial vehicle in the jth step in the ith obstacle avoidance constraint areai(j) Representing the thrust vector of the unmanned aerial vehicle in the jth step in the ith obstacle avoidance constraint area, AdSystem matrix representing unmanned aerial vehicles, BdAn input matrix representing the drone is presented,
t0(i) for the start time, t, of each convex regionf(i) The end time of each convex region.
The obstacle avoidance constraint of the drone may be expressed as:
AiPi(j)≤bi；
wherein, Pi(j)＝[pix(j) piy(j) piz(j)]TAnd the track of the unmanned aerial vehicle is always in the convex area by the constraint, so that the unmanned aerial vehicle is ensured not to contact with the obstacle in the whole course.
The state constraint of the drone may be expressed as:
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)。
wherein, X0、XfShow unmanned aerial vehicle initial state and target state respectively, the initial state and the target state of unmanned aerial vehicle have been retrained to the above formula to unmanned aerial vehicle state's continuity when having guaranteed different convex areas and switching.
The thrust constraint of the drone is given by:
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu；
wherein | · | purple sweet2Denotes the 2-norm, umaxRepresents the maximum thrust provided by the drone and Δ u represents the maximum thrust rate of change.
The above formula gives the maximum thrust constraint and the maximum thrust change constraint of the drone.
In conclusion, the three-dimensional obstacle avoidance trajectory planning model of the unmanned aerial vehicle is obtained as follows:
s.t.Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
AiPi(j)≤bi；
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)；
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu。
and solving the convex optimization problem to obtain a state sequence and a control sequence which meet the requirements.
Example (b):
the design of the solution according to the invention will be further explained below by way of an example of a certain representative embodiment.
The first step is as follows: and (5) defining the design task of the trajectory planning algorithm.
The design task of the trajectory planning algorithm is as follows: given a three-dimensional map containing a plurality of convex polyhedral obstacles, marking a starting point x0And target point xfFor a given drone and a specified time of flight tfinalThe most fuel efficient flight path and the corresponding control sequence u are given.
The map data and the start point target point data are shown in FIG. 5, where the start point coordinate is x0(477) with target point coordinates xfTime of flight t ═ (17178)final25s, the boundaries of the three axes of the map are [ -1,21 [ ]]。
The second step is that: and giving out a reference path of the aircraft through an A-x algorithm.
Firstly, designing a cost function:
f(n)＝g(n)+h(n)；
wherein f (n) is from the starting point x0Moving to target point x via node x (n)fG (n) is from the starting point x0A movement cost for moving to a designated square along a path generated by reaching the square, wherein the movement cost is a linear distance between centers of two squares, and h (n) is a linear distance from the designated square to a target point xfUsing the currently assigned center point of the square to the target point xfThe straight line distance of the center point of the square is used as the estimated cost.
The reference flight path obtained according to the flow of fig. 2 is shown in fig. 6, and the specific reference path coordinates are shown in table 1.
TABLE 1 unmanned aerial vehicle reference flight trajectory
The third step: and providing obstacle avoidance constraint consisting of a series of convex polyhedrons by an iterative region expansion method based on semi-definite programming.
A series of convex constraint regions obtained by an iterative region expansion algorithm according to the process shown in fig. 3 and 4 are shown in fig. 7, where a given error ∈ is 0.02, and a specific convex region obstacle avoidance constraint matrix a is takeni、biThe following were used:
the fourth step: and (3) planning the three-dimensional obstacle avoidance online track of the unmanned aerial vehicle based on convex optimization.
Firstly, aiming at the problem of unmanned aerial vehicle track generation, an optimal control problem model comprising performance indexes, kinematic constraints, obstacle avoidance constraints, state constraints and thrust constraints is established.
The trajectory generation performance index J of the drone is shown by the following formula, by minimizing the throttle to obtain the most fuel efficient flight trajectory.
Wherein N iscDenotes the number of convex regions, Step 100 denotes the number of steps per convex polyhedron region, ui(j, N) represents the nth axial thrust component in the thrust vector of the jth step in the ith convex region, i ═ 1,2c]，j＝[1,2,...,Step]，n＝[1,2,3]And | represents an absolute value.
The kinematic constraint of the drone is represented by a system of differential equations represented by:
wherein, Xi＝[pix piy piz vix viy viz]T，pix、piy、pizRespectively the position components of the unmanned aerial vehicle on the x, y and z axes in the ith obstacle avoidance constraint area, vix、viy、vizRespectively the speed components u of the unmanned aerial vehicle on the x, y and z axes in the ith obstacle avoidance constraint areaiRepresenting the thrust vector of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, AdSystem matrix representing unmanned aerial vehicles, BdRepresenting the input matrix of the unmanned aerial vehicle, and defining specific parameters of a system matrix and the input matrix as follows without loss of generality:
in order to convert the problem into a convex optimization problem, the problem needs to be discretized, a state transition matrix of the problem is a first-order approximation of a state matrix, and the discretized form is as follows:
Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
wherein, Xi(j)＝[pix(j) piy(j) piz(j) vix(j) viy(j) viz(j)]T，pix(j)、piy(j)、piz(j) Respectively the position components of x, y and z axes in the jth step of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, vix(j)、viy(j)、viz(j) Respectively the speed components u, u and z of the unmanned aerial vehicle in the jth step in the ith obstacle avoidance constraint areai(j) Representing the thrust vector of the unmanned aerial vehicle in the jth step in the ith obstacle avoidance constraint area, AdSystem matrix representing unmanned aerial vehicles, BdAn input matrix representing the drone is presented,
t0(i) for the start time, t, of each convex regionf(i) The end time of each convex region. Let t0(i)＝0，tf(i) Is given according to the number of reference path nodes in the convex region, and the specific value is tf(1)＝12，tf(2)＝1，tf(3)＝2，tf(4)＝3，tf(5)＝3，tf(6)＝4。
The obstacle avoidance constraint of the drone may be expressed as:
AiPi(j)≤bi；
wherein, Pi(j)＝[pix(j) piy(j) piz(j)]TAnd the track of the unmanned aerial vehicle is always in the convex area by the constraint, so that the unmanned aerial vehicle is ensured not to contact with the obstacle in the whole course.
The state constraint of the drone may be expressed as:
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)；
wherein the starting point state is X0＝[4 9 1 0 0 0]TThe target point state is Xf＝[17 12 8 0 0 0]T。
The thrust constraint of the drone is given by:
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu；
wherein | · | purple sweet2Denotes the 2-norm, umax＝10，Δu＝1。
In conclusion, the three-dimensional obstacle avoidance trajectory planning model of the unmanned aerial vehicle is obtained as follows:
s.t.Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
AiPi(j)≤bi；
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)；
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu；
the convex optimization problem is solved, and the position track meeting the requirement is obtained as shown in fig. 8, and the thrust sequence is shown in fig. 9.
Claims (5)
1. An unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A and convex optimization algorithm is characterized by comprising the following steps:
firstly, defining a design task of a track planning algorithm;
secondly, obtaining a reference path of the unmanned aerial vehicle through an A-star algorithm;
thirdly, providing obstacle avoidance constraint consisting of a series of convex polyhedrons by an iterative region expansion method based on semi-definite programming;
and fourthly, aiming at the unmanned aerial vehicle system, providing a three-dimensional obstacle avoidance track planning model of the unmanned aerial vehicle, and obtaining a state sequence and a control sequence of the unmanned aerial vehicle by resolving the model.
2. The unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on the a and convex optimization algorithm according to claim 1, wherein in the first step, the design task of the trajectory planning algorithm is: given a three-dimensional map containing a plurality of convex polyhedral obstacles, marking a starting point x0And target point xfFor a given drone and a specified time of flight tfinalThe flight path for the most fuel efficient and the corresponding control sequence are given.
3. The unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on the a and convex optimization algorithm according to claim 1, wherein in the second step, a design cost function is as follows:
f(n)＝g(n)+h(n)；
wherein f (n) is from the starting point x0Moving to target point x via node x (n)fG (n) is from the starting point x0A movement cost of moving to a designated cell along a path generated to reach the cell, h (n) is a movement from the designated cell to a target point xfThe estimated cost of (a).
4. The unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on the a and convex optimization algorithm according to claim 1, wherein in the third step, the convex region obstacle avoidance constraint is expressed as:
L＝{x|Aix≤bi}；
wherein, constraint equation matrix Ai∈Rm×n，bi∈Rm，i＝[1,2，...，Nc]，NcAnd the number of convex regions, m is the number of constraint elements, and n is the dimension number, wherein n is 3.
5. The unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on the a and convex optimization algorithm according to claim 1, wherein in the fourth step, the unmanned aerial vehicle three-dimensional obstacle avoidance trajectory planning model is as follows:
s.t.Xi(j+1)＝Xi(j)+Δt(AdXi(j)+Bdui(j))；
AiPi(j)≤bi；
X1(1)＝X0；
XNc(Step)＝Xf；
Xi(Step)＝Xi+1(1)；
||ui(j)||2≤umax；
|ui(Step,n)-ui+1(1,n)|＜Δu；
wherein N iscDenotes the number of convex regions, Step 100 denotes the number of steps per convex polyhedron region, ui(j, N) represents the nth axial thrust component in the thrust vector of the jth step in the ith convex region, i ═ 1,2c]，j＝[1,2,...,Step]，n＝[1,2,3]- | represents an absolute value, | - | non-woven phosphor2Denotes the 2-norm, Xi(j)＝[pix(j) piy(j) piz(j) vix(j) viy(j) viz(j)]T，Pi(j)＝[pix(j) piy(j) piz(j)]T，pix(j)、piy(j)、piz(j) Respectively the position components of x, y and z axes in the jth step of the unmanned aerial vehicle in the ith obstacle avoidance constraint area, vix(j)、viy(j)、viz(j) Respectively the speed components of the unmanned aerial vehicle on the X, y and z axes in the jth step in the ith obstacle avoidance constraint area, X0、XfRespectively representing the starting state and the target state of the unmanned aerial vehicle, AdSystem matrix representing unmanned aerial vehicles, BdInput matrix, u, representing dronesi(j) Representing the thrust vector u of the j step of the unmanned aerial vehicle in the ith obstacle avoidance constraint areamaxRepresents the maximum thrust provided by the drone,
t0(i) for the start time, t, of each convex regionf(i) For the end time of each convex region, Δ u represents the maximum thrust rate of change.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110567745.6A CN113296536B (en) | 2021-05-24 | 2021-05-24 | Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110567745.6A CN113296536B (en) | 2021-05-24 | 2021-05-24 | Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113296536A true CN113296536A (en) | 2021-08-24 |
| CN113296536B CN113296536B (en) | 2022-04-05 |
Family
ID=77324512
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110567745.6A Active CN113296536B (en) | 2021-05-24 | 2021-05-24 | Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113296536B (en) |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105911867A (en) * | 2016-06-16 | 2016-08-31 | 哈尔滨工程大学 | Ship thrust distribution method based on NSGA-II algorithm |
| CN105929844A (en) * | 2016-04-26 | 2016-09-07 | 哈尔滨工业大学 | Obstacle avoidance method for soft landing of object outside earth under multi-obstacle constraint environment |
| CN108444482A (en) * | 2018-06-15 | 2018-08-24 | 东北大学 | A kind of autonomous pathfinding barrier-avoiding method of unmanned plane and system |
| CN109828600A (en) * | 2019-01-09 | 2019-05-31 | 北京理工大学 | Time optimal quick three-dimensional obstacle-avoiding route planning method |
| CN110398980A (en) * | 2019-06-05 | 2019-11-01 | 西安电子科技大学 | A kind of unmanned aerial vehicle group cooperates with the path planning method of detection and avoidance |
| CN110601172A (en) * | 2019-06-20 | 2019-12-20 | 中国电力工程顾问集团西南电力设计院有限公司 | Multi-direct-current coordination controller design method based on convex polyhedron uncertainty |
| CN110632941A (en) * | 2019-09-25 | 2019-12-31 | 北京理工大学 | Trajectory generation method for target tracking of unmanned aerial vehicle in complex environment |
| US20200012829A1 (en) * | 2016-05-07 | 2020-01-09 | Canyon Navigation, LLC | Navigation Using Self-Describing Fiducials |
| CN111486851A (en) * | 2020-04-27 | 2020-08-04 | 中国人民解放军国防科技大学 | Method and device for planning short-distance relative motion three-dimensional obstacle avoidance track of spacecraft |
| CN111811511A (en) * | 2020-06-23 | 2020-10-23 | 北京理工大学 | Unmanned aerial vehicle cluster real-time track generation method based on dimension reduction decoupling mechanism |
| US20200369292A1 (en) * | 2019-05-20 | 2020-11-26 | Tata Consultancy Services Limited | Method and system for path planning |
| WO2020239092A1 (en) * | 2019-05-30 | 2020-12-03 | 深圳市道通智能航空技术有限公司 | Unmanned aerial vehicle and flight area planning method and device therefor and storage medium |
| CN112596549A (en) * | 2020-12-29 | 2021-04-02 | 中山大学 | Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule |
-
2021
- 2021-05-24 CN CN202110567745.6A patent/CN113296536B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105929844A (en) * | 2016-04-26 | 2016-09-07 | 哈尔滨工业大学 | Obstacle avoidance method for soft landing of object outside earth under multi-obstacle constraint environment |
| US20200012829A1 (en) * | 2016-05-07 | 2020-01-09 | Canyon Navigation, LLC | Navigation Using Self-Describing Fiducials |
| CN105911867A (en) * | 2016-06-16 | 2016-08-31 | 哈尔滨工程大学 | Ship thrust distribution method based on NSGA-II algorithm |
| CN108444482A (en) * | 2018-06-15 | 2018-08-24 | 东北大学 | A kind of autonomous pathfinding barrier-avoiding method of unmanned plane and system |
| CN109828600A (en) * | 2019-01-09 | 2019-05-31 | 北京理工大学 | Time optimal quick three-dimensional obstacle-avoiding route planning method |
| US20200369292A1 (en) * | 2019-05-20 | 2020-11-26 | Tata Consultancy Services Limited | Method and system for path planning |
| WO2020239092A1 (en) * | 2019-05-30 | 2020-12-03 | 深圳市道通智能航空技术有限公司 | Unmanned aerial vehicle and flight area planning method and device therefor and storage medium |
| CN110398980A (en) * | 2019-06-05 | 2019-11-01 | 西安电子科技大学 | A kind of unmanned aerial vehicle group cooperates with the path planning method of detection and avoidance |
| CN110601172A (en) * | 2019-06-20 | 2019-12-20 | 中国电力工程顾问集团西南电力设计院有限公司 | Multi-direct-current coordination controller design method based on convex polyhedron uncertainty |
| CN110632941A (en) * | 2019-09-25 | 2019-12-31 | 北京理工大学 | Trajectory generation method for target tracking of unmanned aerial vehicle in complex environment |
| CN111486851A (en) * | 2020-04-27 | 2020-08-04 | 中国人民解放军国防科技大学 | Method and device for planning short-distance relative motion three-dimensional obstacle avoidance track of spacecraft |
| CN111811511A (en) * | 2020-06-23 | 2020-10-23 | 北京理工大学 | Unmanned aerial vehicle cluster real-time track generation method based on dimension reduction decoupling mechanism |
| CN112596549A (en) * | 2020-12-29 | 2021-04-02 | 中山大学 | Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule |
Non-Patent Citations (10)
| Title |
|---|
| GEOVANNI FLORES-CABALLERO等: "Optimized Path-Planning in Continuous Spaces for Unmanned Aerial Vehicles Using Meta-Heuristics", 《IEEE ACCESS 》 * |
| MICHAEL SZMUK等: "Real-Time Quad-Rotor Path Planning for Mobile Obstacle Avoidance Using Convex Optimization", 《2018 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 * |
| 刘富春等: "小型无人直升机的模型预测控制算法研究", 《控制理论与应用》 * |
| 宋庆恒，郑福春: "基于无人机的物联网无线通信的潜力与方法", 《物联网学报》 * |
| 李鑫等: "基于凸优化的有限推力远程转移轨迹优化", 《航天控制》 * |
| 王劲博等: "火箭返回着陆问题高精度快速轨迹优化算法", 《控制理论与应用》 * |
| 罗诚: "无人机路径规划算法研究", 《中国优秀博硕士学位论文全文数据库(硕士)基础科学辑》 * |
| 谭峰,等: "高超声速再入飞行器基于凸优化的模型预测轨迹跟踪控制", 《第三十二届中国控制会议论文集（C卷）中国自动化学会控制理论专业委员会会议论文集》 * |
| 陈光荣，等: "凸优化与A*算法结合的路径避障算法", 《控制与决策》 * |
| 韩月起等: "基于凸近似的避障原理及无人驾驶车辆路径规划模型预测算法", 《自动化学报》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113296536B (en) | 2022-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110320933B (en) | Unmanned aerial vehicle obstacle avoidance movement planning method under cruise task | |
| CN109828600B (en) | Time-optimal rapid three-dimensional obstacle avoidance path planning method | |
| CN108958285B (en) | Efficient multi-unmanned aerial vehicle collaborative track planning method based on decomposition idea | |
| Goerzen et al. | A survey of motion planning algorithms from the perspective of autonomous UAV guidance | |
| CN110989626B (en) | Unmanned aerial vehicle path planning method based on control parameterization | |
| WO2018176596A1 (en) | Unmanned bicycle path planning method based on weight-improved particle swarm optimization algorithm | |
| CN111780777A (en) | Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning | |
| Patel et al. | Trajectory generation for aircraft avoidance maneuvers using online optimization | |
| CN110320930A (en) | The reliable transform method of multiple no-manned plane flight pattern based on Voronoi diagram | |
| CN110580740B (en) | Multi-agent cooperative three-dimensional modeling method and device | |
| CN114610066A (en) | Method for generating formation flight tracks of distributed cluster unmanned aerial vehicles in complex unknown environment | |
| CN114967744A (en) | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance | |
| CN107632616A (en) | A kind of unmanned plane collaboration paths planning method based on three-dimensional space curve | |
| CN111746523A (en) | Vehicle parking path planning method and device, vehicle and storage medium | |
| Ma et al. | A Fast path re-planning method for UAV based on improved A* algorithm | |
| CN114326810B (en) | Obstacle avoidance method of unmanned aerial vehicle in complex dynamic environment | |
| CN113296536B (en) | Unmanned aerial vehicle three-dimensional obstacle avoidance algorithm based on A-star and convex optimization algorithm | |
| Wu et al. | A Non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning | |
| Niendorf et al. | Multi-query path planning for an unmanned fixed-wing aircraft | |
| Liu | Motion planning for micro aerial vehicles | |
| Chen et al. | A two-stage method for UCAV TF/TA path planning based on approximate dynamic programming | |
| CN114879676A (en) | Multi-robot formation form changing and dynamic obstacle avoiding method | |
| Chen et al. | From topological map to local cognitive map: a new opportunity of local path planning | |
| CN113885567A (en) | Multi-unmanned aerial vehicle collaborative path planning method based on conflict search | |
| Chu et al. | Track planning of multi-rotor unmanned aerial vehicle in the complex environment space |
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 |