Under-forest scene-oriented autonomous unmanned aerial vehicle speed adaptive trajectory planning method

Abstract

The invention provides an under-forest scene-oriented autonomous unmanned aerial vehicle speed adaptive trajectory planning method, and belongs to the technical field of unmanned aerial vehicle trajectory planning. The method comprises the following steps: step 1, creating a custom forest environment and transmitting the custom forest environment to an ROS; 2, taking the positioning truth value and the depth image as input of a trajectory planning method; 3, performing parameterized representation on the planning trajectory; 4, a trajectory planning method comprises front-end search and rear-end optimization; in the front-end searching part, the shortest safe path is searched, and the risk weights of the distance and the speed direction are comprehensively considered; 5, constructing a total target optimization function at the rear-end optimization part; 6, carrying out nonlinear optimization on the total target optimization function through Nlopt; and step seven, trajectory tracking is carried out by using a geometric control algorithm, and a speed adaptive flight experiment is completed in a dense environment. The speed self-adaption function can be achieved, the smoothness is high, the flight length is short, the flight time is short, and the comprehensive performance is excellent.

Description

Technical Field

[0001] This invention relates to an autonomous unmanned aerial vehicle (UAV) speed-adaptive trajectory planning method for forest environments, belonging to the field of UAV trajectory planning technology. Background Technology

[0002] Forests play a vital role in biodiversity, ecological balance, and water conservation. However, with the continuous increase in forest area in my country, traditional under-canopy forestry data collection typically consumes significant manpower, resources, and time. The development of drone technology offers a new approach to under-canopy data collection. With its low cost and high mobility, drones can replace arduous manual operations, reducing labor and time costs, overcoming accessibility limitations caused by complex terrain and dense vegetation, and simultaneously expanding data coverage.

[0003] The safety of drones is paramount when using them for under-canopy data collection. Since trajectory tracking error is directly proportional to speed, and the speed of the tracker is affected in the under-canopy environment, the tracker often fails to achieve ideal tracking results. Furthermore, dense obstacles in the under-canopy environment can easily obstruct camera or LiDAR sensors. Adaptive trajectory planning techniques, which involve slowing down when approaching obstacles and accelerating when leaving them, can effectively overcome these problems, but frequent speed changes can lead to uneven trajectories.

[0004] Therefore, trajectory planning algorithms should possess both safety and high smoothness in terms of speed adaptability to meet the needs of safe and efficient flight of autonomous drones in forest environments. Summary of the Invention

[0005] The purpose of this invention is to solve the problems existing in the prior art and to provide an autonomous drone speed adaptive trajectory planning method for forest scenarios. This method can meet the needs of safe and efficient flight of autonomous drones in forest environments and achieve speed adaptive trajectory planning that combines safety and high smoothness.

[0006] The objective of this invention is achieved through the following technical solution:

[0007] An autonomous UAV velocity-adaptive trajectory planning method for forest environments includes the following steps:

[0008] Step 1: Create a custom forest environment and transfer the core modules of the custom forest environment to ROS through cross-platform compilation;

[0009] Step 2: Obtain ground truth and depth images through the simulation platform, and send them to the UAV through the ROS node method in Step 1. Increase the frequency of publishing ground truth and depth images by expanding the ROS nodes, and use the ground truth and depth images as input for the trajectory planning method.

[0010] Step 3: Parametrically represent the trajectory planned by the trajectory planning method using cubic non-clamped uniform B-spline curves, and adjust the node span of the cubic non-clamped uniform B-spline curves. Determine the positions of the first and last control points of the planned trajectory;

[0011] Step 4: The trajectory planning method includes front-end search and back-end optimization; in the front-end search part, the shortest safe path is searched and the risk weights that comprehensively consider distance and speed direction are used to realize the variable step size target expansion for initial speed adjustment.

[0012] Step 5: In the backend optimization section, construct the overall target optimization function:

[0013] Step 6: Perform nonlinear optimization on the overall objective function constructed in Step 5 using Nlopt to obtain the UAV trajectory optimized by the backend.

[0014] Step 7: The UAV trajectory obtained in Step 6 through backend optimization is used for trajectory tracking using a geometric control algorithm to complete a speed adaptive flight experiment in a dense environment.

[0015] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0016] The method of this invention can effectively achieve speed adaptation function, and has high smoothness, short flight length and flight time, and excellent overall performance.

[0017] Most existing planning algorithms are not designed for actual forest drone trajectory planning scenarios, and a few have only verified their robustness in simple and sparse forest environments. This invention proposes a speed-adaptive trajectory planning method for autonomous drones in forest scenarios. It achieves both safety and high smoothness through hierarchical speed-adaptive B-spline trajectory generation. The trajectory is parameterized using B-spline curves to ensure continuity, and acceleration penalties related to obstacle distances are optimized at the nodes of the B-spline to improve trajectory smoothness. The front end searches for the shortest safe path and uses risk weights that comprehensively consider distance and velocity direction to achieve variable step size target expansion for initial speed adjustment. The back end optimizes the velocity at the B-spline nodes based on the velocity penalty term to achieve speed adaptation. Experiments conducted in a custom Airsim forest simulation environment show that the method of this invention can effectively achieve speed adaptation with high smoothness, short flight length and flight time, and can safely pass through dense forests even at increased maximum flight speed. Attached Figure Description

[0018] Figure 1This is a simulation image of a dense forest environment created using AirSim, as defined in this invention.

[0019] Figure 2 This is a framework diagram for implementing the autonomous unmanned aerial vehicle (UAV) speed adaptive trajectory planning method for forest scenarios according to the present invention.

[0020] Figure 3 This is a flight trajectory diagram of the Ego-planner algorithm in the simulation.

[0021] Figure 4 This is a flight trajectory diagram of the EVA-planner algorithm in the simulation.

[0022] Figure 5 The image shows the flight trajectory of the autonomous UAV speed adaptive trajectory planning method for forest scenarios according to the present invention in a simulation. Detailed Implementation

[0023] The present invention will be further described in detail below with reference to the accompanying drawings: This embodiment is implemented under the premise of the technical solution of the present invention, and detailed implementation methods are given, but the protection scope of the present invention is not limited to the following embodiments.

[0024] like Figure 2 As shown, this embodiment presents a method for adaptive trajectory planning of autonomous drones in forest environments, based on a specific implementation scheme for drone trajectory planning in a simulated environment. The method includes the following steps:

[0025] Step 1: To verify the autonomous UAV speed adaptive trajectory planning method for forest scenarios proposed in this invention, a scenario was created on the Windows platform based on Airsim and Virtual Engine, as follows: Figure 1 The example shows a custom forest environment. The core modules are then packaged into an executable program that can run on Linux systems through cross-platform compilation.

[0026] Cross-platform compatibility refers to compiling and packaging a forest simulation environment on the Windows platform that can be used and run on Linux systems. The core modules of the custom forest environment mainly include a C++ compilation module and a dynamic interaction module for the forest environment. The dynamic interaction module refers to giving the tree models solid entities so that there are collision effects when the drone comes into contact with the trees, rather than just textures.

[0027] Step 2: Obtain the ground truth location and depth image through the simulation platform Airsim, and send it through ROS nodes so that the drone can receive this topic node.

[0028] By increasing the frequency of localization ground truth and depth image releases through expanding ROS nodes, and using these localization ground truth and depth images as input to the planning algorithm, a geometric control algorithm is employed for trajectory tracking control. The simulation platform is then built; the specific process is as follows: Figure 2 As shown.

[0029] Step 3: Parametrically represent the planned trajectory using cubic non-clamped uniform B-spline curves and adjust the node spans. Determine the positions of the first and last control points.

[0030] The B-spline contains three elements: Control points , Nodes as well as The relationship between the three is as follows: ;

[0031] The formula for a B-spline curve is as follows:

[0032]

[0033] in, yes B-spline basis functions;

[0034] B-spline curves can be viewed as weighted linear combinations of control points and their corresponding basis functions. The domain of the B-spline trajectory is... Within this domain, any interval between adjacent nodes Corresponding to a local trajectory, the first The B-segment spline curve is calculated in matrix form, using the following formula:

[0035]

[0036] In the formula, , yes The th B-spline basis function Each base matrix.

[0037] For a cubic non-clamped uniform B-spline curve, the control points for velocity and acceleration are:

[0038]

[0039] in, For time intervals, for The corresponding derivative, that is, the derivative of acceleration.

[0040] At the node The relationship between the location of the point and its derivative and the control points is as follows:

[0041]

[0042]

[0043]

[0044] in, The cubic B-spline at the nodes The functional relationship between the control points at the given location. They are respectively the corresponding arrive Control points along the path within the time period;

[0045] To ensure the continuity of the old and new trajectories after each fixed time interval or when replanning after encountering obstacles, it is necessary to ensure that at nodes... The position, velocity, and acceleration at the starting point of the cubic uniform B-spline curve, and the current initial state (position). ,speed acceleration Same as control point With the initial state and the span of the B-spline nodes The relationship is as follows:

[0046]

[0047] like If the object is inside an obstacle, adjust the span of the B-spline curve nodes. The control point positions are dynamically adjusted and then recalculated to obtain safe initial control points; the velocity and acceleration at the end of the new trajectory are 0, and the trajectory must precisely pass through the target endpoint. Then we have:

[0048] .

[0049] in, These all correspond to the control points at the last moment, because the drone basically reaches the designated position at the last moment, so the three control points are the target endpoint positions.

[0050] Step 4: The front end searches for the shortest safe path and uses risk weights that comprehensively consider distance and speed direction to achieve variable step size target expansion for initial speed adjustment.

[0051] use The algorithm searches for the shortest geometric path from the current position to the target point, then extracts key nodes with different forward and backward expansion directions within the UAV's sensor perception range, and then expands the target with a variable step size that satisfies dynamic constraints on the path formed by connecting the key points. The expansion step size is obtained by the following formula:

[0052] ,

[0053]

[0054] in, It is an adjustable rate of change coefficient. Gradient of the Euclidean directed distance field With speed The cosine of the angle between them. Risk weighting and These represent the upper and lower limits of speed adjustment, respectively. It is the starting distance for speed adjustment; It is a path point Distance to the nearest obstacle;

[0055] The trajectory planning method of this invention is applicable to both lidar and visual cameras. However, the visual sensor used in the simulation of this embodiment, namely the depth camera, is generally arranged in the front half of the UAV fuselage, placed horizontally with the lens facing forward.

[0056] Step size is based on The direction information of the previous extension point on the shortest safe path is determined by the algorithm, which can roughly reflect the expected speed of the quadcopter at this time. The direction points in the direction of the fastest increase in the distance field value, that is, the direction from the obstacle to free space. When the drone's speed... and Risk weighting when flying in the opposite direction (i.e., towards an obstacle). Increase the step size, decrease the step size, and the drone decelerates to ensure safety; when and When the directions are the same (i.e., away from the obstacle), By reducing the size of the rotor, the quadcopter can increase its speed to improve efficiency.

[0057] The variable step size target expansion still results in discrete path points within a certain safety space. The time interval has already been obtained. Fitting the discrete points yields an initial B-spline curve passing through them, and the control points corresponding to this B-spline are... .

[0058] Step 5: In the backend optimization section, construct the overall target optimization function. The formula is as follows:

[0059]

[0060] in, It's a smoothness cost. It's the cost of collisions. It is a kinematic dynamic feasibility penalty item. It is an environmental adaptive safety perception penalty item. It's an acceleration penalty item. , , and These are the weights of these items.

[0061] Smoothness cost minimization The integral of the square of is expressed as follows:

[0062]

[0063] in, Number of control points;

[0064] The expression for the collision cost is as follows:

[0065]

[0066] in, Control point Distance to the nearest obstacle It is a safe distance threshold, which pushes the trajectory away from the obstacle.

[0067]

[0068] Kinematics and Dynamics Feasibility Penalty Item Penalty trajectory exceeds the maximum allowed value and The velocity or acceleration is used to ensure that the planned trajectory conforms to the physical motion constraints of the quadrotor, as expressed below:

[0069]

[0070] in, Let be the penalty function for velocity. Let be the penalty function for acceleration. Both refer to speed. and acceleration One of them, if For speed , For acceleration ,but Let be the penalty function for velocity, if For speed , For acceleration ,but This is a penalty function for acceleration;

[0071] Environmental Adaptive Safety Perception Penalty Item The expression is as follows:

[0072]

[0073] in, It is a node The distance from the trajectory point to the nearest obstacle. To punish the activation distance, when At that time, the speed penalty item is activated;

[0074] The squared term amplifies the influence of nearby obstacles, making the optimization algorithm more sensitive to trajectory points close to obstacles;

[0075] The expression for the acceleration penalty term is as follows:

[0076]

[0077] in, Activate distance as a penalty.

[0078] Step Six: In the backend optimization section, Nlopt is used to perform nonlinear optimization on the overall objective function to reduce the smoothness cost. Collision costs and kinematics and dynamics feasibility penalty items The optimization target is the control points of each derivative of the B-spline curve. The control points of each derivative can be obtained from the control points. express.

[0079] To achieve adaptive velocity on a B-spline trajectory, optimization needs to be performed directly on the position points of the trajectory rather than the B-spline control points, and the distance between the position points cannot be too large. A default node span is set. The value should be smaller. It is known that at the B-spline nodes... The position and velocity at the B-spline node Apply the penalty item for adaptive safety perception in the environment Optimization is a crucial step in achieving speed adaptation.

[0080] For speed optimization at nodes, if the default node span is used... If the span is too large, the distance between points at each node will also be too large, which may result in critical areas (such as the point closest to an obstacle or a curve in the trajectory) not being effectively optimized. If the span is too small, it will not only be difficult to ensure that the B-spline control points have sufficient optimization space, but it will also increase the backend optimization time. The control points used in this embodiment during the simulation process... The value is 0.3s.

[0081] Velocity adaptation causes the velocity to change constantly, which leads to uneven estimation. At the node... The acceleration penalty term is optimized to constrain acceleration, reduce velocity changes, and increase smoothness.

[0082] Step 7: After obtaining the drone trajectory through backend optimization, perform trajectory tracking control using geometric control algorithms to complete the speed adaptive flight experiment in a dense environment. Figures 3-5 The following are trajectory diagrams for different algorithms. Table 1 shows the average performance of the algorithms across ten flights.

[0083] Table 1. Comparison of results for different algorithms in terms of length, time, and smoothness cost metrics.

[0084]

[0085] In the same environment, due to the different drone trajectories and field of view directions, the completeness of the maps constructed by different algorithms varies, but this does not affect the trajectory analysis. Experimental results show that Ego-Planner initially calculates a smooth initial trajectory that leads directly to the destination, without considering the influence of obstacles. When an obstacle is encountered during flight, a replanning mechanism is activated to adjust the trajectory for obstacle avoidance, resulting in a higher cost for trajectory smoothing. While EVA-Planner achieves speed adaptation like the method of this invention, it uses discrete system state sequences to represent the trajectory, resulting in noticeable zigzag turns and poor smoothness, and also has the longest flight time. The method of this invention has the shortest flight trajectory length. Although it decelerates when approaching obstacles, the overall flight time is not long. Furthermore, this method maintains high trajectory smoothness while achieving speed adaptation, resulting in superior overall performance.

[0086] The above description is merely a preferred embodiment of the present invention. These specific embodiments are different implementations based on the overall concept of the present invention, and the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A speed-adaptive trajectory planning method for autonomous unmanned aerial vehicles (UAVs) in forest environments, characterized in that: Includes the following steps: Step 1: Create a custom forest environment and transfer the core modules of the custom forest environment to ROS through cross-platform compilation; Step 2: Obtain ground truth and depth images through the simulation platform, and send them to the UAV through the ROS node method in Step 1. Increase the frequency of publishing ground truth and depth images by expanding the ROS nodes, and use the ground truth and depth images as input for the trajectory planning method. Step 3: Parametrically represent the trajectory planned by the trajectory planning method using cubic non-clamped uniform B-spline curves, and adjust the node span of the cubic non-clamped uniform B-spline curves. Determine the positions of the first and last control points of the planned trajectory; Step 4: The trajectory planning method includes front-end search and back-end optimization; in the front-end search part, the shortest safe path is searched and the risk weights that comprehensively consider distance and speed direction are used to realize the variable step size target expansion for initial speed adjustment. Step 5: In the backend optimization section, construct the overall target optimization function: Step 6: Perform nonlinear optimization on the overall objective function constructed in Step 5 using Nlopt to obtain the UAV trajectory optimized by the backend. Step 7: The UAV trajectory obtained in Step 6 through backend optimization is used for trajectory tracking using a geometric control algorithm to complete a speed adaptive flight experiment in a dense environment.

2. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest environments according to claim 1, characterized in that, The creation of the custom forest environment described in step one is carried out on the Windows platform based on Airsim and the virtual engine. The core module of the custom forest environment is packaged into an executable program that can run under the Linux system through cross-platform compilation and then transferred to ROS under the Linux system.

3. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest settings according to claim 1, characterized in that, The B-spline described in step three contains three elements: Control points , Nodes as well as The relationship between the three is as follows: ; The formula for a B-spline curve is as follows: in, yes B-spline basis functions; B-spline curves can be viewed as weighted linear combinations of control points and their corresponding basis functions. The domain of the B-spline trajectory is... Within this domain, any interval between adjacent nodes Corresponding to a local trajectory, the first The B-segment spline curve is calculated in matrix form, using the following formula: In the formula, , yes The th B-spline basis function Each base matrix.

4. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest environments according to claim 3, characterized in that, The specific method for parametrically representing the planned trajectory of the trajectory planning method using cubic non-clamped uniform B-spline curves in step three is as follows: At the nodes of a cubic non-clamped uniform B-spline curve The relationship between the location of the point and its derivative and the control points is as follows: in, The cubic B-spline at the nodes The functional relationship between the control points at the given location. They are respectively the corresponding arrive Control points along the path within the time period; To ensure the continuity of the old and new trajectories after each fixed time interval or when replanning after encountering an obstacle, it is necessary to ensure that at each node... That is, the position, velocity, and acceleration at the starting point of a cubic non-clamped uniform B-spline curve, and the position of the current initial state. ,speed acceleration Same, control points Compared with the initial state and the span of the B-spline curve nodes The relationship is as follows: like If the object is inside an obstacle, adjust the span of the B-spline curve nodes. The control point positions are dynamically adjusted and then recalculated to obtain safe initial control points; the velocity and acceleration at the end of the new trajectory are 0, and the trajectory must precisely pass through the target endpoint. Then we have: in, These all correspond to the control points at the last moment, because the drone basically reaches the designated position at the last moment, so the three control points are the target endpoint positions.

5. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest environments according to claim 1, characterized in that, The method described in step four, which involves searching for the shortest safe path and using risk weights that comprehensively consider both distance and velocity direction, is as follows: use The algorithm searches for the shortest safe path from the current position to the target point, then extracts key nodes with different forward and backward expansion directions within the UAV's sensor perception range, and then expands the target with a variable step size that satisfies dynamic constraints on the path formed by connecting the key points. The expansion step size is obtained by the following formula: ， in, It is an adjustable rate of change coefficient. Gradient of the Euclidean directed distance field With speed The cosine of the angle between them. Risk weighting and These represent the upper and lower limits of speed adjustment, respectively. It is the starting distance for speed adjustment; It is a path point Distance to the nearest obstacle; After expanding the target with a variable step size, the discrete points are fitted to obtain an initial B-spline curve passing through the discrete points. The control points corresponding to this initial B-spline curve are: .

6. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest settings according to claim 1, characterized in that, Step five describes the construction of the overall objective optimization function. The formula is as follows: in, It's a smoothness cost. It's the cost of collisions. It is a kinematic dynamic feasibility penalty item. It is an environmental adaptive safety perception penalty item. It's an acceleration penalty item. , , and The weights of the above items; The smoothness cost is minimized The integral of the square of is expressed as follows: in, Number of control points; The expression for the collision cost is as follows: in, Control point Distance to the nearest obstacle It is a safe distance threshold, which pushes the trajectory away from the obstacle. The kinematics and dynamics feasibility penalty item Penalty trajectory exceeds the maximum allowed value and The velocity or acceleration is used to ensure that the planned trajectory conforms to the physical motion constraints of the quadrotor, as expressed below: in, Let be the penalty function for velocity. Let be the penalty function for acceleration. Both refer to speed. and acceleration One of them, if For speed , For acceleration ,but Let be the penalty function for velocity, if For speed , For acceleration ,but This is a penalty function for acceleration; The environmental adaptive safety perception penalty item The expression is as follows: in, It is a node The distance from the trajectory point to the nearest obstacle. To punish the activation distance, when At that time, the speed penalty item is activated; The acceleration penalty item The expression is as follows: in, Activate distance as a penalty.

7. The method for adaptive trajectory planning of autonomous unmanned aerial vehicles (UAVs) in forest environments according to claim 6, characterized in that, The method for nonlinearly optimizing the overall objective function constructed in step five using Nlopt in step six is ​​as follows: For smoothness cost Collision costs and kinematics and dynamics feasibility penalty items The optimization target is the control points of each order derivative of a cubic non-clamped uniform B-spline curve. The control points of each order derivative are determined by the control points. express; Set the default node span It is known that at node The position and velocity at the node Apply the penalty item for adaptive safety perception in the environment Optimization is a crucial step in achieving speed adaptation; at the node Optimize acceleration penalty item By constraining acceleration, velocity changes are reduced, and smoothness is increased.

Images