Unmanned aerial vehicle autonomous obstacle avoidance and exploration method and device, electronic equipment and medium

Abstract

The embodiment of the invention provides an unmanned aerial vehicle autonomous obstacle avoidance and exploration method and device, electronic equipment and a medium, and the method comprises the steps: completing the pose estimation of an unmanned aerial vehicle and the construction of a three-dimensional point cloud map based on an RGB image and a depth image through employing a VINS-Fusion algorithm for a dense forest scene, and generating an octree map; a heuristic function and a cost function in the Hybrid A * algorithm are improved, and the improved heuristic function and the improved cost function are obtained; initializing Fast-planner algorithm parameters, based on the octree map, through an improved heuristic function, calculating obstacle cost, updating obstacle three-dimensional coordinates, calculating a flight safety distance, through an improved cost function, traversing local point clouds in a neighborhood range of the unmanned aerial vehicle, searching a path with the minimum cost, extracting an optimal path course, and constructing a safety corridor; and initializing parameters, and completing autonomous obstacle avoidance and exploration in the constructed safe corridor according to the optimal path course.

Description

Technical Field

[0001] This document relates to the field of unmanned aerial vehicle navigation technology, and in particular to a method, device, electronic equipment and medium for autonomous obstacle avoidance and exploration of unmanned aerial vehicles. Background Art

[0002] The geographical location of forest protection projects is generally steep and the traffic is not stable, which increases the difficulty of forest maintenance. With the development of ecological projects, the forest protection management area of ​​my country's forestry projects has gradually increased, the workload of management personnel has increased, and the management affairs are heavy, which has increased the difficulty and pressure of work.

[0003] When facing natural disasters, how to complete rescue and relief work efficiently will be an urgent problem to be solved. The quadcopter drone generates lift through multiple rotors and controls the aircraft's attitude by changing the rotation speed of each rotor. It has the advantages of being small and flexible, taking off and landing vertically, and hovering accurately. Dense forest exploration is its core application scenario. Using drones to explore dense forests will help liberate productivity and improve work efficiency.

[0004] The dense forest scene is complex and there are many obstacles, which brings challenges to UAV flight. Due to the unknown environmental information, the traditional path planning method will have redundant points and inflection points, resulting in problems such as convergence time field, high cost of path node expansion, and easy occurrence of local optimality. Therefore, how to construct a more reasonable path planning solution will be an urgent problem to be solved. Summary of the invention

[0005] The purpose of the present invention is to provide a method, device, electronic device and medium for autonomous obstacle avoidance and exploration of unmanned aerial vehicles, aiming to solve the above-mentioned problems in the prior art.

[0006] The present invention provides an autonomous obstacle avoidance and exploration method for a drone, comprising:

[0007] For dense forest scenes, the VINS-Fusion algorithm is used to complete the drone's pose estimation and three-dimensional point cloud map construction based on the RGB image and depth map, and generate an octree map;

[0008] The heuristic function and cost function in the Hybrid A* algorithm are improved according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the maximum safety distance between the drone and the obstacle, and the improved heuristic function and cost function are obtained;

[0009] Initialize the Fast-planner algorithm parameters, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance through the improved heuristic function, traverse the local point cloud within the UAV neighborhood through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and build a safe corridor;

[0010] Initialize parameters and complete autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading.

[0011] The present invention provides an autonomous obstacle avoidance and exploration device for a drone, comprising:

[0012] The map construction module is used for dense forest scenes. It uses the VINS-Fusion algorithm to complete the drone's pose estimation and three-dimensional point cloud map construction based on the RGB image and depth image, and generates an octree map;

[0013] Function improvement module, used to improve the heuristic function and cost function in the Hybrid A* algorithm according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the limit safety distance between the drone and the obstacle, and obtain the improved heuristic function and cost function;

[0014] A calculation module is used to initialize the parameters of the Fast-planner algorithm, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance, traverse the local point cloud within the neighborhood of the drone through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and construct a safe corridor;

[0015] The execution module is used to initialize parameters and complete autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading.

[0016] An embodiment of the present invention also provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the above-mentioned drone autonomous obstacle avoidance and exploration method when executed by the processor.

[0017] An embodiment of the present invention further provides a computer-readable storage medium, on which a program for implementing information transmission is stored. When the program is executed by a processor, the steps of the above-mentioned drone autonomous obstacle avoidance and exploration method are implemented.

[0018] The method adopts the embodiment of the present invention, by constructing an octree map, improving the heuristic function, adding forward, steering, and backward penalty items to the cost function, improving the efficiency of obstacle avoidance and autonomous exploration of the drone, reducing safety risks, and being applicable to dense forest scenes. Finally, the local planning algorithm, the global planning algorithm, and the Fast-planner are combined to complete trajectory optimization, which can efficiently complete the exploration and traversal of dense forest scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate one or more embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0020] Figure 1 is a flow chart of a method for autonomous obstacle avoidance and exploration of a drone according to an embodiment of the present invention;

[0021] Figure 2 is a flowchart of detailed processing of the autonomous obstacle avoidance and exploration method for a drone according to an embodiment of the present invention;

[0022] Figure 3 is a processing flow chart of completing autonomous obstacle avoidance and exploration according to the optimal path heading in an embodiment of the present invention;

[0023] Figure 4 is a schematic diagram of an autonomous obstacle avoidance and exploration device for a drone according to an embodiment of the present invention;

[0024] Figure 5 is a schematic diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0025] In response to the problems in the prior art, an embodiment of the present invention provides an autonomous obstacle avoidance and exploration method for drones in dense forest scenes. The method implements drone pose estimation and generates an octree map based on VINS-Fusion. For dense forest scenes, an improved hybrid A* algorithm is proposed to reduce the risk of drone collision. Specifically, a distance penalty function and a weight coefficient are introduced into the evaluation function so that the safe distance between the drone and the obstacle will change dynamically with the density of the obstacle. Furthermore, in order to avoid frequent turns that cause unstable operation of the drone and ensure that the path is as smooth as possible, an embodiment of the present invention adds forward, turning, and backward penalty terms to the cost function to increase the efficiency of drone exploration. Finally, the local planning algorithm, the global planning algorithm, and the Fast-planner are combined to complete trajectory optimization, and the exploration and traversal of the dense forest scene are efficiently completed.

[0026] In order to enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the following will be combined with the drawings in one or more embodiments of this specification to clearly and completely describe the technical solutions in one or more embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this document.

[0027] Method Embodiment

[0028] According to an embodiment of the present invention, a method for autonomous obstacle avoidance and exploration of a drone is provided. Figure 1 FIG. 1 is a flow chart of the autonomous obstacle avoidance and exploration method for a drone according to an embodiment of the present invention. Figure 1 As shown, the autonomous obstacle avoidance and exploration method for a drone according to an embodiment of the present invention specifically includes:

[0029] Step S101, for the dense forest scene, the VINS-Fusion algorithm is used to complete the pose estimation and three-dimensional point cloud map construction of the drone based on the RGB image and the depth image, and an octree map is generated;

[0030] Step S102, improving the heuristic function and cost function in the Hybrid A* algorithm according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the limit safety distance between the drone and the obstacle to obtain improved heuristic function and cost function; specifically including:

[0031] The distance penalty function k(n) shown in Formula 2 is introduced into the evaluation function f(n) shown in Formula 1. Meanwhile, only the non-holonomic constraints of the UAV are considered without considering the environmental obstacle constraints, and the heuristic function is improved:

[0032] f(n)=g(n)+h(n)+k(n) Formula 1;

[0033]

[0034] Among them, g(n) represents the actual cost function of each child node, h(n) represents the heuristic function that only considers the constraints of the drone itself without considering the constraints of environmental obstacles. When only this constraint is used, the drone will approach the target point directly based on the shortest path without considering obstacles, l is the direct distance between the drone and the obstacle, l min is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. maxis the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. α is the weight coefficient, and its value is adjusted appropriately according to the number and distribution of obstacles. When 0≤l≤l min When , it belongs to the collision area, the area is impassable, the nodes in the area are considered invalid nodes, and there is no need to search and expand. The value of the distance penalty function k(n) is +∞; when l min <l<l max When , it belongs to the collision risk area. When the distance l between the node and the obstacle increases, the distance penalty function k(n) will decrease. The node farther away from the obstacle has a higher priority, that is, the node closer to the obstacle has a lower expansion probability.

[0035] The forward, reverse and backward extension penalty terms are added to the cost function. The planned path will make the drone fly forward as much as possible. The actual cost is shown in Formula 3:

[0036]

[0037] Among them, g p (n) is the parent node n p The price, g k,i (n) is child node n i The cost, α i is the weight coefficient, L is the expansion step length, f is the compensation reduction coefficient, and its value is 0.1. Indicates the heading angle of the drone at the child node, Indicates the heading angle of the drone at the parent node; through g 1,i (n) Give incentives to forward flight and increase the number of node forward expansions, through g 2,i (n) Penalize backward flight and reduce the number of node expansions. 3,i (n) Penalize the change of node heading angle that causes the change of moving direction to reduce the number of turns.

[0038] Step S103, initialize the Fast-planner algorithm parameters, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance, traverse the local point cloud within the neighborhood of the drone through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and build a safe corridor; building a safe corridor specifically includes:

[0039] Establish a path point set M, a safe corridor point set corridor and a safe corridor set corridors, where M stores the path points that meet the requirements, and corridors stores the safe corridor point set that meets the requirements. Traverse the octree map to determine the path point ni Whether it is outside the previous safe corridor. If it meets the constraints, it is stored in corridors, otherwise it is stored in M;

[0040] Waypoint n i The yaw angle is According to the conditions, first follow x + Expand points A and B along x - Expand points C and D, update the horizontal coordinates of A, B, C, and D, and then follow the y + Expand points A and D along the y direction - Expand points B and C in the same direction, update the vertical coordinates of A, B, C, and D, and the expansion principle also satisfies the distance penalty function. The distance between the boundary of the safe corridor and the obstacle must not be less than l min , put points A, B, C, and D into the corridor;

[0041] The rectangle formed by the four points A, B, C, and D is a safe corridor. Put the corridors into the collection corridors, then clear M and corridors to continue searching for the next path point that meets the conditions.

[0042] Step S104, initializing parameters, and completing autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading. Specifically, Figure 3 As shown:

[0043] Initialize the drone’s takeoff altitude, maximum flight speed, and desired target attitude;

[0044] Determine the parent node, calculate the evaluation function f(n) of the node to be expanded according to the parent node, obtain the node with the smallest evaluation function value, and determine whether the node with the smallest evaluation function value is in the safety corridor and close to the target point. If both are judged to be yes, navigate to the target point; otherwise, return to redetermine the parent node.

[0045] In summary, the embodiment of the present invention uses the VINS-Fusion algorithm to complete the pose estimation and three-dimensional point cloud map construction of the drone based on the RGB map and the depth map, and further generates an octree map. At the same time, the heuristic function is improved for the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the limit safety distance between the drone and the obstacle to reduce the risk of collision. Further, the Fast-planner algorithm parameters are initialized, the obstacle cost is calculated, the three-dimensional coordinates of the obstacle are updated, the flight safety distance is calculated, the local point cloud within the drone neighborhood is traversed, the path with the minimum cost is found, and the optimal path heading is extracted. Finally, the drone uses the flight control software to subscribe to topics such as task modules, control instructions, and drone status, set and publish the expected posture, and initialize parameters including take-off altitude, safety altitude, descent speed, and geographic fence. Initialize the controller class and communication class, control the controller and communicate with PX4, and extract the heading angle of the optimal path for track flight.

[0046] The above technical solution of the embodiment of the present invention is described in detail below in conjunction with the accompanying drawings.

[0047] like Figure 2 As shown, in the technical solution of the embodiment of the present invention, the pose estimation and three-dimensional point cloud map construction of the UAV are completed based on the RGB image and the depth image based on the VINS-Fusion algorithm, and an octree map is further generated. At the same time, the heuristic function is improved according to the environmental obstacle density, the Euclidean distance between the UAV and the obstacle, and the limit safety distance between the UAV and the obstacle to reduce the risk of collision. Further, the Fast-planner algorithm parameters are initialized, the obstacle cost is calculated, the three-dimensional coordinates of the obstacle are updated, the flight safety distance is calculated, the local point cloud within the UAV neighborhood is traversed, the path with the minimum cost is found, and the optimal path heading is extracted. Finally, the take-off height, maximum flight speed, and expected target posture of the UAV are initialized, and autonomous obstacle avoidance and exploration are completed in combination with the optimal path and heading.

[0048] Taking into account the obstacle constraints, the present invention also constructs a safety corridor, reasonably adjusts the size of the safety corridor, and constrains the search nodes to be within the safety corridor, avoiding collisions caused by ignoring the size of the drone itself and being close to obstacles, making the planned path safer. Target point For example, the process of generating a safe corridor is as follows:

[0049] a. Establish a path point set M, a safe corridor point set corridor and a safe corridor set corridors. M stores the path points that meet the requirements, and corridors stores the safe corridor point set that meets the requirements. Traverse the map and determine the path point n iWhether it is outside the previous safe corridor, if it meets the constraints, it is stored in corridors, otherwise it is placed in M.

[0050] b. Path point n i The yaw angle is According to the conditions, first follow x + Expand points A and B along x - Expand points C and D, update the horizontal coordinates of A, B, C, and D, and then follow the y + Expand points A and D along the y direction - Expand points B and C in the same direction, update the vertical coordinates of A, B, C, and D, and the expansion principle also satisfies the distance penalty function. The distance between the boundary of the safe corridor and the obstacle must not be less than l min , put points A, B, C, and D into the corridor.

[0051] The rectangle formed by the four points cA, B, C, and D is a safe corridor. Put corridors into the collection corridors, then clear M and corridors to continue searching for the next path point that meets the conditions.

[0052] The evaluation function of the improved Hybrid A* algorithm obtained in the embodiment of the present invention is f(n)=g(n)+h(n)+k(n). The flow chart of the improved Hybrid A* algorithm is as follows: Figure 3 shown.

[0053] Device Example 1

[0054] According to an embodiment of the present invention, a drone autonomous obstacle avoidance and exploration device is provided. Figure 4 Schematic diagram of the autonomous obstacle avoidance and exploration device for a drone according to an embodiment of the present invention. Figure 4 As shown, the autonomous obstacle avoidance and exploration device for a drone according to an embodiment of the present invention specifically includes:

[0055] The map construction module 40 is used for dense forest scenes, using the VINS-Fusion algorithm to complete the pose estimation and three-dimensional point cloud map construction of the UAV based on the RGB image and the depth image, and generate an octree map;

[0056] The function improvement module 42 is used to improve the heuristic function and the cost function in the Hybrid A* algorithm according to the environmental obstacle density, the Euclidean distance between the UAV and the obstacle, and the limit safety distance between the UAV and the obstacle to obtain the improved heuristic function and the cost function; the function improvement module 42 is specifically used for:

[0057] The distance penalty function k(n) shown in Formula 2 is introduced into the evaluation function f(n) shown in Formula 1. Meanwhile, only the non-holonomic constraints of the UAV are considered without considering the environmental obstacle constraints, and the heuristic function is improved:

[0058] f(n)=g(n)+h(n)+k(n) Formula 1;

[0059]

[0060] Among them, g(n) represents the actual cost function of each child node, h(n) represents the heuristic function that only considers the constraints of the drone itself without considering the constraints of environmental obstacles. When only this constraint is used, the drone will approach the target point directly based on the shortest path without considering obstacles, l is the direct distance between the drone and the obstacle, l min is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. max is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. α is the weight coefficient, and its value is adjusted appropriately according to the number and distribution of obstacles. When 0≤l≤l min When , it belongs to the collision area, the area is impassable, the nodes in the area are considered invalid nodes, and there is no need to search and expand. The value of the distance penalty function k(n) is +∞; when l min <l<l max When , it belongs to the collision risk area. When the distance l between the node and the obstacle increases, the distance penalty function k(n) will decrease. The node farther away from the obstacle has a higher priority, that is, the node closer to the obstacle has a lower expansion probability.

[0061] The forward, reverse and backward extension penalty terms are added to the cost function. The planned path will make the drone fly forward as much as possible. The actual cost is shown in Formula 3:

[0062]

[0063] Among them, g p (n) is the parent node n p The price, g k,i (n) is child node n i The cost, α i is the weight coefficient, L is the expansion step length, f is the compensation reduction coefficient, and its value is 0.1. Indicates the heading angle of the drone at the child node, Indicates the heading angle of the drone at the parent node; through g 1,i (n) Give incentives to forward flight and increase the number of node forward expansions, through g 2,i(n) Penalize backward flight and reduce the number of node expansions. 3,i (n) Penalize the change of node heading angle that causes the change of moving direction to reduce the number of turns.

[0064] The calculation module 44 is used to initialize the parameters of the Fast-planner algorithm, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance, traverse the local point cloud within the neighborhood of the drone through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and construct a safe corridor; the calculation module 44 is specifically used to:

[0065] Establish a path point set M, a safe corridor point set corridor and a safe corridor set corridors, where M stores the path points that meet the requirements, and corridors stores the safe corridor point set that meets the requirements. Traverse the octree map to determine the path point n i Whether it is outside the previous safe corridor. If it meets the constraints, it is stored in corridors, otherwise it is stored in M;

[0066] Waypoint n i The yaw angle is According to the conditions, first follow x + Expand points A and B along x - Expand points C and D, update the horizontal coordinates of A, B, C, and D, and then follow the y + Expand points A and D along the y direction - Expand points B and C in the same direction, update the vertical coordinates of A, B, C, and D, and the expansion principle also satisfies the distance penalty function. The distance between the boundary of the safe corridor and the obstacle must not be less than l min , put points A, B, C, and D into the corridor;

[0067] The rectangle formed by the four points A, B, C, and D is a safe corridor. Put the corridors into the collection corridors, then clear M and corridors to continue searching for the next path point that meets the conditions;

[0068] The execution module 46 is used to initialize parameters and complete autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading. The execution module 46 is specifically used to:

[0069] Initialize the drone’s takeoff altitude, maximum flight speed, and desired target attitude;

[0070] Determine the parent node, calculate the evaluation function f(n) of the node to be expanded according to the parent node, obtain the node with the smallest evaluation function value, and determine whether the node with the smallest evaluation function value is in the safety corridor and close to the target point. If both are judged to be yes, navigate to the target point; otherwise, return to redetermine the parent node.

[0071] The embodiment of the present invention is a device embodiment corresponding to the above method embodiment. The specific operations of each module can be understood by referring to the description of the method embodiment, which will not be repeated here.

[0072] Device Example 2

[0073] An embodiment of the present invention provides an electronic device, such as Figure 5 As shown, it includes: a memory 50, a processor 52, and a computer program stored in the memory 50 and executable on the processor 52. When the computer program is executed by the processor 52, the steps described in the method embodiment are implemented.

[0074] Device Example 3

[0075] An embodiment of the present invention provides a computer-readable storage medium, on which a program for implementing information transmission is stored. When the program is executed by the processor 52, the steps described in the method embodiment are implemented.

[0076] The computer-readable storage medium in this embodiment includes, but is not limited to, ROM, RAM, magnetic disk or optical disk, etc.

[0077] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for autonomous obstacle avoidance and exploration of a drone, characterized in that: include: For dense forest scenes, the VINS-Fusion algorithm is used to complete the drone's pose estimation and three-dimensional point cloud map construction based on the RGB image and depth map, and generate an octree map; The heuristic function and cost function in the Hybrid A* algorithm are improved according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the maximum safety distance between the drone and the obstacle, and the improved heuristic function and cost function are obtained; Initialize the Fast-planner algorithm parameters, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance through the improved heuristic function, traverse the local point cloud within the UAV neighborhood through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and build a safe corridor; Initialize parameters and complete autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading.

2. The method according to claim 1, characterized in that The heuristic function and cost function are improved according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the limit safety distance between the drone and the obstacle. The improved heuristic function and cost function specifically include: The distance penalty function k(n) shown in Formula 2 is introduced into the evaluation function f(n) shown in Formula 1. Meanwhile, only the non-holonomic constraints of the UAV are considered without considering the environmental obstacle constraints, and the heuristic function is improved: f(n)=g(n)+h(n)+k(n) Formula 1; Among them, g(n) represents the actual cost function of each child node, h(n) represents the heuristic function that only considers the constraints of the drone itself without considering the constraints of environmental obstacles. When only this constraint is used, the drone will approach the target point directly based on the shortest path without considering obstacles, l is the direct distance between the drone and the obstacle, l min is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. max is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. α is the weight coefficient, and its value is adjusted appropriately according to the number and distribution of obstacles. When 0≤l≤l min When , it belongs to the collision area, the area is impassable, the nodes in the area are considered invalid nodes, and there is no need to search and expand. The value of the distance penalty function k(n) is +∞; when l min <l<l max When , it belongs to the collision risk area. When the distance l between the node and the obstacle increases, the distance penalty function k(n) will decrease. The node farther away from the obstacle has a higher priority, that is, the node closer to the obstacle has a lower expansion probability.

3. The method according to claim 2, characterized in that The cost function is improved, and the improved cost function further includes: The forward, reverse and backward extension penalty terms are added to the cost function. The planned path will make the drone fly forward as much as possible. The actual cost is shown in Formula 3: Among them, g p (n) is the parent node n p The price, g k,i (n) is child node n i The cost, α i is the weight coefficient, L is the expansion step length, f is the compensation reduction coefficient, and its value is 0.

1. Indicates the heading angle of the drone at the child node, Indicates the heading angle of the drone at the parent node; through g 1,i (n) Give incentives to forward flight and increase the number of node forward expansions, through g 2,i (n) Penalize backward flight and reduce the number of node expansions. 3,i (n) Penalize the change of node heading angle that causes the change of moving direction to reduce the number of turns.

4. The method according to claim 1, characterized in that: The construction of safe corridors specifically includes: Establish a path point set M, a safe corridor point set corridor and a safe corridor set corridors, where M stores the path points that meet the requirements, and corridors stores the safe corridor point set that meets the requirements. Traverse the octree map to determine the path point n i Whether it is outside the previous safe corridor. If it meets the constraints, it is stored in corridors, otherwise it is stored in M; Waypoint n i The yaw angle is According to the conditions, first follow x + Expand points A and B along x - Expand points C and D, update the horizontal coordinates of A, B, C, and D, and then follow the y + Direction expansion of points A and D, - Expand points B and C along the y direction, update the ordinates of A, B, C, and D, and the expansion principle also satisfies the distance penalty function. The distance between the safety corridor boundary and the obstacle must not be less than l min , put points A, B, C, and D into the corridor; The rectangle formed by the four points A, B, C, and D is a safe corridor. Put the corridors into the collection corridors, then clear M and corridors to continue searching for the next path point that meets the conditions.

5. The method according to claim 1, characterized in that Initializing parameters, in the constructed safety corridor, completing autonomous obstacle avoidance and exploration according to the optimal path heading specifically includes: Initialize the drone’s takeoff altitude, maximum flight speed, and desired target attitude; Determine the parent node, calculate the evaluation function f(n) of the node to be expanded according to the parent node, obtain the node with the smallest evaluation function value, and determine whether the node with the smallest evaluation function value is in the safety corridor and close to the target point. If both are judged to be yes, navigate to the target point; otherwise, return to redetermine the parent node.

6. An autonomous obstacle avoidance and exploration device for a drone, characterized in that: include: The map construction module is used for dense forest scenes. It uses the VINS-Fusion algorithm to complete the drone's pose estimation and three-dimensional point cloud map construction based on the RGB image and depth image, and generates an octree map; Function improvement module, used to improve the heuristic function and cost function in the Hybrid A* algorithm according to the environmental obstacle density, the Euclidean distance between the drone and the obstacle, and the limit safety distance between the drone and the obstacle, and obtain the improved heuristic function and cost function; A calculation module is used to initialize the parameters of the Fast-planner algorithm, calculate the obstacle cost based on the octree map, update the three-dimensional coordinates of the obstacle, calculate the flight safety distance, traverse the local point cloud within the neighborhood of the drone through the improved cost function, find the path with the minimum cost, extract the optimal path heading, and construct a safe corridor; The execution module is used to initialize parameters and complete autonomous obstacle avoidance and exploration in the constructed safety corridor according to the optimal path heading.

7. The device according to claim 6, characterized in that The function improvement module is specifically used for: The distance penalty function k(n) shown in Formula 2 is introduced into the evaluation function f(n) shown in Formula 1. Meanwhile, only the non-holonomic constraints of the UAV are considered without considering the environmental obstacle constraints, and the heuristic function is improved: f(n)=g(n)+h(n)+k(n) Formula 1; Among them, g(n) represents the actual cost function of each child node, h(n) represents the heuristic function that only considers the constraints of the drone itself without considering the constraints of environmental obstacles. When only this constraint is used, the drone will approach the target point directly based on the shortest path without considering obstacles, l is the direct distance between the drone and the obstacle, l min is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. max is the maximum safe distance between the drone and obstacles, and its value is determined by the actual size of the drone. α is the weight coefficient, and its value is adjusted appropriately according to the number and distribution of obstacles. When 0≤l≤l min When , it belongs to the collision area, the area is impassable, the nodes in the area are considered invalid nodes, and there is no need to search and expand. The value of the distance penalty function k(n) is +∞; when l min <l<l max When , it belongs to the collision risk area. When the distance l between the node and the obstacle increases, the distance penalty function k(n) will decrease. The node farther away from the obstacle has a higher priority, that is, the node closer to the obstacle has a lower expansion probability. The forward, reverse and backward extension penalty terms are added to the cost function. The planned path will make the drone fly forward as much as possible. The actual cost is shown in Formula 3: Among them, g p (n) is the parent node n p The price, g k,i (n) is child node n i The cost, α i is the weight coefficient, L is the expansion step length, f is the compensation reduction coefficient, and its value is 0.

1. Indicates the heading angle of the drone at the child node, Indicates the heading angle of the drone at the parent node; through g 1,i (n) Give incentives to forward flight and increase the number of node forward expansions, through g 2,i (n) Penalize backward flight and reduce the number of node expansions. 3,i (n) Penalize the change of node heading angle that causes the change of moving direction to reduce the number of turns.

8. The device according to claim 6, characterized in that The calculation module is specifically used for: Establish a path point set M, a safe corridor point set corridor and a safe corridor set corridors, where M stores the path points that meet the requirements, and corridors stores the safe corridor point set that meets the requirements. Traverse the octree map to determine the path point n i Whether it is outside the previous safe corridor. If it meets the constraints, it is stored in corridors, otherwise it is stored in M; Waypoint n i The yaw angle is According to the conditions, first follow x + Expand points A and B along x - Expand points C and D, update the horizontal coordinates of A, B, C, and D, and then follow the y + Expand points A and D along the y direction - Expand points B and C in the same direction, update the vertical coordinates of A, B, C, and D, and the expansion principle also satisfies the distance penalty function. The distance between the boundary of the safe corridor and the obstacle must not be less than l min , put points A, B, C, and D into the corridor; The rectangle formed by the four points A, B, C, and D is a safe corridor. Put the corridors into the collection corridors, then clear M and corridors to continue searching for the next path point that meets the conditions; The execution module is specifically used for: Initialize the drone’s takeoff altitude, maximum flight speed, and desired target attitude; Determine the parent node, calculate the evaluation function f(n) of the node to be expanded according to the parent node, obtain the node with the smallest evaluation function value, and determine whether the node with the smallest evaluation function value is in the safety corridor and close to the target point. If both are judged to be yes, navigate to the target point; otherwise, return to redetermine the parent node.

9. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the computer program is executed by the processor, the steps of the autonomous obstacle avoidance and exploration method for a drone as described in any one of claims 1 to 5 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores an implementation program for information transmission, and when the program is executed by the processor, the steps of the autonomous obstacle avoidance and exploration method for a drone as described in any one of claims 1 to 5 are implemented.

Images