Koopman operator based model predictive control for trajectory tracking of an omnidirectional mobile manipulator

Abstract

Omnidirectional mobile manipulators (OMMs) have been widely used due to their high mobility and operating flexibility. However, since OMMs are complex nonlinear systems with uncertainties, the dynamic modeling and control are always challenging problems. Koopman operator theory provides a data-driven modeling method to construct explicit linear dynamic models for the original nonlinear systems, using only input-output data. It then allows to design control system based on well-established model-based linear control methods. This paper designs a Koopman operator based model predictive control (MPC) scheme for trajectory tracking control of an OMM. Firstly, using Koopman operator and extended dynamic mode decomposition method, an approximate high-dimensional linear dynamic explicit expression for the OMM system is obtained. Then MPC is employed to achieve tracking control based on the derived linear Koopman model. Finally, to show modeling accuracy for the OMM, the Koopman model is evaluated via both simulation and experimental tests. The control performances of the Koopman operator based MPC design are also verified in the simulation and experimental results.

Introduction

Omnidirectional mobile manipulators (OMMs) consist of an omnidirectional mobile platform and at least one manipulator fixed on the platform. The omnidirectional mobile platform can simultaneously and independently carry out translational and rotational motion. Therefore, OMMs have broad application prospects in many fields.

In the literature, many control approaches have been designed for the trajectory tracking control of OMMs. A decentralized trajectory tracking control method was proposed by Viet et al. ¹ for an OMM considering disturbances and friction. Two controllers were respectively designed to control the mobile platform and manipulator. Fareh et al. ² rearranged the dynamic model to take the form of two interconnected subsystems, and the trajectory tracking control law was designed for the two subsystems, respectively. Watanabe et al. ³ derived a kinematic and dynamical model for a mobile manipulator using the Newton-Euler method. The controller was designed based on computed torque control and resolved acceleration control. A reactive architecture and impedance control was proposed for an OMM by Djebrani et al. ⁴ to ensure reliable task execution. Zhang et al. ⁵ presented a feedback control scheme based on repetitive motion planning. An experimental mobile manipulator assembled on an omnidirectional platform by a dual-arm torso with a human-like structure was presented by Suárez et al. ⁶ Guo et al. ⁷ analyzed the kinematic equation parameters that affects the motion accuracy of the omnidirectional system. For teleoperation of a dual-arm omnidirectional mobile robot, Song et al. ⁸ presented a shared-control-based design which can perform user-commanded tasks or autonomous operations. Avanzini ⁹ developed a constrained model predictive control for mobile robotic manipulators, which it is assumed that the robot is velocity controlled. However, for control designs mentioned above, the control performances depend on the nonlinear dynamic models, and most control methods cannot deal with constraint problems. The dynamic modeling for OMMs is complicated and time-consuming since OMMs are multi-input and multi-output systems with high nonlinearity, strong coupling and complex uncertainty. The controller design based on complex nonlinear models may lead to poor performances, if the model accuracy cannot be guaranteed.

For complex nonlinear systems, linearization is often considered for the convenience of system analysis. Typical linearization methods include feedback linearization, ¹⁰ Taylor linearization, ¹¹ piecewise linearization, ¹² and orthogonal function approximation linearization, ¹³ etc. However, the limitations of these methods are obvious. Feedback linearization requires the precise mathematical model of the control system. Taylor linearization is only carried out near the working point, and just obtains the approximate expression of the original system. Piecewise linearization requires more information about nonlinear control system model. The orthogonal function approximation linearization method has a large computational burden and the controller design is complex. Koopman operator proposed by B. O. Koopman in 1931 provides a powerful tool for the study of nonlinear systems. ¹⁴ The basic idea is to re-express the state evolution process of the original nonlinear system as a linear process by elevating the state of the system to an infinite dimensional space. In other words, via Koopman operator, the infinite-dimensional linear representation of the original nonlinear system is obtained. Koopman operator theory can be used for global linearization of nonlinear dynamical systems. This data-driven modeling process only requires experimental or simulation data, which is completely different from traditional modeling methods.

However, although the nonlinear systems can be converted into infinite dimensional linear systems, Koopman operator is difficult to be applied in practice due to the complexity of its infinite dimension. Dynamic mode decomposition (DMD) method ¹⁵ and extended dynamic mode decomposition (EDMD) method ¹⁶ analyze the spectrum characteristics of the Koopman operator, to achieve the finite dimensional approximation of the infinite dimensional Koopman operator and get the dominant linear expression of the system model. The finite dimensional linear model of the original system is applicative to the model-based linear controller design approaches. Moreover, the DMD and EDMD are mainly based on least-squares regression algorithm, which are computationally flexible and easy to implement. The emergence and development of these data-driven methods make it possible for the Koopman operator method to be applied to many practical problems, and it also provides a new development direction for the control scheme in the era of big data.

In the literature, the EDMD-based Koopman operator approximation methods have been applied to dynamic control systems. Ian et al. ¹⁷ applied Koopman operator theory to the robot control system. A fully data-driven model was obtained using Koopman operator, and then the optimal control was designed based on this model. Simulation and experimental validations were carried out on the Sphero SPRK robot and an inverted pendulum system. Based on the Koopman dynamics model, a linear quadratic regulator (LQR) feedback control scheme was designed by Mamakoukas et al. ¹⁸ Simulations and experiments verified the effectiveness of the modeling and control of the tail-actuated robotic fish, and the Koopman-based LQR control was experimentally verified for the first time. Bruder et al. ¹⁹ obtained the Koopman high-dimensional linear model of a soft robot firstly based on EDMD method. Then the model was identified using a nonlinear autoregressive model with external input and a feed-forward neural network, which was compared with the Koopman high-dimensional linear model to verify the advantage of Koopman operator in the approximation accuracy of the real model. Model predictive control (MPC) scheme was designed and the trajectory tracking control performances were evaluated in the experiments. The active learning algorithm was introduced into the selection of the Koopman operator data sets by Ian. ²⁰ By designing an active learning controller, the information carrying rate of Koopman operator was improved. Then a LQR control scheme was designed for a Van der Pol oscillator to verify the control performance of the model. In addition, experiments on a free-falling quadrotor have verified the ability of combining active learning active learning with the Koopman operator to perform a single execution model learning.

In this paper, a Koopman operator based MPC scheme is designed for trajectory tracking control of an OMM. The proposed control method is data-driven and does not need any prior knowledge. And Koopman operator can transform nonlinear MPC into linear MPC, thus non-convex optimization is avoided, which can meet the real-time requirements. Moreover, extensive experiments are carried out to verify the effectiveness of the MPC based on Koopman operator for rigid mobile manipulators. The OMM is composed of a mobile platform with three omnidirectional wheels and a parallelogram manipulator. Firstly, the Koopman operator for dynamics systems is briefly introduced. Then a high-dimensional linear model of the OMM, known as the Koopman model, is obtained using EDMD-based Koopman operator approximation method. This process is purely input-output data-driven and does not require any prior knowledge or dynamic model information of the OMM system. Linear MPC scheme is selected to design a trajectory tracking controllers for the robot, based on the Koopman model. Finally, simulations and experiments are carried out to validate the modeling accuracy of the derived Koopman model. The control performances of the MPC scheme based on the Koopman model, are also verified via both simulations and experimental tests.

The remainder of this paper is as follows. Firstly, the prior knowledge of Koopman operator and EDMD algorithm is introduced. The Koopman representation for the control systems is presented. Secondly, MPC design is presented based on the Koopman model obtained. Thirdly, simulation results are given. Then, experimental tests are shown to evaluate the Koopman model and verify the effectiveness of the Koopman operator based MPC design. Finally, the conclusions are drawn.

Prior knowledge

Koopman operator for dynamics systems

The Koopman operator is a powerful tool to simplify the analysis of nonlinear dynamic systems, which can transform a finite-dimensional nonlinear dynamic system into an infinite-dimensional linear system. Consider the following discrete-time nonlinear dynamics system:

$$\mathit{\xi }(k+1)=f(\mathit{\xi }(k)),$$

(1)

where $\mathit{\xi }(k)\in \mathit{S}$ is the $n$ -dimensional system state on a smooth manifold $\mathit{S}$ , and $\mathit{S}\subseteq {\mathbb{R}}^n$ is the state space. The parameter $k$ is the discrete time step, and $\mathit{\xi }(k)$ represents the state of the system at the time step $k$ . $f$ is the function which maps the state $\mathit{\xi }(k)$ forward time step $k$ into the future to $\mathit{\xi }(k+1)$ , that is, the evolution of system states.

Define a real-valued observable function on the state space $g:\mathit{S}\to \mathbb{R}$ , which is an element of an infinite-dimensional Hilbert space. The Koopman operator acts on the real-valued observation function $g$ , which is an infinite-dimensional linear operator:

$$(\mathcal{K}g)(\mathit{\xi }(k))=g\circ f(\mathit{\xi }(k)),$$

(2)

where ° indicates function composition. In addition, the Koopman operator $\mathcal{K}$ acts on the function $g$ of the state space, not on the state $\mathit{\xi }(k)$ , thus even though the state of the original nonlinear system is finite-dimensional, $\mathcal{K}$ retains the property of infinite dimensions.

It can be obtained that:

$$(\mathcal{K}g)(\mathit{\xi }(k))=g\circ f(\mathit{\xi }(k))=g(\mathit{\xi }(k+1)).$$

(3)

In other words, using the Koopman operator $\mathcal{K}$ , an infinite-dimensional linear dynamic system is defined, which represents the evolution of the state observation $g(\mathit{\xi }(k))$ to the next time step.

For more detailed information about Koopman operator theory, the reader may refer to the work by Proctor. ²¹

Finite-dimensional approximation of the data-driven Koopman operator

In this subsection, the EDMD method is introduced, which approximates the Koopman operator and the Koopman eigenvalue, eigenfunction, and mode tuples. ¹⁶

The eigendecomposition of the linear Koopman operator $\mathcal{K}$ is as follows:

$$\mathcal{K}{\psi }_j(\mathit{\xi }(k))={\lambda }_j{\psi }_j(\mathit{\xi }(k)),j=1,2,\cdots ,\infty ,$$

(4)

where ${\psi }_j(\mathit{\xi }(k))$ represents the eigenfunctions of the Koopman operator, and ${\lambda }_j$ represents the corresponding eigenvalues.

Then the observation function $g$ can be rewritten as:

$$g(\mathit{\xi }(k))=\sum _{j=1}^{\infty }{\psi }_j(\mathit{\xi }(k)){\mathit{\upsilon }}_j,$$

(5)

where the vector coefficients ${\mathit{\upsilon }}_j$ are called Koopman modes. The Koopman eigenvalues, eigenfunctions and modes triplet are the Koopman tuple. Also, the Koopman eigenfunctions ${\psi }_j(\mathit{\xi }(k))$ are considered as basis observable functions.

Due to the infinite dimension of the Koopman operator, it is challenging to calculate and difficult to implement in practical applications. Therefore, the finite-dimension approximation to the Koopman operator is very necessary. Then a finite subspace approximation to the operator $\mathit{K}\in {\mathbb{C}}^{N_k\times N_k}$ is required.

A subset of the basis functions $\psi (\mathit{\xi })=\left\{\begin{array}{cccc}{\psi }_1(\mathit{\xi }),\hfill & {\psi }_2(\mathit{\xi }),\hfill & \cdots \hfill & {\psi }_{N_k}(\mathit{\xi })\hfill \end{array}\right\}$ is defined, the span of which is the finite subspace denoted as $F_D$ . Then the operator $\mathit{K}$ acting on $\mathit{\psi }(\mathit{\xi })$ is then represented in discrete time as:

$$(\mathcal{K}g)(\mathit{\xi }(k))=\mathit{K}\mathit{\psi }(\mathit{\xi }(k))\mathit{\upsilon }+\mathit{r}(\mathit{\xi }(k)),$$

(6)

where $\mathit{\upsilon }$ represents the weight coefficient and $\mathit{r}(\mathit{\xi }(k))$ is the residual term.

Provide a dataset $\mathcal{D}={\{\mathit{\xi }(k)\}}_{k=1}^P$ . The approximate Koopman operator $\mathit{K}$ can be obtained using least-squares minimization over the parameters of $\mathit{K}$ :

$$\underset{\mathit{K}}{min}\frac{1}{2}\sum _{k=1}^{P-1}\Vert \mathit{\psi }(\mathit{\xi }(k+1))-\mathit{K}\mathit{\psi }(\mathit{\xi }(k))\mathit{\upsilon }{\Vert }^2,$$

(7)

where $P$ is the number of the collected data.

The solution is given by

$$\mathit{K}=\mathit{E}{\mathit{N}}^{†},$$

(8)

where † denotes the Moore-Penrose pseudoinverse and

$$\begin{array}{c}\mathit{E}=\frac{1}{P}\sum _{k=1}^{P-1}\mathit{\psi }(\mathit{\xi }(k+1))\mathit{\psi }{(\mathit{\xi }(k))}^T\hfill \\ \multicolumn{1}{c}{\mathit{N}=\frac{1}{P}\sum _{k=1}^{P-1}\mathit{\psi }(\mathit{\xi }(k))\mathit{\psi }{(\mathit{\xi }(k))}^T}\end{array}.$$

(9)

Convergence of EDMD to the Koopman operator has been analyzed. ²²

In addition, the Koopman operator theory is also applicable to dynamic systems with control. According to the method by Korda and Mezić, ²³ consider the following nonlinear dynamic system with control:

$$\mathit{\xi }(k+1)=f(\mathit{\xi }(k),\mathit{u}(k)),$$

(10)

where $\mathit{\xi }(k)\in {\mathbb{R}}^n$ is the state of the system, and $\mathit{u}(k)\in {\mathbb{R}}^m$ is the control input.

The system state $\mathit{\xi }(k)$ and the control input $\mathit{u}(k)$ are merged into an augmented state: $\mathit{\chi }=[{\mathit{\xi }}^T,{{\mathit{u}}_{\mathbf{s}}}^T{]}^T$ , ${\mathit{u}}_s$ denotes the all control sequences, that is $({\mathit{u}}_i{)}_{i=0}^{\infty }$ . Then the augmented system can be written as:

$$\mathit{\chi }(k+1)=F(\mathit{\chi }(k))=\left[\begin{array}{c}f(\mathit{\xi }(k),{\mathit{u}}_{\mathbf{s}}(0))\hfill \\ \multicolumn{1}{c}{{\mathit{u}}_{\mathbf{s}}(k+1)}\end{array}\right],$$

(11)

where, ${\mathit{u}}_{\mathrm{s}}(k)$ denotes the $k$ th element of the sequence ${\mathit{u}}_{\mathrm{s}}$ .

For system (11), Koopman operator is defined as follows:

$$(\mathcal{K}g)(\mathit{\chi }(k))=g\circ F(\mathit{\chi }(k)).$$

(12)

For the sake of controller design, a high-dimensional linear dynamic model in state space is built using Koopman operator. The high-dimensional linear model constructed is as follows:

$$\begin{array}{c}\mathit{z}(k+1)=\mathit{Az}(k)+\mathit{Bu}(k)\hfill \\ \multicolumn{1}{c}{\hat{\mathit{\xi }}(k)=\mathit{Cz}(k)}\end{array},$$

(13)

where $\mathit{z}(k)=\psi (\xi )\in {\mathbb{R}}^N$ is the state of the high-dimensional system, and $N$ is the dimension of high-dimensional system. $\hat{\mathit{x}}(k)\in {\mathbb{R}}^n$ is the estimation of the state $\mathit{\xi }(k)$ . $\mathit{A}\in {\mathbb{R}}^{N\times N}$ , $\mathit{B}\in {\mathbb{R}}^{N\times m}$ and $\mathit{C}\in {\mathbb{R}}^{n\times N}$ are linear time-invariant matrices.

According to the work by Korda and Mezić, ²³ an analytic solution to find $\mathit{A}$ , $\mathit{B}$ and $\mathit{C}$ in (13) can be easily obtained.

Provide real state and control input data of the original nonlinear system and define data sets as follows:

$$\begin{array}{c}\mathit{\xi }=\left[\begin{array}{ccc}\mathit{\xi }(1)\hfill & \cdots \hfill & \mathit{\xi }(P)\hfill \end{array}\right]\hfill \\ \multicolumn{1}{c}{\mathit{Y}=\left[\begin{array}{ccc}\mathit{\xi }(2)\hfill & \cdots \hfill & \mathit{\xi }(P+1)\hfill \end{array}\right]}\\ \multicolumn{1}{c}{\mathit{U}=\left[\begin{array}{ccc}\mathit{u}(1)\hfill & \cdots \hfill & \mathit{u}(P)\hfill \end{array}\right]}\end{array},$$

(14)

where $\mathit{\xi }(k)$ and $\mathit{u}(k)$ satisfy the relation shown by (10). The matrices $\mathit{A}$ , $\mathit{B}$ , and $\mathit{C}$ in (13) can be obtained as the solutions to the following optimization problems:

$$\begin{array}{c}\underset{\mathit{A},\mathit{B}}{min}\Vert {\mathit{Y}}_{\mathrm{lift}}-\mathit{A}{\mathit{\xi }}_{\mathrm{lift}}-\mathit{BU}\Vert \hfill \\ \multicolumn{1}{c}{\underset{\mathit{C}}{min}\Vert \mathit{\xi }-\mathit{C}{\mathit{\xi }}_{\mathrm{lfit}}\Vert }\end{array},$$

(15)

where ${\mathit{\xi }}_{\mathrm{lfit}}=[\mathit{\psi }(\mathit{\xi }(1)),...,\mathit{\psi }(\mathit{\xi }(P))],$ ${\mathit{Y}}_{\mathrm{lfit}}=[\mathit{\psi }(\mathit{\xi }(2)),...,\mathit{\psi }(\mathit{\xi }(P+1))].$

The analytical solutions to (15) are:

$$\begin{array}{c}[\mathit{A},\mathit{B}]={\mathit{Y}}_{\mathrm{lift}}{[{\mathit{\xi }}_{\mathrm{lift}},\mathit{U}]}^{†}\hfill \\ \multicolumn{1}{c}{\mathit{C}={\mathit{XX}}_{\mathrm{lift}}^{†}}\end{array}.$$

(16)

Now, a Koopman model of the nonlinear system (10) is obtained by (13) and (16) which can be used to predict the future state and design the controller.

• Remark 1. ²³ If the basis observable functions $\left\{\begin{array}{cccc}{\psi }_1(\mathit{\xi }),\hfill & {\psi }_2(\mathit{\xi }),\hfill & \cdots \hfill & {\psi }_{N_k}(\mathit{\xi })\hfill \end{array}\right\}$ contain the state itself, that is, ${\psi }_i(\mathit{\xi })={\xi }_i$ , where $i=1,2,\cdots ,n$ and $x_i$ is one state of the system, the solution to (16) is $\mathit{C}=\left[\begin{array}{cc}{\mathit{I}}_n\hfill & 0\hfill \end{array}\right]$ .

• Remark 2.^20,24 The basis functions of state are required to contain nonlinearity. Appropriate basis functions can effectively improve the estimation accuracy of the finite dimensional Koopman operator obtained by EDMD. Often the basis functions are artificially defined, and this process requires specific knowledge and a lot of attempts. In recent years, the development of neural networks provides a new solution for the discovery of basis functions. A deep learning approach was proposed by Yeung et al. ²⁴ to discovery the basis functions from data, which shows that deep neural networks can be used to discover the basis functions actively and obtain an accurate approximation of the Koopman operator.

• Remark 3. For the OMM control system, there is no positive correlation between the dimension of the lifted state-space $N$ and the Koopman model accuracy. Therefore, in practice, the calculation cost and model accuracy should be balanced to select the appropriate dimension.

• Remark 4. For more detailed information about equation (13), the readers can refer to the work by Korda and Mezić. ²³

Model predictive control for the OMM

The model-based control method is the most mature and widely used control method among the many control methods. At present, MPC is the most popular model-based control design technique, which uses the model to predict the future output and optimizes control input within a finite time horizon, considering various constraints. The optimized input is applied to a single time step, and then iterate on the optimization. ²⁵ The predictive control principles of linear and nonlinear systems are similar, that is, model-based prediction, online rolling optimization, and feedback correction. However, the optimization problem in nonlinear MPC suffers from heavy calculation burden due to a nonlinear cost function and nonlinear constraints. Koopman high-dimensional model can transform nonlinear MPC into linear MPC, thus non-convex optimization is avoided. ²⁶ Therefore, the control task of nonlinear dynamic systems can be accomplished by designing a linear MPC scheme based on Koopman model.

For the OMM control system, the original state vector is $\mathit{\xi }(k)={\left[\begin{array}{cc}\mathit{q}{(k)}^T\hfill & \mathit{\upsilon }{(k)}^T\hfill \end{array}\right]}^T\in {\mathbb{R}}^{10}$ . $\mathit{q}(k)={\left[\begin{array}{ccccc}x(k)\hfill & y(k)\hfill & \varphi (k)\hfill & {\theta }_1(k)\hfill & {\theta }_2(k)\hfill \end{array}\right]}^T\in {\mathbb{R}}^5$ is the position state of OMM, where $x(k)$ , $y(k)$ , and $\varphi (k)$ are three plane degrees of freedom representing the x-coordinate, y-coordinate, and rotation angle of the OMM respectively, while ${\theta }_1(k)$ and ${\theta }_2(k)$ are the two joint angles of the manipulator. $\mathit{\upsilon }(k)=\frac{\mathit{q}(k)-\mathit{q}(k-1)}{\Delta t}\in {\mathbb{R}}^5$ is the velocity state, which adopts the backward Euler method. $\Delta t$ is the sampling time. Control input $\mathit{u}(k)={\left[\begin{array}{ccccc}{u}_1(k)\hfill & u_2(k)\hfill & u_3(k)\hfill & u_4(k)\hfill & u_5(k)\hfill \end{array}\right]}^T\in {\mathbb{R}}^5$ is the vector consisting of the input voltages of five motors. The goal of trajectory tracking control of an OMM is to give a desired trajectory, and a controller is designed to enable the OMM to track the desired trajectory in real time under the action of the control input. In other words, under effective control, the real outputs (position and velocity) of the OMM at the moment can be infinitely close to the desired values. Based on the method mentioned above, the Koopman model (KM) of the OMM can be obtained as equation (13). The prediction equation for the future state of the high-dimensional system $\mathit{z}$ can be written as follows:

$$\begin{array}{c}\mathit{z}(k+1|k)=\mathit{Az}(k)+\mathit{Bu}(k)\hfill \\ \multicolumn{1}{c}{\mathit{z}(k+2|k)=\mathit{Az}(k+1|k)+\mathit{Bu}(k+1|k)}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{\mathit{z}(k+N_c|k)=\mathit{Az}(k+N_c-1|k)+\mathit{Bu}(k+N_c-1|k)}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{\mathit{z}(k+N_p|k)=\mathit{Az}(k+N_p-1|k)+\mathit{Bu}(k+N_p-1|k)}\end{array},$$

(17)

where $N_c$ is the control horizon, $N_p$ is the prediction horizon, $\mathit{z}(k+i|k)$ and $\mathit{u}(k+i|k)$ represent the predictive state and control input at time $k$ , respectively.

Define $\mathit{y}(k)$ as the output of the OMM system. For this system, the system output is the position, that is, $\mathit{y}(k)=\mathit{q}(k)$ . Then, according to equation (13), the predicted output is represented as:

$$\hat{\mathit{y}}(k)=\hat{\mathit{q}}(k)=\mathit{Cz}(k).$$

(18)

The prediction equation of the system is rewritten in the compact form as follows:

$$\mathit{Y}(k)=\mathit{Fz}(k)+\mathbf{\Phi }\mathit{U}(k),$$

(19)

where

$$\begin{array}{c}\hat{\mathit{Y}}(k)\underset{=}{\Delta }{[\hat{\mathit{y}}{(k+1|k)}^T,\hat{\mathit{y}}{(k+2|k)}^T,...,\hat{\mathit{y}}{(k+N_p|k)}^T]}^T,\hfill \\ \mathit{U}(k)\underset{=}{\Delta }{[\mathit{u}{(k|k)}^T,\mathit{u}{(k+1|k)}^T,...,\mathit{u}{(k+N_c-1|k)}^T]}^T,\hfill \\ \mathit{F}={[\mathit{CA},\mathit{C}{\mathit{A}}^2,...,\mathit{C}{\mathit{A}}^{N_p}]}^T,\hfill \\ \mathbf{\Phi }=\left[\begin{array}{cccc}\mathit{CB}\hfill & 0\hfill & \cdots \hfill & 0\hfill \\ \multicolumn{1}{c}{\mathit{CAB}}& \mathit{CB}\hfill & \cdots \hfill & 0\hfill \\ \multicolumn{1}{c}{⋮}& ⋮\hfill & \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{\mathit{C}{\mathit{A}}^{N_p-1}\mathit{B}}& \mathit{C}{\mathit{A}}^{N_p-2}\mathit{B}\hfill & \cdots \hfill & \mathit{C}{\mathit{A}}^{N_p-N_c}\mathit{B}\hfill \end{array}\right].\hfill \end{array}$$

The predictive reference signals are given by:

$${\mathit{Y}}_d(k)\underset{=}{\Delta }{[{\mathit{y}}_d{(k+1|k)}^T,{\mathit{y}}_d{(k+2|k)}^T,...,{\mathit{y}}_d{(k+N_p|k)}^T]}^T.$$

(20)

The objective of MPC is to minimize the following cost function:

$$J=(\mathit{Y}(k)-{\mathit{Y}}_d(k){)}^T\mathit{Q}(\mathit{Y}(k)-{\mathit{Y}}_d(k))+\mathit{U}(k{)}^T\mathit{RU}(k),$$

(21)

where $\mathit{Q}$ and $\mathit{R}$ are positive definite matrices, and

$$\begin{array}{c}J={(\mathit{Fz}(k)+\mathbf{\Phi }\mathit{U}(k)-{\mathit{Y}}_r(k))}^T\mathit{Q}\hfill \\ \multicolumn{1}{c}{(\mathit{Fz}(k)+\mathbf{\Phi }\mathit{U}(k)-{\mathit{Y}}_d(k))+\mathit{U}{(k)}^T\mathit{RU}(k)}\\ \multicolumn{1}{c}{={(\mathit{Fz}(k)-{\mathit{Y}}_d(k))}^T\mathit{Q}(\mathit{Fz}(k)-{\mathit{Y}}_d(k))}\\ \multicolumn{1}{c}{+2\mathit{U}{(k)}^T{\mathbf{\Phi }}^T\mathit{Q}(\mathit{Fz}(k)-{\mathit{Y}}_d(k))}\\ \multicolumn{1}{c}{+\mathit{U}{(k)}^T(\mathit{R}+{\mathbf{\Phi }}^T\mathit{Q}\mathbf{\Phi })\mathit{U}(k)}\end{array}.$$

(22)

Because the first term on the right hand of (22) is independent of $\mathit{U}(k)$ , it has:

$$\begin{array}{c}\underset{\mathit{U}}{min}J\Rightarrow \underset{\mathit{U}}{min}\hat{J}\hfill \\ \multicolumn{1}{c}{s.t.\mathit{z}(k)=\mathbf{\Psi }(x(k))}\\ \multicolumn{1}{c}{\left|u_i\right|\le u_{\max }}\end{array},$$

(23)

and

$$\begin{array}{c}\hat{J}=2\mathit{U}{(k)}^T{\mathbf{\Phi }}^T\mathit{Q}(\mathit{Fz}(k)-{\mathit{Y}}_r(k))\hfill \\ \multicolumn{1}{c}{+\mathit{U}{(k)}^T\left({\mathbf{\Phi }}^T\mathit{Q}\mathbf{\Phi }\right)\mathit{U}(k)}\\ \multicolumn{1}{c}{=2\mathit{U}{(k)}^T\mathit{H}+\mathit{U}{(k)}^T\mathit{WU}(k)}\end{array},$$

(24)

where $\left|u_i\right|\le u_{\max }$ represents the voltage constraints of the OMM system, limited by the DC motors, and $\mathit{H}={\mathbf{\Phi }}^T\mathit{Q}(\mathit{Fz}(k)-{\mathit{Y}}_r(k))$ , $\mathit{W}={\mathbf{\Phi }}^T\mathit{Q}\mathbf{\Phi }+\mathit{R}$ . The above equation can be solved as a standard linear quadratic programing problem, and the global optimal solution can be obtained quickly. By solving equation (24), the optimal voltage sequence $\mathit{U}(k)$ in the predicted horizon $N_p$ can be obtained. The first element in $\mathit{U}(k)$ is the output voltage at time $k$ . Block diagram of the proposed control design is show as Figure 1.

Figure 1.

Block diagram of the proposed control design.

In order to improve the trajectory tracking control performances of MPC, set ${\mathit{Q}}_{N_p}\in {\mathbb{R}}^{5\times 5}$ in $\mathit{Q}=\mathrm{diag}(\stackrel{N_p-1}{\overbrace{{\mathit{Q}}_a,{\mathit{Q}}_a,...,{\mathit{Q}}_a}},{\mathit{Q}}_{N_p})$ be larger than ${\mathit{Q}}_a\in {\mathbb{R}}^{5\times 5}$ . $\mathit{R}=\mathrm{diag}(\stackrel{N_c}{\overbrace{{\mathit{R}}_c,{\mathit{R}}_c,...,{\mathit{R}}_c}})$ is the voltage optimization term. In practical application, $\mathit{Q}$ and $\mathit{R}$ matrices need to be adjusted, which can not only satisfy the high-precision trajectory tracking, but also make the actuator outputs meet the constraints.

Simulations

In simulations, a mathematical dynamic model established by Euler-Lagrange method is employed to approximate the real OMM. The approximate mathematical dynamic model in continuous-time form with voltage input is described as follows:

$$\mathit{M}\stackrel{··}{\mathit{q}}+{\mathit{C}}_0\stackrel{·}{\mathit{q}}+\mathit{G}=\mathit{Bu},$$

(25)

where $\mathit{q}={\left[xy\varphi {\theta }_1{\theta }_2\right]}^T$ is the position state of OMM, $\mathit{M}\in {\mathbb{R}}^{5\times 5}$ is the inertia matrix, ${\mathit{C}}_0\in {\mathbb{R}}^{5\times 5}$ represents the centrifugal and Coriolis matrix, $\mathit{G}\in {\mathbb{R}}^{5\times 1}$ is the gravity term, $\mathit{u}={\left[u_1{u}_2{u}_3{u}_4{u}_5\right]}^T$ is the supplied voltage of five motors.

The input and output data are collected using random initial states and random control input sequences which can stimulate the robot’s dynamic characteristics. The data set is established including the generated output data and control input data. Taking the current state, control input and output state data as a data set, a total of 23,500 sets of data are collected in the simulations used to calculate the high-dimensional Koopman model. The dimension of the lifted state-space is $N=12$ . Thin plate spline radial basis function with center at ${\mathit{\xi }}_0$ is chosen as basis function, that is, $\psi (\xi )=\Vert \mathit{\xi }-{\mathit{\xi }}_0{\Vert }^2\log (\Vert \mathit{\xi }-{\mathit{\xi }}_0\Vert )$ . Then using the method proposed, Koopman model (13) is obtained, and MPC scheme is then designed based on the linear model.

To verify the effectiveness of Koopman operator based MPC (K-MPC), two trajectories with different characteristics are used, which are considered as sinusoidal trajectory and linear trajectory according to the desired trajectory of the mobile platform. Define the desired position of the OMM as ${\mathit{q}}_d={\left[\begin{array}{ccccc}{x}_d\hfill & y_d\hfill & {\varphi }_d\hfill & {\theta }_{1d}\hfill & {\theta }_{2d}\hfill \end{array}\right]}^T$ , then the two desired trajectories are set as follows:

• 1. The desired sinusoidal trajectory:

$$\{\begin{array}{c}{x}_d[\mathrm{m}]=0.8\cos \left(\frac{\mathrm{\pi }t}{30}\right)\hfill \\ \multicolumn{1}{c}{y_d[\mathrm{m}]=0.8\cos \left(\frac{\mathrm{\pi }t}{30}\right)}\\ \multicolumn{1}{c}{{\varphi }_d[\mathrm{rad}]=\sin \left(\frac{\mathrm{\pi }t}{30}\right)}\\ \multicolumn{1}{c}{{{\theta }_1}_d[\mathrm{m}]=0.6\sin \left(\frac{\mathrm{\pi }t}{10}\right)}\\ \multicolumn{1}{c}{{{\theta }_2}_d[\mathrm{m}]=0.6\sin \left(\frac{\mathrm{\pi }t}{10}\right)}\end{array}0\mathrm{s}\le t\le 60\mathrm{s}.$$

• 2. The desired piecewise linear trajectory:

$$\begin{array}{c}\multicolumn{1}{c}{x_d[\mathrm{m}]=\{\begin{array}{c}0.1t,0\mathrm{s}\le t<10\mathrm{s}\hfill \\ \multicolumn{1}{c}{1,10\mathrm{s}\le t<20\mathrm{s}}\\ \multicolumn{1}{c}{1-0.1(t-20)20\mathrm{s}\le t<30\mathrm{s}}\\ \multicolumn{1}{c}{0,30\mathrm{s}\le t<40\mathrm{s}}\end{array},}\\ \multicolumn{1}{c}{y_d[\mathrm{m}]=\{\begin{array}{c}0,0\mathrm{s}\le t<10\mathrm{s}\hfill \\ \multicolumn{1}{c}{0.1(t-10),10\mathrm{s}\le t<20\mathrm{s}}\\ \multicolumn{1}{c}{120\mathrm{s}\le t<30\mathrm{s}}\\ \multicolumn{1}{c}{1-0.1(t-30),30\mathrm{s}\le t<40\mathrm{s}}\end{array},}\\ \multicolumn{1}{c}{{\varphi }_d[\mathrm{rad}]=\{\begin{array}{c}0,0\mathrm{s}\le t<10\mathrm{s}\hfill \\ \multicolumn{1}{c}{0.1(t-10),10\mathrm{s}\le t<20\mathrm{s}}\\ \multicolumn{1}{c}{120\mathrm{s}\le t<30\mathrm{s}}\\ \multicolumn{1}{c}{1-0.1(t-30),30\mathrm{s}\le t<40\mathrm{s}}\end{array},}\\ \multicolumn{1}{c}{{\theta }_1[\mathrm{rad}]={\theta }_2[\mathrm{rad}]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right).}\end{array}$$

In simulations, the parameters used in K-MPC are set as follows:

$$\begin{array}{c}{\mathit{Q}}_a=\mathrm{diag}(0.1,0.1,0.1,0.1,0.1),\hfill \\ {\mathit{Q}}_{N_p}=\mathrm{diag}(100,100,100,100,100),\hfill \\ \multicolumn{1}{c}{{\mathit{R}}_c=\mathrm{diag}(1\times {10}^{-6},1\times {10}^{-6},1\times {10}^{-6},1\times {10}^{-6},}\\ \multicolumn{1}{c}{1\times {10}^{-6}).}\end{array}$$

Simulations results are displayed in Figures 2 and 3. It can be seen that the OMM can complete the trajectory tracking task of the two reference trajectories. The proposed control scheme K-MPC achieves satisfactory performances, without any prior knowledge and model information.

Figure 2.

Simulation results of the sinusoidal trajectory: (a) Tracking responses of the OMM. (b) Tracking errors of the OMM.(c) Control input $\mathit{u}$ .

Figure 3.

Simulation results of the linear trajectory: (a) Tracking responses of the OMM. (b) Tracking errors of the OMM.(c) Control input $\mathit{u}$ .

Experiments

Experimental setup

Figure 4 shows the prototype platform. The omnidirectional mobile platform consists of three omnidirectional wheels arranged at $120°$ intervals. Each wheel is actuated by a DC motor. The manipulator is a parallelogram arm with four links. The parallelogram arm has two degrees of freedom (i.e. ${\theta }_1$ and ${\theta }_2$ ), which are respectively driven by one DC motor. The five DC motors are identical and are Maxon DC motors with gear reduction ratio of 186 and the nominal voltage of 24 V.

Figure 4.

OMM developed by our lab.

The proposed control approach is implemented on a personal computer (Intel(R) Core(TM) i7-4770 CPU@3.40GHz) via Matlab/Simulink. The control signals generated from a personal computer are then transmitted to the OMM via WIFI module (part NO. ESP8266). The MCU (part NO. STM32F103VET6) in the control box receives the control signals and then sends the corresponding pulse-width modulation (PWM) signal to the five motor drivers (part NO. LMD18200). A motion capture system (OptiTrack) is employed to measure the position and orientation of the OMM. Figure 5 shows the schematic of the experimental setup.

Figure 5.

Schematic of the experimental setup.

Data collection and model identification

In the experiment, irreversible damage may be caused to the motor if the control voltage input changes too fast. Thus it is not feasible to give a random voltage sequence directly, as in simulations. Therefore, in order to balance the randomness and smoothness of the voltage, a voltage generation strategy is designed. The voltage range of the motors for the mobile platform is between −24 and 24 V. Due to the limited movement space of the manipulator, the voltage is limited between −5 and 5 V in order to avoid the excessive movement speed. A matrix ${\mathrm{\Upsilon }}^{5\times 1500}$ of uniformly distributed random numbers between the voltage range is proposed first. Then set the transition period $T_{u1}$ or $T_{u2}$ , which is much longer than the sampling time, as the interval of time between two adjacent elements of $\mathrm{\Upsilon }$ . In order to ensure the smoothness of the voltage signal, the random control inputs are obtained by followings:

$$u_i(t)=\{\begin{array}{c}\frac{{\mathrm{\Upsilon }}_{i,k_1+1}-{\mathrm{\Upsilon }}_{i,k_1}}{T_{u1}}(t-(k_1-1)T_{u1})+{\mathrm{\Upsilon }}_{i,k_1},i=1,2,3\hfill \\ \multicolumn{1}{c}{\frac{{\mathrm{\Upsilon }}_{i,k_2+1}-{\mathrm{\Upsilon }}_{i,k_2}}{T_{u1}}(t-(k_2-1)T_{u2})+{\mathrm{\Upsilon }}_{i,k_2},i=4,5}\end{array},$$

(26)

where $k_1=\mathrm{floor}(t/T_{u1})$ and $k_2=\mathrm{floor}(t/T_{u2})$ are the current indexes into the lookup table at time $t$ . The transition periods were set as $T_{u1}=0.3$ s and $T_{u2}=0.15$ s. Data sets of 57 trajectories were collected in the experiments, of which the first 50 sets were training sets used to calculate the Koopman high-dimensional model, and the last seven sets were test sets used to evaluate the approximation accuracy of the high-dimensional model. Each data collection process lasts for 5 s and the sampling time is 0.025 s. Then Koopman model was fit from the collected data, including state (current state and output state) data and control input (voltage) data.

Experimental results

Koopman model evaluation

The performance of Koopman model was benchmarked against Lagrange model (LM) of the OMM. Two sets of the collected data in the experiments were employed in the model evaluation. The two voltage sequences in the test sets are shown in Figure 6, and the comparative results of the output state trajectory are presented in Figure 7.

Figure 6.

Random voltage sequences in experiment test sets: (a) The first set of random voltage. (b) The second set of random voltage.

Figure 7.

The predicted trajectory of LM and KM under two random voltage sequences: (a) The predicted trajectory under the random voltage in Figure 6(a). (b) The predicted trajectory under the random voltage in Figure 6(b).

It is obvious that KM has better predicted performance than LM for the evolution of the system and its predicted state trajectory is closer to the real model. Accurate prediction capability can improve the MPC performances. In order to quantitatively illustrate the advantages of the KM in the experiments, relative root mean squared error (RRMSE) is defined to evaluate the model accuracy:

$$\mathrm{RRMSE}(\mathrm{\%})=100·\frac{\sqrt{\sum _k\Vert {\mathit{\xi }}_{\mathrm{pred}}(k{T}_s)-{\mathit{\xi }}_{\mathrm{true}}(k{T}_s){\Vert }_2^2}}{\sqrt{\sum _k\Vert {\mathit{\xi }}_{\mathrm{true}}(k{T}_s){\Vert }_2^2}},$$

(27)

where $T_s$ is sampling period.

In the experiments, RRMSE was calculated to measure the approximation accuracy of the high-dimensional linear model to the real OMM experimental system. Seven sets of the collected data were selected randomly to calculate the RRMSE of different trajectories respectively, and the average value was obtained. The calculation results of KM and LM are displayed in Table 1. The RRMSE results show that KM has higher modeling accuracy than LM of the OMM system. Averagely, the accuracy of KM is improved by about 25%, compared with LM.

Table 1.

Predicted RRMSE over 5 s horizon.

Model	1	2	3	4	5	6	7	Avg.
LM	40.05%	49.85%	49.00%	33.63%	30.04%	29.12%	33.72%	37.92%
KM	25.55%	32.46%	36.13%	29.24%	25.14%	25.31%	23.46%	28.18%

Validations of the control effectiveness

In this part, experimental tests were carried out to verify the trajectory tracking performances of K-MPC. The desired trajectories used in experiments are the same as those in simulations. In the experiments, the parameters used in K-MPC were set as follows:

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{Q}}_a=\mathrm{diag}(3,5,5,5,5),}\\ \multicolumn{1}{c}{{\mathit{Q}}_{N_p}=\mathrm{diag}(265,300,320,480,480),}\\ \multicolumn{1}{c}{{\mathit{R}}_c=\mathrm{diag}(8\times {10}^{-5},8\times {10}^{-5},8\times {10}^{-5},5\times {10}^{-3},}\\ \multicolumn{1}{c}{5\times {10}^{-3}).}\end{array}$$

Experiments results are displayed in Figures 8 and 9. It can be seen that the OMM can complete the trajectory tracking task of the two reference trajectories, which validates the effectiveness of the proposed control design in the experimental environment.

Figure 8.

Experimental results of the sinusoidal trajectory: (a) Tracking responses of the OMM. (b) Tracking errors of the OMM.(c) Control input $\mathit{u}$ .

Figure 9.

Experimental results of the linear trajectory: (a) Tracking responses of the OMM. (b) Tracking errors of the OMM.(c) Control input $\mathit{u}$ .

In the complex and changeable practical application environment, it is more difficult to model and control the system. The experiments prove that K-MPC method provides an effective solution to achieve the modeling and control of complex nonlinear systems.

Conclusions

In this paper, a data-driven modeling and control method based on Koopman operator theory have been successfully applied to an OMM. Using EDMD-based Koopman operator approximation method, KM of the OMM system has been obtained firstly. The KM accuracy has been evaluated compared with LM qualitatively and quantitatively, which shows KM has higher accuracy. The K-MPC scheme has been designed and has been shown to be capable of commanding OMM to follow a reference trajectory in simulations and experiments. This method makes the modeling of the complex nonlinear dynamic systems more easier, leading to the rapid development of new control strategies and applications. To the best of our knowledge, this is the first experimental validation of Koopman-based MPC control for rigid manipulators.

Future work will consider the method of choosing the most effective basis functions to achieve high precision approximation. Also, the real-time uncertainties of the systems in the practical application are expected to be included in the Koopman model in the future development.