2.1 Kinematics of MWOMR

The dynamics of MWOMR can be described by

where the states $x=\left[p_{x}, p_{y,}, \theta\right]^{T} \in \mathbb{R}^{n}$ consist of the position of the mobile robot in cartesian coordinate $[p_{x,\:}p_{y}]^{T}$ (m,m) and the orientation angle $\theta$ (rad), and the inputs $u=\left[v_{x}, v_{y}, \omega\right]^{T} \in \mathbb{R}^{m}$ are the linear speed $v_{x}$ (m/s), the lateral speed $v_{y}$ (m/s), and the angular speed $\omega$ (rad/s) of the robot. Rotation matrix $R(\theta_{t})$ in eq(1) is denoted as

The configuration of all wheels in MWOMR is essential as it determines the movement of MWOMR. Fig 1 illustrates the proper orientation of each wheel. In MWOMR, a motor drives each wheel separately so it can move in any direction freely. The relation of the motor’s angular speed and the input vector $u$ is given by

where

$H =\begin{pmatrix}1&& -1&& -a-b \\ 1&&1 &&a+b \\ 1&&-1 &&a+b \\ 1&& 1&&-a-b\end{pmatrix}.$ $a$ (m) and $b$ (m) are the half of robot’s wheel base and half of robot’s wheel track, respectively. $w_{m} \in \mathbb{R}^{4}$ (rad/s) is a vector which describes the angular speed of each motor matching the setup in Fig 1 and $r$ (m) is the wheels’ radius. The full derivation of the mecanum-wheeled omnidirectional mobile robot is presented in ⁽⁸⁾.

From the relation of each motors’ speed and robot’s speed in eq(3), the robot velocity constraint is constructed as introduced in ⁽⁵⁾,

where

$\|H u\|_{\infty}:=\left|v_{x}\right|+\left|v_{y}\right|+(a+b)|\omega| .$

This paper assumes that the states are bounded by a compact set

2.2 Path-Following Problem

There are particularly three types of control problems in order to steer the system to the desired point: setpoint stabilization, trajectory tracking, and path-following. The setpoint stabilization is a problem to drive the system states to a constant reference, whereas trajectory tracking and path-following steer the states to follow varying references.

The objective of the trajectory tracking is to follow a certain reference in a specified time. This may limit the accuracy because it does not incorporate the hardware limit that is already stated in eq(4) and eq(5). In contrast, the goal of path-following is to reach and follow a geometric path that does not depend on the time variable. Path-following does not require the system to be at one point in a specific time.

In the path-following, the references to be tracked is denoted as path $P$ which can be defined as a set of points $r$ parameterized by a dimensionless $s$

where the $s$ is called a path parameter. The map $p: \mathbb{R} \mapsto \mathbb{R}^{n}$ is assumed to be continuously differentiable. Note that the time evolution of $s(t)\in[s_{0},\:s_{1}]$ is not known a priori, but it is another degree of freedom to choose in this problem. Here, a virtual input $\phi_{t}$ is chosen to parameterize the evolution of $s_{t}$ as

In the path-following, the controller is designed to achieve these conditions ⁽¹⁾:

i) Path convergence

$\lim _{t \rightarrow \infty}\left\|x_{t}-p\left(s_{t}\right)\right\|=0$

ii) Forward motion

$\dot{s}_{t} \geq 0$ and $\lim _{t \rightarrow \infty}\left\|s_{t}-s_{1}\right\|=0$

iii) Constraint satisfaction

$x_{t} \in X, u_{t} \in U \quad \forall t \in\left[t_{0}, \infty\right)$

Condition i) ensures that the system moves along the desired path. Condition ii) makes sure that the robot is moving forward along the path. The last condition guarantees that the system follows the hardware limitations.

3.1 MPC Formulation for Path-Following

In order to calculate the optimal input for path-following problem, the following cost function is considered

where $P,\: Q,\:$ and $R$ are positive definite and $(·)_{k+j | k}$ means the predicted value at time step $k+j$ based on the value measured at time step $k$. The first term of eq(9) ensures that the system tracks the specified path, and the third term forces the robot to move forward along the specified path.

Suppose that $bold{u}=\left\{u_{k|k},\: u_{k+1|k},\:\ldots ,\: u_{k+N-1|k}\right\}$ and $bold\Phi =\left\{\phi_{k|k},\:\phi_{k+1|k,\:}\ldots ,\:\phi_{k+N-1|k}\right\}$ denote the sequences of all predicted inputs and virtual inputs, respectively. Assume that $x_{0}=\left[p_{x},\: p_{y},\:\theta\right]^{T}$ and $s_{0}$ are given, the MPC control law is obtained by solving the following optimization problem

subject to

The first element of sequence $bold{u}$, $u_{k|k}$, is applied to the robot and $s_{k+1|k}$ becomes the next initial value $s_{0}$ of problem eq(10).

In the case where no initial condition for parameter $s$ is given when $t=0$, $s_{t_{0}}$ can be obtained by solving the following optimization problem:

3.2 MPC Implementation in ROS

Since MPC needs the state of the robot to solve the optimization problem, knowing the robot position and orientation angle becomes a critical issue. To measure the state of the robot, this work uses three kinds of sensor to get the robot position and rotation angle: encoder, IMU, and lidar. The data from each sensor is fused using EKF localization inside a robot localization package in ROS ⁽⁹⁾. The results of the localization are the robot position and orientation angle and can be accessed through ROS localization nodes.

After collecting the required information for the optimization problem, the MPC problem can be solved directly from the robot. In ROS, the MPC problem is defined using CasADi as a tool to model nonlinear optimizations ⁽¹⁰⁾. CasADi creates an optimization object only once before the robot starts. Then, the optimal control input can be computed by applying the IPOPT algorithm to solve the optimization problem eq(10) ⁽¹¹⁾.

The optimal control input, which is the robot speed commands, is passed through the robot and converted to wheels’ angular speed by eq(3). The MPC implementation in MWOMR using ROS is illustrated in Fig 2.

4.1 Simulation Setup

The simulation is performed in the GAZEBO simulator using Nexus Robot 4WD mecanum wheel mobile robot model as illustrated in Fig 3a. The simulation is carried out in Ubuntu 18.04 LTS in a computer with AMD Ryzen 3950X. Table 1 summarizes all parameters and weights used in NMPC formulation for simulation.

4.2 Experiment Setup

Jetson Nano 4GB is installed on the robot to solve the MPC optimization problem. The picture of the developed MWOMR is given in Fig 3b. The robot’s parameters and weights are given in Table 2.