CN112068586B - Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method - Google Patents
Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method Download PDFInfo
- Publication number
- CN112068586B CN112068586B CN202010774434.2A CN202010774434A CN112068586B CN 112068586 B CN112068586 B CN 112068586B CN 202010774434 A CN202010774434 A CN 202010774434A CN 112068586 B CN112068586 B CN 112068586B
- Authority
- CN
- China
- Prior art keywords
- track
- cost
- polynomial
- time
- adjustment
- 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
- 238000005457 optimization Methods 0.000 title claims abstract description 62
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000005265 energy consumption Methods 0.000 claims abstract description 15
- 238000009826 distribution Methods 0.000 claims abstract description 6
- 239000006185 dispersion Substances 0.000 claims abstract description 3
- 230000001133 acceleration Effects 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 5
- 230000006870 function Effects 0.000 description 18
- 238000010586 diagram Methods 0.000 description 9
- 238000004590 computer program Methods 0.000 description 7
- 230000009466 transformation Effects 0.000 description 4
- 230000036461 convulsion Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003321 amplification Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012733 comparative method Methods 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64C—AEROPLANES; HELICOPTERS
- B64C27/00—Rotorcraft; Rotors peculiar thereto
- B64C27/04—Helicopters
- B64C27/08—Helicopters with two or more rotors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
- B64U10/00—Type of UAV
- B64U10/10—Rotorcrafts
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
- B64U2201/00—UAVs characterised by their flight controls
- B64U2201/10—UAVs characterised by their flight controls autonomous, i.e. by navigating independently from ground or air stations, e.g. by using inertial navigation systems [INS]
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Mechanical Engineering (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention discloses a space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method, which comprises the steps of searching a front end path to obtain discrete path points, distributing time intervals to each section of path by adopting a trapezoidal time distribution method, and optimizing by using an unconstrained polynomial coefficient to obtain an initial trajectory path; based on the duration cost and the energy consumption cost of the initial track, carrying out proportion adjustment on the time intervals under the condition of considering the constraint, solving the optimal adjustment proportion and updating the time intervals of all track sections; then alternately performing two links of unconstrained polynomial coefficient optimization and proportional adjustment time interval, and finishing the circulating step when the track cost tends to converge; the whole track path is subjected to integral proportion adjustment, the whole track is ensured to meet constraint conditions, and a final optimized track is obtained; and controlling the aircraft to follow the track by using the path points obtained by the time dispersion of the polynomial track. The invention not only realizes the space-time joint optimization of the track, but also greatly improves the calculation rate.
Description
Technical Field
The invention belongs to the technical field of navigation track planning of four-rotor unmanned aerial vehicles, and particularly relates to a track planning method of a four-rotor unmanned aerial vehicle based on space-time joint optimization.
Background
In recent decades, quad-rotor unmanned aerial vehicles have gained research interest with the development of various new technologies, such as integrated circuits, micro-electromechanical systems, new control theories and methods, etc. The quad-rotor unmanned aerial vehicle is small in size, flexible in movement, capable of taking off and landing vertically, capable of hovering in the air and free of alternatives. In addition, the perfection of communication technology, control principle, SLAM algorithm, path planning algorithm and the like gradually improves the performance index and various capabilities of the quad-rotor unmanned aerial vehicle, and besides the application in the military field, the application of the quad-rotor unmanned aerial vehicle has been expanded to a plurality of fields such as earthquake relief, agricultural plant protection, movie and television shooting.
The path planning of the four-rotor unmanned aerial vehicle can be generally divided into a front-end path search part and a rear-end trajectory planning part. Aiming at the problem of planning the track at the rear end of the four rotors, the QP problem of the minimum trace snap integral is modeled based on the segmented polynomial track in the prior art, a closed solution of the optimal boundary condition of the polynomial track is derived, and the key point of the closed solution is concentrated on the track space optimization part; the method has the advantages that the path search under the constraint of front-end dynamics is combined with the optimization of the B-spline curve at the rear end, the dynamics constraint of the track is ensured by adjusting the node length of the non-uniform B-spline curve, the optimization of the track time period is neglected, and the optimality of the track after the node length adjustment cannot be ensured; and a space-time joint optimal trajectory generation method is proposed and proved for the first time by circularly and alternately carrying out constrained polynomial boundary condition optimization and constrained polynomial time interval optimization based on the segmented polynomial. But the rate of track generation is slowed down due to the complex solution of the two constrained optimization problems and the cyclic optimization structure of the method.
In summary, although various solutions already exist in the field of trajectory planning, a real-time and fast trajectory generation method based on space-time joint optimization is still lacking.
Disclosure of Invention
The technical problem to be solved by the invention is to provide a trajectory planning method for a quad-rotor unmanned aerial vehicle based on space-time joint optimization, based on a piecewise polynomial model, decoupling polynomial coefficient optimization and polynomial time interval optimization, and redesigning the polynomial time interval optimization method, so that space-time joint optimization of the trajectory is realized, and meanwhile, the calculation rate is greatly improved.
The invention adopts the following technical scheme:
a space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method comprises the following steps:
s1, searching the front end path to obtain discrete path points, and distributing time interval T to each path by adopting a trapezoidal time distribution methodiObtaining an initial track path through unconstrained polynomial coefficient optimization;
s2, carrying out segmentation processing on the initial track path obtained in the step S1, and considering constraint and time interval T based on duration cost and energy consumption cost of the initial trackiCarrying out proportion adjustment, solving the optimal adjustment proportion and updating the time intervals of all track sections;
s3, alternately performing two links of unconstrained polynomial coefficient optimization and proportional adjustment time interval, and ending the circulation step when the track cost tends to converge;
and S4, performing integral proportion adjustment on the whole track path to ensure that the whole track meets the constraint condition to obtain a final optimized track, and controlling the aircraft to follow the track by using the path points obtained by time dispersion of the polynomial track.
Specifically, in step S1, the trajectory of the quad-rotor unmanned aerial vehicle is represented based on a piecewise polynomial, and the three-dimensional trajectory of each unmanned aerial vehicle is represented as:
P(t)＝Aβ(t),t∈[0,T]
wherein A ∈ R3×(N+1)Is a polynomial coefficient matrix, N is a polynomial order, and beta (t) ═ tN...t2,t,1)TIs the polynomial basis function and T is the time interval of the corresponding segment polynomial.
Specifically, in step S2, a certain initial trajectory is set to pi(T) calculating a cost function, scaling T for each polynomial trace segmentiIs kTiAnd solving the optimal adjustment proportion best _ k under the condition that the constraint conditions v (t) is less than or equal to v _ max and a (t) is less than or equal to a _ max are met, and updating the polynomial time interval according to the obtained optimal proportion best _ k.
Further, the optimal ratio best _ k is:
wherein, k _ light is the critical ratio of each track time interval adjustment, cost _ k is the track duration cost, and cost _ d is the track energy consumption cost.
Still further, the present invention is characterized in that T is proportionally adjustediIs kTiThen, the corresponding trajectory cost function is:
cost_t'＝kcost_t
the cost _ t 'is a track duration cost after the proportion adjustment, the cost _ t is a track duration cost, the cost _ d' is a track energy consumption cost after the proportion adjustment, and the cost _ d is a track energy consumption cost.
Specifically, in step S3, the unconstrained polynomial coefficient optimization step keeps the current polynomial time interval unchanged, and solves the polynomial coefficient that minimizes cost _ d without considering the constraint of the maximum speed and the maximum acceleration, and the optimization problem is expressed as a QP problem solution, and derives a corresponding closed-form optimal solution.
Specifically, in step S4, when the constrained optimal proportional interval adjustment link and the unconstrained polynomial coefficient optimization link are performed in a loop until the reduction of the cost function converges to a predetermined threshold, the overall proportional adjustment link is performed, so that the constraint conditions v (t) v _ max and a (t) a _ max are satisfied on the whole trajectory.
Further, the process of adjusting the overall time proportion of a certain section of track is as follows:
pi(t)＝a5t5+a4t4+a3t3+a2t2+a1t+a0,t∈[0,Ti]
wherein, the track pi' (t) is a locus pi(t) updating the trajectory according to the overall adjustment ratio kf.
Furthermore, the invention is characterized in that the overall adjustment ratio kf is:
kf＝scale,if scale>1
kf＝1,if scale≤1
wherein v _ all _ max is the maximum speed of the whole track, and a _ all _ max is the maximum acceleration of the whole track.
Furthermore, the invention is characterized in that:
compared with the prior art, the invention has at least the following beneficial effects:
the invention relates to a space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method, which realizes space-time joint optimization based on a segmented polynomial trajectory through unconstrained polynomial coefficient optimization and constrained optimal proportion time interval adjustment; meanwhile, closed-form solutions are derived from the unconstrained polynomial coefficient optimization module and the constrained optimal proportion time adjustment module, so that the calculation rate is greatly improved.
Furthermore, based on a piecewise polynomial model, the differential and integral of the track can be solved in a closed manner, the search of an extreme value is easy, the decoupling of the three dimensional tracks is facilitated, and the smoothness of the track is ensured.
Furthermore, a closed-form solution of the optimal time adjustment proportion is derived based on the duration cost and the energy consumption cost of the current track, and the track cost is optimized under the condition that the constraint condition is met.
Further, setting a trajectory cost function as
Meanwhile, the track time cost and the track energy cost are optimized, and the change of the track cost after proportional time adjustment is listed.
Furthermore, polynomial coefficient optimization is carried out on the current track through an unconstrained polynomial coefficient optimization link, track cost is continuously optimized, and meanwhile smoothness of a joint of two sections of tracks is guaranteed.
Further, the optimal proportion time interval adjusting link with constraint and the unconstrained polynomial coefficient optimizing link are circularly carried out, and the track cost is iteratively optimized until the track cost is converged.
Furthermore, the overall regulation proportion is deduced based on the constraint conditions, and the track is ensured to meet the constraint conditions.
Furthermore, for the whole proportion adjustment link, the adjustment can be completed only by modifying the polynomial coefficient and the time interval of each track segment.
In conclusion, the invention not only realizes the space-time joint optimization of the track, but also greatly improves the calculation rate.
The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.
Drawings
FIG. 1 is a flow chart of a trajectory planning method of the present invention;
FIG. 2 is a graph of path cost comparison for the present invention and comparison method;
FIG. 3 is a diagram of the optimization effect of 6 segments of tracks;
fig. 4 is a comparison graph of path planning effects in a random map, wherein (a) is a comparison graph of a trajectory optimization result of the method of the present invention and an existing open source code trajectory optimization result, and (b) is a detail amplification comparison graph of two trajectories.
Detailed Description
Firstly, modeling is carried out aiming at the track optimization problem
The method comprises the following steps of representing the four-rotor unmanned aerial vehicle track based on a piecewise polynomial, wherein each section of three-dimensional track is represented as:
P(t)＝Aβ(t),t∈[0,T]
wherein A ∈ R3×(N+1)Is a polynomial coefficient matrix, N is a polynomial order, and beta (t) ═ tN...t2,t,1)TIs a polynomial basis function, T is the polynomial time interval, and the polynomial boundary condition is expressed as:
when the polynomial time interval T, the initial boundary condition d (0), and the terminal boundary condition d (T) are determined, the polynomial locus of the segment is uniquely determined.
Setting a trajectory cost function as:
wherein m is the total number of track segments, TiFor the time interval of the ith track, the first term in the integral is the track duration cost, the second term is the track energy consumption cost, when j is equal to 2, the acceleration of the track is corresponded, when j is equal to 3, the jerk of the track is corresponded, when j is equal to 4, the snap of the track is corresponded, and the w is correspondedt,wdThe two costs are respectively weighted, and the track optimization requires that the track with the minimum cost function is solved under the condition that constraint conditions v (t) is less than or equal to v _ max and a (t) is less than or equal to a _ max are met.
Referring to fig. 1, the invention provides a space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method, including the following steps:
s1, distributing time interval T to each path by trapezoidal time distribution method for discrete path points obtained by front path searchiObtaining an initial track through unconstrained polynomial coefficient optimization, wherein the obtained track does not meet the constraint of the maximum speed and the maximum acceleration because constraint conditions are not considered;
s2, regarding the initial track, the invention provides a time interval TiThe optimized closed optimization method carries out segmented processing on the whole path, and based on the duration cost and the energy consumption cost of the initial trajectory, the time interval T is considered under the condition of considering the constraintiCarrying out proportion adjustment, solving the optimal adjustment proportion and updating the time intervals of all track sections;
and adjusting the optimal proportion time interval with the constraint based on the existing track, and optimizing the time interval in a segmented manner. Setting a certain initial track as follows:
pi(t)＝a5t5+a4t4+a3t3+a2t2+a1t+a0,t∈[0,Ti]
the principle of the method is illustrated by a one-dimensional polynomial track of 5 th degree, j is 3, and the cost function of the track section is calculated as follows:
cost_t＝ρTi
wherein, cost _ t is the track duration cost, and cost _ d is the track energy consumption cost.
For each section of polynomial locus, proportional adjustment TiIs kTiThe purpose of the constrained optimal scale interval adjustment is to solve the optimal adjustment scale k when the constraint conditions v (t) ≦ v _ max, a (t) ≦ a _ max are satisfied, first calculating the critical scale:
where v _ rmax is the actual maximum velocity of the segment of the trajectory, and a _ rmax is the actual maximum acceleration of the segment of the trajectory.
When the ratio is adjusted by TiIs kTiThen, the corresponding trajectory cost function is:
cost_t'＝kcost_t
the approximation is simplified to obtain:
cost_t'＝kcost_t
the constrained optimal proportion interval adjustment problem then translates into:
s.t.k＞＝k_tight
the closed-form solution is obtained as:
and then, the polynomial time interval is updated according to the obtained optimal proportion best _ k.
S3, alternately performing two links of unconstrained polynomial coefficient optimization and constrained optimal proportion time interval adjustment, and ending the circulation step when the track cost tends to converge;
the unconstrained polynomial coefficient optimization link not only solves the optimal polynomial coefficient under current time distribution based on the track after time interval adjustment of the constrained optimal proportion, but also solves the problem of discontinuous speed and acceleration at the joint of two sections of tracks after time interval adjustment according to the optimal proportion (the optimal proportion obtained by each section of tracks is different).
And in the unconstrained polynomial coefficient optimization link, the current polynomial time interval is kept unchanged, the polynomial coefficient with the smallest cost _ d is solved, at the moment, the optimization problem can be expressed as QP problem solving, the closed optimal solution can be deduced, and the practical application and the deduction are referred to specific implementation examples.
And S4, carrying out integral proportion adjustment on the whole path to ensure that the whole track meets the constraint condition.
And when the optimal proportional time interval adjusting link with constraint and the unconstrained polynomial coefficient optimizing link are circularly carried out until the cost function converges to the specified threshold, carrying out the integral proportional adjusting link to ensure that the constraint condition v (t) is less than or equal to v _ max and the constraint condition a (t) is less than or equal to a _ max is met on the whole track.
And if the maximum speed of the whole track is v _ all _ max and the maximum acceleration of the whole track is a _ all _ max, the integral adjustment proportion kf is as follows:
kf＝scale,if scale>1
kf＝1,if scale≤1
the overall time proportion of the piecewise polynomial is adjusted only by updating the time interval of each piecewise polynomial, and the polynomial coefficient is updated according to the overall adjusting proportion kf. The whole time proportion adjustment process of a certain section of track comprises the following steps:
pi(t)＝a5t5+a4t4+a3t3+a2t2+a1t+a0,t∈[0,Ti]
wherein, the track pi' (t) is a locus pi(t) updating the trajectory according to the overall adjustment ratio kf.
60 points are randomly sampled in a cube with the side length of 40 to serve as a front end path search result, the performance of the method is compared with that of an advanced path planning method, and the comparison result is shown in table 1.
TABLE 1 comparison of Performance between the method of the present invention and the advanced Path planning method
Setting the parameters of the method of the invention as j-3, wt＝1024,w d20, for the randomly generated 60 three-dimensional path points, the method and "Wang Z, Zhou X, Xu C, et al]The space-time joint optimal trajectory generation method based on the piecewise polynomial in 2020' performs trajectory optimization, records 20-time operation results, and compares final trajectory costs as shown in FIG. 2,wherein the dotted line is the result of the comparative method and the solid line is the result of the method of the present invention.
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. The components of the embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Examples
Randomly sampling 6 three-dimensional coordinates in a cube with the side length of 40 as path points, setting the initial speed, the initial acceleration, the initial jerk, the terminal speed, the terminal acceleration and the terminal jerk to be zero, and setting the constraint conditions as follows:
v_max＝7m/s,a_max＝4m/s2
cost function parameter m 6, j 3, wt＝1024,wd＝50。
After the time interval is distributed by using the trapezoidal time distribution method, the derivation steps of the unconstrained polynomial coefficient optimization link are as follows, and the principle is still explained by taking a one-dimensional situation as a principle:
cost _ d of a certain uniaxial fifth-order polynomial locus is:
the optimization problem for the whole track is converted into:
the equality constraint conditions comprise zero state constraint at the initial moment and the terminal moment, path point constraint, and continuous constraint of the speed and the acceleration at the joint of the two sections of tracks. The coefficient matrix A is converted into boundary conditions by a transformation matrix M, and then a transformation matrix C is constructedTDecomposing the boundary condition into a fixed boundary condition dFAnd a free boundary condition dPThe above equality constraints can be transformed by constructing the transformation matrix CTEmbodied, then the optimization problem is converted to minimize cost _ d:
by cost _ d to dpDerivation to determine optimal free boundary conditions
Then, the optimal polynomial coefficient is recovered through the transformation matrix as follows:
and completing a complete polynomial coefficient optimization link without maximum speed and maximum acceleration constraints to obtain an initial track. And then, carrying out optimal proportion adjustment time interval and unconstrained polynomial coefficient optimization through circulation until the track cost tends to converge. And finally, when the integral proportion is adjusted, because the maximum speed constraint and the maximum acceleration constraint are considered in the step of adjusting the time interval of the segmented optimal proportion in the previous optimization step, when the track cost tends to be converged, the maximum speed constraint and the maximum acceleration constraint can reach tight constraint or slightly exceed a specified value, and the influence of the integral proportion adjustment on the whole track cost function can be almost ignored. The result of the 6-segment trajectory optimization at a time is shown in fig. 3.
The complete path planning is simulated: firstly, a random map is established, as shown in fig. 4, after an initial point three-dimensional coordinate and an end point three-dimensional coordinate are set, an initial path point is obtained by using an RRT method, and a white discrete path point is obtained after simplifying the initial path point. And then, optimizing the track by adopting the method, and generating the track by utilizing open source codes in the text' Wang Z, Zhou X, Xu C, et al. It can be seen that in the simulation process, the distance between the front end path points obtained by the RRT method is relatively short, and the actual trajectories obtained by the two methods are basically consistent, but the speed of the method of the present invention is improved by about 10 times.
In conclusion, the trajectory planning method for the four-rotor unmanned aerial vehicle based on space-time joint optimization is based on the piecewise polynomial model, the trajectory cost function is optimized through two links of unconstrained polynomial coefficient optimization and time interval optimal proportion adjustment, the trajectory is guaranteed to meet constraint through integral proportion adjustment, space-time joint optimization of the trajectory is achieved, and meanwhile the calculation rate is greatly improved.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.
Claims (4)
1. A space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method is characterized by comprising the following steps:
s1, representing the four-rotor unmanned aerial vehicle track based on a piecewise polynomial, searching the front end path to obtain discrete path points, and adopting a trapezoidal time distribution methodAssigning a time interval T to each segment of the pathiObtaining an initial trajectory path through a polynomial coefficient optimization link without maximum flight speed and maximum flight acceleration constraints;
s2, carrying out segmentation processing on the initial track path obtained in the step S1, and based on the duration cost and the energy consumption cost of the initial track, considering the maximum flying speed and the maximum flying acceleration constraint and the time interval TiCarrying out proportion adjustment, solving the optimal adjustment proportion and updating the time intervals of all track sections; specifically, let a certain segment of initial trajectory be pi(T) calculating a cost function, scaling T for each polynomial trace segmentiIs kTiK is the time adjustment proportion of a certain polynomial track, the optimal adjustment proportion best _ k under the condition that the constraint conditions v (t) is less than or equal to v _ max and a (t) is less than or equal to a _ max is solved, v (t) is the polynomial track speed, a (t) is the polynomial track acceleration, v _ max is the maximum flight speed of the unmanned aerial vehicle, a _ max is the maximum flight acceleration of the unmanned aerial vehicle, and the polynomial time interval is updated according to the obtained optimal proportion best _ k; the optimal ratio best _ k is:
wherein k _ light is a critical proportion of each track time interval adjustment, cost _ t is track duration cost, and cost _ d is track energy consumption cost;
proportional adjustment of TiIs kTiThen, the corresponding trajectory cost function is:
cost_t'＝kcost_t
the cost _ t 'is track duration cost after the proportion adjustment, the cost _ t is track duration cost, the cost _ d' is track energy consumption cost after the proportion adjustment, and the cost _ d is track energy consumption cost;
s3, alternately carrying out polynomial coefficient optimization without maximum flight speed and maximum flight acceleration constraints and polynomial time interval adjustment with maximum flight speed and maximum flight acceleration constraints, wherein the track cost is the weighted sum of the track duration cost and the track energy consumption cost, and the loop step is ended when the track cost tends to converge;
s4, performing integral proportion regulation of time intervals on the whole segmented polynomial track path to ensure that the whole track meets the constraint of high flight speed and maximum flight acceleration to obtain a final optimized track, and controlling the aircraft to follow the track by using path points obtained by time dispersion of the final optimized track; the whole time proportion adjustment process of a certain section of track comprises the following steps:
pi(t)＝a5t5+a4t4+a3t3+a2t2+a1t+a0,t∈[0,Ti]
wherein, the track pi' (t) is a locus pi(t) updating the trajectory according to the overall adjustment ratio kf.
2. The space-time jointly optimized quad-rotor unmanned aerial vehicle trajectory planning method according to claim 1, wherein in step S1, the quad-rotor unmanned aerial vehicle trajectory is represented based on a piecewise polynomial, and the three-dimensional trajectory of each quad-rotor unmanned aerial vehicle is represented as:
P(t)＝Aβ(t),t∈[0,T]
wherein A ∈ R3×(N+1)Is a polynomial coefficient matrix, N is a polynomial order, and beta (t) ═ tN…t2,t,1)TIs the polynomial basis function and T is the time interval of the corresponding segment polynomial.
3. The space-time joint optimization trajectory planning method for a quadrotor unmanned aerial vehicle according to claim 1, wherein in step S3, the unconstrained polynomial coefficient optimization unit keeps the current polynomial time interval constant, and solves the polynomial coefficient that minimizes trajectory energy consumption cost _ d without considering the constraint of maximum speed and maximum acceleration, and the optimization problem is expressed as QP problem solution, and a corresponding closed optimal solution is derived.
4. The space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method according to claim 1, wherein the overall regulation ratio kf is:
kf＝scale,if scale>1
kf＝1,if scale≤1
wherein v _ all _ max is the maximum speed of the whole track, and a _ all _ max is the maximum acceleration of the whole track.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010774434.2A CN112068586B (en) | 2020-08-04 | 2020-08-04 | Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010774434.2A CN112068586B (en) | 2020-08-04 | 2020-08-04 | Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112068586A CN112068586A (en) | 2020-12-11 |
| CN112068586B true CN112068586B (en) | 2021-08-13 |
Family
ID=73657002
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010774434.2A Active CN112068586B (en) | 2020-08-04 | 2020-08-04 | Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112068586B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022133775A1 (en) * | 2020-12-23 | 2022-06-30 | 深圳元戎启行科技有限公司 | Trajectory data processing method and apparatus, computer device, and storage medium |
| CN114089743B (en) * | 2021-11-01 | 2023-12-12 | 厦门理工学院 | Trajectory optimization method of distribution room mobile robot |
| CN114721433B (en) * | 2022-04-11 | 2024-03-19 | 华南理工大学 | Unmanned aerial vehicle-based collision-free polynomial track generation method |
| CN116185051B (en) * | 2022-09-07 | 2023-09-29 | 浙江大学 | Time optimal track planning method and device based on four-rotor dynamics model |
| CN117250855B (en) * | 2023-11-14 | 2024-02-13 | 安徽大学 | Flying robot track planning method based on multi-objective optimization |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106909164A (en) * | 2017-02-13 | 2017-06-30 | 清华大学 | A kind of unmanned plane minimum time smooth track generation method |
| CN109885088A (en) * | 2019-03-12 | 2019-06-14 | 西安交通大学 | Unmanned plane during flying track optimizing method in edge calculations network based on machine learning |
Family Cites Families (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7623679B2 (en) * | 2006-12-13 | 2009-11-24 | Accuray Incorporated | Temporal smoothing of a deformation model |
| CN102298391A (en) * | 2011-04-27 | 2011-12-28 | 哈尔滨工业大学 | Motion trail planning method for heavy-duty industrial robot in operating space |
| US9292010B2 (en) * | 2012-11-05 | 2016-03-22 | Rockwell Automation Technologies, Inc. | Online integration of model-based optimization and model-less control |
| US9639813B2 (en) * | 2013-04-26 | 2017-05-02 | Disney Enterprises, Inc. | Method and device for three-weight message-passing optimization scheme |
| WO2017056038A1 (en) * | 2015-09-29 | 2017-04-06 | University Of Malta | Fast flight trajectory optimisation for in-flight computation and flight management systems |
| CN105159096B (en) * | 2015-10-10 | 2017-08-29 | 北京邮电大学 | A kind of redundancy space manipulator joint moment optimization method based on particle cluster algorithm |
| CN106909161B (en) * | 2017-01-05 | 2019-10-18 | 浙江大学 | A kind of optimum attitude motor-driven planing method of agility satellite zero drift angle imaging |
| CN107367938A (en) * | 2017-08-10 | 2017-11-21 | 上海理工大学 | One kind is used for mechanical arm time optimal trajectory planning method |
| US10606277B2 (en) * | 2017-09-18 | 2020-03-31 | Baidu Usa Llc | Speed optimization based on constrained smoothing spline for autonomous driving vehicles |
| CN108445898B (en) * | 2018-05-14 | 2021-03-09 | 南开大学 | Four-rotor unmanned aerial vehicle system motion planning method based on differential flatness characteristic |
| CN108549324B (en) * | 2018-05-16 | 2019-07-05 | 山东大学 | Workpiece for high speed sorting system follows crawl method for planning track and system |
| US10823575B2 (en) * | 2018-06-27 | 2020-11-03 | Baidu Usa Llc | Reference line smoothing method using piecewise spiral curves with weighted geometry costs |
| CN110125927A (en) * | 2019-03-18 | 2019-08-16 | 中国地质大学（武汉） | Mechanical arm method for planning track and system based on self-adapted genetic algorithm |
| CN109857134A (en) * | 2019-03-27 | 2019-06-07 | 浙江理工大学 | Unmanned plane tracking control system and method based on A*/minimum_snap algorithm |
| CN110874569B (en) * | 2019-10-12 | 2022-04-22 | 西安交通大学 | Unmanned aerial vehicle state parameter initialization method based on visual inertia fusion |
| CN111113409B (en) * | 2019-11-21 | 2021-05-11 | 东南大学 | Multi-robot multi-station cooperative spot welding planning method based on step-by-step optimization |
-
2020
- 2020-08-04 CN CN202010774434.2A patent/CN112068586B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106909164A (en) * | 2017-02-13 | 2017-06-30 | 清华大学 | A kind of unmanned plane minimum time smooth track generation method |
| CN109885088A (en) * | 2019-03-12 | 2019-06-14 | 西安交通大学 | Unmanned plane during flying track optimizing method in edge calculations network based on machine learning |
Non-Patent Citations (1)
| Title |
|---|
| 多约束条件下的机器人时间最优轨迹规划;钱东海等;《制造业自动化》;20110610(第11期);第7-11页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112068586A (en) | 2020-12-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112068586B (en) | Space-time joint optimization four-rotor unmanned aerial vehicle trajectory planning method | |
| CN110320932B (en) | Formation form reconstruction method based on differential evolution algorithm | |
| CN112631335B (en) | Event triggering-based multi-quad-rotor unmanned aerial vehicle fixed time formation method | |
| CN113919485B (en) | Multi-agent reinforcement learning method and system based on dynamic hierarchical communication network | |
| CN111443728B (en) | Chaos wolf optimization-based unmanned aerial vehicle formation control method | |
| CN104850009A (en) | Coordination control method for multi-unmanned aerial vehicle team based on predation escape pigeon optimization | |
| CN112363532B (en) | Method for simultaneously taking off and gathering multiple unmanned aerial vehicles based on QUATRE algorithm | |
| WO2023197092A1 (en) | Unmanned aerial vehicle path planning method based on improved rrt algorithm | |
| CN116954239B (en) | Unmanned aerial vehicle track planning method and system based on improved particle swarm optimization | |
| CN113253603A (en) | Design method of unmanned aerial vehicle active disturbance rejection controller based on FOPSO algorithm | |
| CN115755956B (en) | Knowledge and data collaborative driving unmanned aerial vehicle maneuvering decision method and system | |
| CN104216289A (en) | Multiple aircraft fleet control method and device based on distributed evolutionary algorithm | |
| CN115454115A (en) | Rotor unmanned aerial vehicle path planning method based on hybrid wolf-particle swarm algorithm | |
| CN111487992A (en) | Unmanned aerial vehicle sensing and obstacle avoidance integrated method and device based on deep reinforcement learning | |
| Shen | Bionic communication network and binary pigeon-inspired optimization for multiagent cooperative task allocation | |
| Wu et al. | Multi-phase trajectory optimization for an aerial-aquatic vehicle considering the influence of navigation error | |
| CN113625767A (en) | Fixed-wing unmanned aerial vehicle cluster collaborative path planning method based on preferred pheromone gray wolf algorithm | |
| CN107203133B (en) | A kind of intelligent soft lunar landing track controller | |
| CN115981375B (en) | Design method of multi-unmanned aerial vehicle time-varying formation controller based on event triggering mechanism | |
| CN116400718A (en) | Unmanned aerial vehicle short-distance air combat maneuver autonomous decision-making method, system, equipment and terminal | |
| CN114815875B (en) | Unmanned aerial vehicle cluster formation controller parameter adjustment method based on intelligent optimization of integrated fully-shooting pigeon clusters | |
| CN116203990A (en) | Unmanned plane path planning method and system based on gradient descent method | |
| CN114253285B (en) | Multi-aircraft collaborative formation gathering method | |
| CN113848982A (en) | Method for planning and tracking control of perching and stopping moving track of quad-rotor unmanned aerial vehicle | |
| Guopeng et al. | Research on Path planning of Three-Dimensional UAV Based on Levy Flight Strategy and Improved Particle Swarm Optimization Algorithm |
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 |