Distributed space search and trajectory planning method and device for multi-UAV collaboration

Abstract

The present invention relates to a distributed space search and trajectory planning method and device for multi-UAV collaboration, the method comprising: sharing maps according to scene depth maps of respective locations output by multiple UAVs, generating a shared probability occupancy map; performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary set according to map information in the shared probability occupancy map; performing operations optimization allocation according to the number of observation points and the number of UAVs in the boundary optimal observation point set, as well as the cost from the current position of the UAV to the observation point, and allocating each UAV to the next target point; performing trajectory planning according to the target points allocated to each UAV, obtaining a continuous expected trajectory that considers obstacle avoidance and collision avoidance between UAVs, and outputting it to the flight controller of each UAV for flight control. The present invention improves the perception capability of the UAV system to the consistency of the environment, and effectively avoids the interference of invalid perception between machines.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) intelligent control, and in particular relates to a distributed space search and trajectory planning method and device for coordinated multi-UAV operation. Background Art

[0002] The main method of existing multi-UAV search is the formation method, which uses multiple UAVs to form a certain shape of formation to expand the perception range of the UAVs, so that the entire formation can quickly complete the search task of the area. However, this method is not suitable for collaborative search in complex spaces, because there are often many obstacles in complex spaces. In this case, maintaining the formation and avoiding obstacles are contradictory requirements. To meet both requirements at the same time, it is necessary to find reasonable parameter settings, which requires a long period of debugging. At the same time, the advantage of the formation is to expand the perception range. Due to the various structures of complex spaces, the expansion of the perception range is limited, and the advantages of the formation cannot be fully utilized, which may cause the method to run slowly in this environment and fail to meet the needs of rapid collaborative traversal of space. Summary of the invention

[0003] In view of the above analysis, the present invention aims to disclose a distributed space search and trajectory planning method and device for multi-UAV collaboration, which is used to ensure the collaborative information perception of multiple UAVs during complex space search, improve environmental consistency judgment, and prevent information conflicts.

[0004] The present invention discloses a distributed space search and trajectory planning method for multi-UAV collaboration, comprising:

[0005] Step S1: Share the scene depth maps of the respective locations output by multiple drones to generate a shared probability occupancy map;

[0006] Step S2: performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary set according to the map information in the shared probability occupied map;

[0007] Step S3: perform operations optimization allocation according to the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point, and allocate each drone to the next target point;

[0008] Step S4: perform trajectory planning according to the target points assigned to each UAV, obtain a continuous expected trajectory that takes into account obstacle avoidance and collision avoidance between UAVs, and output it to the flight controller of each UAV for flight control.

[0009] Furthermore, the step S1 includes:

[0010] Step S101, performing a multi-drone mutual concealment determination to determine whether there is a concealed drone;

[0011] Step S102: performing a mask operation on the depth information captured by the obscured UAV to obtain a processed depth map;

[0012] Step S103: converting the depth map into an obstacle point cloud based on the processed depth information, and generating a shared probability occupancy map according to the obstacle point clouds simultaneously observed by all drones;

[0013] The shared probability occupancy map is composed of a dynamic array, in which each data unit stores the observation probability obtained by multiple drones and the state corresponding to the map element determined by the observation probability; and the observation probability and observation state are updated according to the obstacle point cloud updated frame by frame.

[0014] Furthermore, the determination of the presence of a concealed drone includes:

[0015] 1) One-time determination: when obtaining the updated depth map and global position of each drone, determine the distance between any two drones. If the distance between the two drones is greater than the depth camera perception distance, it is determined that there is no mutual perception between the two drones; otherwise, proceed to the next step;

[0016] 2) Secondary determination: Determine whether one of the UAVs is within the field of view of the other UAV based on the relative position vector of the two UAVs and the viewing angle vector of the onboard depth camera;

[0017] The determination formula is: Result = minf (sign (dir ij dir r )),in, sign is a mathematical symbol function; dir ij UAV i and UAVs j The relative position vector of dir r For UAV j The viewing angle vector of

[0018] When Result is 0, it means UAV i Not in UAV j Otherwise, it appears in the field of view and affects the estimation of the depth camera.

[0019] Furthermore, in step S2, it includes:

[0020] Step S201, searching the current state of each data unit in the shared probability occupied map to find a boundary set for distinguishing unknown and known areas of the map;

[0021] The data units at the junction of the unknown state and the free state in the shared probability occupancy map belong to the points on the boundary. Their own state is the free state, and the state of their neighboring units is the unknown state;

[0022] Step S202: Search for the position of the observation point that maximizes the observation efficiency of each boundary in the boundary set;

[0023] During the search process, the best observation point position is determined by searching for the mean point of the boundary, the positions of the two spatially farthest points at both ends of the boundary, the central normal of the boundary, and the center point position calculated from the positions of the two spatially farthest points at both ends of the boundary and the perception field of view FOV of the depth camera.

[0024] Furthermore, in step S3, the M observation points VP m ∈{VP1,…,VP M} are assigned to N drones UAV n ∈{UAV1,…,UAV N}, one drone is assigned only one observation point, and after the assignment, all drones reach their respective observation points at the minimum cost; the following assignment optimization mathematical model is established:

[0025] Whether the drone UAV n participates in the search is represented as y n ∈{0,1}; whether the observation point VP m is assigned to the drone UAV n is represented as x mn ∈{0,1}; where the cost for the drone UAV n to reach the observation point VP m is represented as d mn , and the total cost for all drones to reach their respective observation points is represented as The total cost Cost is minimized through assignment optimization.

[0026] Furthermore, assignment optimization is performed separately according to the two cases where the number of observation points is less than the number of drones and the number of observation points is greater than the number of drones, so that the total cost is minimized;

[0027] Case 1: When M < N, the assignment optimization problem is formulated as:

[0028]

[0029]

[0030]

[0031]

[0032]

[0033]

[0034] Case 2: When M>N, the allocation optimization problem can be expressed as:

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041] Furthermore, the cost of the drone to the observation point is the total observation cost consisting of distance cost, speed cost and speed change cost.

[0042] Furthermore, in step S4, it specifically includes:

[0043] Step S401, first path planning: performing path planning according to the obstacle avoidance perception map of each UAV to generate a continuous expected trajectory for each simulated UAV;

[0044] Step S402, second path planning; judging the collision between drones according to the continuous expected trajectory of each robot in the same time period; for the two simulated drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of drones.

[0045] Furthermore, step S402 includes:

[0046] 1) The trajectory curves in the same time period are transmitted between multiple UAVs, and the time period is downsampled to obtain multiple sampling moments;

[0047] 2) For the continuous expected trajectory of all UAVs, the sampling position points of each UAV are calculated at each sampling time;

[0048] 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3;

[0049] 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two preceding and following Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the line between the two Bezier curve control points is extended in a direction away from the other party's expected trajectory, so that the distance between the position points on the line at the same time exceeds the safe distance.

[0050] The present invention also discloses a multi-UAV coordinated distributed space search and trajectory planning device, comprising:

[0051] A shared map generation module is used to share maps based on scene depth maps of the respective locations output by multiple drones, and generate a shared probability occupancy map;

[0052] A boundary search module, for performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary combination according to the map information in the shared probability occupied map;

[0053] The target point allocation module is used to allocate the next target point to each drone based on the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point;

[0054] The trajectory planning module is used to plan the trajectory according to the target points assigned to each UAV, obtain the continuous expected trajectory that takes into account obstacle avoidance and avoidance of collision between UAVs, and output it to the flight controller of each UAV for flight control.

[0055] The present invention can achieve one of the following beneficial effects:

[0056] The present invention solves the problem of independent mutual perception of distributed cluster systems, improves the system's ability to perceive environmental consistency, and effectively avoids interference from invalid perception between machines. In addition, a method of operations optimization is used to allocate tasks for distributed clusters, solving the problem of lack of planning in distributed cluster operations, and minimizing the time and energy consumption of the entire system tasks on a global scale. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0058] Figure 1 This is a flow chart of the distributed space search and trajectory planning method for multi-UAV collaboration in the first embodiment of the invention;

[0059] Figure 2 This is a flow chart of generating a shared probability occupancy map in Embodiment 1 of the invention;

[0060] Figure 3 This is a flow chart of the method for searching boundaries and optimal boundary viewpoints in the first embodiment of the invention;

[0061] Figure 4 This is a flow chart of the trajectory planning method in the first embodiment of the invention;

[0062] Figure 5 This is a principle block diagram of the distributed space search and trajectory planning device for multi-UAV collaboration in the second embodiment of the invention. DETAILED DESCRIPTION

[0063] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used to illustrate the principles of the present invention together with the embodiments of the present invention.

[0064] Embodiment 1

[0065] One embodiment of the present invention discloses a distributed space search and trajectory planning method for multiple UAVs, such as Figure 1 As shown, including:

[0066] Step S1: Share the scene depth maps of the respective locations output by multiple drones to generate a shared probability occupancy map;

[0067] Step S2: performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary according to the map information in the shared probability occupied map;

[0068] Step S3: perform operations optimization allocation according to the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point, and allocate each drone to the next target point;

[0069] Step S4: perform trajectory planning according to the target points assigned to each UAV, obtain a continuous expected trajectory that takes into account obstacle avoidance and collision avoidance between UAVs, and output it to the flight controller of each UAV for flight control.

[0070] In the specific technical solution of this embodiment, multi-UAV space exploration requires that the UAVs share the perceived scene information to generate and update the shared map. Since the depth cameras of the UAVs can perceive each other, when sharing the map, each UAV needs to mask the neighboring UAVs within its field of view.

[0071] Specifically, Figure 2 As shown, the step S1 includes:

[0072] Step S101, performing a multi-drone mutual concealment determination to determine whether there is a concealed drone;

[0073] The determination of the presence of a covered drone includes:

[0074] 1) One-time determination: when obtaining the updated depth map and global position of each drone, determine the distance between any two drones. If the distance between the two drones is greater than the depth camera perception distance, it is determined that there is no mutual perception between the two drones; otherwise, proceed to the next step;

[0075] 2) Secondary determination: Determine whether one of the UAVs is within the field of view of the other UAV based on the relative position vector of the two UAVs and the viewing angle vector of the onboard depth camera.

[0076] Specifically, in the secondary determination of UAV i Is it present in UAV j Within the field of view, it is determined by the following formula:

[0077] Result = minf(sign(dir ij dir r )),r∈0,1,2,3;

[0078] in, dir r For UAV j The viewing angle vector of dir ij UAV i and UAVs j The relative position vector of ; sign is a mathematical symbol function;

[0079] UAV i 、UAV j Global location data of

[0080] UAV j The perspective vector dir r for:

[0081]

[0082]

[0083]

[0084]

[0085] in, UAV i The rotation matrix from the camera coordinate system to the global coordinate system, angle top 、angle bottom 、angleleft and angle right UAV i The camera's upward, downward, left and right viewing angles.

[0086] If Result is 0, it means UAV i Not in UAV j If it is within the field of view, it means it appears within the field of view and affects the estimation of the depth camera.

[0087] Step S102: performing a mask operation on the depth information captured by the obscured UAV to obtain a processed depth map;

[0088] During the mask operation, the UAV i The spatial position is converted to UAV j In the camera coordinate system:

[0089]

[0090]

[0091] in, From global coordinate system to UAV j The transformation matrix of the camera coordinate system, For UAV i In UAV j The coordinates in the camera coordinate system, is the intrinsic parameter matrix of the UAV depth camera;

[0092] Thus obtaining UAV i The projected pixel coordinates [u ij ,v ij ,1] T , its projection shape on the depth map is simplified to a circle, the radius of which is the UAV i Wheelbase L i The equivalent projection length on the pixel plane is r i , expressed as:

[0093]

[0094] In the pixel plane, it satisfies (uu ij ) 2 +(vv ij ) 2 ≤r i The depth of the pixel is set to 0, thus completing the mask operation.

[0095] Step S103: converting the depth map into an obstacle point cloud based on the processed depth information, and generating a shared probability occupancy map according to the obstacle point clouds simultaneously observed by all drones;

[0096] Specifically, according to the intrinsic and extrinsic parameter data of the depth camera, the processed depth information corresponding to the two-dimensional pixel coordinates in the depth map is converted into a three-dimensional obstacle space point cloud in the global coordinate system;

[0097] Among them, according to the camera's intrinsic parameter data, the pixel plane is mapped to the camera coordinate system as follows:

[0098]

[0099] Among them, u and v are the two-dimensional pixel coordinates of the image; X, Y, and Z are the coordinates of the camera coordinate system; is the internal parameter matrix, f x 、f y 、c x 、c y is the internal parameter of the camera;

[0100] According to the camera's external parameter data, the coordinates in the camera coordinate system are transferred to the global coordinate system as follows:

[0101]

[0102] Among them, X w , Y w , Z w is the three-dimensional coordinate in the global coordinate system; R is the rotation matrix of the camera extrinsic parameter, and t is the translation vector of the camera extrinsic parameter.

[0103] The shared probability occupancy map is composed of a dynamic array, in which each data unit stores the observation probability obtained by multiple drones and the state corresponding to the map element determined by the observation probability; and the observation probability and observation state are updated according to the obstacle point cloud updated frame by frame.

[0104] Specifically, when the shared probability occupancy map is initialized, an initial value is assigned to each data unit, indicating that the initial state of all data units with respect to the map element is an unknown state;

[0105] When the current observation state of a map element in the updated 3D obstacle point cloud is occupied, the updated observation probability S in the corresponding data unit of the shared probability occupation map is new =S old +loUAV;

[0106] Among them, S old is the probability of the previous observation; loUAV is the numerical increment of the state occupied in the current observation; phit is the pre-set probability of observing an occupied state, 0.5 <p hit <1.

[0107] For shared maps among multiple drones, the map unit is in the UAV i In the UAV j It may be free in one drone, and it may be in another state in other drones, so the state of map units in the shared map is determined by all drones.

[0108] So the probability of the current observation updated in the shared probability occupancy map is determined by all drones:

[0109]

[0110] Among them, S old,share is the previous observation probability; loUAV i UAV i The current observation is the numerical increment of the occupied state; N is the number of drones.

[0111] By updating, when the shared probability occupies the data unit of the map, the observation probability S new,share When a certain threshold is reached, the map unit storing the value is marked as occupied; when it is lower than the threshold, it is marked as free; when it is equal to the initialization value, it is marked as unknown.

[0112] The threshold value can be preset according to actual conditions.

[0113] Specifically, Figure 3 As shown, in step S2, it includes:

[0114] Step S201, searching the current state of each data unit in the shared probability occupied map to find a boundary set for distinguishing unknown and known areas of the map;

[0115] The data unit at the intersection of the unknown state and the free state in the shared probability occupation map belongs to the boundary, its own state is the free state, and the state of its neighboring unit is the unknown state. It can be expressed as:

[0116] F = {cell 0,free ,…,cell l,free ,…,cell L,free};

[0117] Neighbor(cell l,free )∈Unknown;

[0118] Among them, Unknown represents the set of unknown states.

[0119] There are multiple boundaries {F0,…,F m ,…,F M} constitutes a boundary set, and the drone searches the boundaries in the set in turn to achieve the purpose of traversing the space.

[0120] Step S202, searching for the observation point position that maximizes the observation efficiency of each boundary in the boundary set;

[0121] During the search process, the optimal observation point position is determined by searching the mean point of the boundary, the positions of the two spatial points farthest apart at both ends of the boundary, and the normal line of the center of the boundary. The center point position calculated from the positions of the two spatial points farthest apart at both ends of the boundary and the perception field of view FOV of the depth camera are used.

[0122] Specifically, the search process for the position of the boundary m observation points includes:

[0123] 1) Obtaining the mean point of the boundary m;

[0124]

[0125] 2) Starting from the boundary mean point, a traversal search is performed to obtain the positions of the two spatial points at both ends of the boundary that are the farthest apart;

[0126] cell max,m =length max dir PCA +cell mean,m

[0127] cell min,m =length min dir PCA +cell mean,m

[0128] Among them, length max is the maximum distance between the searched boundary point and the mean point, lenght min is the minimum distance between the searched boundary point and the mean point, dir PCA is the principal component direction of all spatial points within the boundary.

[0129] The target point cell that meets the conditions will be obtained by searching in the boundary max,m and cell min,m , determined as the spatial point with the farthest distance between the two ends of the boundary.

[0130] 3) Determine the normal line of the center of the boundary according to the positions of the two spatial points farthest from each other at both ends of the boundary;

[0131] The center normal direction is expressed as:

[0132]

[0133] z w is the z-axis in the global coordinate system.

[0134] 4) The best observation point position is determined based on the center point position calculated from the two spatial points at both ends of the boundary and the perception field of view FOV of the depth camera.

[0135] The center point position calculated from the spatial points at both ends of the boundary is:

[0136]

[0137] Among them, s m It is expressed as the distance between the best observation point VP and the center point Mid on the center normal:

[0138]

[0139] Therefore, the optimal observation point VP can be expressed as:

[0140] VP m =Mid m +s m dir normal,m ;

[0141] The best observation points on each boundary constitute the boundary best viewpoint set.

[0142] In step S3, compared with the spatial search of a single drone, the spatial search of multiple drones needs to determine the appropriate observation points for each drone in order to achieve the effect of global cost optimization. Therefore, it is necessary to simplify the problem into a mathematical model and solve it through operations optimization problem.

[0143] That is, according to the number of observation points and the number of UAVs in the optimal observation point set of the boundary, as well as the cost from the current position of the UAV to the observation point, operations optimization allocation is performed to assign each UAV to the next target point.

[0144] Specifically, the M observation points VP m ∈{VP1,…,VP M}Assigned to N UAVs n ∈{UAV1,…,UAV N}, a drone is only assigned one observation point, and after the assignment, all drones reach their respective observation points at the minimum cost. The following allocation optimization mathematical model is established:

[0145] UAV n Whether to participate in the search is indicated by y n ∈{0,1}. Whether to set the observation point VP mAllocated to the unmanned aerial vehicle (UAV) n Denoted as x mn ∈ {0, 1}. Among them, the UAV n To the observation point VP m The cost is denoted as d mn The total cost for all UAVs to reach their respective observation points is denoted as Minimize the total cost Cost through allocation optimization.

[0146] Since the number of observation points and UAVs cannot be guaranteed to be the same, when the number of observation points is less than the number of UAVs, only a part of the UAVs participate in the search. When the number of observation points is greater than the number of UAVs, all UAVs must participate in the search. In both cases, the UAVs participating in the search are assigned only one observation point.

[0147] In this embodiment, allocation optimization is performed separately according to the two cases where the number of observation points is less than the number of UAVs and the number of observation points is greater than the number of UAVs to minimize the total cost.

[0148] Case 1: When M < N, the allocation optimization problem is formulated as follows:

[0149]

[0150]

[0151]

[0152]

[0153]

[0154]

[0155] When M > N, the number of observation points is greater than the number of UAVs. All UAVs participate in the search task and are assigned observation points. Therefore x mn ≤ y n Still holds. Since each UAV is assigned only one observation point, there are observation points that are not involved in the assignment, No longer holds, but can be expressed as,

[0156] Case 2: When M > N, the allocation optimization problem is formulated as follows:

[0157]

[0158]

[0159]

[0160]

[0161]

[0162]

[0163] Preferably, UAV n To observation point VP m The cost is the total observation cost d consisting of distance cost, speed cost and speed change cost mn .

[0164] Specifically, the total observation cost includes:

[0165] 1) Distance cost; consider UAV n Current location to observation point VP m The reachable distance of the location is obtained by using the global A-star to plan the path length from the current location of the drone to the point:

[0166] cost reach_dist =length(A*path);

[0167] 2) Speed ​​cost: Consider UAV n Path end speed and observation point VP m The degree of coincidence of the center normal of the position. This value is the angle between the vector direction of the path point at the end of the path planned by the global A star and the normal vector of the center of the boundary:

[0168] cost vel_normal =cos -1 (dir normal dir path_end );

[0169] 3) Speed ​​change cost; consider UAV n The current speed reaches the observation point VP m The change in position is the angle between the current velocity vector direction of the drone and the normal vector at the center of the boundary:

[0170] cost vel_change =cos -1 (dir vel_current dir path_end );

[0171] The total observation cost of the observation point obtained by combining the above three observation costs is:

[0172] d mn =cost sum =cost reach_dist+cost vel_normal +cost vel_change .

[0173] Through the above allocation optimization mathematical model, a suitable observation point can be found as the next target point for each UAV participating in the search.

[0174] In step S4, each UAV performs trajectory planning according to the assigned next target point to obtain a continuous desired trajectory that considers obstacle avoidance and avoidance of collisions between UAVs, such as Figure 4 As shown, specifically including:

[0175] Step S401, first path planning: performing path planning according to the obstacle avoidance perception map of each UAV to generate a continuous expected trajectory for each UAV;

[0176] Step S402, second path planning; judging the collision between drones according to the continuous expected trajectory of each robot in the same time period; for the two drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of drones.

[0177] Specifically, in step S401, it includes:

[0178] 1) Perform one-time planning based on global A-star path planning to generate discrete paths;

[0179] The global A-star algorithm is used to find a discrete safe path from the current position to the target position in the current shared probability occupation map. The minimum node of the path is the minimum volume unit of the map. In the A-star algorithm, the total cost of each node in the map is expressed as the sum of the cost from the starting point and the cost from the end point:

[0180] f(n)=g(n)+h(n);

[0181] Starting from the starting point, the A-star algorithm searches for the adjacent nodes with the minimum total cost in turn until the end point is found. The total cost of the node is stored in a priority queue structure to ensure that the algorithm can quickly extract the node with the minimum total cost. In the end, a safe path with the minimum unit of map resolution is obtained, which is represented by path points of consecutive adjacent map unit voxels.

[0182] 2) Down-sampling is performed at the set path distance to generate sampled path points;

[0183] The discrete path calculated by the global A star is downsampled at an appropriate path distance, and continuous adjacent path voxel points are sampled at appropriate distance intervals to form more dispersed path point data.

[0184] 3) Using the downsampled path points as Bezier curve control points, a parameterized trajectory curve equation with respect to time is established;

[0185] Create a parametric trajectory curve equation with respect to time:

[0186]

[0187]

[0188] Among them, w i represents the weight of the i-th item, Indicates the number of combinations.

[0189] 4) Generate the continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

[0190] By using parameterized Bezier curves to represent safe discrete paths as continuous expected trajectories over time, the expected state of the UAV at each moment can be obtained.

[0191] Specifically, in step S402, it includes:

[0192] 1) The trajectory curves in the same time period are transmitted between multiple UAVs, and the time period is downsampled to obtain multiple sampling moments;

[0193] The sampling time set composed of multiple sampling time is T = {0, Δt, 2Δt, 3Δt, ..., nΔt}; n is the number of sampling points, Δt is the sampling interval;

[0194] 2) For the continuous expected trajectory of all UAVs, the sampling position points of each UAV are calculated at each sampling time;

[0195] After sampling, the set of sampling position points on the n-segment Bezier curve of the k-th drone is:

[0196]

[0197] 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3;

[0198] 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in the direction away from the other party's expected trajectory, so that the distance between the position points on the connection line at the same time exceeds the safe distance;

[0199] Specifically, on the m-segment Bezier curve, the distance between the two sampling position points of drones i and j at the same time is expressed as:

[0200]

[0201] exist When f is the safe distance; in the secondary trajectory planning, two control points {Q m ,Q m+1};

[0202] For control points {Q m ,Q m+1} Move the distance distance along the sampling position difference vector dir; the updated control point obtains the updated control point {Q m,new ,Q m+1,new};in,

[0203]

[0204]

[0205]

[0206]

[0207] In the formula, λ m , m+1 ≥1 is the pull-off coefficient.

[0208] 5) Reconstruct the Bezier trajectory curve based on the updated control points to obtain the trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0209] According to the updated control points {Q0,Q1,…,Q m,new ,Q m+1,new ,…,Q n}Reconstruct the Bezier trajectory curve to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0210] The continuous expected trajectory of each UAV is output to the flight controller of each UAV for flight control.

[0211] In summary, the distributed space search and trajectory planning method for multi-UAV collaboration in this embodiment solves the problem of independent mutual perception of distributed cluster systems, improves the system's ability to perceive environmental consistency, and effectively avoids interference from invalid perception between machines. In addition, a method of operations optimization is used to allocate tasks for distributed clusters, solving the problem of lack of planning for distributed cluster operations, and minimizing the time and energy consumption of the entire system tasks on a global scale.

[0212] Embodiment 2

[0213] One embodiment of the present invention discloses a distributed space search and trajectory planning device for multiple UAVs, such as Figure 5 As shown, including:

[0214] A shared map generation module is used to share maps based on scene depth maps of the respective locations output by multiple drones, and generate a shared probability occupancy map;

[0215] A boundary search module, for performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary combination according to the map information in the shared probability occupied map;

[0216] The target point allocation module is used to allocate the next target point to each drone based on the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point;

[0217] The trajectory planning module is used to plan the trajectory according to the target points assigned to each UAV, obtain the continuous expected trajectory that takes into account obstacle avoidance and avoidance of collision between UAVs, and output it to the flight controller of each UAV for flight control.

[0218] The specific technical details of the drone in this embodiment are the same as those in the previous embodiment. Please refer to the previous embodiment for details, and no further description will be given here.

[0219] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A distributed space search and trajectory planning method for multi-UAV collaboration, characterized in that: include: Step S1: Share the scene depth maps of the respective locations output by multiple drones to generate a shared probability occupancy map; Step S2: performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary set according to the map information in the shared probability occupied map; Step S3: perform operations optimization allocation according to the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point, and allocate each drone to the next target point; Step S4: perform trajectory planning according to the target points assigned to each UAV, obtain a continuous desired trajectory that takes into account obstacle avoidance and collision avoidance between UAVs, and output it to the flight controller of each UAV for flight control; In step S3, Observation points Assigned to drones , one drone is assigned only one observation point, and after the assignment, all drones reach their respective observation points at the minimum cost; the following allocation optimization mathematical model is established: Drones Whether to participate in the search is indicated by ; Whether to observe the point Assign to drone Expressed as Among them, drones To the observation point The cost is expressed as , the total cost of all drones reaching their respective observation points is expressed as ; Through allocation optimization, the total cost reach a minimum; According to the two situations where the number of observation points is less than the number of drones and the number of observation points is greater than the number of drones, the allocation optimization is performed to minimize the total cost; Situation 1: When , the allocation optimization problem is expressed as: ； ； ； ； ； ； Situation 2: When , the allocation optimization problem is expressed as: ； ； ； ； ； 。 2. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 1 is characterized in that: The step S1 includes: Step S101, performing a multi-drone mutual concealment determination to determine whether there is a concealed drone; Step S102: performing a mask operation on the depth information captured by the obscured UAV to obtain a processed depth map; Step S103: converting the depth map into an obstacle point cloud based on the processed depth information, and generating a shared probability occupancy map according to the obstacle point clouds simultaneously observed by all drones; The shared probability occupancy map is composed of a dynamic array, in which each data unit stores the observation probability obtained by multiple drones and the state corresponding to the map element determined by the observation probability; and the observation probability and observation state are updated according to the obstacle point cloud updated frame by frame.

3. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 2 is characterized in that: The determination of the presence of a covered drone includes: 1) One-time judgment: when obtaining the updated depth map and global position of each drone, judge the distance between any two drones. If the distance between the two drones is greater than the perception distance of the depth camera, it is determined that there is no mutual perception between the two drones; otherwise, proceed to the next step; 2) Secondary determination: Determine whether one of the drones is within the field of view of the other drone based on the relative position vector of the two drones and the viewing angle vector of the onboard depth camera; The secondary determination formula is: in, ; is a mathematical symbolic function; For drones and The relative position vector of for The viewing angle vector of when 0, indicating Not Available Otherwise, it appears in the field of view and affects the estimation of the depth camera.

4. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 1 is characterized in that: In step S2, it includes: Step S201, searching the current state of each data unit in the shared probability occupied map to find a boundary set for distinguishing unknown and known areas of the map; The data unit at the intersection of the unknown state and the free state in the shared probability occupation map belongs to the point on the boundary, its own state is the free state, and the state of its neighboring unit is the unknown state; Step S202, searching for the observation point position that maximizes the observation efficiency of each boundary in the boundary set; During the search process, the mean point of the boundary, the positions of the two spatial points farthest from each other at both ends of the boundary, the normal line of the boundary center, and the center point position calculated based on the positions of the two spatial points farthest from each other at both ends of the boundary and the perception field of view of the depth camera are searched. FOV Determine the best observation point location.

5. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 1 is characterized in that: The cost of the drone to the observation point is the total observation cost consisting of distance cost, speed cost and speed change cost.

6. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 1 is characterized in that: In step S4, it specifically includes: Step S401, first path planning: performing path planning according to the obstacle avoidance perception map of each UAV to generate a continuous expected trajectory for each simulated UAV; Step S402, second path planning; judging the collision between drones according to the continuous expected trajectory of each robot in the same time period; for the two simulated drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of drones.

7. The distributed space search and trajectory planning method for multi-UAV collaboration according to claim 6 is characterized in that: Step S402 includes: 1) The trajectory curves in the same time period are transmitted between multiple drones, and the time period is downsampled to obtain multiple sampling moments; 2) For the continuous expected trajectory of all UAVs, the sampling position of each UAV is calculated at each sampling time; 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3; 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two preceding and following Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in a direction away from the other party's expected trajectory, so that the distance between the position points on the connection line at the same time exceeds the safe distance.

8. A device using the multi-UAV coordinated distributed space search and trajectory planning method as described in any one of claims 1 to 7, characterized in that: include: A shared map generation module is used to share maps based on scene depth maps of the respective locations output by multiple drones, and generate a shared probability occupancy map; A boundary search module, for performing boundary search to obtain a boundary set and a boundary optimal viewpoint set corresponding to the boundary combination according to the map information in the shared probability occupied map; The target point allocation module is used to allocate the next target point to each drone based on the number of observation points and the number of drones in the boundary optimal observation point set, as well as the cost from the current position of the drone to the observation point; The trajectory planning module is used to plan the trajectory according to the target points assigned to each UAV, obtain the continuous expected trajectory that takes into account obstacle avoidance and avoidance of collision between UAVs, and output it to the flight controller of each UAV for flight control.

Images