UAV rapid formation obstacle avoidance method based on improved DWA and potential energy method
Through the improved dynamic window method and potential energy method, combined with fuzzy control rules, dynamic obstacle avoidance and rapid formation of drones in complex environments is achieved, and the problem of difficulty in effectively avoiding obstacles and formations in complex environments in the existing technology is solved, and the task efficiency is improved.
Patent Information
- Application Number
- CN202411213932.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-31
- Publication Date
- 2025-03-25
AI Technical Summary
The prior art is difficult to achieve dynamic obstacle avoidance and rapid formation of drones in complex environments, affecting mission efficiency.
The improved dynamic window method (DWA) and potential energy method are adopted to construct reference coordinate system, virtual structure method formation model, velocity sampling space, potential energy evaluation function and improved target directional evaluation function, combined with fuzzy control rules, the evaluation function weight is adaptively adjusted to achieve safe and fast formation obstacle avoidance of the drone.
The dynamic obstacle avoidance and rapid formation capabilities of the drone have been improved, and the formation obstacle avoidance can be effectively achieved in complex environments and improve mission efficiency.
Smart Images
Figure CN119690091A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of formation flight control, and in particular relates to a UAV fast formation obstacle avoidance method based on improved DWA and potential energy method. Background Art
[0002] Multi-UAV formation obstacle avoidance is one of the important research topics in the field of formation flight control and is widely used in military, environmental monitoring, disaster relief, terrain mapping and other fields. In actual flight, UAVs will face the challenge of dynamic obstacles, which requires them to be able to perform effective dynamic obstacle avoidance. After successfully avoiding obstacles, it is also critical to quickly restore the formation, otherwise it will be difficult to maintain effective communication, which will affect the efficiency of completing the mission. Existing fixed-wing UAV formation obstacle avoidance still has the above problems.
[0003] In order to solve the problems of dynamic obstacle avoidance and rapid formation, the present invention provides a UAV rapid formation obstacle avoidance method based on improved DWA and potential energy method, which improves the dynamic obstacle avoidance capability and rapid formation capability of the UAV and can realize effective formation obstacle avoidance in complex environments. Summary of the invention
[0004] (1) Construct reference coordinate system, fixed-wing aircraft coordinate system, virtual structure method formation model and UAV three-dimensional motion equation;
[0005] (2) Construct the fixed-wing UAV velocity sampling space, i.e., the search space of the dynamic window method, and generate the predicted trajectory based on the sampling space and the UAV kinematic model;
[0006] (3) Based on the artificial potential energy function and the positions of static and dynamic obstacles, the potential energy evaluation function is constructed in combination with the relevant prediction points in the predicted trajectory;
[0007] (4) Improve the target orientation evaluation function based on the deviation angle magnification factor and the UAV heading deviation;
[0008] (5) Based on the sigmoid function, construct the speed control function of the fixed-wing UAV;
[0009] (6) Based on fuzzy control rules, the weights of the evaluation function are adaptively adjusted to prevent the fixed-wing UAV from falling into the local optimum.
[0010] Specifically, the step (1) is to establish the three-dimensional motion equation of each fixed-wing UAV in the formation, and the specific steps are as follows:
[0011] (1.1) The reference coordinate system refers to the north-east coordinate system, which is used to describe the position and posture of the fixed-wing UAV. The UAV motion equation is established as follows:
[0012]
[0013] Where Δt is the sampling time, is the real-time speed of the UAV, (X g(t) ,Y g(t) ,Z g(t) ) is the current coordinate, (X g(t+1) ,Y g(t+1) ,Z g(t+1) ) is the coordinate of the next moment.
[0014] (1.2) From the body coordinate system oX w X w X w To the reference coordinate system oX g X g X g The transformation matrix is:
[0015]
[0016] Where c and s are the cosine and sine values respectively.
[0017] (1.3) Establish the relationship between angular velocity and Euler angle, the formula is as follows:
[0018]
[0019] where ξ=[φ,θ,ψ] T ,ω=[ω x ,ω y ,ω z ] T ,ω x ,ω y and ω z is the component of the angular velocity ω on the body coordinate axis.
[0020] (1.4) Establish the drone attitude equation as follows:
[0021]
[0022] (1.5) Let the virtual formation be P, P = P i (i=1,2,3···I) are virtual formation members, among which P1 is the virtual leader and flies at a constant speed V0. According to the geometric constraints and the specific position of the virtual leader, the specific position of each virtual wingman can be calculated. The position of P2 is as follows:
[0023]
[0024] Among them, P 1N , P 1E , P 1D and P 2N , P 2E ,P2D They represent the components of the virtual leader and virtual wingman 2 in the north, east, and toward the center of the earth, respectively. α is the angle between the drones in the formation. The positions of the remaining virtual wingmen can be calculated using the same method.
[0025] Specifically, the steps for constructing the velocity space in step 2 are as follows:
[0026] (2.1) Due to the limitation of hardware conditions, the speed of fixed-wing UAV is limited to a certain range. The formula is as follows:
[0027]
[0028] (2.2) Due to the limitation of acceleration, the speed of fixed-wing UAV is limited to a certain range. The formula is as follows:
[0029]
[0030] v cur and ω cur are the real-time linear velocity and angular velocity respectively; a, b, c, and d represent the maximum linear acceleration and angular acceleration respectively.
[0031] (2.3) Fixed-wing UAVs require a certain linear speed to maintain lift and prevent stalling, so their speed is limited to a certain range. The formula is as follows:
[0032]
[0033] Since fixed-wing drones require a certain linear speed to maintain lift, the dis min (v,ω x ,ω y ,ω z ) is defined as the deceleration distance from the current speed to the minimum speed at which the fixed-wing UAV can maintain flight.
[0034] (2.4) Therefore, the entire velocity space V can be expressed as follows:
[0035] V=V s ∩V d ∩V a (9)
[0036] Specifically, the steps for constructing the potential energy evaluation function in step 3 are as follows:
[0037] (3.1) Input the coordinates of dynamic obstacles and static obstacles;
[0038] (3.2) constructing a potential energy evaluation function based on the artificial potential energy function;
[0039]
[0040] Formula 10 is the potential energy function of the repulsive potential field, where k is the proportionality coefficient, ρ(q,q obs ) is a vector whose direction is from the obstacle to the drone and whose magnitude is the distance between the drone and the obstacle. ρ0 is a constant representing the maximum range of the potential field. Formula 11 is the sum of the potential energy received by the drone, where i represents the number of predicted trajectory points and j represents the number of obstacles. When the predicted trajectory of the drone is within the range of the potential field, the potential energy of the relevant trajectory points will be calculated. This allows trajectories that are far away from obstacles to receive high scores, thus achieving dynamic obstacle avoidance.
[0041] Specifically, the steps of constructing the improved target orientation evaluation function in step (4) are as follows:
[0042] (4.1) By constructing the deviation angle magnification factor, the formula is as follows:
[0043] ω(θ)=e kθ (12)
[0044] θ is the deviation angle. When the deviation angle is small, ω(θ) is close to 1. When the deviation angle increases, ω(θ) will increase rapidly, thereby amplifying the influence of the deviation angle.
[0045] (4.2) After substituting the deviation angle magnification factor into the pitch target orientation function and the yaw target orientation function, the formula is as follows:
[0046] Heading ψ =(180-|ψ|) ω(ψ) (13)
[0047] Heading θ =(180-|θ ) ω(θ) (14)
[0048] When the deviation angle is small, the target orientation function's evaluation of larger and smaller yaw trajectories is close to that of the original target orientation function. When the deviation angle increases, the target orientation function's evaluation of larger and smaller deviation trajectories will be more inclined to trajectories with smaller deviation angles than the original target orientation function.
[0049] Specifically, the steps of establishing the sigmoid speed control function in step (5) are as follows:
[0050] (5.1) By introducing the Sigmoid function, the formula is as follows:
[0051]
[0052] (5.2) Construct the speed control function, the formula is as follows:
[0053]
[0054] where v target is the target speed, v max is the maximum speed, v min is the minimum speed, v current is the current speed, d safe is the safety distance, d buffer is the buffer distance. When the distance between the wingman and the target point is greater than the safety distance, formula (16) is executed. When the distance between the wingman and the target point is less than the safety distance, formula (17) is executed.
[0055] Specifically, the steps of establishing the fuzzy control adaptive adjustment evaluation function weight in step (6) are as follows:
[0056] (6.1) The above steps include three evaluation functions, plus the following distance evaluation function, which constitute the entire evaluation function of the improved dynamic window method;
[0057] G=α·Heading ψ +β·Heading θ +γ·Dist-δ·Pe (18)
[0058]
[0059] (6.2) Let d g is the distance between the UAV and the target point, d o is the distance between the UAV and the obstacle. The target point of the virtual leader is (xgoal, ygoal, zgoal), as shown in formula (20). Let (d g ,d o ) is the input and the four weight parameters (α, β, γ, δ) are the output.
[0060] (6.3) The input fuzzy set is divided into near (PS), medium (PM), and far (PB). The output has 4 weight parameters, which are small (PS), medium (PM), and large (PB). The weight range is [0, 0.5]. The membership function is as follows: Figure 3 and Figure 4 shown.
[0061] (6.4) The final weights (α, β, γ, δ) are given by the matrix [d g d o ] and the fuzzy relationship matrix R.
[0062] [α,β,γ,δ]=[d g d o ]·R (20) Through the above improvements, fixed-wing UAVs can safely and quickly perform formation and obstacle avoidance. BRIEF DESCRIPTION OF THE DRAWINGS
[0063] Attached Figure 1 It is a flow chart of the present invention;
[0064] Attached Figure 2 It is a formation information diagram of the present invention;
[0065] Attached Figure 3 A membership function diagram for fuzzy control input in the present invention;
[0066] Attached Figure 4 It is the membership function diagram of the fuzzy control output in the present invention;
[0067] Attached Figure 5 It is the fast formation obstacle avoidance diagram in the present invention; DETAILED DESCRIPTION
[0068] The following further describes a UAV fast formation obstacle avoidance method based on improved DWA and potential energy method provided by the present invention in conjunction with the accompanying drawings of the specification;
[0069] The present invention provides a UAV fast formation obstacle avoidance method based on improved DWA and potential energy method, comprising the following steps:
[0070] Step (1) constructing a reference coordinate system, a fixed-wing body coordinate system, a virtual structure method formation model, and a three-dimensional motion equation of the UAV;
[0071] (1.1) The reference coordinate system refers to the north-east coordinate system, which is used to describe the position and posture of the fixed-wing UAV. The UAV motion equation is established as follows:
[0072]
[0073] Where Δt is the sampling time, is the real-time speed of the UAV, (X g(t) ,Y g(t) ,Z g(t) ) is the current coordinate, (X g(t+1) ,Y g(t+1) ,Z g(t+1) ) is the coordinate of the next moment.
[0074] (1.2) From the body coordinate system oX w X w X w To the reference coordinate system oX g X g X g The transformation matrix is:
[0075]
[0076] Where c and s are the cosine and sine values respectively.
[0077] (1.3) Establish the relationship between angular velocity and Euler angle, the formula is as follows:
[0078]
[0079] where ξ=[φ,θ,ψ] T ,ω=[ω x ,ω y ,ω z ] T ,ω x ,ω y and ω z is the component of the angular velocity ω on the body coordinate axis.
[0080] (1.4) Establish the drone attitude equation as follows:
[0081]
[0082] (1.5) Let the virtual formation be P, P = P i (i=1,2,3···I) are virtual formation members, among which P1 is the virtual leader and flies at a constant speed V0. According to the geometric constraints and the specific position of the virtual leader, the specific position of each virtual wingman can be calculated. The position of P2 is as follows:
[0083]
[0084] Among them, P 1N , P 1E , P 1D and P 2N , P 2E ,P 2D They represent the components of the virtual leader and virtual wingman 2 in the north, east, and geocentric directions respectively. α is the angle between the drones in the formation. The positions of the remaining virtual wingmen can also be obtained using the same method.
[0085] Step (2) constructing a fixed-wing UAV velocity sampling space, i.e., a search space for the dynamic window method, and generating a predicted trajectory based on the sampling space and the UAV kinematic model;
[0086] (2.1) Due to the limitation of hardware conditions, the speed of fixed-wing UAV is limited to a certain range. The formula is as follows:
[0087]
[0088] (2.2) Due to the limitation of acceleration, the speed of fixed-wing UAV is limited to a certain range. The formula is as follows:
[0089]
[0090] v cur and ω cur are the real-time linear velocity and angular velocity respectively; a, b, c, and d represent the maximum linear acceleration and angular acceleration respectively.
[0091] (2.3) Fixed-wing UAVs require a certain linear speed to maintain lift and prevent stalling, so their speed is limited to a certain range. The formula is as follows:
[0092]
[0093] Since fixed-wing drones require a certain linear speed to maintain lift, the dis min (v,ω x ,ω y ,ω z ) is defined as the deceleration distance from the current speed to the minimum speed at which the fixed-wing UAV can maintain flight.
[0094] (2.4) Therefore, the entire velocity space V can be expressed as follows:
[0095] V=V s ∩V d V a (29)
[0096] Step (3) constructing a potential energy evaluation function based on the artificial potential energy function and the positions of static and dynamic obstacles in combination with relevant prediction points in the predicted trajectory;
[0097] (3.1) Input the coordinates of dynamic obstacles and static obstacles;
[0098] (3.2) Constructing the potential energy evaluation function based on the artificial potential energy formula;
[0099]
[0100] Formula 30 is the potential energy function of the repulsive potential field, where k is the proportionality coefficient, ρ(q,q obs ) is a vector with a direction from the obstacle to the drone and a magnitude of the distance between the drone and the obstacle. ρ0 is a constant that represents the maximum range of the potential field. Formula 31 is the sum of the potential energy received by the drone, where i represents the number of predicted trajectory points and j represents the number of obstacles. When the predicted trajectory of the drone is within the range of the potential field, the potential energy of the relevant trajectory points will be calculated. This allows trajectories that are far away from obstacles to receive high scores, thereby achieving dynamic obstacle avoidance.
[0101] Step (4) improving the target orientation evaluation function based on the deviation angle magnification factor and the UAV heading deviation;
[0102] (4.1) By constructing the deviation angle magnification factor, the formula is as follows:
[0103] ω(θ)=e kθ (32)
[0104] θ is the deviation angle. When the deviation angle is small, ω(θ) is close to 1. When the deviation angle increases, ω(θ) will increase rapidly, thereby amplifying the influence of the deviation angle.
[0105] (4.2) After substituting the deviation angle magnification factor into the pitch target orientation function and the yaw target orientation function, the formula is as follows:
[0106] Heading ψ =(180-|ψ|) ω(ψ) (33)
[0107] Heading θ =(180-|θ|) ω(θ) (34)
[0108] When the deviation angle is small, the target orientation function's evaluation of larger and smaller yaw trajectories is close to that of the original target orientation function. When the deviation angle increases, the target orientation function's evaluation of larger and smaller deviation trajectories will be more inclined to trajectories with smaller deviation angles than the original target orientation function.
[0109] Step (5) constructing a speed control function of the fixed-wing UAV based on the sigmoid function;
[0110] (5.1) By introducing the Sigmoid function, the formula is as follows:
[0111]
[0112] (5.2) Construct the speed control function, the formula is as follows:
[0113]
[0114] where v target is the target speed, v max is the maximum speed, v min is the minimum speed, v current is the current speed, d safe is the safety distance, d buffer is the buffer distance. When the distance between the wingman and the target point is greater than the safety distance, formula (36) is executed. When the distance between the wingman and the target point is less than the safety distance minus the buffer distance, formula (37) is executed.
[0115] Step (6) adaptively adjusts the evaluation function weight based on fuzzy control rules to prevent the fixed-wing UAV from falling into the local optimum.
[0116] (6.1) The above steps include three evaluation functions, plus the following distance evaluation function, which constitute the entire evaluation function of the improved dynamic window method;
[0117] G=α·Heading ψ +β·Heading θ +γ·Dist-δ·Pe (38)
[0118]
[0119] (6.2) Let d g is the distance between the UAV and the target point, d o is the distance between the UAV and the obstacle. The target point of the virtual leader is (xgoal, ygoal, zgoal), as shown in formula (40). Let (d g ,d o ) is the input and the four weight parameters (α, β, γ, δ) are the output.
[0120] (6.3) The input fuzzy set is divided into near (PS), medium (PM), and far (PB). The output has 4 weight parameters, which are small (PS), medium (PM), and large (PB). The weight range is [0, 0.5]. The membership function is as follows: Figure 3 and Figure 4 shown.
[0121] (6.4) The final weights (α, β, γ, δ) are given by the matrix [d g d o ] and the fuzzy relationship matrix R.
[0122] [α,β,γ,δ]=[d g d o ]·R (40)
[0123] Through the above improvements, fixed-wing UAVs can safely and quickly perform formation and obstacle avoidance.
Claims
1. A UAV fast formation obstacle avoidance method based on improved DWA and potential energy method, characterized in that: The steps include: Step 1, construct the reference coordinate system, the fixed-wing body coordinate system, the virtual structure method formation model and the UAV three-dimensional motion equation; Step 2: construct the fixed-wing UAV velocity sampling space, i.e., the search space of the dynamic window method, and generate the predicted trajectory based on the sampling space and the UAV kinematic model; Step 3, based on the artificial potential energy function and the positions of static and dynamic obstacles, combined with relevant prediction points in the predicted trajectory, construct a potential energy evaluation function; Step 4, based on the deviation angle magnification factor and the UAV heading deviation, improve the target orientation evaluation function; Step 5: Based on the sigmoid function, construct the speed control function of the fixed-wing UAV; Step 6: Based on the fuzzy control rules, the evaluation function weights are adaptively adjusted to prevent the fixed-wing UAV from falling into the local optimum.
2. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that In step 1, the influence of multiple attitude angles of the UAV is considered to construct a three-dimensional motion equation.
3. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that In step 2, the hardware condition restrictions, acceleration restrictions, and stall restrictions of the drone are taken into consideration to construct a fixed-wing drone speed sampling space and generate a predicted trajectory within the restrictions.
4. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that The potential energy method is introduced in step 3, and the formula is as follows: Construct the potential energy evaluation function, the formula is as follows: Where i represents the number of predicted trajectory points, and j represents the number of obstacles. When the predicted trajectory of the drone is within the potential field range, the potential energy of the relevant trajectory points will be calculated. This allows trajectories that are far away from obstacles to receive high scores, thus achieving dynamic obstacle avoidance.
5. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that In step 4, in order to quickly restore the drone's heading, a deviation angle magnification factor is introduced, and the formula is as follows: ω(θ)=e kθ (3) Then the target orientation function is improved, and the formula is as follows: Heading ψ =(180-ψ) ω(ψ) (4) Heading θ =(180-θ) ω(θ) (5) It speeds up the UAV's heading recovery speed, which indirectly promotes the rapid formation of UAVs.
6. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that In step 5, in order to achieve fast formation, the sigmoid function is introduced, and the formula is as follows: A speed control function is constructed, and the formula is as follows: Optimized speed control, facilitating fast formations.
7. The UAV fast formation obstacle avoidance method based on improved DWA and potential energy method according to claim 1, characterized in that In step 6, in order to prevent the UAV from falling into a local optimum, fuzzy control is used to adaptively adjust the weight of the evaluation function.