阅读说明：本技术 一种快速无人机路径重构方法 (Rapid unmanned aerial vehicle path reconstruction method ) 是由 郑多 裴培 林德福 宋韬 李斌 范世鹏 于 2020-07-14 设计创作，主要内容包括：本发明公开了一种快速无人机路径重构方法,该方法在探测到路径中存在障碍物时,将障碍物模拟成椭圆形,利用凸优化在求解优化问题的全局优化和快速求解的特性,快速重构出可以规避椭圆形障碍物的新的规划路径,为了确保新的规划路径的准确与可靠性,再通过迭代的方式进一步修正该规划路径,直至基本达到稳态为止。(The invention discloses a rapid unmanned aerial vehicle path reconstruction method, which is characterized in that when an obstacle exists in a path, the obstacle is simulated into an ellipse, a new planned path capable of avoiding the elliptical obstacle is rapidly reconstructed by utilizing the characteristics of global optimization and rapid solution of convex optimization in solving an optimization problem, and the planned path is further modified in an iteration mode until a steady state is basically achieved in order to ensure the accuracy and reliability of the new planned path.)

1. A rapid unmanned aerial vehicle path reconstruction method is characterized by comprising the following steps:

step 1, detecting whether an obstacle exists in an original planned path of an unmanned aerial vehicle in real time through a detector installed on the unmanned aerial vehicle;

step 2, when finding that an obstacle exists in the original planned path, detecting and obtaining the circle center position of the obstacle, the major axis radius and the minor axis radius of the obstacle;

and 3, obtaining a new planning path through reconstruction.

2. The fast drone path reconstruction method of claim 1,

in said step 3, a new planned path (x) is to be obtained_v[i]，y_v[i]) Satisfying constraints in the following equations (one) and (two):

and the new planned path (x) to be obtained_v[i]，y_v[i]) The objective function J can be minimized, and the minimum-taking objective function is described in the following equation (three):

wherein, χ_c[i]Representing cos (psi)_v)[i]，χ_s[i]Denotes sin (psi)_v)[i]V denotes the current speed of the drone, χ_a[i]To representa_maxIndicating maximum allowable acceleration of the drone, #_vmaxRepresenting the maximum allowable flight azimuth of the drone,

x_v[i]representing the X-axis coordinate, y, at a discrete point i in the new planned path_v[i]Representing the Y-axis coordinate at discrete point i in the new planned path,

x_v[i+1]representing the X-axis coordinate, y, at discrete point i +1 in the new planned path_v[i+1]Denotes the Y-axis coordinate, χ, at discrete point i +1 in the new planned path_s[i+1]Representing the sine value of the velocity azimuth at discrete point i +1 in the new planned path;

t_dwhich represents a discrete step of time in size,

representing the Y-axis coordinate of the Nth discrete point in the original planned path;

A₁、A₂and A₃Respectively, represent intermediate variables of the trajectory planning calculation.

3. The fast drone path reconstruction method of claim 2,

A₁、A₂and A₃Are obtained by the following formula:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the original planned path,representing the Y-axis coordinate at a discrete point i in the original planned path;

x_nx-axis coordinate, y, representing the center of the n-th obstacle_nY-axis coordinates representing the center of the nth obstacle area;

a_nlong semi-axis representing the nth obstacle, b_nThe minor axis of the nth obstacle is indicated.

4. The fast drone path reconstruction method of claim 3,

r_nobtained by the following formula:

5. the fast drone path reconstruction method of claim 2,

in step 3, after obtaining the new planned path, iterating the obtained new planned path so as to further optimize the planned path and obtain the final planned path.

6. The fast drone path reconstruction method of claim 5,

the iterative process comprises the following sub-steps:

substep 1, solving out the intermediate variable A according to the obtained new planning path₁′、A₂' and A₃′；

Substep 2, converting the intermediate variable A₁′、A₂' and A₃' into the constraint equation (five),

by making a new planned path (x) to be obtained_v[i]，y_v[i]) Satisfies the constraint equations (one) and (five), and the new planned path (x) to be obtained_v[i]，y_v[i]) The target function J can take the minimum value to obtain the new planning path to be obtained;

the minimum-value-taking objective function is described in the following formula (three):

and 3, repeating the substep 1 and the substep 2, and solving the deviation between two adjacent planning paths in real time to determine whether the iteration is terminated.

7. The fast drone path reconstruction method of claim 6,

in the substep 3, if the deviation value between two adjacent planned paths is not less than the set value epsilon, the new planned path obtained by the last iteration is used for resolving the intermediate variable A₁′、A₂' and A₃', continue to repeat substep 1; if the deviation value between two adjacent planned paths is smaller than a set value epsilon, namely the following formula (four) is established, stopping iteration, and obtaining a new planned path as a final planned path through the last iteration;

||Y^k-Y^k-1| < epsilon (four)

Wherein k denotes the number of iterations, Y^kRepresenting the planned path resulting from the k-th iteration,

preferably, epsilon has a value of

8. The fast drone path reconstruction method of claim 6,

intermediate variable A₁′、A₂' and A₃' byObtained by the following formula:

intermediate variable r_n' is obtained by the following formula:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the planned path from the last iteration,and representing the Y-axis coordinate of the discrete point i in the planning path obtained in the last iteration.

Technical Field

The invention relates to automatic path planning of an unmanned aerial vehicle, in particular to a rapid unmanned aerial vehicle path reconstruction method.

Background

When the unmanned aerial vehicle flies according to a preset flight path, due to the fact that obstacles may appear suddenly due to environment changes, the unmanned aerial vehicle needs to reconstruct the preset flight path in the flying process and perform flying operation according to a new path.

However, in the prior art, the re-planning of the path requires much information, the resolving process is complex, resolving time is long, and it is difficult to provide a new planned path in real time; and the automatically planned path is greatly influenced by the overall dimension of the obstacle, and often needs to go around a farther path, consuming more time, and avoiding the obstacle in the actual working process generally by a manual control mode, so that the automation degree is low.

For the reasons, the inventor of the present invention has made an intensive study on the existing path planning method for unmanned aerial vehicles, and is expected to design a new path planning method capable of solving the above problems.

Disclosure of Invention

In order to overcome the problems, the inventor of the invention carries out intensive research and designs a rapid unmanned aerial vehicle path reconstruction method, when an obstacle exists in a path, the method simulates the obstacle into an ellipse, utilizes the characteristics of global optimization and rapid solution of convex optimization in solving an optimization problem to rapidly reconstruct a new planned path which can avoid the elliptical obstacle, and further corrects the planned path in an iterative mode until the planned path basically reaches a steady state in order to ensure the accuracy and reliability of the new planned path, thereby completing the invention.

Specifically, the invention aims to provide a rapid unmanned aerial vehicle path reconstruction method, which comprises the following steps:

step 1, detecting whether an obstacle exists in an original planned path of an unmanned aerial vehicle in real time through a detector installed on the unmanned aerial vehicle;

and 2, detecting and acquiring the circle center position of the obstacle, the major axis radius and the minor axis radius of the obstacle when the obstacle exists in the original planned path.

And 3, obtaining a new planning path through reconstruction.

Wherein in said step 3 a new planned path (x) is to be obtained_v[i]，y_v[i]) Satisfying constraints in the following equations (one) and (two):

and the new planned path (x) to be obtained_v[i]，y_v[i]) The minimum value of the objective function can be obtained, and the minimum value of the objective function is described in the following formula (three):

wherein, χ_c[i]Representing cos (psi)_v)[i]，χ_s[i]Denotes sin (psi)_v)[i]V denotes the current speed of the drone, χ_a[i]To representa_maxIndicating maximum allowable acceleration of the drone, #_vmaxRepresenting the maximum allowable flight azimuth of the drone,

x_v[i]representing the X-axis coordinate, y, at a discrete point i in the new planned path_v[i]Representing the Y-axis coordinate at discrete point i in the new planned path,

x_v[i+1]representing the X-axis coordinate, y, at discrete point i +1 in the new planned path_v[i+1]Denotes the Y-axis coordinate, χ, at discrete point i +1 in the new planned path_s[i+1]Representing the sine value of the velocity azimuth at discrete point i +1 in the new planned path;

t_drepresenting discrete time steps;

representing the Y-axis coordinate at the Nth discrete point in the original planned pathMarking;

A₁、A₂and A₃Respectively, represent intermediate variables of the trajectory planning calculation.

Wherein A is₁、A₂And A₃Obtained by the following formula:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the original planned path,representing the Y-axis coordinate at a discrete point i in the original planned path;

x_nx-axis coordinate, y, representing the center of the n-th obstacle_nY-axis coordinates representing the center of the nth obstacle area;

a_nlong semi-axis representing the nth obstacle, b_nThe minor axis of the nth obstacle is indicated.

Wherein r is_nObtained by the following formula:

in step 3, after obtaining a new planned path, iterating the obtained new planned path so as to further optimize the planned path and obtain a final planned path.

Wherein the iterative process comprises the sub-steps of:

substep 1, solving out the intermediate variable A according to the obtained new planning path₁′、A₂' and A₃′；

Substep 2, converting the intermediate variable A₁′、A₂' and A₃' into the constraint equation (five),

by making a new planned path (x) to be obtained_v[i]，y_v[i]) Satisfies the constraint equations (one) and (five), and the new planned path (x) to be obtained_v[i]，y_v[i]) The target function J can take the minimum value to obtain the new planning path to be obtained;

the minimum-value-taking objective function is described in the following formula (three):

and 3, repeating the substep 1 and the substep 2, and solving the deviation between two adjacent planning paths in real time to determine whether the iteration is terminated.

In substep 3, if the deviation value between two adjacent planned paths is not less than the set value epsilon, the new planned path obtained by the last iteration is used for resolving the intermediate variable A₁′、A₂' and A₃', continue to repeat substep 1; if the deviation value between two adjacent planned paths is smaller than a set value epsilon, namely the following formula (four) is established, stopping iteration, and obtaining a new planned path as a final planned path through the last iteration;

||Y^k-Y^k-1| < epsilon (four)

Wherein k denotes the number of iterations, Y^kRepresenting the planned path resulting from the k-th iteration,

preferably, epsilon has a value of

Wherein the intermediate variable A₁′、A₂' and A₃' is obtained by the following formula:

intermediate variable r_n' is obtained by the following formula:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the planned path from the last iteration,and representing the Y-axis coordinate of the discrete point i in the planning path obtained in the last iteration.

The invention has the advantages that:

the rapid unmanned aerial vehicle path reconstruction method provided by the invention has the advantages of simple calculation process and rapid calculation, and can realize solution within 1 second to obtain a new planned path, thereby realizing real-time planning control.

Drawings

Fig. 1 shows an overall logic flow diagram of a fast unmanned aerial vehicle path reconstruction method according to the invention;

fig. 2 shows a schematic diagram of a planned path and an obstacle in an experimental example according to the present invention.

Detailed Description

The invention is explained in more detail below with reference to the figures and examples. The features and advantages of the present invention will become more apparent from the description.

The word "exemplary" is used exclusively herein to mean "serving as an example, embodiment, or illustration. Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. While the various aspects of the embodiments are presented in drawings, the drawings are not necessarily drawn to scale unless specifically indicated.

According to the fast unmanned aerial vehicle path reconstruction method provided by the invention, as shown in fig. 1, the method comprises the following steps:

step 1, detecting whether an obstacle exists in an original planned path of an unmanned aerial vehicle in real time through a detector installed on the unmanned aerial vehicle;

step 2, when finding that an obstacle exists in the original planned path, detecting and obtaining the circle center position of the obstacle, the major axis radius and the minor axis radius of the obstacle, and if the obstacle is not found, controlling the aircraft to continuously fly along the original planned path until the target position is reached;

and 3, obtaining a new planning path through reconstruction.

Preferably, the detector specifically includes one or more of a radar detector, an infrared sensing detector, an ultrasonic detector, a laser distance sensor, and a binocular vision detector.

In a preferred embodiment, in step 1, the unmanned aerial vehicle is loaded with a map and a flight path in the unmanned aerial vehicle before takeoff, models the environment through a detector in real time during the flight process, observes whether a modeled obstacle is on the flight path or not in an absolute coordinate system of the map, and performs path planning again if the obstacle is on the flight path.

In a preferred embodiment, in step 2, the position and direction of the obstacle are detected by a radar, a binocular vision detection system is formed by two cameras to detect the distance, or a laser distance sensor is used to detect the distance, a camera is used to take a picture of the obstacle, the outline of the obstacle is analyzed, the obstacle is rounded into an ellipse according to the maximum outline, the circle center position, the major axis radius and the minor axis radius of the obstacle are further obtained, and the obstacle is ensured to fall into the ellipse completely in the rounding process.

The paths referred to in the present application are all paths on a two-dimensional plane, and the corresponding coordinate points and obstacles are also all coordinate points and obstacles on the two-dimensional plane.

In the application, each planned path is dispersed into N points, and the value of N is related to the length of the path and is generally 100-500.

In a preferred embodiment, in said step 3, the new planned path to be obtained consists of (x)_v[i]，y_v[i]) Represents; wherein x is_v[i]Representing the X-axis coordinate, y, at discrete point i in the new planned path_v[i]Representing the Y-axis coordinate at discrete point i in the new planned path; discrete point i represents any one discrete point in the planned path.

The new planned path (x) to be obtained_v[i]，y_v[i]) Satisfying constraints in the following equations (one) and (two):

and a standNew planned path (x) to be obtained_v[i]，y_v[i]) The objective function J can be minimized, and the minimum-taking objective function is described in the following equation (three):

wherein, χ_c[i]Representing cos (psi)_v)[i]I.e. the azimuth of the velocity ψ of a discrete point i in the new planned path_vCosine value of (d); chi shape_s[i]Denotes sin (psi)_v)[i]I.e. the azimuth of the velocity ψ of a discrete point i in the new planned path_vThe sine value of (d); v represents the current speed of the unmanned aerial vehicle, and is obtained through a GPS (global positioning system) arranged on the unmanned aerial vehicle and the inertial navigation of the unmanned aerial vehicle; chi shape_a[i]To representa_maxThe maximum allowable acceleration of the unmanned aerial vehicle is represented and is a set value of an unmanned aerial vehicle control system; psi_vmaxThe maximum allowable flight azimuth angle of the unmanned aerial vehicle is represented and is a set value of the unmanned aerial vehicle control system; x is the number of_vX-axis coordinates representing discrete coordinate locations in the new planned path; y is_vY-axis coordinates representing discrete coordinate locations in the new planned path; y is_v[1]Representing the Y-axis coordinate, Y, at the first discrete point in the new planned path_v[2]Representing the Y-axis coordinate, Y, at a second discrete point in the new planned path_v[N]Representing the Y-axis coordinate at the Nth discrete point in the new planned path; cos psi_vA cosine value representing a velocity azimuth;an angular velocity representing an azimuth of velocity; chi shape_s[i]Representing the velocity azimuth sine value at discrete point i in the new planned path.

t_dRepresenting discrete time steps by estimated time of flight t_fDividing the number by the number N of the discrete points to obtain the number; the estimated time of flight t_fThe remaining distance in the planned path is divided by the speed of the aircraftAnd (4) obtaining the final product.

x_v[i+1]Representing the X-axis coordinate, y, at discrete point i +1 in the new planned path_v[i+1]Denotes the Y-axis coordinate, χ, at discrete point i +1 in the new planned path_s[i+1]Representing the velocity azimuth sine value at discrete point i +1 in the new planned path.

Representing the Y-axis coordinate at the first discrete point in the original planned path,representing the Y-axis coordinate at the second discrete point in the original planned path,representing the Y-axis coordinates at the nth discrete point in the original planned path.

A₁、A₂And A₃Intermediate variables respectively representing trajectory planning calculations are obtained by:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the original planned path,representing discrete points i in the original planned pathY-axis coordinates of (d);

x_nthe X-axis coordinate of the circle center of the nth obstacle is obtained through modeling of a detector; y is_nAnd Y-axis coordinates representing the circle center of the nth obstacle area are obtained through modeling of the detector.

a_nLong semi-axis representing the nth obstacle, b_nA minor axis representing the nth obstacle, a when there are a plurality of obstacles_nN in (1) represents the nth obstacle, and a group A is calculated corresponding to each obstacle₁、A₂And A₃Each group A₁、A₂And A₃Both correspond to a set of constraints of equations (one) and (two), i.e. n obstacles correspond to n sets of constraints of equations (one) and (two).

r_nRepresenting intermediate variables in the trajectory planning process, obtained by:

in a preferred embodiment, in step 3, after obtaining a new planned path, iterating the obtained new planned path so as to further optimize the planned path to obtain a final planned path; and when the deviation between two new planned paths obtained by two adjacent iterations is smaller than a set value, the iteration is stopped, and the last planned path is the final planned path.

The iterative process comprises the following sub-steps:

substep 1, solving out the intermediate variable A according to the obtained new planning path₁′、A₂' and A₃′，

Wherein r is_n' denotes an intermediate variable in the trajectory planning process, obtained by:

wherein the content of the first and second substances,representing the X-axis coordinate at discrete point i in the planned path from the last iteration,and representing the Y-axis coordinate of the discrete point i in the planning path obtained in the last iteration.

x_nThe X-axis coordinate of the circle center of the nth obstacle is obtained through modeling of a detector; y is_nAnd Y-axis coordinates representing the circle center of the nth obstacle area are obtained through modeling of the detector.

a_nLong semi-axis representing the nth obstacle, b_nA minor axis representing the nth obstacle, a when there are a plurality of obstacles_nN in (1) represents the nth obstacle, and a group A is calculated corresponding to each obstacle₁′、A₂' and A₃', each group A₁′、A₂' and A₃' both correspond to a set of constraints of formula (one) and formula (two), i.e. n obstacles correspond to n sets of constraints of formula (one) and formula (two);

substep 2, converting the intermediate variable A₁′、A₂' and A₃' into the constraint equation (five),

by making a new planned path (x) to be obtained_v[i]，y_v[i]) Satisfies the constraint equations (one) and (five), and the new planned path (x) to be obtained_v[i]，y_v[i]) Can make it possible toObtaining a target function J, taking the minimum value, and obtaining a new planning path to be obtained;

the minimum-value-taking objective function is described in the following formula (three):

and a substep 3 of calculating the deviation between two adjacent planned paths in real time, and if the deviation value between the two adjacent planned paths is not less than a set value epsilon, calculating an intermediate variable A by using a new planned path obtained by the last iteration₁′、A₂' and A₃', repeating substep 1; and if the deviation value between two adjacent planned paths is smaller than the set value epsilon, stopping iteration when the following formula (four) is satisfied, and obtaining a new planned path as a final planned path through the last iteration.

||Y^k-Y^k-1| < epsilon (four)

Wherein k denotes the number of iterations, Y^kRepresenting the planned path, Y, resulting from the k-th iteration^k-1Representing the planned path from the (k-1) th iteration,

namely, it isFor example, the following steps are carried out:

planned path obtained by first iteration

Planned path obtained by second iteration

Planned path obtained by iteration of (k-1)

Planned path obtained by k iteration

In the present invention, preferably, epsilon is a valueIn meters, when the set value epsilon is (10, 10), that isWhen the current is over;

the above formula (IV) can be equivalently rewritten as:

namely, it isThe maximum value of the N terms is less than 10, andwhen the maximum value of the N terms is also smaller than 10, the above inequality holds.

By setting the iteration and the iteration termination condition, the accuracy of path planning can be considered, the iteration time can be greatly shortened, the time from finding the obstacle to obtaining the final planned path is controlled within 1 second, and the unmanned aerial vehicle can fly along the corresponding new planned path according to the obstacle information in real time.

Experimental example:

the starting point of the originally planned path of the unmanned aerial vehicle is (0,0), and the end point of the planned path is (40km,30 km); the trajectory of the original planned path is shown in fig. 2 as a thin solid line.

There is an obstacle in the planned path, which is AZ as shown in fig. 2₁、AZ₂、AZ₃、AZ₄、AZ₅The specific parameters are as follows:

numbering	Center coordinate (km)	Long axis (Km)	Short axis (Km)
				1	(15，10)	5	9
2	(25，10)	3.5	3.5
				3	(35，11.5)	5	1.5
4	(25，0)	6	5
				5	(26.5，20)	8	5

After the unmanned aerial vehicle takes off from the starting point, the obstacle is detected in real time, and when the obstacle exists in the original planned path, a new planned path (x) is reconstructed_v[i]，y_v[i]) Such that (x)_v[i]，y_v[i]) Satisfies the constraint formula (one) and the constraint formula (two);

and planning a path (x)_v[i]，y_v[i]) The minimum value of the objective function can be obtained, and the minimum value of the objective function is described in the following formula (three):

wherein the intermediate variable A₁、A₂And A₃Resolving through an original planning path;

after a new planning path is obtained, the constraint is carried out through the constraint formula (one) and the constraint formula (five), so that the objective function takes the minimum value, the reconstruction iteration is carried out step by step,

wherein the intermediate A₁′、A₂' and A₃' calculation of the planned path obtained by the last iteration, when two adjacent planned paths are obtainedWhen the deviation value is less than the set value (9.85,8.33), the iteration stops.

The time taken to obtain the final planned path, which is shown by the dashed line in fig. 2, is 0.9 seconds.

Comparative example:

the starting point of the original planned path of the unmanned plane is (0,0), the end point of the planned path is (40km,30km), the obstacles existing in the planned path are the same as those in the experimental example,

a new planned path is obtained using GPOPS (general non-linear programming solver) in 1052.4 seconds, and the obtained planned path is shown by a dotted line in fig. 2.

As can be seen from a comparison between the dotted line and the dashed line in fig. 2, the method provided by the present application can achieve a path planning effect similar to that of a general nonlinear programming solver; the new planned path obtained by the method perfectly avoids the obstacle, the task requirement of obstacle avoidance can be met, the resolving time is short, the resolving time is shortened from 1052.4 seconds of a general nonlinear programming solver to 0.9 second, and the resolving efficiency is improved by over thousand times, so that the method can be applied to an aircraft for real-time planning processing.

The present invention has been described above in connection with preferred embodiments, but these embodiments are merely exemplary and merely illustrative. On the basis of the above, the invention can be subjected to various substitutions and modifications, and the substitutions and the modifications are all within the protection scope of the invention.