Path planning and cooperative control method of heterogeneous unmanned cluster system in dynamic environment

Abstract

The invention discloses a path planning and cooperative control method for a heterogeneous unmanned cluster system in a dynamic environment. The method comprises the following steps: firstly, constructing a distributed position observer of predefined time, and estimating initial and final formation target positions of each follower unmanned aerial vehicle by utilizing local information interaction between the unmanned aerial vehicles; constructing a global path planner based on an adaptive variable solution space RRT algorithm, and planning a safe global path when each unmanned aerial vehicle passes through the dense obstacle area; a local path planner based on a predefined time prediction filter is constructed, and the potential collision risk between the unmanned aerial vehicle and a dynamic obstacle in formation tracking is avoided; and finally, performing motion control on the unmanned aerial vehicle by adopting a sliding mode control method according to the safe global path and the local path planning result, thereby realizing fixed-time formation tracking control of the unmanned aerial vehicle cluster. By using the method, the multi-unmanned aerial vehicle system can realize safe flight in a complex dynamic environment, and the control performance when a formation tracking flight task is realized is ensured.

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, with the characteristics of high efficiency, flexibility and strong stability, multi-unmanned aerial 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 in recent decades. These works specifically involve global and local path planning to safely avoid obstacles, as well as the design of distributed formation control protocols for achieving different control objectives. Among them, how to simultaneously consider the global path optimization and local dynamic obstacle avoidance of multiple UAVs in complex environments and achieve coordinated formation tracking of the entire system within a fixed time has always been an urgent problem to be solved in the field of precise navigation and collaborative control of swarm unmanned systems.

[0003] In terms of global path planning for multi-UAV swarms. The global path planning of swarm unmanned systems aims to effectively plan a safe path for each UAV in the swarm globally in a known environment. However, most of the existing path planning schemes focus on the path planning of a single UAV. If they are directly applied to multi-UAV systems, they have a slow convergence speed and search efficiency. At the same time, the existing global path planning algorithms only consider sparse and 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 obstacles and dynamic obstacles to ensure the safe flight of multi-UAV systems is currently a hot and difficult issue.

[0005] In terms of formation control of multi-UAV swarms, the core of encirclement control is to design a controller so that the positions of each follower UAV converge to the convex hull formed by the positions of multiple leader UAVs and keep the states consistent. However, as the complexity of multi-UAV system flight missions gradually increases, such as multi-point detection and reconnaissance, regional exploration and mapping, distributed target tracking, and multi-location material distribution and rescue, single formation encirclement control is difficult 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] In addition, in the design of most existing collaborative control solutions, the UAV model is mostly a simplified linear position model, while ignoring the attitude model containing the rotation angle, which weakens the flight control maneuverability of the UAV. At the same time, the existing controller design technology only considers the steady-state performance of the system but not the transient performance of the system, and cannot guarantee the rapidity of the 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 swarm 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, some studies have introduced finite-time and fixed-time lemmas and adopted sliding mode control methods 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 object, the technical solution of the present invention is: a multi-UAV system is composed of C virtual leaders and N followers, and the multi-UAV system communicates internally through a connected topological graph, and performs comprehensive path planning and distributed formation collaborative control on the multi-UAV system 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, and set the observer convergence time 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 to cross 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, and the position and size of the solution space are determined according to the position 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 in formation tracking. In this step, a prediction filter based on a predefined time is constructed to estimate the speed of the obstacle; 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 in 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: According to 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: 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 of 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 a 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, For x 0c The derivative of x 0c and v 0care the position and speed of the cth virtual leader respectively, c=1,2,…,C, 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 Respectively represent the position and attitude of the i-th UAV, and They are 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. 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. Among them: 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 quantities, 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 drone. 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 ith 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 constructed predefined time distributed position observer 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. Constructing the Lyapunov function of the observer and taking its derivative, 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 the formation. 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 adds 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 according to 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: according to the relationship between the generated random probability value and the preset threshold, the coordinate values ​​of the nearest node of the target node or the starting node and the target node are averaged;

[0040] The size of the solution space is determined by: determining the size of the solution space according to 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, and the initial and final formation target positions estimated by the observer are used to plan a safe global path for each UAV when it passes through 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 the target node sp g .

[0044] S302. Selecting 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] In order to solve the problem of excessive dispersion of sampling points and a large number of invalid sampling points caused by sampling in the entire space of the traditional RRT algorithm, which significantly increases the computational load, the adaptive idea is introduced to transform the original fixed sampling space into a variable solution space that is dynamically adjusted with the sampling process, so as to narrow the necessary exploration space, thereby reducing the search complexity and improving the search efficiency of the algorithm.

[0047] Design of cylindrical new 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 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;

[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 is used 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 is used 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 midpoint of the position is (x cc ,y cc );

[0052] h Avss The determination method is: h 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] 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 to 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 generated 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 increase 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 the target node sp g The interior angle of the triangle Right now then This 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 extension direction is facing the target point sp g , a longer step length is used This allows the random tree to expand toward 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 drone cluster to cross the dense obstacle area is fixed to 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 the potential collision risk of the UAV with 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, wherein the predefined time filter is designed as:

[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 is 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 ith UAV to obstacles, r im is the influence radius of the drone, 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 safety boundary R ob , establish a collision critical point prediction model, the specific content includes the following three parts:

[0067] P1. Determine whether the drone should avoid obstacles by the following formula:

[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 speed 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 α , the obstacle poses a collision threat to the drone, and the drone avoids the obstacle; on the contrary, when α ob ≥α ob0 The drone does not avoid obstacles.

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

[0071]

[0072] P3. Under the premise of satisfying the UAV motion performance constraints, select the collision critical point with the smallest speed direction change in F1 and F2 as the direction guidance point F for the UAV to avoid obstacles, that is:

[0073]

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

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

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

[0077]

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

[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 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 drone. 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, so that the UAV can 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 singular phenomena 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 the local optimum. 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 no possible paths are 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 path planning and collaborative control algorithm design 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 is composed of C virtual leaders and N followers, which communicate with each other through a connected topological graph. The path planning and collaborative control method of the multi-UAV system in a complex dynamic environment provided by the present invention has the following specific steps:

[0098] Step 1: Establish a UAV 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, For x 0c The derivative of x 0c and v 0c are the position and speed 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 Respectively represent the 3D position and attitude of the i-th UAV, and They are 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. 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 the virtual control quantity, 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 drone. 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 ith 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 parameters of the distributed position observer. 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. Constructing the Lyapunov function of the observer and taking its derivative, 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 the formation. 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 crosses dense obstacle areas (such as dense forest areas).

[0117] The characteristic of this step is that 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 example 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 the target node sp g .

[0121] S302. Selecting 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] In order to solve the problem of excessive dispersion of sampling points and a large number of invalid sampling points caused by sampling in the entire space of the traditional RRT algorithm, which significantly increases the computational load, the adaptive idea is introduced to transform the original fixed sampling space into a variable solution space that is dynamically adjusted with the sampling process, so as to narrow the necessary exploration space, thereby reducing the search complexity and improving the search efficiency of the algorithm.

[0125] Design of cylindrical new 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 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.

[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 is used 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 The position coordinates of sp gx , sp gy The target node sp g The 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 is used 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, 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 to the target node more purposefully. Considering the target node sp g For the new node sp new The new node generation method attracted by 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 increase 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 the target node sp g The interior angle of the triangle Right now then This 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 length 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 clipping 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 the potential collision risk of the UAV with dynamic obstacles in 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 the potential collision risk of the UAV with dynamic obstacles in 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 example 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, wherein the predefined time filter is designed as:

[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 is 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 ith UAV to obstacles, r im is the influence radius of the drone, 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 safety 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. Determine whether the drone should avoid obstacles by the following formula:

[0154]

[0155] Among them, V uo is the relative speed between the drone and the obstacle, which can be expressed 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 speed 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 α , the obstacle poses a collision threat to the drone, and the drone avoids the obstacle; on the contrary, when α ob ≥α ob0 The drone does not avoid obstacles.

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

[0157]

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

[0159]

[0160] in,

[0161] The desired 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 the position and attitude tracking controller of the multi-UAV system to realize 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 drone:

[0165]

[0166] Among them, isj=1,2,3 represent the 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 drone and the desired trajectory, and They are the expected position and speed information of the UAV planned in step 3 respectively.

[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 Smaller, 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 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. 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] Among them, sj=1,2,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 only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. A path planning and collaborative control method for a heterogeneous unmanned cluster system in a dynamic environment, characterized in that: The multi-UAV system consists of C virtual leaders and N followers. The multi-UAV system communicates internally through a connected topology graph. The multi-UAV system performs comprehensive path planning and collaborative control 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 a distributed position observer to estimate the initial formation target position and the final formation target position of each follower UAV; Step 2: Construct a global path planner based on the adaptive variable solution space RRT algorithm, and use the initial formation target position and the final formation target position estimated by the distributed position observer to plan a safe global path for each UAV to cross 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, and the position and size of the solution space are determined according to the position 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 coordinated control according to claim 1, characterized in that: In step 2, the adaptive variable solution space RRT algorithm adds 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 coordinated control 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: according to the relationship between the generated random probability value and the preset threshold, the coordinate values ​​of the nearest node of the target node or the starting node and the target node are averaged; The size of the solution space is determined by: determining the size of the solution space according to 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 coordinated control according to claim 1, characterized in that: The step 2 further includes: taking 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 in a time of t total , each time period is set as Δt, the initial global path after clipping is divided into t total / Δt path points are processed, and the B-spline trajectory optimization is performed on the processed path curve to obtain the trajectory position, velocity and acceleration information of the global safety path corresponding to each UAV.

5. The path planning and coordinated control according to claim 1, characterized in that: The fourth step comprises: 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 a fixed time; Among them, 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; and a fixed time is selected as the time for the error to slide to 0 on the sliding surface after reaching the sliding surface.

6. The path planning and coordinated control 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 / (λ min (H)mT o ) 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. 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 is the connection weight b between the virtual leader and the follower ic .

7. The path planning and coordinated control according to claim 2, characterized in that: The second step comprises: 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 the 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: 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 is used 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 is used 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 determination method is: h 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 generated 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 the 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 coordinated control according to claim 1, characterized in that: The prediction filter based on the predefined time constructed in step 3 is: Among them, T f is a predefined time 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 coordinated control according to claim 2, characterized in that: In step 5, the fixed time sliding surface s containing the piecewise function isj for: in, In the formula, e 1,isj is the position error between the UAV and the expected trajectory, e 2,isj is the speed error between the drone 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