Multi-Agent Planning

Source:

[1] L. Quan et al., ‘Robust and efficient trajectory planning for formation flight in dense environments’, IEEE Trans. Robot., vol. 39, no. 6, pp. 4785–4804, Dec. 2023, doi: 10.1109/TRO.2023.3301295.

[2] L. Quan, L. Yin, C. Xu, and F. Gao, “Distributed swarm trajectory optimization for formation flight in dense environments,” in Proc. Int. Conf. Robot. Autom., 2022, pp. 4979–4985.

[3] Z. Wang, X. Zhou, C. Xu, and F. Gao, "Geometrically constrained trajectory optimization for multicopters", IEEE Trans. Robot., vol. 38, no. 5, pp. 3259–3278, Oct. 2022, doi: 10.1109/TRO.2022.3160022.

Notation

Sign	Meaning
$\mathbf{A}$	Adjacency matrix of the formation graph
$a_m$	Maximum acceleration
${\mathbf{c}}_i$	Coefficients of the $i^{\text{th}}$ piece
$\mathbf{D}$	Degree matrix of the formation graph
$d_o$	Safety threshold
$d_r$	Safe clearance between each robot
$\text{des}$	Sign for desired trajectory
$f_s(\cdot )$	Differentiable formation similarity error metric: $(5)$
$H(\cdot )$	Cost function
$\mathcal{H}(\cdot )$	Continuous-time constraint function: similarity in group formation, dynamic feasibility, obstacle avoidance, and reciprocal avoidance of swarms
$J(\cdot )$	2nd-order continuous cost function
$J_s$	Formation similarity error
$J_u$	Uniform distribution cost
$\mathbf{L}$	Laplacian matrix of the formation graph
$\hat{\mathbf{L}}$	Symmetric normalized Laplacian matrix
$M_c$	Number of sample points with corresponding time stamps
$\mathcal{M}(\cdot )$	Parameter mapping from $(\mathbf{q},\mathbf{T})$ to $\mathbf{c}$
$m$	Dimension of the robot
$N$	Number of robots
$\mathcal{P}$	Cost: subscript $f$: swarm formation similarity; $e$: control effort; $t$: total time; $o$: obstacle avoidance; $r$: swarm reciprocal avoidance; $d$: dynamic feasibility
${\mathbf{p}}_{i,j}$	The $j^{\text{th}}$ sample point of the $i^{\text{th}}$ robot trajectory
${\mathbf{p}}_{i,j}^{\ast }$	The optimal $j^{\text{th}}$ sample point of the $i^{\text{th}}$ robot trajectory
$p(t)$	Trajectory, piecewise polynomial
${\mathbf{p}}^{(s-1)}(t)$	The $(s-1{)}^{\text{th}}$ derivative of the trajectory, i.e., the control input
${\mathbf{p}}^{[s-1]}(t)$	$(\mathbf{p}(t{)}^{\mathrm{\top }},\dot{\mathbf{p}}(t{)}^{\mathrm{\top }},\cdots ,{\mathbf{p}}^{(s-1)}(t{)}^{\mathrm{\top }}{)}^{\mathrm{\top }}\in {\mathbb{R}}^{ms}$
${\mathbf{p}}_i(t)$	The $i^{\text{th}}$ piece of the trajectory
${\overline{\mathbf{p}}}_0$	Initial state
${\overline{\mathbf{p}}}_f$	Final state
${\tilde{\mathbf{p}}}_{i,j}$	The $j^{\text{th}}$ constraint point of the $i^{\text{th}}$ piece
$Q$	Degree of the polynomial, here $Q=2s-1=5$
$\mathit{q}$	Intermediate waypoints
$s$	Integrator order, if controlling jerk, $s=3$
$\mathit{T}$	Time allocation for each piece
$T_{\mathrm{\Sigma }}$	Total time of the trajectory
${\mathfrak{T}}_{\text{MINCO}}$	The class of trajectory parameterization based on piecewise polynomials aiming at minimizing control effort
$\mathbf{U}$	Squared distance vector: $(|{\hat{\mathbf{p}}}_{i,1}^{\ast }-{\hat{\mathbf{p}}}_{i,0}^{\ast }{|}_2^2,\cdots ,|{\hat{\mathbf{p}}}_{i,M_c}^{\ast }-{\hat{\mathbf{p}}}_{i,M_c-1}^{\ast }{|}_2^2)\in {\mathbb{R}}^{M_c}$
$v_m$	Maximum velocity
${\mathit{w}}_i$	The weight vector of the robot $i$, ${\mathbb{R}}^N$, essentially the weights of its $N$ adjacent edges
$\mathit{\beta }(t)$	Polynomial basis vector: $\mathit{\beta }(t)=[1,t,\dots ,t^N{]}^{\mathrm{\top }}$
$\delta$	Sampling time interval
${\lambda }_s,{\lambda }_u$	Weights for the formation similarity and uniform distribution costs
${\kappa }_i$	Number of constraint points for the $i^{\text{th}}$ piece
$\mathrm{\Phi }$	All other robots in the swarm
$\varphi$	Element in $\mathrm{\Phi }$
$\rho$	Time regularization weight

1 Adaptive description of swarm formation (Sec. IV in [1])

1.1 Graph-based Formation Definition

In this article, a swarm formation of $N$ robots is modeled by an undirected graph $\mathcal{G}=(\mathcal{V}\mathcal{,}\mathcal{E})$, where $\mathcal{V}:=\{1,2,...,N\}$ is the set of vertices, and $\mathcal{E}\subset \mathcal{V}\times \mathcal{V}$ is the set of edges. In graph $\mathcal{G}$, the vertex $i$ represents the $i^{th}$ robot with position vector ${\mathbf{p}}_i=[x_i,y_i,z_i]\in {\mathbb{R}}^3$ . An edge $e_{ij}\in \mathcal{E}$ that connects vertex $i\in \mathcal{V}$ and vertex $j\in \mathcal{V}$ means that robot $i$ and $j$ can measure the geometric distance between each other.

In our work, each robot can obtain the positions of all robots $\{{\mathbf{p}}_1,...,{\mathbf{p}}_i,...,{\mathbf{p}}_N\}$, thus the graph $\mathcal{G}$ is complete. Then we determine the adjacency matrix $\mathbf{A}\in {\mathbb{R}}^{N\times N}$ and degree matrix $\mathbf{D}\in {\mathbb{R}}^{N\times N}$ of the formation graph $\mathcal{G}$ by

$$\begin{array}{}\text{(1)}& & A_{ij}=w_{ij}=\parallel {\mathbf{p}}_i-{\mathbf{p}}_j{\parallel }^2,\text{(2)}& & D_{ij}=\{\begin{array}{ll}\sum _{j=1}^N{A}_{ij},& \text{if}i=j,\\ 0,& \text{otherwise},\end{array}\end{array}$$

where the non-negative edge weight $w_{ij}$ is the squared distance between the $i^{th}$ and $j^{th}$ robots, and $\parallel \cdot \parallel$ denotes the Euclidean norm. Thus, the corresponding Laplacian matrix is

$$\begin{array}{}\text{(3)}& \mathbf{L}=\mathbf{D}-\mathbf{A}.\end{array}$$

With the above matrices, the symmetric normalized Laplacian matrix of graph $\mathcal{G}$ is defined as

$$\begin{array}{}\text{(4)}& \hat{\mathbf{L}}={\mathbf{D}}^{-1/2}\mathbf{L}{\mathbf{D}}^{-1/2}=\mathbf{I}-{\mathbf{D}}^{-1/2}\mathbf{A}{\mathbf{D}}^{-1/2},\end{array}$$

where $\mathbf{I}\in {\mathbb{R}}^{N\times N}$ is the identity matrix. $\hat{\mathbf{L}}$ contains the information that is invariant to scale, translation, and rotation.

Finally, we use graph theory to describe various desired formation shapes, such as squares, hexagons, and pyramids.

By specifying the desired positions ${\mathbf{p}}_i^d=[x_i^d,y_i^d,z_i^d]\in {\mathbb{R}}^3,i=1,...,N$, computing ${\hat{\mathbf{L}}}_{\text{des}}$ is simple.

It's important to note that the desired formation shape is independent of the coordinate system as long as the relative positions are provided.

1.2 Differentiable Formation Similarity Error Metric

To assess the deviation from the desired formation, we propose a differentiable formation similarity error metric as

$$\text{begin{aligned} f\_s \& = f\_s(mathbf{p}\_1,...,mathbf{p}\_i,...,mathbf{p}\_N) = f\_s (mathbf{A},mathbf{D}) = f\_s(mathbf{hat{L}},mathbf{hat{L}}\_{des}) \& = parallelmathbf{hat{L}}-mathbf{hat{L}}\_{des}parallel^2\_F = operatorname{tr}{(mathbf{hat{L}}-mathbf{hat{L}}\_{des})^top(mathbf{hat{L}}-mathbf{hat{L}}\_{des})}, tag{5} end{aligned}}$$

where $\mathrm{tr}\{\cdot \}$ denotes the trace of a matrix, $\hat{\mathbf{L}}$ is the symmetric normalized Laplacian of the current swarm formation, ${\hat{\mathbf{L}}}_{\text{des}}$ is the counterpart of the desired formation. Frobenius norm $\parallel \cdot {\parallel }_F$ is used in our distance metric.

As a graph representation matrix, $\hat{\mathbf{L}}$ contains information about the graph structure. This allows $f_s$ to consider only the geometric shape of the formation, and not be influenced by scaling, translation, or rotation. Additionally, $f_s$ is a dimensionless value that solely reflects the error in formation shape similarity.

In particular, under the distributed framework, each robot can only change its positions to reduce the overall formation similarity error. Therefore, the only variable for robot $i$ in $(5)$ is ${\mathbf{p}}_i$, and $f_s({\mathbf{p}}_1,...,{\mathbf{p}}_i,...,{\mathbf{p}}_N)$ can be simplified as $f_s({\mathbf{p}}_i)$.

Our metric is analytically differentiable with respect to the position of each robot. For robot $i$, we use the weights of its $N$ adjacent edges $\{e_{i1},e_{i2},...,,e_{iN}\}$ to form a weight vector ${\mathit{w}}_i=[w_{i1},w_{i2},...,,w_{iN}{]}^{\mathrm{\top }}$. By the chain rule, the gradient of $f_s$ with respect to ${\mathbf{p}}_i$ can be written as

$$\begin{array}{}\text{(6)}& \frac{\partial f_s}{\partial {\mathbf{p}}_i}=\frac{\partial f_s}{\partial {\mathit{w}}_i^{\mathrm{\top }}}\frac{\partial {\mathit{w}}_i}{\partial {\mathbf{p}}_i}.\end{array}$$

According to our metric $(5)$, the gradient of $f_s$ with respect to each weight $w_{ij}$ can be computed as follow

$$\begin{array}{rl}\frac{\partial f_s}{\partial w_{ij}}& =\mathrm{tr}\left\{{\left(\frac{\partial f_s}{\partial \hat{\mathbf{L}}}\right)}^{\mathrm{\top }}\left(\frac{\partial \hat{\mathbf{L}}}{\partial w_{ij}}\right)\right\},\\ \frac{\partial f_s}{\partial \hat{\mathbf{L}}}& =\frac{\partial ||\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{\text{des}}|{|}_F^2}{\partial \hat{\mathbf{L}}}=2(\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{\text{des}}),\\ \text{(7)}& \frac{\partial \hat{\mathbf{L}}}{\partial w_{ij}}& =-\frac{\partial ({\mathbf{D}}^{-1/2}\mathbf{A}{\mathbf{D}}^{-1/2})}{\partial w_{ij}}.\end{array}$$

Then the gradient $\partial f_s/\partial {\mathit{w}}_i$ can be written as

$$\begin{array}{}\text{(8)}& \partial f_s/\partial {\mathit{w}}_i=[\partial f_s/\partial w_{i1},\partial f_s/\partial w_{i2},...,\partial f_s/\partial w_{iN}{]}^{\mathrm{\top }}.\end{array}$$

As for $\partial {\mathit{w}}_i/\partial {\mathbf{p}}_i$, the Jacobian can be easily derived since the weight function $(1)$ is a differentiable quadratic form.

1.3 Optimal Formation Position Sequence

TODO: Previous work Previous work [2] incorporated $f_s$ directly into the trajectory optimization, making formation flight a coupled trajectory optimization problem. While this method is suitable for small-scale formation flight, it becomes computationally inefficient as the number $N$ of robots increases.

Considering the simplified equation for coupled trajectory optimization

$$\begin{array}{}\text{(9)}& \underset{{\mathbf{p}}_{i,0},...,{\mathbf{p}}_{i,M_c}}{min}\sum _{j=0}^{M_c}{f}_s({\mathbf{p}}_{i,j})+J_{\text{other}},\end{array}$$

where ${\mathbf{p}}_{i,j}$ represent the $j^{th}$ sample point of $i^{th}$ robot trajectory in $(19)$ for convenience. $J_{\text{other}}$ represents all other cost functions, and $M_c$ is the number of sample points with corresponding timestamps.

The primary purpose of calculating $f_s$ is to supply gradient information for minimizing formation similarity error. However, since the graph $\mathcal{G}$ is a complete graph, computing $f_s$ has a complexity of $O(N^2)$.

Consequently, the coupled trajectory optimization $(9)$ also exhibits high complexity of $O(N^2)$ in each iteration, limiting its applicability to large-scale swarm operations.

To address this issue, we must identify an equivalent approach with reduced computational complexity to replace the function of $f_s$ in $(9)$. We introduce the concept of optimal formation position ${\mathbf{p}}_{i,j}^{\ast }$ for robot $i$ at timestamp $j$, which is the position that minimizes the formation similarity error $f_s$. Fig.1(a) illustrates this concept using a 2D formation as an example.

Figure 1[1]: Illustration of optimal formation position sequence using a 2D formation. (a) The surface shows the profile of the similarity metric when one UAV moves in the plane and the other three remain still. The minimum suggests the optimal formation position to form the desired shape. (b) The sequence of optimal formation positions corresponds to the timestamps.

It is evident from the figure that there exists an optimal formation position ${\mathbf{p}}_{i,j}^{\ast }$ that results in a minimal formation similarity error, and the partial derivative is $\partial f_s/\partial {\mathbf{p}}_{i,j}^{\ast }=0$.

In the future period with a sequence of timestamps $\{0,...,j,...,M_c\}$, we represent the expected positions of robot $i$ with the optimal formation position sequence ${\mathbf{p}}_i^{\ast }=\{{\mathbf{p}}_{i,0}^{\ast },\cdots ,{\mathbf{p}}_{i,j}^{\ast },\dots ,{\mathbf{p}}_{i,M_c}^{\ast }\}$, as shown in Fig. 1(b). By precomputing ${\mathbf{p}}_i^{\ast }$, we can utilize its quadratic distance to replace the gradient information offered by $f_s$ in $(11)$, thus decreasing the computational requirements as follows

$$\begin{array}{}\text{(10)}& f_s({\mathbf{p}}_{i,j})\Rightarrow \parallel {\mathbf{p}}_{i,j}-{\mathbf{p}}_{i,j}^{\ast }{\parallel }^2.\end{array}$$

Since the optimal solutions of $f_s$ and quadratic distance cost are equivalent, the trajectory approaches the positions with minimal formation similarity error, maintaining the desired formation.

Thus, we can effectively solve the coupled trajectory optimization with a two-step procedure

$$\begin{array}{rl}& \text{1.}{\mathbf{p}}_i^{\ast }=\text{argmin}\sum _{j=0}^{M_c}{f}_s({\mathbf{p}}_{i,j}),\\ \text{(11)}& \stackrel{{\mathbf{p}}_i^{\ast }}{\Rightarrow }& \text{2.}\underset{{\mathbf{p}}_{i,0},...,{\mathbf{p}}_{i,M_c}}{min}\parallel {\mathbf{p}}_{i,j}-{\mathbf{p}}_{i,j}^{\ast }{\parallel }^2+J_{\text{other}}.\end{array}$$

As a result, the previously required calculation of $f_s$ in each trajectory optimization process is replaced by the computation of the quadratic distance, simplifying the optimization problem. This significantly reduces computational demands and enables large-scale swarm formation.

Formula $(11)$ indicates that trajectory optimization in the next section is performed on discretized points. Non-uniform discretized points may lead to poor trajectories and sub-optimal performance. Therefore it is crucial to ensure a uniform distribution of these points to maintain the effectiveness of the optimization process.

In engineering practice, since graphs $\mathcal{G}$ are constructed from a series of discretized timestamps as depicted in Fig. 1(b), each ${\mathbf{p}}_{i,j}^{\ast }$ is independent.

To ensure a smoother trajectory, we introduce the uniform optimal formation position sequence ${\hat{\mathbf{p}}}_i^{\ast }$, which is generated by considering the formation similarity error $J_s$ and the uniform distribution cost $J_u$

$$\begin{array}{rl}\text{(12)}& {\hat{\mathbf{p}}}_i^{\ast }& =\text{argmin}{\lambda }_s{J}_s+{\lambda }_u{J}_u,J_s& =\sum _{j=0}^{M_c}{f}_s({\hat{\mathbf{p}}}_{i,j}^{\ast }),\\ \text{(13)}& J_u& =\mathbb{E}({\mathbf{U}}^2)-\mathbb{E}(\mathbf{U}{)}^2=\frac{\parallel \mathbf{U}{\parallel }_2^2}{M_c}-\frac{\parallel \mathbf{U}{\parallel }_1^2}{(M_c{)}^2},\end{array}$$

where ${\lambda }_s$ and ${\lambda }_u$ are the relative weights. $\mathbb{E}(\cdot )$ is mathematic expectation and the squared distance vector $\mathbf{U}\in {\mathbb{R}}^{M_c}$ is

$$\begin{array}{}\text{(14)}& \mathbf{U}=(\parallel {\hat{\mathbf{p}}}_{i,1}^{\ast }-{\hat{\mathbf{p}}}_{i,0}^{\ast }{\parallel }_2^2,\cdots ,\parallel {\hat{\mathbf{p}}}_{i,M_c}^{\ast }-{\hat{\mathbf{p}}}_{i,M_c-1}^{\ast }{\parallel }_2^2).\end{array}$$

We use the quasi-Newton method to solve this unconstrained optimization problem $(12)$ and generate uniform ${\hat{\mathbf{p}}}_i^{\ast }$ for the later trajectory optimization $(18)$.

By doing so, the trajectory resulting from these discretized points in the next section can be smoother and avoid sudden spatial changes.

2 Spatial-temporal trajectory optimization for formation flight

2.1 Trajectory Representation

The differential flatness of multicopters benefits trajectory generation without integrating differential equations. Moreover, the motion planning of multicopters can be performed on low-dimensional smooth trajectories.

Here we adopt a state-of-the-art trajectory representation named MINCO to achieve minimum control effort spatial-temporal trajectory planning for swarm aerial robots in three-dimensional environments. MINCO conducts spatial-temporal deformation of the flat-output $M$-piece trajectory $p(t)$ by decoupling the space and time parameters with a linear-complexity mapping $\mathcal{M}$

$$\begin{array}{}\text{(15)}& p(t)={\mathcal{M}}_{\mathbf{q},\mathbf{T}}(t),\mathrm{\forall }t\in [t_0,t_M],\end{array}$$

where $\mathbf{q}=({\mathbf{q}}_1,\cdots ,{\mathbf{q}}_{M-1}{)}^{\mathrm{\top }}\in {\mathbb{R}}^{3\times (M-1)}$ are the adjacent intermediate points between each pair of connected pieces and $\mathbf{T}=(T_1,\cdots ,T_M{)}^{\mathrm{\top }}\in {\mathbb{R}}_{>0}^M$ the time duration of each piece.

A $m$-dimensional $M$-piece trajectory $p(t)$ is represented by piecewise polynomials. And $i^{th}$ piece $p_i(t)$ is defined as a multi-degree polynomial ($Q=5$ in this paper)

$$\begin{array}{}\text{(16)}& p_i(t)={\mathbf{c}}_i^{\mathrm{\top }}\mathit{\beta }(t),\mathrm{\forall }t\in [0,T_i],\end{array}$$

where ${\mathbf{c}}_i\in {\mathbb{R}}^{(Q+1)\times m}$ is the coefficient matrix and $\mathit{\beta }(t)=[t^0,t^1,\cdots ,t^Q{]}^{\mathrm{\top }}$ is the natural basis.

For an $s$-integrator ($s=3$ here) chain dynamics system, a $M$-piece $2s-1$ degree trajectory $p(t)$ is defined by constant boundaries and minimum control effort $\{\mathbf{q},\mathbf{T}\}$.

Furthermore, MINCO is advanced in convert $\{\mathbf{q},\mathbf{T}\}$ to $\{\mathbf{c},\mathbf{T}\}$ using a linear-time and space parameter mapping $\mathbf{c}=\mathcal{M}(\mathbf{q},\mathbf{T})$, where $\mathbf{c}=({\mathbf{c}}_1^{\mathrm{\top }},\cdots ,{\mathbf{c}}_M^{\mathrm{\top }}{)}^{\mathrm{\top }}$ is polynomial coefficients.

2.2 Problem Formulation

After determining the desired formation shape in the last section, we expect a cluster of trajectories for swarm robots, which are smooth, collision-free, and formation maintained.

In practice, navigating swarm robots in an unknown dense environment with FOV-limited sensors and onboard computer requires an efficient real-time planner focusing on local information. Besides, centralized optimization is limited by the scale of the swarm.

Therefore, we choose a distributed local trajectory optimization for formation flight as follows

$$\begin{array}{}\text{(17a)}& \underset{\mathbf{q},\mathbf{T}}{min}& {\int }_{t_0}^{t_M}\parallel p^{(s)}(t){\parallel }^{2}dt+\rho \cdot T_{\mathrm{\Sigma }},\text{(17b)}& \text{s.t.}& p(t)={\mathcal{M}}_{\mathbf{q},\mathbf{T}}(t),\mathrm{\forall }t\in [t_0,t_M],\text{(17c)}& & {\mathbf{p}}^{[s-1]}(0)={\overline{\mathbf{p}}}_0,\text{(17d)}& & {\mathbf{p}}^{[s-1]}(t_M)={\overline{\mathbf{p}}}_f,\text{(17e)}& & \mathcal{H}(p(t),...,p^{(s)}(t))⪯\mathbf{0},\mathrm{\forall }t\in [t_0,t_M].\end{array}$$

We define costs $(17a)$ for smoothness and aggressiveness to achieve smooth and efficient flight. $\rho$ is time regularization parameter, $T_{\mathrm{\Sigma }}=\sum _{i=1}^M{T}_i$. The state of robot $p(t)$ $(17b)$ is parameterized by the optimization variables $\{\mathbf{q},\mathbf{T}\}$. ${\mathbf{p}}^{[s-1]}(t)=(p(t{)}^{\mathrm{\top }},\dot{p}(t{)}^{\mathrm{\top }},...,p^{(s-1)}(t{)}^{\mathrm{\top }}{)}^{\mathrm{\top }}\in {\mathbb{R}}^{ms}$ represents the higher-order derivatives of a chain dynamic system with $s$-integrator. Boundary conditions involve initial state ${\overline{\mathbf{p}}}_0\in {\mathbb{R}}^{ms}$ $(17c)$ and terminal state ${\overline{\mathbf{p}}}_f\in {\mathbb{R}}^{ms}$ $(17d)$. Continuous-time constraints $\mathcal{H}$ $(17e)$ include swarm formation similarity, dynamic feasibility, obstacle avoidance, and swarm reciprocal avoidance.

2.3 Constraints Transcription

To solve the continuous constrained optimization problem $(17)$ in real-time, we use the optimization variable of MINCO $(15)$ to eliminate all kinds of equality constraints $(17b)$-$(17d)$ (See MINCO for details). And penalty function method TODO is used to deal with the inequality constraints $(17e)$. Then, every integral is evaluated by a finite sum of sample points.

Finally, the continuous constrained optimization problem is converted to a discrete unconstrained optimization problem

$$\begin{array}{}\text{(18)}& \underset{\mathbf{q},\mathbf{T}}{min}\sum _x{\lambda }_{\star }{\tilde{J}}_{\star }(\mathbf{q},\mathbf{T},\delta ),\end{array}$$

where ${\tilde{J}}_{\star }$ are various terms of cost function or penalties, and ${\lambda }_{\star }$ are relative weights. Subscripts $\star =\{f,e,t,o,r,d\}$:

• ($f$) swarm formation similarity;
• ($e$) denote control effort;
• ($t$) total time;
• ($o$) obstacle avoidance;
• ($r$) swarm reciprocal avoidance;
• ($d$) dynamic feasibility.

$\delta$ is the sampling time interval.

In the previous work[2], we used the fixed number sampling points ${\hat{\mathbf{p}}}_{i,j}=p_i((j/{\kappa }_i)\cdot T_i)$ to transform the optimization problem, where $p_i(t)$ is the $i^{th}$ piece trajectory and ${\kappa }_i$ is the fixed sample number on this piece.

However, considering that the total time $T_{\mathrm{\Sigma }}$ changes during the optimization process, the fixed number sampling points ${\hat{\mathbf{p}}}_{i,j}$ are difficult to space on the whole trajectory equally.

Therefore, we take fixed-time-interval sampling points for the whole trajectory to ensure the accuracy of the penalty function sampling transformation

$$\begin{array}{rl}& {\tilde{\mathbf{p}}}_j(t)=p_i(j\delta -\sum _{l=1}^{i-1}{T}_l),\\ \text{(19)}& & j\in \{0,\cdots ,\kappa \},\kappa =\lfloor \frac{T_{\mathrm{\Sigma }}}{\delta }\rfloor ,\end{array}$$

where $\kappa$ is the sample number and $T_l$ is the preceding time for any $1⩽l<i$.

For the trajectory planning of swarm robots, the fixed time interval sampling points ${\tilde{\mathbf{p}}}_j(t)$ can simplify the optimization problem. Compared with ${\hat{\mathbf{p}}}_{i,j}$, the timestamp corresponding to ${\tilde{\mathbf{p}}}_j(t)$ is fixed, so the states of other robots at this timestamp are also constant during the optimization process.

Therefore, it is feasible to calculate the states of other robots w.r.t ${\tilde{\mathbf{p}}}_j(t)$ according to the broadcast trajectories before optimization.

Then we can solve the uniform formation position sequence optimization $(12)$ in advance and use ${\hat{\mathbf{p}}}_i^{\ast }$ to replace the formation similarity metric $f_s$ in trajectory optimization $(17a)$ of $i^{th}$ robot. This decoupled formation trajectory optimization results in higher computational efficiency, making our method suitable for large-scale swarm robots.

Despite the optimization problem is not differentiable when sampling number $\kappa$ changes, the cost function remains continuous w.r.t. time duration $\mathbf{T}$. We use the quasi-Newton method to solve the non-smooth discrete unconstrained optimization problem $(18)$.

2.4 Cost Functions and Gradients

Given the fixed sampling time interval $\delta$, we can evaluate the cost functions and gradients of the whole trajectory by a finite sum of sampling points ${\tilde{\mathbf{p}}}_j(t)$.

The cost of various general purpose penalties at $j^{th}$ sampling points is

$$\begin{array}{}\text{(20)}& {\mathcal{P}}_{\star }(\mathbf{c},\mathbf{T},j\delta )={\mathcal{P}}_{\star }({\tilde{\mathbf{p}}}_j(t)),\end{array}$$

then the cost function ${\tilde{J}}_{\star }$ in $(18)$ is calculated as follows

$$\begin{array}{rl}{\tilde{J}}_{\star }(\mathbf{q},\mathbf{T},\delta )=& J_{\star }(\mathbf{c},\mathbf{T},\delta ),\\ =& \delta \sum _{j=0}^{\kappa }{\overline{\omega }}_j{\mathcal{P}}_{\star }(\mathbf{c},\mathbf{T},j\delta )+\\ \text{(21)}& & +\frac{1}{2}(T_{\mathrm{\Sigma }}-\kappa \delta )[{\mathcal{P}}_{\star }(\mathbf{c},\mathbf{T},\kappa \delta )+{\mathcal{P}}_{\star }(\mathbf{c},\mathbf{T},T_{\mathrm{\Sigma }})],\end{array}$$

where $({\overline{\omega }}_0,{\overline{\omega }}_1,\cdots ,{\overline{\omega }}_{\kappa -1},{\overline{\omega }}_{\kappa })=(1/2,1,\cdots ,1,1/2)$ are the orthogonal coefficients following the trapezoidal rule.

And MINCO allows any second-order continuous cost function ${\tilde{J}}_{\star }(\mathbf{q},\mathbf{T})$ to be represented by $J_{\star }(\mathbf{c},\mathbf{T})$. Hence, $\partial {\tilde{J}}_{\star }/\partial \mathbf{q}$ and $\partial {\tilde{J}}_{\star }/\partial \mathbf{T}$ can be efficiently obtained from $\partial J_{\star }/\partial \mathbf{c}$ and $\partial J_{\star }/\partial \mathbf{T}$ respectively, which is benefit to the construction and solution of the optimization problem.

In $(19)$, the sampling time $t=j\delta -\sum _{l=1}^{i-1}{T}_l$ is related to the preceding time $T_l$, so the gradient of $J_{\star }$ w.r.t ${\mathbf{c}}_i$ and $T_l$ are computed as

$$\begin{array}{}\text{(22)}& \frac{\partial J_{\star }}{\partial {\mathbf{c}}_i}& =\frac{\partial J_{\star }}{\partial {\mathcal{P}}_{\star }}\frac{\partial {\mathcal{P}}_{\star }}{\partial {\tilde{\mathbf{p}}}_j(t)}\frac{\partial {\tilde{\mathbf{p}}}_j(t)}{\partial {\mathbf{c}}_i},\text{(23)}& \frac{\partial J_{\star }}{\partial T_l}& =\frac{\partial J_{\star }}{\partial {\mathcal{P}}_{\star }}\frac{\partial {\mathcal{P}}_{\star }}{\partial {\tilde{\mathbf{p}}}_j(t)}\frac{\partial {\tilde{\mathbf{p}}}_j(t)}{\partial t}\frac{\partial t}{\partial T_l},\text{(24)}& \frac{\partial {\tilde{\mathbf{p}}}_j(t)}{\partial {\mathbf{c}}_i}& =\mathit{\beta }(t),\frac{\partial {\tilde{\mathbf{p}}}_j(t)}{\partial t}={\dot{\tilde{\mathbf{p}}}}_j(t),\frac{\partial t}{\partial T_l}=\{\begin{array}{ll}0,& l=i,\\ -1,& l<i,\end{array}\end{array}$$

where the calculation of $\partial J_{\star }/\partial {\mathcal{P}}_{\star }$ is simple and the details of ${\mathcal{P}}_{\star }({\tilde{\mathbf{p}}}_j(t))$ for various general purpose are given as follow.

• Cost of Swarm Formation Similarity ${\mathcal{P}}_f$ In Sec. 1.3, we decouple the formation similarity error metric from trajectory optimization by constructing an unconstrained optimization problem to calculate the uniform optimal formation position sequence ${\hat{\mathbf{p}}}_i^{\ast }$ for each sampling point. This improvement avoids multiple calculations of formation similarity metric $f_s$. Then, we use the quadratic form to calculate the cost of swarm formation similarity

$$\begin{array}{}\text{(25)}& {\mathcal{P}}_f({\tilde{\mathbf{p}}}_j(t))=max\{\parallel {\tilde{\mathbf{p}}}_j(t)-{\hat{\mathbf{p}}}_{i,j}^{\ast }{\parallel }^2,0{\}}^3.\end{array}$$

• Control Effort $J_e$ The $s^{th}$ ($s=3$ here) control input for the trajectory and its gradients are written as

$$\begin{array}{}\text{(26)}& J_e& =\sum _{i=1}^M{\int }_0^{T_i}\parallel p_i^{(s)}(t){\parallel }^2\mathrm{d}t,\text{(27)}& \frac{\partial J_e}{\partial {\mathbf{c}}_i}& =2({\int }_0^{T_i}{\mathit{\beta }}^{(s)}(t){\mathit{\beta }}^{(s)}(t{)}^{\mathrm{\top }}\mathrm{d}t){\mathbf{c}}_i,\text{(28)}& \frac{\partial J_e}{\partial T_i}& ={\mathbf{c}}_i^{\mathrm{\top }}{\mathit{\beta }}^{(s)}(T_i){\mathit{\beta }}^{(s)}(T_i{)}^{\mathrm{\top }}{\mathbf{c}}_i.\end{array}$$

• Total Time $J_t$ In order to ensure the aggressiveness of the trajectory, we minimize the total time $J_t=\sum _{i=1}^M{T}_i$. The gradients are given by $\partial J_t/\partial \mathbf{c}=\mathbf{0}$ and $\partial J_t/\partial \mathbf{T}=\mathbf{1}$.

• Cost of Obstacle Avoidance ${\mathcal{P}}_{\mathcal{o}}$ Obstacle avoidance penalty $J_o$ is computed using Euclidean Signed Distance Field (ESDF). We penalize the sampling points which are too close to the obstacles

$$\begin{array}{}\text{(29)}& {\mathcal{P}}_o({\tilde{\mathbf{p}}}_j(t))& =max\{{\psi }_o({\tilde{\mathbf{p}}}_j(t)),0{\}}^3,\text{(30)}& {\psi }_o({\tilde{\mathbf{p}}}_j(t))& =d_o-d_o({\tilde{\mathbf{p}}}_j(t)),\end{array}$$

where $d_o$ is the safety threshold set according to the actual situation and $d_o({\tilde{\mathbf{p}}}_j(t))$ is the distance between ${\tilde{\mathbf{p}}}_j(t)$ and the closest obstacle around it. The gradient of ${\mathcal{P}}_o$ w.r.t ${\tilde{\mathbf{p}}}_j(t)$ is

$$\begin{array}{}\text{(31)}& \frac{\partial {\mathcal{P}}_o}{\partial {\tilde{\mathbf{p}}}_j(t)}=-\mathrm{\nabla }{d}^{\mathrm{\top }},\end{array}$$

where the $\mathrm{\nabla }d$ is the gradient of ESDF in ${\tilde{\mathbf{p}}}_j(t)$.

• Cost of Swarm Reciprocal Avoidance ${\mathcal{P}}_{\mathcal{r}}$ We penalize ${\tilde{\mathbf{p}}}_j(t)$ when it is too close to the trajectories $p_{\varphi }(t),\varphi \in \mathrm{\Phi }$ at the fixed timestamp $t=j\delta$, where $\mathrm{\Phi }$ represents the all other robots in the swarm. Compared to previous work[2], the state of other robots with fixed timestamp $p_{\varphi }(j\delta )$ are constant during the optimization process and do not produce a gradient w.r.t $\mathbf{T}$ for the cost function $J_r$. So the optimization problem and the gradients are simplified. The cost of swarm reciprocal avoidance is defined as

$$\begin{array}{}\text{(32)}& {\mathcal{P}}_r({\tilde{\mathbf{p}}}_j(t))=\sum _{\mathrm{\Phi }}max\{{\psi }_r({\tilde{\mathbf{p}}}_j(t),p_{\varphi }(j\delta )),0{\}}^3,\text{(33)}& {\psi }_r({\tilde{\mathbf{p}}}_j(t),p_{\varphi }(j\delta ))=d_r^2-\parallel {\tilde{\mathbf{p}}}_j(t)-p_{\varphi }(j\delta ){\parallel }^2,\end{array}$$

where $d_r$ is the safe clearance between each robot. And the gradient of ${\mathcal{P}}_r$ w.r.t ${\tilde{\mathbf{p}}}_j(t)$ is

$$\begin{array}{}\text{(34)}& \frac{\partial {\mathcal{P}}_r}{\partial {\tilde{\mathbf{p}}}_j(t)}=-2({\tilde{\mathbf{p}}}_j(t)-p_{\varphi }(j\delta ){)}^{\mathrm{\top }}.\end{array}$$

• Cost of Dynamic feasibility ${\mathcal{P}}_{\mathcal{d}}$ We limit the maximum value of velocity and acceleration to guarantee that the robots can execute the trajectory.

$$\text{begin{aligned} mathcal{P}\_d(tilde{mathbf{p}}\_j(t))\&=mathcal{P}\_{d,v}(tilde{mathbf{p}}\_j(t))+mathcal{P}\_{d,a}(tilde{mathbf{p}}\_j(t)), mathcal{P}\_{d,v}(tilde{mathbf{p}}\_j(t)) \&= max { parallel dot{tilde{mathbf{p}}}\_j(t)parallel^2 - v\_m^2, 0}^3, mathcal{P}\_{d,a}(tilde{mathbf{p}}\_j(t)) \&= max { parallel ddot{tilde{mathbf{p}}}\_j(t)parallel^2 - a\_m^2, 0}^3, tag{35} end{aligned}}$$

where $v_m$ and $a_m$ are the maximum velocity and acceleration.

2.5 Discussion on solution quality of trajectory optimization

The proposed trajectory optimization process $(17)$ aims to solve a challenging multi-stage Linear Quadratic Minimum Time (LQMT) problem, which is inherently non-convex and non-linear. Additionally, incorporating ESDF for obstacle avoidance introduces further non-convex constraints. As a result, guaranteeing the global optimal solution with the quasi-Newton method is not always possible.

To address concerns regarding local minima and infeasible solutions, we have implemented measures that prioritize safety and dynamic feasibility while maintaining high-performance formation flight.

• Firstly, we utilize hybrid-A searching algorithm* to generate initial trajectories that are collision-free and dynamically feasible, ensuring a valid final solution trajectory.
• During optimization, we give greater weight to obstacle avoidance and dynamic constraints to prioritize safety and feasibility.
• Additionally, we conduct collision checks on trajectories to enhance safety.
• Moreover, our distributed swarm optimization framework effectively mitigates the impact of local minima on overall formation performance.

Implementing these measures, our method reliably achieves robust formation flight while maintaining computational efficiency.

9 Conclusion

Procedure (two-step decoupled formation trajectory optimization)

Inputs:

• Desired formation (relative positions) $\{{\mathbf{p}}_i^d{\}}_{i=1}^N$;
• Initial/terminal states $({\overline{\mathbf{p}}}_0,{\overline{\mathbf{p}}}_f)$;
• Obstacle ESDF $d_o(\cdot )$;
• Safety clearances $(d_o,d_r)$;
• Dynamic limits $(v_m,a_m)$;
• Weights ${\lambda }_{\star }$;
• Sampling interval $\delta$.

1. Describe the desired formation with a graph.

• Build the (complete) formation graph $\mathcal{G}$, compute $\mathbf{A}$, $\mathbf{D}$, and normalized Laplacian $\hat{\mathbf{L}}$ via $(1)$–$(4)$.
• From $\{{\mathbf{p}}_i^d\}$ compute ${\hat{\mathbf{L}}}_{\text{des}}$ (codes).

bool SwarmGraph::setDesiredForm()

2. Define formation similarity and its gradients.

• Use the differentiable similarity metric $f_s(\hat{\mathbf{L}},{\hat{\mathbf{L}}}_{\text{des}})$ in $(5)$ and its gradients $(6)$–$(8)$ to measure formation-shape error (codes).

bool PolyTrajOptimizer::swarmGraphGradCostP()

3. Precompute the (uniform) optimal formation position sequence.

• For each robot $i$ and each discrete timestamp $j\in \{0,\dots ,M_c\}$, solve the formation-only optimization to obtain the optimal formation positions ${\mathbf{p}}_{i,j}^{\ast }$ (Sec. 1.3).
• Replace the $O(N^2)$ formation similarity term by a quadratic distance surrogate $(10)$, yielding the decoupled two-step pipeline $(11)$.
• Add the uniformity regularizer and solve $(12)$–$(14)$ to obtain the uniform sequence ${\hat{\mathbf{p}}}_{i,j}^{\ast }$.

4. Initialize a feasible trajectory for each robot.

• Generate an initial collision-free, dynamically feasible trajectory (e.g., hybrid-A*) to warm start the optimizer (Sec. 2.5). Codes: computeInitReferenceState $\to$ astarWithMinTraj $\to$ astarSearchAndGetSimplePath $\to$ AstarSearch

5. Parameterize each trajectory using MINCO.

• Represent the piecewise polynomial trajectory with variables $(\mathbf{q},\mathbf{T})$ using $(15)$–$(16)$: (Codes: Variables & Grad in costFunctionCallback).
• Formulate the distributed local optimization objective and constraints as $(17)$.

6. Transcribe continuous constraints with fixed-interval sampling.

• Sample the full trajectory using the fixed time interval $\delta$ to obtain ${\tilde{\mathbf{p}}}_j(t)$ as in $(19)$, so timestamps remain constant while $\mathbf{T}$ changes.
• For reciprocal avoidance, query other robots’ broadcast trajectories at the fixed timestamps $t=j\delta$.

7. Assemble the discrete unconstrained optimization objective.

• Convert $(17)$ into the sampled objective $(18)$ using trapezoidal integration $(21)$.
• Use the following penalty/cost terms at samples:
  • Formation similarity via distance to ${\hat{\mathbf{p}}}_{i,j}^{\ast }$ $(25)$.
  • Control effort and time terms $(26)$–$(28)$.
  • Obstacle avoidance using ESDF $(29)$–$(31)$.
  • Reciprocal avoidance $(32)$–$(34)$.
  • Dynamic feasibility $(35)$.

8. Optimize per robot with quasi-Newton and analytic gradients.

• Compute gradients w.r.t. coefficients and durations using the MINCO mapping and chain rules $(22)$–$(24)$.
• Solve $(18)$ with a quasi-Newton method until convergence; run collision checks and keep high weights on obstacle/dynamic penalties for safety (Sec. 2.5).

9. Execute and iterate.

• Broadcast the optimized trajectory to neighbors and execute; repeat the above steps in a distributed replanning loop as new environment/neighbor information arrives.