CN112650306A - Unmanned aerial vehicle motion planning method based on dynamics RRT - Google Patents

Unmanned aerial vehicle motion planning method based on dynamics RRT Download PDF

Info

Publication number
CN112650306A
CN112650306A CN202011566611.4A CN202011566611A CN112650306A CN 112650306 A CN112650306 A CN 112650306A CN 202011566611 A CN202011566611 A CN 202011566611A CN 112650306 A CN112650306 A CN 112650306A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
dynamics
rrt
drone
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.)
Pending
Application number
CN202011566611.4A
Other languages
Chinese (zh)
Inventor
张福彪
丁宇
林德福
王亚凯
杨希雯
郎帅鹏
刘明成
毛杜芃
周天泽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202011566611.4A priority Critical patent/CN112650306A/en
Publication of CN112650306A publication Critical patent/CN112650306A/en
Pending legal-status Critical Current

Links

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/12—Target-seeking control

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 an unmanned aerial vehicle motion planning method based on dynamics RRT, which comprises the following steps: step 1, detecting and obtaining environmental information; step 2, establishing an unmanned aerial vehicle motion planning model; and 3, obtaining an unmanned aerial vehicle obstacle avoidance planning movement path. The unmanned aerial vehicle motion planning method based on dynamics RRT does not need to geometrically divide a search area, has high coverage rate of a search space and wide search range, can explore unknown areas as much as possible, enhances the purpose and flexibility of unmanned aerial vehicle motion planning, can enable the unmanned aerial vehicle to rapidly plan safe and feasible progressive optimal paths conforming to dynamics constraints, and solves the problem of motion planning of a multi-degree-of-freedom unmanned aerial vehicle in a complex environment and a dynamic environment.

Description

Unmanned aerial vehicle motion planning method based on dynamics RRT
Technical Field
The invention belongs to the technical field of unmanned aerial vehicle obstacle avoidance, and particularly relates to a dynamic RRT-based unmanned aerial vehicle motion planning method.
Background
With the rapid update and development of unmanned aerial vehicles of different types, models and purposes, the unmanned aerial vehicle needs to have various capabilities such as scene perception, obstacle avoidance, optimal calculation, information transfer, multi-level assistant driving and the like in order to better complete various tasks given by human beings and finally realize autonomous flight. The motion planning capability serves as one of basic capabilities required by the unmanned aerial vehicle, and plays a role in starting and stopping in the unmanned aerial vehicle autonomous flight system module. The motion planning problem is defined as finding a dynamically feasible and constrained trajectory of the drone between the start state and the target state without colliding with obstacles in the environment.
In the prior art, the sampling-based RRT (fast-search random tree) method and its variants (RRT) are effective in providing global path planning for fixed-wing aircraft through static obstacles. However, the RRT method is to create a tree constructed of possible operations from the starting point to the target point, all obstacles are considered as static obstacles in the RRT-based method, and therefore, in each control time step, no one has a chance to regenerate a new collision-free path or modify an existing tree that has grown before, resulting in a long time to obtain the calculation result. The RRT method is probabilistically complete, i.e., when the number of samples approaches infinity, the probability of finding a solution by the method approaches 1, but the probability of the RRT method converging on the optimal solution is 0. In addition, the RRT method does not consider the path cost among nodes in the expansion process, cannot ensure the optimality of the planning result, is only suitable for a system with simple dynamics, and cannot be suitable for the motion planning of the multi-degree-of-freedom unmanned aerial vehicle in a complex environment and a dynamic environment.
Therefore, it is necessary to provide a method for planning the motion of an unmanned aerial vehicle, so that the unmanned aerial vehicle can quickly plan a safe and feasible optimal progressive path conforming to the kinetic constraint in a complex and variable environment, and the unmanned aerial vehicle can safely complete a task and reduce loss.
Disclosure of Invention
In order to overcome the above problems, the present inventors have conducted intensive research to linearize the motion/dynamics equation of the drone into a system state equation, introduce optimal control under motion constraint, define a cost function considering energy loss and completion time, design a planner of fixed final state-free final time, and accurately and optimally connect any state node pair of any system and controllable linear dynamics in state space of any dimension. The invention constructs a feasible obstacle avoidance progressive optimal track through a multi-iteration optimization strategy, solves the motion planning of the multi-degree-of-freedom unmanned aerial vehicle in a complex environment and a dynamic environment, and thus completes the invention.
Specifically, the present invention aims to provide the following:
in a first aspect, a method for planning the movement of an unmanned aerial vehicle based on dynamics RRT is provided, the method comprising the following steps:
step 1, detecting and obtaining environmental information;
step 2, establishing an unmanned aerial vehicle motion planning model;
and 3, obtaining an unmanned aerial vehicle obstacle avoidance planning movement path.
Wherein, step 3 comprises the following substeps:
step 3-1, initializing a random tree, and sampling in a barrier-free space;
step 3-2, traversing and searching the nearest node, updating the father node, and performing random tree rewiring;
and 3-3, obtaining the obstacle avoidance planning movement path of the unmanned aerial vehicle.
Wherein, in the step 3-1, an obstacle-free full-state space is established, sampling is carried out in the space,
the obstacle-free all-state space is shown as the following formula (one):
Figure BDA0002860850440000031
wherein, x [ t ]]E X represents the system state of the drone, X ═ RnIs the state space of the drone;
u[t]e.u represents the control input of the drone, U ═ RmIs the control input space of the unmanned aerial vehicle;
A∈Rn×nand B ∈ Rn×mIs constant and given.
In step 3-2, the nearest node is found through the following formula (two):
Figure BDA0002860850440000032
wherein c [ pi ]]Representing a cost of the trajectory; τ represents the arrival time or duration of the trace; u (t) represents the control input of the drone on the trajectory; r is formed by Rm×mIs positive and given.
Wherein the optimal control input of the unmanned aerial vehicle on the trajectory is obtained by the following formula (three):
Figure BDA0002860850440000033
where τ represents a given fixed arrival time, t ≦ 0 ≦ τ;
x0,x1represents two states, where x [0 ]]＝x0,x[τ]＝x1；
G (t) denotes a weighted controllability Gramian matrix.
Wherein the optimal arrival time of the unmanned aerial vehicle on the track is obtained by the following formula (IV):
Figure BDA0002860850440000034
Figure BDA0002860850440000035
wherein the content of the first and second substances,
Figure BDA0002860850440000041
in step 3-2, after the nearest node is obtained, the collision detection is determined by the following formula (six):
judge(M,N,P)＝((y-y1)(x2-x1))＞(y2-y1)(x-x1) (VI)
Wherein M, N, P represents M ═ x1,y1)，N＝(x2,y2)，P＝(x,y)。
And 4, carrying out obstacle avoidance detection on the obtained unmanned aerial vehicle obstacle avoidance planning motion path so as to ensure that the unmanned aerial vehicle effectively flies and avoids obstacles.
In a second aspect, a computer-readable storage medium is provided, storing a dynamics RRT-based drone motion planning program, which when executed by a processor, causes the processor to perform the steps of the dynamics RRT-based drone motion planning method.
In a third aspect, a computer device is provided, comprising a memory and a processor, the memory storing a dynamics RRT-based drone motion planning program, which when executed by the processor, causes the processor to perform the steps of the dynamics RRT-based drone motion planning method.
The invention has the advantages that:
(1) according to the unmanned aerial vehicle motion planning method based on dynamics RRT provided by the invention, geometric division of a search area is not needed, the coverage rate of a search space is high, the search range is wide, and an unknown area can be explored as far as possible; a progressive solution framework of the obstacle avoidance optimal track is constructed through a multi-iteration optimization strategy, the calculated amount is reduced, and a safe unmanned aerial vehicle flight track can be generated at a higher speed;
(2) the unmanned aerial vehicle motion planning method based on dynamics RRT provided by the invention provides a cost function considering the energy loss and the completion time of the unmanned aerial vehicle, and enhances the purpose and the flexibility of the unmanned aerial vehicle motion planning;
(3) the dynamics RRT-based unmanned aerial vehicle motion planning method provided by the invention designs a planner with fixed final state-free final time, so that the method is suitable for linear differential constraint unmanned aerial vehicles, a feasible unmanned aerial vehicle track is generated, and the progressive optimality of any system with controllable linear dynamics in a state space with any dimension is ensured.
Drawings
Fig. 1 shows a flow chart of a method for dynamics-based RRT unmanned aerial vehicle motion planning according to a preferred embodiment of the invention;
fig. 2 shows the motion trajectory of the drone according to the method of embodiment 1 of the invention;
fig. 3 shows a motion trajectory of an unmanned aerial vehicle based on the RRT obstacle avoidance planning method in the prior art.
Detailed Description
The present invention will be described in further detail below with reference to preferred embodiments and examples. The features and advantages of the present invention will become more apparent from the description.
The word "exemplary" is used exclusively herein to mean "serving as an example, embodiment, or illustration. Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments.
The inventors have found that conventional RRT methods rely on connecting any pair of nodes in a straight trajectory to represent a viable path, but for drone dynamics systems, a straight connection between a pair of nodes is typically not a valid trajectory due to the differential constraints of the system. Therefore, the invention improves the traditional RRT method, provides the unmanned aerial vehicle motion planning method based on dynamics RRT, and constructs a feasible obstacle avoidance progressive optimal track through a multi-iteration optimization strategy so as to solve the motion planning of the multi-degree-of-freedom unmanned aerial vehicle in a complex environment and a dynamic environment.
In a first aspect of the present invention, a method for planning the motion of an unmanned aerial vehicle based on dynamics RRT is provided, where the method includes the following steps:
step 1, detecting and obtaining environmental information;
step 2, establishing an unmanned aerial vehicle motion planning model;
and 3, obtaining an unmanned aerial vehicle obstacle avoidance planning movement path.
The method is described in further detail below:
step 1, detecting and obtaining environmental information.
In the invention, the environment information is obtained by detecting through a detection device arranged on the unmanned aerial vehicle, and the detection device can be a binocular camera, a laser radar, a photoelectric pod and the like.
Wherein, obstacle information in the environment is obtained through detection, namely an obstacle map is obtained.
And 2, establishing an unmanned aerial vehicle motion planning model.
Wherein, the establishing of the unmanned aerial vehicle motion planning model refers to: modeling the unmanned aerial vehicle, specifically, deducing and establishing a kinematic model and a dynamic model of the unmanned aerial vehicle by combining a ground coordinate system E (X, Y, Z) and a body coordinate system B (X, Y, Z) with Newton's law.
And 3, obtaining an unmanned aerial vehicle obstacle avoidance planning movement path.
As shown in fig. 1, step 3 includes the following substeps:
and 3-1, initializing a random tree and sampling in a barrier-free space.
The random tree is initialized by the starting state of the unmanned aerial vehicle, and specifically, when the iteration number is 1, the random tree is initialized in a free state space by taking the starting state of the unmanned aerial vehicle as a starting state point.
In the present invention, the starting state of the drone is the position and speed of the drone, for example, in the present invention, the position coordinate of the starting state of the drone is set to (1,1), and the speed is set to (0, 0).
According to a preferred embodiment of the invention, an obstacle-free full-state space is created, in which the sampling takes place,
wherein, the obstacle-free all-state space is shown as the following formula (one):
Figure BDA0002860850440000071
wherein, x [ t ]]E X represents the system state of the drone, X ═ RnIs the state space of the drone;
u[t]e.u represents the control input of the drone, U ═ RmIs the control input space of the unmanned aerial vehicle;
A∈Rn×nand B ∈ Rn×mIs constant and given.
In the invention, since the state space equation described by the formula (one) is obtained by linearization according to a kinematic model and a dynamic model, A ∈ Rn×nAnd B ∈ Rn×mThe values are constant and given, and are determined according to the model of the unmanned aerial vehicle, and the values can be changed along with different modeling or different model assumptions.
Wherein, the full state space includes unmanned aerial vehicle's positional information, speed information and arrival time information.
In the present invention, a new random point is generated in the full state space through sampling.
And 3-2, traversing and searching the nearest node, updating the father node, and performing random tree rewiring.
After the random points are generated through the steps, traversing the existing adjacent nodes in the random tree, calculating the distance between each node and the random point, and finding out the node closest to the random point.
According to a preferred embodiment of the present invention, the nearest node is found by the following formula (two):
Figure BDA0002860850440000072
wherein c [ pi ]]Representing a cost of the trajectory; τ denotes the trackThe time of arrival or duration of time; u (t) represents the control input of the drone on the trajectory; r is formed by Rm×mIs positive and given.
Wherein the positive definite definition is that M is n-order square matrix, if there is z for any non-zero vector zTMz＞0(zTRepresenting the transpose of z), M is referred to as a positive definite matrix. R is an m-th order square matrix which is positive-definite if and only if there is z for all non-zero real coefficient vectors zTRz > 0, wherein zTRepresents a transpose of z; corresponding to the above formula (two), indicating u (t)TRu(t)＞0。
In the invention, when a nearest node is defined, the motion constraint of the unmanned aerial vehicle is considered to introduce optimal control, if the state cost of transferring from one node to another node is very small, the two state nodes are adjacent, a cost function considering energy loss and completion time is preferably defined in the invention, as shown in the formula (II), compared with the cost function only considering distance in the prior art, the cost function in the invention can tunably balance the duration of a track and the control work of expansion, and the aim and the flexibility of unmanned aerial vehicle motion planning are enhanced.
In a further preferred embodiment, the optimal control input (i.e. optimal u (t)) of the drone on the trajectory is obtained by the following formula (three):
Figure BDA0002860850440000081
where τ represents a given fixed arrival time, t ≦ 0 ≦ τ;
x0,x1represents two states, where x [0 ]]＝x0,x[τ]＝x1；
G (t) denotes a weighted controllability Gramian matrix.
In the invention, whether the control input of the unmanned aerial vehicle on the track is optimal or not can be judged through the planning controller of the fixed final state-the free final time defined by the formula (III).
In a still further preferred embodimentThe optimal arrival time tau of the unmanned aerial vehicle on the track*(i.e., optimum τ) is obtained by the following formula (iv):
Figure BDA0002860850440000091
Figure BDA0002860850440000092
wherein the content of the first and second substances,
Figure BDA0002860850440000093
in the invention, the optimal control input and the optimal arrival time of the unmanned aerial vehicle on the track are obtained through the formulas (three) and (four), so that the state cost for transferring from one state node to another state node is low, and the energy loss of the unmanned aerial vehicle is the lowest.
Preferably, the optimal trajectory of the drone is obtained by the following formula (v):
Figure BDA0002860850440000094
Figure BDA0002860850440000095
wherein t is more than 0 and less than tau*。
According to a preferred embodiment of the present invention, after obtaining the closest node, the determination of collision detection is preferably performed by the following formula (six):
judge(M,N,P)＝((y-y1)(x2-x1))＞(y2-y1)(x-x1) (VI)
Wherein M, N, P represents M ═ x1,y1)，N＝(x2,y2)，P＝(x,y)。
In the case of collision detection of an obstacle, the obstacle has a rectangular shapeThe collision mechanism is relatively complex, and the specific collision mechanism is that a new random point x is generated in the process of expanding the random treerandWith the nearest node xnearThe connected track can not intersect with any side of the rectangular barrier, namely the rectangular barrier collision detection problem is converted into the problem that the track intersects with the rectangle.
Specifically, a Boolean value pool is set for one side of a rectangleiWhen the boliWhen 1, it indicates a collision, when pool occursiWhen 0, no collision occurs. Therefore, the judge function in the equation (six) is a boolean function, and when the right side of the equation is true, the judge is 1, and conversely, the judg is 0.
In a further preferred embodiment, the judgment is made by the following formula (seven):
booli＝
(judge(xnear,Vertex1,Vertex2)≠judge(xnew,Vertex1,Vertex2))
and
(judge(xnear,xnew,Vertex1)≠judge(xnear,xnew,Vertex2) (seven)
Wherein, VertexiTwo fixed points representing one side of the rectangular obstacle.
In the invention, through collision detection, if collision occurs, the step 3-1 and the step 3-2 are repeatedly carried out; if no collision occurs, the random state point x generated by samplingrandNode x being a valid new statenewAnd the corresponding nearest node is used as a father node of the new node to update, so that the original branch is deleted, and the rewiring of the random tree is completed.
Wherein for some path cost c x on the random treenear,xnew]R as xnewWhere r is the neighbor radius. Calculating (c [ x ] in sequencenear]+c[xnear,xnew]) And selecting x corresponding to the minimum valuenear|minAs xnewThe parent node of (2).
In the present invention, a neighbor node refers to a node that falls within an area that can be covered by a communication radius with the node as a center, that is, all nodes that can directly communicate with and can be connected to the node are referred to as neighbor nodes of the node. The neighbor radius is the communication radius.
For existing state point xnear' if c [ x ] satisfies the obstacle avoidance conditionnew,xnear′]< r and c [ x ]new]+c[xnew,xnear′]＜c[xnear′]Let xnewIs xneaAnd the new father node of the' deletes the original branch at the same time to complete the rewiring of the random tree T.
According to the invention, through rewiring of the random tree, redundant paths of the random tree after new nodes are generated are reduced, the path cost is reduced, and any pair of nodes can be optimally connected through a straight line track.
And 3-3, obtaining the obstacle avoidance planning movement path of the unmanned aerial vehicle.
In the invention, preferably, the steps 3-1-3-2 are repeated until the new node contains the target point x of the unmanned aerial vehiclegoalUntil now.
More preferably, the obstacle avoidance planning path from the starting state to the target state of the unmanned aerial vehicle is obtained in the random tree through a backtracking method.
According to a preferred embodiment of the present invention, after the obstacle avoidance planning motion path of the unmanned aerial vehicle is obtained in step 3, step 4 is further included, where obstacle avoidance detection is performed on the obtained obstacle avoidance planning motion path, so as to ensure that the unmanned aerial vehicle effectively flies and avoids obstacles.
According to the unmanned aerial vehicle motion planning method based on dynamics RRT provided by the invention, the motion/dynamics equation of the unmanned aerial vehicle is linearized into a system state equation, optimal control is introduced under motion constraint, a cost function considering energy loss and completion time is defined, a planner with fixed final state-free final time is designed, any state node pair of any system and controllable linear dynamics in any dimension state space are accurately and optimally connected, the planning result returned by the motion planning method is greatly reduced in time cost, the planning speed is improved, the unmanned aerial vehicle flight path is more suitable for being used as the unmanned aerial vehicle flight path, and compared with the conventional RRT method, the unmanned aerial vehicle can rapidly plan a safe and feasible gradual optimal path conforming to the dynamics constraint.
The invention also provides a computer readable storage medium storing a dynamics RRT-based drone movement planning program, which when executed by a processor, causes the processor to perform the steps of the dynamics RRT-based drone movement planning method.
The unmanned aerial vehicle motion planning method based on dynamics RRT can be realized by means of software and a necessary general hardware platform, wherein the software is stored in a computer-readable storage medium (comprising a ROM/RAM, a magnetic disk and an optical disk) and comprises a plurality of instructions for enabling a terminal device (which can be a mobile phone, a computer, a server, a network device and the like) to execute the method.
The invention also provides a computer device comprising a memory and a processor, wherein the memory stores a dynamics RRT-based unmanned aerial vehicle motion planning program, and when the program is executed by the processor, the processor executes the steps of the dynamics RRT-based unmanned aerial vehicle motion planning method.
Examples
The present invention is further described below by way of specific examples, which are merely exemplary and do not limit the scope of the present invention in any way.
Example 1
In this embodiment, when the vertical maneuver of the drone is ignored, it is considered that obstacle avoidance of the drone is performed at a constant height, and the drone moves in a two-dimensional plane, so that the model can be simplified into a 2D model, the starting position coordinates of the drone are set to (1,1), the target position coordinates are set to (700 ), the extension step length is set to 30 meters, and two rectangular obstacles are set in the flight path.
And (3) carrying out obstacle avoidance motion planning on the unmanned aerial vehicle through dynamics RRT:
(1) let iteration number i equal to 1, with the start of the droneState is the initial state point xstarThe random tree T is initialized in free state space.
(2) In the ith iteration, a new random state point x is obtained by sampling in a free state spacerand. Finding a father node x between the existing adjacent nodes on the random tree T, and obtaining a corresponding cost function value c [ x ] by the following formula programming controller]：
Figure BDA0002860850440000121
Figure BDA0002860850440000131
(3) Judging the collision detection through the following formula, and if the collision occurs, performing the step (2) again; on the contrary, let xrandIs a legal random state point xnew：
judge(M,N,P)＝((y-y1)(x2-x1))＞(y2-y1)(x-x1)
booli＝
(judge(xnear,Vertex1,Vertex2)≠judge(xnew,Vertex1,Vertex2))
and
(judge(xnear,xnew,Vertex1)≠judge(xnear,xnew,Vertex2))
Wherein, the judging method of each side of the rectangular barrier is the same.
(4) For some path cost c x on the random tree Tnear,xnew]R as xnewWhere r is the neighbor radius. Calculating (c [ x ] in sequencenear]+c[xnear,xnew]) And selecting x corresponding to the minimum valuenear|minAs xnewThe parent node of (2).
(5) For theExisting state point xnear' if c [ x ] meets the condition of avoiding the obstacle in the third stepnew,xnear′]< r and c [ x ]new]+c[xnew,xnear′]＜c[xnear′]Let xnewIs xnear'And deleting the original branches at the same time of the new father node, and completing the rewiring of the random tree T.
Sixthly, repeating the steps until the new node contains a target point x of the unmanned aerial vehiclegoalUntil now. The starting state x can be obtained in the random tree T by a backtracking methodstarTo the target state xgoalThe obstacle avoidance planning path.
Examples of the experiments
Experimental example 1
The motion trajectory of the dynamic RRT-avoidance based planning method described in example 1 is compared with the motion trajectory of the prior art RRT-avoidance based planning method, and the results are shown in fig. 2 and 3, respectively.
The steps of the RRT obstacle avoidance planning method in the prior art are shown in the literature, "lingyi fan, gevayan, wu kayao, huang yibin, wang yannan.
Step 1: and (6) initializing an algorithm. Setting an initial position, a target position and a total iteration number of path planning, and setting the current iteration number to be equal to 0;
step 2: random uniform sampling with sampling points of xrandRepresenting and adding 1 to the current iteration number i;
and step 3: finding x in search tree GrandNearest node xnearest；
And 4, step 4: at xnearestTowards xrandAdding corresponding increment in the direction to obtain a new node xnew；
And 5: find with xnewAs a center, RnearForming a set X for all nodes on G in the radius rangenear；
Step 6: calculating XnearCollision risk assessment function ofGauss(xnear,xnew) As a component of a cost function;
and 7: calculating XnearTo obtain xnewThe minimum value (expressed in min (c')) of the minimum cost function values and the parent nodes thereof is set as xnewAnd setting the node corresponding to the value as xnewParent node of (in x)minRepresents);
and 8: updating XnearParent node and cost function value;
and step 9: the algorithm terminates when xnewAnd target point xgoalIs less than the target deviation tolerance RfindAnd planning a feasible path by the algorithm, terminating the algorithm, and outputting the search tree graph and the feasible path.
As can be seen from fig. 2 and 3, the method provided in embodiment 1 of the present invention randomly generates new state node xrandAnd collision detection is carried out on the random tree and adjacent nodes, so that the generated random tree cannot be expanded into an obstacle, an obstacle avoidance task can be stably completed, the planned path is ensured to be safe, and the method has progressive optimality under the condition that the probability is complete (along with the increase of sampling points, the probability of finding the path is 1) (when the iteration number of the algorithm is close to infinity, the probability of containing the optimal solution is close to 1).
According to the method disclosed by the embodiment of the invention, the motion/dynamics equation of the unmanned aerial vehicle is linearized into a system state equation, optimal control is introduced under motion constraint, a cost function considering energy loss and completion time is defined, a planner of a fixed final state-free final time is designed, any state node pair of any system and controllable linear dynamics in any dimension state space are accurately and optimally connected, the time cost of the planning result returned by the motion planning method disclosed by the invention is greatly reduced, the planning speed is improved, the method is more suitable for being used as the flight trajectory of the unmanned aerial vehicle, and compared with the conventional RRT algorithm, the feasibility and the superiority of the unmanned aerial vehicle obstacle avoidance motion planning by utilizing the dynamics RRT method are proved.
The invention has been described in detail with reference to specific embodiments and illustrative examples, but the description is not intended to be construed in a limiting sense. Those skilled in the art will appreciate that various equivalent substitutions, modifications or improvements may be made to the technical solution of the present invention and its embodiments without departing from the spirit and scope of the present invention, which fall within the scope of the present invention.

Claims (10)

1. A method for planning the movement of an unmanned aerial vehicle based on dynamics RRT is characterized by comprising the following steps:
step 1, detecting and obtaining environmental information;
step 2, establishing an unmanned aerial vehicle motion planning model;
and 3, obtaining an unmanned aerial vehicle obstacle avoidance planning movement path.
2. The method according to claim 1, characterized in that step 3 comprises the following sub-steps:
step 3-1, initializing a random tree, and sampling in a barrier-free space;
step 3-2, traversing and searching the nearest node, updating the father node, and performing random tree rewiring;
and 3-3, obtaining the obstacle avoidance planning movement path of the unmanned aerial vehicle.
3. The method according to claim 2, characterized in that in step 3-1 an obstacle-free full-state space is established, in which the sampling is performed,
the obstacle-free all-state space is shown as the following formula (one):
Figure FDA0002860850430000011
wherein, x [ t ]]E X represents the system state of the drone, X ═ RnIs the state space of the drone;
u[t]e.u represents the control input of the drone, U ═ RmIs the control input space of the unmanned aerial vehicle;
A∈Rn×nand B ∈ Rn×mIs constant and given.
4. The method of claim 2, wherein in step 3-2, the nearest node is found by the following formula (two):
Figure FDA0002860850430000012
wherein c [ pi ]]Representing a cost of the trajectory; τ represents the arrival time or duration of the trace; u (t) represents the control input of the drone on the trajectory; r is formed by Rm×mIs positive and given.
5. The method of claim 4, wherein the optimal control input for the on-track drone is obtained by the following equation (three):
Figure FDA0002860850430000021
where τ represents a given fixed arrival time, t ≦ 0 ≦ τ;
x0,x1represents two states, where x [0 ]]＝x0,x[τ]＝x1；
G (t) denotes a weighted controllability Gramian matrix.
6. The method of claim 4, wherein the optimal arrival time of the on-track drone is obtained by the following equation (four):
Figure FDA0002860850430000022
Figure FDA0002860850430000023
wherein the content of the first and second substances,
Figure FDA0002860850430000024
7. the method according to claim 2, wherein in step 3-2, after obtaining the nearest node, the judgment of the collision detection is made by the following formula (six):
judge(M,N,P)＝((y-y1)(x2-x1))＞(y2-y1)(x-x1) (VI)
Wherein M, N, P represents M ═ x1,y1)，N＝(x2,y2)，P＝(x,y)。
8. The method according to claim 1, wherein the dynamics RRT-based unmanned aerial vehicle motion planning method further includes step 4 of performing obstacle avoidance detection on the obtained unmanned aerial vehicle obstacle avoidance planning motion path to ensure that the unmanned aerial vehicle flies and avoids obstacles effectively.
9. A computer readable storage medium, characterized by storing a dynamics RRT-based drone movement planning program, which when executed by a processor, causes the processor to perform the steps of the dynamics RRT-based drone movement planning method of one of claims 1 to 8.
10. A computer device comprising a memory and a processor, the memory storing a dynamics RRT-based drone motion planning program that, when executed by the processor, causes the processor to perform the steps of the dynamics RRT-based drone motion planning method of one of claims 1 to 8.
CN202011566611.4A 2020-12-25 2020-12-25 Unmanned aerial vehicle motion planning method based on dynamics RRT Pending CN112650306A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011566611.4A CN112650306A (en) 2020-12-25 2020-12-25 Unmanned aerial vehicle motion planning method based on dynamics RRT

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011566611.4A CN112650306A (en) 2020-12-25 2020-12-25 Unmanned aerial vehicle motion planning method based on dynamics RRT

Publications (1)

Publication Number Publication Date
CN112650306A true CN112650306A (en) 2021-04-13

Family

ID=75363065

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011566611.4A Pending CN112650306A (en) 2020-12-25 2020-12-25 Unmanned aerial vehicle motion planning method based on dynamics RRT

Country Status (1)

Country Link
CN (1) CN112650306A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359748A (en) * 2021-06-22 2021-09-07 浙江科技学院 Improved Multi-RRT path planning method based on fusion prediction and AGV trolley
CN113485356A (en) * 2021-07-27 2021-10-08 西北工业大学 Robot rapid movement planning method
WO2023093378A1 (en) * 2021-11-29 2023-06-01 中移(成都)信息通信科技有限公司 Path planning method and apparatus, device, and storage medium
CN116400733A (en) * 2023-05-06 2023-07-07 北京理工大学 Self-adaptive adjustment random tree full-coverage path planning method for reconnaissance unmanned aerial vehicle

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100174435A1 (en) * 2009-01-07 2010-07-08 Samsung Electronics Co., Ltd. Path planning apparatus of robot and method thereof
CN107608372A (en) * 2017-08-14 2018-01-19 广西师范大学 It is a kind of that path planning method is cooperateed with improving the multiple no-manned plane that PH curves are combined based on improvement RRT algorithms
CN108983780A (en) * 2018-07-24 2018-12-11 武汉理工大学 One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm
CN110794869A (en) * 2019-10-30 2020-02-14 南京航空航天大学 RRT-Connect algorithm-based robot metal plate bending feeding and discharging path planning method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100174435A1 (en) * 2009-01-07 2010-07-08 Samsung Electronics Co., Ltd. Path planning apparatus of robot and method thereof
CN107608372A (en) * 2017-08-14 2018-01-19 广西师范大学 It is a kind of that path planning method is cooperateed with improving the multiple no-manned plane that PH curves are combined based on improvement RRT algorithms
CN108983780A (en) * 2018-07-24 2018-12-11 武汉理工大学 One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm
CN110794869A (en) * 2019-10-30 2020-02-14 南京航空航天大学 RRT-Connect algorithm-based robot metal plate bending feeding and discharging path planning method

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
STEFANO PRIMATESTA: "An Innovative Algorithm to Estimate Risk Optimum Path for Unmanned Aerial Vehicles in Urban Environments", 《TRANSPORTATION RESEARCH PROCEDIA》 *
李成雷: "基于改进RRT-connect的四旋翼无人机避障轨迹规划算法", 《传感器与微系统》 *
笑扬轩逸: "《CSDN》", 27 March 2019 *
葛佳昊: "基于动力学RRT*的自由漂浮空间机器人轨迹规划", 《航空学报》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359748A (en) * 2021-06-22 2021-09-07 浙江科技学院 Improved Multi-RRT path planning method based on fusion prediction and AGV trolley
CN113359748B (en) * 2021-06-22 2022-05-10 杭州奇派自动化设备有限公司 Improved Multi-RRT path planning method based on fusion prediction and AGV trolley
CN113485356A (en) * 2021-07-27 2021-10-08 西北工业大学 Robot rapid movement planning method
CN113485356B (en) * 2021-07-27 2022-06-21 西北工业大学 Robot rapid movement planning method
WO2023093378A1 (en) * 2021-11-29 2023-06-01 中移(成都)信息通信科技有限公司 Path planning method and apparatus, device, and storage medium
CN116400733A (en) * 2023-05-06 2023-07-07 北京理工大学 Self-adaptive adjustment random tree full-coverage path planning method for reconnaissance unmanned aerial vehicle
CN116400733B (en) * 2023-05-06 2023-10-20 北京理工大学 Self-adaptive adjustment random tree full-coverage path planning method for reconnaissance unmanned aerial vehicle

Similar Documents

Publication Publication Date Title
Gao et al. Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments
Qiu et al. A multi-objective pigeon-inspired optimization approach to UAV distributed flocking among obstacles
Qin et al. Autonomous exploration and mapping system using heterogeneous UAVs and UGVs in GPS-denied environments
CN112650306A (en) Unmanned aerial vehicle motion planning method based on dynamics RRT
Zhihao et al. Virtual target guidance-based distributed model predictive control for formation control of multiple UAVs
Ergezer et al. 3D path planning for multiple UAVs for maximum information collection
Tisdale et al. Autonomous UAV path planning and estimation
CN108958285B (en) Efficient multi-unmanned aerial vehicle collaborative track planning method based on decomposition idea
Obermeyer Path planning for a UAV performing reconnaissance of static ground targets in terrain
Hoang et al. Angle-encoded swarm optimization for uav formation path planning
Andersson et al. Receding-horizon lattice-based motion planning with dynamic obstacle avoidance
Ille et al. Collision avoidance between multi-UAV-systems considering formation control using MPC
CN110146085B (en) Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration
Yang et al. Distributed optimal consensus with obstacle avoidance algorithm of mixed-order UAVs–USVs–UUVs systems
Shanmugavel et al. Collision avoidance and path planning of multiple UAVs using flyable paths in 3D
CN112539750B (en) Intelligent transportation vehicle path planning method
Spurny et al. Cooperative transport of large objects by a pair of unmanned aerial systems using sampling-based motion planning
CN115033016B (en) Heterogeneous unmanned cluster formation obstacle avoidance method and system
CN111766783A (en) Cluster system-oriented formation enclosure tracking method capable of converging in limited time
Zammit et al. Comparison of a* and rrt in real–time 3d path planning of uavs
Li et al. Autonomous waypoints planning and trajectory generation for multi-rotor UAVs
CN112731929A (en) Ackerman model-based mobile robot obstacle avoidance path planning method
Ke et al. Cooperative path planning for air–sea heterogeneous unmanned vehicles using search-and-tracking mission
CN115639830A (en) Air-ground intelligent agent cooperative formation control system and formation control method thereof
Fragoso et al. Dynamically feasible motion planning for micro air vehicles using an egocylinder

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210413

RJ01 Rejection of invention patent application after publication