Asynchronous Spatial-Temporal Allocation for Trajectory Planning of Heterogeneous Multi-Agent Systems

For agent $i\in\mathcal{N}$, its state and control input at time $t$ are denoted as $x^{i}(t)\in\mathbb{R}^{m}$ and $u^{i}(t)\in\mathbb{R}^{q}$, $m,q\in\mathbb{Z}^{+}$, respectively. Its dynamic model is described as follows:

where $\mathcal{X}^{i}$ and $\mathcal{U}^{i}$ denote the set of available states and control inputs, respectively; $f^{i}(\bullet)$ reflects the differential constraint.

Note that the dynamic models of the agents in (1) are heterogeneous, which include the commonly-encountered double integrator, unicycle and bicycle as special cases, elaborated as follows:

• •

  Double integrator: $x=[p_{x},p_{y},v_{x},v_{y}]^{\mathrm{T}}$, $u=[a_{x},a_{y}]^{\mathrm{T}}$, $f(x,u)=[v_{x},v_{y},a_{x},a_{y}]^{\mathrm{T}}$, where $\mathcal{X}=\left\{x\in\mathbb{R}^{4}\ |\ |v_{x}|,|v_{y}|\leq v_{\text{max}}\right\}$, $\mathcal{U}=\left\{u\in\mathbb{R}^{2}\ |\ |a_{x}|,|a_{y}|\leq a_{\text{max}}\right\}$;

• •

  Unicycle: $x=[p_{x},p_{y},\theta,v]^{\mathrm{T}}$, $u=[a,\omega]^{\mathrm{T}}$, $f(x,u)=[v\cos\theta,v\sin\theta,\omega,a]^{\mathrm{T}}$, where $\mathcal{X}=\left\{x\in\mathbb{R}^{4}\ |\ 0\leq v\leq v_{\text{max}}\right\}$, $\mathcal{U}=\left\{u\in\mathbb{R}^{2}\ |\ |\omega|\leq\omega_{\text{max}},|a|\leq a_{\text{max}}\right\}$;

• •

  Bicycle: $x=[p_{x},p_{y},\theta,v]^{\mathrm{T}}$, $u=[a,\delta]^{\mathrm{T}}$, $f(x,u)=[v\cos\theta,v\sin\theta,v\tan\delta/L,a]^{\mathrm{T}}$, where $L\in\mathbb{R}^{+}$, $\mathcal{X}=\left\{x\in\mathbb{R}^{4}\ |\ 0\leq v\leq v_{\text{max}}\right\}$, $\mathcal{U}=\left\{u\in\mathbb{R}^{2}\ |\ |\delta|\leq\delta_{\text{max}},|a|\leq a_{\text{max}}\right\}$;

in which $p$, $v$, $a$, $\theta$, $\omega$, $\delta$ and $L$ represent the agent’s position, linear velocity, acceleration, heading angle, angular velocity, steering angle and wheelbase, and the subscripts $x$, $y$ denote the $x$, $y$ coordinates, respectively.

In order to characterize the inter-agent collision avoidance, we first define the representation of the agents’ trajectories as follows:

At any time $t>0$, for any pair of agents $i$ and $j$, they are collision-free if and only if $S^{i}\left(x^{i}(t)\right)\cap S^{j}\left(x^{j}(t)\right)=\emptyset$. Accordingly, their trajectories are collision-free if and only if $\mathcal{T}raj^{i}(0,+\infty)\cap\mathcal{T}raj^{j}(0,+\infty)=\emptyset$.

In this work, the trajectory planning is carried out in a receding horizon way, wherein $x_{n^{i}}^{i}(t)$ is the planned state at $t$ in agent $i$’s $n^{i}$-th replanning. Additionally, the planning horizon of the $n^{i}$-th replanning is denoted as $T_{n^{i}}^{i}\in\mathbb{R}^{+}$. The time when agent $i$ finishes its $n^{i}$-th replanning step is denoted as $t_{n^{i}}^{i}$, which is also the starting time of the $n^{i}$-th trajectory. The $n^{i}$-th replanned trajectory is denoted as $\mathcal{T}raj_{n^{i}}^{i}(t_{n^{i}}^{i},+\infty)$. Particularly, for the time beyond the planning horizon, i.e., $t>t_{n^{i}}^{i}+T_{n^{i}}^{i}$, we enforce $x_{n^{i}}^{i}(t)=x_{n^{i}}^{i}(t_{n^{i}}^{i}+T_{n^{i}}^{i})$.

Then, for agent $i$ in its $n^{i}$-th replanning, the trajectory generation problem can be formulated as the following optimal control problem (OCP):

where $t\in[t_{n^{i}}^{i},t_{n^{i}}^{i}+T_{n^{i}}^{i}]$; $\hat{x}^{i}(t_{n^{i}}^{i})$ is the predicted state at $t_{n^{i}}^{i}$; and $C(x_{n^{i}}^{i},u_{n^{i}}^{i})$ is the objective function, defined as

which is utilized to drive agent $i$ to its target and penalize the control inputs $u_{n^{i}}^{i}(\tau)$, where $\delta x_{n^{i}}^{i}(\tau)=x_{n^{i}}^{i}(\tau)-x^{i}_{\text{target}}$, $Q$ and $P$ are the weighted matrices. In the ideal case, the lower-level controller is assumed to perfectly follow the planned trajectory such that $\hat{x}^{i}(t_{n^{i}}^{i})=\mathcal{T}raj_{n^{i}-1}^{i}(t_{n^{i}}^{i})$.

Note that the collision avoidance constraints (2b) are impossible to be directly established at the replanning stage, since the exact future motions of other agents are unavailable. To solve this problem, we propose a method to dynamically reformulate constraints (2b) such that the OCP (2) can be formulated and solved in an asynchronous manner while the inter-agent collision is theoretically avoided.

STA between agents $i$ and $j$ aims to allocate the time-stamped feasible region for replanning, so as to avoid collision between agents. The definition of STA is given below.

Since STA is updated along with the replanning process, we use $\mathbb{A}_{m}^{ij}$ to represent the allocation after the $({m-1})$-th update in the sequel.

Allocation update is a core part in our method which keeps the collision avoidance constraints up-to-date. For agent $i$, there is a waiting time $T_{w}^{i}$ between two consecutive trajectory calculation times $T_{c}^{i}$ (see Fig. 3). In this work, we particularly enforce that $T_{w}^{i}>T_{c}^{i}$, in order to ensure a lower bound of the updating frequency.

At time $t^{i}_{n^{i}-1}$ when the $(n^{i}-1)$-th replanning step is finished, the newly planned trajectory $\mathcal{T}raj_{n^{i}-1}^{i}$ is broadcast to other waiting agents. Specifically, if agent $j$ stays at waiting time after its $n^{j}$-th replanning, it will reach a renewal with agent $i$ based on $\mathcal{T}raj_{n^{j}}^{j}$ and $\mathcal{T}raj_{n^{i}-1}^{i}$. Fig. 3 provides an illustration. During the waiting time, agent $i$ can also receive other agents’ data and reach renewals. A renewal is defined as follows:

where $t_{s}^{R_{m}}\triangleq\max\left\{t_{(n^{i}-1)+1}^{i},t_{n^{j}+1}^{j}\right\}$ is the start time of the $m$-th renewal. Moreover, a part of this renewal from $t_{a}$ to $t_{b}$ is denoted as $\mathcal{R}_{m}^{ij}(t_{a},t_{b})\triangleq\{\mathcal{H}^{ij}(t)\times t\ |\ t\in[t_{a},t_{b})\}$.

The process that produces a renewal via $\mathcal{T}raj_{n^{i}-1}^{i}$ and $\mathcal{T}raj_{n^{j}}^{j}$ is to generate hyperplane in order to split the space into two halves $\mathcal{H}^{ij}(t)$ and $\mathcal{H}^{ji}(t)$ based on $S^{i}(x_{n^{i}-1}^{i}(t))$ and $S^{j}(x_{n^{j}}^{j}(t))$ for $t\in[t_{s}^{R_{m}},+\infty)$. According to the separating hyperplane theorem [24], if $S^{i}(x_{n^{i}-1}^{i}(t))\cap S^{j}(x_{n^{j}}^{j}(t))=\emptyset$, then we can always find a hyperplane that separates those two polygons. One of the possible implementations of separating hyperplane is using GJK algorithm [25, 13].

Notably, despite the fact that the duration concerned is $[t_{s}^{R_{m}},+\infty)$ for the $m$-th renewal, only the duration $[t_{s}^{R_{m}},t_{e}^{R_{m}})$, where $t_{e}^{R_{m}}\triangleq\max\left\{t_{n^{i}-1}^{i}+T_{n^{i}-1}^{i},t_{n^{j}}^{j}+T_{n^{j}}^{j}\right\}$, needs to be considered. Because for $t>t_{e}^{R_{m}}$, $S^{i}(x_{n^{i}-1}^{i}(t))=S^{i}(x_{n^{i}-1}^{i}(t_{e}^{R_{m}}))$ and $S^{j}(x_{n^{j}}^{j}(t))=S^{j}(x_{n^{j}}^{j}(t_{e}^{R_{m}}))$ hold according to the assumption in Section II-C that for $t>t_{n^{i}}^{i}+T_{n^{i}}^{i}$, $x_{n^{i}}^{i}(t)=x_{n^{i}}^{i}(t_{n^{i}}^{i}+T_{n^{i}}^{i})$. Thus, we have $\mathcal{H}^{ij}(t)=\mathcal{H}^{ij}(t_{e}^{R_{m}})$ as well as $\mathcal{H}^{ji}(t)=\mathcal{H}^{ji}(t_{e}^{R_{m}})$ for $t>t_{e}^{R_{m}}$.

For STA, when the first renewal starts, it is initialized as $\mathbb{A}_{1}^{ij}=\mathcal{R}_{1}^{ij}(t^{R_{1}},+\infty)$. In particular, for the first renewal, we enforce $t_{s}^{R_{1}}=t^{R_{1}}$ to avoid collision during $[t^{R_{1}},t_{s}^{R_{1}}]$. Then, for a new renewal $\mathcal{R}_{m}^{ij}$, the STA is updated as follows:

where $\mathbb{A}_{m-1}^{ij}(t^{R_{1}},t_{s}^{R_{m}})=\left\{\mathcal{H}^{ij}(t)\times t\ |\ t\in[t^{R_{1}},t_{s}^{R_{m}})\right\}.$ In this updating mechanism, STA is only updated for the time beyond $t_{s}^{R_{m}}$, which can also be reflected in Fig. 3.

A pair of agents reach their renewal if and only if one of them just finishes its replanning and the other one stays at waiting time. This is a typical asynchronous method and thus called Asynchronous Spatial-Temporal Allocation.

In OCP (2), during agent $i$’s $n^{i}$-th replanning, given $\mathbb{A}_{m}^{ij}$ as the latest STA with agent $j$, the constraints (2b) can be replaced by

where $\mathcal{N}^{i}$ is the set of agents that have ever established communication with agent $i$. Based on this replacement, we further reformulate the OCP (2):

where $t\in[t_{n^{i}}^{i},t_{n^{i}}^{i}+T_{n^{i}}^{i}]$ and $t^{\prime}\in(t_{n^{i}}^{i}+T_{n^{i}}^{i},t_{e}^{R_{m}}]$. Note that the constraints (4) consider an intractably infinite time interval $t\in[t_{n^{i}}^{i},+\infty)$. We replace it with constraints (5a) and (5b) which only consider a finite horizon, thus facilitating the application. The reason why OCP (2) with constraints (4) is equal to OCP (5) can be found in the Lemma 1 of Appendix A. In real implementation, the underlying continuous OCP will be reformulated and resolved in a discrete form, which will be stated in Section IV-A.

The overall planning algorithm built up by ASTA is outlined in Alg. 1. At the beginning, each agent initializes its trajectory (Line 1), where we enforce that $x^{i}(0)\in\mathcal{X}^{i}_{e}\triangleq\left\{x\ |\ \exists u,f^{i}(x,u)=0\right\}$. Afterwards, the agent scans its communication network to find all neighbors (Line 1) and then broadcasts its trajectory via communication network (Line 1). Then, for each neighbor, it opens an independent thread to wait for other agents’ feedback (Line 1). Once receiving a confirmation signal, the allocation will be updated as in equation (3). Thereafter, the receivers regarding all neighbors are closed (Line 1) and the new trajectory is planned according to OCP (5). However, in this work, the feasibility of the OCP cannot be guaranteed, which highlights an area for our future work. To tackle this, if the given OCP (5) is infeasible, the previous trajectory will be continuously adopted. Finally, the trajectory is sent to the lower-level controller, which controls the agent’s motion (Line 1). The whole loop will be carried out indefinitely until this agent reaches its target position.

Note that there may happen potential collision between a pair of agents due to conflicts in their safety constraints, since they may adopt different versions of allocations. To avoid this, the proposed method is designed to partially update STA after a specific time $t_{s}^{R_{m}}$ as (3). Thanks to such a design, inter-agent collision can be theoretically avoided, which is supported by the following theorem.

Besides safety guarantee, another key point is that as the allocation is updated in a triggered way, a lower bound of updating frequency is required. Otherwise, due to the out-of-date allocation, the efficiency of trajectory planning of the swarm will be adversely affected. Therefore, we enforce that the waiting time $T_{w}^{i}$ is longer than the computation time $T_{c}^{i}$ in Section III-B. Thus, the following property can be obtained.

This subsection demonstrates how to implement the proposed planning algorithm on digital platforms.

Due to the fact that the OCP (5) is characterized in the continuous-time manner, it requires to be reformulated as a numerical optimization problem via discretization based on a sampling time interval $h^{i}\in\mathbb{R}$. Accordingly, the length of horizon $K^{i}$ will be determined as $K^{i}=\lfloor\frac{T^{i}}{h^{i}}\rfloor$. Consequently, in the $n^{i}$-th replanning step, the planned state $x_{n^{i}}^{i}(t_{n^{i}}^{i}+kh^{i})$ at time $t_{n^{i}}^{i}+kh^{i}$ and the control input $u_{n^{i}}^{i}(t_{n^{i}}^{i}+(k-1)h^{i})$ at time $t_{n^{i}}^{i}+(k-1)h^{i}$, where $k\in\mathcal{K}^{i}\triangleq\{1,2,\ldots,K^{i}\}$ are chosen as the optimization variables. Towards the constraints (5a) and (5b), they can be rewritten as ${p_{v}^{i}}^{\mathrm{T}}a^{ij}(t_{n^{i}}^{i}+kh^{i})>b^{ij}(t_{n^{i}}^{i}+kh^{i})$, where $k\in\mathcal{K}^{i}$, $p_{v}^{i}\in V_{n^{i}}^{i}(t_{n^{i}}^{i}+kh^{i})$, $V_{n^{i}}^{i}(t)$ is the collection of vertices of polygon regarding the planned state $x_{n^{i}}^{i}(t+kh^{i})$, $a^{ij}(t_{n^{i}}^{i}+kh^{i})$ and $b^{ij}(t_{n^{i}}^{i}+kh^{i})$ represent the hyperplane obtained at time $t_{n^{i}}^{i}+kh^{i}$. Other constraints can be similarly discretized at these time steps.

In terms of implementation, an agent only needs to consider its neighbors within a specified radius. This reduces the number of optimization constraints and achieves manageable computational complexity and admissible scalability. The obstacle avoidance scheme from our previous work [17] can be introduced to apply the proposed method in obstacle environments. Furthermore, in our implementation, an agent can determine the relative time difference between each other via communication. Thus, a global clock synchronization is eliminated. Lastly, we adopt the deadlock resolution scheme in [16] to prevent deadlock.

In this subsection, the OCP discretized as in Section IV-A is formulated and resolved by acados [26]. In addition, the proposed communication mechanism is realized via Robot Operating System (ROS). All the simulation cases are run on a single computer with an Intel Core [email protected], utilizing multiple programs.

The first simulation example is that $8$ agents navigate to their antipodal position in a circle with a diameter of $4.0$m. The detailed information about the underlying agents and the simulation results are listed in Table I. Their diameters are uniformly set as $0.4$m, but their maximum speeds range from $0.6{\rm m/s}$ to $1.0{\rm m/s}$. In this simulation, as elaborated in Section II-A, agents with different dynamic models are considered. The agents’ trajectories and inter-agent distance are depicted in Fig. 4, from which it can be observed that the minimum inter-agent distance is around $0.5$m, larger than the safety distance $0.4\rm{m}$. It can be seen that safety is guaranteed during the simulation. This task is finished within $8.9$s and the maximum transition length is $4.8$m as shown in Table I.

Three more complicated scenarios are further simulated. In the first scenario of Fig. 5, we consider a system composed of $16$ agents, confirming its effectiveness in handling large-scale systems. In its second scenario, $8$ UGVs with bicycle model exchange their lateral positions concurrently, which simulates complicated lane changes in urban road traffic. The result indicates a relatively smooth and fast position exchange, which further exhibits the practicability of our method in real traffic management. To further evaluate the proposed method in an obstacle environment, we conducted a simulation as shown in Fig. 6. The result indicates that the method can be extended to handle asynchronous trajectory planning in obstacle scenarios while ensuring safety.

Then, we compare the proposed ASTA with five state-of-the-art methods [4, 16, 8, 13, 5]. The dynamic models of the underlying robots are uniformly determined as double-integrators with maximum velocity and acceleration set as $1.0{\rm m/s}$ and $1.5{\rm m/s^{2}}$, respectively. Furthermore, they uniformly have a planning horizon as $2.3{\rm s}$. In this scenario, eight agents need to navigate to their antipodal positions without inter-agent collision. The results are depicted in Fig. 7 and provided in Table II. Executed in a sequential (Seq.) way, the Ego-swarm [4] outperforms other methods in computing time and has a considerably good transition time and distance. The LSC [13], IMPC-DR [16] and DMPC [8] all adopt synchronously (Syn.) concurrent replanning. All of them can complete this task effectively, but with a relatively longer transition distance compared to Ego-Swarm. In addition, replanning in an asynchronous (Asyn.) way, MADER in [5] has a relatively worse performance in this crowded scenario. This may be due to its check-recheck scheme, which results in more conservative trajectories. In contrast, our method provides quicker and shorter transitions in addition to a relatively lower computational cost. In summary, with additional capability to deal with the task in an asynchronous way, the proposed method not only outperforms its asynchronous counterpart in [5], but also performs better than the others in [4, 16, 8, 13].

In this subsection, we verify the effectiveness of our method via hardware experiments. In these experiments, $8$ UGVs of $5$ different hardware models are involved as shown in Fig. 8. Their dynamic models include unicycles, double integrators, and kinematic bicycles. The radii of these agents range from $0.15$m to $0.26$m, and their maximum velocities vary from $0.5{\rm m/s}$ to $1.0{\rm m/s}$. The architecture of this multi-robot system is depicted in Fig. 8. The agents’ positions are obtained via the indoor motion capture system OptiTrack and transmitted to other agents via WiFi and the ROS-based communication system. The MPC-based trajectory tracker is utilized to track the planned trajectory whose results are sent to the actuator controller. The computation of the trajectory planner, the MPC tracker and the actuator controller is accomplished by a Raspberry Pi 4B onboard computer. To address the communication delays in experiments, we implemented a feedforward compensation mechanism to mitigate their impact.

The first scenario named “crossing” lets eight UGVs cross in a $3.6{\rm m}\times 4.2{\rm m}$ rectangle ground. Since the maximum radius of the underlying agents reaches up to $0.26$m, this testing ground is thus crowded. The actual moving snapshot is provided in Fig. 1, where these agents can finish this navigation without any inter-agent collision. Furthermore, the average speed of all UGVs can reach up to $80\%$ of their corresponding maximum velocities, which demonstrates a considerably high agility.

The second scenario is utilized to show the scalability of our method when an agent encounters new neighbors at an un-signalized intersection. Six agents carry on reciprocating motion in a specific area. After tens of seconds, the other two Ackermann UGVs launch their trajectory planning procedures and set up communication with others. Then, these two agents are sent to pass through this area. The result is illustrated in Fig. 9. Two Ackermann UGVs only take less than $5$s to finish this task, which means that their average speeds can reach up to almost $0.95{\rm m/s}$, i.e., $95\%$ of the maximum velocity. For more information about those experiments, please refer to the supplementary video.