CN115097857B - Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment - Google Patents
Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment Download PDFInfo
- Publication number
- CN115097857B CN115097857B CN202210842336.7A CN202210842336A CN115097857B CN 115097857 B CN115097857 B CN 115097857B CN 202210842336 A CN202210842336 A CN 202210842336A CN 115097857 B CN115097857 B CN 115097857B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- map
- obstacle
- node
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005457 optimization Methods 0.000 claims abstract description 20
- 238000001514 detection method Methods 0.000 claims abstract description 6
- 238000011478 gradient descent method Methods 0.000 claims abstract description 5
- 238000007689 inspection Methods 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 3
- 239000002245 particle Substances 0.000 description 10
- 230000000694 effects Effects 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000006185 dispersion Substances 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
- 238000011160 research Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/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 track planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, which comprises the steps of collecting current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupancy probability grid map, and constructing ESDF map based on the occupancy probability grid map; the method comprises the steps that through a front-end path planning algorithm considering a yaw angle, the appearance of the unmanned aerial vehicle is abstracted into a combination of a plurality of balls, whether the distances between the circle centers of all the balls and nearest obstacles are smaller than the radius of the balls or not is inquired through a ESDF map to perform collision detection, and a path which accords with the kinematic constraint of the unmanned aerial vehicle and is free of collision of the whole unmanned aerial vehicle is obtained; in the back-end track optimization algorithm, the appearance of the unmanned aerial vehicle is abstracted into a plurality of ball combinations, the distance information between all ball centers and the obstacle and the gradient direction information far away from the obstacle are queried from a ESDF map, and then a gradient descent method is utilized to optimize the path. According to the invention, the front end and the rear end of the unmanned aerial vehicle body safety constraint model are established, so that the track safety and feasibility are ensured in a complex environment.
Description
Technical Field
The invention relates to the field of track planning of multi-rotor unmanned aerial vehicles, in particular to a real-time track 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 that a trajectory meeting the dynamics/kinematics constraint, the smoothness constraint and the collision-free constraint of the robot is generated in a complex environment. The algorithm comprises a path planning algorithm at the front end and a track optimization algorithm at the rear end. The front-end path planning algorithm of the rotor unmanned aerial vehicle obtains a rough passable path, and the method mainly comprises a path planning algorithm based on searching and a path planning algorithm based on sampling. The search-based path planning algorithm is represented by an a-algorithm, which combines the Dijkstra algorithm with the breadth-first algorithm to search for an optimal path faster by the action of a heuristic function. The sampling-based path planning algorithm, which is based on RRT, randomly samples in space, connects the nearest sampling point into the path tree and considers whether it has a better parent node or not until the region near the end point is explored. The front-end path planning algorithm provides an optimized initial value for the back-end track optimization algorithm, so that the track can be quickly converged to an optimal solution under the condition that constraint conditions are met. In order to save calculation resources, the current rotor unmanned aerial vehicle track planning algorithm is to make unmanned aerial vehicle equivalent as particles, only consider the planning of x, y and z three axes when carrying out path planning, and not consider the influence of unmanned aerial vehicle shape on the track, if the real size of the unmanned aerial vehicle is huge, the result that the searched path is not feasible is caused.
For track planning of a rotary-wing unmanned aerial vehicle, the recent research mainly surrounds track planning of a particle model of the unmanned aerial vehicle, and a track planning method which is easy to realize and is universal in consideration of the appearance of the rotary-wing unmanned aerial vehicle is lacked.
In the prior art, the rotor unmanned aerial vehicle appearance is represented by a convex polyhedron, the flight corridor is used for representing safety constraint to avoid collision with an obstacle, the convex polyhedron meeting the rotor unmanned aerial vehicle in planning is always contained in the flight corridor, and then the track is determined to be collision-free. The flight corridor generating algorithm finds the furthest and non-overrun 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 first point, and circulates for a plurality of times until the terminal point is contained in the convex polyhedron. The algorithm is used for rotor unmanned aerial vehicle racing, is applicable to the stronger small-size rotor unmanned aerial vehicle of mobility, if be used for large-scale unmanned aerial vehicle, then there are following two problems: (1) The actual shape of the unmanned aerial vehicle is not considered by the front-end path planning algorithm, and the front-end optimization considering the shape of the unmanned aerial vehicle is added to the front end of particle model planning, so that the unmanned aerial vehicle cannot be solved; (2) When the rear end is optimized in the flight corridor, each point of the plane convex polyhedron must be guaranteed to be in the same flight corridor convex hull, but for a large unmanned aerial vehicle, the plane convex polyhedron is difficult to guarantee. And the algorithm is based on the known situation of the global map, and the planning of each vertex on the convex polyhedron of the unmanned aerial vehicle is quite labor-consuming.
Therefore, there is a need to propose a real-time trajectory planning method for large-scale rotary-wing unmanned aerial vehicles in narrow complex environments.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a real-time track planning method considering the appearance of the rotor unmanned aerial vehicle in a complex environment.
The technical scheme of the invention is as follows: the embodiment of the invention provides a real-time track planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, which comprises the following steps:
(1) Collecting current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupancy probability grid map, and constructing ESDF map based on the occupancy probability grid map;
(2) The front-end path planning algorithm of the yaw angle is considered, the appearance of the unmanned aerial vehicle is abstracted into a plurality of ball combinations, whether the distances between the circle centers of all the balls and the nearest obstacle are smaller than the radius of the balls or not is checked through the ESDF map established in the step (1), and collision detection is carried out, so that a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision of the whole machine is obtained;
(3) And (3) abstracting the appearance of the unmanned aerial vehicle into a plurality of ball combinations in a rear-end track optimization algorithm, inquiring the distance information between all ball centers and the obstacle and the gradient direction information far away from the obstacle from a ESDF map, and optimizing the path obtained in the step (2) by using a gradient descent method.
The beneficial effects of the invention are as follows: 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 an unmanned aerial vehicle particle model, does not expand obstacles, 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. By comparing with a track planning method based on a particle model, the method that the unmanned aerial vehicle is regarded as particles and the obstacle is inflated can be seen to be likely to search a collision path, and the method can obtain a safe collision-free track under the condition of approximately the same search time. The path searched by the front end is more attached to the result of the rear end optimization, and a collision-free track can be still 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, wherein the front end and the rear end of the real-time track planning scheme are respectively provided with an unmanned aerial vehicle body safety constraint model, so that the track is safe and feasible in a complex environment. The method of the invention analytically expresses the geometric shape of the rotor unmanned aerial vehicle into a combination form of convex geometric bodies. The front-end path planning algorithm considers the planning of yaw angle yaw and performs collision checking of the overall shape. And the rear-end track optimization algorithm uses a chain rule to conduct the gradient at the circle center to the track point 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 the effect of particle model planning for a large rotor unmanned aerial vehicle;
Fig. 2 is a schematic diagram of a method of analytical expression of rotor unmanned aerial vehicle geometry;
FIG. 3 is a front end path planning effect diagram;
Fig. 4 is a back-end trajectory optimization effect diagram.
Detailed Description
Reference will now be made in detail to exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numbers in different drawings refer to the same or similar elements, unless otherwise indicated. The implementations described in the following exemplary examples do not represent all implementations consistent with the invention. Rather, they are merely examples of apparatus and methods consistent with aspects of the invention as detailed in the accompanying claims.
The following describes in detail a real-time trajectory planning method taking the appearance of a rotor unmanned aerial vehicle into consideration in a complex environment according to the present invention with reference to the accompanying drawings. The features of the examples and embodiments described below may be combined with each other without conflict.
The embodiment of the invention provides a real-time track planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment. Based on the current local map information and the real-time positioning information of the unmanned aerial vehicle, a grid map and a ESDF map are established, and collision with the surrounding environment is rapidly detected by abstracting the appearance of the unmanned aerial vehicle into a combination of limited balls. The front-end path searching algorithm obtains a rough but safe and feasible local path, the rear end optimizes the track obtained by the front end, and the track meets the power/kinematics constraint of the unmanned aerial vehicle on the premise that the track is constrained to the whole safety of the fuselage, namely, the track is smoother and feasible.
The method mainly relates to the processing of the constraint of the front and rear ends of an algorithm on the overall shape of the unmanned aerial vehicle, and for convenience of description, the method takes a rotor unmanned aerial vehicle with a rectangular shape (length of 1.2m and width of 0.8 m) and a negligible z-axis as an example. The method specifically comprises the following steps:
(1) And acquiring current local map information and real-time positioning information of the unmanned aerial vehicle, and establishing an occupancy probability grid map and a ESDF map.
The present embodiment uses an occupancy probability grid map to describe the environmental information that, given the known robot positioning information, generates a map from measurement data from noisy sensors, such as lidar, binocular cameras, etc. The grid map uses a binarization method to indicate whether a certain grid is occupied by an obstacle, and for grid m i, the probability of being occupied is p (m i|z1:t,x1:t), which indicates the probability z 1:t of observing the positioning information x 1:t for a given grid m i and 1 to t times. The larger the probability value, the higher the probability that the grid is occupied.
The embodiment of the invention uses ESDF (European character distance field) map to represent the potential field information of the environmental barrier, and the map is built based on an occupancy probability grid map to obtain the distance between each grid and the nearest barrier. For x, y, z triaxial respectively constructs the distance function of barrier grid and surrounding grid, the lower envelope of these functions is the distance function of grid and its nearest barrier, can be expressed as:
Where p represents the grid of the query, q is the grid where the obstacle is located, To occupy all grids in the 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, three linear interpolation of eight grids around the point is required.
And updating the real-time data of the occupancy probability grid map and the ESDF map, storing map information in a vector form, and obtaining barrier information by inquiring index values of corresponding grids when path planning is carried out.
(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 unmanned plane is abstracted into a plurality of ball combinations (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 an established ESDF map to detect collision.
The method is divided into a front-end path planning part and a rear-end track optimizing part, and according to differential flat dynamics characteristics of a rotor unmanned aerial vehicle model, a flat output space of the rotor unmanned aerial vehicle can be represented by x, y, z and yaw angle yaw. For a general rotor unmanned aerial vehicle, a particle model can meet the planning requirement, 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 actually passed through by the unmanned aerial vehicle can be obtained, as shown in fig. 1, so that the overall appearance of the unmanned aerial vehicle needs to be considered when the path planning is carried out. The kinematic characteristics of the large rotor unmanned aerial vehicle prevent the large rotor unmanned aerial vehicle from flying in a large attitude, namely pitch and roll are rotated greatly, so that only the kinematic constraint of yaw is considered during planning.
The front-end path planning algorithm in the embodiment of the invention obtains an initial path by utilizing a kinematics-based A-algorithm considering yaw angle yaw, and specifically comprises the following steps:
(2.1) expanding nodes from a starting point, obtaining a plurality of motion primitives based on different control input amounts, and reserving the motion primitives meeting constraint conditions by carrying out safety and dynamic feasibility inspection on the motion primitives;
The common a-algorithm takes the euclidean distance from the starting point to the current node as a cost function, takes the euclidean distance from the current node to the end point as a heuristic function, and selects a node with the smallest addition of the cost function and the heuristic function in the nodes to be expanded as a next path point when searching the end point each time. For the algorithm A based on dynamics, the kinematic characteristics of the unmanned aerial vehicle need to be considered when the node is expanded, the motion track of the unmanned aerial vehicle can be expressed as a polynomial about time t, the real-time track planning of the appearance of the rotary wing unmanned aerial vehicle is considered, the planning of a yaw angle needs to be added, and four independent one-dimensional time parameterized polynomial equations are used for representing:
p(t):＝[px(t),py(t),pz(t),pyaw(t)]T
Where p μ (t) is the time parameterized polynomial equation for each dimension, μ ε { x, y, z, yaw }, a k is the polynomial coefficient, and k is the polynomial order. Make the following steps For the state vector, makeFor control input, its state space model can be expressed as:
The complete solution of the state equation is expressed as:
the initial state is x (0), the control input quantity is u (t), and the track of the rotor unmanned aerial vehicle system is x (t).
When expanding the node, a set of discrete control amounts is input for the duration τ given the current state of the rotorcraftGiven n=2 in the embodiment of the present invention, the input amount of the yaw angle yaw is the angular acceleration, assuming that the acceleration of each axis is the input amount. The [ -u max,umax ] of each axis is uniformly discretized as/>Then (2r+1) 4 motion primitives can be obtained for each expansion node.
In order to balance the calculation consumption of front-end and back-end planning, let r=2, and each motion primitive can enable the corresponding node to be added into the extensible node set only through feasibility and security inspection. The feasibility check requires that the speed and acceleration of the 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 elements are collision-free, and the distance from each sphere center of the spherical combination to the nearest obstacle is inquired and calculated, wherein the distance cannot be smaller than the radius of the sphere.
Fast collision detection is performed by abstracting the form of the drone to a combination of a limited number of balls (two balls are an example in the embodiment of the invention).
In order to meet the real-time performance of track generation, the invention abstracts the unmanned aerial vehicle body into the combination of two balls for safety inspection, and the principle is as follows: and checking whether the distance between the centers of the two balls and the nearest obstacle is smaller than the radius of the balls to judge whether the two balls collide, and inquiring ESDF map information to obtain the distance between the centers of the two balls and the nearest obstacle. The invention can achieve the real-time effect aiming at the collision detection method considering the shape of the unmanned aerial vehicle because the calculation complexity of the query operation is O (n).
And (2.2) calculating nodes corresponding to each motion primitive according to the motion primitive obtained in the step (2.1), evaluating a cost function and a heuristic function of each node, and adding the nodes 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 the next node; repeating the steps until the searched node is the end point, and obtaining the path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision of the whole machine.
The cost function J (T) of the node is designed as a function of the control input u (T) with respect to time T, namely:
Where ρ is the weight of time t.
The heuristic function J (Th) of the node is designed to solve the optimization problem from the current node state to the end state without considering collision, and aims to find the optimal transition time t, namely:
where p μc,vμc,pμg,vμg is the position and speed of the current node and destination drone by causing
The minimum J (Th) is obtained at the optimal time t.
If the node to be expanded is empty and does not reach the end point, the input control quantity of the yaw is subjected to finer dispersion, r yaw =4, the (2r+1) 3*(2ryaw +1) motion primitives are regenerated, and if no expandable node exists, the previous node is returned to search a path with different topologies again.
The rough track generated by the A-algorithm based on kinematics, which considers the yaw angle, provides a better initial value for the track optimization of the rear end, accelerates the optimization speed of the rear end, and avoids the condition that the optimization fails due to the fact that the general A-algorithm searches an infeasible area.
(3) And (3) constructing a non-particle model of the unmanned aerial vehicle through a back-end track optimization algorithm, abstracting the geometric shape of the unmanned aerial vehicle into an analytical expression method, and optimizing the local path obtained in the step (2).
The back end of the invention performs track optimization based on B-spline or MINCO track class. Taking a B spline as an example in the embodiment of the invention, the front-end path can be converted into a pb-order B spline of the control points Q= { Q_0, Q_1, …, Q_N } and the locus meeting the constraint condition is obtained by optimizing the positions of the control points. The track optimization is based on a gradient descent method, and the gradient direction for reducing the objective function value is obtained by deriving the objective function, so that the position of the control point is optimized to minimize the objective function, and the objective function is a function related to the control point. Its objective function can be expressed as:
f＝λsfs+λcfc+λ(fv+fa)
Wherein f s is a smooth cost function, lambda s is a coefficient corresponding to the self-defined smooth cost function, and the track is smoother by obtaining the geometric information of the track. f c is a collision cost function, and lambda c is a coefficient corresponding to the self-defined collision cost function, so that the track is far away from the obstacle. f v and f a are dynamic feasibility cost functions, lambda is a coefficient corresponding to the self-defined dynamic feasibility cost function, and the speed and the acceleration of the robot are limited to be not more than the limit.
In order to calculate the collision constraint, distance information of the two centers of sphere and the obstacle in the environment and gradient direction information far away from the obstacle are obtained from a ESDF map respectively, and then the distance and gradient information are transferred to a point P O according to the rotation and translation relation of P A、PB、PO, so that the track is pushed away from the obstacle along the direction of gradient descent to avoid collision, taking the point P A of the sphere as an example, and the point P A can be expressed as a function of the centroid P O and the heading angle:
for the current course angle of the robot,/> For P O to P A about heading angle/>Is a translation amount of (a). According to the chain law, the gradient of the constraint function J (i.e. the collision cost function f c) with respect to the robot position and heading angle can be found:
Wherein, The gradient of the P A point with respect to the obstacle, which is derived from the ambient distance field.
The method of abstracting the rotor unmanned aerial vehicle geometry into an analytical representation of the invention includes, but is not limited to, using a combination of finite circles, and any other combination of convex polyhedrons may be used.
The front-end path planning effect is shown in fig. 3. The back-end trajectory optimization effect graph is shown in fig. 4. Compared with the prior art, the path searched by the front end is more matched with the result of the rear end optimization, and a safe collision-free track can be obtained when the obstacle is a corner.
The above embodiments are merely for illustrating the design concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement the same, the scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications according to the principles and design ideas of the present invention are within the scope of the present invention.
Claims (4)
1. The real-time track planning method considering the appearance of the rotor unmanned aerial vehicle in a complex environment is characterized by comprising the following steps of:
(1) Collecting current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupancy probability grid map, and constructing ESDF map based on the occupancy probability grid map;
(2) The front-end path planning algorithm of the yaw angle is considered, the appearance of the unmanned aerial vehicle is abstracted into a plurality of ball combinations, whether the distances between the circle centers of all the balls and the nearest obstacle are smaller than the radius of the balls or not is checked through the ESDF map established in the step (1), and collision detection is carried out, so that a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision of the whole machine is obtained;
Said step (2) comprises the sub-steps of:
(2.1) expanding nodes from a starting point, obtaining a plurality of motion primitives based on different control input amounts, and reserving the motion primitives meeting constraint conditions by carrying out safety and dynamic feasibility inspection on the motion primitives;
The step (2.1) is specifically as follows:
When the nodes are expanded, the kinematic characteristics of the unmanned aerial vehicle are considered, the motion track of the unmanned aerial vehicle is expressed as a polynomial about time t, the real-time track planning of the appearance of the rotary wing unmanned aerial vehicle is considered, the planning of a yaw angle is added, and four independent one-dimensional time parameterized polynomial equations are used for representing:
p(t):＝[px(t),py(t),pz(t),pyaw(t)]T
Wherein p μ (t) is a time parameterized polynomial equation of each dimension, μ∈ { x, y, z, yaw }, a k is a polynomial coefficient, and k is a polynomial order; make the following steps For the state vector, makeIs a control input;
its state space model is expressed as:
The complete solution of the state equation is expressed as:
The initial state is represented as x (0), the control input quantity is u (t), and the track of the rotor unmanned aerial vehicle system is represented as x (t);
When expanding the node, a set of discrete control amounts is input for the duration τ given the current state of the rotorcraft The input amount of the yaw angle yaw is angular acceleration; the [ -u max,umax ] of each axis is uniformly dispersed intoThen (2r+1) 4 motion primitives can be obtained by the expansion node each time;
the step (2.1) further includes performing feasibility and security check on each motion primitive to enable the corresponding node to join in the extensible node set, specifically: each motion element can enable the corresponding node to be added into the expandable node set through feasibility and security inspection; the feasibility inspection requires that the speed and the acceleration of the 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 elements are collision-free, and the distance from each sphere center of the spherical combination to the nearest obstacle is inquired and calculated, wherein the distance cannot be smaller than the radius of the sphere; the method comprises the steps of carrying out rapid collision detection by abstracting the appearance of the unmanned aerial vehicle into a combination of a limited number of balls; checking whether the distance between the circle centers of every two balls and the nearest obstacle is smaller than the radius of the balls to judge whether the balls collide or not, and obtaining the distance between the balls and the nearest obstacle by inquiring ESDF map information;
(2.2) calculating nodes corresponding to each motion primitive according to the motion primitive obtained in the step (2.1), evaluating a cost function and a heuristic function of each node, and adding the nodes 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 the next node; repeating the steps until the searched node is the end point, and obtaining a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision with the whole machine;
The node cost function in the step (2.2) is a function of the control input u (t) and the time t, and the formula is as follows:
wherein ρ is the weight of time t;
the heuristic function of the node is:
where p μc,vμc,pμg,vμg is the position and speed of the current node and destination drone by causing
Obtaining the minimum heuristic function J (Th) at the optimal time t;
(3) Abstracting the appearance of the unmanned aerial vehicle into a plurality of ball combinations in a rear-end track optimization algorithm, inquiring the distance information between all ball centers and the obstacle and the gradient direction information far away from the obstacle from a ESDF map, and optimizing the path obtained in the step (2) by using a gradient descent method;
In the step (3), a gradient direction for reducing the objective function value is obtained by deriving an objective function by using a gradient descent method, and the objective function is optimized according to the following formula:
f＝λsfs+λcfc+λ(fv+fa)
Wherein f s is a smooth cost function, and lambda s is a coefficient corresponding to the self-defined smooth cost function; f c is a collision cost function, and lambda c is a coefficient corresponding to the self-defined collision cost function; f v and f a are dynamic feasibility cost functions, and lambda is a coefficient corresponding to the self-defined dynamic feasibility cost function;
the process of calculating the collision cost function is specifically:
The appearance of the rotor unmanned aerial vehicle is equivalent to the combination of two balls, P O is the center of mass of the rotor unmanned aerial vehicle, P A and P B are the centers of the two balls respectively, the distance information between the two centers of balls and an obstacle in the environment and the gradient direction information far away from the obstacle are obtained from a ESDF map respectively, and then the distance and gradient information are transmitted to a P O point according to the rotation and translation relation of P A、PB、PO, so that the track is pushed away from the obstacle along the gradient descending direction to avoid collision; p A is represented as a function of centroid P O and heading angle:
for the current course angle of the robot,/> For P O to P A about heading angle/>Is a translation amount of (a); according to the chain law, the gradient of the constraint function J, namely the collision cost function f c, with respect to the position and heading angle of the robot is calculated:
Wherein, The gradient of the P A point with respect to the obstacle, which is derived from the ambient distance field.
2. The real-time trajectory planning method considering the appearance of a rotorcraft in a complex environment according to claim 1, wherein the process of constructing ESDF a map based on an occupancy probability grid map is specifically:
ESDF map shows the potential field information of environmental obstacle, for x, y and z three axes, respectively constructing distance functions of obstacle grid and surrounding grids, the lower envelope curve of these functions is the distance function of grid and its nearest obstacle Can be expressed as:
Where p represents the grid of the query, q is the grid where the obstacle is located, To occupy all grids in the probability grid map, f (q) is a sampling function of q.
3. 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 front-end path planning algorithm considering the yaw angle is a kinematics-based a-algorithm considering the yaw angle yaw.
4. 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 back-end trajectory optimization algorithm is a trajectory optimization based on B-splines or MINCO trajectory classes.
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 CN115097857A (en) | 2022-09-23 |
| CN115097857B true 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) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735B (en) * | 2023-08-24 | 2023-11-10 | 北京理工大学 | Unmanned aerial vehicle cluster track planning method and system based on edge calculation |
| CN117870653B (en) * | 2024-03-13 | 2024-05-14 | 中国科学技术大学 | Method for establishing and updating two-dimensional differential Euclidean symbol distance field map |
| CN118111447B (en) * | 2024-04-28 | 2024-06-25 | 南开大学 | 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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115097857A (en) | 2022-09-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115097857B (en) | Real-time track planning method considering appearance of rotor unmanned aerial vehicle in complex environment | |
| Cheng et al. | RRT-based trajectory design for autonomous automobiles and spacecraft | |
| US11217107B2 (en) | Simultaneous representation of moving and static obstacles for automatically controlled vehicles | |
| Kendoul et al. | Modeling and control of a small autonomous aircraft having two tilting rotors | |
| De Filippis et al. | A minimum risk approach for path planning of UAVs | |
| Neto et al. | On the generation of trajectories for multiple UAVs in environments with obstacles | |
| Ren et al. | Bubble planner: Planning high-speed smooth quadrotor trajectories using receding corridors | |
| CN111707269A (en) | Unmanned aerial vehicle path planning method in three-dimensional environment | |
| Respall et al. | Fast sampling-based next-best-view exploration algorithm for a MAV | |
| CN113885567B (en) | Collaborative path planning method for multiple unmanned aerial vehicles based on conflict search | |
| Ji et al. | Mapless-planner: A robust and fast planning framework for aggressive autonomous flight without map fusion | |
| CN111723983B (en) | Time parameterization route planning method and system for unmanned aerial vehicle in unknown environment | |
| CN115690608A (en) | Fast object detection for vehicle situational awareness | |
| Tsintotas et al. | The MPU RX-4 project: Design, electronics, and software development of a geofence protection system for a fixed-wing vtol uav | |
| Lee et al. | Real-time directed rapidly exploring random tree path planning for air collision avoidance | |
| Martínez Novo et al. | Fast RRT* 3D-sliced planner for autonomous exploration using MAVs | |
| Cao et al. | Mission-Oriented Trajectory Optimization for Search-and-Rescue Multirotor UAVs in Cluttered and GPS-Denied Environments | |
| CN114578824A (en) | Unknown environment autonomous exploration method suitable for air-ground dual-purpose robot | |
| Choi et al. | Collision avoidance of unmanned aerial vehicles using fuzzy inference system-aided enhanced potential field | |
| Chu et al. | Track planning of multi-rotor unmanned aerial vehicle in the complex environment space | |
| Huang et al. | Path planning for UAV reconnoitring in complex environment based on improved visibility graph and genetic algorithm | |
| Paulson et al. | The rapid development of bespoke small unmanned aircraft: A proposed design loop | |
| Wang et al. | Path Planning of Load Transportation by Multi-Quadrotor Formation Based on RRT-APF Method | |
| Scott et al. | Mission performance evaluation of low-speed small unmanned aerial systems using virtual range and stereo camera sensors | |
| Jiang et al. | Improved Double-tree RRT* Algorithm for Efficient Path Planning of Mobile Robots |
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 |