In this paper, the lower normal and bold letters denote the scalars and vectors, respectively, while the upper bold case variants represent matrices. The left and right super-scripts denoted by k and T will be used to denote the iteration index of the optimizer and transpose of the vectors and matrices. The time-stamp of any variable will be denoted by t. The symbol . 2 stands for l ₂ norm. We summarize some of the main symbols in Table 1 while some are also introduced in their first place of use. At some places, we perform a special construction where time-stamped variables are stacked to form a vector. For example, x _i will be formed by stacking x _i (t) at different time instants.

Our optimizer is designed for robots with holonomic motion models. That is, the motion along each axis is decoupled from each other. This is a common assumption made in quadrotor motion planning. Many commercially available wheeled mobile robots also have similar kinematic model. Under certain conditions, even motion planning for car-like vehicles also adopt similar kinematic model and thus our optimizer is suitable for those as well Werling et al. (2010).

For holonomic robots modeled as series of integrators, the joint trajectory optimization can be formulated in the following manner. min x i t , y i t , z i t ∑ t , i x ̈ i 2 t + y ̈ i 2 t + z ̈ i 2 t , (1a) x i t 0 , x ̇ i t 0 , x ̈ i t 0 , y i t 0 , y ̇ i t 0 , y ̈ i t 0 , z i t 0 , z ̇ i t 0 , z ̈ i t 0 = b o , i , ∀ i (1b) x i t f , x ̇ i t f , x ̈ i t f , y i t f , y ̇ i t f , y ̈ i t f , z i t f , z ̇ i t f , z ̈ i t f = b f , i , ∀ i (1c) − x i t − x j t y i t − y j t z i t − z j t T S x i t − x j t y i t − y j t z i t − z j t + 1 ≤ 0 , ∀ t , i , j ∈ 1,2 , … , n r , j ≠ i , S = a 2 0 0 0 a 2 0 0 0 b 2 (1d) The cost function Eq. 1a minimizes the squared norm of the acceleration at each time instant for all the robots. The equality constraints Eqs. 1b and 1c enforces the initial and final boundary conditions on positions, velocity, and accelerations on each robot trajectory. The pair-wise collision avoidance constraints are modeled by inequalities Eq. 1d, wherein we have assumed that the robots are shaped as axis-aligned spheroids with axis dimensions (a, a, b). For the ease of exposition, we consider all robots to have the same shape. Extension to a more general setting is trivial. The constraints Eq. 1d are typically enforced at pre-selected discrete time-stamps, and thus a fine resolution of discretization is necessary for accurately satisfying the constraints. For now, we do not consider any static obstacles in the environment in the formulation above. The extension is trivial as static obstacles can be considered robots with zero velocity and whose trajectories are not updated within the optimizer’s iteration.

Let the trajectory of each robot along each motion axis x, y, z be parameterized through n _v number of variables. For example, these variables could be time-stamped way-points representing the trajectory or the coefficients of their polynomial representation (see Eq. 8). Then, for a set-up with n _r number of robots and a planning horizon of n _p , optimization Eqs. 1a–1d involves n _r ∗n _v variables, and 18∗n _r equality constraints. The number of pair-wise collision constraints would be n r 2 ∗ n p .

The number of decision variables in optimization Eqs. 1a–1d scales linearly with the number of robots. Although this increase poses a computational challenge, the main difficulty in solving the optimization stems from the non-convex pair-wise collision avoidance constraints Eq. 1d as the rest of the cost and constraint functions are convex. Moreover, the number of collision avoidance constraints increases exponentially with the number of robots. Existing works (Augugliaro et al., 2012; Chen et al., 2015; Li et al., 2020; Park et al., 2020; Rastgar et al., 2021) have adopted different simplifications on the collision avoidance constraints to make multi-robot trajectory optimization more tractable. We thus next present a categorization of these works based on the exact methodology used.

Similar to (Bento et al., 2013), we break the joint multi-robot trajectory optimization Eqs. 1a–1d into decoupled smaller sub-problems at each iteration. This is illustrated in Figure 1. At a conceptual level, this decoupling process can be interpreted in the following manner: the robots communicate among themselves the trajectories they obtained in the previous iteration of the optimizer. Each robot then uses them to independently formulate their collision avoidance constraints. Our work differs from existing works in the way the decoupled problems illustrated in Figure 1 is solved. As mentioned before, a trivial approach to solving the sub-problems in parallel CPU threads is not scalable for tens of robots. In contrast, our main idea in this paper is to develop a GPU accelerated optimizer that can solve a batch of optimization problems in one shot.

In this sub-section, we aim to provide a succinct mathematical abstraction of our main idea. We discuss a special class of problems that are simple to solve in batch fashion. To this end, consider the following batch of equality constrained QPs, i ∈ {1, 2, …, n _r }. min ξ i 1 2 ξ i T Q ̄ ξ i + q ̄ i T ξ i , st:  A ̄ ξ i = b ̄ i (3) In total, there are n _r QPs to be solved, each defined over variable ξ _i . The QPs defined in Eq. 3 have a unique structure. The Hessian Q ̄ and the constrained matrix A ̄ are shared across the problems and only the vectors q ̄ i and b ̄ i varies across the batch. This special structure leads to efficient batch solution formulae. To see how, note that each QP in the batch can be reduced to solving the following set of linear equations. Q ̄ A ̄ T A ̄ 0 ξ i μ i = q ̄ i b ̄ i , ∀ i ∈ 1,2 , … , n r (4) where μ _i are the dual optimization variables. Now, it can be observed that the matrix on the left-hand side of Eq. 4 is independent of the batch index i, and thus, the solutions for the entire batch can be computed in one shot through Eq. 5. ξ 1 |…| ξ n r μ 1 |…| μ n r = Q ̄ A ̄ T A ̄ 0 − 1 ︷ matrix q ̄ 1 q ̄ 2 … q ̄ n r b ̄ 1 b ̄ 2 … b ̄ n r ︷ stacked vectors , (5) where | represents that the columns are stacked horizontally. The batch solution Eq. 5 amounts to multiplying one single matrix with a batch of vectors. Furthermore, the matrix is constant, and its dimension is independent of the number of problems in the batch. Thus, operation Eq. 5 can be trivially parallelized over GPUs using off-the-shelf libraries like JAX (Bradbury et al., 2020).How it all fits: In the next few sub-sections, we will show how the distributed sub-problems of Eqs. 1a–1d, shown in Figure 1 can be solved efficiently in a batch setting. Specifically, we reformulate these problems in such a way that the most intensive part of their solution process reduces to solving a batch of QPs with the special structure presented in Eq. 3.

An important building block of our approach is rephrasing the collision avoidance constraints into the following polar representation from (Rastgar et al., 2021; Rastgar et al., 2020). f c x i t , y i t = x i t − x ̄ j t − a d i j t sin β i j t cos α i j t y i t − y ̄ j t − a d i j t sin β i j t sin α i j t z i t − z ̄ j t − b d i j t cos β i j t , d i j t ≥ 1 , (6) where α _ij (t), β _ij (t), d _ij (t) are unknown variables that will be computed by the optimizer along with each robot’s trajectory. Physically, α _ij (t) and βij(t) represent the 3D solid angle of the line-of-sight connecting robot i and j based on the predicted motion of the latter. The variable d _ij (t) is the ratio of the length of the line-of-sight vector with minimum safe distance a 2 + a 2 + b 2 (see (Rastgar et al., 2021)).

Using Eq. 6, we can reformulate the distributed sub-problems presented in Figure 1 for the ith robot in the following manner. We reiterate that ( x ̄ j ( t ) , y ̄ j ( t ) , z ̄ j ( t ) ) , ∀ j ≠ i is known based on the prediction of the trajectories of other robots. min x i t , y i t , z i t , d i j t , α i j t , β i j t ∑ t x ̈ i 2 t + y ̈ i 2 t + z ̈ i 2 t , (7a) x i t 0 , x ̇ i t 0 , x ̈ i t 0 , y i t 0 , y ̇ i t 0 , y ̈ i t 0 , z i t 0 , z ̇ i t 0 , z ̈ i t 0 = b o , i , ∀ i (7b) x i t f , x ̇ i t f , x ̈ i t f , y i t f , y ̇ i t f , y ̈ i t f , z i t f , z ̇ i t f , z ̈ i t f = b f , i , ∀ i (7c) f c : x i t − x ̄ j t − a d i j t sin β i j t cos α i j t y i t − y ̄ j t − a d i j t sin β i j t sin α i j t z i t − z ̄ j t − b d i j t cos β i j t (7d) d i j t ≥ 1 , ∀ t , j , j | j ∈ 1,2 , … , n r , j ≠ i (7e)

Optimization Eqs. 7a–7e is expressed in terms of functions and thus has the so called infinite dimensional representaton. To obtain a finite-dimensional form, we assume some parametric form for this functions. For different d i j ( t ) , α a i j ( t ) , β i j ( t ) , we assume a way-point paramterization. That is, these functions are represented through values at discrete time instants. The trajectories along each motion axis are represented as following polynomials. x i t 1 x i t 2 ⋮ x i t n p = P c x , i , x ̇ i t 1 x ̇ i t 2 ⋮ x ̇ i t n p = P ̇ c x , i , x ̈ i t 1 x ̈ i t 2 ⋮ x ̈ i t n p = P ̈ c x , i . (8)

Similar expressions as Eq. 8 can be written for the y, z component of the trajectory as well. The matrix P is formed with time dependent polynomial basis functions. Using Eq. 8, we can re-write Eqs. 7a–7e in the following matrix form. min ξ 1 , i , ξ 2 , i , ξ 3 , i , ξ 4 , i 1 2 ξ 1 , i T Q ξ 1 , i , (9a) A e q ξ 1 , i = b e q , (9b) F ξ 1 , i = g i ξ 2 , i , ξ 3 , i , ξ 4 , i , (9c) ξ 4 , i ≥ 1 , (9d)

where, ξ _1,i = (c _x,i , c _y,i , c _z,i ), ξ _2,i = α _ij , ξ _3,i = β _ij and ξ _4,i = d _ij . Note that α _ij is formed by stacking α _ij (t) at different time instants. Similar construction is followed for other elements in ξ ₂, ξ ₃. The matrix Q is block diagonal matrix with P ̈ T P ̈ as main diagonal block. The affine constraint Eq. 9b is a matrix representation of the initial and final boundary conditions Eqs. 7b and 7c. The matrix A _eq and vector b _eq is constructed in the following manner. A e q = A 0 0 A , A e q = P 1 | P ̇ 1 | P ̈ 1 | P − 1 | P ̈ − 1 P ̈ − 1 T , b e q = b 0 , i b f , i , (10) where, P 1 , P ̇ 1 , P ̈ 1 , P − 1 , P ̇ − 1 , P ̈ − 1 represents the first and last elements of the corresponding matrices.

Matrix F and vector g _i defining constraints Eq. 7e are constructed as F = F o 0 0 0 F o 0 0 0 F o g i = g x , i ξ 2 , i , ξ 3 , i , ξ 4 , i g y , i ξ 2 , i , ξ 3 , i , ξ 4 , i g z , i ξ 2 , i , ξ 3 , i , ξ 4 , i (11)

where, g x , i = x ̄ j + a d i j ⁡ sin β i j ⁡ cos α i j , ∀ j , g y , i = y ̄ j + a d i j ⁡ sin β i j ⁡ sin α i j , ∀ j , g z , i = z ̄ j + b d i j ⁡ cos β i j , ∀ j (12)

and F _o is formed by vertically stacking P, n _r − 1 times. The vectors x ̄ j , y ̄ j , z ̄ j are formed by stacking x ̄ j ( t ) , y ̄ j ( t ) , z ̄ j ( t ) at different time instants.

REMARK 1. The subscript i signifies that Eqs. 9a–9c is constructed for the ith agent.

REMARK 2. All the non-convexity in optimization Eqs. 9a–9d is rolled into the equality constraint Eq. 9c.

REMARK 3. The matrices A _eq , F in optimization Eqs. 9a–9d is independent of the robot index. In other words, these matrices remain the same irrespective of the which sub-problems shown in Figure 1 we are solving.

Remark 3 sheds light behind our motivation of presenting the elaborate reformulations of the collision avoidance constraints. In fact, on the surface, our chosen representation Eq. 6 seems substantially more complicated than the conventional form Eq. 1d based on the Euclidean norm. In the next sub-section, we present an optimizer that can leverage the insights presented in Remark 3. More precisely, we will show that the due to the matrices A _eq , F being independent of the robot index i, the most intensive part of solving Eqs. 9a–9d reduces to the batch QP structure presented in Section 3.1.

Our proposed optimizer for Eqs. 9a–9d relies on relaxing the non-convex equality constraints Eq. 9c as l ₂ penalties and incorporating them into the cost function in the following manner. min ξ 1 , i , ξ 2 , i , ξ 3 , i , ξ 4 , i ( 1 2 ξ 1 , i T Q ξ 1 , i − ⟨ λ i , ξ 1 , i ⟩ + ρ 2 F ξ 1 , i − g i ξ 2 , i , ξ 3 , i , ξ 4 , i 2 2 ) (13)

As the residual of the constraint term is driven to zero, we recover the solution to the original problem. To this end, the parameter λ _i , known as the Lagrange multiplier, plays an important part. Its role is to appropriately weaken the effect of the primary cost function so that the optimizer can focus on minimizing the constraint residual (Taylor et al., 2016). The parameter ρ is a scalar and is typically constant. However, it is possible to increase or decrease it depending on the magnitude of the constraint residual at each iteration of the optimizer.

The relaxation of non-convex equality constraints, as augmented Lagrangian (AL) cost, is extensively used in non-convex optimization (Ferranti and Keviczky, 2017; Ferranti et al., 2018). However, what differentiates our use of AL from existing works is how we minimize Eq. 13. Typical approaches towards non-convex optimization are based on first (and sometimes second) order Taylor Series expansion of the non-convex costs or constraints. In contrast, we adopt an Alternating Minimization (AM) based approach, wherein at each iteration, we minimize only one of the variable blocks amongst ξ _1,i , ξ _2,i , ξ _3,i , ξ _4,i while others are held constant at specific values. In the next section, we present the various steps of our AM optimizer and highlight how it never requires any linearization of cost or constraints. Moreover, we show how the AM steps naturally lead to a simple yet efficient batch update rule using which we can solve Eqs. 9a–9d for all the robots in one shot.

Algorithm 1

Alternating Minimization Based Solution for the Ith Sub-Problem

Our AM based optimizer for minimizing Eq. 13 subject to Eqs. 9b–9d is presented in Algorithm 1. Here, the left superscript k is used to track the values of the variable across iteration. For example, ^k ξ _2,i denotes the value of this respective variable at iteration k.

The Algorithm begins (line 1) by providing the initial guesses for ξ _2,i , ξ _3,i , ξ _4,i . The main optimizer iterations run within the while loop for the specified max iteration limit or till the constraint residuals are a below specified threshold. Each step within the while loop involves solving a convex optimization over just one variable block. We present a more detailed analysis of each of the steps next.

Our optimizer is tested using the following benchmarks.

• The robots’ start and goal positions are sampled along the circumference of a circle.

By changing the number and positions of robots and static obstacles, we created several variations of the mentioned benchmarks and utilized them to validate our optimizer. Figures 2A–I presents a few qualitative results in a diverse set of environments. Figures 2A–C shows an environment with 32 robots and 20 obstacles. Interestingly, we observe a circular pattern formation among the robots while passing through narrow passages between static obstacles. In Figures 2D–F, 36 robots initially arranged in a grid are given the task to navigate to a line formation while avoiding collisions with each other and with the four static obstacles in the environment. Figure 3 shows the execution of the computed trajectories in a high-fidelity physics engine called Gazebo available in Robot Operation System (Koenig and Howard, 2004).

A conceptually simple way of validating the convergence of the proposed optimizer is to observe the trends in residual of constraints Eq. 9c over iterations. If the residuals converge to zero, the computed trajectories are guaranteed to be collision-free. Figure 4 empirically provides this validation. It presents ‖Fξ _1,i − g _i ‖ averaged over all i. Furthermore, we average the residuals over different trials in various benchmarks. We can observe from Figure 4 that, on average, 100 iterations are sufficient to obtain residuals around 0.01. A further increase in residuals can be obtained by increasing the number of iterations but at the expense of increasing the computation time. In our implementation, we adopt a heuristic wherein we inflate the size of the robots with four times the typical residual observed after 100 iterations.

For a further sanity check, we check for inter-robot and robot-obstacle distances at each point along the computed trajectories (Figure 5). Collisions are considered to have happened if the distances are less than sum of the robots’ (blue line in Figure 5) or robot-obstacles’ (yellow line in Figure 5) radii. Figure 5 summarizes the average behavior observed across several trials, which validates the satisfaction of the collision avoidance requirement.

This subsection compares our optimizer with existing state-of-the-art approaches (Rastgar et al., 2021) and (Park et al., 2020). We use the following metrics for bench-marking.

• Smoothness cost: It is computed as the norm of the second-order finite-difference of the robot position at different time instances.