Multi-Agent Feedback Motion Planning using Probably Approximately Correct Nonlinear Model Predictive Control

Unified control and path planning for multi-robot systems is a challenging problem to address, especially in the presence of complex stochastic dynamics and measurement uncertainty. Even in discrete state and action spaces, the complexity of finding an optimal solution to the multi-agent path planning problem is NP-hard [1], and the computation time increases exponentially with the number of robots. The computational complexity is only further exacerbated in more realistic scenarios characterized by continuous state and action spaces where the multi-robot team must achieve objectives like formation control, obstacle avoidance, and reason about stochastic underactuated nonlinear dynamics.

Centralized controllers, as referenced in [2], [3], [4], and [5] are commonly employed to address these issues. However, these centralized approaches can be slow and not scale as the team size increases. Another limitation is that centralized approaches may not be able to update quickly enough to accommodate changing environments or imperfect environmental data, which could compromise the effectiveness of the path plan.

This paper builds on the sampling-based Stochastic Nonlinear Model Predictive Control (SNMPC) algorithm, Probably Approximate Correct NMPC (PAC-NMPC), as cited in [6], to enable distributed multiple robot collaboration and feedback motion planning in the presence of static and dynamic obstacles. In particular, we introduce a set of costs and constraints that capture the expected behavior of teammates over a finite horizon and enable probabilistically-safe formation control in cluttered environments. Our contributions are:

• •

  A receding-horizon feedback motion planning framework for distributed, probabilistically-safe multi-robot formation control in obstacle fields under dynamics and measurement uncertainty.

• •

  A terminal cost inspired by gyroscopic obstacle avoidance to improve finite-horizon planning in the presence of static and dynamic obstacles.

• •

  The demonstration of the algorithm’s effectiveness through numerical simulation in complex environments and its ability to scale to higher dimensions.

Researchers have explored a wide variety of strategies for achieving unified formation control and obstacle avoidance [7] [8]. Behavior-based controllers are one class of methods that have tried to address this problem [9],[10], [11]. These controllers combine the outputs from a set of sub-controllers that promote separate objectives, such as formation error, obstacle avoidance, and goal error. Typically, these approaches suffer from poor robustness and generalizability.

Consensus-based approaches achieve formation control by relying on local information to facilitate the convergence of all agents to the same information state[12, 13]. Some approaches have explored the joint formation control and obstacle avoidance problem [14] and others have extended consensus-based formation control to simple non-holonomic systems (e.g., [15], [16]).

Artificial Potential Fields (APF) are a set of approaches stemming from the path planning approach by Khatib [17]. APFs have been used to achieve formations while avoiding obstacles [18] [19] [20]. These allow for fixed formation and re-updating the controller after every time step and can be used for both leader-follower and other formation types, but are often limited to simple dynamics models.

Virtual structures achieve formation control by reasoning about the geometric performance of a rigid structure [21] [22]. Virtual structures have been used to adapt Probabilistic Roadmaps (PRM) [23]. Typically, virtual structures offer limited flexibility and obstacle avoidance.

Machine Learning and Reinforcement Learning have been used to handle multi-agent formation. For swarm robotics, learning has been used on the dynamics of nonlinear systems to give bounds on formation error [24]. Other approaches use policy search to solve nonlinear optimization problems within formation control [25] [26]. Reinforcement learning for leader-follower and swarm robotics has been used, training Double Deep Q-Networks to obtain proper formation behavior [27] or to achieve multi-robot planning in the presence of dynamic obstacles [28]. Oftentimes, learning-based methods demonstrate degraded performance in out-of-distribution environments and during sim-to-real transfer.

In recent years, online trajectory optimization and Nonlinear Model Predictive Control (NMPC) have emerged a powerful approaches for achieving multi-robot planning and control. Some approaches have applied NMPC to achieve formation control [29, 30, 31, 32, 33]. Other approaches have explored inter-agent collision avoidance via distributed NMPC [34, 35] or by combining NMPC with conflict-based search [36]. A few approaches have explored NMPC to achieve formation control in the presence of obstacles. [37] employs a virtual structure framework and [38] is restricted to a linear dynamics model.

To our knowledge, our approach is the first method to employ SNMPC to achieve distributed, probabilistically-safe multi-agent collision avoidance and formation control in cluttered environments for a large class of stochastic nonlinear dynamical systems and measurement uncertainty.

Our proposed multi-agent formation controller utilizes Probably Approximately Correct Nonlinear Model Predictive Control (PAC-NMPC) [6]. PAC-NMPC uses Iterative Stochastic Policy Optimization (ISPO) [39] to formulate the search for a local time-varying feedback control policy of the form $\mathbf{u}_{t}=\mathbf{K}_{t}(\bm{\tau}^{d}(\bm{\xi},\mathbf{x}_{0}))\left(\mathbf{x}_{t}^{d}(\bm{\xi},\mathbf{x}_{0})-\mathbf{x}_{t}\right)+\mathbf{u}_{t}^{d}(\bm{\xi})$, as a stochastic optimization problem. This is achieved by sampling the policy parameters, $\bm{\xi}$, from a surrogate distribution given by the probability density function $p(\bm{\xi}|\bm{\nu})$ and defined by hyper-parameters $\bm{\nu}$. Here $\bm{\tau}^{d}\triangleq\{\mathbf{x}_{0}^{d},\mathbf{u}_{0}^{d},\mathbf{x}_{1}^{d},\mathbf{u}_{1}^{d},...,\mathbf{u}_{N_{T}-1}^{d},\mathbf{x}_{N_{T}}^{d}\}$ is the nominal trajectory computed using a nominal deterministic discrete-time dynamics model $\mathbf{x}_{t+1}^{d}=\mathbf{x}_{t}^{d}+\mathbf{f}(\mathbf{x}_{t}^{d},\mathbf{u}_{t}^{d})\Delta t$ over $N_{T}$ time steps. $\mathbf{x}_{t}^{d}\in\mathbb{R}^{N_{x}}$ and $\mathbf{u}_{t}^{d}\in\mathbb{R}^{N_{u}}$ are the nominal states and control inputs at time index $t$ respectively. $\Delta t$ is the discrete time step and $\mathbf{K}_{t}(\bm{\tau}^{d}(\bm{\xi},\mathbf{x}_{0}))\in\mathbb{R}^{N_{u}\times N_{x}}$ is a sequence of time-varying feedback gains computed using the finite horizon, discrete, time-varying linear quadratic regulator (TVLQR) [40]. The policy is parameterized only by the nominal input sequence, so that $\bm{\xi}~=[{\mathbf{u}^{d}_{0}}^{T}\ {\mathbf{u}^{d}_{1}}^{T}\ ...\ {\mathbf{u}^{d}_{N_{T}-1}}^{T}]^{T}$. The surrogate distribution, $p(\bm{\xi}|\bm{\nu})$, is parameterized as a multivariate Gaussian, $\mathcal{N}\left(\bm{\xi}|\bm{\mu},\bm{\Sigma}\right)$, with a mean, $\bm{\mu}$, and a covariance matrix, $\bm{\Sigma}$. Since the covariance matrix is diagonal, the hyperparameters can be written as $\bm{\nu}\triangleq[\bm{\mu}^{T}\ diag(\bm{\Sigma})^{T}]^{T}$. For a discrete-time trajectory sequence $\bm{\tau}=\{\mathbf{x}_{0},\mathbf{u}_{0},\mathbf{x}_{1},\mathbf{u}_{1}...,\mathbf{u}_{N_{T}-1},\mathbf{x}_{N_{T}}\}$, one can define a non-negative trajectory cost function, $J(\bm{\tau})\geq 0$, and a trajectory constraint violation function $C(\bm{\tau})\in\{0,1\}$.

During each planning iteration, PAC-NMPC optimizes a weighted objective function $\bm{\nu}^{*}=\operatorname*{arg\,min}_{\bm{\nu}}\min_{\alpha>0}(\mathcal{J}^{+}_{\alpha}(\bm{\nu})+\gamma\mathcal{C}^{+}_{\alpha}(\bm{\nu}))$ where $\mathcal{J}^{+}_{\alpha}(\bm{\nu})$ is the PAC bound on $J(\bm{\tau})$, $\mathcal{C}^{+}_{\alpha}(\bm{\nu})$ is the PAC bound on $C(\bm{\tau})$, and $\gamma$ is positive weighting coefficient. These PAC bounds, which are derived in [39], take the form

where $\widehat{\mathcal{J}}_{\alpha}(\bm{\nu})$ is a robust estimator of the expected cost, $d(\bm{\nu})$ is a distance between distributions, $\Phi_{\alpha}(\delta)$ is a concentration-of-measure term, and $1-\delta$ is the bound confidence.

This optimization ensures that the chosen control policy satisfies performance and safety requirements with a specified confidence level. In essence, PAC-NMPC offers a robust and statistically guaranteed approach for controlling systems with uncertainties.

This section presents a centralized and distributed approach for PAC-NMPC to control a team of $M$ agents in formation, navigating an obstacle-filled environment to a goal state $\mathbf{x}^{G}$.

In real-world applications, relying solely on shared state information leads to discrepancies due to sensor noise, communication delays, and environmental uncertainties. These factors introduce discrepancies between an agent’s actual state and the state information received by other agents. Not accounting for the uncertainty in the state measurement could lead to collisions or worse performance.

We model the state measurement error as a zero-mean Gaussian noise with a covariance $\mathbf{\Sigma}_{p}$. The received state of agent $j$ is represented as $\hat{\mathbf{x}}^{j}\sim\mathcal{N}(\mathbf{x}^{j},\mathbf{\Sigma}_{p})$ where $\mathbf{x}^{j}$ is the true state and $\hat{\mathbf{x}}^{j}$ is the received state. To reason about state measurement uncertainty, the algorithm samples the state from the normal distributions for each agent during trajectory rollouts. This approach enables an agent to consider other agents’ state uncertainties and plan its trajectory to enable robust, collision-free navigation under measurement uncertainty.

We conducted Monte Carlo simulations to test the effectiveness of multi-agent PAC-NMPC for formation control and collision avoidance.

We simulate a stochastic bicycle model with acceleration and steering rate inputs for each agent. We denote ${\mathbf{x}_{t}}=[p_{x},p_{y},\theta,v,\delta_{s}]^{T}$ as the state vector, ${\mathbf{u}_{t}}=[\dot{v},\dot{\delta_{s}}]^{T}$ as the control vector, $l=0.33$ as the wheel base, and $\bm{\Gamma}=diag(\left[0.001,0.001,0.1,0.2,0.001\right])$ as the covariance. The stochastic bicycle model is defined as

The acceleration is limited to $-1\ m/s^{2}\leq\dot{v}\leq 1\ m/s^{2}$, the steering rate is limited $-1\ rad/s\leq\dot{\delta_{s}}\leq 1\ rad/s$, the velocity is limited to $-0.5\ m/s\leq v\leq 2\ m/s$, the steering angle is limited to $-0.4\ rad\leq\delta_{s}\leq 0.4\ rad$.

We generated fifty random obstacle fields of 10 non-overlapping circular obstacles with an inflated radius of 0.6 meters to test the formation control in obstacle-filled environments. The obstacles were between ${\bf p}_{min}=[3,-4]$ and ${\bf p}_{max}=[10,4]$. The fields were generated with free space before and after the obstacle field to allow the formation to converge. The desired formation was a 3-agent wedge. The formation goals were defined and calculated as

where $l$ is the distance in the x-direction behind the leader, and $h$ is the distance in the y-direction behind the leader. For these experiments, $l=h=1\text{ meter}$. We chose these distances to make a formation too small to wrap around obstacles but not large enough where the agents were too far away to interact.

We chose a goal at $\mathbf{p}^{G,1}$ = [15, 0]. We sampled the leader agent state from a uniform distribution between $x_{min}^{1}$ = [-1,
-2, $-\frac{\pi}{8}$, 0.0, -0.2] and $x_{max}^{1}$ = [1, 2, $\frac{\pi}{8}$, 1.0, 0.2]. The follower agents’ initial positions were placed in the wedge formation with some noise sampled from a uniform distribution with a bound of $\pm$[0.5, 0.5, $\frac{\pi}{16}$, 0.5, 0.2].

We used an open environment to test the inter-agent collision avoidance further. Three agents were placed symmetrically around a 5-meter-radius circle, given a goal point antipodal to the start position. In these trials, the optimal paths of all three agents would meet in the center of the circle to encourage head-on collisions.

The parameters were $\omega_{1}=0.5$, $\omega_{2}=0.1$, $k$ = 3, $k_{obs,static}=1$, $k_{att,static}=8$, $k_{obs,agent}=k_{att,agent}=0.5$, ${\bf Q}_{f}^{1}=diag([1.0\ 1.0])$ and ${\bf Q}_{f}^{2,3}=diag([1.0\ 1.5])$

In this paper, we presented a distributed receding-horizon SNMPC approach for navigating a multi-robot team through an obstacle field in formation. We demonstrated in simulation that using a gyroscopic avoidance-based cost enables the controller to keep formation by avoiding collisions and local minima. Further, we showed that control policy sharing could enable collision avoidance under state measurement uncertainty. Future work could explore heterogeneous teams and apply this model to more complex environments and dynamics.

We gratefully acknowledge the support of the Army Research Laboratory under grant W911NF-22-2-0241.