SPF-EMPC planner: A real-time motion planner for multi-robot in cluttered environments with dynamic obstacles and uncertainties

Abstract

Safe and efficient navigation for multi-robot systems in cluttered, dynamic environments remains challenging, primarily due to uncertainties from internal disturbances and dynamic external conditions. While existing methods enable real-time navigation in static or sparse settings, they often fail to respond effectively in environments dense with both static and moving obstacles. To address this limitation, this article proposes a novel safety probability field-based extended state model predictive control (SPF-EMPC) planner. The framework first introduces a safety probability field to model dynamic obstacles and integrates it with an unconstrained optimization approach for online generation of collision-free trajectories. Subsequently, an extended state model predictive controller ensures accurate trajectory tracking by explicitly accounting for robot model constraints and state perturbations, thereby guaranteeing practical feasibility. Both simulations and physical experiments demonstrate that the proposed method reliably prevents inter-robot and robot–obstacle collisions, even under significant motion and control uncertainties.

Introduction

The deployment of autonomous multi-robot systems in real-world applications is rapidly expanding, spanning areas such as post-disaster search and rescue, ¹ forest fire detection, ² logistics and warehousing, ³ and infrastructure damage inspection. ⁴ Such scenarios require systems that can not only navigate agilely through cluttered, unstructured environments but also maintain robustness against both internal disturbances and dynamic external conditions. However, achieving simultaneous resilience to these dual challenges remains an open problem in current research.

In cluttered and dynamic environments, mission failures in large-scale multi-robot deployments typically stem from two interrelated sources of uncertainty: environmental uncertainty and robot-induced uncertainty. Environmental uncertainty refers to limitations or inaccuracies in perception, which hinder robotic swarms from reliably detecting and fully avoiding dynamic obstacles. This perceptual gap creates significant collision risks, especially in densely populated and highly mobile scenarios. Velocity Obstacle (VO) and its variants, such as Reciprocal Velocity Obstacle and Hybrid Reciprocal Velocity Obstacle,^5–7 are widely used to address dynamic obstacles in such settings. However, these methods remain limited in handling prediction uncertainty: errors in position and velocity measurements directly influence the computation of the VO cone, potentially introducing collision risks. Meanwhile, ego-series algorithms,^8–10 which perform well in dense static environments, generally assume known or constant obstacle trajectories when dealing with dynamic obstacles—an assumption that often does not hold in practice, where trajectories are typically unknown and highly variable. Robot-induced uncertainty, on the other hand, arises from inaccuracies in the robots’ physical models (e.g. dynamics parameters) or their localization systems. These errors introduce internal perturbations that can degrade control performance, resulting in inter-robot collisions or significantly prolonged mission times. To explicitly address internal uncertainties, PUMA ¹¹ addresses internal uncertainty by integrating uncertainty propagation into the optimization constraints, dynamically adjusting collision avoidance bounds and velocity limits based on predicted state covariances. Similarly, SWIFT ¹² handles internal uncertainties (e.g. sensor noise and motion drift) through a confidence-guided cross-attention mechanism to selectively weight neighbor interactions. Despite these advances, simultaneously handling unpredictable dynamic obstacles and internal system uncertainties remains a daunting challenge.

To solve these problems, we propose a two-layer robust framework for multi-robot motion planning: the safety probability field-based extended state model predictive control (SPF-EMPC) planner, which combines a lightweight local trajectory planner with an iterative trajectory tracker. In trajectory planning, a safety probability field is constructed to model dynamic obstacles with motion perturbations. Requirements such as obstacle avoidance and smoothness are integrated into an efficient unconstrained optimized trajectory generation, thus reducing the computational load. In trajectory tracking, we explicitly account for state localization and model errors. Model predictive control using an extended state observer and a temporal self-sampling approach enables the robot to accurately track targets with time demands, ensuring the effectiveness of the planned trajectory and the robustness of the whole system. The entire framework successfully scales to multi-robot applications with significant advantages in terms of success rate and number of iterations, avoiding the over-conservatism of existing real-time methods while providing security at a low computational cost.

Further, we perform a distributed deployment in a real-world environment as shown in Figure 1. The environment is unknown in advance, and robots need to reach their destinations safely without formation in a cluttered environment, in which the irregular movement of dynamic obstacles and imprecise control of the robots pose potential collision risks. The main contributions are summarized below:

Figure 1.

Illustration of multi-robot navigation in a cluttered environment using the proposed safety probability field-based extended state model predictive control (SPF-EMPC) framework. (Top) The three-wheeled omnidirectional mobile robot platform used for real-world experiments. (Bottom) Physical navigation scenario involving static obstacles, a moving obstacle (highlighted with a green marker within a red circle), and multiple robots. Colored markers beneath obstacles represent perception data from different robots, demonstrating the distributed perception and real-time planning capabilities of the system.

Online trajectory generator for dynamic obstacle avoidance with uncertainties: We construct a safety probability field for obstacles that takes into account obstacle velocity and position perturbations, and apply an unconstrained optimization method to generate trajectories online for multiple robots in cluttered environments.

Robust trajectory tracker for precise robot following considering perturbations: We design extended state model predictive control to filter robot localization and model perturbations, and accurately track the generated trajectories using a temporal self-sampling method.

Simulation and physical validation: We conducted multi-robot traversal experiments in both simulation and physical environments to validate real-time performance and effectiveness of the proposed method.

Related work

In recent years, significant progress has been made in trajectory planning for obstacle avoidance using voxel maps^13,14 and optimization methods.^15,16 The space occupied by the obstacle is usually represented as a set of constraints on the free space, and the selection of constraint type determines the performance of the optimization problem. In deterministic environments, some work, such as ego-serials,^8–10 achieve real-time navigation in complex obstacle environments, but treat dynamic obstacles as instantaneous static obstacles or cooperative robots, and the voxel maps they rely on do not adequately characterize dynamic obstacles. For obstacles with motion perturbations, there are two main strategies in the literature to characterize them: using differentiable surfaces such as ellipsoids, ¹⁷ or using convex polyhedra such as rectangles.^18,19

The obstacle can be described by a differentiable surface as a nonlinear constraint on the optimization problem, which can be efficiently solved by a gradient-based solver. ¹⁵ To facilitate real-time processing, conservative estimates of obstacles with motion uncertainty, such as sigma hulls based on confidence levels. ²⁰ Although the method does not guarantee global optimality, its reduced computational cost makes it widely adopted for most time-critical motion planning tasks. ²¹ The overly conservative obstacle models guarantee collision-free behavior, they greatly consume the remaining free space, thus further diminishing the feasibility of time-critical navigation tasks in cluttered environments containing multiple robots and obstacles. ¹⁷ In addition, existing methods only make conservative estimates on the obstacle volume and do not fully utilize other information of dynamic obstacles.

Alternatively, obstacles can be characterized by convex polyhedra occupied in the space, using the occupied space as a hard constraint to be avoided in the optimization problem. Xu et al. ¹⁹ use the convex polyhedral space of obstacles to construct safe flight corridors to ensure collision-free trajectories for static obstacles. In recent years, model predictive control (MPC) methods^22–24 have emerged actively as a dynamic obstacle avoidance method. MPC can be combined with dynamic obstacle avoidance to generate and track trajectories that satisfy the robot’s motion and actuation constraints. In the work, ²⁵ moving obstacles are packed into ellipses, and spatial constraints are imposed as optimization metrics. To further consider obstacles perturbations, Jian et al. ²⁶ integrate a control barrier function within the MPC framework to avoid pedestrians. There are also several work^27,28 that utilize geometric representations and MPC to probabilistically avoid dynamic obstacles. Recent efforts have made some progress in computational efficiency,^17,27 but their computational cost grows exponentially with the number of obstacles and robots, and they are unable to meet real-time requirements.

In this article, we combine the advantages of efficient computation of the gradient-based planner and robust control of the MPC to construct a two-layer framework that is easy to extend. In order to prevent cooperative robots from becoming dynamic obstacles due to control errors from localization and model perturbations, this article integrates the interference between obstacle motion and robot control in cluttered environments, which is different from previous work shown in Table 1. The designed safety probability field can fully utilize the size and velocity information of dynamic obstacles, and the extended state observer can utilize the historical information of robot states. The proposed method can effectively cope with perturbations, and generate and track safe trajectories for multi-robot in cluttered environments online.

Table 1.

Comparison of recent motion planning methods for dynamic obstacle avoidance based on key attributes in the table.

Method	Group scale	Static obstacles	Perturbation consideration	Verification
EGO-Swarm 9	Multiple	Yes	Not considered	Hardware
EGO-Swarm2 10	Multiple	Yes	Not considered	Hardware
MADER 29	Multiple	Yes	Not considered	Simulation
RMADER 30	Multiple	Yes	Not considered	Hardware
DPMPC 25	Single	Yes	obstacles	Simulation
VIGO 16	Single	Yes	Not considered	Hardware
CCNMPC 17	Multiple	No	Obstacles	Hardware
SPF-EMPC (Ours)	Multiple	Yes	Obstacles and robots	Hardware

EGO: ESDF-free Gradient-based lOcal; MADER: Multiagent Trajectory Planner Robust to Communication Delay in Dynamic Environments; RMADER: Robust Multiagent Trajectory Planner Robust to Communication Delay in Dynamic Environments; DPMPC: Dynamic Polynomial-based Model Predictive Control; SPF-EMPC: safety probability field-based extended state model predictive control.

Methodology

System framework overview

This work introduces a multi-robot distributed trajectory planning framework encompassing three principal components: the sensing and position module, the local trajectory planning module, and a trajectory tracking module, as shown in Figure 2.

Figure 2.

Schematic overview of the proposed SPF-EMPC framework. The system operates in a closed-loop manner: upon local sensing of a dynamic obstacle, a real-time SPF is generated. This SPF, along with the local static map, informs an unconstrained local trajectory planner to iteratively compute a collision-free path. The resulting time-parameterized trajectory is then accurately tracked by an EMPC, which compensates for robot state and model perturbations. Throughout this process, perception and localization modules provide continuous state feedback and collision checking. SPF-EMPC: safety probability field-based extended state model predictive control; SPF: safety probability field; EMPC: extended state model predictive controller.

Each robot has a distinct sensing range within this distributed system for its planning and control processes. And trajectories are communicated through a prioritization mechanism (This article sets a fixed priority according to the robot number), robots with a lower priority need to consider obstacle avoidance with other robots with a higher priority. The whole system faces disruptions from moving obstacles, robot’s localization and model.

Safety probability field

In contrast to the fixed nature of static obstacles, the future state of dynamic obstacles similar to pedestrians cannot be accurately predicted, making it difficult to effectively generate safety trajectories. Therefore, this section constructs a safety probability field to describe dynamic obstacles with motion perturbations.

This work enhances the real-time performance and effectiveness of obstacle avoidance by referring to studies on pedestrian prediction^10,31 and introducing an elliptic dynamic probability field with a binary Gaussian distribution, as shown in Figure 3. This elliptical safety probability field with motion bias describes the moving intention of the obstacle and degenerates into a general circular field when the velocity is zero (static obstacle), providing accurate guidance for the robot to avoid obstacle. By avoiding regions with high collision probability, safe trajectories can be generated that consider finite future states and avoid being overly conservative.

Figure 3.

The adaptive safety probability field (SPF) around a dynamic obstacle. The elliptical field, shaped by the obstacle’s current velocity and size, visualizes collision risk (color gradient). The dashed circle indicates the obstacle’s predicted position at the next time step.

The probability field is designed concerning the Gaussian distribution as follows:

$$U\left({\mathbf{p}}_i^o,{\mathbf{v}}_i^o,\mathbf{p}\right)=\frac{\exp (-\frac{1}{2}{(\mathbf{p}-{\mathbf{p}}_{c_i}^o)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{p}-{\mathbf{p}}_{c_i}^o))}{2\pi |\mathbf{\Sigma }{|}^{1/2}}$$

(1)

$${\mathbf{p}}_{c_i}^o={\mathbf{p}}_i^o+(1-\lambda ){\mathbf{v}}_i^{o}dt,\lambda \in [0,1]$$

(2)

where ${\mathbf{p}}_i^o$ and ${\mathbf{v}}_i^o$ denote the current position and velocity of the $i\mathrm{th}$ obstacle; $\mathbf{p}$ is the query point; ${\mathbf{p}}_{c_i}^o$ is the center of the probability field; $dt$ is the sampling interval; $\lambda \in [0,1]$ weights the focus on the current position; and $\mathbf{\Sigma }$ is the covariance matrix. The function $U({\mathbf{p}}_i^o,{\mathbf{v}}_i^o,\mathbf{p})$ outputs the likelihood of the dynamic obstacle being located at $\mathbf{p}$ , as shown in Figure 4. The figure illustrates the spatially varying collision probability, which decays radially outward from each ellipse’s center—higher values correspond to greater risk. A circular field indicates a stationary obstacle (modeled as i.i.d.), while an elliptical one reflects velocity-driven uncertainty. By circumventing high-risk zones, our approach generates safe trajectories that incorporate the obstacle’s near-future motion, thereby reducing computational cost and avoiding excessive conservatism in planning.

Figure 4.

Safety probability field illustrations for varying obstacle velocities. The green circle indicates the robot’s current position.

For the covariance matrix $\mathbf{\Sigma }$ , we set ${\theta }_i=\arctan ({\mathbf{V}}_{iy}^o/{\mathbf{V}}_{ix}^o)$ so that the rotation matrix $\mathbf{R}$ and deflation matrix $\mathbf{\Lambda }$ are related to the obstacle’s size and velocity:

$$\begin{gathered} \begin{array}{c}\mathrm{\Sigma }=\mathbf{R}\mathrm{\Lambda }{\mathbf{R}}^{\mathrm{T}}\end{array} \\ =\left[\begin{array}{cc}\cos {\theta }_i& -\sin {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\end{array}\right]\left[\begin{array}{cc}{a_i}^2& 0\\ 0& {b_i}^2\end{array}\right]{\left[\begin{array}{cc}\cos {\theta }_i& -\sin {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\end{array}\right]}^{\mathrm{T}} \end{gathered}$$

(3)

Assuming that the dynamic obstacle can be wrapped by a minimum outer circle with radius $r_i^o$ , the parameters $a_i=r_i^o+v_i^{o}t/2$ and $b_i=r_i^o$ are defined for the inner elliptical circle in Figure 3. According to the above formulas, the probability field for the moving obstacle is established, and its partial derivative concerning the position can be calculated as follows:

$$\frac{\mathrm{\partial }U\left({\mathbf{p}}_i^o,{\mathbf{v}}_i^o,\mathbf{p}\right)}{\mathrm{\partial }\mathbf{p}}=-U{\mathbf{\Sigma }}^{-1}(\mathbf{p}-{\mathbf{p}}_{c_i}^o)$$

(4)

With the real-time motion of dynamic obstacles, the safety probability field is dynamically updated according to their latest position, velocity, and dimensional information. This real-time adaptive mechanism enables the robot to consistently assess the threat intensity imposed by nearby moving obstacles

Multi-robot trajectory generation

In cluttered environments with multiple static and moving obstacles, the rapid generation of safe trajectories is essential for robots to respond to environmental changes on time and navigate safely. In this section, an unconstrained optimization method based on safety probability field is used to study the multi-robot trajectory generation problem.

Trajectory representation

For the following system:

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$$

(5)

where $f$ is the state transition function, $\mathbf{x}$ is the state variable, and $\mathbf{u}$ is the input. The system is said to be differentially flat ³² if $\mathbf{x}$ and $\mathbf{u}$ can be represented by an analytic expression with a flat output $\mathbf{z}$ and its finite derivatives. In this case, only the higher-order differentiable trajectory of the center of mass and its yaw angle need to be planned for the trajectory planning of the system.

According to the differential flatness property, the MINCO (Minimum Control) trajectory parameterization technology ³³ can simplify high-dimensional trajectories into a series of waypoints $\{{\mathbf{p}}_{11}^r,\dots ,{\mathbf{p}}_{1k}^r,\dots ,{\mathbf{p}}_{ij}^r,\dots ,{\mathbf{p}}_{Nk}^r,{\mathbf{p}}_e^r\}$ and the corresponding time $t_{ij}$ to flexibly respond to complex and changing environments, where ${\mathbf{p}}_{11}^r$ and ${\mathbf{p}}_e^r$ represent the starting position and the ending position, respectively. The notation ${\mathbf{p}}_{ij}^r$ denotes the $j$ th waypoint of the $i$ th segment within the trajectory. The variable $N$ is the total number of segments, while $k$ indicates the number of waypoints per segment. For notational convenience, this article adopts the shorthand ${\mathbf{p}}_{ij}^r\equiv {\mathbf{p}}_{(i-1)\times k+j}^r$ . Consequently, the set of waypoints can be represented as follows:

$$S=\left\{{\mathbf{p}}_1^r,\dots ,{\mathbf{p}}_i^r,\dots ,{\mathbf{p}}_{N\times k}^r,{\mathbf{p}}_{N\times k+1}^r\right\},{\mathbf{p}}_i^r\in {\mathbb{R}}^n$$

(6)

where $n$ is the dimension of the motion space. For the cost function $G(\mathbf{P},\mathbf{T})=H(\mathbf{C}(\mathbf{P},\mathbf{T}),\mathbf{T})$ , $G$ and $H$ are the same costs in two different forms. The partial derivatives of the cost concerning the waypoints ${\mathbf{p}}_i$ and the unconstrained time $T_i$ are derived from the subsequent equations:

$$\begin{array}{rl}\frac{\mathrm{\partial }G}{\mathrm{\partial }{\mathbf{p}}_i}& =\mathrm{tr}\left\{{\left(\frac{\mathrm{\partial }\mathbf{C}}{\mathrm{\partial }{\mathbf{p}}_i}\right)}^{\mathrm{T}}\frac{\mathrm{\partial }H}{\mathrm{\partial }\mathbf{C}}\right\}\\ & =\mathrm{tr}\left\{{\left({\mathbf{L}}^{-1}\frac{\mathrm{\partial }\left(\mathbf{L}\mathbf{C}\right)}{\mathrm{\partial }{\mathbf{p}}_i}\right)}^{\mathrm{T}}\frac{\mathrm{\partial }H}{\mathrm{\partial }\mathbf{C}}\right\}\end{array}$$

(7)

$$\begin{array}{rl}\frac{\mathrm{\partial }G}{\mathrm{\partial }{T}_i}& =\frac{\mathrm{\partial }H}{\mathrm{\partial }{T}_i}+\mathrm{tr}\left\{{\left(\frac{\mathrm{\partial }\mathbf{C}}{\mathrm{\partial }{T}_i}\right)}^{\mathrm{T}}\frac{\mathrm{\partial }H}{\mathrm{\partial }\mathbf{C}}\right\}\\ & =\frac{\mathrm{\partial }H}{\mathrm{\partial }{T}_i}-\mathrm{tr}\left\{{\left(\frac{\mathrm{\partial }\mathbf{L}}{\mathrm{\partial }{T}_i}\mathbf{C}\right)}^{\mathrm{T}}{\mathbf{L}}^{-\mathrm{T}}\frac{\mathrm{\partial }H}{\mathrm{\partial }\mathbf{C}}\right\}\end{array}$$

(8)

where $\mathbf{C}$ is the coefficient matrix, $\mathbf{L}$ is the condition matrix linking the trajectory segments, and $\mathrm{tr}\{\cdot \}$ denotes the operation of obtaining the trace of the matrix. The specific settings can be related to the work. ³³

Optimization problem construction

According to equations (14) and (15), the omnidirectional mobile robot studied in this paper has a differential flatness property similar to the quadrotor when the flat output $\mathbf{z}=[x{}_{\mathrm{w}},y{}_{\mathrm{w}},\theta {}_{\mathrm{bw}}]$ is set. Combining this differential flatness with the MINCO trajectory parameterization method transforms the trajectory optimization problem into an unconstrained problem to speed up the optimization process. The overall cost function is defined as follows:

$$C_{\mathrm{tot}}=C_{\mathrm{ego}}+{\omega }_{\mathrm{obs}}\cdot C_{\mathrm{obs}}$$

(9)

where $C_{\mathrm{ego}}$ is the weighted cost combination of smoothness, dynamic feasibility, time and mutual avoidance. The $C_{\mathrm{obs}}$ and ${\omega }_{\mathrm{obs}}$ are the cost and weight of static and dynamic obstacle avoidance, respectively. In this article, we specifically discuss the obstacle avoidance part and refer to previous work ¹⁰ to set $C_{\mathrm{ego}}$ .

The safety probability field is employed to find guidance points for estimating the collision cost and gradient required by the gradient-based optimizer. The process of setting specific guidance points is illustrated in Figure 5. SPF can calculate the probability that a collision occurs at a given point, and a collision probability exceeding the prescribed threshold indicates a potential collision. When a collision risk is detected for the given waypoints within the current trajectory, the guide points will be sought according to the safe gradients of the waypoints to generate a collision-free trajectory. To ensure completeness of the trajectory solution, an alternative trajectory is obtained on the other side according to the symmetry line. Ultimately, the optimal trajectory is selected based on a comparison of the costs of the trajectories. It should be noted that the optimization-based approach itself is susceptible to oscillations due to the dynamic nature of the environment. However, engineering settings can improve this limitation, for example, by automatically adjusting the threshold based on the number of oscillations.

Figure 5.

Trajectory optimization using the safety probability field (SPF). The initial trajectory (green curve) enters the SPF of a dynamic obstacle. A risky waypoint (light green dot) is identified and projected along the collision probability gradient (black arrow) to a safe guidance point (blue dot). An alternative trajectory (blue dashed curve) is then generated on the obstacle’s opposite side, guided by a symmetry line (purple dashed) through the risk zone.

According to the SPF, each dangerous waypoint ${\mathbf{p}}_i^r$ for dynamic obstacles ${\mathbf{p}}_j^o$ is paired with a corresponding guidance point ${\mathbf{p}}_{ij}^g$ . Subsequently, using the predefined safety threshold $d_{\mathrm{saf}}$ , the collision cost and gradient at the collision control point can be calculated as follows:

$$C_{\mathrm{dyn}}=\sum _i\sum _j{\lambda }_i{\left(d_{\mathrm{saf}}-\left({\mathbf{p}}_i^r-{\mathbf{p}}_{ij}^g\right)\cdot \frac{\mathrm{\nabla }{\mathbf{g}}_{ij}}{\Vert \mathrm{\nabla }{\mathbf{g}}_{ij}\Vert }\right)}^3$$

(10)

$$\mathrm{\nabla }{\mathbf{g}}_{ij}=U\left({\mathbf{p}}_j^o,{\mathbf{v}}_j,{\mathbf{p}}_i^r\right){\mathbf{\Sigma }}^{-1}({\mathbf{p}}_i^r-{\mathbf{p}}_{c_j}^o)$$

(11)

where $i$ and $j$ denote the path point and obstacle serial numbers, respectively, the weight assigned to the path point is ${\lambda }_i$ . The direction of the negative gradient of the SPF, denoted by $\mathrm{\nabla }{\mathbf{g}}_{ij}$ , can influence the trajectory by guiding waypoints towards a safe region.

The gradient of the collision cost concerning the trajectory polynomial coefficients $\mathbf{C}$ and time $\mathbf{T}$ are calculated using the chain rule as follows:

$$\begin{array}{rl}\frac{\mathrm{\partial }{C}_{\mathrm{dyn}}}{\mathrm{\partial }\mathbf{C}}=& -3\sum _i\sum _j{\lambda }_i\beta \left(t_i\right){\left(\frac{\mathrm{\nabla }{\mathbf{g}}_{ij}}{\Vert \mathrm{\nabla }{\mathbf{g}}_{ij}\Vert }\right)}^{\mathrm{T}}\\ & \cdot {\left(d_{\mathrm{saf}}-\left({\mathbf{p}}_i^r-{\mathbf{p}}_{ij}^g\right)\frac{\mathrm{\nabla }{\mathbf{g}}_{ij}}{\Vert \mathrm{\nabla }{\mathbf{g}}_{ij}\Vert }\right)}^2\end{array}$$

(12)

$$\begin{array}{rl}\frac{\mathrm{\partial }{C}_{\mathrm{dyn}}}{\mathrm{\partial }\mathbf{T}}=& -3\sum _i\sum _j{\lambda }_i{\mathbf{v}}_i{\alpha }_i{\left(\frac{\mathrm{\nabla }{\mathbf{g}}_{ij}}{\Vert \mathrm{\nabla }{\mathbf{g}}_{ij}\Vert }\right)}^{\mathrm{T}}\\ & \cdot {\left(d_{\mathrm{saf}}-\left({\mathbf{p}}_i^r-{\mathbf{p}}_{ij}^g\right)\frac{\mathrm{\nabla }{\mathbf{g}}_{ij}}{\Vert \mathrm{\nabla }{\mathbf{g}}_{ij}\Vert }\right)}^2\end{array}$$

(13)

where $\beta (t_i)=[1,t_i,{t_i}^2,{t_i}^3,{t_i}^4,{t_i}^5]$ is the basis function of the trajectory segment, ${\alpha }_i=i/k$ is the scaling factor of the trajectory segment, and ${\mathbf{v}}_i$ is the required speed corresponding to the waypoint. According to equations (7) and (8), the derivatives of the dynamic collision cost concerning the waypoints and the unconstrained time can be calculated. After that, unconstrained optimization will accelerate the trajectory generation process.

Trajectory tracking control

In practice, robots are subject to both internal and external disturbances. External disturbances introduce localization errors, which compromise the accuracy of control force computation, while internal disturbances cause model mismatches that degrade the application of control forces. Together, these perturbations induce control uncertainties, resulting in trajectory tracking errors and reduced navigation efficiency. Consequently, robust controllers capable of explicitly accounting for such disturbances are essential for safe and reliable operation. Since both localization inaccuracies and model deviations manifest as state uncertainties, this section presents an extended state observer designed to estimate these compounded perturbations. Combined with a self-sampling model predictive control scheme, the proposed approach enables precise and robust tracking of iteratively planned trajectories.

Robot model

The three-wheeled omnidirectional mobile robot employed in this article is characterized by its flexible motion capabilities, as depicted in Figure 6.

Figure 6.

Schematic diagram of an omnidirectional mobile robot model.

The positive direction of the robot body coordinate system’s x-axis corresponds to the wheel axis direction, and the wheels are evenly distributed at intervals of ${120}^{\circ }$ . The kinematic and kinetic equations of the robot are as follows:

$$\dot{\mathbf{z}}=\left[\begin{array}{c}{v}_{wx}\\ v_{wy}\\ \omega \end{array}\right]=\mathbf{A}({\left[\begin{array}{ccc}0& -1& -\mathrm{L}\\ \cos 60\circ & \sin 60\circ & -\mathrm{L}\\ -\cos 60\circ & \sin 60\circ & -\mathrm{L}\end{array}\right]}^{-1}+{\mathbf{\Delta }}_1)\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right]+{\mathbf{w}}_v$$

(14)

$$\ddot{\mathbf{z}}=\left[\begin{array}{c}{\dot{v}}_{wx}\\ {\dot{v}}_{wy}\\ \dot{\omega }\end{array}\right]=\mathbf{A}(\left[\begin{array}{ccc}-\frac{\sqrt{3}}{2\mathrm{MR}}& -\frac{\sqrt{3}}{2\mathrm{MR}}& 0\\ -\frac{1}{2\mathrm{MR}}& -\frac{1}{2\mathrm{MR}}& \frac{1}{\mathrm{MR}}\\ -\frac{\mathrm{L}}{\mathrm{IR}}& -\frac{\mathrm{L}}{\mathrm{IR}}& -\frac{\mathrm{L}}{\mathrm{IR}}\end{array}\right]+{\mathbf{\Delta }}_2)\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]+{\mathbf{w}}_a$$

(15)

$$\mathbf{A}\mathbf{=}\left[\begin{array}{ccc}\cos {\theta }_{\mathrm{bw}}& -\sin {\theta }_{\mathrm{bw}}& 0\\ \sin {\theta }_{\mathrm{bw}}& \cos {\theta }_{\mathrm{bw}}& 0\\ 0& 0& 1\end{array}\right]$$

(16)

where $\mathbf{A}$ is the coordinate transformation matrix; $v_i$ and ${\tau }_i$ denote the wheels’ linear velocities and control torques, respectively; $\omega$ is the robot’s angular velocity; ${\theta }_{\mathrm{bw}}$ defines the body-to-world orientation; $v_{bx}$ and $v_{wx}$ are the robot’s velocities expressed in the body and world frames. Physical parameters include the platform radius $\mathrm{L}=0.04\mathrm{m}$ , mass $\mathrm{M}$ , wheel radius $\mathrm{R}$ , and moment of inertia $\mathrm{I}$ . The terms ${\mathbf{w}}_v$ and ${\mathbf{w}}_a$ represent measurement noise in the kinematic and kinetic states, while ${\mathbf{\Delta }}_1$ and ${\mathbf{\Delta }}_2$ correspond to model errors.

Extended state MPC

The effects caused by model error and localization noise can be jointly attributed to the uncertainties of the robot states. According to equations (14) to (16), the discrete state transfer equation can be expressed as follows:

$${\hat{\mathbf{z}}}_k=f\left({\hat{\mathbf{z}}}_{k-1},{\mathbf{u}}_{k-1}\right)+{\mathbf{w}}_k$$

(17)

where ${\hat{\mathbf{z}}}_k$ is the current measured state, ${\mathbf{w}}_k$ is the total perturbation, and $f$ is the state transfer function. For each state variable, this paper employs a unified extended state observer (ESO) ³⁴ to monitor the extended perturbed state and provide compensation for the actual measured state as follows:

$$\left[\begin{array}{c}{\dot{\overline{z}}}_k\\ {\dot{d}}_k\end{array}\right]=\left[\begin{array}{cc}-l_1& 1\\ -l_2& 0\end{array}\right]\left[\begin{array}{c}{\overline{z}}_k\\ d_k\end{array}\right]+\left[\begin{array}{cc}b& l_1\\ 0& l_2\end{array}\right]\left[\begin{array}{c}{u}_k\\ {\hat{z}}_k\end{array}\right]$$

(18)

where ${\overline{z}}_k$ is the state estimation and $d_k$ is the perturbation estimate. According to the kinematic model, the control gain is set to $b=1$ , and the ESO gains are $l_1=2w$ and $l_2=w^2$ , with the poles of the system uniformly configured at $-w$ .

After obtaining the estimated state, this work fits a polynomial trajectory using the positions, velocities, and timestamps of adjacent waypoints from the reference trajectory ${\mathbf{t}\mathbf{r}\mathbf{a}\mathbf{j}}_{\mathrm{ref},k}$ . This efficiently transforms the future waypoint data into trajectory parameters, enabling accurate tracking of the continuously iterated and optimized local trajectories from the planner.

$${\mathbf{t}\mathbf{r}\mathbf{a}\mathbf{j}}_{\mathrm{ref},k}=\mathbf{t}\mathbf{r}\mathbf{a}\mathbf{j}({\overline{\mathbf{z}}}_k,t_0+(k-1)dt),\mathrm{\forall }k=1,2,\dots ,N$$

(19)

where ${\overline{\mathbf{z}}}_k$ is the current estimated state after ESO processing. Adaptive sampling is performed based on the current time $t_0$ , the time horizon $H$ , the sampling interval $dt$ , and the information about the trajectory parameters. Finally, the optimal control problem is formulated as follows:

$$\underset{{\mathbf{z}}_k,{\mathbf{u}}_k}{min}\sum _{k=0}^{H-1}\left({\lambda }_p{\Vert {\mathbf{p}}_k-{\mathbf{t}\mathbf{r}\mathbf{a}\mathbf{j}}_{\mathrm{ref},k}\Vert }^2+{\lambda }_u{\Vert {\mathbf{u}}_k\Vert }^2\right)$$

(20\rm a)

$$\mathrm{s}.\mathrm{t}.{\overline{\mathbf{z}}}_k=\mathrm{eso}\left({\hat{\mathbf{z}}}_k,{\mathbf{u}}_k\right)$$

(20\rm b)

$${\hat{\mathbf{z}}}_{k+1}=f\left({\overline{\mathbf{z}}}_k,{\mathbf{u}}_k\right)+{\mathbf{w}}_{k+1}$$

(20\rm c)

$${\mathbf{z}}_0={\mathbf{z}}_{\mathrm{init}}$$

(20\rm d)

$${\mathbf{z}}_{\mathrm{lb}}\le {\mathbf{z}}_k\le {\mathbf{z}}_{\mathrm{ub}}$$

(20\rm e)

$${\mathbf{u}}_{\mathrm{lb}}\le {\mathbf{u}}_k\le {\mathbf{u}}_{\mathrm{ub}}$$

(20\rm f)

where the state variables are represented by the vector $\mathbf{z}=[x{}_{\mathrm{w}},y{}_{\mathrm{w}},\theta {}_{\mathrm{bw}}]$ , and the control inputs are represented by the vector $\mathbf{u}=[v_1,v_2,v_3]$ . The ${\mathbf{w}}_{k+1}$ denotes the total noise. The non-zero track error factor ${\lambda }_p$ and the control input factor ${\lambda }_u$ are designed to limit the solution space to balance tracking accuracy with the smoothness of the control input.

Results and discussion

To further assess the efficacy of the proposed method, we perform a series of experiments in both simulation and real-world scenarios. The L-BFGS algorithm ³⁵ and the CasADi toolkit ³⁶ are utilized to address unconstrained trajectory optimization and optimal control problems, respectively. These algorithms are integrated into the ROS environment through C++ code. Separate ROS nodes have been engineered to allow a transition between simulation and physical application without altering the codebase, operating in a distributed manner on an Intel i9-13900K. The full experimental video is available at https://www.bilibili.com/video/BV1xpmNYJE5F/.

Figure 7 shows the execution time of the trajectory generator and the tracking controller, demonstrating the superior real-time performance of combining the two parts. The trajectory generator is capable of avoiding obstacles in cluttered environments in real time. Although it requires additional time to construct the SPF when encountering dynamic obstacles with motion perturbations, the average optimization time is still within 2 ms. The tracking controller focuses on the robot’s control problem and can compute control quantities that meet the robot’s constraints within 8 ms, taking into account state perturbations induced by localization and model. The total duration of the overall framework is <10 ms, allowing for a timely response to dynamic changes in the environment.

Figure 7.

Algorithm running time, where $\mu$ denotes the mean execution time. Left: The execution time of trajectory generation, with average times of 0.63, 0.67, and 1.34 ms in obstacle-free, static obstacle, and dynamic obstacle scenarios, respectively. Right: The execution time of extended state model predictive controller (EMPC) for trajectory tracking, with an average of 8 ms. The total algorithm runtime remains within 10 ms, enabling real-time operation.

Experimental setup

For trajectory planning, we set the attention in equation (2) $\lambda =0.5$ in the safety probability field and select a fifth-order polynomial for trajectory parameterization in unconstrained optimization, achieving a balance between the algorithm’s real-time performance and trajectory smoothness. In trajectory tracking, the ESO gains in equation (18) are set to $l_1=1$ and $l_2=0.25$ . The MPC domain in equation (19) is configured with $H=5$ , $N=15$ , using a time step of $dt=0.1s$ .

To describe the perturbations in the environment, we assume that the state of the $i$ th obstacle at the next time step conforms to ${\mathbf{p}}_{i,t_{k+1}}^o={\mathbf{p}}_{i,t_k}^o+{\mathbf{v}}_{i,t_k}^{o^{\prime }}(t_{k+1}-t_k)+{\mathbf{p}}_{ie}^o$ and ${\mathbf{v}}_{i,t_k}^{o^{\prime }}={\mathbf{v}}_{i,t_k}^o+{\mathbf{v}}_{ie}^o$ . Both the position deviation ${\mathbf{p}}_{ie}^o$ and the velocity deviation ${\mathbf{v}}_{ie}^o$ of the obstacles adhere to a Gaussian distribution. Furthermore, the state deviations (1 m deviation between the x and y states, $0.1\circ$ deviation in $\theta$ ) are introduced for the robot in the simulation, thereby simulating the measured state.

Simulation experiment

A multi-robot simulation is conducted in a complex dynamic environment, as depicted in Figure 8. Each robot is capable of independently avoiding obstacles and safely reaching its destination. Their target positions are set in opposing configurations to ensure a complete interaction among the robots.

Figure 8.

Multi-robot navigation in cluttered dynamic environments using SPF-EMPC. Upside: Twelve robots navigate among static (squares) and dynamic (circled) obstacles with distributed perception (colored markers). Downside: An individual robot tracks iterative trajectories via extended state MPC while accounting for estimated state uncertainty (green region) and a dynamic obstacle (purple circle), demonstrating simultaneous handling of internal and external uncertainties. SPF-EMPC: safety probability field-based extended state model predictive control; MPC: model predictive control.

Due to the lack of source code for related work and differences in scenario configurations, this paper compares the trajectory generation aspects with the current state-of-the-art method and performs an ablation study of the components of the overall framework. These evaluations are intended to demonstrate the necessity and superiority of the proposed framework.

Comparison of trajectory generation

The local planner EGO-Swarm2 ³⁷ and its variant with a accurate prediction of dynamic obstacles serve as benchmarks. The accurate prediction variant treats obstacles as collaborative robots that share their future accurate trajectories at all times, which is too ideal to be realized in reality, but is used here as a comparison to validate the performance of the proposed method. The comparative data at varying levels of perturbation are presented in Table 2. Among the performance metrics, the overtime rate indicates the likelihood that the robot is trapped in the environment, failing to reach its target. The number of iterations refers to the mean number of optimizations required for a single robot within a single experimental trial.

Table 2.

Quantitative comparison under motion uncertainty.

Perturbation level	Methods	SR (%)	OR (%)	Dynamic distance (m)	Path length (m)	Travel time (seconds)	Iterations
0.25  $\mathrm{\Sigma }$	ES2	14	0	2.30 $|$ 0.67 $|$ 1.70	25.43 $|$ 25.13 $|$ 0.44	34.22 $|$ 32.00 $|$ 4.15	253.74
	ES2*	76	10	1.76 $|$ 0.97 $|$ 1.11	26.41 $|$ 25.15 $|$ 1.29	35.30 $|$ 31.01 $|$ 2.71	69.92
	Ours	72	2	1.52 $|$ 0.62 $|$ 0.85	27.17 $|$ 25.14 $|$ 2.42	38.83 $|$ 32.00 $|$ 7.02	58.4
1  $\mathrm{\Sigma }$	ES2	10	0	1.50 $|$ 0.73 $|$ 0.79	25.62 $|$ 25.15 $|$ 0.44	31.80 $|$ 31.00 $|$ 0.57	270.16
	ES2*	62	4	1.41 $|$ 0.85 $|$ 3.41	26.53 $|$ 25.28 $|$ 1.33	36.08 $|$ 31.50 $|$ 3.59	92.52
	Ours	56	2	1.38 $|$ 0.69 $|$ 0.62	29.64 $|$ 25.18 $|$ 4.44	39.34 $|$ 33.50 $|$ 5.10	55.78
2  $\mathrm{\Sigma }$	ES2	12	2	1.39 $|$ 0.57 $|$ 0.68	26.21 $|$ 25.20 $|$ 1.28	33.33 $|$ 31.50 $|$ 1.57	300.96
	ES2*	52	4	1.39 $|$ 0.57 $|$ 0.68	27.37 $|$ 25.46 $|$ 1.88	38.33 $|$ 33.00 $|$ 5.37	101.74
	Ours	40	2	1.47 $|$ 0.53 $|$ 1.01	29.35 $|$ 25.17 $|$ 3.71	39.60 $|$ 31.50 $|$ 5.35	60.36

SPF-EMPC: safety probability field-based extended state model predictive control; SR: success rate; OR: overtime rate.

Performance of the proposed SPF-EMPC is benchmarked against EGO-Swarm2 (ES2) and its ideal variant with perfect prediction (ES2*). Results are averaged over 50 Monte Carlo trials conducted in a cluttered randomized environment (five robots, four dynamic, and six static obstacles) under three perturbation levels (0.25  $\mathrm{\Sigma }$ , 1  $\mathrm{\Sigma }$ , 2  $\mathrm{\Sigma }$ ; where (1  $\mathrm{\Sigma }:{{\sigma }_p}^o=1\times {10}^{-4},{{\sigma }_v}^o=1\times {10}^{-2}$ )). Reported metrics include SR, OR, dynamic distance, path length, travel time, and optimization iterations. For each metric, values are shown as mean $|$ min $|$ std, with the best performer per row highlighted in bold.

We conduct the simulations in a spatially limited environment containing multiple irregular moving obstacles with speeds close to the robot’s, making it difficult to achieve full obstacle avoidance. As illustrated in Table 2, the success rate of the three methods declines gradually with increasing perturbation level. Notably, ES2 exhibits the lowest overtime rate and requires the shortest distance and time to navigate. However, ES2 perceives dynamic obstacles as static in real-time and does not account for motion perturbations, resulting in the lowest success rate and most iterations. The variant ES2*, which assumes that obstacles follow perfectly predictable trajectories, markedly enhances the success rate and iteration count compared to ES2. Nevertheless, it tends to induce oscillations when confronted with uncertain obstacles, leading to a higher overtime rate. Moreover, its ideal prediction model is challenging to implement in practical scenarios. In contrast, the proposed method achieves a considerably higher success rate than ES2 and nearly matches the ideal ES2* performance, with the fewest iterations and dynamic distance. The result indicates that the SPF-based unconstrained planning approach converges rapidly and avoids excessive conservatism in generating safe trajectories.

Ablation comparison

The superiority of the proposed two-layer framework in real-time is shown in Figure 7. To confirm the efficacy of each component of our proposed approach in terms of obstacle avoidance success rate under consideration of perturbations, we performed an ablation comparison, as shown in Table 3. Specifically, in the variant lacking self-sampling (SS), the trajectory referenced in equation (19) is substituted with a generic MPC based on the initial discrete trajectory. For the version without SPF, the speed of the safety probability field is set to zero, degenerating into a circular field with the minimum safe distance.

Table 3.

Performance comparison of the proposed SPF-EMPC with other versions at different perturbation levels ( $1{\mathrm{\Sigma }}^{\prime }$ : 1 m deviation between the x and y states, $0.1\circ$ deviation in $\theta$ ) in a simulated environment, based on SR, OR, and TT.

Perturbation level	Methods	SR (%)	OR (%)	TT (seconds)
$0{\mathrm{\Sigma }}^{\prime }$	SPF-EMPC w/o SS	48	0	47.50
	SPF-EMPC w/o SPF	56	4	30.00
	Ours	86	2	33.12
$1{\mathrm{\Sigma }}^{\prime }$	SPF-EMPC w/o ESO	42	2	76.43
	Ours	88	0	33.42

SPF-EMPC: safety probability field-based extended state model predictive control; SR: success rate; OR: overtime rate; TT: travel time; SS: self-sampling; SPF: safety probability field; ESO: extended state observer.

The data in the table show that even under ideal localization conditions ( $0{\mathrm{\Sigma }}^{\prime }$ ), the SPF-EMPC cannot accurately follow the iterative trajectory without SS, ultimately resulting in the lowest recorded success rate. Furthermore, the absence of the SPF severely affects the system’s ability to avoid dynamic obstacles, resulting in a significant decrease in success rate. Considering the inherent deviation in the robot’s state, the lack of ESO complicates the controller’s ability to monitor the state, potentially issuing erroneous commands and increasing the complexity of real-time optimization. As a result, this leads to a decrease in success rate and a significant increase in travel time. These findings demonstrate the effectiveness and necessity of integrated components in our proposed approach, especially when facing irregularly moving obstacles and imprecise robot state.

Enhanced performance validation

In this section, we further validate the performance of the proposed algorithm in both narrow environment and dynamic obstacle environment. To evaluate the collaborative obstacle avoidance capability of the proposed algorithm in constrained spaces, we constructed a narrow passage crossing scenario involving eight robots, as illustrated in Figure 9. The passage width was configured at 1.2 times the robot diameter, with initial robot positions symmetrically distributed at both ends of the passage and target positions set on the opposite side. Experimental results demonstrate that the robots initiated coordinated avoidance maneuvers at 16 seconds, formed an orderly queue by 23 seconds, and successfully completed the passage crossing within 30 seconds, exhibiting continuous trajectory curvature without any deadlock situations. To address the motion uncertainty of dynamic obstacles, the position deviation of the obstacle is set as ${\mathbf{v}}_{ie}^o\sim \mathcal{N}(\mathit{\mu }p,\mathbf{\Sigma })$ , and the velocity deviation as $\mathbf{p}{ie}^o\sim \mathcal{N}({\mathit{\mu }}_v,\mathbf{\Sigma })$ . Here, $\mathit{\mu }p$ and $\mathit{\mu }v$ represent the position and velocity of the obstacle at the previous time step, respectively. The deviation for the position component is $\sigma p=1\times {10}^{-4}$ , and for the velocity component, it is $\sigma v=1\times {10}^{-2}$ . The maximum speed of the obstacle matches the robot’s speed.

Figure 9.

Coordinated navigation of eight robots through a constrained narrow passage. The sequence demonstrates the efficacy of the proposed safety probability field-based extended state model predictive control (SPF-EMPC) framework in dense, structured environments: At $t=16$ seconds, robots initiate distributed collision-avoidance maneuvers. By $t=23$ seconds, they self-organize into a collision-free queue. At $t=30$ seconds, all robots successfully traverse the passage with smooth, non-deadlocking trajectories, validating the method’s capability for cooperative motion in tight spaces.

For clarity, dynamic obstacles are displayed in a circular configuration, as shown in Figure 10. The robot avoids obstacles in real time by leveraging the safety probability field generated by dynamic obstacles. At t=16 seconds and t=20 seconds, the robot proactively avoids the obstacles by predicting their movement trends based on the probability field.

Figure 10.

Eight robots navigate through a dense environment with two dynamic obstacles. (a) $t$ =17 seconds and (b) $t$ =20 seconds.

Physical experiment

In a real-world scenario, a scaled-down platform, ³⁸ measuring $1.374\times 2.432\text{m}$ , was constructed for conducting experiments. A three-wheeled omnidirectional robot was designed for physical testing, as shown in Figure 1.

Figure 11 shows an example of actual running. In the experiment, several static obstacles and one dynamic obstacle are set up to hinder the movement of the robots. The motion of the dynamic obstacle, the interference at the platform seams, and the robots’ traveling all pose challenges. Robots need to utilize their limited perception and communication capabilities to avoid various obstacles and other robots, and ultimately reach the goal point. Practical experiments demonstrate that the proposed method can enable the safe navigation of multiple robots in a dynamic environment. In this process, the results of the velocities returned by the robot are shown in Figure 12. The profiles confirm that all control commands remained within actuator limits, validating the dynamic feasibility and constraint compliance of the proposed trajectory tracking controller.

Figure 11.

Physical validation in a dynamic environment. (a) Setup with eight robots, eight static obstacles, and a dynamic obstacle. (b) A robot adjusts its trajectory in real-time to handle the irregular motion of the dynamic obstacle and localization inaccuracies at platform seams. (c) All robots reach their target positions collision-free, demonstrating the robustness of the safety probability field-based extended state model predictive control (SPF-EMPC) framework under real-world uncertainties.

Figure 12.

Wheel speed profiles from physical experiment, showing the measured linear velocities $(v_1,v_2,v_3)$ via onboard encoders.

Discussion

In this work, we propose SFC-EMPC, a novel method that demonstrates superior performance to the EGO-Swarm2 baseline in multi-robot cooperative motion planning under uncertain dynamic obstacles. Specifically, it achieves a 40% higher success rate and a 75% improvement in optimization efficiency in such challenging environments. Compared to the conventional MPC method, the proposed approach not only achieves the shortest path time in the presence of state perturbations, but also improves the collision avoidance success rate by 46% while tracking time-varying trajectories under such perturbed conditions.

The proposed method achieves higher success rates and optimization efficiency against uncertain dynamic obstacles due to two key mechanisms. First, a Gaussian-based safety probability field (SPF) models obstacle motion intention, providing a predictive ‘‘look-ahead” capability. By integrating this probabilistic forecast with the MINCO spatio-temporal joint trajectory optimization in a distributed manner, the planner can anticipate dynamic obstacle behavior in advance, thereby significantly improving navigation success. The SPF is essentially a normalized 2D Gaussian function; when multiple obstacles fall within the robot’s planning horizon, their individual fields can be efficiently fused into a Gaussian mixture-like collective SPF. This formulation is computationally lightweight, contributing to high efficiency during joint optimization. Second, the extended state observer (ESO) embedded in the MPC framework continuously estimates and compensates for both internal and external disturbances in real time, enabling safe and robust tracking of the planned trajectory under practical uncertainties.

Computational complexity. The computational efficiency of the SPF-EMPC framework stems from its distributed nature. The trajectory generator’s per-robot cost is dominated by the gradient-based optimizer, whose complexity scales with the number of local waypoints and obstacles within the SPF. Crucially, this cost is independent of the total number of robots, as each agent performs its own local planning. The extended state MPC solves a fixed-horizon problem, resulting in a constant per-step computation that does not scale with team size. This structure yields an overall per-robot complexity of $\mathcal{O}(1)$ with respect to the number of robots, which is empirically validated by the consistent sub-10 ms execution times measured in our multi-robot experiments (Figure 8).

Despite its advantages, our approach has several limitations. First, the coordination among multiple robots heavily relies on communication, and communication delays or packet loss will inevitably lead to collisions. Second, The MINCO optimizer utilized in SFC-MPC necessitates robots with differential flatness properties, leaving it unsuitable for other robot types. Third, when multiple robots navigate through narrow passages, deadlocks may arise, especially as the number of robots increases significantly.

Future work will focus on enhancing system robustness and generality by addressing three critical challenges: improving robustness to communication delays through asynchronous, event-driven planning; reducing dependency on specific dynamic models (e.g. differentially flat systems) by developing adaptive, platform-agnostic frameworks such as primitive-based approaches; and resolving deadlocks in confined spaces via spatio-temporal corridor scheduling to pre-allocate passage rights.

Conclusion

This article proposes SPF-EMPC, a multi-robot motion planner robust to dynamic obstacles and system uncertainties. Its key contributions are asfollows:

Introduces a safety probability field for efficient, iterative online trajectory generation via unconstrained optimization.

Employs polynomial fitting and an extended state observer to ensure accurate trajectory tracking under disturbances.

In simulation, matches the performance of an ideal baseline with significantly fewer optimization iterations.

Hardware experiments with eight robots demonstrate real-time capability and reliability in practice.