Fast Trajectory Planning for Multiple Quadrotors using Relative Safe Flight Corridor

F ast T rajectory Planning f or Multiple Quadr otors using Relativ e Safe Flight Corridor Jungwon Park 1 and H. Jin Kim 1 Abstract — This paper presents a new trajectory planning method for multiple quadrotors in obstacle-dense en vir onments. W e suggest a relati ve safe flight corridor (RSFC) to model safe region between a pair of agents, and it is used to generate linear constraints for inter-collision a voidance by utilizing the conv ex hull property of r elative Bernstein polynomial. Our appr oach employs a graph-based multi-agent pathfinding algorithm to generate an initial trajectory , which is used to construct a safe flight corridor (SFC) and RSFC. W e express the trajectory as a piecewise Bernstein polynomial and formulate the trajectory planning problem into one quadratic programming problem using linear constraints from SFC and RSFC. The proposed method can compute collision-free trajectory for 16 agents within a second and f or 64 agents less than a minute, and it is validated both through simulation and indoor flight test. I . I N T RO D U C T I O N Multi-agent systems consisting of micro aerial vehicles (MA Vs) are receiving attention from many industrial do- mains due to their agility , mobility , and applicability . T o maximize their capabilities for v arious missions such as cooperativ e surveillance [1] and transportation [2], it re- quires to generate safe trajectories for multiple quadrotors in a complex environment within a short time. Howe ver , it has been challenging to ef ficiently formulate constraints to avoid obstacles and other agents. Furthermore, deadlock may happen if agents are packed in a narrow space. In this paper , we focus on an ef ficient planning method in terms of both cost and computation time which generates safe, dynamically feasible trajectories in an obstacle-dense en vironment without deadlock. One popular approach to generate multi-agent trajectories is a centralized optimization method In [3], constraints for collision a voidance are reformulated in integer constraints for mixed-inte ger quadratic programming (MIQP). Howe ver , it requires over 500–1000 seconds to optimize the trajectory of 2–4 agents due to the computational complexity of the MIQP . In [4], sequential conv ex programming (SCP) is proposed to replace non-con ve x constraints with con vex ones. SCP shows good performance when planning a small number of quadrotors, but it is intractable for a large team and complex environment. The authors of [5] suggest nonlinear programming (NLP) which combines sequential planning to deal with nonlinear constraint directly . This use of sequential planning method allows to achiev e better scalability , but it 1 Jungwon Park is with the Department of Mechanical and Aerospace Engineering, Seoul National University , Seoul, South Korea qwerty35@snu.ac.kr 1 H. Jin Kim is with the Department of Mechanical and Aerospace Engineering, Seoul National University , Seoul, South Korea hjinkim@snu.ac.kr Fig. 1: SFC (semi-transparent blue box) and RSFC (semi- transparent green box) used for planning trajectories for two agents (red and blue lines). The trajectory of agent 2 (blue line) is generated within the intersection between SFC and RSFC to av oid obstacles and inter-collision model between two agents (red box) has a limitation that no feasible solution can be found for a crowded situation. A decentralized method also has been considered to reduce total planning time by distributing the computational load. Approaches based on LQR-obstacle [6] and buf fered V oronoi cells [7] show that they can generate a collision-free path in real time. Ho wev er , such distributed methods are not able to guarantee the completeness, no deadlock. In [8] and [9], distributed model predictive control (DMPC) is proposed to optimize the control input instead of a piecewise polynomial path, but they do not consider obstacles. In single quadrotor path planning, many researchers have adopted a safe flight corridor (SFC) to model free space in a map. SFC is composed of connected conv ex sets, and it can be represented as linear inequality constraints for obstacle av oidance in quadratic programming (QP) [10, 11, 12]. In [13], SFC is used to separate a safe region of each agent in quadrotor swarm. By resizing SFC iterativ ely , trajectories of quadrotor swarm can be refined separately without inter -collision. This decoupled iterativ e optimization method shows good scalability in a maze-like en vironment, but it requires many iteration steps for the con vergence of the overall cost. In this paper , we propose a new centralized multi-agent path planning method that uses a relati ve safe flight corridor (RSFC) to find a feasible trajectory without any sequential or iterativ e process. Similar to SFC, RSFC utilizes a property of Bernstein polynomial to con vert non-conv ex inter-collision av oidance constraints into linear ones as illustrated in Fig. 1, and it does not need an additional resizing process to find the feasible trajectory . Thus, our proposed method can optimize a piece wise polynomial trajectory by using QP only once, and it guarantees a feasible solution of QP does not cause a collision and deadlock. Recently , distributed planning is receiving much attention due to scalability , yet centralized methods can still provide the quality solution by the efficient formulation using RSFC. Our main contributions can be summarized as follows. • A collision av oidance constraint formulation method using relativ e safe flight corridor is proposed, which does not require sequential or iterativ e process. • A fast trajectory optimization framew ork is presented in an obstacle-dense en vironment, which guarantees collision- and deadlock-free. This paper is structured as follows. The problem statement is presented in section II. In section III, we describe the method of multi-agent trajectory planning using relativ e safe flight corridor . Experimental results are presented in section IV. Finally , section V contains conclusions. I I . P RO B L E M S T A T E M E N T Consider a multi-agent robot system that consists of N q quadrotors. Each quadrotor is assumed to hav e different size with radius r 1 , ..., r N q but has the same dynamic limit. W e assume that prior kno wledge of the free space F and obstacle O is given in 3D occupancy map and start, goal point of the i th quadrotor is assigned as s i , g i . It has been shown that quadrotor dynamics is differentially flat and trajectory can be represented in piecewise polyno- mials with flat outputs in time t [14]. Thus,trajectory of i th quadrotor , p i ( t ) , can be represented in M -se gment piecewise polynomials. In this paper , we aim to generate continuous, smooth trajectory p i ( t ) for all i = 1 , ..., N q which minimizes the integral of the square of the n th deriv ative and does not collide with any obstacle and other agents. I I I . M E T H O D The overall structure of our proposed method is depicted in Fig. 2. Our method consists of three steps. First, the discrete path planner plans the initial trajectory using the multi-agent pathfinding (MAPF) algorithm. Then safe flight corridor (SFC) and relative safe flight corridor (RSFC) are constructed based on the initial trajectory . Finally , SFC and RSFC are conv erted into inequality constraints of quadratic programming (QP) and we obtain the desired trajectory by utilizing the con vex hull property of Bernstein polynomial. The detail of each part is described in the following subsec- tions. A. Initial T rajectory Planning When planning the trajectory of a single qu adrotor , many researchers have divided the planning process into initial trajectory planning and trajectory refinement, and such two- step method is now being adopted in the multi-agent case [15, 13]. Inspired by that, we first plan the discrete initial trajectory by using a graph-based MAPF algorithm. The initial trajectory of the i th quadrotor , p i init , is defined as an array of waypoints that connect start and goal position in a graph. In the MAPF , the cost function is defined as the sum of each trajectory’ s length. There have been many researches about MAPF algorithm such as HCA* [16], M* [17], conflict-based search (CBS) [18]. Among them, we choose enhanced CBS (ECBS) [19] as a discrete initial trajectory planner because it can find a suboptimal solution in a short time and we can specify the bound of the solution cost. In other words, it is guaranteed that the cost of the trajectory is lower than c w · optimal cost, where c w is a user-specified bounding factor . T o utilize the graph-based ECBS in our problem, discrete planner translates 3D occupancy map into a 3D grid map. After translation, ECBS computes a discrete initial trajectory that connects start and goal points. If start and goal points are not located on the 3D grid map, then we use the nearest grid points to obtain trajectory and append the start/goal points to both ends. Finally , to calculate relative trajectory between two agents, we match the initial trajectory of all agents as the same length l max by appending each goal point at the end of the trajectory , where l max is the length of the longest initial trajectory . B. Safe Flight Corridor Construction SFCs of the i th quadrotor , S F C i 1 , ..., S F C i M s , are defined as con vex sets that do not collide with obstacle and are sequentially connected: for m = 1 , ..., M s S F C i m ⊕ C i obs ∈ F (1a) and for m = 1 , ..., M s − 1 S F C i m ∩ S F C i m +1 6 = ∅ (1b) where ⊕ is the Minkovski sum and C i obs is the obstacle- collision model for the i th quadrotor , which is defined as a sphere with radius r i representing safety clearance between an obstacle and a quadrotor . The trajectory of i th quadrotor is collision-free from ob- stacle if for arbitrary t ∈ [0 , T ] , there exists m ∈ { 1 , ..., M s } such that p i ( t ) ∈ S F C i m , where T is the total flight time. W e construct SFC by the axis-search method. W e initialize corridors with a predefined size at each waypoint of the initial trajectory . Then, except the corridors from the start and goal points, we expand them in the previous waypoint direction to connect two sequential con vex sets. All waypoints except start and goal points are aligned on the 3D grid map, thus we can achiev e the condition (1b) by this method. After that, we expand each corridor in all the other axis-aligned directions until it has a maximum possible free space. Finally , we delete duplicated corridors. Fig. 2: Overvie w of the proposed method. The proposed method can plan the trajectory in the 3D space, but in this example, we plan the trajectory in 2D space for the conv enience of explanation. W e assigned the start positions for agent 1 (red) and agent 2 (blue) at (0, -2), (0, -2), and goal positions at (0, 2), (2, 0). W e con vert 3D occupancy map into the 3D grid map and compute discrete initial trajectory by using MAPF algorithm. SFC of each agent (red and blue boxes in SFC construction) is constructed along waypoints of their initial trajectories to prevent obstacle collision. RSFC (green boxes in RSFC construction) is used to confine a relativ e trajectory between two agents. Finally , SFC and RSFC are translated into linear inequality constraints and QP solver generates the continuous smooth trajectory that satisfies SFC and RSFC constraints. C. Relative Safe Flight Corridor Construction As SFC models a obstacle-free space as a con vex set, RSFC models a free space for ev asiv e maneuver between two agents. W e define RSFCs between the i th and j th agents, RS F C i,j 1 , ..., RS F C i,j M r , as con vex sets that do not in vade the collision region between the i th and j th agents and are sequentially connected: for m = 1 , ..., M r RS F C i,j m ∩ C i,j inter = ∅ (2a) and for m = 1 , ..., M r − 1 RS F C i,j m ∩ R S F C i,j m +1 6 = ∅ (2b) where C i,j inter is a inter-collision model between i th and j th quadrotors which has a rectangular parallelepiped oriented with the body frame of i th quadrotor . Note that C i,j inter can vary for each pair of agents, which mean that it can handle different size of quadrotors. In our implementation, we assign length and width of C i,j inter as 2( r i + r j ) and a height as 2 c dw ( r i + r j ) to consider downw ash effect, where c dw is downw ash coefficient. The trajectory of j th quadrotor does not collide with i th quadrotor if for arbitrary t ∈ [0 , T ] , there exists m ∈ { 1 , ..., M s } such that p j ( t ) − p i ( t ) ∈ RS F C i,j m . Construction of RSFC is described in Fig. 3. First, we con- vert the initial trajectory into the relativ e initial trajectory for each pair of agents (Fig. 3b). The relative initial trajectory of i th and j th quadrotors, p i,j init , can be obtained by subtracting corresponding waypoints of two initial trajectories. Ne xt, for each waypoint in the relati ve path, we choose proper RSFC from RSFC candidates. Using the quadrotors’ differential flatness property , we design RSFC candidates to reduce the number of decision variable at the optimization step. There are six RSFC candidates in direction ± x, ± y , ± z , and each candidate RS F C µ is defined as follows: RS F C µ = ( { p | p · n µ > r i + r j } if µ = ± x, ± y { p | p · n µ > c dw ( r i + r j ) } if µ = ± z (3) where n µ is a unit vector in the direction µ ∈ {± x, ± y , ± z } . For each waypoint p i,j init [ k ] in p i,j init , any RS F C µ can be selected if µ satisfies the following condition: p i,j init [ k ] · n µ > 0 (4) Howe ver , redundant RSFC transitions along the w aypoints may increase the number of polynomial segments and the computation time. Figs. 3c and 3d show the e xample. When we generate a smooth relativ e trajectory in RSFC, we need to plan two segment polynomials to represent the relativ e trajectory if there is one transition of RSFC (i.e. RS F C i,j 1 → RS F C i,j 2 ) along the waypoints as shown in Fig. 3c. Howe ver , if three transitions are inv olved (i.e. (a) Initial trajectories of the i th (red) and j th (blue) quadrotors. (b) Relativ e initial trajec- tory of the j th quadrotor with respect to the i th . (c) RSFC generated. (d) Example of Redundant RSFC transition. Fig. 3: Construction process of RSFC. F or the con venience of explanation, it is depicted in 2D space. The initial trajectory and the relati ve initial trajectory of two multirotors are sho wn in Figs. 3a and 3b. The inter-collision model between two agents are depicted as a red box and RSFC is constructed along the waypoints of relative path av oiding the inter- collision model. Redundant RSFC transitions may occur as shown in Fig. 3d, so the greedy algorithm is used to minimize the number of RSFC transitions and the result is shown in Fig. 3c. RS F C i,j 1 → R S F C i,j 2 → R S F C i,j 3 → R S F C i,j 4 ) as shown in Fig. 3d, we hav e to plan two more polynomial segments compared to the previous one. Thus, we use the greedy algorithm (Alg. 1) to minimize the number of RSFC transitions. The algorithm receiv es p i init and p j init as input and returns RS F C i,j . It initializes RS F C i,j (line 3) as an empty array and s µ as an array of all zero with length l max (line 4-5). After initialization, the algorithm verifies RSFC candidates using (4) and sa ves the result in s µ (line 6-12). At the end of the relativ e path, it finds an RSFC candidate that includes the maximum number of waypoints and appends the candidate to RS F C i,j (line 14-15). After that, it goes to the last waypoint among the included ones (line 16). Then again it finds the maximum including candidate until it reaches the start point of relativ e path (line 17-20). Note that the ne w candidate must not be located at the opposite side of the previous candidate because quadrotors cannot jump through an empty space between two opposite candidates (line 18). Algorithm 1 RSFC construction 1: Input: p i init , p j init 2: l max ← max( size( p i init ), size( p j init )) 3: R S F C i,j ← ∅ 4: for all µ ∈ { + x, + y, + z , − x, − y, − z } do 5: initialize s µ [ l max ] to 0 6: for n ← 1 to l max do 7: for all µ ∈ { + x, + y , + z , − x, − y , − z } do 8: if ( p j init [ n ] − p i init [ n ]) · n µ > 0 then 9: if n = 1 then 10: s µ [ n ] ← 1 11: else 12: s µ [ n ] ← s µ [ n − 1] + 1 13: n ← l max 14: µ M ← arg max µ ( s µ [ n ]) 15: R S F C i,j . push front ( R S F C µ M ) 16: n ← n − s µ M [ n ] 17: while n > 0 do 18: µ M ← arg max µ 6 = − µ M ( s µ [ n ]) 19: RS F C i,j . push front ( R S F C µ M ) 20: n ← n − s µ M [ n ] 21: return RS F C i,j D. T ime Se gment Allocation T o formulate an optimization problem, we determine the time se gment of the piece wise polynomial trajectory and allocate each SFC and RSFC to each polynomial segment. Let p i m ( t ) be the m th segment of p i ( t ) which is defined at t ∈ [ t i m − 1 , t i m ] . The time segment of the i th quadrotor t i s is defined as: t i s = [ t i 0 , ..., t i M ] (5) In this paper , we set the trajectory of all agents to have the same time segment t s because it is necessary for our algorithm to utilize the con vex hull property of Bernstein basis polynomial. Ho wev er , it can result in too many decision variables, which can increase the computation time. Thus, the following method is used to decrease the number of decision variables. T o construct the time segment for all agents, we generate partial time se gments t sp from SFC and RSFC. Alg. 2 shows the process of finding partial time segment. The algorithm receiv es SFC or RSFC and initial or relativ e initial trajectory as its input, and searches for the middle waypoint among the intersection of two sequential con vex sets (line 11-13). After that, the algorithm records the index of this middle waypoint to assign as the location at which the SFC or RSFC transition occurs (line 14). In other words, m th SFC or RSFC is allocated before the time ( n +  count/ 2  ) ∗ t step and m + 1 th SFC or RSFC is allocated after time ( n +  count/ 2  ) ∗ t step , where n +  count/ 2  is the index of middle waypoint among the intersection of m th and m + 1 th con ve x sets. In the SFC case, it is guaranteed that there exists a waypoint in two sequential SFCs because we connect the waypoint Algorithm 2 Finding partial time segment 1: Input: Initial or relativ e initial trajectory p init , 2: Array of sequential conv ex sets C , 3: T ime step t step 4: t sp ← ∅ 5: m ← 1 6: for n ← 1 to l max do 7: if m ≥ size ( C ) then 8: break 9: if p init [ n ] ∈ ( C [ m ] ∩ C [ m + 1]) then 10: count ← 1 11: while p init [ n + count ] ∈ ( C [ m ] ∩ C [ m + 1]) 12: and n + count ≤ l max do 13: count ← count + 1 14: t sp . push back ( n +  count/ 2  ∗ t step ) 15: n ← n +  count/ 2  16: m ← m + 1 17: else if p init [ n ] ∈ C [ m + 1] then 18: t sp . push back (( n + 0 . 5) ∗ t step ) 19: m ← m + 1 20: return t sp with SFC by the axis-search method. Ho wev er , in the RSFC case, there may be no waypoint in an intersection between two sequential RSFCs (line 17). In this case, we do not use the integer index because it can make an infeasible constraint when SFC and RSFC are changed simultaneously at the same time. Instead, we use a heuristic method that gi ves a time delay to RSFC transition to av oid the simultaneous change of SFC and RSFC (line 18). It may increase the number of the decision variables, but we can increase the success rate of finding a feasible trajectory . This algorithm always returns an array with a maximum size of 2 l max , so it is guaranteed that the piecewise trajectory has a maximum of 2 l max − 1 segments. After generating the partial time segments, we combine all of them into one and sort them. W e delete duplicated elements and we generate the total time segment by append- ing the start time and total flight time at each end of the combined array . This method can reduce the size of the total time segment by overlapping the elements of partial time segment as much as possible. W e allocate SFC and RSFC to time segment by comparing t sp with t s . For example, assume that t s is determined as [0 , 1 , 2 , 3] and t sp of SFC is [2] . Then we can guess that S F C i 1 is assigned for the first and second segments of piecewise polynomials and S F C i 2 is assigned for the third segment. Let S F C i ( m ) and RS F C i ( m ) be con ve x sets that are allocated to the m th polynomial segment. In this example, SFC is allocated as S F C i (1) = S F C i (2) = S F C i 1 and S F C i (3) = S F C i 2 . E. T rajectory Optimization In the optimization step, we plan the smooth polynomial trajectory using SFC, RSFC and time segment t s , but it is difficult to handle SFC and RSFC with standard polynomial basis. Thus, we formulate the piecewise polynomial p i ( t ) of all agents as a piecewise Bernstein polynomial. Bernstein basis polynomials of degree N are defined as: B k,N ( t ) =  N k  t k (1 − t ) k (6) for t ∈ [0 , 1] and k = 0 , 1 , ..., N , and Bernstein polynomial is the linear combination of Berstein basis polynomials. The m th segment of p i ( t ) can be represented in Berstein polynomial as: p i m ( t ) = c i m, 0 B 0 ,n ( τ m ) + ... + c i m,N B 0 ,N ( τ m ) (7) where τ m = t − t m − 1 t m − t m − 1 and c i m = [ c i m, 0 , ..., c i m,N ] is the vector consisting of all control points of p i m ( t ) . It is shown that a Bernstein polynomial has a conv ex hull property [20], in other words, a Bernstein polynomial p i ( t ) is confined within the con ve x hull of its control points c i m . In [10, 11, 12, 13], it has been used to confine p i m ( t ) within S F C i ( m ) by limiting control point c i m within S F C i ( m ) . Here, this con vex hull property can be used to confine the relativ e polynomial trajectory . Assume that p i ( t ) and p j ( t ) hav e the same time segment, then the m th segment of p i,j m can be written as: p i,j m ( t ) = N X k =0 ( c j m,k − c i m,k ) B 0 ,n ( τ m ) = N X k =0 c i,j m,k B 0 ,n ( τ m ) (8) where c i,j m,k = c j m,k − c i m,k for k = 0 , ..., N is the control point of p i,j m ( t ) . W e can observe that the relati ve Bernstein polynomial is also a Bernstein polynomial. Therefore, by the con ve x hull property , we can enforce quadrotors i, j not to collide with each other by limiting all control points c i,j m,k within R S F C i m . In this way , we can generate the safe trajectory by adjusting RSFC for each pair of agents. Our decision vector c consists of all control points of p i m ( t ) for m = 1 , ..., M and i = 1 , ..., N q : c = [ c 1 1 T , ..., c 1 M T , ..., c N q 1 T , ..., c N q M T ] T (9) where M is the total number of polynomial segments that all agents share, and it is up to 2 l max as explained in section III-D. The cost function of polynomials is defined as follows: J = N q X i =1 X µ ∈{ x,y ,z } Z T 0 ( d n p i µ ( t ) dt n ) 2 (10) where T is total flight time, and this can be represented into a quadratic form. In this paper, we set n = 3 , so that it minimizes the integral of the square jerk of total trajectory . It is a reasonable choice because we can minimize the input aggressiv eness of quadrotor [21]. The waypoint constraints for start, goal positions and con- tinuity constraints for smooth trajectory can be reformulated in linear equality constraints ( A eq c = b eq ). Therefore, our trajectory generation problem is reformulated as quadratic programming (QP) problem: minimize c T Qc subject to A eq c = b eq c i m,k ∈ S F C i ( m ) , ∀ i, k c j m,k − c i m,k ∈ RS F C i,j ( m ) , ∀ i, j > i, k where Q is the Hessian cost matrix deri ved by concatenating all Hessian cost function of individual agents with a block- diagonal matrix form. The detailed formulation of equality constraints can be found in [12]. Note that we need only one QP to generate a smooth trajectory for all agents. During optimization, we do not consider dynamic limits because they can be infeasible constraints for QP . Instead, we scale the time segment for all agents uniformly after optimization, similar to [13]. I V . E X P E R I M E N T S A. Implementation Details W e implement our proposed method in C++14. W e use the Octomap library [22] to represent the 3D occupancy map and use the dynamicEDT3D library [23] to compute distance between corridor and obstacle for SFC construction. For trajectory optimization, CPLEX QP solver [24] is used to solve (III-E). W e model the collision models of quadrotors with radius r = 0 . 15 m and downw ash coefficient c dw = 2 based on the specification of Crazyflie 2.0 in [13]. For initial trajectory planning, the grid size of the 3D grid map is determined to 0.5 m in x, y-axis directions and 1 m in z-axis direction. W e set the degree of polynomials to N = 5 and gi ve constraints to be continuous up to acceleration. Fig. 4 shows the planning result of 16 agents. B. Computation T ime Evaluation W e e valuate the computation time on a PC running Ubuntu 16.04. with Intel Core i7-7700 @ 3.60GHz CPU and 16G RAM. Our experiment is conducted in 10 m × 10 m × 2.5 m space. W e randomly deploy 30 trees of size 0.3 m × 0.3 m × 1–2.5 m. Start positions of quadrotors are uniformly distributed in a boundary of the xy-plane in 1 m height, and we assigned the goal points at the opposite to their start position as shown in Fig. 4. W e conduct the experiments by randomly changing the location of obstacles and measure the computation time of each step. T able I shows the a verage computation time of 30 experiments. The proposed method takes about a second for 16 quadrotors and a minute for 64 quadrotors. Although it uses the solver with O ( n 3 ) time complexity , the actual total computation time is short enough. C. Success Rate Analysis In section III-D, we give the time delay at the RSFC partial time segment to av oid infeasible constraints. T o verify that,we compared the two time allocation method, one is time allocation with RSFC time delay and the other is time allocation without the time delay by changing the line 18 of Alg. 2 to n ∗ t step . W e plan the trajectory of 16 quadrotors 50 times to measure the success rate. W e use the same en vironment setting in section IV -B except quadrotor size. Fig. 5 sho ws that the time allocation with RSFC time delay has a higher probability to find a feasible solution. It also shows a 100 percent success rate when quadrotor size is 0.15 m and 0.2 m. As expected, the success rate of both methods decrease as the quadrotor size increase. D. Flight T est W e demonstrate our algorithm with 6 Crazyflie 2.0 quadro- tors in a 5 m x 7 m x 2.5 m space. Crazyswarm [25] is used to follo w the pre-computed trajectory , and V icon motion capture system is used to estimate the position of each agent at 100 Hz. It takes 0.138 seconds to plan the trajectory for all agents. Fig. 6 sho ws the snapshot of flight test and the pre-computed trajectory . Full flight is presented in the supplemental video. V . C O N C L U S I O N S In this paper , we propose a trajectory planning method using RSFC to deal with the inter-collision problem in a multi-MA V system. RSFC models the free space for inter- collision av oidance into a conv ex set, and we show that it can be con verted into linear constraints by utilizing the con vex hull property of Bernstein polynomial. T o generate trajectory for multiple quadrotors, we adopt the ECBS algorithm to obtain the initial trajectory in a 3D grid map. Then we construct SFC and RSFC based on the initial trajectory , and allocate them to each segment considering infeasible simul- taneous transition. Finally , an optimization solver generates a smooth trajectory that is collision-free and deadlock-free. The proposed method can generate a safe trajectory for 64 agents in a minute, and flight test is executed to validate our solution. In future work, we plan to reduce the computational ef fort of our work for online trajectory generation and we plan to dev elop more precise time allocation method that guarantees a feasible solution. Fig. 4: T op-do wn vie w of 16 quadrotors in a 10 m × 10 m × 2.5 m random forest map. Goal points are opposite to start positions. T ABLE I: Computation time by the number of quadrotors Agents ECBS( c w =1.3) (s) SFC Construction (s) RSFC Construction (s) T raj. Optimization (s) T otal Comp. Time (s) 4 0.034 0.039 1.68E-5 0.034 0.11 8 0.037 0.053 4.84E-5 0.139 0.23 16 0.048 0.081 1.70E-4 0.800 0.93 32 0.059 0.137 5.77E-4 6.65 6.86 64 0.167 0.256 2.14E-3 50.7 51.2 Fig. 5: Success rate of trajectory planning for 16 agents by the time allocation method. R E F E R E N C E S [1] Martin Saska et al. “Swarm distribution and deploy- ment for cooperativ e surveillance by micro-aerial ve- hicles”. In: Journal of Intelligent & Robotic Systems 84.1-4 (2016), pp. 469–492. [2] Hyoin Kim et al. “Motion planning with mo vement primitiv es for cooperative aerial transportation in ob- stacle environment”. In: 2017 IEEE International Confer ence on Robotics and Automation (ICRA) . IEEE. 2017, pp. 2328–2334. [3] Daniel Mellinger, Alex Kushleye v, and V ijay Kumar . “Mixed-inte ger quadratic program trajectory genera- tion for heterogeneous quadrotor teams”. In: Robotics and A utomation (ICRA), 2012 IEEE International Confer ence on . IEEE. 2012, pp. 477–483. [4] Federico Augugliaro, Angela P Schoellig, and Raf- faello D’Andrea. “Generation of collision-free trajec- tories for a quadrocopter fleet: A sequential con ve x programming approach”. In: Intelligent Robots and (a) Snapshots of flight test. (b) T rajectory plan for flight test. Fig. 6: Flight test with 6 quadrotors. Full flight is presented in the supplemental video. Systems (IR OS), 2012 IEEE/RSJ International Con- fer ence on . IEEE. 2012, pp. 1917–1922. [5] D Reed Robinson et al. “An Efficient Algorithm for Optimal T rajectory Generation for Heterogeneous Multi-Agent Systems in Non-Conv ex En vironments”. In: IEEE Robotics and Automation Letters 3.2 (2018), pp. 1215–1222. [6] Daman Bareiss and Jur V an den Ber g. “Reciprocal col- lision av oidance for robots with linear dynamics using lqr-obstacles”. In: Robotics and A utomation (ICRA), 2013 IEEE International Conference on . IEEE. 2013, pp. 3847–3853. [7] Dingjiang Zhou et al. “Fast, on-line collision av oid- ance for dynamic vehicles using buf fered voronoi cells”. In: IEEE Robotics and Automation Letters 2.2 (2017), pp. 1047–1054. [8] Li Dai et al. “Distributed MPC for formation of multi- agent systems with collision avoidance and obstacle av oidance”. In: Journal of the F ranklin Institute 354.4 (2017), pp. 2068–2085. [9] Peng W ang and Baocang Ding. “A synthesis approach of distributed model predictive control for homoge- neous multi-agent system with collision a voidance”. In: International Journal of Contr ol 87.1 (2014), pp. 52–63. [10] Sarah T ang and V ijay Kumar . “Safe and complete trajectory generation for robot teams with higher- order dynamics”. In: 2016 IEEE/RSJ International Confer ence on Intelligent Robots and Systems (IROS) . IEEE. 2016, pp. 1894–1901. [11] Sikang Liu et al. “Planning dynamically feasible tra- jectories for quadrotors using safe flight corridors in 3-d complex en vironments”. In: IEEE Robotics and Automation Letters 2.3 (2017), pp. 1688–1695. [12] Fei Gao et al. “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial”. In: 2018 IEEE International Con- fer ence on Robotics and Automation (ICRA) . IEEE. 2018, pp. 344–351. [13] W olfgang H ¨ onig et al. “T rajectory planning for quadrotor swarms”. In: IEEE T ransactions on Robotics 34.4 (2018), pp. 856–869. [14] Daniel Mellinger and V ijay Kumar . “Minimum snap trajectory generation and control for quadrotors”. In: Robotics and Automation (ICRA), 2011 IEEE Interna- tional Confer ence on . IEEE. 2011, pp. 2520–2525. [15] Y ang Xu et al. “Concurrent Optimal T rajectory Plan- ning for Indoor Quadrotor Formation Switching”. In: Journal of Intelligent & Robotic Systems (2018), pp. 1–18. [16] Da vid Silver . “Cooperativ e Pathfinding.” In: AIIDE (2005), pp. 117–122. [17] Glenn W agner and Howie Choset. “M*: A complete multirobot path planning algorithm with performance bounds”. In: 2011 IEEE/RSJ International Confer - ence on Intelligent Robots and Systems . IEEE. 2011, pp. 3260–3267. [18] Guni Sharon et al. “Conflict-based search for optimal multi-agent pathfinding”. In: Artificial Intellig ence 219 (2015), pp. 40–66. [19] Max Barer et al. “Suboptimal variants of the conflict- based search algorithm for the multi-agent pathfinding problem”. In: Seventh Annual Symposium on Combi- natorial Sear ch . 2014. [20] Michael Zettler and J ¨ urgen Garloff. “Robustness anal- ysis of polynomials with polynomial parameter depen- dency using Bernstein expansion”. In: IEEE T ransac- tions on A utomatic Contr ol 43.3 (1998), pp. 425–431. [21] Mark W Mueller, Markus Hehn, and Raff aello D’Andrea. “A computationally efficient motion prim- itiv e for quadrocopter trajectory generation”. In: IEEE T ransactions on Robotics 31.6 (2015), pp. 1294–1310. [22] Armin Hornung et al. “OctoMap: An efficient proba- bilistic 3D mapping framew ork based on octrees”. In: Autonomous r obots 34.3 (2013), pp. 189–206. [23] Boris Lau, Christoph Sprunk, and W olfram Burgard. “Efficient grid-based spatial representations for robot navigation in dynamic en vironments”. In: Robotics and Autonomous Systems 61.10 (2013), pp. 1116– 1130. [24] ILOG CPLEX. 12.7. 0 User’s Manual . 2016. [25] James A Preiss et al. “Crazyswarm: A large nano- quadcopter swarm”. In: Robotics and Automation (ICRA), 2017 IEEE International Confer ence on . IEEE. 2017, pp. 3299–3304. AC K N OW L E D G M E N T This material is based upon work supported by the Min- istry of Trade, Industry & Energy(MO TIE, Korea) under Industrial T echnology Innovation Program. No.10067206, ‘Dev elopment of Disaster Response Robot System for Life- saving and Supporting Fire Fighters at Complex Disaster En vironment’ This work was supported by the Robotics Core T echnol- ogy Dev elopment Project (10080301) funded by the Ministry of T rade, Industry and Energy (MoTIE, K orea)