CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (2024)

Yibin Yang, Shaobing Xu, Xintao Yan, Junkai Jiang, Jianqiang Wang, Heye HuangThis work was supported in part by National Natural Science Foundation of China, Science Fund for Creative Research Groups (52221005) and the Key Project (52131201). (Corresponding author: Heye Huang.)Y. Yang, J. Jiang, S. Xu, J. Wang and H. Huang are with the School of Vehicle and Mobility, Tsinghua University, Beijing 100084, China. (Email: yyb19,hhy18,jiangjk21@mails.tsinghua.edu.cn; shaobxu, wjqlws@tsinghua.edu.cn).X. Yan is with the Department of Civil and Environmental Engineering, University of Michigan, Ann Arbor, MI 48109 USA. (xintaoy@umich.edu)

Abstract

This paper presents an efficient algorithm, naming Centralized Searching and Decentralized Optimization (CSDO), to find feasible solution for large-scale Multi-Vehicle Trajectory Planning (MVTP) problem.Due to the intractable growth of non-convex constraints with the number of agents, exploring various hom*otopy classes that imply different convex domains, is crucial for finding a feasible solution.However, existing methods struggle to explore various hom*otopy classes efficiently due to combining it with time-consuming precise trajectory solution finding.CSDO, addresses this limitation by separating them into different levels and integrating an efficient Multi-Agent Path Finding (MAPF) algorithm to search hom*otopy classes.It first searches for a coarse initial guess using a large search step, identifying a specific hom*otopy class.Subsequent decentralized Quadratic Programming (QP) refinement processes this guess, resolving minor collisions efficiently.Experimental results demonstrate that CSDO outperforms existing MVTP algorithms in large-scale, high-density scenarios, achieving up to 95% success rate in 50m $\times$ 50m random scenarios around one second. Source codes are released in https://github.com/YangSVM/CSDOTrajectoryPlanning.

Index Terms:

Multi-robot systems, Path planning for multiple mobile robots or agents, nonholonomic motion planning.

I Introduction

Multi-vehicle trajectory planning (MVTP) seeks to generate a set of collision-free trajectories for multiple vehicles, from current positions to pre-set goals in a known unstructured environment, while minimizing travel time [1, 2]. It is a fundamental problem with diverse applications, such as cooperative parking and warehouse automation. In practical applications, there is a need to efficiently obtain solutions with a limited time [3]. As a non-convex optimization problem, MVTP necessitates a trade-off between solution quality and computational efficiency [4]. Particularly, in scenarios involving a large number of vehicles within a narrow space, the frequency of vehicle-to-obstacle and vehicle-to-vehicle conflicts increases, complicating the search for optimal or even feasible solutions [5, 6]. This work aims to develop an efficient algorithm that quickly finds feasible solutions with a high success rate for large-scale MVTP problems.

I-A Related Work

The hom*otopy class can be loosely defined as a set of solutions that are capable of continuous deformation into one another, without intersecting obstacles or other agents [7]. Exploring various hom*otopy classes is essential for solving large-scale MVTP problems [1]. Therefore, we will evaluate the scalability of current MVTP algorithms in narrow space with their ability to explore hom*otopy classes. Table I summarizes the existing MVTP algorithms.

Methods

Feature

Runtime

Narrow Space

Scalability

Coupled

Optimal; Complete.

Very slow

Very Small

Distributed planning

Lack of cooperation.

Fast

Small

Constraint Reduction

Optimal.

Relative slow

Large

Tube construction

Highly rely on initial guess.

Fast

Small

Grid search based

Underuse efficient MAPF;

Step size trade-off.

Variable

Large

Coupled planning methods [8, 9] treat all vehicles as a single, very high dimensional agent. This approach relies solely on the optimizer’s capability to traverse between different hom*otopy classes. While coupled planning methods guarantee completeness and even optimality, the computational complexity increases rapidly with growing number of non-convex constraints.In general, coupled planning methods exhibit poor scalability.

Distributed Planning methods address MVTP in a single-agent manner, treating others as moving obstacles [10, 11, 12]. They have high efficiency in sparse scenarios but struggle with coordination, restricting the exploration of hom*otopy classes. In practice, these methods often struggle to generate high-quality collaboration and the success rate decreases as scale increases, particularly in obstacle-dense scenarios.

Constraint reduction [1, 13, 14] dynamically adjusts the problem’s complexity by adding or removing constraints, continuously approaching a feasible or even optimal solution.They achieve transitions between different hom*otopy classes by solving different nonlinear programming problems (NLP). However, solving NLP can be time-consuming.

Tube construction methods [15, 16, 17] construct a safe corridor for each vehicle so the vehicle can be separated with the obstacles and other vehicles.Tube construction’s solution is strictly hom*otopic with the reference trajectories. Therefore, it only searches limited hom*otopy classes and performs poorly without an approximately feasible initial guess.

Grid search [18, 2] based methods discretize vehicle poses, actions, and space, utilizing a search algorithm to find discrete trajectories. This search algorithm is closely linked to a well-studied problem known as Multi-Agent Path Finding (MAPF), focusing on planning collision-free paths for multiple agents in a grid-like environment while minimizing travel time.Despite the NP-hard nature of MAPF, various efficient sub-optimal algorithms can generate paths for hundreds of agents in under a second [19], aligning with MVTP’s need for finding feasible solutions efficiently. However, the potential of these efficient MAPF algorithms remains largely unexplored in the MVTP field [20]. Moreover, akin to single-agent grid searching motion planning algorithms, when the search step size is too small, the search space becomes too large, posing challenges for real-time requirements. Conversely, when the search step size is too large, the solution space diminishes, making it challenging to find a solution, and collisions may occur between search steps, rendering the solution infeasible. Therefore, grid search-based methods are more suitable for generating a coarse initial guess containing hom*otopy class information rather than directly generating fine solutions.

I-B Motivation and Contribution

Existing methods search limited hom*otopy classes or search them inefficiently.In this letter, we propose a hierarchical planner, centralized searching and decentralized optimization (CSDO), tailored for large-scale MVTP problem. Specifically, by adapting the efficient MAPF algorithm priority based search (PBS) [19] for MVTP to generate an initial guess, CSDO aims at searching various hom*otopy classes with a large grid size, despite initial minor collisions. We further employ a tube construction method to generate a safe and precise solution. By transforming multi-vehicle constraints into single-vehicle planning problems near this initial guess, we simplify the MVTP into parallel, solvable ones, enhancing efficiency.

Accordingly, the main contributions are outlined as follows.

1.
CSDO, an efficient, scalable multi-vehicle trajectory planning algorithm, employs a hierarchical framework to enhance search capabilities across diverse hom*otopy classes. Experiments demonstrate CSDO outperforms existing methods in random scenarios, especially in large scale and high-density environments.
2.
A seamless adaptation of the priority-based search method from the MAPF domain into the complex non-holonomic MVTP problems, enabling efficient exploration for feasible or near-feasible solutions.
3.
An efficient distributed local solver is introduced. Given a hom*otopically correct reference solution, the local solver can generate feasible solutions within approximately 300 ms for nearly 100 agents on a typical 100 m × 100 m size random map.

II Problem Definition

A MVTP problem can be defined by a ten element tuple $\langle M,\mathcal{W},\mathcal{O},z,\mathcal{R},s,g,f,\mathcal{T},P\rangle$ . Consider a system consisting of $M$ front-steering agents $a^{(1)},a^{(2)},...,a^{(M)}$ operating in a continuous planar workspace $\mathcal{W}\subset\mathbb{R}^{2}$ . We use $[M]$ to denote the set $\{1,2,...,M\}$ for simplicity. There is some random static obstacles lying in the environment and occupying the workspace $\mathcal{O}$ . We use superscript $(i)$ to represent the variable related to agent $a^{(i)}$ . $z^{(i)}=[x^{(i)},y^{(i)},\theta^{(i)}]^{T}\in\mathbb{R}^{3}$ refers to the state of agent $a^{(i)}$ , where $(x^{(i)},y^{(i)})$ is the position of rear axis center of agent $a^{(i)}$ and $\theta^{(i)}$ is its yaw angle.We use agent occupancy function $\mathcal{R}(z):\mathbb{R}^{3}\rightarrow\mathcal{W}$ to represent the workspace occupied by the agent’s body at state $z$ .Agent’s trajectory is represented by a sequence of vehicle states sampled at fixed time interval $\Delta t$ . It is denoted as $\mathcal{T}=[z_{0},z_{1},...,z_{\tau}]$ , where the $\tau+1$ is the number of states in the trajectory.For one MVTP task, agent $a^{(i)}$ ’s trajectory $\mathcal{T}^{(i)}$ need to start from the start state $s^{(i)}$ and end at the goal state $g^{(i)}$ . The MVTP task is finished only when all the agents arrive at their goals at a specific time $\tau_{f}$ .

z^{(i)}_{0}=s^{(i)},z^{(i)}_{\tau_{f}}=g^{(i)},\forall i\in[M].

(1)

The control input of agent $a^{(i)}$ is denoted as $u^{(i)}=[v^{(i)},\omega^{(i)}]^{T}$ , where $\omega^{(i)}=\dot{\phi}^{(i)}$ , $\phi^{(i)}$ is the front-wheel steering angle, $v^{(i)}$ is the velocity. For agent $a^{(i)}$ , its planned trajectory $z^{(i)}_{t}$ should be kinematic feasible for the Ackermann-steering model $f$ . We denote $\zeta=[x,y,\theta,\phi]^{T}$ to include the state and steering angle. The kinematic model can be written as follows,

	$\displaystyle\zeta^{(i)}_{t+1}$	$\displaystyle=\zeta^{(i)}_{t}+\begin{bmatrix}v^{(i)}_{t}\cos(\theta^{(i)}_{t})%\\v^{(i)}_{t}\sin(\theta^{(i)}_{t})\\v^{(i)}_{t}\tan(\phi^{(i)}_{t})/L\\\omega\\\end{bmatrix}\Delta t,$		(2)
	$\displaystyle\forall 0$	$\displaystyle\leq t<\tau_{f},i\in[M],$		(2)

\left|v^{(i)}_{t}\right|\leq v_{max},\left|\omega^{(i)}_{t}\right|\leq\omega_{%max},\forall 0\leq t<\tau_{f},i\in[M],

(3)

\left|\phi^{(i)}_{t}\right|\leq\phi_{max},\forall 0\leq t\leq\tau_{f},i\in[M],

(4)

where the $L$ is the vehicle wheelbase. The agents cannot collide with static obstacles or any other agents, i.e.

\mathcal{R}(z^{(i)}_{t})\cap\mathcal{O}=\emptyset,\forall t\geq 0,\forall i\in%[M],

(5)

\mathcal{R}(z^{(i)}_{t})\cap\mathcal{R}(z^{(j)}_{t})=\emptyset,\forall t\geq 0%,\forall i,j\in[M],i\neq j.

(6)

The solution plan $P$ , comprising collision-free trajectories for all agents, is measured by its $makespan$ $\tau_{f}$ . Considering the summarized elements, a traditional optimal control problem can be formulated as

Minimize	$\displaystyle\quad\quad\tau_{f}$	(7)
s.t.	$\displaystyle\quad\textrm{Boundary Constraints (\ref{eq:boundary_ocp})},$
	$\displaystyle\quad\textrm{Kinematic Constraints (\ref{eq:kine_ocp})},(\ref{eq:%control_max_ocp}),(\ref{eq:phi_max_ocp}),$
	$\displaystyle\quad\textrm{Static Collision Constraints (\ref{eq:static_ocp})},$
	$\displaystyle\quad\textrm{Inter-Agent Collision Constraints (\ref{eq:inter_ocp%})}.$

III Method

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (1)

The overall CSDO framework is illustrated in Fig. 1. Upon receiving the start poses, goal poses, and obstacle information, these components are combined to form a Multi-Vehicle Trajectory Planning (MVTP) instance. Subsequently, the centralized priority based searching phase generates coarse trajectories as an initial guess. The decentralized Quadratic Programming (QP) refinement follows, where inter-vehicle constraints are divided and solved in parallel using the QP formulation. Finally, the obtained solution is then sent to the respective multiple vehicles, which subsequently track the collision-free trajectories to reach their goals.

III-A Centralized Priority based Searching

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (2)

By discretizing the MVTP problem, we can utilize search algorithms to find an initial guess. As in Fig. 2, the deiscretizing process consists of state $z$ discretization, kinematic model $f$ discretization and agent collision detection implementation in formula (5-6) as in single agent search algorithm hybrid A* [21]. The state is still stored as a continuous vector but only a limited number of states are kept. As in Fig. 2(a),we discretize the workspace $\mathcal{W}$ into $n_{xy}$ grids. Every grid only has $n_{\theta}$ angles. Then at most $n_{xy}\times n_{\theta}\times(\tau_{f}+1)\times M$ states can be stored. For every agent at a specific time, its continuous state $z$ will calculate a corresponding discrete state $\hat{z}=[\hat{x},\hat{y},\hat{\theta}]^{T}$ based on the L2 loss, where $(\hat{x},\hat{y})$ is the center position of the grid and $\hat{\theta}\in\{0,2\pi/n_{\theta},...,2\pi(n_{\theta}-1)/n_{\theta}\}$ . As in Fig. 2(a), if two states $z_{1}^{(i)}(t_{k})$ , $z_{2}^{(i)}(t_{k})$ of same agent and timestamp have the same discrete state $\hat{z^{(i)}(t_{k})}$ , then only the state with minimum cost is saved. The kinematic model $f$ is simplified to an action set. $Actions=\{FL,FS,FR,BL,BS,BR,Wait\}$ , which stand for front-max-steering-left, front-straight, front-max-steering-right, back-max-steering-left, back-straight, back-max-steering-right and wait respectively. Except for the $Wait$ action, all the actions, travels for the same distance. Collision detection utilizes the Separating Axis Theorem and checks only if vehicles collide at the sampling moment, without considering collisions between state transitions.

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (3)

Our overall framework is illustrated in Fig. 3. Centralized searching is divided into two layers of search. In the high level, each node represents a subproblem, and each node contains a set of partial priority orders as constraints. Specifically, for any two agents, if $i\prec j$ , then $j$ has a lower priority than $i$ , and $j$ treats $i$ as a dynamic obstacle to avoid in the low-level planner. For any two agents, they may not have a relationship of $i\prec j$ or $j\prec i$ , meaning they ignore each other and may have collisions. If collisions arise within a node’s plan, two possible partial orders according to a specific conflict will be added to generate child nodes until collisions are entirely resolved.

III-A1 High-level search

Input :MAPF instance

2 $Root\leftarrow$ GENERATEROOT() ;

III-A2 Low-level planner

We directly adopt the spatiotemporal Hybrid A* from [2] as our low-level planner. Given a workspace $\mathcal{W}$ and static obstacle occupancy workspace $\mathcal{O}$ , the higher priority agents’ trajectories $highTrajs$ , a predefined start state $s_{i}$ and goal state $g_{i}$ , the low-level path planner spatiotemporal hybrid A* (STHA*) will search a feasible trajectory. Compared with Hybrid A*, the STHA* add a time dimension to deal with the dynamic obstacles.

In the algorithm 2, it initiates by inserting the start node into open heap [Line 2]. Each node in open heap stores its state $z$ and timestamp t. The heap pops the node with minimum $f$ value, where $f=h+g$ , $h$ represents the admissible heuristic function, similar to that used in Hybrid A*, and $g$ denotes the action cost from the start node to the current node. Generally, the greater the distance traveled and the more frequent reversing or changing direction, the higher the $g$ value. A node is popped from the heap for expansion [Lines 4-5]; if the expanded path is valid and collision-free, a solution can be obtained [Lines 6-9]. Otherwise, we iterate through the action set $Actions={FL,FS,FR,BL,BS,BR,Wait}$ to generate subsequent neighbor states of $n$ [Lines 10-11]. Invalid nodes are discarded [Lines 12-13], while valid ones calculated their $f$ , $g$ , and $h$ values [Lines 14-15]. If $n^{\prime}$ is not in the open heap, we push it into the heap [Lines 16-17]. Alternatively, if a node $n^{\prime}$ matches an existing heap node by timestamp, and state but offers a lower $g$ cost value, it replaces the existing one [Lines 18-21]. The whole process terminates with an empty solution if the heap empties[Line 22].

Input : $\mathcal{O},highTrajs,s_{i},g_{i}$

1Open $\leftarrow$ new min heap sorted by node’s f value;

2Push $(s_{i},t=0)$ into Open;

3while $Open\neq\emptyset$ do

4 $Open\leftarrow$ Open.pop();

5 $path_{toGoal}\leftarrow AnalyticExpand(n,g_{i})$ ;

6if $path_{toGoal}$ has no Collision with $highTrajs$ and $\mathcal{O}$ then

7 $path_{toN}\leftarrow$ Get Path By Finding $n$ ’s preceding nodes ;

8 $path\leftarrow path_{toN}$ + $path_{toGoal}$ ;

9return $path$ ;

11foreach $action\in Actions$ do

12 $n^{\prime}\leftarrow$ Generate Next State from $n$ by $action$ ;

13if $n^{\prime}$ has collision with $highTrajs$ or $\mathcal{O}$ then

14continue ;

15 // drop this node

17 $n^{\prime}.cost\leftarrow n.cost+cost(action)$ ;

18 $n^{\prime}.h\leftarrow n^{\prime}.cost+heuristic(n^{\prime},g_{i})$ ;

19if $n^{\prime}\notin$ Openthen

20Push $n^{\prime}$ into Open ;

22else

23 $n_{same}^{\prime}\leftarrow$ same Discrete State as $n^{\prime}$ in Open;

24if $n^{\prime}.cost<n_{same}^{\prime}.cost$ then

25Replace the $n_{same}^{\prime}$ to $n^{\prime}$ in Open;

return $\emptyset$ ;

III-B Decentralized QP

As shown in Fig. 1, the decentralized QP takes an initial guess as input, then constructs safe corridors along them, and finally outputs trajectories for all agents.

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (4)

III-B1 Initial Guess Interpolation

The initial guess, after a simple interpolation, may encounter three types of minor collisions, as illustrated in Fig. 4: type A: collisions between two vehicles, type B: collisions with obstacles, and type C: off-map states. For clarity, we denote $\bar{x}$ as the variable related to the interpolated initial guess. For instance, $\bar{z}^{(i)}_{t}$ is the state of agent $i$ in the timestamp $t$ in the dense initial guess.

To facilitate collision detection, we employ two uniformly distributed circles to cover the rectangular shape of the vehicle [13]. As illustrated in the Fig. 5, the circle centers are positioned at the quadrant points. The formulas for calculating the centers and radii of the two circles are as follows.

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (5)

	$x^{F(i)}_{t}=x^{(i)}_{t}+L_{f2x}\cos{\theta^{(i)}_{t}},y^{F(i)}_{t}=y^{(i)}_{t%}+L_{f2x}\sin{\theta^{(i)}_{t}},$		(8a)
	$x^{R(i)}_{t}=x^{(i)}_{t}+L_{r2x}\cos{\theta^{(i)}_{t}},y^{R(i)}_{t}=y^{(i)}_{t%}+L_{r2x}\sin{\theta^{(i)}_{t}},$		(8b)
	$r_{v}=\frac{1}{2}\sqrt{\frac{1}{4}(L_{F}^{2}+L_{B}^{2})+L_{B}^{2}},$		(8c)

where the $L_{f2x}=(3L_{F}-L_{B})/4$ is distance from the front disc center to the rear-axis center, $L_{r2x}=(L_{F}-3L_{B})/4$ is distance from the rear disc center to the rear-axis center, $L_{F}$ is the distance from rear axis to the front bumper, $L_{B}$ is distance from rear axis to the rear bumper. $Y^{F(i)}_{t}=[x^{F(i)}_{t},y^{F(i)}_{t}]^{T}$ is the center of the front circle. $Y^{R(i)}_{t}=[x^{R(i)}_{t},y^{R(i)}_{t}]^{T}$ refers to the rear circle’s center. $r_{v}$ is the radius of the circle. We denote $Y=[x^{F},y^{F},x^{R},y^{R}]^{T}$ as the vector of the circles positions. For any $i\in[M],0\leq t\leq\tau_{f}$ , its linear form is as follows:

	$Y_{t}^{(i)}=D_{t}^{(i)}\zeta_{t}^{(i)}+e_{t}^{(i)},$		(9a)
	$D_{t}^{(i)}=\begin{bmatrix}1&0&-L_{f2x}\sin\bar{\theta}_{t}^{(i)}&0\\0&1&L_{f2x}\cos\bar{\theta}_{t}^{(i)}&0\\1&0&-L_{r2x}\sin\bar{\theta}_{t}^{(i)}&0\\0&1&L_{r2x}\cos\bar{\theta}_{t}^{(i)}&0\\\end{bmatrix},\\$		(9b)
	$e_{t}^{(i)}=\begin{bmatrix}L_{f2x}(\cos{\bar{\theta}_{t}^{(i)}}+\bar{\theta}_{%t}^{(i)}\sin\bar{\theta}_{t}^{(i)})\\L_{f2x}(\sin{\bar{\theta}_{t}^{(i)}}-\bar{\theta}_{t}^{(i)}\cos\bar{\theta}_{t%}^{(i)})\\L_{r2x}(\cos{\bar{\theta}_{t}^{(i)}}+\bar{\theta}_{t}^{(i)}\sin\bar{\theta}_{t%}^{(i)})\\L_{r2x}(\sin{\bar{\theta}_{t}^{(i)}}-\bar{\theta}_{t}^{(i)}\cos\bar{\theta}_{t%}^{(i)})\end{bmatrix}.\\$		(9c)

III-B2 Neighbor Pair Searching

Given a distance threshold $R_{trust}$ , we iterate through the plan to search pairs of agents $a^{(i)}$ and $a^{(j)}$ with a distance less than $2\sqrt{2}R_{trust}$ at the same timestamp. The distance function between two states $(\zeta^{(i)}_{t},\zeta^{(j)}_{t})$ is defined as follows:

	$\displaystyle dist$	$\displaystyle(\zeta^{(i)}_{t},\zeta^{(j)}_{t})=\min(\left\\|Y^{F(i)}_{t}-Y^{F(j%)}_{t}\right\\|,\left\\|Y^{F(i)}_{t}-Y^{R(j)}_{t}\right\\|,$		(10)
		$\displaystyle\quad\left\\|Y^{R(i)}_{t}-Y^{F(j)}_{t}\right\\|,\left\\|Y^{R(i)}_{t}%-Y^{R(j)}_{t}\right\\|)-2r_{v},$		(10)

where the $Y^{F(i)}_{t}$ , $Y^{R(i)}_{t}$ , $Y^{F(j)}_{t}$ , $Y^{R(j)}_{t}$ are the agent $a^{(i)}$ ’s front disc center, back disc center, agent $a^{(j)}$ ’s front disc center and rear disc center respectively. The search result, denoted as $NPairs$ , comprises neighbor pairs and their corresponding timestamps. $NPairs=\{(a^{(i)},a^{(j)},t)|dist(\zeta^{(i)}_{t},\zeta^{(j)}_{t})<2\sqrt{2}R_%{trust},\forall i,j\in[M],\forall 0\leq t\leq\tau_{f}\}$ .

III-B3 Neighbor Pair Separation

After obtaining the neighbor pairs $NPairs$ , we utilize the plane derived by the perpendicular bisector of the disc’s center as the constraint for mutual avoidance between vehicles. Each neighbor pair $(a^{(i)},a^{(j)},t)$ generates 4 separation planes { $C_{ffj}^{(i)},C_{frj}^{(i)},C_{rfj}^{(i)},C_{rrj}^{(i)}$ } for agent $a^{(i)}$ . Fig. 5 illustrates the process foragent $a^{(i)}$ and agent $a^{(j)}$ —calculating perpendicular bisectors, offset by a distance of $r_{v}$ on both sides to obtain the separation half-plane $C_{ffj}^{(i)}$ . Similarly, we generate the separation half-plane $C_{frj}^{(i)}$ for front-to-rear disc avoidance between agents $a^{(i)}$ and $a^{(j)}$ , and obtain the separation half-planes $C_{rfj}^{(i)}$ and $C_{rrj}^{(i)}$ between the rear disc of agent $i$ and the discs of agent $a^{(j)}$ .

A_{c}^{(i)}Y^{(i)}\leq\overrightarrow{0},\forall i\in[M]

(11)

where the $A_{c}^{(i)}$ denotes the corresponding half planes which are relevant to agent $a^{(i)}$ according to $NPairs$ .When the initial guess is given, $A_{c}^{(i)}$ is a constant matrix.

To focus the search within the neighbor of the initial guess, we restrict the variation range of the disk to $R_{trust}$ , i.e.,

\left|Y^{(i)}_{t}-\bar{Y}^{(i)}_{t}\right|\leq R_{trust}\overrightarrow{1},%\forall i\in[M],0\leq t\leq\tau_{f}.

(12)

Remark.

Constraints (11)-(12) are equivalent to constraint (6), guaranteeing that there are no collisions between vehicles at each discrete time step.

For neighbor pairs, they are separated by the planes as in constraint (11). For non-neighbor pairs, they are separated by the variation range constraint (12).

Remark.

Constraints (11)-(12) decouple the trajectory variables between different vehicles.

$A_{c}^{(i)}$ and $\bar{Y}^{(i)}_{t}$ are constants determined by the initial guess. For any two different vehicles $a^{(i)}$ and $a^{(j)}$ , their variables $\zeta^{(i)}$ and $\zeta^{(j)}$ will not appear in the same inequality.

The above process decouples agents for distributed problem-solving. Without loss of generality, we describe the processing procedure for agent $a^{(i)}$ in the following steps.

III-B4 Robust Corridor Construction

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (6)

To handle the static obstacle avoidance constraint (5), we adapt the method from [23] to generate a corridor along its initial guess.

For clarity, ensuring that disc with radii $r_{v}$ does not go out of the map is equivalent to maintaining a distance of $r_{v}$ from the border. As in Fig. 6, we erode the map by a distance of $r_{v}$ to define the safety space enclosed by the dotted line. Similarly, we dilate the obstacles by a distance of $r_{v}$ .

As illustrated in Fig. 6, we sequentially extend the empty box clockwise in all four directions until it encounters dilated obstacles, the eroded map boundary, or reaches the maximum allowed length. Details can be found in [23].

Note that our initial point may be in a colliding or out-of-map state, as previously mentioned in Fig. 4, causing the algorithm to immediately return an empty box. Therefore, we must relocate the initial point to a safe position before generating the box. In cases of a colliding state, we move the point outside of the grey circle. If it remains unsafe due to collisions with other obstacles, we gradually rotate it around the obstacle center until it becomes safe. For initial points that are out-of-map, we project them onto the map boundary. The corridor constraints can be summarized as follows,

Y^{(i)}_{t,min}\leq A_{static,t}^{(i)}Y^{(i)_{t}}\leq Y^{(i)}_{t,max},\forall 0%\leq t\leq\tau_{f},

(13)

where $A_{static,t}^{(i)}$ denotes the generated corridor as in Fig. 6.

III-B5 QP Formulation

Finally, we linearize the kinematic constraints. The objective function is set to minimize changes in velocity and steering wheel angle, aiming to smooth the trajectory.The kinematic constraints are linearized as follows:

	$\zeta_{t+1}=A_{t}\zeta_{t}+B_{t}u_{t}+c_{t},$		(14a)
	$A_{t}=\begin{bmatrix}1&0&-\bar{v}_{t}\sin\bar{\theta}_{t}\Delta t&0\\0&1&-\bar{v}_{t}\cos\bar{\theta}_{t}\Delta t&0\\0&0&1&\frac{\bar{v}_{t}*\Delta t}{L\cos^{2}\bar{\phi}}\\0&0&0&1\end{bmatrix},$		(14b)
	$B_{t}=\begin{bmatrix}\cos\bar{\theta}_{t}\Delta t&0\\\sin\bar{\theta}_{t}\Delta t&0\\\frac{\tan\bar{\phi}*\Delta t}{L}&0\\0&\Delta t\end{bmatrix},$		(14c)
	$c_{t}=[\bar{\theta}_{t}\bar{v}_{t}\sin\hat{\theta_{t}}\Delta t,-\bar{\theta}_{%t}\bar{v}_{t}\cos\hat{\theta_{t}}\Delta t,-\frac{\bar{\phi}\Delta t}{L\cos^{2}%{\phi_{t}}}]^{T},$		(14d)

where $(A_{t},B_{t},c_{t})$ are the associated coefficients at time $t$ .

The objective function is as follows,

J=\Sigma_{t}(\alpha_{v}(\Delta v_{t}^{(i)})^{2}+\alpha_{\omega}(\omega_{t}^{(i%)})^{2}),

(15)

where $\alpha_{v}$ , $\alpha_{\omega}$ are the weighting parameters.

With the aforementioned elements summarized, a complete QP is formulated as follows,

$\displaystyle\min_{\zeta^{(i)},u^{(i)}}$	$\displaystyle J$	(16)
s.t.	$\displaystyle\zeta_{0}^{(i)}=[s^{(i)T},0]^{T},\zeta_{\tau_{f}}^{(i)}=[g^{(i)T}%,0]^{T},$
	$\displaystyle\left\|v^{(i)}_{t}\right\|\leq v_{max},\left\|\omega^{(i)}_{t}\right%\|\leq\omega_{max},\forall 0\leq t<\tau_{f},$
	$\displaystyle\left\|\phi^{(i)}_{t}\right\|\leq\phi_{max},\forall 0\leq t\leq\tau%_{f},$
	$\displaystyle\textrm{Constraints}\quad(\ref{eq:inter})-(\ref{eq:linear_kine})%\quad\textrm{related to $a^{(i)}$}$

IV Experiment

To demonstrate the effectiveness of our method, we conduct experiments on randomly generated obstructed maps as well as obstacle-free maps. We incrementally increased the number of agents in the problem, resulting in more congested maps, with a specific emphasis on showcasing the effectiveness of our approach in addressing large-scale MVTP problems.

IV-A Simulation Settings

The benchmark consists of various map sets with different sizes (50m $\times$ 50m, 100m $\times$ 100m), varying numbers of agents (5 to 100), and includes both obstructed and obstacle-free maps. Each map set contains 60 instances, resulting in a total of 1800 test instances in this testing benchmark.We assume all agents are hom*ogeneous and share the following parameters: the vehicle’s shape is 3 m × 2 m, with a minimum turning radius of 3 m, and a maximum speed of 1 m/s.All algorithms are assessed using their respective open-source implementations, with most executed on an Intel Xeon Gold 622R CPU at 2.90 GHz in C++, while Fast-ASCO runs on an Intel Core i7-9750H CPU at 2.6 GHz in Matlab.

IV-B Simulation results

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (7)

CSDO: Enhancing Efficiency and Success in Large-Scale Multi-Vehicle Trajectory Planning (8)

Method	50m*50m 25 obstacles map with 5,10,15,20,25 agents
	Success Rate $\uparrow$					Runtime (s) $\downarrow$					Makespan (s) $\downarrow$
SCC	100.00%	95.00%	81.67%	36.67%	5.00%	0.04	0.42	2.16	7.14	13.39	43.35	48.74	52.21	54.99	54.55
PP	100.00%	95.00%	83.33%	85.00%	51.67%	0.03	0.14	0.36	1.23	2.80	43.35	49.70	53.29	60.03	65.17
FastASCO	90.00%	73.33%	70.00%	65.00%	51.67%	2.46	9.86	24.12	50.64	99.00	56.12	61.58	67.87	73.11	74.73
CSDO	95.00%	95.00%	98.33%	96.67%	90.00%	0.04	0.10	0.25	0.90	3.42	48.42	55.32	59.71	67.06	73.77
	50m*50m empty map with 5,10,15,20,25 agents
SCC	100.00%	98.33%	98.33%	70.00%	10.00%	0.03	0.40	1.70	7.19	8.63	41.71	48.22	51.36	52.25	56.34
PP	100.00%	98.33%	96.67%	90.00%	73.33%	0.02	0.09	0.23	0.58	1.28	41.66	48.52	51.75	54.95	59.48
FastASCO	98.33%	96.67%	88.33%	93.33%	85.00%	1.44	7.09	16.52	31.89	53.85	52.25	59.88	63.26	66.26	68.82
CSDO	100.00%	100.00%	98.33%	98.33%	96.67%	0.01	0.03	0.08	0.26	0.83	44.75	52.53	58.31	62.25	66.46

The performance of CSDO is evaluated through a comparative analysis with various MVTP algorithms, focusing on success rate, runtime, and solution quality (makespan). A general time limit of 20 s is applied, except for Fast-ASCO, which is allowed 200 s due to Matlab implementation. Figure 7 presents the results for both map sizes, while Table II displays results for a map size of 50 m $\times$ 50 m.

Seq-CL-CBS (SCC) [2] is a grid search-based method and a prioritized planning version of CL-CBS for large-scale problems. CL-CBS, leveraging the optimal MAPF algorithm Conflict based Search (CBS), forms the basis for SCC, which organizes agents into $k_{b}$ groups for sequential planning with CL-CBS; each subsequent group views the prior as dynamic obstacles. With $k_{b}=2$ , SCC performs well with small numbers of agents and generally achieves the best makespan. However, its computation time increases exponentially with large-scale problems due to CBS. In sum, SCC performs worse than CSDO in terms of success rate and runtime.

Prioritized Planning (PP) is a grid search-based prioritized planning method. It randomly assigns each agent an order and plans their trajectories sequentially based on this order. It is implemented by setting $k_{b}=M$ using SCC. In theory, PP is not a complete or optimal method and may perform poorly if an inappropriate order is chosen. In our simulations, PP scales better than SCC, and the solution quality does not deteriorate significantly. However, compared to CSDO, PP exhibits poorer performance in terms of success rate and runtime. CSDO relies on PBS, which searches various partial order sets and has higher completeness. Additionally, CSDO dynamically adjusts its priority during planning to optimize performance. Thus, in crowded scenarios, CSDO may run faster than PP. Regarding solution quality, PP may outperform CSDO, as CSDO prioritizes success rate over makespan optimization.

Fast ASCO [13], an advanced ASCO variant [1], excels in constraint reduction, offering an optimal solution despite slower performance in Matlab.It optimizes for travel time and comfort but has longer makespan due to a detailed kinematics model. Despite this, it achieves superior success rates in large-agent scenarios, outperforming near-optimal methods like SCC for groups of 20 and 25 agents in a 50 m $\times$ 50 m map.

CSDO has the best success rate and computation time, benefiting from PBS’s efficiency and its hierarchical framework. In scenarios with few agents, most methods achieve success rates close to or at 100%. However, as the number of agents increases from 5 to 25 in dense scenarios with 25 obstacles, CSDO maintains a 90% success rate, while existing algorithms begin breaking down, dropping to 51.67%. Moreover, CSDO operates under 1s for 20 agents, significantly outperforming other algorithms.

IV-C CSDO Limitation and Analysis

Number of agents	30	50	70	90
Success rate	96.67%	90.00%	11.67%	1.67%
Search success rate	100%	90.00%	11.67%	1.67%
Centralized Search time(s)	0.87	7.46	9.73	24.72
DQP time(s)	0.06	0.10	0.20	0.28

CSDO is an efficient MVTP algorithm with a high success rate, but it is not an optimal or complete algorithm due to constraints of PBS.As shown in Table III, the success rate is slightly lower than the search rate, indicating that failures are primarily caused by centralized searching.Meanwhile, it’s clear that the search time remains the primary component of the overall runtime.While the simulation demonstrate the efficiency and high success rate of CSDO, MAPF is an NP-hard problem. In simulated crowded scenarios, CSDO may still fail to solve due to timeouts.

V Conclusion

This work introduces CSDO, an efficient algorithm for large-scale multi-vehicle trajectory planning, leveraging a combination of centralized priority-based searching and decentralized optimization. By employing MAPF algorithms, CSDO efficiently identifies feasible solutions to the MVTP optimization challenge. It also accelerates the resolution of MVTP problems efficiently, by decomposing the joint optimization problem into a series of smaller, manageable ones that can be processed in parallel, enabling real-time processing near potential solutions.Through an extensive set of experiments, we demonstrate that CSDO efficiently discovers solutions within a limited time compared to other methods, without significant loss in solution quality, and supports collision-free trajectory planning in large-scale, high-density scenarios.In the future, we will try to extend our algorithm to real-vehicle test environments with dynamic and intensive traffic participants.

References

[1]B.Li, Y.Ouyang, Y.Zhang, T.Acarman, Q.Kong, and Z.Shao, “Optimal Cooperative Maneuver Planning for Multiple Nonholonomic Robots in a Tiny Environment via Adaptive-Scaling Constrained Optimization,” IEEE Robotics and Automation Letters, vol.6, no.2, pp. 1511–1518, Apr. 2021, conference Name: IEEE Robotics and Automation Letters.
[2]L.Wen, Y.Liu, and H.Li, “CL-MAPF: Multi-Agent Path Finding for Car-Like robots with kinematic and spatiotemporal constraints,” Robotics and Autonomous Systems, vol. 150, p. 103997, Apr. 2022.
[3]H.Huang, Y.Liu, J.Liu, Q.Yang, J.Wang, D.Abbink, and A.Zgonnikov, “General optimal trajectory planning: Enabling autonomous vehicles with the principle of least action,” Engineering, 2023.
[4]S.Tang and V.Kumar, “Safe and complete trajectory generation for robot teams with higher-order dynamics,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE, 2016, pp. 1894–1901.
[5]M.Čáp, J.Vokřínek, and A.Kleiner, “Complete decentralized method for on-line multi-robot trajectory planning in well-formed infrastructures,” in Proceedings of the international conference on automated planning and scheduling, vol.25, 2015, pp. 324–332.
[6]B.Şenbaşlar and G.S. Sukhatme, “Asynchronous real-time decentralized multi-robot trajectory planning,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE, 2022, pp. 9972–9979.
[7]J.Park, S.Karumanchi, and K.Iagnemma, “hom*otopy-based divide-and-conquer strategy for optimal trajectory planning via mixed-integer programming,” IEEE Transactions on Robotics, vol.31, no.5, pp. 1101–1115, 2015.
[8]T.Schouwenaars, B.DeMoor, E.Feron, and J.How, “Mixed integer programming for multi-vehicle path planning,” in 2001 European Control Conference (ECC), Sep. 2001, pp. 2603–2608.
[9]B.Li, H.Liu, D.Xiao, G.Yu, and Y.Zhang, “Centralized and optimal motion planning for large-scale AGV systems: A generic approach,” Advances in Engineering Software, vol. 106, pp. 33–46, Apr. 2017.
[10]C.Ma, Z.Han, T.Zhang, J.Wang, L.Xu, C.Li, C.Xu, and F.Gao, “Decentralized Planning for Car-Like Robotic Swarm in Cluttered Environments,” in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2023, pp. 9293–9300, iSSN: 2153-0866.
[11]C.E. Luis, M.Vukosavljev, and A.P. Schoellig, “Online Trajectory Generation With Distributed Model Predictive Control for Multi-Robot Motion Planning,” IEEE Robotics and Automation Letters, vol.5, no.2, pp. 604–611, Apr. 2020, conference Name: IEEE Robotics and Automation Letters.
[12]J.Alonso-Mora, P.Beardsley, and R.Siegwart, “Cooperative Collision Avoidance for Nonholonomic Robots,” IEEE Transactions on Robotics, vol.34, no.2, pp. 404–420, Apr. 2018, conference Name: IEEE Transactions on Robotics.
[13]Y.Ouyang, B.Li, Y.Zhang, T.Acarman, Y.Guo, and T.Zhang, “Fast and Optimal Trajectory Planning for Multiple Vehicles in a Nonconvex and Cluttered Environment: Benchmarks, Methodology, and Experiments,” in 2022 International Conference on Robotics and Automation (ICRA), May 2022, pp. 10 746–10 752.
[14]Y.Chen, M.Cutler, and J.P. How, “Decoupled multiagent path planning via incremental sequential convex programming,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), May 2015, pp. 5954–5961, iSSN: 1050-4729.
[15]W.Hönig, J.A. Preiss, T.K.S. Kumar, G.S. Sukhatme, and N.Ayanian, “Trajectory Planning for Quadrotor Swarms,” IEEE Transactions on Robotics, vol.34, no.4, pp. 856–869, Aug. 2018, conference Name: IEEE Transactions on Robotics.
[16]J.Park, J.Kim, I.Jang, and H.J. Kim, “Efficient Multi-Agent Trajectory Planning with Feasibility Guarantee using Relative Bernstein Polynomial,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), May 2020, pp. 434–440, iSSN: 2577-087X.
[17]G.Shi, W.Hönig, X.Shi, Y.Yue, and S.-J. Chung, “Neural-swarm2: Planning and control of heterogeneous multirotor swarms using learned interactions,” IEEE Transactions on Robotics, vol.38, no.2, pp. 1063–1079, 2021.
[18]J.Li, M.Ran, and L.Xie, “Efficient Trajectory Planning for Multiple Non-Holonomic Mobile Robots via Prioritized Trajectory Optimization,” IEEE Robotics and Automation Letters, vol.6, no.2, pp. 405–412, Apr. 2021, conference Name: IEEE Robotics and Automation Letters.
[19]H.Ma, D.Harabor, P.J. Stuckey, J.Li, and S.Koenig, “Searching with Consistent Prioritization for Multi-Agent Path Finding,” Proceedings of the AAAI Conference on Artificial Intelligence, vol.33, no.01, pp. 7643–7650, Jul. 2019, number: 01.
[20]K.Okumura, M.Machida, X.Défa*go, and Y.Tamura, “Priority inheritance with backtracking for iterative multi-agent path finding,” Artificial Intelligence, vol. 310, p. 103752, 2022.
[21]D.Dolgov, S.Thrun, M.Montemerlo, and J.Diebel, “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments,” The International Journal of Robotics Research, vol.29, no.5, pp. 485–501, Apr. 2010, publisher: SAGE Publications Ltd STM.
[22]J.Li, T.A. Hoang, E.Lin, H.L. Vu, and S.Koenig, “Intersection Coordination with Priority-Based Search for Autonomous Vehicles,” Proceedings of the AAAI Conference on Artificial Intelligence, vol.37, no.10, pp. 11 578–11 585, Jun. 2023, number: 10.
[23]B.Li, T.Acarman, Y.Zhang, Y.Ouyang, C.Yaman, Q.Kong, X.Zhong, and X.Peng, “Optimization-Based Trajectory Planning for Autonomous Parking With Irregularly Placed Obstacles: A Lightweight Iterative Framework,” IEEE Transactions on Intelligent Transportation Systems, vol.23, no.8, pp. 11 970–11 981, Aug. 2022, conference Name: IEEE Transactions on Intelligent Transportation Systems.