# RL-based Variable Horizon Model Predictive Control of Multi-Robot Systems
using Versatile On-Demand Collision Avoidance

Shreyash Gupta1, Abhinav Kumar1, Niladri S. Tripathy2 and Suril V. Shah1
1Shreyash Gupta, Abhinav Kumar and Suril V. Shah are with the Department of
Mechanical Engineering, Indian Institute of Technology Jodhpur, Jodhpur, India
<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>S.
Tripathy is with the Department of Electrical Engineering, Indian Institute of
Technology Jodhpur, Jodhpur, India<EMAIL_ADDRESS>
###### Abstract

Multi-robot systems have become very popular in recent years because of their
wide spectrum of applications, ranging from surveillance to cooperative
payload transportation. Model Predictive Control (MPC) is a promising
controller for multi-robot control because of its preview capability and
ability to handle constraints easily. The performance of the MPC widely
depends on many parameters, among which the prediction horizon is the major
contributor. Increasing the prediction horizon beyond a limit drastically
increases the computation cost. Tuning the value of the prediction horizon can
be very time-consuming, and the tuning process must be repeated for every
task. Moreover, instead of using a fixed horizon for an entire task, a better
balance between performance and computation cost can be established if
different prediction horizons can be employed for every robot at each time
step. Further, for such variable prediction horizon MPC for multiple robots,
on-demand collision avoidance is the key requirement. We propose Versatile On-
demand Collision Avoidance (VODCA) strategy to comply with the variable
horizon model predictive control. We also present a framework for learning the
prediction horizon for the multi-robot system as a function of the states of
the robots using the Soft Actor-Critic (SAC) RL algorithm. The results are
illustrated and validated numerically for different multi-robot tasks.

##  I Introduction

Multi-robot systems (MRS) are a collection of autonomous or semi-autonomous
robots that can coordinate and work together to achieve a common goal. They
are increasingly popular in various applications such as search and rescue,
exploration, surveillance, and logistics [1][2]. It can provide better
efficiency, increased flexibility, and improved reliability compared to single
robots in these applications [3]. However, controlling a group of robots
presents a unique set of challenges, such as communication, task allocation,
and coordination. To overcome these challenges, one must devise control
strategies to guarantee that each robot can perform its tasks effectively and
efficiently while maintaining overall system performance. Model Predictive
Control (MPC) is considered one of the suitable controllers for multi-robot
systems due to its ability to handle complex tasks, nonlinear dynamics and
constraints. MPC is a control technique that uses a predictive model of the
system and an optimization algorithm to generate control actions that minimize
a cost function while satisfying system constraints [4]. In multi-robot
systems, MPC can be used to optimize the coordination and motion of the robots
in real-time while ensuring collision avoidance and efficient task allocation
[5]. MPC can also handle uncertainties [6] such as sensor noise and robot
failures, making it a robust and reliable control strategy for multi-robot
systems. In MPC, tuning of various parameters is crucial in determining the
controller’s performance. Selecting improper tuning parameters can lead to
unstable or inefficient control behavior, e.g., if the prediction horizon is
too short or the weighting factors are chosen poorly, the control system may
not accurately track the reference trajectory or may consume excessive amounts
of energy. The tuning process can be time-consuming and finding optimal values
of tuning parameters require domain expertise and systematic experimentation
to find the best set of parameters.

The prediction horizon in MPC is the length of time over which the future
behavior of the system is predicted to generate optimal control actions. The
prediction horizon is an important parameter that affects the performance of
MPC. A longer prediction horizon can result in more accurate predictions of
the system’s behavior, leading to better control performance. However, it
requires more computational resources, leading to slower control response time
and potential instability. On the other hand, a shorter prediction horizon can
result in faster control response time but it may lead to less accurate
predictions of the system’s behavior and suboptimal control performance [14].
This observation served as the impetus for the development of the adaptive or
variable horizon model predictive control. Researchers have devised several
techniques to decide the optimal value of the prediction horizon. In [7], [8],
the value of the horizon was included as an optimization variable in the MPC
scheme. However, this may make the optimization problem non-quadratic and can
lead to an increase in computation load. The quality of a predicted reference
signal was used to decide how far off into the future one can effectively
define the original MPC problem [9]. An iterative deepening search where
stability criteria are checked on each iteration to identify the lowest
stabilizing horizon is one of the heuristics-based approaches proposed in
[10]. Most of these methods are dependent on heuristic approaches, which may
not be optimum for a wide spectrum of multi-robot applications. Hence,
learning-based approaches can be advantageous in estimating the prediction
horizon for every time-step. Reinforcement Learning was used in [11], to learn
the prediction horizon for a single robot avoiding static obstacles and was
used in [12] to tune other meta-parameters of MPC. Further, extending such an
approach to multi-robot systems may also require to deal with static and
dynamic obstacles. The on-demand collision avoidance approach is shown to be
advantageous in dealing with dynamic obstacles within MPC [13]. However, in
the presence of variable prediction horizon, its preview capability may get
compromised.

To the best of our knowledge, RL based variable horizon MPC for multi-robot
systems with improved on-demand collision avoidance to negotiate both static
and dynamic obstacles is not reported in the literature. This serves as the
main motivation behind this work. The contributions of this paper can be
summarized as:

  1. 1.

We develop a versatile on-demand collision avoidance method (VODCA) for
variable horizon MPC.

  2. 2.

We propose a framework to learn the prediction horizons of multiple robots in
presence of static and dynamic obstacles using reinforcement learning.

  3. 3.

Numerical simulations are presented for different multi-robot configurations
to confirm our propositions.

##  II Problem Description and Proposed Solution

###  II-A Problem Description:

Consider an environment with $N$ robots, where each robot can be defined using
a single integrator model as

$\boldsymbol{\dot{x}}_{i}=\boldsymbol{A}_{c}\boldsymbol{x}_{i}+\boldsymbol{B}_{c}\boldsymbol{u}_{i},$
(1)

where state and input matrices $\boldsymbol{A}_{c}\in\mathbb{R}^{2\times
2}=\boldsymbol{0}$ and $\boldsymbol{B}_{c}\in\mathbb{R}^{2\times
2}=\boldsymbol{I}$, respectively. (1) can be discretized to obtain a discrete-
time linear model as

$\boldsymbol{x}_{i}[k+1]=\boldsymbol{A}\boldsymbol{x}_{i}[k]+\boldsymbol{B}\boldsymbol{u}_{i}[k]$
(2)

where $\boldsymbol{A}\in\mathbb{R}^{2\times 2}=\boldsymbol{I}$,
$\boldsymbol{B}\in\mathbb{R}^{2\times 2}=h\boldsymbol{I}$, $\boldsymbol{I}$ is
an identity matrix, $h$ is the sampling time and $k$ is the discrete time-
step.

Given the multiagent system consisting of $N$ robots with static and dynamic
obstacles, the work addresses the following two problems :

  * •

Problem 1: Develop a method to ensure on-demand collision avoidance of $N$
robots with variable horizon Model Predictive Control (MPC).

  * •

Problem 2: Learn the optimal sequence of the prediction horizon for all the
$N$ robots to complete their task avoiding collision with static and dynamic
obstacles.

###  II-B Proposed Solution:

In scenarios involving multiple robots, the ability to detect and prevent
potential collisions is crucial. However, the current on-demand collision
avoidance method [13] was insufficient to guarantee collision avoidance
because the length of the prediction horizon ($N_{i}^{k}$) of $i^{th}$ robot
at $k^{th}$ step varies at each time-step. To solve Problem 1, we propose
VODCA (Versatile On-demand Collision Avoidance), a generalization of the on-
demand collision avoidance technique, to guarantee the safe operation of
multi-robot systems. The VODCA has the ability to ensure collision avoidance
with variable horizon MPC, by exploiting the preview capability of MPC. Based
on the values of prediction horizons of individual robots at individual time
steps, the VODCA partitions the collision avoidance issue into three cases.
These cases are then solved individually to generate collision avoidance
constraints. The method is discussed in detail in Section III-C.

To solve Problem 2, we use Soft Actor-Critic (SAC) RL algorithm to learn the
optimal sequence of prediction horizon for each robot in a multi-robot system.
We aim to learn a sequence, $\\{N_{i}^{0},N_{i}^{1},N_{i}^{2},\dots\\}$ for
each robot $i$, as a function of the states of the robots. We use a single
Neural Network (NN) to learn the prediction horizon sequence of all the
robots. The RL framework learns online with immediate rewards acquisition. The
policy network takes in the states of all the robots and outputs a pair of
mean and standard deviation for each robot. This Gaussian distribution, mean
and standard deviation are passed through the hyperbolic tangent function to
confine the value between -1 and 1; this value then is scaled and rounded off
to obtain the prediction horizons of all the robots,
$\\{N_{1}^{k},N_{2}^{k},\dots,N_{N}^{k}\\}$, at that time step $k$.

The next section discusses the variable horizon MPC formulation and the
proposed collision avoidance method, VODCA.

##  III Variable prediction horizon MPC with a versatile on-demand collision

In this section, we present variable prediction horizon MPC for multi-robot
systems with a versatile on-demand collision avoidance strategy.

###  III-A The Prediction Model

The prediction model for $i^{th}$ robot can be given by,

$\hat{\boldsymbol{x}}_{i}[k_{t}+1|k]=\boldsymbol{A}\hat{\boldsymbol{x}}_{i}[k_{t}|k]+\boldsymbol{B}\hat{\boldsymbol{u}}_{i}[k_{t}|k]$
(3)

where $k$ is current time step, $\hat{(\cdot)}[k_{t}|k]$ represents the
predicted value of $(\cdot)[(k_{t}+k)h]$ with available information at $kh$,
$k_{t}\in\\{0,1,2,\dots,N_{i}^{k}-1\\}$ and $h$ is the sampling time.
$N_{i}^{k}$ represents the length of the prediction horizon of $i^{th}$ robot
at $k^{th}$ time-step and it varies with change in $k$. Using (3), for the
prediction horizon $N_{i}^{k}$, the prediction model can be written in the
stacked form as

$\hat{\boldsymbol{X}}_{i}=\boldsymbol{F}\bar{\boldsymbol{x}}_{i}[0|k]+\boldsymbol{\Phi}\hat{\boldsymbol{U}}_{i},$
(4)

where $\hat{\boldsymbol{X}}_{i}\in\mathbb{R}^{2N_{i}^{k}}$ and
$\hat{\boldsymbol{U}}_{i}\in\mathbb{R}^{2N_{i}^{k}}$ are the stacked predicted
states and inputs respectively, and $\bar{\boldsymbol{x}}_{i}[0|k]$ is the
current measured state of the $i^{th}$ robot. The matrices
$\boldsymbol{F}\in\mathbb{R}^{2N_{i}^{k}\times 2}$ and
$\boldsymbol{\Phi}\in\mathbb{R}^{2N_{i}^{k}\times 2N_{i}^{k}}$ are given as

$\boldsymbol{F}=\begin{bmatrix}\boldsymbol{A}\\\ \boldsymbol{A}^{2}\\\ .\\\
.\\\ .\\\
\boldsymbol{A}^{N_{i}^{k}}\end{bmatrix},\boldsymbol{\Phi}=\begin{bmatrix}\boldsymbol{B}&\boldsymbol{0}&\cdot&\boldsymbol{0}\\\
\boldsymbol{AB}&\boldsymbol{B}&\cdot&\boldsymbol{0}\\\ .&.&.&.\\\ .&.&.&.\\\
.&.&.&.\\\
\boldsymbol{A}^{N_{i}^{k}-1}\boldsymbol{B}&\boldsymbol{A}^{N_{i}^{k}-2}\boldsymbol{B}&\cdot&\boldsymbol{B}\end{bmatrix}.$
(5)

Since the prediction horizon $N_{i}^{k}$ varies, the dimensions of matrices
$\boldsymbol{F}$ and $\boldsymbol{\Phi}$ will change with time.

###  III-B Cost Function

We use a cost function comprising two terms: the first term minimizes the
error between the current and desired states, and the second term minimizes
the control input. Let the desired state vector is denoted as
$\boldsymbol{X}_{i}^{d}\in\mathbb{R}^{2N_{i}^{k}}$. It is worth noting that
for variable prediction horizon MPC the size of $\boldsymbol{X}_{i}^{d}$ will
depend on the length of $N_{i}^{k}$. The cost function can then be represented
as

$J_{i}^{MPC}=(\boldsymbol{X}^{d}_{i}-\boldsymbol{\hat{X}}_{i})^{\top}\boldsymbol{Q}(\boldsymbol{X}^{d}_{i}-\boldsymbol{\hat{X}}_{i})+\boldsymbol{\hat{U}_{i}}^{\top}\boldsymbol{W}\boldsymbol{\hat{U}_{i}},$
(6)

where $\boldsymbol{Q}\in\mathbb{R}^{2N_{i}^{k}\times 2N_{i}^{k}}\geq 0$ and
$\boldsymbol{W}\in\mathbb{R}^{2N_{i}^{k}\times 2N_{i}^{k}}>0$ are the weight
matrices. The optimization problem solves for the minimization of
$J_{i}^{MPC}$ and yields optimal control inputs, ensuring the maneuver of
robots from initial to desired states. In the next section, we discuss the
proposed generalized collision avoidance method for multi-robot systems with
variable prediction horizons.

###  III-C Versatile On-demand Collision Avoidance (VODCA)

On-demand collision avoidance schemes [13, 15] take advantage of the preview
capability of MPC, look into the future predictions of respective states of
the robots, and calculate the collision avoidance constraints only when it
detects a collision in the future. However, the above schemes for the fixed
horizon MPC when applied to variable prediction horizon MPC may suffer from a
lack of preview capability when $N_{i}^{(k-1)}>N_{i}^{k}$. Therefore, we
develop Versatile On-Demand Collision Avoidance (VODCA), the generalized
version of On-demand collision avoidance [13] for variable prediction horizon
MPC to ensure collision-free maneuver of multiple robots towards their desired
state.

The collision is detected by checking the error between the predicted states
of $i^{th}$ robot,
$\boldsymbol{\hat{X}}_{i}^{(k-1)}\in\mathbb{R}^{2N_{i}^{(k-1)}}$ and the
predicted states of $j^{th}$ robot,
$\boldsymbol{\hat{X}}_{j}^{(k-1)}\in\mathbb{R}^{2N_{j}^{(k-1)}}$ or the
$l^{th}$ obstacle, $\boldsymbol{O}_{b,l}$. The possibility of
$N_{i}^{(k-1)}\neq N_{j}^{(k-1)}$, makes this error calculation unfeasible. To
overcome this problem, the VODCA first finds

$N_{min}^{(k-1)}=min(N_{i}^{(k-1)},N_{j}^{(k-1)}).$ (7)

Next, we resize the length of the predicted state vector to
$2N_{min}^{(k-1)}$. Now consider that robots and static obstacles are modeled
as a disc with a radius equal to $r_{min}/2$. Then, the minimum distance
required for collision avoidance and safe maneuvering of the robots in the
environment is $r_{min}$. The collision detection is performed based on the
predicted states obtained at the previous time-step $(k-1)$. It can be said
that there would be a collision in the future, if and only if, the distance
($d_{i}$) between the $i^{th}$ the robot and $j^{th}$ robot (which serves as a
dynamic obstacle (DO)) or the $i^{th}$ robot and $l^{th}$ static obstacle (SO)
satisfies the following:

$d_{i}=\begin{cases}\left\|\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]-\boldsymbol{\hat{x}}_{j}[k_{c,i}|k-1]\right\|<{r}_{min}&\text{If
DO}\\\
\left\|\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]-{\boldsymbol{o}}_{b,l}\right\|<{r}_{min}&\text{If
SO}\end{cases}$ (8)

where, $k_{c,i}(\leq N_{min}^{(k-1)})$ is the time-step when the robot $i$
detects a collision with another robot or static obstacle. Therefore, in order
to avoid collision, $i^{th}$ robot must attain a position at
$(k_{c,i}-1+k)^{th}$ time-step such that,

$\begin{cases}\left\|\boldsymbol{\hat{x}}_{i}[k_{c,i}-1|k]-\boldsymbol{\hat{x}}_{j}[k_{c,i}|k-1]\right\|\geq{r}_{min}&\text{If
DO}\\\
\left\|\boldsymbol{\hat{x}}_{i}[k_{c,i}-1|k]-{\boldsymbol{o}}_{b,l}\right\|\geq{r}_{min}&\text{If
SO}\end{cases}$ (9)

is satisfied. Equation (9) can be linearized using Multivariate Taylor-Series
expansion about $\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]$ and can be rearranged
to obtain [13][15]

${\boldsymbol{b}}_{i}^{\top}\boldsymbol{\hat{x}}_{i}[k_{c,i}-1|k]\geq c_{i},$
(10)

where,

${\boldsymbol{b}}_{i}=\begin{cases}(\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]-\boldsymbol{\hat{x}}_{j}[k_{c,i}|k-1])&\text{If
DO}\\\ (\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]-{\boldsymbol{o}}_{b,l})&\text{If
SO}\end{cases}$ (11)

and
$c_{i}=(r_{min}d_{i}-d_{i}^{2})+\boldsymbol{b}_{i}^{\top}\boldsymbol{\hat{x}}_{i}[k_{c,i}|k-1]$.
According to [13], placing constraints on
$\boldsymbol{\hat{x}}_{i}[k_{c,i}|k]$ rather than
$\boldsymbol{\hat{x}}_{i}[k_{c,i}-1|k]$ (i.e. one time-step after the observed
collision) improves collision avoidance even more. Stacked form of eq. (10)
can be obtained by re-writing the eq. using
$\boldsymbol{\hat{X}}_{i}^{k}\in\mathbb{R}^{2N_{i}^{k}}$ as,

$\boldsymbol{g}_{i}^{\top}\boldsymbol{\hat{X}}_{i}^{k}\geq c_{i}$ (12)

where $\boldsymbol{\hat{x}}_{i}[k_{c,i}|k]$ is the $k_{c,i}^{th}$ vector of
$\boldsymbol{\hat{X}}_{i}^{k}$.

As mentioned earlier, in the case of the variable horizon MPC when
$N_{i}^{(k-1)}>N_{i}^{k}$, it may happen that $k_{c,i}$ is greater than
$N_{i}^{k}$. In such cases, (10) cannot be written in the stacked form given
in (12). This established the requirement of VODCA.

###### Remark 1

The value of $k_{c,i}$ will always be less than or equal to $N_{i}^{k-1}$, as
the collision is being predicted on the basis of information available at
$(k-1)$ time-step. In fixed horizon MPC, $N_{i}^{k}$ is equal to $N_{i}^{k-1}$
and hence $k_{c,i}$ will also be less than or equal to $N_{i}^{k}$; but in
variable horizon MPC, $N_{i}^{k}$ is not necessarily equal to $N_{i}^{k-1}$,
therefore, a clear relation between $k_{c,i}$ and $N_{i}^{k}$ cannot be
established.

To deal with this, VODCA breaks down the variable $\boldsymbol{g}_{i}$ as
follows

$\boldsymbol{g}_{i}^{\top}=\begin{cases}\begin{bmatrix}\boldsymbol{b}_{i}^{\top}&\boldsymbol{0}^{\top}_{2(N_{i}^{k}-1)\times
1}\end{bmatrix}^{\top}&\text{ for $k_{c,i}=1$}\\\
\begin{bmatrix}\boldsymbol{0}^{\top}_{2(k_{c,i}-1)\times
1}&\boldsymbol{b}_{i}^{\top}&\boldsymbol{0}^{\top}_{2(N_{i}^{k}k_{c,i})\times
1}\end{bmatrix}^{\top}&\text{for $k_{c,i}<N_{i}^{k}$}\\\
\begin{bmatrix}\boldsymbol{0}^{\top}_{2(N_{i}^{k}-1)\times
1}&\boldsymbol{b}_{i}^{\top}\end{bmatrix}^{\top}&\text{for
$k_{c,i}=N_{i}^{k}$}\end{cases}$ (13)

Using (13), (12) can be rearranged to obtain the following inequality
constraints,

$\boldsymbol{A}_{i}^{coll}\hat{\boldsymbol{U}}_{i}\leq\boldsymbol{b}_{i}^{coll},$
(14)

where
$\boldsymbol{A}_{i}^{coll}=-\boldsymbol{g}_{i}^{\top}\boldsymbol{\Phi}\text{
and
}\boldsymbol{b}_{i}^{coll}=\boldsymbol{g}_{i}^{\top}\boldsymbol{F}\bar{\boldsymbol{x}}_{i}[0|k]-c_{i}$.
The bounds on the control input, which ensures that it within the physical
limits of the robot, can be stated as,

$\boldsymbol{lb}\leq\boldsymbol{\hat{U}}\leq\boldsymbol{ub},$ (15)

where $\boldsymbol{lb}\in\mathbb{R}^{2N_{i}^{k}}$ and
$\boldsymbol{ub}\in\mathbb{R}^{2N_{i}^{k}}$ are the lower and upper bounds,
respectively. Algorithm 1 describes implementing the proposed VODCA.

The collision avoidance constraints (14) and the control input bounds (15) are
augmented and written in the form of inequality constraints as,

$\boldsymbol{A}_{i}^{ineq}\boldsymbol{\hat{U}}_{i}\leq\boldsymbol{b}_{i}^{ineq},$
(16)

where
$\boldsymbol{A}_{i}^{ineq}={\begin{bmatrix}{\boldsymbol{A}^{coll}_{i}}^{\top}&-\boldsymbol{I}^{\top}&\boldsymbol{I}^{\top}\end{bmatrix}}^{\top}\in\mathbb{R}^{(4N_{i}^{k}+1)\times
2N_{i}^{k}}$ and
$\boldsymbol{b}^{ineq}_{i}={\begin{bmatrix}{\boldsymbol{b}^{coll}_{i}}^{\top}&-\boldsymbol{lb}^{\top}&\boldsymbol{ub}^{\top}\end{bmatrix}}^{\top}\in\mathbb{R}^{(4N_{i}^{k}+1)\times
1}$.

The quadratic optimization problem for a multi-robot system with collision
avoidance can be stated as

$\displaystyle\min_{\boldsymbol{\hat{U}_{i}}}$ $\displaystyle J^{MPC}_{i}$
(17) subject to
$\displaystyle\boldsymbol{A}_{i}^{ineq}\boldsymbol{\hat{U}}_{i}\leq\boldsymbol{b}_{i}^{ineq}$

The solution to this optimization problem generates an optimal control input
sequence that encourages robots to converge to their respective desired state
without getting collided. For variable horizon MPC, the selection of a
prediction horizon is key challenge and will be addressed in the subsequent
section.

Algorithm 1 Versatile On-demand Collision Avoidance (VODCA).

1:Initialize: $N_{i}^{k-1}$, $N_{i}^{k}$, $r_{min}$, F, $\boldsymbol{\Phi}$,
$\hat{\textbf{X}}[k-1]$, $\boldsymbol{O_{b}}$, $\bar{\textbf{x}}[k]$

2:Output: $\boldsymbol{A}_{i}^{coll}$ and $\boldsymbol{b}_{i}^{coll}$

3:$\boldsymbol{d}_{i}^{all}\leftarrow$
calDist$(\boldsymbol{\hat{X}},\boldsymbol{O_{b}},N_{i}^{k-1},N_{i}^{k})$

4:if $\boldsymbol{d}_{i}^{all}<r_{min}$ then

5:

6: $[d_{i},k_{c,i},\boldsymbol{b}_{i}]\leftarrow$
colsnPredict$(\boldsymbol{\hat{X}},\boldsymbol{O_{b}})$

7: $c_{i}\leftarrow$
calculateCi$(r_{min},d_{i},\boldsymbol{b}_{i},\boldsymbol{\hat{X}}[k-1],k_{c,i})$

8: if $k_{c,i}=1$ then

9:
$\boldsymbol{g}_{i}^{\top}=\begin{bmatrix}\boldsymbol{b}_{i}^{\top}&\boldsymbol{0}^{\top}_{2(N_{i}^{k}-1)\times
1}\end{bmatrix}^{\top}$

10: else if $k_{c,i}<N_{i}^{k}$ then

11:
$\boldsymbol{g}_{i}^{\top}=\begin{bmatrix}\boldsymbol{0}^{\top}_{2(k_{c,i}-1)\times
1}&\boldsymbol{b}_{i}^{\top}&\boldsymbol{0}^{\top}_{2(N_{i}^{k}-k_{c,i})\times
1}\end{bmatrix}^{\top}$

12: else if $k_{c,i}=N_{i}^{k}$ then

13:
$\boldsymbol{g}_{i}^{\top}=\begin{bmatrix}\boldsymbol{0}^{\top}_{2(N_{i}^{k}-1)\times
1}&\boldsymbol{b}_{i}^{\top}\end{bmatrix}^{\top}$

14: end if

15:end if

16:$[\boldsymbol{A}_{i}^{coll},\boldsymbol{b}_{i}^{coll}]\leftarrow$
calColsnConst$(\boldsymbol{g}_{i},\boldsymbol{\Phi},c_{i},\textbf{F},\bar{\textbf{x}}_{i}[0|k])$

##  IV LEARNING OF PREDICTION HORIZON

In this work, we propose using RL to learn the prediction horizon for each
robot. We employ Soft Actor-Critic (SAC) [16], an entropy-maximization actor-
critic RL algorithm with a parameterized stochastic policy, to determine the
value of the prediction horizon at each time-step of MPC. In RL, the
environment is defined as a Markov Decision Process (MDP), which is
represented using a tuple
$(\mathcal{S},\mathcal{A},\mathcal{P},\mathcal{R},\gamma)$. For the given
environment, $\mathcal{S}$ is the set of all possible states, $\mathcal{A}$ is
the set of all possible actions, $\mathcal{P}$ is the state transition
function, $\mathcal{R}$ is the reward function, and $\gamma\in[0,1)$ is the
discount factor which decides how future rewards are valued. The aim of RL is
to come up with a policy $\pi_{\theta}$ such that it maximizes the expected
sum of rewards acquired over the states visited by the policy in an episode,
i.e., to optimize:

$J^{RL}(\theta)=\underset{\theta}{\max}\mathbb{E}\Biggr{[}\sum_{k=0}^{K}\gamma^{k}R(s,\pi_{\theta}(s))\Biggr{]},\forall
s_{0}\in\mathcal{S}_{0}.$ (18)

where $K$ is the maximum number of time steps and $\theta$ represents the
design parameters of the policy network. The trajectory distribution produced
by the policy and the state transition function, as well as the starting state
distribution $\mathcal{S}_{0}$, are taken into account when calculating the
expectation.

In SAC, an additional entropy term is augmented with the RL objective function
defined as,

$\alpha\mathcal{H}(\pi_{\theta}(\cdot|s))=\alpha\mathbb{E}_{a\sim\pi_{\theta}(a|s)}[-\text{log}(\pi_{\theta}(a|s))].$
(19)

This entropy term favors stochastic policies and encourages exploration of the
environment. The intensity of exploration can be regulated by a temperature
parameter $\alpha$ which determines the relative importance of the entropy
term against the reward. For higher values of $\alpha$, the policy will be
explorative in nature, and for lower values of $\alpha$, it will be
exploitative in nature.

We build on the result reported in [11] to extend it to learn the prediction
horizon of multiple robots in the presence of static and dynamic obstacles
instead of a single robot avoiding static obstacles. Hence, the environment
consists of total $N$ robots with dynamics (2), completing their tasks by
avoiding obstacles using MPC with VODCA. The RL state vector
$s\in\mathbb{R}^{2N}$ consists of measured positions
$\boldsymbol{\bar{x}}_{i}[0|k]$, of all the robots. The policy function used
is a stochastic policy that inputs $s$ and outputs a mean ($\mu_{\theta}$) and
log-standard deviation ($\sigma_{\theta}$) of a Gaussian distribution. The
policy network outputs a pair of mean and log-standard deviation for each
robot. A normal distribution is then instantiated with the mean and standard
deviation, and a sample is drawn from it using the reparameterization trick.
Policy action is then obtained by transforming this sample using a hyperbolic
tangent function to ensure it falls within the range [-1,1], implemented as:

$\pi_{\theta,i}(s)=tanh(\mu_{\theta,i}+\sigma_{\theta,i}\odot\mathcal{N}(0,1)),$
(20)

where $\pi_{\theta,i}(s)$ is the policy output corresponding to $i^{th}$
robot. The Gaussian policy allows for smooth and differential mapping, which
is important for efficient gradient-based optimization during training. The
prediction horizon is a natural number. It was observed in the literature [14]
that in robotic applications, a value of prediction horizon greater than 50 is
rarely used; therefore, we choose an upper bound on the prediction horizon,
$N_{i}^{max}$ equal to 49. The value of the prediction horizon for each robot
can be estimated from $\pi_{\theta,i}(s)$ by first scaling the output from the
$tanh$’s limits of -1 and 1, to 1 and $N_{i}^{max}$, and then rounding up the
output to the next natural number:

$N_{i}^{k}=\text{ceiling}(\text{scale}(\pi_{\theta,i}(s),[-1,1],[1,N_{i}^{max}])),$
(21)

$N_{i}^{k}$ represents the prediction horizon of $i^{th}$ robot generated at
$k^{th}$ time-step. It is reported in [11] that the gradients of the RL
problem are not affected by these transformations as they are applied in the
environment, while the gradients are calculated based on the unscaled and
unceilinged outputs from $\pi_{\theta,i}(s)$.

The Reward function consists of five terms. The propagation reward $R_{p}$,
ensures the robots have maximum possible movement towards their goal positions
on each step. The computation reward $R_{h}$, enables the network to generate
the minimum prediction horizons. The collision reward $R_{c}$, penalizes
collision among the robots and with the obstacles. The termination reward
$R_{term}$, rewards every robot on reaching the vicinity of the goal within an
error threshold $\bar{e}$. The variance reward $R_{v}$, is the last term that
enforces the network to minimize variation among the prediction horizons of
the robots and thus provides the equal ability to all the robots to predict
the collisions. The proposed reward function is given below:

$R=\sum_{i=1}^{N}\Biggr{[}R_{p,i}-R_{h,i}-R_{c,i}+R_{term,i}\Biggr{]}-R_{v}$
(22)

The propagation reward $R_{p,i}$ is designed in such a way that it provides a
greater reward to a robot when it moves toward the goal and penalizes when it
moves away, i.e.,

$R_{p,i}=\begin{cases}25+250\Delta e_{i}&\text{If }\Delta e_{i}\geq 0\\\
250\Delta e_{i}&\text{If }\Delta e_{i}<0,\end{cases}$ (23)

where $\Delta e_{i}=e_{i}^{(k-1)}-e_{i}^{k}$, the change in error between two
consecutive time-steps.
$e_{i}^{k}=\left\|\boldsymbol{x}_{i}^{d}-\boldsymbol{\bar{x}}_{i}[k]\right\|$
is the error between the desired and current position of the $i^{th}$ robot at
$k^{th}$ time-step. A robot can be said to be moving towards its desired
position if $e_{i}^{(k-1)}\geq e_{i}^{k}$. Next, $R_{h,i}$ can be defined as

$R_{h,i}=\lambda_{h}N_{i}^{k},$ (24)

where $\lambda_{h}$ is a constant. $R_{c,i}$ is defined as

$R_{p,i}=\begin{cases}100+(K-k)&\text{If collides}\\\
0&\text{otherwise}.\end{cases}$ (25)

Here, we are considering that a robot has collided when it comes in the
vicinity of $0.5r_{min}$ with any other robot or obstacle. Further, $R_{v}$ is
given as

$R_{v}=Var(N_{i}^{k}).$ (26)

Finally, $R_{term,i}$ is defined as

$R_{term,i}=\begin{cases}0&\text{If }e_{i}^{k}\geq\bar{e}\\\ 150&\text{If
}e_{i}^{k}<\bar{e}.\end{cases}$ (27)

###  IV-A Implementation and Network Architecture

The framework learns the prediction horizon online with immediate rewards at
every time step. The critic network architecture consists of two fully
connected layers with ReLU activation functions. The first layer takes in the
concatenated state-action pair as input, and the output of the final layer is
a single scalar value representing the soft Q-value. The soft Q-value
formulation allows for a more robust training process by adding stochasticity
to the target values. There are 256 nodes in each layer of the critic network.
The actor architecture consists of two fully connected layers with ReLU
activation functions. The first layer takes in the state as input, and the
output of the final layer is split into two branches: one branch outputs the
mean of the policy distribution, and the other outputs the logarithm of the
standard deviation. The actor-network also has 256 nodes in each layer. We use
two soft Q-value networks, and two target Q-value networks and take their
minimum to estimate the soft Q-value better.

##  V RESULTS AND DISCUSSIONS

The proposed framework was implemented on various multi-robot tasks like set-
point control, position exchange, and segregation of multiple robots into the
desired number of groups. All the simulations were run on a Linux-based Ubuntu
20.01 having intel core $i7$-$8^{th}$ Gen with $8GB$ RAM. The value of various
parameters used in numerical studies are given in Table I, where
$\boldsymbol{I}$ is the identity matrix of appropriate dimensions.

TABLE I: Input parameters $h=0.2sec$ |  $\boldsymbol{Q}=5\boldsymbol{I}$ |  $\boldsymbol{W}=10\boldsymbol{I}$
---|---|---
$r_{min}=1.5m$ |  $ub=1.5m/s$ |  $lb=-1.5m/s$
$K=350$ |  $\lambda_{h}=0.001$ |  $\bar{e}=0.1$

For simplicity, we will use the abbreviation RL for the proposed RL based
variable horizon MPC and FH for fixed horizon MPC. First, we present set point
control of two robots to show the efficacy of the proposed framework. The
initial positions of Robot-1 and -2 are (12.5, 2.5) and (2.5, 12.5),
respectively, and the final positions are (12.5, 22.5) and (22.5, 12.5),
respectively, all in meters. Fig.1 represent the motion plots generated using
RL and FH, respectively.

Figure 1: Set point control of two robots. It must be noted that the robots
take a longer path in FH compared to RL.

(a) FH30

(b) RL

Figure 2: Control inputs of both the robots for set point control. $u_{y,1}$
and $u_{x,2}$ are identical in the highlighted portion, in case of FH30. This
results in more collision constraint activation and a longer path in this
configuration.

The RL takes a shorter path in comparison to the best-performing FH. This
behaviour can be better understood by looking at the control inputs of both
the robots in the case of RL and FH, shown in Fig. 2(b) and Fig. 2(a),
respectively. In Fig. 2(a), it can be observed that the values of $u_{y,1}$
and $u_{x,2}$ (shown as solid green and dashed blue lines, respectively) are
identical till time-step 40, which drives both the robots to their midpoint
(12.5, 12.5) and they end up being into continuous collision avoidance tussle,
which enforces them to take a longer path. In contrast, in RL (Fig. 2(b)), the
values of $u_{y,1}$ and $u_{x_{2}}$ are not identical. This happens because of
the variable prediction horizons at each time step. We want the robots to
maneuver in a way that activates the collision avoidance constraints as
minimally as possible, which will result in less computation as well as a
smoother path. Due to the variable horizon, RL generated different magnitudes
of control inputs for both the robots and activated the collision avoidance
constraints close to a minimum number of times.

(a) Number of collision constraints activation.

(b) Prediction horizon wrt time-step.

Figure 3: Number of collision constraint activation and prediction horizon
propagation for set-point control task. Here, the red colored bar depicts the
fixed horizon which was not able to complete the task.

The collision activations count for the different fixed horizons and RL is
shown in Fig. 3(a). Fig. 3(b), shows the propagation of prediction horizon
over time-steps, for both the robots. Next, a position exchange task was
performed using 14 robots in the presence of three static obstacles. In Fig.
4, seven robots started from the left side, and the other seven started from
the right side and were required to exchange their positions. We have chosen
path cost as the performance metric. Fig. 5(a) compares the path cost of RL
and various FHs. RL was best performing with an improvement of 7.07% over the
best performing fixed horizon, FH20. It must be noted that the path cost does
not have a linear relation with the value of the prediction horizon. This
clearly emphasizes the need for learning-based variable prediction horizon
MPC, as it would be difficult to find out an optimal fixed horizon. CPU
computation time and task completion time (= no. of iterations $\times h$) for
various FHs and RL is also shown in Fig. 5(b). The task completion time of RL
was comparable to best-performing FHs, but the computation time was higher in
RL, which can be reduced using a better computing device. This case proves
that the framework is scalable and can be used to perform different multi-
robot tasks with many robots with equally good or better performance than FH.
This shows the efficacy of the proposed framework.

It is worth reporting that there were few configurations in which only RL
could complete the task successfully. One such case is shown in Fig. 6, when
FH was used, robots got into local minima around a static obstacle or collided
with other robots. But RL was able to find the path because of the different
prediction horizons of every robot at each time step. It can be seen that the
proposed RL-based variable prediction MPC for multi-robot systems was able to
complete all the above-mentioned tasks with equally good or better performance
than fixed horizon MPC. Moreover, it has some clear advantages over fixed
horizon MPC like dealing with local minima and minimum collision constraints
activation. Successful completion of all the discussed cases proves that the
proposed variable horizon MPC with VODCA is scaleable to many robots.

Figure 4: Motion plot of 14 robots exchanging their position in dynamic
environment using RL based variable horizon MPC. The number in square brackets
is the prediction horizon of the respective robot at that time-instant.

(a) Path cost of different fixed horizon and RL.

(b) Computation time and task completion time.

Figure 5: Comparison of performance cost and computation cost for different
fixed prediction horizons and RL-based variable horizon. It can be noted here
that RL-based variable horizon MPC is the best performing with an improvement
of 7.07% over the best fixed horizon. Here, the red colored bar depicts the
fixed horizons which were not able to complete the task. Figure 6: Motion plot
of set-point control of four robots. Only RL based variable horizon MPC was
able to complete the manoeuvre while avoiding the obstacles.

##  VI CONCLUSIONS AND FUTURE WORKS

In this work, we proposed Versatile on-demand collision avoidance (VODCA) for
variable prediction horizon MPC. VODCA mitigates the drawback of on-demand
collision avoidance in the form of limited preview capability when the length
of prediction horizon, $N_{i}^{(k-1)}>N_{i}^{k}$. Further, we propose learning
variable prediction horizon for multi-robot systems in presence of dynamic and
static obstacles using the Soft Actor-Critic Algorithm. We introduced a novel
reward function which was able to drive robots towards their desired
positions, enforcing minimum prediction horizon and drastic change in its
value.

In order to show the efficacy of the proposed framework, three different
experiments are presented. First, a set point control of two robots was
carried out where RL-based variable horizon MPC generated a smaller path with
a minimum number of collision constraint activation. Next, a position exchange
of fourteen robots was carried out in presence of static and dynamic
obstacles. Here, for a given robot, other robots serve as dynamic obstacles.
The simulation results showed that the robots were successfully able to
exchange their positions and the proposed framework performs better than any
fixed horizon MPC. Finally, an example of four robot systems in a dynamic
environment was presented where fixed horizon MPC fails but the proposed RL-
based variable horizon MPC was able to find the solution. This shows the
superiority of the proposed framework. Future work will focus on real hardware
implementation and extending this framework for varied multi-robot tasks.

##  VII ACKNOWLEDGMENTS

The authors gratefully acknowledge the contribution of Sudhir Pratap Yadav for
insightful discussion on RL and Saurabh Chaudhary for helping in generating
results.

## References

  * [1] Dias MB, Zlot R, Kalra N, Stentz A. ”Market-based multirobot coordination: A survey and analysis.” Proceedings of the IEEE (2006):1257-70.
  * [2] Rizk Y, Awad M, Tunstel EW. ”Cooperative heterogeneous multi-robot systems: A survey.” ACM Computing Surveys (2019):1-31.
  * [3] Gautam A, Mohan S. ”A review of research in multi-robot systems.” IEEE 7th International Conference on Industrial and Information Systems (2012):1-5.
  * [4] Camacho EF, Alba CB. ”Model predictive control.” Springer Science and Business Media (2013).
  * [5] Zhu EL, Stürz YR, Rosolia U, Borrelli F. ”Trajectory optimization for nonlinear multi-agent systems using decentralized learning model predictive control.” IEEE Conference on Decision and Control (2020):6198-6203.
  * [6] Gonzalez R, Fiacchini M, Guzmán JL, Alamo T. ”Robust tube-based MPC for constrained mobile robots under slip conditions.” IEEE Conference on Decision and Control held jointly with Chinese Control Conference (2009):5985-5990.
  * [7] Scokaert PO, Mayne DQ. ”Min-max feedback model predictive control for constrained linear systems.” IEEE Transactions on Automatic control (1998):1136-42.
  * [8] Richards A, How JP. ”Robust variable horizon model predictive control for vehicle maneuvering.” International Journal of Robust and Nonlinear Control: IFAC‐Affiliated Journal (2006):333-51.
  * [9] Droge G, Egerstedt M. ”Adaptive time horizon optimization in model predictive control.” American Control Conference (2011):1843-1848
  * [10] Krener AJ. ”Adaptive horizon model predictive control.” IFAC-PapersOnLine (2018):31-6.
  * [11] Bøhn E, Gros S, Moe S, Johansen TA. ”Reinforcement learning of the prediction horizon in model predictive control.” IFAC-PapersOnLine (2021):314-20.
  * [12] Bøhn E, Gros S, Moe S, Johansen TA. ”Optimization of the Model Predictive Control Meta-Parameters Through Reinforcement Learning.” arXiv preprint arXiv:2111.04146 (2021).
  * [13] Luis, Carlos E., and Angela P. Schoellig. ”Trajectory generation for multiagent point-to-point transitions via distributed model predictive control.” IEEE Robotics and Automation Letters (2019): 375-382.
  * [14] Schwenzer M, Ay M, Bergs T, Abel D. ”Review on model predictive control: An engineering perspective.” The International Journal of Advanced Manufacturing Technology (2021):1327-49.
  * [15] Gupta S, Chaudhary S, Maurya D, Joshi SK, Tripathy NS, and Shah SV. ”Segregation of Multiple Robots Using Model Predictive Control With Asynchronous Path Smoothing.” IEEE Conference on Control Technology and Applications (2022): 1378-1383
  * [16] Haarnoja T, Zhou A, Abbeel P, Levine S. ”Soft actor-critic: Off-policy maximum entropy deep reinforcement learning with a stochastic actor.” International Conference on Machine Learning (2018): 1861-1870