A Dual Forward–Backward Algorithm to Solve Convex Model Predictive Control for Obstacle Avoidance in a Logistics Scenario

Abstract

In recent years, the logistics sector expanded significantly, leading to the birth of smart warehouses. In this context, a key role is represented by autonomous mobile robots, whose main challenge is to find collision-free paths in their working environment in real-time. Model Predictive Control Algorithms combined with global path planners, such as the A* algorithm, show great potential in providing efficient navigation for collision avoidance problems. This paper proposes a Dual Forward–Backward Algorithm to find the solution to a Model Predictive Control problem in which the task of driving a mobile robotic platform into a bi-dimensional semi-structured environment is formulated in a convex optimisation framework.

1. Introduction

In recent decades, the global economy has seen a massive development in the global value chain combined with the growth of “Industry 4.0” and “Internet plus” phenomena [1,2]. In spite of the impact of the COVID-19 pandemic, in an increasingly interconnected world, the flow of goods across different countries has become more and more frequent. This has led to a significant expansion of logistics markets [3]. With these developments, the need for greater efficiency and reliability in the transportation and storage process has become more and more intense. Against this background, the smart logistics industry, whose goal is the transition of the logistics industry to new technologies, has come into being. The smart warehouse is an automated warehouse based on several automated and interconnected technologies. It is characterised by high levels of automation, low labour costs, and high efficiency [4]. To develop a smart warehouse, a key role is represented by warehouse logistics robots. Indeed, in the process of rapid development of the logistics industry, smart sorting is one of the critical links to ensure that the logistics process can handle, in a timely manner, the massive material demand. Ultimately, the goal is to find a collision-free path between two points in the work environment. Implementing an optimal path planning algorithm would significantly reduce the operation time of logistics robots, while also reducing wear and energy consumption, and increasing the productivity and overall quality of the logistics industry. In addition, automation of such tasks would relieve human workers of repetitive and dull activities, which may cause harm to their health. Moreover, the COVID-19 pandemic makes the use of robots more attractive as they are not affected by health concerns or lock-down policies [5]. More generally, path planning of mobile robots in semi-structured environments, i.e., environments in which only the position of a subset of obstacles is known a priori, is a crucial topic in robotics, not just for the logistics sector [6]. The pursuit of increasingly optimised models and algorithms in terms of path length and smoothness, running time, and reliability is an active research field.

Path planning for mobile robots is mainly classified into two categories: global planning and local planning [7]. From all the known techniques, Model Predictive Control Algorithms (MPCA), in combination with global path planning strategies such as A* algorithm [8], show some of the greatest potential to provide efficient navigation in collision avoidance problems [9]. In this context, a common strategy is to model obstacles as multiple convex polytopes leading to a convex optimisation problem whose solution is guaranteed [10].

Several algorithms have been proposed to solve convex optimisation problems. Several early approaches were based on the Active-Set (AS) method [11], which initially estimates the optimal active set. This algorithm then repeatedly uses gradient and Lagrange multiplier information to eliminate one index from the current estimate of the active constraints while adding a new index until optimality is reached [12]. Later, algorithms based on the Interior-Point (IP) method, which involves modelling the constraints as barrier functions [13], became popular in solving convex optimisation problems

The key novelty of this work is the development and use of a Dual Forward–Backward Algorithm (DFBA) [14] to solve a Model Predictive Control problem. This DFBA can compute the optimal trajectory to avoid obstacles in a convex optimisation framework, and this is vital for obstacle avoidance in mobile robotic platforms operating in two-dimensional, semi-structured environments such as warehouses. Although the DFBA requires a longer computational time than the IP and AS algorithms, formulation of the optimisation problem associated with MPCA is relatively straightforward, and this is suited to real-time applications.

Section 2 presents the physical model of the system and the geometrical representation of the environment. This is then used to define the mathematical formulation in terms of an optimisation problem. Finally, there is a description of the algorithms needed to solve the proposed optimisation problem. A case study to test the proposed algorithms and the corresponding results are described in Section 3. A discussion of the results is presented in Section 4. Finally, the conclusion and future works are presented in Section 5.

2. Materials and Methods

This section presents a mathematical description of the problem introduced in Section 1, defines its equivalent optimisation problem, and describes the proposed numerical algorithms to solve it.

2.1. Problem Description

This subsection describes the system and environment models employed to solve the problem of autonomously driving a mobile robotic platform, avoiding obstacles in a two-dimensional, semi-structured environment.

2.1.1. Dynamic Model

As a first approximation, the mobile robot can be modelled as a point mass moving on a plane without considering gravity effects. The corresponding equation of motion is

$$\ddot{\mathit{p}}=\mathit{u},$$

where $\ddot{\mathit{p}}$ is the acceleration of the robot and $\mathit{u}$ is the actuator control signal. The improved Euler method [15] can be used to obtain the discrete time model of the system:

$$\left\{\begin{array}{c}{\mathit{v}}_{k+1}={\mathit{v}}_k+{\mathit{u}}_k{T}_s\hfill \\ {\mathit{p}}_{k+1}={\mathit{p}}_k+{\mathit{v}}_k{T}_s+\frac{{\mathit{u}}_k}{2}{T}_s^2\hfill \end{array}\right.,$$

(1)

where $T_s$ is the sample time, ${\mathit{u}}_k$ is the actuator control signal at a given time instant k, while ${\mathit{p}}_{k+1}$, ${\mathit{v}}_{k+1}$ and ${\mathit{p}}_k$, ${\mathit{v}}_k$ are the position and velocity at a given time instant $k+1$ and k, respectively. Equation (1) can be written in state-space form as follows:

$${\mathit{z}}_{k+1}=A{\mathit{z}}_k+B{\mathit{u}}_k,$$

(2)

where ${\mathit{z}}_k={\left[\begin{array}{cc}{\mathit{p}}_k& {\mathit{v}}_k\end{array}\right]}^T$ and ${\mathit{z}}_{k+1}={\left[\begin{array}{cc}{\mathit{p}}_{k+1}& {\mathit{v}}_{k+1}\end{array}\right]}^T$ are the states of the system at a given time instant k and $k+1$, respectively. Details about matrices A and B are presented in Appendix A. In the problem presented here, dynamic laws are considered as equality constraints. Indeed, (2) can be rearranged as follows:

$$-A{\mathit{z}}_k-B{\mathit{u}}_k+{\mathbf{I}}_4{\mathit{z}}_{k+1}={\mathbf{0}}_{4,1},$$

(3)

where ${\mathbf{I}}_n$ is the $n\times n$ identity matrix and ${\mathbf{0}}_{m,n}$ is the $m\times n$ zero matrix.

2.1.2. Actuation Model

A real actuator is not able to produce arbitrarily large accelerations and velocities. This saturation phenomenon can be modelled as the set, $\Delta$, defined by the intersection of linear inequalities:

$$\Delta =\left\{\begin{array}{c}\forall {\mathit{u}}_k\in {\mathbb{R}}^2,\forall {\mathit{v}}_k\in {\mathbb{R}}^2:\hfill \\ -u_{min}{𝟙}_{2,1}\le {\mathit{u}}_k\le u_{max}{𝟙}_{2,1}\hfill \\ -v_{min}{𝟙}_{2,1}\le {\mathit{v}}_k\le v_{max}{𝟙}_{2,1}\hfill \end{array}\right\},$$

(4)

where ${𝟙}_{m,n}$ is a $m\times n$ matrix all of whose entries are one, while $u_{max}$, $u_{min}$, $v_{max}$, and $v_{min}$ are real positive numbers representing the limits of the actuator.

2.1.3. Obstacle Model

All the obstacles are modelled as circular or rectangular subsets of ${\mathbb{R}}^2$, as reported in Figure 1. A generic circular obstacle $O_{c_i}$ is defined as follows:

$$O_{c_i}=\left\{\begin{array}{c}\forall \mathit{P}\in {\mathbb{R}}^2,{\mathit{P}}_{c_i}\in {\mathbb{R}}^2,{\rho }_i\in \mathbb{R}:\hfill \\ {∥\mathit{P}-{\mathit{P}}_{c_i}∥}^2<{\rho }_i^2\hfill \end{array}\right\},$$

where ${\mathit{P}}_{c_i}$ and ${\rho }_i$ are the centre and the radius of $O_{c_i}$, respectively. Similarly, a generic rectangular obstacle $O_{r_i}$ is defined as follows:

$$O_{r_i}=\left\{\begin{array}{c}\forall \mathit{P}\in {\mathbb{R}}^2,{\mathit{r}}_{i_j}\in {\mathbb{R}}^2,{\mathit{V}}_{i_j}\in {\mathbb{R}}^2:\hfill \\ ⟨{\mathit{r}}_{i_j},\mathit{P}-{\mathit{V}}_{i_j}⟩<0\mathrm{for}j=1,\dots ,4\hfill \end{array}\right\},$$

where ${\mathit{V}}_{i_j}$ is the $j^{th}$ vertex of $O_{r_i}$, ${\mathit{r}}_{i_j}$ is the outward-pointing normal vector to the $j^{th}$ side of $O_{r_i}$, and $⟨·,·⟩$ indicates the inner product. Then, the robot is constrained to move into a non-convex set:

$$\mathrm{\Pi }={\mathbb{R}}^2\backslash \left\{O_c\cup O_r\right\},$$

where

$$O_c=\left\{\bigcup _{i=1}^{n_c}{O}_{c_i}\right\},O_r=\left\{\bigcup _{i=1}^{n_r}{O}_{r_i}\right\},$$

and $n_c$ and $n_r$ are the number of circular and rectangular obstacles, respectively.

Figure 1. Obstacles model.

The maximum convex polytope ${\Omega }_t\subset \mathrm{\Pi }$ containing the centre of the robot ${\mathit{p}}_k$, as shown in Figure 2, is defined as

$${\Omega }_t=\left\{\begin{array}{c}\forall \mathit{P}\in {\mathbb{R}}^2,{\mathit{n}}_i\in {\mathbb{R}}^2,{\mathit{P}}_i\in {\mathbb{R}}^2:\hfill \\ -⟨{\mathit{n}}_i,\mathit{P}-{\mathit{P}}_i⟩\le 0\mathrm{for}i=1,\dots ,n_a\hfill \end{array}\right\},$$

(5)

where $n_a$ is the minimum number of lines necessary to define ${\Omega }_t$, while ${\mathit{n}}_i$ and ${\mathit{P}}_i$ have different meaning depending on the obstacle type. For circular obstacles, ${\mathit{n}}_i={\mathit{p}}_k-{\mathit{P}}_{c_i}$ and ${\mathit{P}}_i$ is the point on the boundary of the $i^{th}$ circular obstacle at minimum distance from ${\mathit{p}}_k$. For each rectangular obstacle, the eight sets ${\Theta }_{i_j}$ and ${\Xi }_{i_j}$ with $j=1,\cdots ,4$ are defined, as shown in Figure 1b. If ${\mathit{p}}_k\in {\Theta }_{i_j}$, ${\mathit{n}}_i={\mathit{r}}_{i_j}$ and ${\mathit{P}}_i={\mathit{V}}_{i_j}$, otherwise, if ${\mathit{p}}_k\in {\Xi }_{i_j}$, ${\mathit{n}}_i={\mathit{p}}_k-{\mathit{V}}_{i_j}$ and ${\mathit{P}}_i={\mathit{V}}_{i_j}$.

Figure 2. Example of computing the maximum convex polytope ${\Omega }_t$.

2.2. Optimisation Problem

The problem described in Section 2.1 can be formulated in a convex optimisation framework. The objective is to find the optimal control inputs to autonomously drive a mobile robot from an initial position to a target position within a semi-structured environment avoiding obstacles.

2.2.1. Objective Function

Considering $n_p$ predictions of the system states and control variables, it is possible to define the optimisation variables $\mathit{\xi }\in {\mathbb{R}}^{n_v}$, where $n_v=6n_p+4$, as

$$\mathit{\xi }={\left[\begin{array}{ccccccc}{\mathit{z}}_0^T& {\mathit{z}}_1^T& \dots & {\mathit{z}}_{n_p}^T& {\mathit{u}}_1^T& \dots & {\mathit{u}}_{n_p}^T\end{array}\right]}^T.$$

Then, introducing the vector containing the initial state, the $n_p$ desired states, and the $n_p$ desired control inputs (set to zero to reduce the control effort)

$${\mathit{\xi }}_d={\left[\begin{array}{ccccccc}{\mathit{z}}_{d_0}^T& {\mathit{z}}_{d_1}^T& \dots & {\mathit{z}}_{d_{n_p}}^T& {\mathbf{0}}_{1,2}& \dots & {\mathbf{0}}_{1,2}\end{array}\right]}^T,$$

the objective can be modelled as a quadratic function

$$f\left(\mathit{\xi }\right)=\frac{1}{2}{∥Q\left(\mathit{\xi }-{\mathit{\xi }}_d\right)∥}_2^2,$$

(6)

where the matrix $Q\in {\mathbb{R}}^{n_v\times n_v}$ is diagonal and allows weighting the cost function to define the priority between increasing the trajectory tracking accuracy in terms of position and velocity and reducing the control effort. The matrix Q has the following structure:

$$Q=diag\left(\left[\begin{array}{cccccc}{\mathit{w}}_z& \dots & {\mathit{w}}_z& {\mathit{w}}_u& \dots & {\mathit{w}}_u\end{array}\right]\right),$$

where the weights of the position error and the velocity error

$${\mathit{w}}_{\mathit{z}}=\left[\begin{array}{cccc}{w}_p& w_p& w_v& w_v\end{array}\right]$$

are repeated $n_p+1$ times, while the weight of the control effort

$${\mathit{w}}_u=\left[\begin{array}{cc}{w}_u& w_u\end{array}\right]$$

is repeated $n_p$ times. The parameters $w_p$, $w_v$, and $w_u$ are real positive numbers.

2.2.2. Equality Constraints

Equation (3) can be seen as an equality constraint which forces the states of the system at instant $k+1$ to satisfy the dynamic equations. Considering a prediction horizon of $n_p$ samples, it is possible to write (3) at each $T_s$ in terms of the optimisation variables $\mathit{\xi }$ as linear equality constraints

$$\Phi \mathit{\xi }-\mathit{b}={\mathbf{0}}_{4\left(n_p+1\right),1}.$$

(7)

Details about matrix $\Phi$ and vector $\mathit{b}$ are presented in Appendix A.

2.2.3. Inequality Constraints

As described in Section 2.1, both the limits of the actuators and the region in which the robotic platform can move are modelled as linear inequalities. Considering a prediction horizon of $n_p$ samples, it is possible to write (4) and (5) in terms of the optimisation variables and in compact form as

$$C\mathit{\xi }-\mathit{d}\le {\mathbf{0}}_{n_d,1},$$

(8)

where $n_d=n_p\left(n_a+8\right)+n_a+4$. Details about matrix C and vector $\mathit{d}$ are presented in Appendix B.

2.2.4. Dual Problem Formulation

Considering the objective function (6) and the constraints (7) and (8), it is possible to define the following optimisation problem:

$$\begin{array}{cccc}\hfill & \underset{\mathit{\xi }\in {\mathbb{R}}^{n_v}}{\min }\hfill & \hfill & \frac{1}{2}{∥Q\left(\mathit{\xi }-{\mathit{\xi }}_d\right)∥}_2^2\hfill \\ \hfill & \mathrm{subject}\mathrm{to}\hfill & \hfill & \Phi \mathit{\xi }-\mathit{b}={\mathbf{0}}_{4\left(n_p+1\right),1}\hfill \\ \hfill & \hfill & \hfill & C\mathit{\xi }-\mathit{d}\le {\mathbf{0}}_{n_d,1}\hfill \end{array}.$$

(9)

Since the objective function is quadratic and the constraints are linear, this optimisation problem is convex. Assuming that a solution to (9) exists, since the objective function is quadratic, it is also continuous at some $\mathit{\xi }$ such that constraints (7) and (8) are satisfied. Assuming $\mathit{x}\in {\mathbb{R}}^n$ and the subset $\Gamma \in {\mathbb{R}}^n$, where $n\in \mathbb{N}$, the indicator function of $\Gamma$ can be defined as

$${\iota }_{\Gamma }\left(\mathit{x}\right)=\left\{\begin{array}{cc}0& \mathit{x}\in \Gamma \\ +\infty & \mathit{x}\notin \Gamma \end{array}\right..$$

Then, introducing the linear operator

$$L={\left[\begin{array}{cc}{\Phi }^T& C^T\end{array}\right]}^T$$

and the vector

$$\mathit{\beta }={\left[\begin{array}{cc}{\mathit{b}}^T& {\mathit{d}}^T\end{array}\right]}^T,$$

the optimisation problem described in (9) can be equivalently rewritten as

$$\begin{array}{cccc}\hfill & \underset{\mathit{\xi }\in {\mathbb{R}}^{n_v}}{\min }\hfill & \hfill & \frac{1}{2}{∥Q\left(\mathit{\xi }-{\mathit{\xi }}_d\right)∥}_2^2+{\iota }_F\left(L\mathit{\xi }-\mathit{\beta }\right),\hfill \end{array}$$

(10)

where F is a set defined as follows:

$$F=\left\{\begin{array}{c}\mathit{\lambda }\in {\mathbb{R}}^{4\left(n_p+1\right)+n_d}:\hfill \\ {\lambda }_i=0\mathrm{for}i=1,\dots ,4\left(n_p+1\right),\hfill \\ {\lambda }_i\le 0\mathrm{for}i=4\left(n_p+1\right)+1,\dots ,4\left(n_p+1\right)+n_d\hfill \end{array}\right\}.$$

Defining

$$g\left(\mathit{\xi }\right)={\iota }_F\left(\mathit{\xi }-\mathit{\beta }\right),$$

it is possible to write (10) as

$$\underset{\mathit{\xi }\in {\mathbb{R}}^{n_v}}{\min }f\left(\mathit{\xi }\right)+g\left(L\mathit{\xi }\right)$$

(11)

The convex conjugate [16] of (11) has the following form:

$$\begin{array}{cccc}& \underset{\mathit{\lambda }\in {\mathbb{R}}^{4\left(n_p+1\right)+n_d}}{\min }\hfill & & f^{*}\left(-L^T\mathit{\lambda }\right)+g^{*}\left(\mathit{\lambda }\right)+⟨\mathit{\lambda },\mathit{\beta }⟩,\hfill \end{array}$$

where $f^{*}\left(\mathit{\zeta }\right)$, $\mathit{\zeta }\in {\mathbb{R}}^{n_v}$, is the Legendre-Fenchel conjugate of $f\left(\mathit{\xi }\right)$ and is equal to

$$f^{*}\left(\mathit{\zeta }\right)=\frac{1}{2}{∥Q^{-1}\mathit{\zeta }∥}_2^2+⟨{\mathit{\xi }}_d,\mathit{\zeta }⟩,$$

while $g^{*}\left(\mathit{\lambda }\right)$ is the Legendre-Fenchel conjugate of $g\left(\mathit{\xi }\right)$ and is equal to

$$g^{*}\left(\mathit{\lambda }\right)={\iota }_{F^{\circ }}\left(\mathit{\lambda }\right),$$

where

$$F^{\circ }=\left\{\mathit{\lambda }\in {\mathbb{R}}^{4\left(n_p+1\right)+n_d}:{\lambda }_i\ge 0\mathrm{for}i=4\left(n_p+1\right)+1,\dots ,4\left(n_p+1\right)+n_d\right\}.$$

2.3. Algorithms

This section presents the algorithms needed to solve the optimisation problem described in Section 2.2. First, the MPCA is explained in detail. Then, the Convex Subset Search Algorithm (CSSA), which is employed to compute the maximum convex subset in which the mobile robot can move avoiding the obstacles, is presented. Finally, the DFBA used to solve (9) is described.

2.3.1. Model Predictive Control Algorithm

Exploiting the current and desired states of the system and the obstacles map, the MPCA computes the optimal sequence of inputs to obtain the $n_p$ predicted states that minimise the objective function. Then, only the first input is applied and all the variables of the optimisation problem (the obstacles map, the constraints matrices C and vectors $\mathit{d}$, and $\mathit{b}$) are updated according to the newly measured position. Moreover, to reduce the number of iterations required by the DFBA, the initial point of the optimisation solver, ${\mathit{\xi }}_i$, is updated using the predictions of the states of the system obtained as the results of the optimisation problem at the previous time instant. The algorithm is repeated every sample time $T_s$. For this application, to define the desired states, the trajectory that connects the initial position to the target one is computed using the A* algorithm. The pseudocode of the MPCA is described in Algorithm 1.

Algorithm 1 MPCA

1: $k=0$ 2: ${\mathit{p}}_k\leftarrow \mathit{\text{initial position}}$ 3: ${\mathit{p}}_f\leftarrow \mathit{\text{target position}}$ 4: ${\mathit{p}}_t\leftarrow \mathit{\text{compute trajectory via A}}*\mathit{algorithm}$ 5: $map\leftarrow \mathit{\text{obstacles map}}$ 6: ${\Omega }_t\leftarrow \mathit{CSSA}\left({\mathit{p}}_k,map\right)$ 7: $u_{max},u_{min}\leftarrow \mathit{\text{acceleration actuators limits}}$ 8: $v_{max},v_{min}\leftarrow \mathit{\text{velocity actuators limits}}$ 9: $C,\mathit{d}\leftarrow$ compute inequality constraint parameters 10: $n_p\leftarrow \mathit{\text{prediction horizon}}$ 11: $T_s\leftarrow \mathit{\text{sample time}}$ 12: $\Phi ,\mathit{b}\leftarrow$ compute equality constraint parameters 13: $Q\leftarrow \mathit{\text{define weight matrix}}$ 14: $\mathfrak{Q}={\left(Q^{T}Q\right)}^{-1}$ 15: ${\mathit{\xi }}_i={\mathbf{0}}_{6n_p+4,1}\leftarrow \mathit{\text{initial point of DFBA}}$ 16: $\delta \leftarrow \mathit{\text{position tolerance}}$ 17: while ${∥{\mathit{p}}_f-{\mathit{p}}_k∥}_2<\delta$ do 18:    ${\mathit{\xi }}_d\leftarrow \mathit{\text{update with}} {\mathit{p}}_t$ 19:    ${\mathit{\xi }}_k\leftarrow \mathit{DFBA}\left({\mathit{\xi }}_i,{\mathit{\xi }}_d,\mathfrak{Q},\Phi ,\mathit{b},C,\mathit{d}\right)$ 20:    ${\mathit{u}}_k\leftarrow \mathit{apply}{\mathit{u}}_0\mathit{invector}{\mathit{\xi }}_k$ 21:    ${\mathit{p}}_k\leftarrow \mathit{\text{update current position with measurement}}$ 22:    $map\leftarrow \mathit{\text{update obstacles map}}$ 23:    ${\Omega }_t\leftarrow \mathit{CSSA}\left({\mathit{p}}_k,map\right)$ 24:    $C,\mathit{d}\leftarrow \mathit{\text{update inequality constraints}}$ 25:    $\mathit{b}\leftarrow \mathit{\text{update dynamic constraints with}} {\mathit{p}}_k$ 26:    ${\mathit{\xi }}_i\leftarrow \mathit{\text{update initial point of DFBA}}$ 27:    $k=k+1$ 28: end while

2.3.2. Convex Subset Search Algorithm

The CSSA takes as its input the current robot position and the updated map describing the obstacles and returns the minimum number of vectors, $\left\{{\mathit{n}}_1,\dots ,{\mathit{n}}_{n_a}\right\}$ and $\left\{{\mathit{P}}_1,\dots ,{\mathit{P}}_{n_a}\right\}$, to compute the inequality constraints required to define ${\Omega }_t$. The pseudocode of the CSSA is described in Algorithm 2.

2.3.3. Dual Forward–Backward Algorithm

Since f is strongly convex with a modulus of convexity $\mu >0$, $f^{*}$ is differentiable on $F^o$, and $\nabla f^{*}$ is $1/\mu$-Lipschitz continuous, the primal problem has a unique solution $\widehat{\mathit{\xi }}$. Furthermore, assuming that the calculus rule for subdifferentials holds, then the dual solution $\widehat{\mathit{\lambda }}$ also exists, the duality gap is zero, and the following Karush–Kuhn–Tucker conditions [13] hold:

$$\widehat{\mathit{\xi }}=\nabla f^{*}\left(-L^{*}\widehat{\mathit{\lambda }}\right)\mathrm{and}L\widehat{\mathit{\xi }}\in \partial g^{*}\left(\widehat{\mathit{\lambda }}\right).$$

Algorithm 2 CSSA$\left({\mathit{p}}_k,map\right)$

1: $h=0$ 2: while $isEmpty\left(map\right)$ do 3:    $O_i\leftarrow \mathit{\text{obstacle at minimum distance from}} {\mathit{p}}_k$ 4:    $i\leftarrow \mathit{\text{index of}} O_i$ 5:    if $isRectangle\left(O_i\right)$ then 6:      if ${\mathit{p}}_k\in {\Theta }_{i_j}$ then 7:         ${\mathit{n}}_h={\mathit{r}}_{i_j}$ 8:      else 9:         ${\mathit{n}}_h={\mathit{p}}_k-{\mathit{V}}_{i_j}$ 10:      end if 11:      ${\mathit{P}}_h={\mathit{V}}_{i_j}$ 12:    else 13:      ${\mathit{n}}_h={\mathit{p}}_k-{\mathit{P}}_{c_i}$ 14:      ${\mathit{P}}_h={\mathit{P}}_{c_i}+{\rho }_i{\mathit{n}}_h/{∥{\mathit{n}}_h∥}_2$ 15:    end if 16:    $t=0$ 17:    $map\leftarrow \mathit{\text{update by removing}} O_i$ 18:    for $q=0$ to $length\left(map\right)-1$ do 19:      if $isDisk\left(O_q\right)$ then 20:         ${\mathit{P}}_{md}={\mathit{P}}_{c_q}+{\rho }_q{\mathit{n}}_h/{∥{\mathit{n}}_h∥}_2$ 21:         if $⟨{\mathit{n}}_h,{\mathit{P}}_{md}-{\mathit{n}}_h⟩<0$ then 22:           $R_t=O_q$ 23:           $t=t+1$ 24:         end if 25:      else 26:         $bool=true$ 27:         for $m=1$ to 4 do 28:           $bool=bool\wedge \left(⟨{\mathit{n}}_h,{\mathit{V}}_{q_m}-{\mathit{P}}_h⟩<0\right)$ 29:         end for 30:         if $bool$ then 31:           $R_t=O_q$ 32:           $t=t+1$ 33:         end if 34:      end if 35:    end for 36:    $map\leftarrow \mathit{\text{update by removing}} \mathit{R}$ 37:    $h=h+1$ 38: end while

Thus, the dual solution uniquely determines the primal solution. Under these conditions, it is possible to solve (9) using the following DFBA:

$$\begin{array}{cc}\hfill & {\mathit{\xi }}_h=\nabla f^{*}(-L^{*}{\mathit{\lambda }}_h),\hfill \\ \hfill & {\mathit{\lambda }}_{h+1}={prox}_{\gamma g^{*}}\left({\mathit{\lambda }}_h+\gamma L{\mathit{\xi }}_h\right),\hfill \\ \hfill & {\mathit{\lambda }}_{h+1}={prox}_{F^o}\left({\mathit{\lambda }}_{h+1}\right),\hfill \end{array}$$

where

$$\begin{array}{cc}\hfill & \nabla f^{*}(-L^{*}{\mathit{\lambda }}_h)=-{\left(Q^{T}Q\right)}^{-1}\left(L^T{\mathit{\lambda }}_h\right)+{\mathit{\xi }}_d,\hfill \\ \hfill & {prox}_{\gamma g^{*}}({\mathit{\lambda }}_h+\gamma L{\mathit{\xi }}_h)={\mathit{\lambda }}_h+\gamma \left(L\mathit{\xi }-\mathit{\beta }\right),\hfill \\ \hfill & {prox}_{F^o}\left({\mathit{\lambda }}_h\right)=\left\{\begin{array}{cc}{\lambda }_{h,i}\hfill & {\lambda }_{h,i}\ge 0\vee i\le 4\left(n_p+1\right)\hfill \\ 0\hfill & {\lambda }_{h,i}<0\wedge i>4\left(n_p+1\right)\hfill \end{array}\right.,\hfill \end{array}$$

${\lambda }_{h,i}$ is the ith component of the optimisation variable of the dual problem at the hth iteration, and $0<\gamma <{\gamma }_m$ with ${\gamma }_m=2\mu /{∥L∥}^2$, where the modulus of convexity $\mu$ is computed as the minimum eigenvalue of $Q^{T}Q$. The pseudocode of the DFBA is described in Algorithm 3.

Algorithm 3 DFBA$\left({\mathit{\xi }}_i,{\mathit{\xi }}_d,\mathfrak{Q},\Phi ,\mathit{b},C,\mathit{d}\right)$

1: $\mathit{L}\leftarrow$ generated by Φ and C 2: $\mathit{\beta }\leftarrow$ generated by $\mathit{b}$ and $\mathit{d}$ 3: ${\mathit{\lambda }}_0={\mathbf{0}}_{4\left(n_p+1\right)+n_d,1}$ 4: ${\mathit{\xi }}_0={\mathit{\xi }}_i$ 5: $\gamma \leftarrow$ initialise considering L 6: $tol\leftarrow \mathit{DFBA}\mathit{termination}\mathit{tolerance}$ 7: $maxIter\leftarrow \mathit{\text{DFBA maximum number of iterations}}$ 8: for $h=0$ to $maxIter-1$ do 9:    ${\mathit{\lambda }}_{h+1}={\mathit{\lambda }}_h+\gamma (L{\mathit{\xi }}_h-\mathit{\beta })$ 10:    for $i=4(n_p+1)+1$ to $length\left({\mathit{\lambda }}_{h+1}\right)$ do 11:      if ${\lambda }_{h+1,i}\le 0$ then 12:         ${\lambda }_{h+1,i}=0$ 13:      end if 14:    end for 15:    ${\mathit{\xi }}_{h+1}=\mathfrak{Q}\left(-{\mathit{L}}^T{\lambda }_{h+1}\right)+{\mathit{\xi }}_d$ 16:    if ${∥{\mathit{\xi }}_{h+1}-{\mathit{\xi }}_h∥}_2/length\left({\mathit{\xi }}_h\right)<tol$ then 17:      stop iteration 18:    end if 19: end for

3. Results

A case study was developed to test the algorithms described in Section 2.3. Consider a $48\mathrm{m}\times 36\mathrm{m}$ warehouse that contains eight shelves and three mobile robots ($R_{m_1}$, $R_{m_2}$, and $R_{m_3}$). The task for each robot is to reach different target positions while avoiding the walls, the shelves, and the other robots. As described in Section 2.1, the walls and the shelves are modelled as rectangular obstacles, whose parameters are reported in Table 1, while the mobile robots are modelled as circular obstacles with a diameter equal to 1 m.

Table 1. Rectangular Obstacles Parameters.

	${\mathit{V}}_{\mathit{i},1}$		${\mathit{V}}_{\mathit{i},2}$		${\mathit{V}}_{\mathit{i},3}$		${\mathit{V}}_{\mathit{i},4}$		Unit
	$\mathit{x}$	$\mathit{y}$	$\mathit{x}$	$\mathit{y}$	$\mathit{x}$	$\mathit{y}$	$\mathit{x}$	$\mathit{y}$
$O_1$	6	32	22	32	22	30	6	30	m
$O_2$	28	32	44	32	44	30	28	30	m
$O_3$	6	24	22	24	22	22	6	22	m
$O_4$	28	24	44	24	44	22	28	22	m
$O_5$	6	16	22	16	22	14	6	14	m
$O_6$	28	16	44	16	44	14	28	14	m
$O_7$	6	8	22	8	22	6	6	6	m
$O_8$	28	8	44	8	44	6	28	6	m
$O_9$	0	38	1	38	1	0	0	0	m
$O_{10}$	49	38	50	38	50	0	49	0	m
$O_{11}$	1	38	49	38	49	37	1	37	m
$O_{12}$	1	1	49	1	49	0	1	0	m

Since all the constraints of the optimisation problem are computed considering each mobile robot to be a point mass, all the obstacles are enlarged by the radius of the mobile robot. Given this environment, the following logistic task is simulated: (i) $R_{m_1}$ has to move an object from point $\mathit{X}$ to point $\mathit{Y}$, then returns to its home position ${\mathit{H}}_1$; (ii) $R_{m_2}$ has to move an object from point $\mathit{Y}$ to point $\mathit{X}$, then returns to its home position ${\mathit{H}}_2$; (iii) $R_{m_3}$ has to store an object at point $\mathit{Z}$, then returns to the home position ${\mathit{H}}_3$. The x and y coordinates of the above-mentioned target positions are listed in Table 2.

Table 2. Target Positions.

	${\mathit{H}}_1$	${\mathit{H}}_2$	${\mathit{H}}_3$	X	Y	Z	Unit
x	3	5	7	14	32	40	m
y	36	36	36	10	20	10	m

Figure 3 shows the simulation environment where the following elements are associated with each robot: (i) a disk representing the robot itself; (ii) a polytope representing the allowed area in which the robot can move; (iii) the path travelled by the robot; (iv) the current target position labelled with the × symbol; (v) the home position represented as a dot. The elements associated with $R_{m_1}$, $R_{m_2}$, and $R_{m_3}$ are depicted in blue, red, and yellow, respectively.

Figure 3. A frame of the simulation of the logistic task showing the mobile robots with their allowed area, their travelled path, and their current target.

A simulation, whose parameters are reported in Table 3, was implemented in Matlab^® 2019b using the Parallel Computing Toolbox^™ to measure the performances of the proposed algorithm.

Table 3. Simulation Parameters.

Algorithm	Parameter	Value	Unit
MPCA	$\delta$	$0.1$	m
	$w_p$	5	1/m
	$w_v$	3	s/m
	$w_u$	1	s${}^2$/m
	$u_{max}$	5	m/s${}^2$
	$u_{min}$	5	m/s${}^2$
	$v_{max}$	$1.5$	m/s
	$v_{min}$	$1.5$	m/s
	$T_s$	$0.1$	s
	$n_p$	10
DFBA	$maxIter$	$5\times {10}^4$
	$tol$	$1\times {10}^{-6}$
	$\mu$	1
	$\gamma$	$0.99{\gamma }_m$

The weights were chosen to promote the trajectory tracking performances rather than reducing the control effort. Figure 4 shows how the proposed algorithm avoids a crash between $R_{m_1}$ and $R_{m_2}$. In Figure 4a, since the desired trajectory of each robot lies within the allowed area, the mutual obstacle constraints are inactive. In Figure 4b, the predicted positions deviate from the desired paths to satisfy the obstacle constraints. In Figure 4c, the two robots react to avoid a crash, leading to an increase in the trajectory tracking error. In Figure 4d, the two robots start to recover their desired paths.

Figure 4. Detail of how the proposed algorithm avoids a crash between $R_{m_1}$ and $R_{m_2}$. For each robot, the predicted positions (grey dots), the desired trajectory (coloured dots), the travelled path (coloured line), and the allowed area (coloured polytope) are reported. $R_{m_1}$ and $R_{m_2}$ are shown in blue and red, respectively. (a) The desired trajectory of each robot lies within the allowed area, hence the obstacles constraints are inactive; (b) The predicted positions deviate from the desired paths to satisfy the obstacle constraints; (c) The two robots react to avoid a crash; (d) The two robots start to recover their desired paths.

The results of the simulation show that all the constraints of the optimisation problem are satisfied and the trajectory tracking error is bounded, except when the constraints are active. Figure 5 summarises the simulation results. In particular, Figure 5a shows the trend over time of the norm of the trajectory tracking error, Figure 5b represents the maximum value of the constraint of the nearest obstacle, while the velocity and control effort of each mobile robot along the x-axis and y-axis are depicted in Figure 5c–e, respectively.

Figure 5. Simulation results. The shaded areas correspond to the avoided collision event described in Figure 4.

4. Discussion

The performance of the proposed DFBA is evaluated by comparing its results with those obtained using IP and AS algorithms to solving the optimisation problem presented in the case study in Section 3. Both these algorithms are implemented in Matlab^® 2019b using the standard functions of the Optimization Toolbox^™, setting the maximum number of iterations and the termination tolerance equal to that of the DFBA, as reported in Table 3. The results of the simulations are presented in Table 4, where:

• $\widehat{e}$ is the mean of the norm of the trajectory tracking error;
• $\sigma \left(e\right)$ is the sample standard deviation of the norm of the trajectory tracking error;
• $max\left(d_{min}\right)$ is the maximum value of the constraint of the nearest obstacle;
• $max\left|v_x\right|$ is the maximum of the velocity of each mobile robot along the x-axis;
• $max\left|v_y\right|$ is the maximum of the velocity of each mobile robot along the y-axis;
• $max\left|u_x\right|$ is the maximum of control effort of each mobile robot along the x-axis.
• $max\left|u_y\right|$ is the maximum of control effort of each mobile robot along the y-axis;
• ${\widehat{t}}_c$ is the mean of the computation time required by the algorithm.

Table 4. Comparison of the results obtained using different optimisation algorithms.

Parameter	${\mathit{R}}_{{\mathit{m}}_{\mathbf{1}}}$			${\mathit{R}}_{{\mathit{m}}_{\mathbf{2}}}$			${\mathit{R}}_{{\mathit{m}}_{\mathbf{3}}}$			Unit
	DFBA	IP	AS	DFBA	IP	AS	DFBA	IP	AS
$\widehat{e}$	$0.12$	$0.12$	$0.12$	$0.12$	$0.11$	$0.11$	$0.12$	$0.11$	$0.11$	m
$\sigma \left(e\right)$	$0.04$	$0.03$	$0.03$	$0.03$	$0.03$	$0.03$	$0.04$	$0.04$	$0.04$	m
$max\left(d_{min}\right)$	$-0.023$	$-0.052$	$-0.035$	$-0.011$	$-0.048$	$-0.031$	$-0.547$	$-0.545$	$-0.545$	m
$max\left|v_x\right|$	$1.50$	$1.50$	$1.50$	$1.50$	$1.50$	$1.50$	$1.43$	$1.41$	$1.41$	m/s
$max\left|v_y\right|$	$1.43$	$1.40$	$1.40$	$1.43$	$1.40$	$1.40$	$1.36$	$1.36$	$1.36$	m/s${}^2$
$max\left|u_x\right|$	$3.91$	$5.00$	$3.81$	$3.53$	$4.77$	$3.53$	$1.69$	$1.46$	$1.46$	m/s
$max\left|u_y\right|$	$2.00$	$1.86$	$2.04$	$1.65$	$1.45$	$3.45$	$1.10$	$1.00$	$1.00$	m/s${}^2$
${\widehat{t}}_c$	$0.030$	$0.022$	$0.025$	$0.029$	$0.021$	$0.024$	$0.027$	$0.022$	$0.022$	s

Table 4. Comparison of the results obtained using different optimisation algorithms.

Parameter	${\mathit{R}}_{{\mathit{m}}_{\mathbf{1}}}$			${\mathit{R}}_{{\mathit{m}}_{\mathbf{2}}}$			${\mathit{R}}_{{\mathit{m}}_{\mathbf{3}}}$			Unit
	DFBA	IP	AS	DFBA	IP	AS	DFBA	IP	AS
$\widehat{e}$	$0.12$	$0.12$	$0.12$	$0.12$	$0.11$	$0.11$	$0.12$	$0.11$	$0.11$	m
$\sigma \left(e\right)$	$0.04$	$0.03$	$0.03$	$0.03$	$0.03$	$0.03$	$0.04$	$0.04$	$0.04$	m
$max\left(d_{min}\right)$	$-0.023$	$-0.052$	$-0.035$	$-0.011$	$-0.048$	$-0.031$	$-0.547$	$-0.545$	$-0.545$	m
$max\left|v_x\right|$	$1.50$	$1.50$	$1.50$	$1.50$	$1.50$	$1.50$	$1.43$	$1.41$	$1.41$	m/s
$max\left|v_y\right|$	$1.43$	$1.40$	$1.40$	$1.43$	$1.40$	$1.40$	$1.36$	$1.36$	$1.36$	m/s${}^2$
$max\left|u_x\right|$	$3.91$	$5.00$	$3.81$	$3.53$	$4.77$	$3.53$	$1.69$	$1.46$	$1.46$	m/s
$max\left|u_y\right|$	$2.00$	$1.86$	$2.04$	$1.65$	$1.45$	$3.45$	$1.10$	$1.00$	$1.00$	m/s${}^2$
${\widehat{t}}_c$	$0.030$	$0.022$	$0.025$	$0.029$	$0.021$	$0.024$	$0.027$	$0.022$	$0.022$	s

All the tested algorithms are able to solve the optimisation problem resulting from driving a mobile robotic platform into a two-dimensional semi-structured environment avoiding obstacles autonomously. They have comparable accuracy regarding the trajectory tracking error and the capability of satisfying both the obstacles’ constraints and the limits of the actuators. Concerning the computation time required to solve the optimisation problem, the IP and AS algorithms are, on average, $24.4\%$ and $17.4\%$ faster than the DFBA. However, unlike IP and AS algorithms, the DFBA can solve the Model Predictive Control problem using only matrix multiplications and operations, such as conditional statements and for loops. This feature is advantageous because it allows an efficient implementation of the MPCA in common micro-controllers.

5. Conclusions

In this paper, to address the problem of driving a mobile robotic platform in a two-dimensional semi-structured environment while autonomously avoiding obstacles, a Dual Forward–Backward Algorithm is proposed that can solve a Model Predictive Control Algorithm within a convex optimisation framework. First, the mathematical details about the problem formulation are described in terms of dynamic equations, system modelling, and algorithms. Then, a case study from the logistics sector, in which three mobile robots have to reach different target positions avoiding walls, shelves, and the other mobile robots within a warehouse environment, is simulated using the proposed optimisation method. Finally, for comparison, the case study is simulated using the Interior-Point algorithm and Active-Set algorithm to evaluate the performances of the proposed Dual Forward–Backward Algorithm.

The results of the simulations show that, for all the implemented algorithms, the constraints given by obstacles and actuators limits are satisfied; furthermore, the trajectory tracking error is bounded and approximately constant for all the simulated mobile robots except when the constraints are active, since the robots need to deviate from the desired path to avoid obstacles or satisfy the limits of the actuators. Although the Dual Forward–Backward Algorithm has a longer computational time than the Interior-Point and Active-Set algorithms, it can solve the Model Predictive Control problem using only matrix multiplications and operations that can be easily implemented in common micro-controllers: this is advantageous since it reduces hardware complexity, development costs, and implementation time. In addition, it is possible to increase the computational efficiency by exploiting the sparsity of the matrices. Finally, by setting the maximum number of iterations of the Dual Forward–Backward Algorithm, it is possible to define the maximum computation time required to satisfy real-time applications.

Future work will focus on implementing and testing the proposed algorithm on real mobile robots in a semi-structured environment. Further developments will exploit predictions of the future positions of each mobile robot, computed by the Model Predictive Control Algorithm, to estimate how the constraints will change in the prediction horizon. This should have the potential to significantly enhance the capability of each robot to avoid collisions, and at the same time optimise the path through earlier planning of course corrections.