Decentralized Motion Planning with Collision Avoidance for a Team of UAVs under High Level Goals

Multi-agent systems have received a significant amount of attention over the last decades. The complexity of many tasks, such as assembling parts and heavy/large object transportation or manipulation, necessitates the employment of a group of robots, rather than a single one, since it offers greater versatility, redundancy and fault tolerance.

In the case of aerial vehicles, tasks involving area coverage/inspection or rescue missions point out the importance of using multi-agent setups. The standard problem of formation control for a team of aerial vehicles is addressed in [1, 2, 3, 4, 5, 6], whereas [7, 8, 9, 10, 11] consider leader-follower formation approaches, where the latter also treats the problem of collision avoidance with static obstacles in the environment; [12], [13] and [14] employ dynamic programming, Model Predictive Control and reachable set algorithms, respectively, for inter-agent collision avoidance. In [15] the cooperative evader pursuit problem is treated.

Ultimately, however, we would like the aerial robots to execute more complex high-level tasks, involving combinations of safety ("never enter a dangerous regions"), surveillance ("keep visiting regions $A$ and $B$ infinitely often") or sequencing ("collect data in region $C$ and upload it in region $D$") properties. Temporal logic languages offer a means to express the aforementioned specifications, since they can describe complex planning objectives in a more efficient way than the well-studied navigation algorithms. Recently, the incorporation of temporal logic-based planning to the robotics and automation field has gained a considerable amount of attention, both in single- and multi-agent setups [16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27]. Regarding aerial vehicles, [28] addresses the vehicle routing problem using MTL specifications and [29] approaches the LTL motion planning using MILP optimization techniques, both in a centralized manner. Markov Decision Processes are used for the LTL planning in [30]. The aforementioned works, however, consider discrete agent models and do not take into account their continuous dynamics.

Another important feature in multi-agent planning and control is the need for decentralization and minimization of inter-agent communication; centralized approaches, where a central system computes the overall team plan or cases where the agents communicate online with each other, can cause computationally expensive procedures, even for small robot teams. In the case of temporal logics, the use of product transition systems incorporating the states of all agents (as e.g., in [25, 17, 30]) can render the solution to the motion planning problem practically infeasible.

Moreover, the majority of works in the related literature of temporal logic-based motion planning considers point-agents (as, e.g. in [19, 24, 23]) and does not take into account potential collisions between them. The latter is a crucial safety property in real-time scenarios, where actual vehicles are used in the motion planning framework.

In this work, we propose a novel decentralized control protocol for the motion planning of a team of aerial vehicles under LTL specifications with simultaneous inter-agent collision avoidance. In particular, we extend previous work on decentralized navigation functions [31] to abstract the motion of each agent as a finite transition system. Then, we employ standard formal-verification techniques to derive plans that satisfy each agent’s LTL specification. The proposed control protocol is decentralized in the sense that each agent has limited sensing information and derives and executes its desired path without communicating with the other agents or knowing their respective goals/specifications. Simulation and experimental results with quadrotors verify the effectiveness of the proposed framework. To the best of the authors’ knowledge, this is the first approach that integrates temporal logic-based motion planning with decentralized navigation functions in an experimental framework with UAVs.

The rest of the paper is organized as follows: Section II introduces notation and preliminary background. Section III describes the problem formulation and the overall system’s model. The control strategy is presented in Section IV. Sections V and VI verify our approach through numerical simulations and experiments, respectively, and Section VII concludes the paper.

Vectors and matrices are denoted with bold lowercase and uppercase letters, respectively, whereas scalars are denoted with non-bold letters. The set of positive integers is denoted as $\mathbb{N}$ and the real $n$-coordinate space, with $n\in\mathbb{N}$, as $\mathbb{R}^{n}$; $\mathbb{R}^{n}_{\geq 0}$ is the set of real $n$-vectors will all elements nonnegative; $\mathcal{B}_{r}(\boldsymbol{c})$ denotes the ball of radius $r\geq 0$ and center $\boldsymbol{c}\in\mathbb{R}^{3}$; Moreover, given a set $A$, the notation $\mathring{A}$ denotes the interior of $A$, $2^{A}$ is the set of all subsets of $A$ and, given a finite sequence $a_{1},\dots,a_{n}$ of elements in $A$, with $n\in\mathbb{N}$, we denote by $(a_{1},\dots,a_{n})^{\omega}$ the infinite sequence $a_{1},\dots,a_{n}a_{1},\dots,a_{n}\dots$ created by repeating $a_{1},\dots,a_{n}$. Finally, $d_{n}:\mathbb{R}^{n}\times\mathbb{R}^{n}\rightarrow\mathbb{R}_{\geq 0}$ is the $n$-dimensional Euclidean distance, with $n\in\mathbb{N}$.

Consider $N$ aerial agents operating in a static workspace that is bounded by a large sphere in $3$-D space $\mathcal{W}=\mathcal{B}_{r_{0}}(\boldsymbol{p_{0}})=\{\boldsymbol{p}\in\mathbb{R}^{3}\text{ s.t. }\lVert\boldsymbol{p}-\boldsymbol{p_{0}}\rVert\leq r_{0}\}$, where $\boldsymbol{p_{0}}\in\mathbb{R}^{3}$ and $r_{0}>0$ are the center and radius of $\mathcal{W}$. Within $\mathcal{W}$ there exist $K$ smaller spheres around points of interest, which are described by $\mathcal{\pi}_{k}=\mathcal{B}_{r_{\pi_{k}}}(\boldsymbol{p_{\pi_{k}}})=\{\boldsymbol{p}\in\mathbb{R}^{3}\text{ s.t. }\lVert\boldsymbol{p}-\boldsymbol{p_{\pi_{k}}}\rVert\leq r_{\pi_{k}}\}\subset\mathcal{W}$, where $\boldsymbol{p_{\pi_{k}}}\in\mathbb{R}^{3},r_{\pi_{k}}>0$ are the central point and radius, respectively, of $\pi_{k}$. We denote the set of all $\pi_{k}$ as $\Pi=\{\pi_{1},\dots,\pi_{K}\}$. For the workspace partition to be valid, we consider that the regions of interest are sufficiently distant from each other and from the workspace boundary, i.e., $d_{3}(\boldsymbol{p_{\pi_{k}}},\boldsymbol{p_{\pi_{k^{\prime}}}})>4\max_{k\in\{1,\dots,K\}}(r_{\pi_{k}})$ and $d_{3}(\boldsymbol{p_{\pi_{k}}},\boldsymbol{p_{0}})<r_{0}-3r_{\pi_{k}},\forall k,k^{\prime}\in\{1,\dots,K\}$ with $k\neq k^{\prime}$. Moreover, we introduce a set of atomic propositions $\Psi_{i}$ for each agent $i\in\{1,\dots,N\}$ that indicates certain properties of interest of agent $i$ in $\Pi$ and are expressed as boolean variables. The properties satisfied at each region $\pi_{k}$ are provided by the labeling function $\mathcal{L}_{i}:\Pi\rightarrow 2^{\Psi_{i}}$, which assigns to each region $\pi_{k},k\in\{1,\dots,K\}$ the subset of the atomic propositions $\Psi_{i}$ that are true in that region.

To demonstrate the efficiency of the proposed algorithm, we consider $N=3$ aerial vehicles $\mathcal{B}_{r_{i}}(\boldsymbol{p_{i}}(t))$, with $r_{i}=0.3$m, $d_{s_{i}}=0.65$m, $\forall i=\{1,2,3\}$, operating in a workspace $\mathcal{W}=\mathcal{B}_{r_{0}}(\boldsymbol{p_{0}})$ with $r_{0}=10$m and $\boldsymbol{p_{0}}=[0,0,0]^{T}$m. Moreover, we consider $K=5$ spherical regions of interest $\mathcal{B}_{r_{\pi_{k}}}(\boldsymbol{p_{\pi_{k}}})$ with $r_{\pi_{k}}=0.4$m, $\forall k=\{1,\dots,5\}$ and $\boldsymbol{p}_{\pi_{1}}=[0,0,2]^{T}$m, $\boldsymbol{p}_{\pi_{2}}=[1,-9,5]^{T}$m, $\boldsymbol{p}_{\pi_{3}}=[-8,-1,4]^{T}$m, $\boldsymbol{p}_{\pi_{4}}=[2,7,-2]^{T}$m and $\boldsymbol{p}_{\pi_{5}}=[7.5,2,-3]^{T}$m. The initial configurations of the agents are taken as $\boldsymbol{p_{1}}(0)=\boldsymbol{p}_{\pi_{1}},\boldsymbol{p_{2}}(0)=\boldsymbol{p}_{\pi_{3}},\boldsymbol{p_{3}}(0)=\boldsymbol{p}_{\pi_{4}}$ and therefore, $\mathcal{A}_{1}(\boldsymbol{p_{1}}(0))\in\pi_{1},\mathcal{A}_{2}(\boldsymbol{p_{2}}(0))\in\pi_{3}$ and $\mathcal{A}_{3}(\boldsymbol{p_{3}}(0))\in\pi_{4}$. An illustration of the described workspace is depicted in Fig. 2.

We consider that agent $2$ is assigned with inspection tasks and has the atomic propositions $\Psi_{2}=\{``\text{ins}_{\text{a}}",``\text{ins}_{\text{b}}",``\text{ins}_{\text{c}}",``\text{ins}_{\text{d}}",``\text{obs}"\}$ with $\mathcal{L}_{2}(\pi_{1})=\{``\text{obs}"\},\mathcal{L}_{2}(\pi_{2})=\{``\text{ins}_{\text{a}}"\},\mathcal{L}_{2}(\pi_{3})=\{``\text{ins}_{\text{b}}"\},\mathcal{L}_{2}(\pi_{4})=\{``\text{ins}_{\text{c}}"\}$ and $\mathcal{L}_{2}(\pi_{5})=\{``\text{ins}_{\text{d}}"\}$, where we have considered that region $\pi_{1}$ is an undesired ("obstacle") region for this agent. More specifically, the task for agent $2$ is the continuous inspection of the workspace while avoiding region $\pi_{1}$. The corresponding LTL specification is $\phi_{2}=(\square\neg``\text{obs}")\land\square(\lozenge``\text{ins}_{\text{a}}"\land\lozenge``\text{ins}_{\text{b}}"\land\lozenge``\text{ins}_{\text{c}}"\land\lozenge``\text{ins}_{\text{d}}")$. Agents $1$ and $3$ are interested in moving around resources scattered in the workspace and have propositions $\Psi_{1}=\Psi_{3}=\{``\text{res}_{\text{a}}",``\text{res}_{\text{b}}",``\text{res}_{\text{c}}",``\text{res}_{\text{d}}",``\text{res}_{\text{e}}"\}$ with $\mathcal{L}_{1}(\pi_{1})=\mathcal{L}_{3}(\pi_{1})=\{\text{res}_{\text{a}}\},\mathcal{L}_{1}(\pi_{2})=\mathcal{L}_{3}(\pi_{2})=\{\text{res}_{\text{b}}\},\mathcal{L}_{1}(\pi_{3})=\mathcal{L}_{3}(\pi_{3})=\{\text{res}_{\text{c}}\},\mathcal{L}_{1}(\pi_{4})=\mathcal{L}_{3}(\pi_{4})=\{\text{res}_{\text{d}}\}$ and $\mathcal{L}_{1}(\pi_{5})=\mathcal{L}_{3}(\pi_{5})=\{\text{res}_{\text{e}}\}$. We assume that $``\text{res}_{\text{a}}"$ is shared between the two agents whereas $``\text{res}_{\text{b}}"$ and $``\text{res}_{\text{e}}"$ have to be accessed only by agent $1$ and $``\text{res}_{\text{c}}"$ and $``\text{res}_{\text{d}}"$ only by agent $3$. The corresponding specifications are $\phi_{1}=\square\neg(``\text{res}_{\text{c}}"\lor``\text{res}_{\text{d}}")\land\square\lozenge(``\text{res}_{\text{a}}"\bigcirc``\text{res}_{\text{e}}"\bigcirc``\text{res}_{\text{b}}")$ and $\phi_{3}=\square\neg(``\text{res}_{\text{b}}"\lor``\text{res}_{\text{e}}")\land\square\lozenge(``\text{res}_{\text{a}}"\bigcirc``\text{res}_{\text{c}}"\bigcirc``\text{res}_{\text{d}}")$, where we have also included a specific order for the access of the resources. Next, we employ the off-the-shelf tool LTL2BA [34] to create the Büchi automata $\mathcal{C}_{i},i=\{1,2,3\}$ and by following the procedure described in Section IV-B, we derive the paths $p_{1}=(\pi_{1}\pi_{5}\pi_{2})^{\omega},p_{2}=(\pi_{3}\pi_{2}\pi_{5}\pi_{4})^{\omega},p_{3}=(\pi_{4}\pi_{1}\pi_{3})^{\omega}$, whose execution satisfies $\phi_{1},\phi_{2},\phi_{3}$. Regarding the continuous control protocol, we chose $k_{g_{i}}=15,\lambda_{i}=5,\forall i\in\{1,2,3\}$ in (4), (6) and the switching duration in (8) was calculated online as $\nu_{i}=0.1t^{\prime}_{i,k}$, where we assume that the large distance between the regions $\pi_{k}$ (see Fig. 2) implies that $t^{\scriptscriptstyle f}_{i,k^{\prime}}>1.1t^{\prime}_{i,k}$ and thus, $\nu_{i}<t^{\scriptscriptstyle f}_{i,k^{\prime}}-t^{\prime}_{i,k}$. The simulation results are depicted in Fig. 3 and 5. In particular, Fig. 5 illustrates the execution of the paths $(\pi_{1}\pi_{5}\pi_{2})^{2}\pi_{1},(\pi_{3}\pi_{2}\pi_{5}\pi_{4})^{2}\pi_{3}\pi_{2}\pi_{5}$ and $(\pi_{4}\pi_{1}\pi_{3})^{2}\pi_{4}$ by agents $1,2$ and $3$ respectively, where the superscript $2$ here denotes that the corresponding paths are executed twice. Fig. 3 depicts the resulting control inputs $u_{i},\forall i\in\{1,2,3\}$. The figures demonstrate the successful execution of the agents’ paths and therefore, satisfaction of the respective formulas with inter-agent collision avoidance.

The validity and efficiency of the proposed solution was also verified through real-time experiments. The experimental setup involved two remotely controlled IRIS+ quadrotors from $3$D Robotics, which we consider to have sensing range $d_{s_{i}}=0.65$m, upper control input bound $\lvert\boldsymbol{u}_{m}\rvert\leq 1$m/s, $m\in\{x,y,z\}$, and bounding spheres with radius $r_{i}=0.3$m, $\forall i\in\{1,2\}$. We considered two $2$-dimensional scenarios in a workspace $\mathcal{W}=\{\boldsymbol{p}\in\mathbb{R}^{2}\text{ s.t. }\lVert\boldsymbol{p}\rVert\leq 2.5\text{m}\}$, i.e. $\boldsymbol{p_{0}}=[0,0]^{T}$ and $r_{0}=2.5$m.

The first scenario included $4$ regions of interest $\Pi=\{\pi_{1},\dots,\pi_{4}\}$ in $\mathcal{W}$, with $r_{\pi_{k}}=0.4,\forall k\in\{1,\dots,4\}$ and $\boldsymbol{p_{\pi_{1}}}=[0,0]^{T}$m, $\boldsymbol{p_{\pi_{2}}}=[-1,0]^{T}$m, $\boldsymbol{p_{\pi_{3}}}=[0,1.25]^{T}$m and $\boldsymbol{p_{\pi_{4}}}=[0.8,-0.7]^{T}$m. The initial positions of the agents were taken such that $\mathcal{A}_{1}(\boldsymbol{p_{1}}(0))\in\pi_{2}$ and $\mathcal{A}_{2}(\boldsymbol{p_{2}}(0))\in\pi_{4}$ (see Fig. 4). We also defined the atomic propositions $\Psi_{1}=\Psi_{2}=\{``\text{obs}",``a",``b",``c"\}$ with $L_{1}(\pi_{1})=L_{2}(\pi_{1})=\{``\text{obs}"\},L_{1}(\pi_{2})=L_{2}(\pi_{2})=\{``a"\},L_{1}(\pi_{3})=L_{2}(\pi_{3})=\{``b"\},L_{1}(\pi_{4})=L_{2}(\pi_{4})=\{``c"\}$. In this scenario, we were interested in area inspection while avoiding the "obstacle" region, and thus, we defined the individual specifications with the following LTL formulas: $\phi_{1}=\phi_{2}=\square\neg``\text{obs}"\land\square\lozenge(``a"\bigcirc``c"\bigcirc``b")$. By following the procedure described in Section IV-B, we obtained the paths $p_{1}=(\pi_{2}\pi_{4}\pi_{3})^{\omega},p_{2}=(\pi_{4}\pi_{2}\pi_{3})^{\omega}$. Fig. 6 depicts the execution of the paths $(\pi_{2}\pi_{4}\pi_{3})^{1}$ and $(\pi_{4}\pi_{2}\pi_{3})^{1}$ by agents $1$ and $2$, respectively, and Fig. 7 shows the corresponding input signals, which do not exceed the control bounds $1$m/s. It can be deduced by the figures that the agents successfully satisfy their individual formulas, without colliding with each other.