CN111580548A - Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle - Google Patents
Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle Download PDFInfo
- Publication number
- CN111580548A CN111580548A CN202010309762.5A CN202010309762A CN111580548A CN 111580548 A CN111580548 A CN 111580548A CN 202010309762 A CN202010309762 A CN 202010309762A CN 111580548 A CN111580548 A CN 111580548A
- Authority
- CN
- China
- Prior art keywords
- speed
- obstacle
- node
- new
- velocity
- 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
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
Abstract
The invention relates to an unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle, which comprises the steps of establishing a random search tree in a sensing range of an unmanned aerial vehicle, taking the current position and speed as initial nodes, taking the intersection point position of a global path and the sensing range as a target position, sampling in a speed feasible region to obtain feasible speed, and then generating a new node. And after collision detection is carried out on the generated edges, the tree is updated, and the process is repeated until the random tree grows to reach the target position. And obtaining a local path in the random tree by using a backtracking method, and continuously flying along the original global path until the unmanned aerial vehicle reaches a target area after passing through the dynamic barrier along the local path. After the collision-free path is rebuilt and the dynamic barrier is passed through according to the collision-free path, the originally set obstacle-avoiding path is recovered to continue driving, so that path re-planning only needs to be executed once without continuously re-planning the path, and the path planning efficiency and the path stability are improved.
Description
Technical Field
The invention relates to the field of robot path planning, in particular to an unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle.
Background
In practical application, unmanned aerial vehicles which autonomously complete tasks such as rescue search, agriculture and surveying and mapping are more and more concerned by people. When navigating in a complex environment, in order to avoid static and dynamic obstacles, the fixed-wing aircraft is vital to regenerate a smooth and continuous path on line under the aerodynamic constraint.
For the path planning of the robot, a sampling-based RRT (fast-search random tree) method and a variant thereof are provided, and the global path planning of the fixed-wing aircraft passing through the static obstacles can be effectively provided. The RRT method is to create a tree of possible operations from the starting point to the target point, all obstacles are considered static obstacles in the RRT-based method, so in each control time step the robot will regenerate a new collision-free path or modify an existing tree that was grown before. The robot needs to wait for a longer time to obtain a calculation result through the RRT method.
Disclosure of Invention
In order to overcome the problem of low calculation efficiency of robot collision-free path planning in the prior art, the invention provides an unmanned aerial vehicle obstacle avoidance method based on spline-RRT and speed obstacle, the obstacle avoidance method combines a spline-RRT algorithm and a speed obstacle (VO) method, a random tree grows in a local area, meanwhile, the speed obstacle (VO) method is used for expanding the edge of the tree and rejecting unavailable nodes, the tree growth efficiency and the tree growth are improved, and the path planning efficiency and the route stability are improved.
In order to solve the technical problems, the invention adopts the technical scheme that: an unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle comprises the following steps:
the method comprises the following steps: in an initial state xstartInitializing a search tree for a root node, wherein a state represents a position and a velocity of the node in the tree;
step two: the cost of each edge of the random tree is divided into two parts, wherein the cost comprises the energy consumption cost of the node and the time consumption cost t;
step three: at each step of the random tree growth, a father node of the random tree is randomly selected;
step four: acquiring a new child node, wherein the position of the child node can be obtained by calculating according to the state of the parent node and the new speed, and the speed of the child node can be obtained by calculating the new speed vA,newAbout an axis eaxisRotation thetarThe angle is obtained.
Step five: directly defining the edge between the father node and the new son node by using the cubic Bezier curve without post-processing;
step six: judging whether a newly generated edge collides with an obstacle in a scene of a dynamic obstacle and a static obstacle;
step seven: calculating energy consumption cost and time consumption cost of a new node when the new node is added into the tree;
step eight: repeating the first step to the seventh step until the new node falls into the target area;
step nine: a feasible path is found in the random tree using backtracking.
In a dynamic environment containing a plurality of static obstacles and dynamic obstacles, the knowledge of the unmanned aerial vehicle on the static environment is known a priori, and a global path is generated offline through a spline-RRT algorithm by utilizing the prior knowledge of a map. When the drone is flying along a global path, the drone may detect dynamic obstacles using the onboard sensors of limited perceived distance when encountering an obstacle moving in front of the drone. When the aircraft detects a dynamic obstacle in the sensing range, the method establishes a random search tree in the sensing range by using the current position and speed as a starting node xstartTaking the intersection point position of the global path and the sensing range as a target position xgoal. Since the speed of the drone is changing, the speed range of the drone relative to the dynamic obstacle also changes. And sampling in the speed feasible region to obtain feasible speed, and then generating a new node. After collision detection is performed on the generated edges, the tree is updated, and the process is repeated until the random tree grows to reach the target position xgoal. And obtaining a local path in the random tree by using a backtracking method, and continuously flying along the original global path until the unmanned aerial vehicle reaches a target area after passing through the dynamic barrier along the local path.
Preferably, in the second step, both the energy consumption cost and the time consumption cost of the root node are initialized to 0.
Preferably, in the fourth step, the specific process is as follows:
s4.1: calculating the speed of the child node by adopting a speed obstacle algorithm, converting a single edge of a random tree into a VO-based scene, and defining a speed obstacle (VO) area of the unmanned plane relative to a dynamic obstacle as follows:
wherein D (p, r) represents centered around position p; a disc with a radius of length r; tau is a time window, namely the unmanned aerial vehicle and the barrier cannot collide in tau time;
a set of speeds representative of the possible collisions of the drone with respect to the obstacle O within the time window τ; p is a radical ofO|ARepresents the relative distance between drone a and obstacle O; v represents the speed of the fixed-wing aircraft; t represents time; r isAORepresents the sum of the total radii of drone a and obstacle O;
s4.2: the relative speed between the airplane and the obstacle is in the VO area, the airplane collides with the obstacle within the time interval tau, and a collision-free speed set is obtained by adopting an optimal mutual-reaction collision avoidance algorithm, and the collision-free speed set is defined as follows:
wherein the content of the first and second substances,
a set of velocities representing the stationary-wing aircraft a not to collide with respect to the obstacle O within the time window τ;
is the current speed of the aircraft; u represents the minimum velocity change of the relative velocity from the VO region, and n is an outer normal vector; the coefficient λ is a responsibility coefficient that determines how much responsibility the aircraft should assume, λ is 1 since the obstacle is uncooperative;
s4.3: calculating kinematic constraints S of unmanned aerial vehicleAThe expression of the collision-free speed set is as follows:
wherein the content of the first and second substances,
is the kinematic constraint S of the drone A relative to the obstacle OAAnd collision-free velocity set
And maximum speed
A feasible relative speed set under constraint; v. ofOIs the velocity of the obstacle O;
the flight path angle is taken as the kinematic constraint of the fixed-wing aircraft, and the stability of the flight path of the unmanned aerial vehicle can be ensured.
S4.4 New relative velocity
The sampling function based on the speed domain is obtained, and specifically:
wherein, R is a random number and is sampled in the interval of [0, 1 ]; α is a parameter to determine the probability of selecting the optimal speed;
s4.5: new velocity vA，newCalculated by the following formula:
wherein v isOIs the speed of the obstacle; new velocity vA，newEnsuring that a straight line path of the fixed-wing aircraft flying from the position of the father node to the position of the son node is collision-free and meeting the kinematic constraint of the fixed-wing aircraft;
s4.6: the edge between the father node and the child node is a cubic Bezier curve, the time consumption from the father node to the child node of the unmanned aerial vehicle is tau, the position of the child node can be obtained by calculation according to the state and the new speed of the father node, and the specific formula is as follows:
Nodenew.p←Nodeparent.p+τ*vA，new
wherein, NodenewRepresenting a new node; nodenewP represents the location of the new node; nodeparentRepresenting a parent node; nodeparentP represents the location of the parent node; τ represents a time window; v. ofA，newRepresents the new velocity generated by drone a;
the speed of the child node can be obtained by converting the new speed vA，newAbout an axis eaxisRotation thetarAngle obtaining;
wherein cos-1Representing an inverse cos function; v. ofA，curIs the speed of the parent node; v. ofA，newIs the velocity of the child node; represents a dot product; | vA，cur| represents vA，cur× denotes cross multiplication, thetarIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newThe included angle between them; e.g. of the typeaxisIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newNormal vector of the plane.
Preferably, α is specifically defined as:
α＝dist(pcur-pgoal)/dist(pstart-pgoal)，
wherein, Pcur、pgoalAnd PstartRespectively representing the current position, the target position and the initial position of the airplane; dist is a function of solving the Euclidean distance between positions;
with the current position and the eyeThe probability of selecting the optimal speed increases with decreasing distance of the target position. Calculated optimal speed
Closest to optimal speed
For guiding a fixed-wing aircraft towards a target position. Thus, the greater the probability of selecting the optimal speed, the faster the random tree grows in the location domain towards the target location.
When R is greater than α, the new relative speed
By subfunction Uniform in working space
Obtained by random uniform sampling, and relative speed when R is less than or equal to α
Selecting the desired speed generated by the subfunction Optimal
Closest optimal relative velocity
Defined by the following equation:
wherein the content of the first and second substances,
is the optimal relative velocity of the drone a relative to the obstacle 0, generated by the function;
is a fixed wing aircraft A against an obstacleDesired speed of object 0.
Preferably, in the fifth step, the formula of the bezier curve b(s) is as follows:
B(s)＝(1-s)3pp+3s(1-s)2pm1+3s2(1-s)pm2+s3pc.
where s is a real number, varying between 0 and 1; p is a radical ofpAnd pcThe positions of the parent node and the child node respectively; pm1And pm2Two control points are respectively arranged;
two control points Pm1And Pm2The formula of (a) is specifically:
pm1＝pp+vp*|pc-pp|/3，
pm2＝pc-vc*|pc-pp|/3.
wherein p ispIs the location of the parent node; v. ofpThe speed of the parent node; p is a radical ofcAs position p of a child nodec、vcIs the velocity of the child node.
Preferably, in the sixth step, the method for determining collision of an obstacle includes:
bezier curves b(s) are contained in convex hulls consisting of control points, which are expanded into convex polyhedrons according to the radius of the drone;
whether intersection exists between the convex polyhedron of the unmanned aerial vehicle and the convex polyhedron of the obstacle is judged by utilizing the separation axis theorem to detect the collision of the obstacle.
Compared with the prior art, the invention has the beneficial effects that: when the dynamic barrier is in the sensing range of the unmanned aerial vehicle, after the collision-free path is rebuilt and the dynamic barrier is passed through according to the collision-free path, the originally set obstacle-avoiding path is recovered to continue driving, so that path re-planning only needs to be executed once without continuously re-planning the path, and the path planning efficiency and the path stability are improved.
Drawings
Fig. 1 is a flow chart of an unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle in the invention.
Detailed Description
The drawings are for illustrative purposes only and are not to be construed as limiting the patent; for the purpose of better illustrating the embodiments, certain features of the drawings may be omitted, enlarged or reduced, and do not represent the size of an actual product; it will be understood by those skilled in the art that certain well-known structures in the drawings and descriptions thereof may be omitted. The positional relationships depicted in the drawings are for illustrative purposes only and are not to be construed as limiting the present patent.
The technical scheme of the invention is further described in detail by the following specific embodiments in combination with the attached drawings:
example 1
Fig. 1 shows an embodiment of an unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle, which includes the following steps:
the method comprises the following steps: in an initial state xstartInitializing a search tree for a root node, wherein a state represents a position and a velocity of the node in the tree;
step two: the cost of each edge of the random tree is divided into two parts, wherein the cost comprises the energy consumption cost of the node and the time consumption cost t; the energy consumption cost and the time consumption cost of the root node are both initialized to 0.
Step three: at each step of the random tree growth, a father node of the random tree is randomly selected;
step four: acquiring a new child node, wherein the position of the child node can be obtained by calculating according to the state of the parent node and the new speed, and the speed of the child node can be obtained by calculating the new speed vA,newAbout an axis eaxisRotation thetarAngle obtaining;
the specific process is as follows:
s4.1: calculating the speed of the child node by adopting a speed obstacle algorithm, converting a single edge of a random tree into a VO-based scene, and defining the VO area of the unmanned plane relative to the dynamic obstacle as follows:
wherein D (p, r) represents centered around position p; a disc with a radius of length r; tau is a time window, namely the unmanned aerial vehicle and the barrier cannot collide in tau time;
a set of speeds representative of the possible collisions of the drone with respect to the obstacle O within the time window τ; p is a radical ofO|ARepresents the relative distance between drone a and obstacle O; v represents the speed of the fixed-wing aircraft; t represents time; r isAORepresents the sum of the total radii of drone a and obstacle O;
s4.2: the relative speed between the airplane and the obstacle is in the VO area, the airplane collides with the obstacle within the time interval tau, and a collision-free speed set is obtained by adopting an optimal mutual-reaction collision avoidance algorithm, and the collision-free speed set is defined as follows:
wherein the content of the first and second substances,
a set of velocities representing the stationary-wing aircraft a not to collide with respect to the obstacle O within the time window τ;
is the current speed of the aircraft; u represents the minimum velocity change of the relative velocity from the VO region, and n is an outer normal vector; the coefficient λ is a responsibility coefficient that determines how much responsibility the aircraft should assume, λ is 1 since the obstacle is uncooperative;
s4.3: calculating kinematic constraints S of unmanned aerial vehicleAThe expression of the collision-free speed set is as follows:
wherein the content of the first and second substances,
is the kinematic constraint S of the drone A relative to the obstacle OAAnd collision-free velocity set
And maximum speed
A feasible relative speed set under constraint; v. ofOIs the velocity of the obstacle O;
the flight path angle is taken as the kinematic constraint of the fixed-wing aircraft, and the stability of the flight path of the unmanned aerial vehicle can be ensured.
S4.4 New relative velocity
The sampling function based on the speed domain is obtained, and specifically:
wherein, R is a random number and is sampled in the interval of [0, 1 ]; α is a parameter to determine the probability of selecting the optimal speed;
the specific definition of α is:
α＝dist(pcur-pgoal)/dist(pstart-pgoal)，
wherein p iscur、pgoalAnd pstartRespectively representing the current position, the target position and the initial position of the airplane; dist is a function of solving the Euclidean distance between positions; as the distance between the current position and the target position decreases, the probability of selecting the optimal speed increases. Calculated optimal speed
Closest to optimal speed
For guiding a fixed-wing aircraft towards a target position. Thus, selecting the optimum speedThe greater the probability, the faster the random tree grows in the location domain to the target location.
When R is greater than α, the new relative speed
By subfunction Uniform in working space
Obtained by random uniform sampling, and relative speed when R is less than or equal to α
Selecting the desired speed generated by the subfunction Optimal
Closest optimal relative velocity
Defined by the following equation:
wherein the content of the first and second substances,
is the optimal relative velocity of the drone a relative to the obstacle 0, generated by the function;
is the desired velocity of the fixed-wing aircraft a relative to the obstacle 0.
S4.5: new velocity vA，newCalculated by the following formula:
wherein v isOIs the speed of the obstacle; new velocity vA，newEnsuring that a fixed-wing aircraft flies to a child node from the position of a parent nodeThe straight path of the positions of the points is collision-free and satisfies the kinematic constraints of the fixed-wing aircraft;
s4.6: the edge between the father node and the child node is a cubic Bezier curve, the time consumption from the father node to the child node of the unmanned aerial vehicle is tau, the position of the child node can be obtained by calculation according to the state and the new speed of the father node, and the specific formula is as follows:
Nodenew·p←Nodeparent·p+τ*vA，new
wherein, NodenewRepresenting a new node; nodenewP represents the location of the new node; nodeparentRepresenting a parent node; nodeparentP represents the location of the parent node; τ represents a time window; v. ofA，newRepresents the new velocity generated by drone a;
the speed of the child node can be obtained by converting the new speed vA，newAbout an axis eaxisRotation thetarAngle obtaining;
wherein cos-1Representing an inverse cos function; v. ofA，curIs the speed of the parent node; v. ofA，newIs the velocity of the child node; represents a dot product; | vA，cur| represents vA，cur× denotes cross multiplication, thetarIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newThe included angle between them; e.g. of the typeaxisIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newNormal vector of the plane.
Step five: directly defining the edge between the father node and the new son node by using the cubic Bezier curve without post-processing; the formula for the bezier curve b(s) is as follows:
B(s)＝(1-s)3pp+3s(1-s)2pm1+3s2(1-s)pm2+s3pc·
where s is a real number, varying between 0 and 1; p is a radical ofpAnd pcThe positions of the parent node and the child node respectively; p is a radical ofm1And Pm2Two control points are respectively arranged;
two control points Pm1And Pm2The formula of (a) is specifically:
pm1＝pp+vp*|pc-pp|/3，
pm2＝pc-vc*|pc-pp|/3.
wherein p ispIs the location of the parent node; v. ofpThe speed of the parent node; p is a radical ofcAs position p of a child nodec、vcIs the velocity of the child node.
Step six: judging whether a newly generated edge collides with an obstacle in a scene of a dynamic obstacle and a static obstacle; the specific method comprises the following steps:
bezier curves b(s) are contained in convex hulls consisting of control points, which are expanded into convex polyhedrons according to the radius of the drone;
whether intersection exists between the convex polyhedron of the unmanned aerial vehicle and the convex polyhedron of the obstacle is judged by utilizing the separation axis theorem to detect the collision of the obstacle.
Step seven: calculating energy consumption cost and time consumption cost of a new node when the new node is added into the tree;
step eight: repeating the first step to the seventh step until the new node falls into the target area;
step nine: a feasible path is found in the random tree using backtracking.
The working principle or working process of the invention is as follows: in a dynamic environment containing a plurality of static obstacles and dynamic obstacles, the knowledge of the unmanned aerial vehicle on the static environment is known a priori, and a global path is generated offline through a spline-RRT algorithm by utilizing the prior knowledge of a map. When the drone is flying along a global path, the drone may utilize limited awareness when encountering an obstacle moving in front of the droneThe on-board sensors of range detect dynamic obstacles. When the aircraft detects a dynamic obstacle in the sensing range, the method establishes a random search tree in the sensing range by using the current position and speed as a starting node xstartTaking the intersection point position of the global path and the sensing range as a target position xgoal. Since the speed of the drone is changing, the speed range of the drone relative to the dynamic obstacle also changes. And sampling in the speed feasible region to obtain feasible speed, and then generating a new node. After collision detection is performed on the generated edges, the tree is updated, and the process is repeated until the random tree grows to reach the target position xgoal. And obtaining a local path in the random tree by using a backtracking method, and continuously flying along the original global path until the unmanned aerial vehicle reaches a target area after passing through the dynamic barrier along the local path.
The beneficial effects of this embodiment: when the dynamic barrier is in the sensing range of the unmanned aerial vehicle, after the collision-free path is rebuilt and the dynamic barrier is passed through according to the collision-free path, the originally set obstacle-avoiding path is recovered to continue driving, so that path re-planning only needs to be executed once without continuously re-planning the path, and the path planning efficiency and the path stability are improved.
It should be understood that the above-described embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. Other variations and modifications will be apparent to persons skilled in the art in light of the above description. And are neither required nor exhaustive of all embodiments. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention should be included in the protection scope of the claims of the present invention.
Claims (6)
1. An unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle is characterized by comprising the following steps:
the method comprises the following steps: in an initial state xstartA search tree is initialized for a root node, and a state represents the position of the node in the treeAnd speed;
step two: the cost of each edge of the random tree is divided into two parts, including the energy consumption cost of the node and the time consumption cost t;
step three: at each step of the random tree growth, a father node of the random tree is randomly selected;
step four: acquiring a new child node, wherein the position of the child node can be obtained by calculating according to the state of the parent node and the new speed, and the speed of the child node can be obtained by calculating the new speed vA，newAbout an axis eaxisRotation thetarAngle obtaining;
step five: directly defining the edge between the father node and the new son node by using the cubic Bezier curve without post-processing;
step six: judging whether a newly generated edge collides with an obstacle in a scene of a dynamic obstacle and a static obstacle;
step seven: calculating energy consumption cost and time consumption cost of a new node when the new node is added into the tree;
step eight: repeating the first step to the seventh step until the new node falls into the target area;
step nine: a feasible path is found in the random tree using backtracking.
2. The spline-rrt and speed obstacle based unmanned aerial vehicle obstacle avoidance method according to claim 1, wherein in the second step, both the energy consumption cost and the time consumption cost of the root node are initialized to 0.
3. The spline-rrt and speed obstacle based unmanned aerial vehicle obstacle avoidance method according to claim 1, wherein in the fourth step, the specific process is as follows:
s4.1: adopting a speed obstacle algorithm to calculate the speed of the child node, converting a single edge of the random tree into a scene based on V0, and defining the speed obstacle area of the unmanned plane relative to the dynamic obstacle as follows:
wherein D (p, r) represents centered around position p; a disc with a radius of length r; tau is a time window, namely the unmanned aerial vehicle and the barrier cannot collide in tau time;
a set of speeds representing the possible collisions of the drone with respect to the obstacle 0 within the time window τ; p is a radical ofO|ARepresents the relative distance between drone a and obstacle O; v represents the speed of the fixed-wing aircraft; t represents time; r isAORepresents the sum of the total radii of drone a and obstacle O;
s4.2: the relative speed between the aircraft and the obstacle is in a speed obstacle area, the aircraft collides with the obstacle within a time interval tau, and a collision-free speed set is obtained by adopting an optimal mutual-reaction collision avoidance algorithm, and the collision-free speed set is defined as follows:
wherein the content of the first and second substances,
a set of velocities representing the stationary-wing aircraft a not to collide with respect to the obstacle O within the time window τ;
is the current speed of the aircraft; u represents the minimum velocity change of the relative velocity leaving the velocity obstacle region, and n is an external normal vector; the coefficient λ is a responsibility coefficient that determines how much responsibility the aircraft should assume, λ is 1 since the obstacle is uncooperative;
s4.3: calculating kinematic constraints S of unmanned aerial vehicleAThe expression of the collision-free speed set is as follows:
wherein the content of the first and second substances,
is the kinematic constraint S of the drone A relative to the obstacle OAAnd collision-free velocity set
And maximum speed
A feasible relative speed set under constraint; v. ofOIs the velocity of the obstacle O;
s4.4: new relative velocity
The sampling function based on the speed domain is obtained, and specifically:
wherein, R is a random number and is sampled in the interval of [0, 1 ]; α is a parameter to determine the probability of selecting the optimal speed;
s4.5: new velocity vA，newCalculated by the following formula:
wherein v isOIs the speed of the obstacle; new velocity vA，newEnsuring that a straight line path of the fixed-wing aircraft flying from the position of the father node to the position of the son node is collision-free and meeting the kinematic constraint of the fixed-wing aircraft;
s4.6: the edge between the father node and the child node is a cubic Bezier curve, the time consumption from the father node to the child node of the unmanned aerial vehicle is tau, the position of the child node can be obtained by calculation according to the state and the new speed of the father node, and the specific formula is as follows:
Nodenew·p←Nodeparent·p+τ*vA，new
wherein, NodenewRepresenting a new node; nodenewP represents the location of the new node; nodeparentRepresenting a parent node; nodeparentP represents the location of the parent node; τ represents a time window; v. ofA，newRepresents the new velocity generated by drone a;
the speed of the child node can be obtained by converting the new speed vA，newAbout an axis eaxisRotation thetarAngle obtaining;
wherein cos-1Representing an inverse cos function; v. ofA，curIs the speed of the parent node; v. ofA，newIs the velocity of the child node; represents a dot product; | vA，cur| represents vA，cur× denotes cross multiplication, thetarIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newThe included angle between them; e.g. of the typeaxisIs the velocity v of the parent nodeA，curAnd generating a new velocity vA，newNormal vector of the plane.
4. The spline-rrt and speed obstacle based unmanned aerial vehicle obstacle avoidance method according to claim 3, wherein α is specifically defined as:
α＝dist(pcur-pgoal)/dist(pstart-pgoal)，
wherein, Pcur、pgoalAnd PstartRespectively representing the current position, the target position and the initial position of the airplane; dist is a function of solving the Euclidean distance between positions;
when R is greater than α, the new relative speed
By subfunction Uniform in working space
Obtained by random uniform sampling, and relative speed when R is less than or equal to α
Selecting the desired speed generated by the subfunction Optimal
Closest optimal relative velocity
Defined by the following equation:
wherein the content of the first and second substances,
is the optimal relative velocity of the drone a relative to the obstacle O as a function generated;
is the desired velocity of the fixed-wing aircraft a relative to the obstacle O.
5. The spline-rrt and speed obstacle based unmanned aerial vehicle obstacle avoidance method according to claim 3, wherein in the step five, the formula of the Bezier curve B(s) is as follows:
B(s)(1-s)3pp+3s(1-s)2pm1+3s2(1-s)pm2+s3pc.
where s is a real number, varying between 0 and 1; p is a radical ofpAnd pcThe positions of the parent node and the child node respectively; pm1And Pm2Two control points are respectively arranged;
two control points pm1And pm2The formula of (a) is specifically:
pm1＝pp+vp*|pc-pp|/3，
pm2＝pc-vc*|pc-pp|/3.
wherein p ispIs the location of the parent node; v. ofpThe speed of the parent node; p is a radical ofcAs position p of a child nodec、vcIs the velocity of the child node.
6. The unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle as claimed in claim 1, wherein in the sixth step, the method for judging obstacle collision is as follows:
bezier curves b(s) are contained in convex hulls consisting of control points, which are expanded into convex polyhedrons according to the radius of the drone;
whether intersection exists between the convex polyhedron of the unmanned aerial vehicle and the convex polyhedron of the obstacle is judged by utilizing the separation axis theorem to detect the collision of the obstacle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010309762.5A CN111580548B (en) | 2020-04-17 | 2020-04-17 | Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010309762.5A CN111580548B (en) | 2020-04-17 | 2020-04-17 | Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111580548A true CN111580548A (en) | 2020-08-25 |
| CN111580548B CN111580548B (en) | 2021-10-19 |
Family
ID=72127073
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010309762.5A Active CN111580548B (en) | 2020-04-17 | 2020-04-17 | Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111580548B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112596549A (en) * | 2020-12-29 | 2021-04-02 | 中山大学 | Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule |
| CN113359796A (en) * | 2021-06-08 | 2021-09-07 | 同济大学 | Unmanned aerial vehicle searching method for underground multi-branch cave |
| CN113467500A (en) * | 2021-07-19 | 2021-10-01 | 天津大学 | Unmanned aerial vehicle non-cooperative target tracking system based on binocular vision |
| CN114281087A (en) * | 2021-12-31 | 2022-04-05 | 东南大学 | Path planning method based on lifetime planning A and speed obstacle method |
| CN114721412A (en) * | 2022-03-16 | 2022-07-08 | 北京理工大学 | Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control |
| JP7459009B2 (en) | 2021-02-12 | 2024-04-01 | 三菱重工業株式会社 | Route planning device, mobile object, route planning method and program |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104155974A (en) * | 2013-07-29 | 2014-11-19 | 深圳信息职业技术学院 | Path planning method and apparatus for robot fast collision avoidance |
| CN104236548A (en) * | 2014-09-12 | 2014-12-24 | 清华大学 | Indoor autonomous navigation method for micro unmanned aerial vehicle |
| EP2955482A1 (en) * | 2014-06-10 | 2015-12-16 | Sikorsky Aircraft Corporation | Aircraft motion planning method |
| US20170241790A1 (en) * | 2016-02-24 | 2017-08-24 | Honda Motor Co., Ltd. | Path plan generating apparatus for mobile body |
| CN108958292A (en) * | 2018-08-23 | 2018-12-07 | 北京理工大学 | A kind of aircraft based on RRT* algorithm is dashed forward anti-method for planning track |
| CN109491389A (en) * | 2018-11-23 | 2019-03-19 | 河海大学常州校区 | A kind of robot trace tracking method with constraint of velocity |
| CN109990787A (en) * | 2019-03-15 | 2019-07-09 | 中山大学 | The method of dynamic barrier is evaded in complex scene by a kind of robot |
| CN110162104A (en) * | 2019-06-14 | 2019-08-23 | 哈尔滨工业大学 | The RRT unmanned plane paths planning method of imitative Driver Vision perception |
| CN110531770A (en) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | One kind being based on improved RRT paths planning method and system |
| CN110703768A (en) * | 2019-11-08 | 2020-01-17 | 福州大学 | Improved dynamic RRT mobile robot motion planning method |
-
2020
- 2020-04-17 CN CN202010309762.5A patent/CN111580548B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104155974A (en) * | 2013-07-29 | 2014-11-19 | 深圳信息职业技术学院 | Path planning method and apparatus for robot fast collision avoidance |
| EP2955482A1 (en) * | 2014-06-10 | 2015-12-16 | Sikorsky Aircraft Corporation | Aircraft motion planning method |
| CN104236548A (en) * | 2014-09-12 | 2014-12-24 | 清华大学 | Indoor autonomous navigation method for micro unmanned aerial vehicle |
| US20170241790A1 (en) * | 2016-02-24 | 2017-08-24 | Honda Motor Co., Ltd. | Path plan generating apparatus for mobile body |
| CN108958292A (en) * | 2018-08-23 | 2018-12-07 | 北京理工大学 | A kind of aircraft based on RRT* algorithm is dashed forward anti-method for planning track |
| CN109491389A (en) * | 2018-11-23 | 2019-03-19 | 河海大学常州校区 | A kind of robot trace tracking method with constraint of velocity |
| CN109990787A (en) * | 2019-03-15 | 2019-07-09 | 中山大学 | The method of dynamic barrier is evaded in complex scene by a kind of robot |
| CN110162104A (en) * | 2019-06-14 | 2019-08-23 | 哈尔滨工业大学 | The RRT unmanned plane paths planning method of imitative Driver Vision perception |
| CN110531770A (en) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | One kind being based on improved RRT paths planning method and system |
| CN110703768A (en) * | 2019-11-08 | 2020-01-17 | 福州大学 | Improved dynamic RRT mobile robot motion planning method |
Non-Patent Citations (2)
| Title |
|---|
| JIAWEI MENG: "UAV Path Planning System Based on 3D Informed RRT* for Dynamic Obstacle Avoidance", 《2018 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND BIOMIMETICS (ROBIO)》 * |
| 江泽强: "改进 RRT 算法在无人机航迹规划中的应用", 《计算机与数学工程》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112596549A (en) * | 2020-12-29 | 2021-04-02 | 中山大学 | Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule |
| CN112596549B (en) * | 2020-12-29 | 2021-12-21 | 中山大学 | Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule |
| JP7459009B2 (en) | 2021-02-12 | 2024-04-01 | 三菱重工業株式会社 | Route planning device, mobile object, route planning method and program |
| CN113359796A (en) * | 2021-06-08 | 2021-09-07 | 同济大学 | Unmanned aerial vehicle searching method for underground multi-branch cave |
| CN113359796B (en) * | 2021-06-08 | 2022-09-06 | 同济大学 | Unmanned aerial vehicle searching method for underground multi-branch cave |
| CN113467500A (en) * | 2021-07-19 | 2021-10-01 | 天津大学 | Unmanned aerial vehicle non-cooperative target tracking system based on binocular vision |
| CN113467500B (en) * | 2021-07-19 | 2022-10-11 | 天津大学 | Unmanned aerial vehicle non-cooperative target tracking system based on binocular vision |
| CN114281087A (en) * | 2021-12-31 | 2022-04-05 | 东南大学 | Path planning method based on lifetime planning A and speed obstacle method |
| CN114281087B (en) * | 2021-12-31 | 2023-11-03 | 东南大学 | Path planning method based on life planning A and speed obstacle method |
| CN114721412A (en) * | 2022-03-16 | 2022-07-08 | 北京理工大学 | Unmanned aerial vehicle trajectory tracking obstacle avoidance method based on model predictive control |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111580548B (en) | 2021-10-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111580548B (en) | Unmanned aerial vehicle obstacle avoidance method based on spline-rrt and speed obstacle | |
| Liu et al. | High speed navigation for quadrotors with limited onboard sensing | |
| CN111399506B (en) | Global-local hybrid unmanned ship path planning method based on dynamic constraint | |
| CN110320933B (en) | Unmanned aerial vehicle obstacle avoidance movement planning method under cruise task | |
| Maini et al. | Path planning for a UAV with kinematic constraints in the presence of polygonal obstacles | |
| Hrabar | Reactive obstacle avoidance for rotorcraft uavs | |
| CN109871031B (en) | Trajectory planning method for fixed-wing unmanned aerial vehicle | |
| CN108196451B (en) | Bionic fish swarm obstacle avoidance behavior control method | |
| Yeo et al. | Onboard flow sensing for downwash detection and avoidance with a small quadrotor helicopter | |
| Manathara et al. | Reactive collision avoidance of multiple realistic UAVs | |
| Ingersoll et al. | UAV path-planning using Bezier curves and a receding horizon approach | |
| Cole et al. | Reactive trajectory generation for multiple vehicles in unknown environments with wind disturbances | |
| Lee et al. | RRT-based path planning for fixed-wing UAVs with arrival time and approach direction constraints | |
| Chen et al. | Collision-free UAV navigation with a monocular camera using deep reinforcement learning | |
| Ryll et al. | Semantic trajectory planning for long-distant unmanned aerial vehicle navigation in urban environments | |
| Chen et al. | Obstacle avoidance strategy for quadrotor UAV based on improved particle swarm optimization algorithm | |
| Zhang et al. | Collision Avoidance of Fixed-Wing UAVs in Dynamic Environments Based on Spline-RRT and Velocity Obstacle | |
| Alturbeh et al. | Real-time obstacle collision avoidance for fixed wing aircraft using b-splines | |
| Angelov et al. | A passive approach to autonomous collision detection and avoidance | |
| Dubey et al. | Droan—disparity-space representation for obstacle avoidance | |
| JP2020529349A (en) | Systems and methods for controlling aircraft based on detected air movements | |
| Masehian et al. | Path planning of nonholonomic flying robots using a new virtual obstacle method | |
| CN115494866A (en) | Multi-unmanned aerial vehicle global and local path intelligent planning method and system | |
| Palmer et al. | Co-operative collision avoidance for unmanned aerial vehicles using both centralised and decoupled approaches | |
| Alves | Path planning and collision avoidance algorithms for small RPAS |
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 |