Unmanned aerial vehicle intelligent homeward flight nest and homeward flight path planning method, storage medium and device

Abstract

The invention discloses an unmanned aerial vehicle intelligent return flight nest and return flight path planning method, a storage medium and a device, and belongs to the technical field of unmanned aerial vehicle navigation and obstacle avoidance. Aiming at the problems of low obstacle avoidance efficiency and high collision risk of a traditional DWA algorithm in a multi-aircraft return scene, a local path is corrected by fusing a two-way search strategy with a global gravitational field, balance calculation efficiency and obstacle avoidance precision are adjusted by adopting a dynamic time step, and a variable weight track evaluation function is designed to dynamically respond to electric quantity and obstacle density changes, so that the obstacle avoidance efficiency is improved. Meanwhile, a semi-plane constraint screening safety speed vector is generated through an ORCA algorithm, and multi-machine collaborative obstacle avoidance is achieved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) navigation and obstacle avoidance technology, and in particular to a method, storage medium, and device for planning an intelligent return-to-home and return-to-home path for a UAV. Background Art

[0002] In recent years, drone technology has developed rapidly, and its application scenarios have penetrated into multiple fields such as industry, agriculture, public security, and urban management. In power inspections, drone swarms can efficiently complete defect detection and data collection in complex environments such as ultra-high voltage transmission lines and offshore wind farms. In the field of logistics and distribution, drone formations are used to transport fresh food and medical supplies in inaccessible areas such as mountainous areas and islands. In precision agriculture, multiple drones collaborate to perform tasks such as farmland mapping, variable-rate spraying, and pest and disease monitoring, significantly improving operational efficiency. In addition, in public safety scenarios, drone swarms are widely used for tasks such as forest fire monitoring, earthquake reconnaissance, and security inspections for large-scale events, requiring frequent travel between the mission area and the drone nest.

[0003] As multi-drone collaborative operations become commonplace, the risk of collisions in dense return-to-home scenarios is becoming increasingly prominent. For example, in environmental monitoring missions, multiple drones must return to a mobile drone nest after completing large-scale air quality monitoring. This can easily lead to collisions due to overlapping flight paths. Current drone return-to-home systems for multi-drone collaborative scenarios mostly use single-drone path planning algorithms. While these algorithms can generate locally optimal paths, they cannot effectively handle the drones' maneuvering and collision avoidance needs, resulting in an exponential increase in the probability of collisions as the return density increases.

[0004] To solve the above problems, this solution proposes a method, storage medium and device for intelligent return-to-home and return-to-home path planning of UAVs. Summary of the Invention

[0005] The purpose of the present invention is to provide a method, storage medium and device for intelligent return-to-home and return-to-home path planning of a UAV, so as to solve the problems raised in the above-mentioned background technology.

[0006] S1. To achieve the above objectives, the present invention provides the following technical solutions: a method for intelligently returning to the home nest and planning the return path of a UAV, which improves the DWA algorithm to obtain the current position of the UAV and the coordinates of the home nest, the coordinates of the obstacle point cloud, the position of the neighboring aircraft, the current speed v and angular velocity w of the UAV;

[0007] S2. Generate candidate trajectory sets based on bidirectional search strategy:

[0008] S2.1 Take the target point P nest As the starting point, move towards the current position of the drone P current Direction generates a virtual gravitational field; calculates the global gravitational component of the current spatial evaluation point coordinate p, and the gravitational field function is: Among them, katt ∈[0.5,2.0], is the gravitational gain coefficient; ε is the numerical stability factor;

[0009] S2.2 generates a virtual gravitational field with the machine nest coordinates as the starting point and calculates the potential energy value at the end point of the trajectory.

[0010] S2.3 improves the evaluation function of the DWA algorithm to predict the dynamic obstacle trajectory, guide the path to converge to the optimal direction, and obtain the best candidate trajectory;

[0011] S2.4 generates candidate trajectories according to the dynamic time step adjustment rule. The dynamic time step calculation formula is: where t base is the basic prediction duration, N crit The set critical obstacle density; v max is the maximum speed of the drone; v is the flight speed;

[0012] S3. Use the ORCA collaborative obstacle avoidance algorithm to evaluate the best candidate trajectory, adjust the best candidate trajectory that will cause collision, and generate the predicted trajectory of the drone, including:

[0013] S3.1. Calculate the minimum collision time between drones based on the set safety radius;

[0014] S3.2. When the collision time is less than a preset value, an ORCA constraint half-plane is generated, and velocity vectors that do not meet the ORCA constraint half-plane are eliminated;

[0015] S3.3. Record the adjustment amount of the velocity and angular velocity of the velocity vector and the optimal candidate trajectory according to the velocity vector.

[0016] S4. Obtain the angular velocity and speed of the predicted trajectory, evaluate the predicted trajectory, and select the predicted trajectory with the highest score as the optimal trajectory. The obstacle avoidance weight β of the scoring function increases with the increase of obstacle density. The calculation formula is:

[0017] Among them, α is the heading angle weight, β is the obstacle avoidance weight, γ is the speed gain weight, d0 is the obstacle avoidance feature distance, w c is the heading angle deviation at the end of the trajectory, d min is the distance from the trajectory to the nearest obstacle.

[0018] S5. Output the optimal trajectory to the flight control actuator.

[0019] Preferably, the calculation formula for the potential energy value of the trajectory endpoint in step 2.2 is: The sampling points of the discretized trajectory are n=10; Δp k is the trajectory segment length, which is determined by the current velocity v and the time step Δt, and the calculation formula is Δpk =v·Δt.

[0020] Preferably, the evaluation function of the improved DWA algorithm in step 2.3 is: Score'(T i )=Score(T i )+λ·U att (T i ), where λ is the potential field coupling coefficient and Score(Ti) is the original DWA score. In step S3.1, the minimum collision time is calculated as follows: Among them, p i is the current position coordinate of UAV i, r i , r j It is the safety radius set by the drone safety distance standard

[0021] Preferably, in step S2.1, when the remaining battery power of the drone is less than 20%, the gravity gain coefficient is increased to 2.0 to enhance path convergence; ε is set to 0.1 to prevent the denominator from being zero.

[0022] Preferably, in step S3, the critical obstacle density is set to N crit =10.

[0023] Preferably, in step S3.2, the generation rule of the ORCA constraint half plane is: the component of the speed adjustment amount of UAV i in the normal vector direction is not less than in Speed ​​adjustment vector, calculated as:

[0024] Preferably, in step S5, the calculation formulas for α, β, and γ are: where d target is the remaining distance to the target point, d safe The safety warning distance is set according to the radar detection accuracy, E remain is the remaining power monitored in real time, E total is the total battery power of the drone.

[0025] A computer-readable storage medium storing one or more programs, characterized in that: the one or more programs include instructions, and when the instructions are executed by a computing device, the computing device executes any one of a method for intelligent return-to-nest and return path planning of a drone.

[0026] A device for intelligent return-to-nest and return-path planning for a drone, characterized in that it comprises one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any one of the methods according to claims 1 to 9.

[0027] Compared with the prior art, the present invention has the following beneficial effects:

[0028] 1. The fully bidirectional search strategy establishes an organic coupling between the global gravitational field and the local obstacle repulsion field, enabling the planning process to both incorporate global optimal information and flexibly respond to the local environment, thereby significantly improving path smoothness and convergence speed.

[0029] 2. Through a dynamic time step mechanism, the algorithm can adjust predictions in real time based on the complexity of the flight environment. Whether in areas with dense obstacles or open spaces, it can balance obstacle avoidance accuracy with real-time calculations, ensuring that the drone can make quick decisions in highly dynamic scenarios without lags or response delays.

[0030] 3. The variable weight evaluation function takes into account key operating parameters such as remaining power and obstacle density, so that when the drone is short of power or the obstacle density changes suddenly, it can actively increase the weight of the safety dimension or energy-saving dimension, thereby improving the environmental adaptability of the overall path planning and the reliability of mission completion.

[0031] 4. The ORCA collaborative obstacle avoidance unit superimposes reciprocal safety constraints between multiple aircraft on the candidate speed space originally generated based on the DWA of a single aircraft. By constructing a constraint half-plane and eliminating high-risk speed vectors, it effectively avoids track overlap and intersection during the group's return, significantly reducing collaborative interference in cluster flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 Schematic diagram of the system architecture of the present invention;

[0033] Figure 2 It is a schematic diagram of the algorithm flow of the present invention; DETAILED DESCRIPTION

[0034] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0035] See also Figures 1 to 2The present invention provides an embodiment of a method, storage medium, and device for intelligent return-to-nest and return-to-nest planning of a UAV, including a nest positioning unit, an environment perception unit, an inter-machine communication unit, an improved DWA algorithm unit, an ORCA collaborative obstacle avoidance unit, a battery unit, and a flight control actuator.

[0036] The aircraft's nest positioning unit is equipped with a dual-frequency Beidou module with a positioning accuracy of ±0.1m; the environmental perception unit includes a 16-line lidar; the inter-machine communication unit includes a LoRa-based ADS-B broadcast module with an update rate of 10Hz; the battery unit includes a battery management system (BMS); the flight control actuator includes an electronic speed regulator and a servo controller, and the servo controller includes a gyroscope.

[0037] The specific implementation steps are as follows:

[0038] S1. Improve the DWA algorithm to obtain the current position of the drone and the coordinates of the aircraft nest, the coordinates of the obstacle point cloud, the position of the neighboring aircraft, the current speed v and angular velocity w of the drone:

[0039] Among them, the machine nest positioning unit obtains the machine nest coordinates P through GPS / RTK positioning nest and the current position coordinates of the drone P current ;

[0040] The environment perception unit generates obstacle point cloud coordinates through lidar SLAM: Nobs is the number of obstacles detected by the lidar, with a detection distance of 150m;

[0041] The inter-machine communication unit includes ADS-B broadcast signal analysis to obtain the neighboring aircraft position and speed set: {(P j ,V j )|j∈[1,N]}, N is the number of adjacent drones, Pj is the position coordinate of the jth adjacent drone, Vj is the speed of the jth adjacent drone, and the update rate is set to 10Hz to quickly update the position information and quickly adjust the drone's return trajectory.

[0042] The servo controller in the flight control actuator measures the current speed v and angular velocity w of the UAV. The electronic speed regulator in the flight control actuator is used to adjust the UAV's movement speed and angular velocity, and feeds back the speed and angular velocity to the improved DWA algorithm unit.

[0043] S2, the improved DWA algorithm unit receives all the parameters obtained in S1 and takes the target point P nest As the starting point, move towards the current position of the drone P current The virtual gravitational field is generated in the direction, and the global gravitational component of the current spatial evaluation point coordinate p is calculated. The gravitational field function is: Among them, k att∈[0.5,2.0] is the gravity gain coefficient (dynamically adjusted according to the remaining power, increased to 2.0 when Eremain<20%); ε is the numerical stability factor used to prevent the denominator from being 0, set to 0.1.

[0044] S3, using a bidirectional search strategy, generates a set of candidate trajectories {Ti} according to the standard DWA method, and each trajectory Ti is stored as a matrix: Among them, [t1x1 y1 w1] means that the trajectory coordinates at time t1 are (x1, y1) and the heading angle is w1.

[0045] S4. The end point of each trajectory Ti Calculate the potential energy value of the global gravitational field. The potential energy evaluation formula is: The fast integration method is used to calculate the potential energy accumulation value to improve the operation efficiency, where the discretized trajectory sampling points are n=10; Δp k is the trajectory segment length, which is determined by the current velocity v and the time step Δt, and the calculation formula is Δp k =v·Δt.

[0046] S5. Incorporate the potential energy value calculated in S4 into the trajectory scoring function to guide the path to converge to the optimal direction. The scoring function of the improved DWA algorithm is: Score'(T i )=Score(T i )+λ·U att (T i ), where λ is the potential field coupling coefficient, which is calibrated by track tracking experiments and set to 0.3, and Score(Ti) is the original DWA algorithm score.

[0047] S6. Loop through S3-S5 until the optimal Score'(Ti) is obtained. Record the candidate trajectory with the highest score and mark it as the best candidate trajectory, where {i=n|i=1,2,...,n}.

[0048] S7, ORCA collaborative obstacle avoidance unit receives the best candidate trajectory of each UAV in S6 and calculates the collision time τ between each UAV col If the collision time is less than the set value, the path of the best candidate trajectory is adjusted to generate a predicted trajectory. For example, if the minimum collision time τ between drones i and j is calculated, col <τ (τ is the collision time threshold determined by human-machine reaction time experiments and is set to 2s), the obstacle avoidance constraint is triggered and step S7.1 is executed; otherwise, step S8 is executed;

[0049] S7.1 Convert the (v,w) generated by DWA into a velocity vector (θ is the angle between the velocity vector and the x-axis);

[0050] S7.2 Calculate the speed difference between drones i and j, divide the responsibility for obstacle avoidance according to the responsibility factor, compare the responsibility factors of drones i and j, and adjust the best candidate trajectory for the drone with the larger responsibility factor. The responsibility factor of i, η i The calculation formula is: 0≤η i ≤1; j's responsibility factor η j The calculation of η is the same as above i The formula states;

[0051] For example, if the responsibility factor of drone i is greater than that of drone j (if it is less, drone j will be adjusted),

[0052] The above adjustments are as follows:

[0053] Step 1: Calculate the velocity adjustment vector of drone i The calculation formula is

[0054]

[0055] Step 2: Generate the ORCA constraint half plane. The constraint rule is that the speed adjustment of drone i in the normal vector direction is not less than The calculation formula for the ORCA constraint half plane is:

[0056] Step 3: Obtain all possible velocity vectors of drone i by taking the value of θ The value range of θ is determined by the structure of the drone, and all velocity vectors are recorded. That is, the above content is to calculate the range of adjustable speed and angular velocity of drone i and find out all adjustment possibilities.

[0057] Step 4: Check whether all velocity vectors generated in step 3 fall within the ORCA half-plane, eliminate velocity vectors that do not satisfy the constraints, and find the set V of velocity vectors that satisfy all ORCA constraints. sample :

[0058] S8, if the set V in S7 sample If there is only one element in , execute S8; otherwise, judge the velocity pairs of the velocity vectors in the set to predict the trajectory and evaluate the optimal trajectory. The specific contents are as follows:

[0059] S8.1 The current flight velocity vt of drone i is decomposed along the x and y axes. When the velocity change occurs within a very small time dt, it can be approximated as uniform linear motion. Combined with the velocity spatial sampling at time t, the predicted trajectory of the drone at time (t + dt) can be obtained: Where at is the current flight acceleration of the UAV at time t; w t is the current flight angular velocity of the UAV at time t; w at is the angular acceleration of the UAV at time t;

[0060] S8.2 scores multiple predicted trajectories and selects the predicted trajectory with the highest score as the optimal trajectory. The scoring function is Where α is the heading angle weight; β is the obstacle avoidance weight. The closer the distance between UAV i and the obstacle, the greater the weight coefficient; γ is the speed gain weight. That is, the smaller the remaining power, the greater the weight coefficient. The dynamic weight calculation formula is: Among them, d0 is the obstacle avoidance feature distance, which is set to 5m, and w c is the heading angle deviation at the end point of the trajectory, dtarget is the remaining distance to the target point, and dmin is the distance between the trajectory and the nearest obstacle. dsafe is the safety warning distance set according to the radar detection accuracy and is set to 5m. Eremain is the remaining power monitored in real time by the BMS system.

[0061] S9, the improved DWA algorithm unit outputs control instructions to the flight control actuator, and the flight control actuator controls the movement of the UAV according to the selected optimal trajectory;

[0062] For S3 in S1-S9 above, the DWA algorithm can also be optimized through dynamic time step adjustment. Compared with the traditional DWA algorithm, the stability and adaptability of obstacle avoidance are increased. Specifically, when obstacles are dense, the step length is shortened to adapt to the increased complexity of the environment. In open areas, the prediction time is extended to speed up the calculation, and the step length is flexibly adjusted. The dynamic time step calculation formula is: Where tbase is the base prediction time, Ncrit is the critical obstacle density set; vmax is the maximum speed of the drone; and v is the flight speed.

[0063] The following table shows the simulation test of the improved DWA fusion algorithm in MATLAB. The experimental object is a small quad-rotor UAV. The parameter settings of the UAV are shown in Table 1. The parameter settings of the improved DWA fusion algorithm are shown in Table 2.

[0064] Table 1 UAV operating parameters

[0065]

[0066] Table 2 Parameter settings of improved DWA fusion algorithm

[0067]

[0068] Scenario 1: Single UAV obstacle avoidance in densely populated areas (50×50m area)

[0069] Table 3 Simulation analysis results of scenario 1

[0070]

[0071] Scenario 2: 20 drones return to base in a coordinated manner (return distance 500m)

[0072] Table 4 Simulation analysis results of scenario 2

[0073]

[0074] Experimental data show that

[0075] 1. The improved DWA fusion algorithm improves obstacle avoidance efficiency by 21.8% to 26.3% compared to traditional methods;

[0076] 2. System resource consumption in multi-machine collaboration scenarios (combined scenarios 1 and 2) is reduced by 67%

[0077] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations within the meaning and range of equivalents of the claims are intended to be included therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

Claims

1. A method for intelligently returning to the home nest and planning the return path of a UAV, characterized in that: The following steps are involved: S1. Improve the DWA algorithm to obtain the current position of the UAV and the coordinates of the aircraft nest, the coordinates of the obstacle point cloud, the position of the neighboring aircraft, the current speed v and angular velocity w of the UAV; S2. Generate candidate trajectory sets based on bidirectional search strategy: S2.1 Take the target point P nest As the starting point, move towards the current position P of the drone current Direction generates a virtual gravitational field; Calculate the global gravitational component of the current spatial evaluation point coordinate p. The gravitational field function is: Among them, k att ∈[0.5,2.0], is the gravitational gain coefficient; ε is the numerical stability factor; S2.2 generates a virtual gravitational field with the machine nest coordinates as the starting point and calculates the potential energy value at the end point of the trajectory. S2.3 improves the evaluation function of the DWA algorithm to predict the dynamic obstacle trajectory, guide the path to converge to the optimal direction, and obtain the best candidate trajectory; S2.4 generates candidate trajectories according to the dynamic time step adjustment rule. The dynamic time step calculation formula is: where t base is the basic prediction duration, N crit The set critical obstacle density; v max is the maximum speed of the drone; v is the flight speed; S3. Use the ORCA collaborative obstacle avoidance algorithm to evaluate the best candidate trajectory, adjust the best candidate trajectory that will cause collision, and generate the predicted trajectory of the drone, including: S3.

1. Calculate the minimum collision time between drones based on the set safety radius; S3.

2. When the collision time is less than a preset value, an ORCA constraint half-plane is generated, and velocity vectors that do not meet the ORCA constraint half-plane are eliminated; S3.

3. Record the adjustment amount of the velocity and angular velocity of the velocity vector and the optimal candidate trajectory according to the velocity vector. S4. Obtain the angular velocity and speed of the predicted trajectory, evaluate the predicted trajectory, and select the predicted trajectory with the highest score as the optimal trajectory. The obstacle avoidance weight β of the scoring function increases with the increase of obstacle density. The calculation formula is: Among them, α is the heading angle weight, β is the obstacle avoidance weight, γ is the speed gain weight, d0 is the obstacle avoidance feature distance, w c is the heading angle deviation at the end of the trajectory, d min is the distance from the trajectory to the nearest obstacle. S5. Output the optimal trajectory to the flight control actuator.

2. The method for intelligently returning to the home station and planning the return path of a UAV according to claim 1, characterized in that: The calculation formula for the potential energy value of the trajectory end point in step 2.2 is: The sampling points of the discretized trajectory are n=10; Δp k is the trajectory segment length, which is determined by the current velocity v and the time step Δt, and the calculation formula is Δp k =v·Δt.

3. The method for intelligently returning to the home nest and planning the return path of a UAV according to claim 2, characterized in that: The evaluation function of the improved DWA algorithm in step 2.3 is: Score'(T i )=Score(T i )+λ·U att (T i ), where λ is the potential field coupling coefficient and Score(Ti) is the original DWA score.

4. The method for intelligently returning to the home nest and planning the return path of a UAV according to claim 1, characterized in that: In step S3.1, the calculation formula for the minimum collision time is: Among them, p i is the current position coordinate of UAV i, r i , r j It is the safety radius set by the drone safety distance standard.

5. The method for intelligently returning to the home station and planning the return path of a UAV according to claim 1, characterized in that: In step S2.1, when the remaining battery power of the UAV is less than 20%, the gravity gain coefficient is increased to 2.0 to enhance path convergence; ε is set to 0.1 to prevent the denominator from being zero.

6. The method for intelligently returning to the home station and planning the return path of a UAV according to claim 1, characterized in that: In step S3, the critical obstacle density is set to N crit =10.

7. The method for intelligently returning to the home nest and planning the return path of a UAV according to claim 4, characterized in that: In step S3.2, the generation rule of the ORCA constraint half plane is: the component of the speed adjustment amount of UAV i in the normal vector direction is not less than in Speed ​​adjustment vector, calculated as:

8. The method for intelligently returning to the home nest and planning the return path of a UAV according to claim 1, characterized in that: In step S5, the calculation formulas for α, β, and γ are: where d target is the remaining distance to the target point, d safe The safety warning distance is set according to the radar detection accuracy, E remain is the remaining power monitored in real time, E total is the total battery power of the drone.

9. A computer-readable storage medium storing one or more programs, characterized in that: The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform any one of the methods according to claims 1 to 8.

10. An intelligent return-to-home and return-to-home path planning device for a UAV, characterized by: The method comprises one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for executing any one of the methods according to claims 1 to 9.

Images