Path planning and cooperative control method for heterogeneous unmanned swarm systems in dynamic environments

Abstract

The present invention discloses a path planning and collaborative control method for a heterogeneous unmanned cluster system in a dynamic environment. First, a distributed position observer of predefined time is constructed, and the initial and final formation target positions of each follower UAV are estimated by using local information interaction between UAVs. A global path planner based on an adaptive variable solution space RRT algorithm is constructed to plan a safe global path for each UAV when it passes through a dense obstacle area; a local path planner based on a predefined time prediction filter is constructed to avoid the potential collision risk of UAVs with dynamic obstacles during formation tracking; finally, based on the safe global path and local path planning results, a sliding mode control method is used to control the motion of the UAVs, thereby realizing fixed-time formation tracking control of the UAV cluster. The use of the present invention can enable a multi-UAV system to achieve safe flight in a complex dynamic environment, and ensure control performance when achieving formation tracking flight missions.

Description

Technical Field

[0001] The present invention relates to the technical field of cluster unmanned aerial systems, and in particular to a path planning and collaborative control method for a heterogeneous unmanned cluster system in a dynamic environment. Background Art

[0002] In recent years, thanks to their high efficiency, flexibility, and strong stability, multi-UAV systems have been widely used in emergency rescue, environmental monitoring, cargo transportation, surveillance and reconnaissance, and industrial (power grid) inspection. Path planning and formation control of UAV systems have been a research hotspot for decades. These efforts specifically involve global and local path planning for safe obstacle avoidance, as well as the design of distributed formation control protocols for achieving different control objectives. Among these, how to simultaneously consider global path optimization and local dynamic obstacle avoidance for multiple UAVs in complex environments, while achieving coordinated formation tracking of the entire system within a fixed timeframe, has been a pressing issue in the field of precise navigation and coordinated control of swarm UAVs.

[0003] Regarding global path planning for multi-UAV swarms, global path planning for swarm unmanned systems aims to effectively plan a safe path for each UAV in a known environment. However, most existing path planning solutions focus on path planning for a single UAV. Directly applying these solutions to multi-UAV systems results in slow convergence and search efficiency. Furthermore, existing global path planning algorithms only consider sparse, simple static obstacles and cannot guarantee safety in environments with dynamic obstacles.

[0004] Therefore, how to design an efficient cluster path planning solution in a complex environment with dense static and dynamic obstacles to ensure the safe flight of multi-UAV systems is currently a hot and difficult issue.

[0005] When it comes to formation control of multi-UAV swarms, the core of encirclement control is to design a controller that ensures the positions of each follower UAV converge within the convex hull formed by the positions of multiple leader UAVs, maintaining consistency. However, as the complexity of multi-UAV system flight missions increases, such as multi-point detection and reconnaissance, regional exploration and mapping, distributed target tracking, and multi-location material distribution and rescue missions, single-formation encirclement control is unable to meet these mission requirements.

[0006] Therefore, under these complex mission conditions, how to group, form, and control multiple UAV systems to improve the operating efficiency of the control system is currently a hot and difficult issue.

[0007] Furthermore, in most existing collaborative control schemes, the drone model is often a simplified linear position model, ignoring the attitude model including rotation angles, which weakens the drone's flight control maneuverability. Furthermore, existing controller design techniques only consider the system's steady-state performance, not its transient performance, making it impossible to ensure the rapidity of system collaborative control.

[0008] Therefore, how to design a controller considering the complete dynamic model of the UAV to ensure the convergence speed of the multi-UAV cluster system when realizing the group formation encirclement and tracking task is also a current research hotspot and difficult problem.

[0009] In order to solve this problem, current research has introduced the finite time and fixed time lemmas and adopted the sliding mode control method to realize the design of finite / fixed time formation tracking controller, thereby ensuring the control performance of UAV clusters in realizing group formation encirclement tasks.

[0010] However, these finite / fixed-time formation tracking control schemes do not consider the path planning problem of multi-UAV systems, and the designed sliding surfaces have singular phenomena, which cannot guarantee the safety and control performance of the cluster system in complex obstacle environments.

[0011] How to propose a group formation encirclement and tracking control scheme that can ensure the safe flight of UAV clusters in complex dynamic environments and has high control efficiency while comprehensively considering the safe navigation and formation control of multi-UAV systems is an urgent problem to be solved. Summary of the Invention

[0012] In view of this, the present invention provides a comprehensive path planning and collaborative control method for a multi-UAV flight system in a dynamic environment, which enables the multi-UAV system to achieve safe flight in a complex dynamic environment and can ensure control performance when implementing formation tracking flight missions.

[0013] To achieve the above objectives, the technical solution of the present invention is as follows: a multi-UAV system consists of C virtual leaders and N followers, and the multi-UAV system communicates internally through a connected topological graph. The multi-UAV system performs integrated path planning and distributed formation collaborative control according to the following steps:

[0014] Step 1: First, a dynamic model for the unmanned aerial system is established, including a dynamic model of a virtual leader and a dynamic model of each follower.

[0015] Step 2: Construct a distributed position observer with a predefined time. The observer convergence time is set to a predefined time T that is independent of the distributed position observer parameters. o , an observer is used to estimate the initial and final formation target positions of each follower UAV.

[0016] Step 3: Construct a global path planner based on the adaptive variable solution space RRT algorithm, and use the initial and final formation target positions estimated by the observer to plan a safe global path for each UAV when it crosses the dense obstacle area.

[0017] Among them, when the RRT algorithm generates the sampling nodes of the random spanning tree, the solution space selected by the sampling nodes adopts a variable solution space. The position and size of the solution space are determined according to the positional relationship between the nearest node of the target node and the target node or the starting node. When determining the expansion step of the random spanning tree, when the angle of the sampling node deviating from the target node is less than the set value, the expansion step of the random spanning tree is increased according to the deviation angle.

[0018] Step 4: Construct a local path planner based on a predefined time prediction filter to avoid the potential collision risk of the UAV with dynamic obstacles during formation tracking. In this step, a prediction filter based on a predefined time is constructed to estimate the obstacle speed; the speed obstacle method is used to perform local path planning based on the UAV speed and the obstacle speed estimated by the filter to avoid the potential collision risk of the UAV with dynamic obstacles during formation tracking; the prediction filter convergence time is set to a predefined time T that is independent of the prediction filter parameters. f ;

[0019] Step 5: Based on the safe global path and local path planning results, the sliding mode control method is used to control the UAV motion and realize the fixed-time formation tracking control of the UAV cluster: Based on the fixed-time sliding mode control method, the error subsystem of each UAV is first constructed, and then the fixed-time sliding mode surface containing the piecewise function is designed. Then, the designed fixed-time sliding mode reaching law is used to obtain the non-singular fixed-time sliding mode controller u for each position subsystem of each UAV. p,isj and the non-singular fixed-time sliding mode controller u for each attitude subsystem Θ,isj , to achieve fixed-time formation tracking control of UAV swarms.

[0020] Furthermore, in step 1, a dynamic model for the unmanned aerial system is established, including a dynamic model of the virtual leader and a dynamic model of each follower, specifically using the following steps:

[0021] The first-order integrator dynamics model of the C virtual leaders is as follows:

[0022]

[0023] in, is x 0c The derivative of x 0c and v 0care the position and velocity of the cth virtual leader, c = 1, 2, …, C, and C is the number of leaders.

[0024] The Euler-Lagrangian dynamics model of the i-th follower UAV is as follows:

[0025]

[0026] in, and Represent the position and attitude of the i-th UAV, and p i and Θ i The second derivative of . where m i For the quality of the drone, Among them, J φi ,J θi ,J ψi Represents the UAV's moment of inertia. and

[0027] They represent the acceleration caused by the resistance during the flight of the UAV and the nonlinear term when the attitude changes, where k i,x ,k i,y ,k i,z ,k i,θ ,k i,φ ,k i,ψ is the air damping coefficient. U pi =[u xi ,u yi ,u zi ] T , where u xi ,u yi ,u zi is the virtual control input of the UAV position subsystem, u xi =(cosφ i sinθ i cosψ i +sinφ i sinψ i ) / u 1i ,u yi =(cosφ i sinθ i sinψ i -sinφ i cosψ i ) / u 1i ,u zi =cosφ i cosθ iu 1i , U Θi =[u 2i ,u 3i ,u 4i ] T .u 1i ,u 2i ,u 3i ,u 4i is the actual control input of UAV i. u 3i =l i (-f 1,i -f 2,i +f 3,i +f 4,i ),u 4i =l τi (f 1,i -f 2,i +f 3,i -f 4,i ), l i and l τi is the scaling factor, f 1,i ,f 2,i ,f 3,i ,f 4,i are the lift of each rotor of UAV i respectively. Represents the acceleration due to gravity.

[0028] UAV actual control input u 1i and the desired roll angle φ ri , pitch angle θ ri The solution formula is as follows:

[0029]

[0030] Among them, u xi ,u yi ,u zi is the virtual control input of the UAV position subsystem, u xi =(cosφ i sinθ i cosψ i +(cosφ i sinθ i cosψ i +sinφ i sinψ i ) / u 1i ,u yi =(cosφ i sinθ i sinψ i -sinφ i cosψ i ) / u1i ,u zi =cosφ i cosθ i u 1i , ψ ri is the desired yaw angle.

[0031] By introducing virtual control variables, the dynamic model of the i-th UAV is transformed into the following compact nonlinear system form:

[0032]

[0033] Among them, sj=1,2,3,4,5,6 are the subsystems of the UAV. isj =p i,x ,p i,y ,p i,z ,φ i ,θ i ,ψ i and are the position (angle) and velocity (angular velocity) of the sjth subsystem of the i-th UAV. is a nonlinear term, b isj =1 / m i ,1 / m i ,1 / m i ,1 / J φi ,1 / J θi ,1 / J ψi ,u isj =u xi ,u yi ,u zi ,u 2i ,u 3i ,u 4i .

[0034] Furthermore, in step 2, a predefined time distributed position observer is constructed, and the distributed observer is used to estimate the initial and final formation target positions of each follower UAV, wherein the predefined time distributed position observer constructed is:

[0035]

[0036] Among them, sig(·) α =|·| α sign(·), sign(·) is the sign function. is the expected position state of the i-th UAV, for The derivative of x 0c is the position status of the cth virtual leader. and is the gain coefficient, m∈(0,1), λ min (·) represents the minimum eigenvalue of the matrix, H is the information transfer matrix, T o >0 is the preset convergence time. Construct the Lyapunov function of the observer and take its derivative, and we can get It satisfies the predefined time lemma form: Therefore, the constructed observer is able to converge within a predefined time. is the positive gain constant. ij =l ic -l jc , l ic and l jc are the formation vectors of the i-th and j-th UAVs relative to the position of the c-th virtual leader, respectively. By designing a suitable formation vector l ij And the information transfer matrix H can realize the grouping of formations. ij is the adjacency weight between two followers, b ic is the connection weight between the virtual leader and the follower.

[0037] In step 3, the adaptive variable solution space RRT algorithm incorporates the attraction of the target node to the new node when determining the expansion direction of the random spanning tree; when generating the sampling nodes of the random spanning tree, the target node or the random sampling point in the solution space is selected as the sampling node based on the relationship between the generated random probability value and the preset threshold.

[0038] The position and size of the solution space are determined according to the generated random probability value:

[0039] The position of the solution space is determined by averaging the coordinates of the nearest node to the target node or the starting node and the target node based on the relationship between the generated random probability value and the preset threshold.

[0040] The size of the solution space is determined by: determining the size of the solution space based on the relationship between the generated random probability and the preset threshold;

[0041] Among them, the selection of the nearest node or the starting node of the target node is determined according to the generated random probability. When the generated random probability is greater than the preset threshold, the nearest node of the target node participates in the determination of the position and size of the solution space; otherwise, the starting node participates in the determination of the position and size of the solution space.

[0042] Furthermore, in step 3, a global path planner based on the adaptive variable solution space RRT algorithm is constructed. The initial and final formation target positions estimated by the observer are used to plan a safe global path for each UAV when crossing the dense forest area. Specifically,

[0043] S301. Initialize the map and use the initial and final formation target positions estimated by the distributed position observer as the starting node sp for global planning st and target node sp g .

[0044] S302. Select RRT algorithm sampling points using the target point bias mechanism and the adaptive variable solution space strategy;

[0045] In order to improve the sampling probability of the target area and enhance the target directivity of the algorithm, the sampling point generation strategy based on the target point bias mechanism is as follows: set the target point bias threshold p target , determine the probability value p of the randomly generated sampling node rand Is it greater than or equal to p target ; If yes, select the target node sp g As the sampling point sp rand , otherwise, select a randomly generated node sp in the sampling space sample As the sampling point sp rand ;

[0046] To address the problem of over-dispersion and a large number of invalid sampling points caused by traditional RRT algorithms sampling the entire space, which significantly increases the computational load, an adaptive approach is introduced to transform the original fixed sampling space into a variable solution space that is dynamically adjusted as the sampling progresses. This narrows the necessary exploration space, thereby reducing search complexity and improving the algorithm's search efficiency.

[0047] Design of a new cylindrical adaptive variable solution space area X Avss =(x Avss ,y Avss ,z Avss ) can be determined from the center position, diameter and height of the cylinder as:

[0048]

[0049] Among them, (x cc ,y cc ) is the center position of the upper and lower bottom surfaces of the cylinder’s adaptive variable solution space, r Avss and h Avss are the diameter and height of the upper and lower bottom surfaces of the adaptive variable solution space respectively;

[0050] (x cc ,y cc ) is determined by judging the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre, then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The midpoint of the position is (x cc ,y cc ); if p Avss ≤p thre , then the starting node sp st With the target node sp g The midpoint of the position is (x cc ,y cc );

[0051] r Avss The determination method is: judge the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre , then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss ; if p Avss ≤p thre , then the starting node sp st With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss Position midpoint as (x cc ,y cc );

[0052] h Avss The method of determining h is: Avss =|z high -z low |; Among them, z low and z high are the z coordinate values ​​of the lowest and highest points in the sampling space respectively;

[0053] S303. Using the target point attraction and adaptive step size strategy to perform random tree expansion on the RRT algorithm;

[0054] Among them, in order to improve the search efficiency of the RRT algorithm, the target attraction mechanism in the potential field method is used to guide the random tree to expand towards the target node more purposefully. The expansion direction of the new node is determined by the attraction of the target point, thereby generating a new node sp new for:

[0055]

[0056] Among them, sp nst is the nearest node of the randomly sampled node, sp rand is the current generation sampling point determined in step S302, λ a is the attraction coefficient of the target point, and Δs is the random tree expansion step size.

[0057] After determining the expansion direction, in order to improve the random tree expansion speed, the random tree expansion step length is changed to an adaptive length. Calculate the current generated sampling point sp rand , randomly sampled node nearest node sp nst and target node sp g The interior angle of the triangle formed Right now then Indicates that the random tree is moving away from the target node sp g In this case, a shorter step length Δs is used to reduce the degree of deviation; on the contrary, when When , it indicates that the expansion direction is facing the target point sp g , a longer step size is used This allows the random tree to expand towards the target point more quickly.

[0058] S304. Path clipping and time redistribution are performed on the initial global path generated by the RRT algorithm, so that the time required for the UAV cluster to cross the dense obstacle area is fixed at t total , and perform B-spline optimization on the above-processed curve to obtain the expected global safety trajectory position corresponding to each UAV speed and acceleration information.

[0059] Furthermore, in step 4, a local path planner based on a predefined time prediction filter is constructed to avoid potential collision risks between drones and dynamic obstacles during formation tracking. Specifically:

[0060] S401. When an obstacle enters the detection range, a predefined time prediction filter is designed to estimate the speed of the obstacle using the detected obstacle position information. The predefined time filter is designed as follows:

[0061]

[0062] Among them, T f is a predefined time that is independent of the filter parameters; x obp is the filter input, i.e., the actual position of the obstacle measured by the sensor, which may contain measurement noise; is the filter output, i.e. the estimated position after filtering, yes The derivative of The approximate value of m∈(0,1), sig(·) α =|·| α sign(·);k f is the positive gain constant.

[0063] S402. Calculate the dynamic safety boundary of the i-th UAV relative to the obstacle:

[0064]

[0065] Among them, k sa ≥3 is the dynamic boundary coefficient, R ob =r im +r ob +R sa is the optimized safety distance of the i-th UAV to obstacles, r im is the influence radius of the UAV, r ob is the dynamic obstacle radiation radius, R sa Expand the safety distance for drones. u and V o are the movement speeds of the quadrotor drone and the obstacle, respectively. The velocity of dynamic obstacles estimated by a predefined time filter.

[0066] S403. Using the speed of the drone V u and the estimated velocity V of the dynamic obstacle o , dynamic security boundary R ob , establish a collision critical point prediction model, which includes the following three parts:

[0067] P1. Use the following formula to determine whether the drone should avoid obstacles:

[0068]

[0069] Among them, V uo and S u S o are the relative velocity and position vectors between the UAV and the unexpected dynamic obstacle, respectively. ob is the relative velocity V uo The angle between the cone line and the control obstacle, α ob0 is 1 / 2 of the half-apex angle of the speed barrier cone. ob <α ob0 When α is negative, the obstacle threatens the UAV and the UAV avoids the obstacle. ob ≥α ob0 The drone does not perform obstacle avoidance.

[0070] P2. Define F1(x1, y1, z1) and F2(x2, y2, z2) as the critical collision points. The two are determined in the same way and can be determined by the following formula (taking F1 as an example):

[0071]

[0072] P3. Under the premise of satisfying the UAV's motion performance constraints, select the critical collision point with the smallest velocity direction change between F1 and F2 as the direction guidance point F for the UAV's obstacle avoidance, that is:

[0073]

[0074] in, The expected obstacle avoidance speed V of the UAV can be further calculated u '=S u F+V o .

[0075] Furthermore, in step 5, the specific process is as follows:

[0076] S501. Construct the position error subsystem of each UAV:

[0077]

[0078] Among them, isj=1,2,3, and Represent the position error and velocity error between the UAV and the desired trajectory, and They are the expected position and speed information of the UAV planned in step 3.

[0079] S502. Design of non-singular fixed-time sliding surface with piecewise function:

[0080]

[0081] in, 0<p1<1<q1,k s1 ,k s2 ,k sf ,γ sf >0,ε sf is a positive constant close to 0. 1,isj |≥ε sf , the sig function is used to construct the sliding surface; when |e 1,isj |<ε sf , the atan function is used to construct the sliding surface.

[0082] S503. Design a fixed-time sliding mode reaching law:

[0083]

[0084] Among them, 0<p2<1<q2, k c1 ,k c2 >0.

[0085] S504. Design a non-singular fixed-time sliding mode controller for each position subsystem of each UAV:

[0086]

[0087] Among them, sj=1,2,3 represent the various position subsystems of the UAV.

[0088] Similarly, the non-singular fixed-time sliding mode controller of each attitude subsystem of each UAV can be obtained:

[0089]

[0090] Among them, sj=4,5,6 represent the various attitude subsystems of the UAV. is the attitude angle obtained by inverse solution, for The second derivative of .

[0091] Beneficial effects:

[0092] (1) The present invention provides a path planning and collaborative control algorithm for a multi-UAV system in a dynamic environment. First, a distributed predefined time position observer is constructed to estimate the initial and final target positions of each follower UAV, and an adaptive variable solution space RRT global planner is designed to provide global path planning for each UAV, which enables the UAV to safely cross the obstacle area within a fixed time. Secondly, using the speed obstacle method, a local path planner based on a predefined time filter is developed to enable the UAV to quickly avoid dynamic obstacles with unknown speeds during the tracking of the global path. Finally, using the planning information of each UAV, a fixed-time sliding mode formation controller is designed to realize formation reconstruction. The controller avoids the singularity in the sliding mode surface and the influence of the initial state on the convergence time.

[0093] (2) When determining the solution space, the location and size of the solution space are determined according to the generated random probability value. The advantages of this are: 1. The randomly generated probability value q Avss Determine the adjustment method of the sampling area and dynamically adjust the search strategy to avoid premature convergence or falling into local optimality. 2. When q Avss When a certain threshold is exceeded, the position of the solution space is determined by the target node and the node closest to the target node, so that the sampling area can be more concentrated near the target node; if q AvssIf the threshold is not exceeded, the sampling area will be more extensive to include the start and target nodes, ensuring that possible paths are not missed. BRIEF DESCRIPTION OF THE DRAWINGS

[0094] Figure 1 Schematic diagram of path planning and collaborative control of multi-UAV systems in a dynamic environment.

[0095] Figure 2 This is a flow chart of the design of the path planning and collaborative control algorithm for a multi-UAV system in a complex dynamic environment provided by the present invention. DETAILED DESCRIPTION

[0096] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0097] like Figure 2 As shown, the multi-UAV flight system targeted by the embodiment of the present invention consists of C virtual leaders and N followers, which communicate with each other through a connected topological graph. The specific steps of the multi-UAV system path planning and collaborative control method in a complex dynamic environment provided by the present invention are as follows:

[0098] Step 1: Establish an unmanned aerial system model, including the dynamic model of the virtual leader and the dynamic model of each follower UAV.

[0099] The first-order integrator dynamics model of C virtual leaders in the multi-UAV system constructed by the present invention is as follows:

[0100]

[0101] in, is x 0c The derivative of x 0c and v 0c are the position and velocity of the cth virtual leader, c = 1, 2, ..., C. C is the number of leaders.

[0102] The Euler-Lagrangian dynamics model of the i-th follower UAV is as follows:

[0103]

[0104] in, and Represent the three-dimensional position and posture of the i-th UAV, and p i and Θ i The second derivative of . Among them, m i For drone quality; Among them, J φi ,Jθi ,J ψi Represents the UAV's moment of inertia. and

[0105] They represent the acceleration caused by the resistance during the flight of the UAV and the nonlinear term when the attitude changes, where k i,x ,k i,y ,k i,z ,k i,θ ,k i,φ ,k i,ψ is the air damping coefficient. U pi =[u xi ,u yi ,u zi ] T , where u xi ,u yi ,u zi is the virtual control input of the UAV position subsystem, u xi =(cosφ i sinθ i cosψ i +sinφ i sinψ i ) / u 1i ,u yi =(cosφ i sinθ i sinψ i -sinφ i cosψ i ) / u 1i ,u zi =cosφ i cosθ i u 1i , U Θi =[u 2i ,u 3i ,u 4i ] T .u 1i ,u 2i ,u 3i ,u 4i is the actual control input of UAV i, where u 3i =l i (-f 1,i -f 2,i +f 3,i +f 4,i ),u 4i =l τi (f 1,i -f 2,i +f3,i -f 4,i ), l i and l τi is the scaling factor. 1,i ,f 2,i ,f 3,i ,f 4,i are the lift of each rotor of UAV i respectively. Represents the acceleration due to gravity.

[0106] UAV actual control input u 1i and the desired roll angle φ ri , pitch angle θ ri The solution formula is as follows:

[0107]

[0108] Among them, u xi ,u yi ,u zi is the virtual control input of the UAV position subsystem, u xi =(cosφ i sinθ i cosψ i +sinφ i sinψ i ) / u 1i ,u yi =(cosφ i sinθ i sinψ i -sinφ i cosψ i ) / u 1i ,u zi =cosφ i cosθ i u 1i , ψ ri is the desired yaw angle.

[0109] By introducing virtual control variables, the dynamic model of the i-th UAV is transformed into the following compact nonlinear system form:

[0110]

[0111] Among them, sj=1,2,3,4,5,6 are the subsystems of the UAV. isj =p i,x ,p i,y ,p i,z ,φ i ,θ i ,ψ i and are the position (angle) and velocity (angular velocity) of the sjth subsystem of the i-th UAV.

[0112] is a nonlinear term, b isj =1 / m i ,1 / m i ,1 / m i ,1 / J φi ,1 / J θi ,1 / J ψi ,u isj =u xi ,u yi ,u zi ,u 2i ,u 3i ,u 4i .

[0113] Step 2: Construct a predefined time distributed position observer and use the distributed observer to estimate the initial formation target position and the final formation target position of each follower UAV. The convergence time of the distributed position observer is set to a predefined time T that is independent of the distributed position observer parameters. o .

[0114]

[0115] Among them, sig(·) α =|·| α sign(·), sign(·) is the sign function. is the expected position state of the i-th UAV, for The derivative of x 0c is the position status of the cth virtual leader. and is the gain coefficient, m∈(0,1), λ min (·) represents the minimum eigenvalue of the matrix, H is the information transfer matrix, T o >0 is the preset convergence time. Construct the Lyapunov function of the observer and take its derivative, and we can get It satisfies the predefined time lemma form: Therefore, the constructed observer is able to converge within a predefined time. is the positive gain constant. ij =l ic -l jc , l ic and l jc are the formation vectors of the i-th and j-th UAVs relative to the position of the c-th virtual leader, respectively. By designing a suitable formation vector lij And the information transfer matrix H can realize the grouping of formations. ij is the adjacency weight between two followers, b ic is the connection weight between the virtual leader and the follower.

[0116] Step 3: Construct a global path planner based on the adaptive variable solution space RRT algorithm, and use the initial formation target position and final formation target position estimated by the observer to plan a safe global path for each UAV when it passes through dense obstacle areas (such as dense forest areas).

[0117] The characteristics of this step are: when the adaptive variable solution space RRT algorithm determines the random spanning tree expansion step size, when the angle of the sampling node deviating from the target node is less than the set value, the random spanning tree expansion step size is increased according to the deviation angle.

[0118] When generating sampling nodes of a random spanning tree, the solution space selected by the sampling nodes adopts a variable solution space, and the position and size of the solution space are determined according to the positional relationship between the nearest node of the target node and the target node or the starting node.

[0119] In the embodiment of the present invention, the specific design method of the global path planner is as follows:

[0120] S301. Initialize the map and use the initial and final formation target positions estimated by the distributed position observer as the starting node sp for global planning st and target node sp g .

[0121] S302. Select RRT algorithm sampling points using the target point bias mechanism and the adaptive variable solution space strategy;

[0122] Among them, in order to improve the sampling probability of the target area and enhance the target directivity of the algorithm, the sampling point generation strategy after the target point bias mechanism is optimized is: set the target point bias threshold p target , determine the probability value p of the randomly generated sampling node rand Is it greater than or equal to p target ; If yes, select the target node sp g As the sampling point sp rand , otherwise, select a randomly generated node sp in the sampling space sample As the sampling point sp rand . Its expression is:

[0123]

[0124] To address the problem of over-dispersion and a large number of invalid sampling points caused by traditional RRT algorithms sampling the entire space, which significantly increases the computational load, an adaptive approach is introduced to transform the original fixed sampling space into a variable solution space that is dynamically adjusted as the sampling progresses. This narrows the necessary exploration space, thereby reducing search complexity and improving the algorithm's search efficiency.

[0125] Design of a new cylindrical adaptive variable solution space area X Avss =(x Avss ,y Avss ,z Avss ) can be determined from the center position, diameter and height of the cylinder as:

[0126]

[0127] Among them, (x cc ,y cc ) is the center position of the upper and lower bottom surfaces of the cylinder’s adaptive variable solution space, r Avss and h Avss are the diameter and height of the upper and lower bottom surfaces of the adaptive variable solution space respectively.

[0128] The position of the solution space, that is, (x cc ,y cc ) is determined by judging the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre , then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The midpoint of the position is (x cc ,y cc ); if p Avss ≤p thre , then the starting node sp st With the target node sp g The midpoint of the position is (x cc ,y cc ). The expression is as follows:

[0129]

[0130] In the above formula, sp gnx 、sp gny For node sp gn Position coordinates of sp gx , sp gy The target node sp g Position coordinates of sp stx , sp styis the starting node sp st The location coordinates of .

[0131] r Avss The determination method is: judge the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre , then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss ; if p Avss ≤p thre , then the starting node sp st With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss ; The expression is as follows:

[0132]

[0133] h Avss The method of determining is: the distance from the lowest point to the highest point in the sampling space, the expression is as follows:

[0134] h Avss =|z high -z low |;

[0135] Among them, z low and z high are the z coordinate values ​​of the lowest and highest points in the sampling space, respectively, p Avss is a randomly generated probability value.

[0136] S303. Using the target point attraction and adaptive step size strategy to perform random tree expansion on the RRT algorithm;

[0137] In order to improve the search efficiency of the RRT algorithm, the target gravity mechanism in the potential field method is used to guide the random tree to expand towards the target node more purposefully. Considering the target node sp g For the new node sp new The new node generation method of the expansion direction is:

[0138]

[0139] Among them, λ a is the attraction coefficient of the target point, and Δs is the random number expansion step.

[0140] After determining the expansion direction, in order to improve the random tree expansion speed, the random tree expansion step length is changed to an adaptive length. Calculate the current generated sampling node sp rand , the nearest node sp of the randomly sampled node nst and target node sp g The interior angle of the triangle formed Right now then Indicates that the random tree is moving away from the target node sp g In this case, a shorter step length Δs is used to reduce the degree of deviation; on the contrary, when When , it indicates that the expansion direction is facing the target node sp g , a longer step size is used This allows the random tree to expand toward the target point more quickly. The formula is:

[0141]

[0142] S304. Path clipping and time redistribution are performed on the initial global path generated by the RRT algorithm, so that the UAV cluster can pass through the dense obstacle area within a time of t total , each time period is set as Δt, the initial global path after cutting is divided into t total / Δt path points, and perform B-spline trajectory optimization on the above processed curve to obtain the expected global safety trajectory position corresponding to each UAV speed and acceleration information.

[0143] Step 4: Construct a local path planner based on a predefined time prediction filter to avoid potential collision risks between the UAV and dynamic obstacles during formation tracking. The prediction filter is used to estimate the obstacle speed. Combining the UAV speed and the obstacle speed estimated by the prediction filter, the speed obstacle method is used to perform local path planning to avoid potential collision risks between the UAV and dynamic obstacles during formation tracking. The prediction filter convergence time is set to a predefined time T that is independent of the prediction filter parameters. f .

[0144] In the embodiment of the present invention, the specific design method of the local path planner is as follows:

[0145] S401. When an obstacle enters the detection range, a predefined time prediction filter is designed to estimate the speed of the obstacle using the detected obstacle position information. The predefined time filter is designed as follows:

[0146]

[0147] Among them, Tf is a predefined time that is independent of the filter parameters; x obp is the filter input, i.e., the actual position of the obstacle measured by the sensor, which may contain measurement noise; is the filter output, i.e. the estimated position after filtering, yes The derivative of The approximate value of m∈(0,1), sig(·) α =|·| α sign(·), k f is the positive gain constant.

[0148] S402. Calculate the dynamic safety boundary of the i-th UAV relative to the obstacle:

[0149]

[0150] Among them, k sa ≥3 is the dynamic boundary coefficient, R ob =r im +r ob +R sa is the optimized safety distance of the i-th UAV to obstacles, r im is the influence radius of the UAV, r ob is the dynamic obstacle radiation radius, R sa Expand the safety distance for drones. The velocity of a dynamic obstacle estimated by a predefined time filter.

[0151] S403. Using the speed of the drone V u and the estimated velocity V of the dynamic obstacle o , dynamic security boundary R ob , establish a collision critical point prediction model.

[0152] In this example, the specific content of the collision critical point prediction model includes the following three parts:

[0153] P1. Use the following formula to determine whether the drone should avoid obstacles:

[0154]

[0155] Among them, V uo is the relative speed between the UAV and the obstacle, which can be represented by the vector V o -V u Get; S u S o is the position vector between the UAV and the unexpected dynamic obstacle. ob is the relative velocity V uoThe angle between the cone line and the control obstacle, α ob0 is 1 / 2 of the half-apex angle of the speed barrier cone. ob <α ob0 When α is negative, the obstacle threatens the UAV and the UAV avoids the obstacle. ob ≥α ob0 The drone does not perform obstacle avoidance.

[0156] P2. Define F1(x1, y1, z1) and F2(x2, y2, z2) as the critical collision points. The two are determined in the same way and can both be determined by the following formula (taking F1 as an example):

[0157]

[0158] P3. Under the premise of satisfying the quadrotor motion performance constraints, select the collision critical point with the smallest velocity direction change between F1 and F2 as the direction guidance point F for the drone to avoid obstacles, that is:

[0159]

[0160] in,

[0161] The expected obstacle avoidance speed V of the UAV can be further calculated u '=S u F+V o , the V u 'Used when encountering dynamic obstacles.

[0162] Step 5: Design a position and attitude tracking controller for the multi-UAV system to achieve fixed-time formation tracking control and attitude synchronization of the UAV cluster.

[0163] In the example of the present invention, the specific design of the fixed-time formation tracking controller is as follows:

[0164] S501. Construct the position error subsystem of each UAV:

[0165]

[0166] Among them, isj=1,2,3 represent p in step 1 respectively. i,x ,p i,y ,p i,z Position in three dimensions, and Represent the position error and velocity error between the UAV and the desired trajectory, and They are the expected position and speed information of the UAV planned in step 3.

[0167] S502. Design of non-singular fixed-time sliding surface with piecewise function:

[0168]

[0169] in, 0<p1<1<q1,k s1 ,k s2 ,k sf ,γ sf >0,ε sf is a positive constant close to 0. 1,isj is the position error between the UAV and the expected trajectory. 1,isj Larger, at this time |e 1,isj |≥ε sf , then the sig function is used to construct the sliding surface. When e 1,isj Small, at this time |e 1,isj |<ε sf , the atan function is used to construct the sliding surface.

[0170] The convergence time of the non-singular fixed-time sliding surface constructed here is fixed. This is because: by constructing the Lyapunov function And taking its derivative, we can get in, It satisfies the fixed-time lemma of the form Therefore, its fixed convergence time is

[0171] S503. Design a fixed-time sliding mode reaching law:

[0172]

[0173] Among them, 0<p2<1<q2, k c1 ,k c2 >0.

[0174] The convergence time of the fixed-time sliding mode reaching law constructed here is fixed. This is because: by constructing the Lyapunov function And taking its derivative, we can get in, It satisfies the fixed-time lemma of the form Therefore, its fixed convergence time is

[0175] S504. Design a non-singular fixed-time sliding mode controller for each position subsystem of each UAV:

[0176]

[0177] Where sj=1, 2, and 3 represent the various position subsystems of the UAV respectively.p,1 =b p,2 =b p,3 =1 / m i ,

[0178] Similarly, the non-singular fixed-time sliding mode controller of each attitude subsystem of each UAV can be obtained:

[0179]

[0180] Among them, sj=4,5,6 represent the attitude subsystems of the UAV respectively, b p,4 =1 / J φi ,b p,5 =1 / J θi ,b p,6 =1 / J ψi , is the attitude angle obtained by inverse solution, for The second derivative of .

[0181] The above specific embodiments merely illustrate the design principles of the present invention. The shapes and names of the components described herein may vary and are not limiting. Therefore, those skilled in the art may modify or substitute equivalents for the technical solutions described in the above embodiments. Such modifications and substitutions, without departing from the inventive spirit and technical solutions of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A path planning and collaborative control method for a heterogeneous unmanned swarm system in a dynamic environment, characterized by: The multi-UAV system consists of C virtual leaders and N followers. The multi-UAV system communicates internally through a connected topology graph. The integrated path planning and coordinated control of the multi-UAV system are performed according to the following steps, including: Step 1: Construct a distributed position observer with a predefined time. The observer convergence time is set to a predefined time T that is independent of the distributed position observer parameters. o , using distributed position observers to estimate the initial formation target position and final formation target position of each follower UAV; Step 2: Build a global path planner based on the adaptive variable solution space RRT algorithm. Use the initial and final formation target positions estimated by the distributed position observer to plan a safe global path for each UAV to traverse the dense obstacle area. Among them, when the RRT algorithm generates the sampling nodes of the random spanning tree, the solution space selected by the sampling nodes adopts a variable solution space. The position and size of the solution space are determined according to the positional relationship between the nearest node of the target node and the target node or the starting node. When determining the expansion step of the random spanning tree, when the angle of the sampling node deviating from the target node is less than the set value, the expansion step of the random spanning tree is increased according to the deviation angle. Step 3: Construct a predefined time prediction filter to estimate the obstacle speed. Combine the UAV speed and the estimated obstacle speed and use the speed obstacle method to perform local path planning to avoid the potential collision risk between the UAV and the dynamic obstacle during the formation tracking process. The prediction filter convergence time is set to a predefined time T that is independent of the prediction filter parameters. f ; Step 4: Based on the safe global path and local path planning results, the sliding mode control method is used to control the motion of the UAVs and realize the fixed-time formation tracking control of the UAV cluster.

2. The path planning and collaborative control method for a heterogeneous unmanned cluster system in a dynamic environment according to claim 1, characterized in that: In step 2, the adaptive variable solution space RRT algorithm incorporates the attraction of the target node to the new node when determining the expansion direction of the random spanning tree; When generating sampling nodes of the random spanning tree, the target node or a random sampling point in the solution space is selected as the sampling node according to the relationship between the generated random probability value and the preset threshold.

3. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment according to claim 1, characterized in that: In step 2, the position and size of the solution space are determined according to the generated random probability value: The position of the solution space is determined by averaging the coordinates of the nearest node to the target node or the starting node and the target node based on the relationship between the generated random probability value and the preset threshold. The size of the solution space is determined by: determining the size of the solution space based on the relationship between the generated random probability and the preset threshold; Among them, the selection of the nearest node or the starting node of the target node is determined according to the generated random probability. When the generated random probability is greater than the preset threshold, the nearest node of the target node participates in the determination of the position and size of the solution space; otherwise, the starting node participates in the determination of the position and size of the solution space.

4. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment according to claim 1, characterized in that: The second step further includes: using the global path generated by the RRT algorithm as the initial global path, performing path clipping and time redistribution, and making the UAV cluster pass through the dense obstacle area for a time of t total , each time period is set as Δt, the initial global path after cutting is divided into t total / Δt path points are processed, and B-spline trajectory optimization is performed on the processed path curve to obtain the trajectory position, velocity and acceleration information of the global safe path corresponding to each UAV.

5. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment according to claim 1, characterized in that: The fourth step includes: A fixed-time sliding mode surface containing piecewise functions is designed. Using the designed fixed-time sliding mode reaching law, a non-singular fixed-time sliding mode controller for each position subsystem and attitude subsystem of each UAV is obtained to realize fixed-time formation tracking control of the UAV cluster. The fixed-time sliding mode reaching law is: the time when the error reaches the sliding mode surface is selected as a fixed time; The fixed-time sliding surface containing a piecewise function is designed as follows: a neighborhood range is designed at the 0 position of the sliding surface. When the error enters the set neighborhood range, a continuous smooth function is used instead of the sign function as the sliding surface of the small neighborhood. Moreover, the time it takes for the error to slide to 0 on the sliding surface after reaching the sliding surface is fixed.

6. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment according to claim 1, characterized in that: In step 1, the distributed position observer for the predefined time is constructed as: Among them, sig(·) α =|·| α sign(·), is the expected position state of the i-th UAV, for The derivative of x 0c is the position status of the cth virtual leader; and is the gain coefficient, m∈(0,1), λ min (·) represents the minimum eigenvalue of the matrix, T o >0 is the preset convergence time; is the positive gain constant; l ij =l ic -l jc , l ic and l jc are the formation vectors of the i-th and j-th UAVs relative to the position of the c-th virtual leader, respectively. By designing the formation vector l ij And the information transfer matrix H realizes the grouping of the formation; w ij is the adjacency weight between two followers, b ic b is the connection weight between the virtual leader and the follower ic .

7. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment as claimed in claim 2, characterized in that: The second step includes: Step S301. Initialize the map and use the initial formation target position and final formation target position of each UAV estimated by the distributed position observer as the starting node sp of the global planning st and target node sp g ; Step S302. Select RRT algorithm sampling points using the target point bias mechanism and the adaptive variable solution space strategy; Among them, the sampling point generation strategy based on the target point bias mechanism is: setting the target point bias threshold p target , determine the probability value p of the randomly generated sampling node rand Is it greater than or equal to p target ; If yes, select the target node sp g As the current generated sampling point sp rand , otherwise, select a randomly generated node sp in the sampling space sample As the current generated sampling point sp rand ; Designed cylindrical adaptive variable solution space area X Avss =(x Avss ,y Avss ,z Avss ) is determined by the center position, diameter and height of the cylinder as: Among them, (x cc ,y cc ) is the center position of the upper and lower bottom surfaces of the cylinder in the adaptive variable solution space; r Avss and h Avss are the diameter and height of the upper and lower bottom surfaces of the adaptive variable solution space respectively; (x cc ,y cc ) is determined by judging the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre , then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The midpoint of the position is (x cc ,y cc ); if p Avss ≤p thre , then the starting node sp st With the target node sp g The midpoint of the position is (x cc ,y cc ); r Avss The determination method is: judge the randomly generated probability value p Avss With the pre-set threshold p thre If p Avss >p thre , then the node sp closest to the target node in the random generated tree is used gn With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss ; if p Avss ≤p thre , then the starting node sp st With the target node sp g The distance plus the space margin r thre As the diameter r of the adaptive variable solution space Avss ; h Avss The method of determining h is: Avss =|z high -z low |; Among them, z low and z high are the z coordinate values ​​of the lowest and highest points in the sampling space respectively; Step S303. Using the target point attraction and adaptive step size strategy to perform random tree expansion on the RRT algorithm to generate a safe global path; Among them, the expansion direction of the new node is determined by the attraction of the target node, thereby generating a new node sp new for: Among them, sp nst is the nearest node of the randomly sampled node, sp rand is the current generation sampling point determined in step S302, λ a is the attraction coefficient of the target node, Δs is the random tree expansion step; After determining the expansion direction, calculate the current generated sampling point sp rand , the nearest node sp of the randomly sampled node nst and target node sp g The internal angle θ of the triangle formed; when θ>90°, it indicates that the random tree is moving away from the target node sp g When θ≤90°, it indicates that the expansion direction is facing the target node sp. g , a relatively long adaptive step size of Δs(1+cosθ) is used to accelerate the expansion of the random tree to the target node.

8. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment as claimed in claim 1, characterized in that: The prediction filter based on predefined time constructed in step 3 is: Among them, T f is a predefined time that is independent of the filter parameters; x obp is the prediction filter input, i.e. the actual position of the obstacle measured by the sensor; is the prediction filter output, i.e. the estimated position after filtering, yes The derivative of is the obstacle speed The approximate value of m∈(0,1), sig(·) α =|·| α sign(·);k f is the positive gain constant.

9. The path planning and collaborative control method for heterogeneous unmanned cluster systems in a dynamic environment as claimed in claim 5, characterized in that: Fixed-time sliding surface s containing piecewise functions isj for: in, Where, e 1,isj is the position error between the UAV and the desired trajectory, e 2,isj is the velocity error between the UAV and the desired trajectory, 0<p1<1<q1, k s1 ,k s2 ,k sf ,γ sf >0,ε sf is a positive constant close to 0; when|e 1,isj |≥ε sf The sig function is used to construct the sliding surface; when |e 1,isj |<ε sf , then the atan function is used to construct the sliding surface; The fixed-time sliding mode reaching law for: Among them, 0<p2<1<q2, k c1 ,k c2 >0.

Images