Online Distributed Trajectory Planning for Quadrotor Swarm with Feasibility Guarantee using Linear Safe Corridor

To achieve online planning, many recent works adopt discretization method, which apply the constraints only at some discrete points, not the whole trajectory [1, 2, 3, 4]. However, this approach can occur the collision if the interval between discrete points is too long. Moreover, most of the works utilize slack variables or soft constraints to avoid the infeasible optimization problem, which increases the probability of collisions. A velocity obstacle (VO)-based method [8, 9] can be considered, but they can cause a collision either since the agents do not follow the constant velocity assumption. On the contrary, the proposed algorithm guarantees collision avoidance for all trajectory points, and it ensures feasibility of the optimization problem without the slack variables or soft constraints.

There are several works that can guarantee both collision avoidance and feasibility. In [6], buffered Voronoi cell (BVC) is presented to confine the agents to a safe region, and in [10], a hybrid braking controller is applied for safety if the planner faces feasibility issues. However, these methods require conservative collision constraints. On the other hand, the proposed method employs less conservative constraints because it constructs the LSC considering the previous trajectories. The authors of [11] suggest an asynchronous planning method using a collision check-recheck scheme. This method guarantees feasibility by executing only verified trajectories through communication. However, it often takes over few seconds for replanning since it blocks the update if it receives the other agent’s trajectories during the planning. Conversely, the proposed method uses a synchronized replanning method, which takes constant time to update trajectories.

Compared to our previous works [7, 12], the proposed method does not require a centralized grid-based path planner or slack variables for feasibility. Thus, the proposed algorithm guarantees inter-agent collision avoidance without soft constraints while enabling online distributed planning.

A deadlock can occur if the agents are crowded in a narrow space without a centralized coordinator. One approach to solve this is a right-hand rule [13], which rotates the goal to the right when the agent detects the deadlock. However, it often causes another deadlock in cluttered environments. In [14], the agents are sequentially asked to plan an alternative trajectory until the deadlock is resolved. However, it requires an additional procedure to decide which robot to yield. On the other hand, the proposed method does not require such a decision process because it determines the priority by the distance to the goal. The authors of [15] present a cooperative strategy based on bidding to resolve the deadlock. However, this method takes more time for replanning as the number of agents increases because it updates only the trajectory of the winning bidder. A similar approaches to our proposed method can be found in [16]. It utilizes the grid-based planner to avoid conflict between agents. However, it often fails to find the discrete path in a compact space because it treats all other agents as static obstacles. To solve this, the proposed method imposes the priority on the agents so that the agents ignore the lower-priority agents while planning the discrete path.

We assume that all agents start their mission with the collision-free initial configuration and they can follow the trajectory accurately. We suppose that all agents can share their current positions, goal points and the trajectories planned in the previous step without delay or loss of communication. We also assume that the free space of the environment is given as prior knowledge.

Due to the differential flatness of quadrotor dynamics [17], we represent the trajectory of agent $i$ at the replanning step $k$, $\textbf{p}^{i}_{k}(t)$, with the piecewise Bernstein polynomial:

where $n$ is degree of polynomial, $\textbf{c}^{i}_{k,m,l}\in\mathbb{R}^{3}$ is the $l^{th}$ control point of the $m^{th}$ segment of the trajectory, $b_{l,n}(t)$ is Bernstein basis polynomials, $T_{k}$ is the start time of the replanning step $k$, $T_{k+m}=T_{k}+m\Delta t$, $\Delta t$ is duration of each segment, and $\tau(t,k,m)=(t-T_{k+m-1})/\Delta t$. Thus, the control points of the piecewise polynomial, $\textbf{c}^{i}_{k,m,l}$ for $m=1,\cdots,M$ and $l=0,\cdots,n$, are the decision variables of our optimization problem. Here, the Bernstein basis can be replaced with another one that satisfies the convex hull property, such as [18]. In this paper, we denote the $m^{th}$ segment of the trajectory as $\textbf{p}^{i}_{k,m}(t)$ and the vector that contains all control points of the trajectory as $\textbf{c}^{i}_{k}\in\mathbb{R}^{3M(n+1)}$.

The trajectory should satisfy the initial condition to match the agent’s current position, velocity and acceleration. Also, the trajectory should be continuous up to the acceleration for smooth flight. Due to the dynamical limit of quadrotor, the agents should not exceed their maximum velocity and acceleration. We can formulate these constraints in affine equality and inequality constraints using the convex hull property of the Bernstein polynomials [19]:

We utilize the trajectory at the previous replanning step, $\textbf{p}^{i}_{k-1}(t)$, as an initial trajectory. If it is the first step of the planning, we then use the current position instead. Suppose that the replanning period is equal to the segment time $\Delta t$. Then we can represent the initial trajectory as piecewise Bernstein polynomial.

where $\hat{\textbf{p}}^{i}_{k}(t)$ is the initial trajectory at the replanning step $k$. We denote the $m^{th}$ segment of the initial trajectory as $\hat{\textbf{p}}^{i}_{k,m}(t)$ and the $l^{th}$ control point of this segment as $\hat{\textbf{c}}^{i}_{k,m,l}$. Note that the control point of initial trajectory can be represented using the previous ones (e.g. $\hat{\textbf{c}}^{i}_{k,m,l}=\textbf{c}^{i}_{k-1,m+1,l}$ for $\forall m<M,l$) and there is no collision between initial trajectories if there is no collision between the previous ones.

A safe flight corridor (SFC), $\mathcal{S}^{i}_{k,m}$, is a convex set that prevents the collision with the static obstacles:

To guarantee the feasibility of the constraints, we construct the SFC using the initial trajectory and the SFC of the previous step.

where $\mathcal{S}(\hat{\textbf{c}}^{i}_{k,M,n})$ is a convex set that contains the point $\hat{\textbf{c}}^{i}_{k,M,n}$ and satisfies $\mathcal{S}(\hat{\textbf{c}}^{i}_{k,M,n})\oplus\mathcal{C}^{i,o}\subset\mathcal{F}$. Note that the SFC in (17) always exists for all replanning steps if the control points of the initial trajectory do not collide with static obstacles. We can find this using the axis-search method [7].

Assume that the control points of $\textbf{p}^{i}_{k,m}$ is confined in the corresponding SFC, i.e. for $m=1,\cdots,M$ and $l=0,\cdots,n$:

Then, there is no collision with static obstacles due to the convex hull property of the Bernstein polynomial [20]. Also, we can prove that the control points of the initial trajectory always belong to the corresponding SFC for all replanning steps.

We define the LSC as a linear constraint that satisfies the following conditions:

where $\textbf{n}^{i,j}_{m}\in\mathbb{R}^{3}$ is the normal vector, $d^{i,j}_{m,l}$ is the safety margin and $\max\langle\mathcal{C}^{i,j},\textbf{n}^{i,j}_{m}\rangle=\max_{\textbf{x}\in\mathcal{C}^{i,j}}\textbf{x}\cdot\textbf{n}^{i,j}_{m}$. Let $\hat{\mathcal{H}}^{i,j}_{k,m}$ is the convex hull of the control points of relative initial trajectory, i.e. $\hat{\mathcal{H}}^{i,j}_{k,m}=\text{Conv}(\{\hat{\textbf{c}}^{i}_{k,m,l}-\hat{\textbf{c}}^{j}_{k,m,l}\mid l=0,\cdots,n\})$. Lemmas 2 and 3 present the main properties of LSC.

We construct the LSC based on Lemma 3. First, we perform the coordinate transformation to convert the collision model to the sphere as shown in Fig. 2. Next, we conduct the GJK algorithm [22] to find the closest points between $\hat{\mathcal{H}}^{i,j}_{k,m}$ and the collision model. Then, we determine the normal vector utilizing the vector crossing the closest points and compute the safety margin using (29). Finally, we obtain the LSC by reversing the coordinate transform.

We compare the LSC with the BVC [6] in Fig. 3. Since the BVC is generated using the agent’s current position only, the desired trajectories from the BVC remain within each static cell, which leads to a conservative maneuver as shown in Fig. 3(a). On the other hand, we construct the LSC utilizing the full trajectory at the previous step. Thus, we can obtain more aggressive maneuver while ensuring collision avoidance, as depicted in Fig. 3(b).

In this work, we adopt the priority-based goal planning to prevent the deadlock in cluttered environments. Alg. 2 describes the proposed goal planning method. We give the higher priority to the agent that has a smaller distance to the goal (30). However, we assign the lowest priority to the goal-reached agent to prevent them from blocking other agent’s path (31). If an agent moves away from the current agent, then we do not consider the priority of that agent for smooth traffic flow (32) (lines 1-6).

Next, we find the nearest agent among the higher priority agents. If the distance to the nearest agent is less than the threshold $d_{th}$, then we assign the goal to increase the distance to the closest agent (lines 7-9). It is necessary to prevent agents from clumping together. If there is no such agent, we search a discrete path $\mathcal{D}$ using a grid-based path planner. Here, the higher-priority agent is considered as a static obstacle. If the planner fails to find a solution, then we retry to find the path without considering the higher-priority agent (line 11). Finally, we find the LOS-free goal in the discrete path (line 12). The LOS-free goal means that the line segment between the goal and the agent’s current position does not collide with higher priority agents and static obstacles.

Fig. 4 describes the mechanism of the Alg. 2. In the left figure, the agent $j$ has lower priority because it is moving away from the agent $i$. Due to that, the agent $i$ ignores the agent $j$ when planning the discrete path. On the other hand, the agent $j$ in the right figure has higher priority because it moves to the agent $i$ and has a smaller distance to the goal. Thus, the agent $i$ finds the LOS-free goal in the discrete path that detours the agent $j$ and static obstacles.

We reformulate the distributed trajectory optimization problem as a quadratic programming (QP) problem using the SFC and LSC:

Here, we add the last constraint to guarantee dynamic feasibility of the initial trajectory. Note that the constraints in (33) guarantees collision-free due to the SFC and Lemma 2. Furthermore, Thm. 2 shows that this problem consists of feasible constraints. Therefore, we can obtain a feasible solution from Alg. 1 for all replanning steps by using a conventional convex solver.

If we conduct the mission with actual quadrotors, the assumption used in Thm. 2 may not be satisfied due to external disturbance or tracking error. To solve this problem, we adopt event-triggered replanning proposed in [1]. This method ignores the tracking error when the error is small. In other word, it utilizes the desired state from the previous trajectory ($\textbf{p}^{i}_{k-1}(T_{k})$) as the initial condition of the current optimization problem (4). If the detected disturbance is too large, we replace the planner with our previous work [12] until the error vanished.

To validate the performance of the proposed method, we compare four online trajectory planning algorithms at the obstacle-free space: 1) DMPC (input space) [1], 2) BVC [6] with right-hand rule, 3) LSC with right-hand rule (LSC-RH), 4) LSC with priority-based goal planning (LSC-PB). We conducted the simulation with 10 to 60 quadrotors in a 3 m $\times$ 3 m $\times$ 2 m empty space. We executed 30 missions with randomly allocated start and goal points for each number of agents. When judging the collision, we use a smaller model with $r^{\forall i}=0.1$ m and $c_{dw}=2.25$ for DMPC because it uses soft constraints, while the other methods use the original one.

The top-left graph of Fig. 5 describes the success rate of each method. We observed that the collision occurred in all failure cases of DMPC even though we adopted the smaller collision model. It implies that collision constraint including the slack variable cannot guarantees the safety in a dense environment even with the bigger safety margin. On the contrary, there was no collision when we use BVC and LSC-based methods, and the deadlock was the only reason for failure. It indicates that the proposed method does not cause optimization failure as guaranteed by Thm. 2. Among the four algorithms, the LSC with priority-based goal planning method shows the highest success rate for all cases. This result shows that priority-based goal planning can solve deadlock better than the right-hand rule.

The top-right and bottom-left figures of Fig. 5 show the total flight time and flight distance per agent, respectively. The proposed method has $50\%$ less flight time and $17\%$ less flight distance on average compared to the BVC-based method. Such performance is similar to DMPC, but the proposed algorithm still guarantees collision avoidance. Compared to the right-hand rule, the priority-based goal planning costs $11\%$ more flight time and $6\%$ more flight distance, but it is acceptable considering the increase in success rate.

The bottom-right graph of Fig. 5 describes the average computation time per agent. The proposed method takes 10.5 ms for 30 agents and 15.5 ms for 60 agents. The proposed algorithm consumes similar computation time to BVC, but needs more computation time than DMPC because it involves more constraints to guarantee collision avoidance.

We compared the proposed algorithm with the EGO-Swarm [3] in two types of obstacle environment. One is a random forest consisting of 10 static obstacles. We deployed 20 agents in a circle with 4 m radius and 1 m height, and we assigned the goal points to antipodes of the start points, as shown in Fig. 6(a). The other is an indoor space with the dimension 10 m $\times$ 15 m $\times$ 2.5 m. For a fair comparison, we increased the sensor range of EGO-Swarm enough to recognize all obstacles. We randomly assigned the start and goal points among the 20 predetermined points as depicted in Fig. 6(b). We executed 30 simulations for each environment changing the obstacle’s position (random forest) or the start and goal points (indoor space).

Table I shows the planning result of both methods in cluttered environments. Ego-Swarm shows faster computation speed in the random forest because it optimizes the trajectory without any hard constraints. However, the success rate of this method is $50\%$ success rate, indicating that EGO-Swarm cannot guarantee collision avoidance. On the other hand, the proposed method shows a perfect success rate and has similar flight time and distance compared to EGO-Swarm. This result shows that the LSC is as efficient as the soft constraint of EGO-Swarm but still guarantees collision avoidance in obstacle environments.

Unlike the random forest, EGO-Swarm fails to find the trajectory to the goal points in the indoor space. It is because EGO-Swarm cannot find the topological trajectory detouring the wall-shaped obstacle. On the other hand, the proposed method reaches the goal for all cases due to the priority-based goal planning.

We conducted the flight test with ten Crazyflie 2.0 quadrotors in the maze-like environment depicted in Fig. 1. We confined the quadrotors in 10 m x 6 m x 1 m space to prevent them from bypassing the obstacle region completely. We assign the quadrotors a mission to patrol the two waypoints to validate the operability of the proposed algorithm. We sequentially computed the trajectories for all agents in a single laptop, and we used the Crazyswarm [26] to broadcast the control command to quadrotors. We utilized the Optitrack motion capture system to observe the agent’s position at 120 Hz. The full flight is presented in the supplemental video.

The flight test was conducted for around 70 seconds, and there was no huge disturbance triggering the planner change. We accumulated the history of the smallest inter-agent distance and agent-to-obstacle distance in Fig. 7(a) and 7(b) respectively. Fig. 7(a) shows that the agents invade desired safe distance for some sample times due to the tracking error. However, there was no physical collision during the entire flight. The computation time per agent is reported in Fig. 7(c), and the average computation time was 6.8 ms.