CN115097857A - Real-time trajectory planning method considering appearance of rotor unmanned aerial vehicle in complex environment - Google Patents
Real-time trajectory planning method considering appearance of rotor unmanned aerial vehicle in complex environment Download PDFInfo
- Publication number
- CN115097857A CN115097857A CN202210842336.7A CN202210842336A CN115097857A CN 115097857 A CN115097857 A CN 115097857A CN 202210842336 A CN202210842336 A CN 202210842336A CN 115097857 A CN115097857 A CN 115097857A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- node
- real
- map
- 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
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 230000004888 barrier function Effects 0.000 claims abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 238000011478 gradient descent method Methods 0.000 claims abstract description 5
- 238000005315 distribution function Methods 0.000 claims abstract description 3
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 5
- 238000007689 inspection Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 3
- 206010048669 Terminal state Diseases 0.000 claims 1
- 239000002245 particle Substances 0.000 description 9
- 230000000694 effects Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000008929 regeneration Effects 0.000 description 1
- 238000011069 regeneration method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000007704 transition Effects 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/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, which comprises the steps of collecting the current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupancy probability grid map, and constructing an ESDF (electronic stability and data distribution function) map based on the occupancy probability grid map; by considering a front-end path planning algorithm of a yaw angle, abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls, and inquiring whether the distance between the circle center of each ball and the nearest barrier is smaller than the radius of the ball through an ESDF map to perform collision detection so as to obtain a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision on the whole machine; and abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls in a back-end trajectory optimization algorithm, inquiring distance information between all ball centers and the obstacle and gradient direction information far away from the obstacle from an ESDF map, and optimizing a path by using a gradient descent method. According to the invention, safety constraint models of the unmanned aerial vehicle body are established at the front end and the rear end, and the safe and feasible track is ensured in a complex environment.
Description
Technical Field
The invention relates to the field of trajectory planning of multi-rotor unmanned aerial vehicles, in particular to a real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment.
Background
The real-time autonomous trajectory planning algorithm of the rotor unmanned aerial vehicle is used for generating a trajectory which meets the kinetic/kinematic constraint, smoothness constraint and collision-free constraint of the robot in a complex environment. The algorithm comprises a front-end path planning algorithm and a rear-end track optimization algorithm. The method mainly comprises a search-based path planning algorithm and a sampling-based path planning algorithm. The path planning algorithm based on search is represented by an A-algorithm, the Dijkstra algorithm and a breadth-first algorithm are combined, and the optimal path is searched faster by means of the action of a heuristic function. The sampling-based path planning algorithm takes RRT as a main part, randomly samples in space, connects the nearest sampling point into a path tree and considers whether the nearest sampling point has a better father node or not until an area near an end point is explored. The front-end path planning algorithm provides an optimized initial value for the rear-end trajectory optimization algorithm, so that the trajectory can be quickly converged to an optimal solution under the condition of meeting constraint conditions. In order to save computing resources, the existing rotor unmanned aerial vehicle trajectory planning algorithm equates an unmanned aerial vehicle to mass points, only planning of three axes x, y and z is considered when path planning is carried out, influence of the shape of the unmanned aerial vehicle on the trajectory is not considered, and if the real size of the unmanned aerial vehicle is large, a result that the searched path is infeasible can be caused.
For the trajectory planning of a rotor unmanned aerial vehicle, research in recent years mainly centers on the trajectory planning of a particle model of the unmanned aerial vehicle, and an easy-to-implement and general trajectory planning method considering the appearance of the rotor unmanned aerial vehicle is lacked.
Among the prior art, use protruding polyhedron to represent rotor unmanned aerial vehicle appearance, use the flight corridor to represent the collision that the security restraint avoided with the barrier, satisfy rotor unmanned aerial vehicle's protruding polyhedron and contain in the flight corridor always when planning, then the orbit is decided to be collision-free. And the flight corridor generation algorithm finds the farthest and unlimited barrier-free point along the path from the first point according to the path searched by the front end, generates a convex polyhedron according to the connecting line between the barrier-free point and the path, and circulates for multiple times until the end point is contained in the convex polyhedron. This algorithm is used for rotor unmanned aerial vehicle race match, is applicable to the small-size rotor unmanned aerial vehicle that mobility is stronger, if be used for large-scale unmanned aerial vehicle, then there are following two problems: (1) the front-end path planning algorithm does not consider the actual shape of the unmanned aerial vehicle, and the problem that solution cannot be achieved due to the fact that the front end planned by a particle model is added into the back-end optimization which considers the shape of the unmanned aerial vehicle is solved; (2) when the rear end is optimized in the flight corridor, each point of the convex polyhedron of the airplane must be ensured to be in the same convex hull of the flight corridor, but the point is difficult to ensure for a large unmanned aerial vehicle. And the algorithm is based on the known condition of a global map, and the calculation power is consumed for planning each vertex on the convex polyhedron of the unmanned aerial vehicle.
Therefore, it is necessary to provide a real-time trajectory planning method for a large-sized rotor unmanned aerial vehicle in a narrow and complex environment.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a real-time track planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, the invention fully considers the kinematics/dynamics characteristics of a large rotor unmanned aerial vehicle, and the appearance constraints of the rotor unmanned aerial vehicle are added at the front end and the rear end of the track planning, so that effective and robust real-time local track generation is carried out.
The technical scheme of the invention is as follows: the embodiment of the invention provides a real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, which comprises the following steps:
(1) acquiring current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupation probability grid map, and establishing an ESDF (extended service distribution function) map based on the occupation probability grid map;
(2) by considering a front-end path planning algorithm of a yaw angle, abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls, and checking whether the distance between the circle centers of all the balls and the nearest barrier is smaller than the radius of the ball through the ESDF map established in the step (1) to perform collision detection, so that a path which is in accordance with the kinematic constraint of the unmanned aerial vehicle and has no collision on the whole machine is obtained;
(3) and (3) abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls in a back-end trajectory optimization algorithm, inquiring distance information between all ball centers and the obstacle and gradient direction information far away from the obstacle from an ESDF map, and optimizing the path obtained in the step (2) by using a gradient descent method.
The invention has the beneficial effects that: the invention mainly designs a real-time track planning method considering the appearance of the large rotor unmanned aerial vehicle in a complex environment, improves a track planning algorithm based on a particle point model of the unmanned aerial vehicle, does not expand barriers, and ensures that the algorithm can quickly obtain a safe and feasible track under the condition of considering the appearance constraint of the large rotor unmanned aerial vehicle. Compared with a track planning method based on a particle model, the method for expanding the obstacle by considering the unmanned aerial vehicle as the particle can be seen that a collision path is likely to be searched, and a safe and collision-free track can be obtained under the condition of approximately the same searching time. The path searched by the front end of the invention is more fit with the result of the rear end optimization, and a collision-free track can still be obtained when the obstacle is a corner. The invention provides a general real-time track planning scheme considering the appearance of a large rotor unmanned aerial vehicle, and the safety constraint models of the unmanned aerial vehicle body are established at the front end and the rear end, so that the track is ensured to be safe and feasible in a complex environment. The method analytically expresses the geometric shape of the rotor unmanned aerial vehicle as a combination form of convex geometric bodies. The front end path planning algorithm considers the planning of the yaw angle yaw and carries out the collision check of the shape of the whole machine. And the rear-end track optimization algorithm transmits the gradient at the circle center to the track point by using a chain rule according to the rotation translation relation between the circle center and the mass center of the unmanned aerial vehicle.
Drawings
FIG. 1 is a diagram of a particle model planning effect of a large-scale rotor unmanned aerial vehicle;
FIG. 2 is a schematic diagram of an analytic expression method of rotor drone geometry;
FIG. 3 is a diagram of the effect of front end path planning;
fig. 4 is a diagram of the effect of back-end trajectory optimization.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the invention, as detailed in the appended claims.
The real-time trajectory planning method considering the appearance of the unmanned gyroplane in a complex environment provided by the invention is explained in detail below with reference to the accompanying drawings. The features of the following examples and embodiments may be combined with each other without conflict.
The embodiment of the invention provides a real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment. And establishing a grid map and an ESDF (extended service life distribution) map based on the current local map information and the real-time positioning information of the unmanned aerial vehicle, and quickly detecting the collision with the surrounding environment by abstracting the appearance of the unmanned aerial vehicle into a combination of limited balls. The front-end path searching algorithm obtains a rough, safe and feasible local path, the rear end optimizes the track obtained by the front end, and the track meets the dynamic/kinematic constraint of the unmanned aerial vehicle on the premise of meeting the constraint of the track on the overall safety of the body, namely the track is smoother and feasible.
The method mainly relates to the processing of the front end and the rear end of an algorithm on the integral shape constraint of the unmanned aerial vehicle, and for convenience of introduction, the invention takes a rotor unmanned aerial vehicle with a rectangular shape (the length is 1.2m, and the width is 0.8m) and a negligible z-axis as an example. The method specifically comprises the following steps:
(1) and acquiring the current local map information and real-time positioning information of the unmanned aerial vehicle, and establishing an occupation probability grid map and an ESDF map.
Embodiments of the present invention use occupancy probability grid maps to describe environmental information, which generate maps from measurement data from noisy sensors, such as lidar, binocular cameras, etc., given the knowledge of robot positioning information. The grid map uses a binarization method to represent whether a certain grid is occupied by an obstacle, and the grid m is subjected to i Probability of being occupied is p (m) i |z 1:t ,x 1:t ) It means for a given grid m i And 1 to t times of positioning information x 1:t Probability z of making an observation 1:t . A higher probability value indicates a higher probability that the grid is occupied.
The embodiment of the invention uses an ESDF (European symbolic distance field) map to represent potential field information of the environmental obstacles, and the map is established based on an occupation probability grid map to obtain the distance between each grid and the nearest obstacle. For the three axes x, y and z, distance functions of the grid of the obstacle and the surrounding grid are respectively constructed, the lower envelope curve of the functions is the distance function of the grid and the nearest obstacle, and the lower envelope curve can be expressed as:
wherein p represents the grid of the query, q is the grid where the obstacle is located,
to occupy all grids in a probability grid map, f (q) is a sampling function of q. To obtain the distance and gradient of a point in space from the nearest obstacle, a trilinear interpolation of the eight grids around the point is required.
And updating real-time data occupying a probability grid map and an ESDF (extended dynamic range distribution) map, storing map information in a vector form, and inquiring index values of corresponding grids to obtain the obstacle information when planning a path.
(2) Obtaining a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision of the whole machine by considering a front-end path planning algorithm of a yaw angle; the appearance of the unmanned aerial vehicle is abstracted into a combination of a plurality of balls (taking two balls as an example), and whether the distance between the centers of two circles and the nearest obstacle is smaller than the radius of the circle is checked through the established ESDF map to carry out collision detection.
The method comprises two parts, namely front-end path planning and rear-end track optimization, and according to the differential flat dynamic characteristic of a rotor unmanned plane model, the flat output space of the rotor unmanned plane can be represented by x, y, z and yaw angle yaw. For a general rotor unmanned aerial vehicle, the planning requirements can be met by using a particle model, and for a front-end path planning algorithm of a large rotor unmanned aerial vehicle, if the particle model is used for planning, a path which cannot be passed by the unmanned aerial vehicle actually can be obtained, as shown in fig. 1, so that the overall shape of the unmanned aerial vehicle needs to be considered when path planning is performed. The kinematics of a large-sized rotor unmanned aerial vehicle make it impossible to fly in a large attitude, i.e. to rotate pitch and roll by a large amount, so that only the kinematics constraint of yaw is considered during planning.
In the embodiment of the present invention, the front end path planning algorithm obtains an initial path by using a kinematics-based a-x algorithm considering the yaw angle yaw, specifically:
(2.1) expanding nodes from a starting point, obtaining a plurality of motion primitives based on different control input quantities, and reserving the motion primitives meeting constraint conditions by carrying out safety and dynamics feasibility inspection on the motion primitives;
in a general A-x algorithm, the Euclidean distance from a starting point to a current node is used as a cost function, the Euclidean distance from the current node to an end point is used as a heuristic function, and when searching for the end point each time, a node with the minimum sum of the cost function and the heuristic function in nodes to be expanded is selected as a next path point until the end point is searched. For a dynamics-based A-algorithm, the kinematics characteristic of the unmanned aerial vehicle needs to be considered when nodes are expanded, the motion track can be expressed as a polynomial related to time t, the real-time track planning of the appearance of the rotor unmanned aerial vehicle is considered, the one-dimensional yaw angle planning needs to be added, and four independent one-dimensional time parameterized polynomial equations are used for expression:
p(t)：＝[p x (t)，p y (t)，p z (t)，P yaw (t)] T
wherein p is μ (t) is the time parameterized polynomial equation for each dimension, μ ∈ { x, y, z, yaw }, a k Is a polynomial coefficient and k is a polynomial order. Make it
Is a state vector, such that
For control input, the state space model can be represented as:
the complete solution to the equation of state is expressed as:
it represents an initial state of x (0), a control input of u (t), and a trajectory of the rotorcraft system of x (t).
In the expansion node, given the current state of the rotorcraft, a set of discrete control quantities is input for the duration τ
In the embodiment of the present invention, when n is 2, which indicates that the acceleration of each axis is used as the input, the input of the yaw angle yaw is the angular acceleration. Of each shaft [ -u [ ] max ，u max ]Is uniformly dispersed into
The extension node can get (2r +1) each time 4 A bar motion primitive.
In order to balance the calculation consumption of front-end and back-end planning, r is 2, and each motion element can enable the corresponding node to be added into the extensible node set only through feasibility and security check. The feasibility inspection requires that the speed and the acceleration of track points on the motion elements do not exceed the maximum limit of the motion capability of the unmanned aerial vehicle, the safety inspection requires that the whole motion element has no collision, the distance from each sphere center of the spherical combination to the nearest obstacle is inquired and calculated, and the distance cannot be smaller than the radius of the sphere.
The rapid collision detection is performed by abstracting the appearance of the drone to a combination of a limited number of balls (two balls are taken as an example in the embodiment of the present invention).
In order to meet the real-time performance of track generation, the unmanned aerial vehicle body is abstracted into a combination of two balls for safety check, and the principle is as follows: whether the distance between the circle centers of the two spheres and the nearest barrier is smaller than the radius of the sphere is checked to judge whether collision occurs, and the distance between the sphere centers and the nearest barrier is obtained by inquiring the ESDF map information. Because the calculation complexity of the query operation is 0(n), the collision detection method considering the shape of the unmanned aerial vehicle can achieve a real-time effect.
And (2.2) calculating a node corresponding to each motion element according to the motion elements obtained in the step (2.1), evaluating a cost function and a heuristic function of each node, and adding the node into a node list to be expanded. Taking the node with the minimum sum of the cost function and the heuristic function in the node list to be expanded as a next node; and repeating the steps until the searched node is the end point, and obtaining a path which accords with the kinematics constraint of the unmanned aerial vehicle and has no collision on the whole machine.
The cost function j (t) of the node is designed as a function of the control input u (t) and the time t, i.e.:
where ρ is the weight of time t.
The heuristic function J (Th) of the nodes is designed to solve the problem of optimization from the current node state to the end state, without considering collisions, in order to find the optimal transition time t, namely:
wherein p is μc ，v μc ，，p μg ，v μg For the current node and destination drone position and velocity, by
If the node to be expanded is empty and does not reach the end point, input control quantity of yaw is more finely dispersed, and r is made to be r yaw Regeneration (2r +1) ═ 4 3 *(2r yaw +1) moving element, if there is no expandable node, returning to the previous node to search a path with different topology.
The rough track generated by the A-algorithm based on kinematics with the yaw angle considered at the front end provides a better initial value for the optimization of the track at the rear end, the optimization speed at the rear end is accelerated, and the condition that the optimization fails due to the fact that a general A-algorithm searches an infeasible area is also avoided.
(3) And (3) constructing a non-particle model of the unmanned aerial vehicle through a back-end trajectory optimization algorithm, abstracting the geometric shape of the unmanned aerial vehicle into an analytic expression method, and optimizing the local path obtained in the step (2).
The rear end of the method carries out track optimization based on B splines or MINCO track classes. In the embodiment of the present invention, taking a B-spline as an example, the front-end path may be converted into a pb-order B-spline of { Q _0, Q _1, …, Q _ N } control point, and a trajectory satisfying the constraint condition is obtained by optimizing the position of the control point. The trajectory optimization is based on a gradient descent method, and the gradient direction in which the objective function value is reduced is obtained by deriving the objective function, so that the position of a control point is optimized to minimize the objective function, which is a function related to the control point. Its objective function can be expressed as:
f＝λ s f s +λ c f c +λ(f v +f a )
wherein f is s As a smooth cost function, λ s And obtaining the geometric information of the track to enable the track to be smoother for the coefficient corresponding to the self-defined smooth cost function. f. of c As a function of collision cost, λ c And leading the track to be far away from the barrier for the coefficient corresponding to the self-defined collision cost function. f. of v And f a And the lambda is a coefficient corresponding to the self-defined dynamic feasibility cost function, and the speed and the acceleration of the robot are limited not to exceed the limit.
In order to generate a safe track considering the shape of the whole unmanned rotorcraft, the shape of the unmanned rotorcraft is equivalent to the combination of two spheres as shown in figure 2 (the five-pointed star is P) A And P B The cross star is P O )，P O For rotor unmanned plane centroid, P A And P B The centers of two balls, P in space A And P B To P O Has a constant value of P A Point sum P B Point to P O The amount of translation between the coordinates is a function of the yaw angle yaw. In order to make the constraint function (collision cost function) a convex function, when calculating collision constraint, respectively obtaining the distance information between two sphere centers and the obstacle in the environment and the gradient direction information far away from the obstacle from the ESDF map, and then according to P A 、P B 、P O The rotational and translational relationships of (A) convey distance and gradient information to P O Point, thereby pushing the trajectory away from the obstacle in the direction of gradient descent to avoid collision. By the center of sphere P A For example, P A Can be expressed as a centroid P O And heading angle:
wherein,
for P derived from an ambient distance field A The gradient of the point with respect to the obstacle.
The present invention abstracts the rotorcraft geometry into analytic expressions including, but not limited to, representation using a finite combination of circles, and any other combination of convex polyhedrons may be applied.
The front end path planning effect diagram is shown in fig. 3. The back end trajectory optimization effect graph is shown in fig. 4. The comparison shows that the path searched by the front end of the invention is more fit with the result of rear end optimization, and a safe and collision-free track can be obtained when the obstacle is a corner.
The above embodiments are only used for illustrating the design idea and features of the present invention, and the purpose of the present invention is to enable those skilled in the art to understand the content of the present invention and implement the present invention accordingly, and the protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes and modifications made in accordance with the principles and concepts disclosed herein are intended to be included within the scope of the present invention.
Claims (10)
1. A real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment is characterized by comprising the following steps:
(1) acquiring current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupation probability grid map, and establishing an ESDF (extended service distribution function) map based on the occupation probability grid map;
(2) by considering a front-end path planning algorithm of a yaw angle, abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls, and checking whether the distance between the circle centers of all the balls and the nearest barrier is smaller than the radius of the ball through the ESDF map established in the step (1) to perform collision detection, so that a path which is in accordance with the kinematic constraint of the unmanned aerial vehicle and has no collision on the whole machine is obtained;
(3) and (3) abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls in a back-end trajectory optimization algorithm, inquiring distance information between all ball centers and the obstacle and gradient direction information far away from the obstacle from an ESDF map, and optimizing the path obtained in the step (2) by using a gradient descent method.
2. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 1, wherein the process of constructing the ESDF map based on the occupancy probability grid map specifically comprises:
the ESDF map represents potential field information of the environmental obstacles, distance functions of obstacle grids and surrounding grids are respectively constructed for three axes of x, y and z, and the lower envelope lines of the functions are the distance functions of the grids and the nearest obstacles
Can be expressed as:
wherein p represents the grid of the query, q is the grid where the obstacle is located,
to occupy all the grids in the probability grid map, f (q) is a sampling function of q.
3. The real-time trajectory planning method taking into account the external shape of the unmanned rotary wing aircraft in a complex environment according to claim 1, wherein the yaw-angle-taking-into-account front-end path planning algorithm is a kinematics-based a-x algorithm taking into account a yaw angle yaw.
4. The real-time trajectory planning method taking into account the profile of a rotary-wing drone in a complex environment according to claim 1, characterized in that said step (2) comprises the following sub-steps:
(2.1) expanding nodes from a starting point, obtaining a plurality of motion primitives based on different control input quantities, and reserving the motion primitives meeting constraint conditions by carrying out safety and dynamics feasibility inspection on the motion primitives;
(2.2) calculating a node corresponding to each motion element according to the motion elements obtained in the step (2.1), evaluating a cost function and a heuristic function of each node, and adding the node into a node list to be expanded; taking the node with the minimum sum of the cost function and the heuristic function in the node list to be expanded as a next node; and repeating the steps until the searched node is the end point, and obtaining a path which accords with the kinematics constraint of the unmanned aerial vehicle and has no collision on the whole machine.
5. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 4, wherein the step (2.1) is specifically as follows:
the kinematics of the unmanned aerial vehicle is considered during node expansion, the motion track of the unmanned aerial vehicle is expressed as a polynomial about time t, the real-time track planning considering the appearance of the rotor unmanned aerial vehicle is added into the one-dimensional yaw angle planning, and four independent one-dimensional time parameterized polynomial equations are used for expression:
p(t):＝[p x (t)，P y (t)，p z (t)，p yaw (t)] T
wherein p is μ (t) is the time parameterized polynomial equation for each dimension, μ ∈ { x, y, z, yaw }, a k Is a polynomial coefficient, k is a polynomial order
As a state vector, make
Is a control input;
its state space model can be expressed as:
the complete solution to the equation of state is expressed as:
it represents that the initial state is x (0), the control input quantity is u (t), and the terminal state is x (t);
in the expansion node, given the current state of the rotorcraft, a set of discrete control quantities is input for the duration τ
The input quantity of the yaw angle yaw is angular acceleration; of each shaft [ -u [ ] max ,u max ]Is uniformly dispersed into
The extension node can get (2r +1) each time 4 A bar motion primitive.
6. The real-time trajectory planning method considering the appearance of the unmanned rotorcraft in the complex environment according to claim 4, wherein the step (2.1) further includes performing feasibility and security check on each motion primitive so as to enable a corresponding node to be added into an extensible node set, specifically: each motion element can enable a corresponding node to be added into an expandable node set only through feasibility and safety check, the feasibility check requires that the speed and the acceleration of track points on the motion elements do not exceed the maximum limit of the motion capability of the unmanned aerial vehicle, the safety check requires that the whole motion element is free of collision, the distance from each sphere center of the spherical combination to the nearest obstacle is inquired and calculated, and the distance cannot be smaller than the radius of a sphere; the shape of the unmanned aerial vehicle is abstracted into a combination of a limited number of balls for rapid collision detection; and checking whether the distance between the circle centers of every two balls and the nearest barrier is smaller than the radius of the ball to judge whether collision occurs or not, wherein the distance between the circle centers of every two balls and the nearest barrier is obtained by inquiring ESDF map information.
7. The real-time trajectory planning method taking into account the external shape of the unmanned rotorcraft in a complex environment according to claim 4, wherein the node cost function in step (2.2) is a function of the control input u (t) and time t, and the formula is as follows:
wherein ρ is the weight of time t;
the heuristic function of a node is:
wherein p is μc ，v μc ,，p μg ，v μg For the current node and destination drone position and speed, by
And obtaining the minimum heuristic function J (Th) under the optimal time t.
8. The real-time trajectory planning method taking into account the appearance of a rotorcraft in a complex environment according to claim 1, wherein the back-end trajectory optimization algorithm is a B-spline or mioco trajectory-based trajectory optimization.
9. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 1, wherein the step (3) is implemented by using a gradient descent method, and optimizing a gradient direction for reducing an objective function value by deriving an objective function, wherein the objective function has the following formula:
f＝λ s f s +λ c f c +λ(f v +f a )
wherein f is s As a smooth cost function, λ s A coefficient corresponding to the self-defined smooth cost function; f. of c As a function of collision cost, λ c A coefficient corresponding to the self-defined collision cost function; f. of v And f a And lambda is a coefficient corresponding to the self-defined kinetic feasibility cost function.
10. The real-time trajectory planning method considering the profile of a rotorcraft in a complex environment according to claim 9, wherein the process of calculating the collision cost function is specifically:
equivalent rotor unmanned aerial vehicle appearance into two ballsCombination of (1), P O For rotor unmanned plane centroid, P A And P B Respectively the centers of the two spheres, respectively obtaining the distance information between the two centers and the obstacle in the environment and the gradient direction information far away from the obstacle from the ESDF map, and then according to the P A 、P B 、P O The rotational and translational relationships of (A) convey distance and gradient information to P O Points to push the trajectory away from the obstacle in the direction of gradient descent to avoid collision; p A Expressed as the center of mass P O And heading angle:
wherein,
for P derived from an ambient distance field A Turning off the lightGradient over obstacles.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210842336.7A CN115097857B (en) | 2022-07-18 | 2022-07-18 | Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210842336.7A CN115097857B (en) | 2022-07-18 | 2022-07-18 | Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115097857A true CN115097857A (en) | 2022-09-23 |
| CN115097857B CN115097857B (en) | 2024-04-30 |
Family
ID=83299280
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210842336.7A Active CN115097857B (en) | 2022-07-18 | 2022-07-18 | Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115097857B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735A (en) * | 2023-08-24 | 2023-09-19 | 北京理工大学 | Unmanned aerial vehicle cluster track planning method and system based on edge calculation |
| CN117870653A (en) * | 2024-03-13 | 2024-04-12 | 中国科学技术大学 | Method for establishing and updating two-dimensional differential Euclidean symbol distance field map |
| CN118111447A (en) * | 2024-04-28 | 2024-05-31 | 南开大学 | Unmanned aerial vehicle motion planning method and device oriented to narrow environment and storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110807782A (en) * | 2019-10-25 | 2020-02-18 | 中山大学 | Map representation system of visual robot and construction method thereof |
| CN111857160A (en) * | 2020-08-17 | 2020-10-30 | 武汉中海庭数据技术有限公司 | Unmanned vehicle path planning method and device |
| CN112180977A (en) * | 2020-09-30 | 2021-01-05 | 苏州臻迪智能科技有限公司 | Trajectory planning method and device, storage medium and aircraft |
| CN113835443A (en) * | 2021-10-08 | 2021-12-24 | 重庆大学 | Trajectory planning method for quadrotor unmanned aerial vehicle based on non-traditional European distance field |
| CN114355980A (en) * | 2022-01-06 | 2022-04-15 | 上海交通大学宁波人工智能研究院 | Four-rotor unmanned aerial vehicle autonomous navigation method and system based on deep reinforcement learning |
-
2022
- 2022-07-18 CN CN202210842336.7A patent/CN115097857B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110807782A (en) * | 2019-10-25 | 2020-02-18 | 中山大学 | Map representation system of visual robot and construction method thereof |
| CN111857160A (en) * | 2020-08-17 | 2020-10-30 | 武汉中海庭数据技术有限公司 | Unmanned vehicle path planning method and device |
| CN112180977A (en) * | 2020-09-30 | 2021-01-05 | 苏州臻迪智能科技有限公司 | Trajectory planning method and device, storage medium and aircraft |
| CN113835443A (en) * | 2021-10-08 | 2021-12-24 | 重庆大学 | Trajectory planning method for quadrotor unmanned aerial vehicle based on non-traditional European distance field |
| CN114355980A (en) * | 2022-01-06 | 2022-04-15 | 上海交通大学宁波人工智能研究院 | Four-rotor unmanned aerial vehicle autonomous navigation method and system based on deep reinforcement learning |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735A (en) * | 2023-08-24 | 2023-09-19 | 北京理工大学 | Unmanned aerial vehicle cluster track planning method and system based on edge calculation |
| CN116774735B (en) * | 2023-08-24 | 2023-11-10 | 北京理工大学 | Unmanned aerial vehicle cluster track planning method and system based on edge calculation |
| CN117870653A (en) * | 2024-03-13 | 2024-04-12 | 中国科学技术大学 | Method for establishing and updating two-dimensional differential Euclidean symbol distance field map |
| CN117870653B (en) * | 2024-03-13 | 2024-05-14 | 中国科学技术大学 | Method for establishing and updating two-dimensional differential Euclidean symbol distance field map |
| CN118111447A (en) * | 2024-04-28 | 2024-05-31 | 南开大学 | Unmanned aerial vehicle motion planning method and device oriented to narrow environment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115097857B (en) | 2024-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115097857B (en) | Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment | |
| De Filippis et al. | A minimum risk approach for path planning of UAVs | |
| CN113885567B (en) | Collaborative path planning method for multiple unmanned aerial vehicles based on conflict search | |
| Chen et al. | Dynamic obstacle avoidance for UAVs using a fast trajectory planning approach | |
| Scholer et al. | Configuration space and visibility graph generation from geometric workspaces for uavs | |
| CN114578856B (en) | Fixed wing unmanned aerial vehicle formation motion characteristic scene representation method | |
| Zhao et al. | Autonomous exploration method for fast unknown environment mapping by using UAV equipped with limited FOV sensor | |
| Tsintotas et al. | The MPU RX-4 project: Design, electronics, and software development of a geofence protection system for a fixed-wing vtol uav | |
| CN115525063A (en) | Trajectory planning method for unmanned helicopter | |
| Chen et al. | Multilayer mapping kit for autonomous UAV navigation | |
| Cao et al. | Mission-Oriented Trajectory Optimization for Search-and-Rescue Multirotor UAVs in Cluttered and GPS-Denied Environments | |
| Yuan et al. | Uav path planning with terrain constraints for aerial scanning | |
| Zuo et al. | Overview of obstacle avoidance algorithms for UAV environment awareness | |
| Wu et al. | Multi-phase trajectory optimization for an aerial-aquatic vehicle considering the influence of navigation error | |
| CN117270565A (en) | Airborne autonomous sensing and flight system based on vision | |
| Wang et al. | Optimal Landing Control of eVTOL Vehicles Using ODE-Based Aerodynamic Model | |
| Mao et al. | Cooperative 3D path planning of multi-UAV via improved fruit fly optimization | |
| Amiri Atashgah et al. | An integrated virtual environment for feasibility studies and implementation of aerial MonoSLAM | |
| Kim et al. | Vision-based pose estimation of fixed-wing aircraft using you only look once and perspective-n-points | |
| Lee et al. | Real‐time collision‐free landing path planning for drone deliveries in urban environments | |
| CN114578824A (en) | Unknown environment autonomous exploration method suitable for air-ground dual-purpose robot | |
| Huang et al. | On the 3D Track Planning for Electric Power Inspection Based on the Improved Ant Colony Optimization and A∗ Algorithm | |
| Geiger et al. | Flight testing a real-time direct collocation path planner | |
| Mi et al. | A 3D smooth mobility model based on semi-random circular movement for FANETs | |
| Choi | A framework for concurrent design and route planning optimization of unmanned aerial vehicle based urban delivery systems |
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 |