CN110146085B - Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration - Google Patents
Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration Download PDFInfo
- Publication number
- CN110146085B CN110146085B CN201910460860.6A CN201910460860A CN110146085B CN 110146085 B CN110146085 B CN 110146085B CN 201910460860 A CN201910460860 A CN 201910460860A CN 110146085 B CN110146085 B CN 110146085B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- point
- random tree
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention belongs to the technical field of unmanned aerial vehicle systems, and discloses an unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration. In the flight process of the unmanned aerial vehicle, an octree environment local map taking the unmanned aerial vehicle as a center is constructed by utilizing an airborne depth camera sensor, and the local map is updated in real time along with the motion of the unmanned aerial vehicle. On the basis of a local map, a sampling-based path planning method for rapidly exploring a random tree is adopted to find key position points of an obstacle avoidance path of the unmanned aerial vehicle, and a path formed by path key position point line segments is converted into a smooth and dynamic flyable track by using a uniform Betz curve. The method provided by the invention is simple and can be used for real-time obstacle avoidance in the unmanned aerial vehicle airborne system with limited computing resources.
Description
Technical Field
The invention mainly relates to the technical field of unmanned aerial vehicle systems, in particular to an unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration. The method provided by the invention is simple and can be used for real-time obstacle avoidance in the unmanned aerial vehicle airborne system with limited computing resources.
Background
In recent years, the unmanned aerial vehicle has the characteristics of high agility, high maneuverability, easy control and the like, so that the unmanned aerial vehicle is widely applied to the application fields of aerial photography, routing inspection, logistics, rescue and the like. In most cases the mission environment of the drone is unknown or there are unpredictable obstacles. In order to meet the requirement of completely autonomous flight in an unknown complex environment, the method has important significance in generating a smooth and dynamically feasible track by real-time local re-planning. There are two main types of methods for avoiding local collisions: the first type is a pure reactive method, which is directly planned according to data of a sensor without constructing an environment map, but the method has high calculation speed, is not suitable for a disordered environment and can be seriously influenced by falling into a local minimum value; the second type is a map-based local obstacle avoidance method, which uses various technologies to calculate a feasible and locally optimal path through a local map constructed from sensor data or known environmental information, and the difficulty of local map construction lies in the rapidity of map construction. This patent expects through constructing octree local map, carries out real-time local orbit replanning and generates no collision orbit, makes unmanned aerial vehicle have the ability of safe autonomous flight in unknown complex environment.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: aiming at the requirement of autonomous obstacle avoidance of the unmanned aerial vehicle, the unmanned aerial vehicle local track re-planning method based on drawing construction and rapid random tree exploration is provided.
Aiming at the problems in the prior art, the invention mainly comprises three parts:
1. local map construction based on a three-dimensional circular buffer area: the method comprises the steps of obtaining point cloud data through a depth distance sensor, utilizing a voxel filter to conduct down-sampling on the point cloud, then converting the point cloud into a grid map stored in an octree form, and enabling the grid map to exist in a circular buffer area with a three-dimensional array as a storage format.
(1) OctreeImage construction
And obtaining a point cloud through a depth distance sensor, and performing down-sampling on the point cloud by using a voxel filter. The point cloud is then converted into a grid map stored in the form of an octree. The concrete contents are as follows:
and y epsilon R is used for representing the occupation condition of the nodes in the octree map. Increasing the value of y while continuously sensing that the node is occupied; otherwise, the value of y is decreased. The probability of the node being occupied is represented by x, and the greater the probability, the higher the probability of being occupied. The translation between x and y is as follows:
wherein x is 0.5, which indicates that the system is not determined, the map construction and the real-time update are realized through the probability logarithm conversion, and the obstacles in the environment are dynamically modeled.
(2) Local map construction based on three-dimensional circular buffer area
And constructing a local map through a three-dimensional circular buffer zone taking the unmanned aerial vehicle as a center based on the constructed octree map. The circular buffer is of size N (N-2)p) A three-dimensional array of pixels). The concrete contents are as follows:
assuming the resolution of the octree map is r, a point p (x, y, z) in three-dimensional space is mapped to a point in the bufferp'(x',y',z')Is mapped as ：
Assume that the center point of the buffer is set to O (O)1,o2,o3) Whether point p' is in the buffer can be determined by the following formula:
the buffer moves with the quadrotors. Let coordinate of four rotors be Q (Q)1,q2,q3). The initial position of the center of the buffer area is the starting point of four rotors. With the movement of the four-rotor aircraft, when q isi-oiAt > 0, oi＝oi+ d. Otherwise, oi＝oiD, d is the moving step of the center of the buffer area.
2. Generating a path key position point based on a fast exploration random tree: initializing a first node of a random tree, then generating an arbitrary random point in the whole state space in each circulation process, traversing each node in the random tree after the random point is generated, calculating the distance between each node and the random point, finding out the node closest to the random point, and generating a new node in an expansion manner in the connecting line direction of the two nodes, namely generating a path key position point; and updating the father node of the new node by searching adjacent nodes in a specified range near the new node to serve as a father node alternative for replacing the new node, and selecting the point with the minimum path cost as the father node of the new node. The concrete contents are as follows:
(1) path key position point generation based on rapid exploration random tree algorithm
First, the tree is initialized to generate the first node xinitDuring each cycle, a random point x is generatedrand. The generation of random points is arbitrary, i.e. can be within the whole state space. After generating random points, traversing each node in the random tree, calculating the distance between each node and the random points, finding out the node closest to the random points, and marking as xnearest. At xnearestAnd xrandThe extension of the connecting line direction generates a new node xnewAnd the new node is the key position point of the path. Fast exploration random tree algorithm at new node xnewFinding neighboring nodes within a specified range r of the neighborhood as an alternative xnewAn alternative to the parent node. The point with the minimum path cost is finally selected to be the xnewThe parent node of (2). The fast-exploration random tree algorithm then reroutes the tree to minimize the path cost of all nodes.
3. Trajectory smoothing based on uniform bezier curves splines: and fitting the path key position points generated by rapidly exploring the random tree by adopting a quintic uniform Betz curve spline to obtain a smooth track based on the smooth part of the track of the uniform Betz curve spline. The concrete contents are as follows:
and fitting the path key position points generated by rapidly exploring the random tree by using a uniform Betz curve spline to obtain a smooth track. The k-1 th order bezier curve spline can be represented by the following equation:
pi∈R3is thattiControl points of time-of-Betz-curve spline, Bi,k(t) is a basis function. In order for the trajectory to be able to be followed by the drone, the trajectory must continue to the fourth derivative of position. The trajectories are thus represented using quintic uniform bezier splines. There is a fixed time interval Δ t between the spline control points of the uniform bezier curve. For a 5-th order Betz curve spline, at time t ∈ [ t ]i,ti+1) The value of p (t) depends only on 6 consecutive control points [ p ]i,pi+1,…,pi+5]. Order:
the trajectory can be expressed as:
wherein M iskIs a matrix of size k x k
Let P be [ P ]i,pi+1,…,pi+5]TThe velocity and acceleration at any time on the trajectory can be calculated.
Since the smoothed trajectory may deviate from the original path, there is a possibility of collision with the obstacle. The trajectory is optimized by adding a midpoint in the middle of the path. Let p be1，p2，p3Key location points, m, of a path planned for fast exploration of stochastic tree algorithms1For a track segment p1p2Middle point of (1), m2For a track segment p2p3The midpoint of (a). m is1And m2Is collision-free because it lies on the optimal piecewise-linear path returned by the fast-exploration random tree search algorithm. Calculate m1And m2End point p'2Judgment of p1p'2And p'2p3Whether or not a collision with an obstacle occurs. If not, p'2Substitution of p in the pathway2. The problem of collision between the Betz curve spline track and an obstacle is solved by adding the key position points of the path, and the safety of the track can be ensured by carrying out multiple times of interpolation when necessary.
By using the method provided by the invention, the problems of autonomous obstacle avoidance and path planning of the unmanned aerial vehicle in an unknown environment can be solved. By the aid of the constructed octree local map, obstacles in the environment can be rapidly modeled in a cluttered environment, and time required is millisecond-level. The method comprises the steps of generating path key position points in real time by using a fast exploration random tree in a local map, fitting the position points by using uniform Betz curve splines to obtain a smooth and dynamically feasible collision-free track, realizing real-time avoidance of local obstacles, and enabling the unmanned aerial vehicle to have the capability of safe and autonomous flight in an unknown complex environment.
Drawings
Fig. 1 is a schematic diagram of a result of a trajectory planning method.
Fig. 2 is a schematic diagram of a result of a local map construction method.
Fig. 3 is a schematic diagram of a result of the real-time local trajectory re-planning method.
Detailed Description
The invention is further described with reference to the following figures and detailed description.
(1) Local map construction based on three-dimensional circular buffer area
Step 1: and acquiring depth information of each point in the image from the depth distance camera, and converting the depth information into point cloud.
Step 2: the point cloud is downsampled using a voxel filter with a grid size of 0.2 mx 0.2 m.
And step 3: the downsampled point cloud is inserted with a resolution of 0.2m according to equation (1)3In the octree map of (1).
And 4, step 4: according to equation (2), the points in the octree map are mapped into a circular buffer consisting of a three-dimensional array of length 64.
And 5: the central point of the buffer area is set as the starting point of the unmanned aerial vehicle, and a local map which follows the unmanned aerial vehicle to move and is updated in real time is obtained, as shown in fig. 1.
(2) Path control point generation and track smoothing based on rapid exploration random tree
Step 1: firstly, a starting point S and a target point T of an unmanned aerial vehicle for executing a task are determined, a local map is constructed, and the local map is updated in real time along with the flight of the unmanned aerial vehicle.
Step 2: and recording the current time t, and calculating a position point p on the global track corresponding to the time t as a target point of the local planning according to the formula (1).
And step 3: based on the constructed local map, according to the formula (3), if the point p is located in the local map and the point p is not occupied by the barrier, a collision-free path from the starting point S to the target point p is obtained by adopting a fast exploration random tree algorithm, and then a safe path point is obtained. Otherwise, after waiting for Δ t seconds, re-executing step (2).
And 4, step 4: according to equation (6), the path points are inserted into the spline of the bezz curve as control points, and the control points are fitted to obtain a smooth and dynamically feasible trajectory, as shown in fig. 2.
And 5: and optimizing the track by adopting a method of increasing path points, so that the smoothed track is closer to the initial path, and the safety of the track is ensured.
Step 6: and (4) according to the formula (8), sending the obtained track and the calculated speed and acceleration to the unmanned aerial vehicle, and enabling the unmanned aerial vehicle to follow the track according to the specified speed.
And 7: and (3) repeating the processes from the step (2) to the step (6) at a time interval delta T until the unmanned aerial vehicle reaches a final target point T, as shown in FIG. 3.
The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may be made by those skilled in the art without departing from the principle of the invention.
Claims (3)
1. An unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration is characterized by mainly comprising three parts: firstly, constructing a local octree map; secondly, generating a path key position point based on a fast exploration random tree on the basis of the local octree map; thirdly, based on the key position points, performing track fitting smoothing by using uniform Betz curve splines; the local octree map is stored in a circular buffer; the circulating buffer area and the unmanned aerial vehicle move simultaneously, and the center of the buffer area of the circulating buffer area moves in a preset step length; the local octree map takes an unmanned aerial vehicle as a center;
the method for generating the route key position points based on the rapid exploration random tree on the basis of the map comprises the following steps: firstly, initializing a first node of a random tree, wherein the node represents a position point in an octree map state space; setting the search times of a random tree, firstly generating an arbitrary random point in an octree map state space in the process of each search, traversing each node in the random tree after the random point is generated, calculating the distance between each node and the random point, finding out the node closest to the random point, and then expanding and generating a new node in the connecting line direction of the two nodes to obtain a path key position point;
before generating a path key position point based on a fast exploration random tree on the basis of a local octree map, the method further comprises the following steps:
recording the current time t, and calculating a position point p on the global track corresponding to the time t as a target point of local planning;
judging whether the target point is located in the local octree map and occupied by the barrier;
if yes, generating a path key position point on the local octree map based on a fast exploration random tree;
point cloud data are obtained through a depth distance sensor, a voxel filter is used for carrying out downsampling on the point cloud, then the point cloud is converted into a grid map stored in an octree form, and the grid map is stored in a circular buffer area with a three-dimensional array as a storage format.
2. The unmanned aerial vehicle real-time evasive re-planning method based on mapping and fast exploration random tree of claim 1,
the path key position point generation based on the rapid exploration random tree comprises the steps that after a new node is obtained, the rapid exploration random tree needs to be re-wired to enable the cost of a path to be minimum; first, it is necessary to search for an adjacent node within a prescribed range near the new node as a parent node alternative for replacing the new node, and then select a point that minimizes the path cost as the parent node of the new node.
3. The unmanned aerial vehicle real-time evasive re-planning method based on mapping and fast exploration random tree of claim 1,
and fitting the path control points generated by rapidly exploring the random tree by adopting a quintic uniform Betz curve spline to obtain a smooth track based on the smooth part of the track of the uniform Betz curve spline.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910460860.6A CN110146085B (en) | 2019-05-30 | 2019-05-30 | Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910460860.6A CN110146085B (en) | 2019-05-30 | 2019-05-30 | Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110146085A CN110146085A (en) | 2019-08-20 |
| CN110146085B true CN110146085B (en) | 2022-04-19 |
Family
ID=67592138
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910460860.6A Active CN110146085B (en) | 2019-05-30 | 2019-05-30 | Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110146085B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113741490A (en) * | 2020-05-29 | 2021-12-03 | 广州极飞科技股份有限公司 | Inspection method, inspection device, aircraft and storage medium |
| CN111707269B (en) * | 2020-06-23 | 2022-04-05 | 东南大学 | Unmanned aerial vehicle path planning method in three-dimensional environment |
| CN111752306B (en) * | 2020-08-14 | 2022-07-12 | 西北工业大学 | Unmanned aerial vehicle route planning method based on fast-expanding random tree |
| CN112379681A (en) * | 2020-11-02 | 2021-02-19 | 中国兵器工业计算机应用技术研究所 | Unmanned aerial vehicle obstacle avoidance flight method and device and unmanned aerial vehicle |
| CN112525195B (en) * | 2020-11-20 | 2022-03-01 | 中国人民解放军国防科技大学 | Multi-target genetic algorithm-based aircraft track rapid planning method |
| CN112747736B (en) * | 2020-12-22 | 2022-07-08 | 西北工业大学 | Indoor unmanned aerial vehicle path planning method based on vision |
| CN114200933A (en) * | 2021-12-06 | 2022-03-18 | 太原科技大学 | Three-dimensional path planning method for bridge crane |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104965518A (en) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | Power inspection tour flying robot air route planning method based on three-dimensional digital map |
| CN106737671A (en) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | The bilayer personification motion planning method of seven degrees of freedom copy man mechanical arm |
| CN106949893A (en) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | The Indoor Robot air navigation aid and system of a kind of three-dimensional avoidance |
| CN107631734A (en) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | A kind of dynamic smoothing paths planning method based on D*_lite algorithms |
| CN108088445A (en) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 3 d grid map path planning system and method based on octree representation |
| CN108776492A (en) * | 2018-06-27 | 2018-11-09 | 电子科技大学 | A kind of four-axle aircraft automatic obstacle avoiding and air navigation aid based on binocular camera |
| CN108876024A (en) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | Path planning, path real-time optimization method and device, storage medium |
| KR20180135695A (en) * | 2017-06-13 | 2018-12-21 | 성균관대학교산학협력단 | A method for checking empty space to install devices, install path, and instal order |
| CN109582032A (en) * | 2018-10-11 | 2019-04-05 | 天津大学 | Quick Real Time Obstacle Avoiding routing resource of the multi-rotor unmanned aerial vehicle under complex environment |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104867174B (en) * | 2015-05-08 | 2018-02-23 | 腾讯科技（深圳）有限公司 | A kind of three-dimensional map rendering indication method and system |
-
2019
- 2019-05-30 CN CN201910460860.6A patent/CN110146085B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104965518A (en) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | Power inspection tour flying robot air route planning method based on three-dimensional digital map |
| CN108088445A (en) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 3 d grid map path planning system and method based on octree representation |
| CN106737671A (en) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | The bilayer personification motion planning method of seven degrees of freedom copy man mechanical arm |
| CN106949893A (en) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | The Indoor Robot air navigation aid and system of a kind of three-dimensional avoidance |
| KR20180135695A (en) * | 2017-06-13 | 2018-12-21 | 성균관대학교산학협력단 | A method for checking empty space to install devices, install path, and instal order |
| CN107631734A (en) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | A kind of dynamic smoothing paths planning method based on D*_lite algorithms |
| CN108876024A (en) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | Path planning, path real-time optimization method and device, storage medium |
| CN108776492A (en) * | 2018-06-27 | 2018-11-09 | 电子科技大学 | A kind of four-axle aircraft automatic obstacle avoiding and air navigation aid based on binocular camera |
| CN109582032A (en) * | 2018-10-11 | 2019-04-05 | 天津大学 | Quick Real Time Obstacle Avoiding routing resource of the multi-rotor unmanned aerial vehicle under complex environment |
Non-Patent Citations (2)
| Title |
|---|
| 刘阿敏.基于三维点云图的无人机路径规划.《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》.2019,(第02期),第11-14,21-23,45-46,50,52页. * |
| 基于三维点云图的无人机路径规划;刘阿敏;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20190215(第02期);第11-14,21-23,45-46,50,52页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110146085A (en) | 2019-08-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110146085B (en) | Unmanned aerial vehicle real-time avoidance rescheduling method based on graph building and rapid random tree exploration | |
| CN107063280B (en) | Intelligent vehicle path planning system and method based on control sampling | |
| US11698638B2 (en) | System and method for predictive path planning in autonomous vehicles | |
| EP3306431B1 (en) | A computer-implemented method and a system for guiding a vehicle within a scenario with obstacles | |
| CN112068588A (en) | Unmanned aerial vehicle trajectory generation method based on flight corridor and Bezier curve | |
| WO2018190833A1 (en) | Swarm path planner system for vehicles | |
| CN112432648A (en) | Real-time planning method for safe motion trail of mobile robot | |
| Shanmugavel et al. | Collision avoidance and path planning of multiple UAVs using flyable paths in 3D | |
| Farmani et al. | Tracking multiple mobile targets using cooperative unmanned aerial vehicles | |
| CN112344938B (en) | Space environment path generation and planning method based on pointing and potential field parameters | |
| Yu et al. | A vision-based collision avoidance technique for micro air vehicles using local-level frame mapping and path planning | |
| CN112650306A (en) | Unmanned aerial vehicle motion planning method based on dynamics RRT | |
| WO2023147769A1 (en) | Systems, methods, and computer-readable media for spatio-temporal motion planning | |
| CN115525063A (en) | Trajectory planning method for unmanned helicopter | |
| Fragoso et al. | Dynamically feasible motion planning for micro air vehicles using an egocylinder | |
| Rocha et al. | A uav global planner to improve path planning in unstructured environments | |
| KR20230083846A (en) | Travel Path Planning Method of Aerial Vehicles in 3-dimensional Environment | |
| CN116048120B (en) | Autonomous navigation system and method for small four-rotor unmanned aerial vehicle in unknown dynamic environment | |
| CN117055556A (en) | Multi-robot formation path planning method and device, electronic equipment and storage medium | |
| CN117075612A (en) | Rapid path planning based on improved visual view and track optimization method based on ESDF map | |
| Dubey et al. | Droan—disparity-space representation for obstacle avoidance | |
| CN115016546B (en) | Unmanned aerial vehicle three-dimensional path planning method and device, electronic equipment and storage medium | |
| Marlow et al. | Local terrain mapping for obstacle avoidance using monocular vision | |
| CN116610109A (en) | Gradient-based forward ant colony algorithm unmanned vehicle path planning method | |
| CN115981313A (en) | Robot path planning method, system, electronic device and storage medium |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Niu Diefeng Inventor after: Ma Zhaowei Inventor after: Hu Jia Inventor after: Wu Lizhen Inventor before: Niu Diefeng Inventor before: Ma Zhaowei Inventor before: Hu Jia Inventor before: Wu Lizhen |
|
| CB03 | Change of inventor or designer information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |