Implementation of a robust data-driven control approach for an ommi-directional mobile manipulator based on koopman operator

Abstract

The dynamic modeling and control of omni-directional mobile manipulators (OMM) are challenging since they are highly nonlinear, strongly coupled, and multi-input multi-output uncertainty systems. Koopman operator theory can provide an explicit linear dynamic model for OMM, only based on input-output data. However, the derived dynamic model usually has modeling errors and cannot capture external unknown disturbances. In this paper, a robust data-driven control scheme is designed and implemented for an OMM, based on Koopman operator and a disturbance observer. Firstly, a data-driven Koopman model of OMM is constructed, solely using the collected input-output data. Then a disturbance observer is designed to online estimate the inherent modeling error of the Koopman model as well as external disturbances. The controller is designed by combining linear MPC with feedback compensation of the estimated disturbances. Finally, both simulations and experimental tests are conducted to verify the effectiveness and robustness of the proposed control scheme.

Introduction

Mobile manipulators are composed of a mobile platform and at least one manipulator fixed on it. Compared with fixed-base manipulators, mobile manipulators have much larger workspace. ¹ Specially, for omnidirectional mobile manipulators (OMMs), the omnidirectional mobile platform can simultaneously and independently carry out translational and rotational motion. Therefore, OMMs are suitable for applications with confined spaces, such as factories. Meanwhile, OMM is a highly nonlinear, strongly coupled, and multi-input multi-output uncertainty system.

In the literature, trajectory tracking control of OMMs is always a challenging problem. The dynamic analysis and control design were given by Watanabe et al. ² for an OMM with three lateral orthogonal wheel assemblies, wherein computed torque control and resolved acceleration control were used. Viet et al. ³ has proposed a tracking control method for a three-wheeled OMM considering disturbance and friction, in which sliding mode controller was employed. A hybrid control strategy for trajectory-tracking control of wheeled mobile robotic manipulators was proposed by Shafei et al. ⁴ in the presence of model uncertainties and external disturbances. A robust neural network-based sliding mode controller was designed by Xu et al. ⁵ for trajectory tracking problem for a redundantly actuated OMM in the presence of uncertainties and disturbances. Li et al. ⁶ presented a non-singular terminal sliding mode control method for an OMM based on extended state observer, wherein the nominal dynamic model is used. An experimental mobile manipulator assembled on an omnidirectional platform with a dual-arm torso with a human-like structure was presented by Suárez et al. ⁷ A controller based on constrained optimization was developed for tracking problems of mobile manipulation by Avanzini et al. ⁸ However, for the research works mentioned above, the controller design completely or partially depends on the nominal dynamic model. Note that the dynamic modeling for OMMs is complicated and time-consuming, due to high nonlinearity, strong coupling, and complex uncertainty.

Koopman operator theory offers a way that allows one to transform a nonlinear dynamical system into an infinite-dimensional linear system, solely using input-output data of the dynamical system.^9,10 In other words, the Koopman methodology is capable of discovering the underlying dynamics and control of a given system from data alone. More specifically, the basic idea is to lift of the state-space to a higher dimensional space, where the evolution of this lifted state is approximately linear. This makes it an attractive data-driven control architecture for OMM control design. Koopman operator theory has been applied in many fields, for example, fluid analysis, ¹¹ robotics,^12–14 power system, ¹⁵ etc.

Since the Koopman operator is an infinite-dimensional operator, it makes practical use prohibitive. In recent works, dynamic mode decomposition (DMD) ¹⁶ and extended dynamic mode decomposition (EDMD) ¹⁷ are popular methods to obtain finite-dimensional linear models. This linear representation allows one to control the nonlinear system using tools from linear control methods, for example, linear model predictive control (MPC), Linear Quadratic Regulator (LQR). However, three main aspects influence the modeling accuracy of the Koopman operator based linear representation (Koopman model), that is, dimensionality, basis functions and data quality. Unless a finite-dimensional invariant subspace exists, ¹⁴ the Koopman operator is infinite-dimensional. Thus, finite-dimensional approximation usually introduces modeling error. Moreover, there has not been a systematic way of choosing the Koopman basis functions for general nonlinear systems. Lastly, the quality of the collected data significantly influences the modeling error. Also, the Koopman model cannot capture unknown external disturbances during system operation.

Disturbance observer is one of the most popular robust control tools due to its simplicity, flexibility, and efficacy. ¹⁸ The basic idea is to estimate both internal (i.e. modeling errors) and external disturbances, and to compensate them in the control signals. Several linear and nonlinear observers have been independently developed in the literature, such as perturbation observer, ¹⁹ equivalent input disturbance (EID) estimator, ²⁰ uncertainty and disturbance estimator (UDE), ²¹ generalized proportional integral observer (GPIO), ²² extended state observer (ESO). ²³ Disturbance observers have been widely applied in various fields, such as robotics,^24–26 to name a few.

In this paper, a robust data-driven control scheme is designed and implemented for an OMM, based on Koopman operator and disturbance observer. Firstly, a data-driven Koopman model of OMM was constructed, solely using the collected input-output data. Then a disturbance observer is designed to online estimate the inherent modeling error of the Koopman model as well as external disturbances. The controller is designed by combining linear MPC with feedback compensation of the estimated total disturbances. Finally, both simulations and experimental tests are conducted to verify the effectiveness and robustness of the proposed control scheme.

Finite-dimensional Koopman representations cannot well capture nonlinear effects inherent in many robotic systems. The main contribution of this work is that a robust data-driven MPC scheme is proposed by combining Koopman operator and disturbance observer, without using any model information. Extensive simulations and experimental tests are carried out to verify the effectiveness and robustness of the proposed control scheme, against typical Koopman operator based MPC.

This paper is organized as follows. Firstly, Koopman operator-based dynamic modeling is presented. Then, the proposed control scheme is developed. Next, the simulation and experiment are introduced respectively. Finally, the conclusion is drawn.

Koopman model

Koopman operator theory

Consider a discrete uncontrolled dynamical system:

$$x(k+1)=f(x(k))$$

(1)

where, $x(k)\in \mathcal{M}$ is the n-dimensional system state on a smooth manifold $\mathcal{M}$ , and $\mathcal{M}\subset {\mathbb{R}}^N$ is the state space. $k$ is the discrete time step. $f$ is the function of state evolution in state space.

The Koopman operator theory provides a method to transform a nonlinear dynamic system into an infinite-dimensional linear system. The Koopman operator $\mathcal{K}:\mathcal{F}\to \mathcal{F}$ is an infinite-dimensional linear operator, which advances the evolution of observables $\mathrm{\Psi }\in \mathcal{F}$ . $\mathcal{F}$ is an invariant function space (usually called observable space) under the action of Koopman operator. Thus,

$$\mathcal{K}\mathrm{\Psi }(x(k))\stackrel{\mathrm{\Delta }}{=}\mathrm{\Psi }(f(x(k)))$$

(2)

where $\mathrm{\Psi }(x)={[{\psi }_1(x),{\psi }_2(x),\dots ,{\psi }_N(x)]}^T$ , ${\psi }_i:\mathcal{M}\to \mathbb{R},(i=1\dots N)$ are user-specified (typically nonlinear) lifting functions. It is important that the operator completely captures all the properties of the underlying dynamical system, as long as the space of the observation $\mathcal{F}$ contains the components of the state $x(k)$ .

Combining equations (1) and (2), it has

$$\mathcal{K}\mathrm{\Psi }(x(k))\stackrel{\mathrm{\Delta }}{=}\mathrm{\Psi }(x(k+1))$$

(3)

In this way, Koopman operator represents a nonlinear system by lifting the state-space to an infinite-dimensional space of observables. Note that (1) is a uncontrolled system, several works have been proposed to extend the Koopman operator to controlled systems.^27–30

Data-driven finite-dimensional approximation to Koopman operator

Since the Koopman operator is infinite-dimension, obtaining finite-dimensional approximations are necessary in practical applications. Extended dynamic mode decomposition (EDMD) is an effective method to obtain the finite-dimensional approximation of the Koopman operator. Specially, EDMD is extended to the controlled system to construct a matrix $K$ , as the finite-dimensional approximation of the Koopman operator $\mathcal{K}$ . ³⁰ In the following, we directly give the results from Milan Korda.

Consider a discrete-time nonlinear controlled dynamical system:

$$x(k+1)=f(x(k),u(k))$$

(4)

where, $u(k)\subset {\mathbb{R}}^m$ is the control input.

For simplicity, one can choose $\mathrm{\Psi }(x(k),u(k))=[{\mathrm{\Psi }}_x^T(x(k)),{\mathrm{\Psi }}_u^T(u(k))]$ . Since there is no need to predict the future control input, choose ${\mathrm{\Psi }}_u(u(k))=u(k)$ . The lifted states are defined as:

$$z(k)={\mathrm{\Psi }}_x(x(k))\equiv [{\psi }_{x1}(x(k)),...,{\psi }_{\mathrm{xN}}(x(k)){]}^T$$

(5)

Then the constructed Koopman model is in the following discrete linear form as:

$$\begin{array}{cc}\multicolumn{1}{c}{z(k+1)}& =\mathrm{Az}(k)+\mathrm{Bu}(k)\hfill \\ \multicolumn{1}{c}{\hat{x}(k)}& =\mathrm{Cz}(k)\hfill \end{array}$$

(6)

where $z(k)\in {\mathbb{R}}^N$ is the lifted state, $N$ is the lifted dimensionality, $\hat{x}(k)$ is an estimate of the original nonlinear system state $x(k)$ , and $A\in {\mathbb{R}}^{N\times N}$ , $B\in {\mathbb{R}}^{N\times m}$ , $C\in {\mathbb{R}}^{n\times N}$ are linear time-invariant matrices.

The matrices $A$ , $B$ , and $C$ in (6) can be obtained by:

$$\underset{A,B}{min}\sum _{k=1}^P\parallel {\mathrm{\Psi }}_x(x(k+1))-A{\mathrm{\Psi }}_x(x(k))-B{\mathrm{\Psi }}_u(u(k)){\parallel }_2^2$$

(7)

$$\underset{C}{min}\sum _{k=1}^P\parallel x(k)-C{\mathrm{\Psi }}_x(x(k)){\parallel }_2^2$$

(8)

The data set formed by the system states and control input of the original nonlinear system is defined as follows:

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

(9)

which satisfy $x(k+1)=f(x(k),u(k))$ . To obtain the matrixs $A,B,C$ , the collected data are lifted as:

$$X_{\mathrm{lift}}=[{\mathrm{\Psi }}_x(x(1)),...,{\mathrm{\Psi }}_x(x(P))]$$

(10)

$$Y_{\mathrm{lift}}=[{\mathrm{\Psi }}_x(x(2)),...,{\mathrm{\Psi }}_x(x(P))]$$

(11)

The numerical solutions of the matrices $A,B,C$ are obtained by substituting the lifted data into (7) and (8):

$$\begin{array}{cc}\multicolumn{1}{c}{[A,B]}& =Y_{\mathrm{lift}}[X_{\mathrm{lift}},U{]}^{†}\hfill \\ \multicolumn{1}{c}{C}& ={\mathrm{XX}}_{\mathrm{lift}}^{†}\hfill \end{array}$$

(12)

So far, the data-driven Koopman model of the original nonlinear system (4) is obtained as (6) and (12).

Building a linear model based on Koopman operator

The OMM prototype consists of a three-wheeled omnidirectional mobile platform and a parallelogram manipulator. The structure of the OMM is shown in Figure 1. Define the state vector of OMM at $k$ time as $\mathbf{x}(k)={[\mathbf{q}{(k)}^T,\stackrel{·}{\mathbf{q}}{(k)}^T]}^T\in {\mathbb{R}}^{10}$ . $\mathbf{q}(k)={[x(k),y(k),\phi (k),{\theta }_1(k),{\theta }_2(k)]}^T\in {\mathbb{R}}^5$ is the position and posture of OMM, where $x(k)$ , $y(k)$ and $\phi (k)$ represents the x-coordinate, y-coordinate, and orientation angle of the mobile platform in the world coordinate system, respectively, while ${\theta }_1(k)$ and ${\theta }_2(k)$ are the two joint angles of the manipulator. $\stackrel{·}{\mathbf{q}}(k)=\frac{\mathbf{q}(k)-\mathbf{q}(k-1)}{\mathrm{\Delta }t}\in {\mathbb{R}}^5$ is the velocity state of the OMM, $\mathrm{\Delta }t$ is the sampling time. $\mathbf{u}(k)={[u_1(k),u_2(k),u_3(k),u_4(k),u_5(k)]}^T\in {\mathbb{R}}^5$ is the control input vector consisting of the voltages of five motors.

Figure 1.

The structure diagram of OMM.

In real applications, when the control input sequence $\mathbf{u}(k)$ is applied to the OMM, the state of OMM $\mathbf{x}(k)$ can then be obtained. By collecting the input/output data, the Koopman model for the OMM can be obtained in the form of (6).

Control system design

As mentioned before, inherent modeling errors are between the Koopman model and the robot, including the unknown external disturbances. In this section, a robust data-driven MPC framework is proposed for the OMM, wherein a disturbance observer is employed to estimated the modeling errors of the Koopman model.

Design of disturbance observer

The Koopman model for OMM with uncertainty can be written as:

$$\begin{array}{cc}\multicolumn{1}{c}{\mathbf{z}(k+1)}& =A\mathbf{z}(k)+B{\mathbf{u}}_t(k)+\mathbf{w}(k)\hfill \\ \multicolumn{1}{c}{\mathbf{y}(k)}& =\overline{C}\mathbf{z}(k)\hfill \end{array}$$

(13)

where $A$ and $B$ are the matrix solved by equation (12), $\mathbf{w}\in {\mathbb{R}}^{5\times 1}$ is the modeling errors between the Koopman model and the robot. $\overline{C}$ is the first five rows of $C$ . Actually, many disturbance observers in the literature can be selected to estimate the modeling errors. In this paper, the disturbance observer proposed by Yang et al. ³¹ is employed, and the discrete form is designed as:

$$\{\begin{array}{c}\mathbf{b}(k)=\hat{\mathbf{w}}(k)-\mathbf{p}(k)\hfill \\ \multicolumn{1}{c}{\mathbf{b}(k+1)=({\mathbf{I}}_{5\times 5}-T_s\mathbf{L})\mathbf{b}(k)}\\ \multicolumn{1}{c}{+T_s\mathbf{L}(-\mathbf{p}(k)-\overline{C}B\mathbf{u}(k))}\end{array}$$

(14)

where $T_s$ is the sampling period, $\hat{\mathbf{w}}(k)$ is the estimated value of the disturbance $\mathbf{w}(k)$ , $\mathbf{b}(k)$ is the state variable of the disturbance observer, $\mathbf{L}$ is the gain of the disturbance observer, $\mathbf{p}(k)$ is the function to be designed related to the system states and is simply set as $\mathbf{p}(k)=\rho \mathbf{y}(k)$ , $\rho$ is the coefficient vector. Actually, the selection of disturbance observer is not limited to the proposed by Yang et al., ³¹ and many disturbance observers can be selected in the proposed control scheme, such as UDE, ²¹ GPIO, ²² and ESO. ²³

Controller design

The designed controller $\mathbf{u}(k)$ includes two parts:

$$\mathbf{u}(k)={\mathbf{u}}_d(k)+{\mathbf{u}}_t(k)$$

(15)

where ${\mathbf{u}}_d(k)$ represents the control efforts for disturbance compensation, and ${\mathbf{u}}_t(k)$ denotes the control efforts for the trajectory tracking control. ${\mathbf{u}}_d(k)$ is designed as:

$${\mathbf{u}}_d(k)=-(\overline{C}B{)}^{-1}\hat{\mathbf{w}}(k)$$

(16)

Combining (13) and (16), the system is reduced as:

$$\begin{array}{cc}\multicolumn{1}{c}{\mathbf{z}(k+1)}& =A\mathbf{z}(k)+B{\mathbf{u}}_t(k)\hfill \\ \multicolumn{1}{c}{\mathbf{y}(k)}& =\overline{C}\mathbf{z}(k)\hfill \end{array}$$

(17)

where ${\mathbf{u}}_t(k)$ can be designed under MPC framework.

The prediction equation of the future states can be described as:

$$\begin{array}{c}\mathbf{z}(k+1|k)=A\mathbf{z}(k)+B{\mathbf{u}}_t(k)\hfill \\ \mathbf{z}(k+2|k)=A\mathbf{z}(k+1|k)+B{\mathbf{u}}_t(k+1|k)\hfill \\ ⋮\hfill \\ \mathbf{z}(k+N_P|k)=A\mathbf{z}(k+N_P-1|k)+B{\mathbf{u}}_t(k+N_P-1|k)\hfill \end{array}$$

(18)

The equation (17) can be written compactly as follows:

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

(19)

wherein:

$$\mathbf{Y}(k)\triangleq {[\mathbf{y}{(k+1|k)}^T,\dots ,\mathbf{y}{\left(k+N_P|k\right)}^T]}^T$$

(20)

$${\mathbf{U}}_t(k)\triangleq {[{\mathbf{u}}_t{(k|k)}^T,\dots ,{\mathbf{u}}_t{(k+N_c-1|k)}^T]}^T$$

(21)

$$\mathbf{F}={[\overline{C}A,\overline{C}{A}^2,\dots ,\overline{C}{A}^{N_p}]}^T$$

(22)

$$\mathbf{\Phi }=\left[\begin{array}{c}\begin{array}{cccc}\overline{C}B\hfill & 0\hfill & \dots \hfill & 0\hfill \\ \multicolumn{1}{c}{\overline{C}\mathrm{AB}}& \overline{C}B\hfill & \dots \hfill & 0\hfill \\ \multicolumn{1}{c}{⋮}& ⋮\hfill & \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{\overline{C}{A}^{N_p-1}B}& \overline{C}{A}^{N_p-2}B\hfill & \dots \hfill & \overline{C}{A}^{N_p-N_c}B\hfill \end{array}\hfill \end{array}\right]$$

(23)

The reference trajectory in the prediction domain is: ${\mathbf{Y}}_r(k)={[{\mathbf{y}}_r{(k+1)}^T,\dots ,{\mathbf{y}}_r{(k+N_p)}^T]}^T$ . The goal of controller design is to minimize the following performance index functions:

$$J={(\mathbf{Y}(k)-{\mathbf{Y}}_r(k))}^T\mathbf{Q}(\mathbf{Y}(k)-{\mathbf{Y}}_r(k))+{\mathbf{U}}_t(k{)}^T\mathbf{R}{\mathbf{U}}_t(k)$$

(24)

where $\mathbf{Q}$ and $\mathbf{R}$ are positive diagonal matrices. The above formula can be transformed as follows:

$$\begin{array}{c}J={(\mathbf{F}z(k)-{\mathbf{Y}}_r(k))}^T\mathbf{Q}(\mathbf{F}z(k)-{\mathbf{Y}}_r(k))\hfill \\ +2{\mathbf{U}}_t(k{)}^T{\mathbf{\Phi }}^T\mathbf{Q}(\mathbf{Fz}(k)-{\mathbf{Y}}_r(k))\hfill \\ +{\mathbf{U}}_t(k{)}^T\left({\mathbf{\Phi }}^T\mathbf{Q}\mathbf{\Phi }\right){\mathbf{U}}_t(k)\hfill \end{array}$$

(25)

Since the first term on the right side of the above formula has no relation with ${\mathbf{u}}_t(k)$ , thus:

$$\begin{array}{cc}\multicolumn{1}{c}{\underset{{\mathrm{U}}_t}{min}\mathbf{J}}& \Rightarrow \underset{{\mathrm{U}}_t}{min}\hat{\mathbf{J}}\hfill \\ \multicolumn{1}{c}{s.t.\mathbf{z}(i+1)}& =A\mathbf{z}(k)+B{\mathbf{u}}_t(k)\hfill \\ \multicolumn{1}{c}{\mathbf{z}}& =\mathrm{\Psi }(x)\hfill \\ \multicolumn{1}{c}{\left|{\mathbf{u}}_i\right|}& \le {\mathbf{u}}_{\max }\hfill \\ \multicolumn{1}{c}{\hat{\mathbf{J}}}& =2{\mathbf{U}}_t(k{)}^T\mathbf{H}+{\mathbf{U}}_t(k{)}^T\mathbf{E}{\mathbf{U}}_t(k)\hfill \end{array}$$

(26)

where

$$\begin{array}{l}\mathbf{H}={\mathbf{\Phi }}^T\mathbf{Q}\left(\mathbf{F}\mathbf{z}(k)-{\mathbf{Y}}_r(k)\right),\mathbf{E}={\mathbf{\Phi }}^T\mathbf{Q}\mathbf{\Phi }+\mathbf{R}\\ \mathbf{Q}=\mathrm{d}iag(\stackrel{N_p-1}{\overbrace{{\mathbf{Q}}_a,{\mathbf{Q}}_a,\dots ,{\mathbf{Q}}_a}},{\mathbf{Q}}_{N_p})\\ \mathbf{R}=\mathrm{d}iag(\stackrel{N_c}{\overbrace{{\mathbf{R}}_c,{\mathbf{R}}_c,\dots ,{\mathbf{R}}_c}})\end{array}$$

Finally, the designed controller can be obtained as:

$$\mathbf{u}(k)=-(\overline{C}B{)}^{-1}\hat{\mathbf{w}}(k)+{\mathbf{u}}_t(k)$$

(27)

where, ${\mathbf{u}}_t(k)$ is solved by (26).

• Remark 1: In the literature, many disturbance observers have been proposed in the past 30 years. Actually, the estimation performance of all disturbance observers depends on the modeling accuracy of the plant. In other words, if the plant model is accurate, a disturbance observer can well estimate the disturbances. In our manuscript, the internal modeling errors of Koopman model (e.g. parameter uncertainties) and external unknown disturbances (such as payload changes) are viewed as lumped disturbances. Therefore, the estimation performance of modeling errors for a disturbance observer depends on the model accuracy of the obtained Koopman model.

Simulation

In this section, effectiveness of the proposed control scheme is compared with Koopman model based MPC (K-MPC) via Matlab. Firstly, a Koopman model is derived from input/output data. Then two typical reference trajectories are selected to compare the two control schemes.

Simulation setup

For data collection in the Koopman modeling, 47 randomly generated trajectories are selected and each trajectory runs for $5s$ , that is, each trajectory generates 500 data points (sampling time is 0.01 s). Thus, the total number of data pairs is 23,500. The dimension of the lifting function is set as 50 dimensions, wherein the first 10 dimensions are the position and speed of the robot. The control signals for the mobile platform are randomly generated and uniformly distributed motor voltages between $-20V$ and $20V$ . The control signals for the manipulator are randomly generated and uniformly distributed motor voltages between $-8V$ and $8V$ . The basis functions are selected as ${\mathrm{\Psi }}_x(x)=\Vert x-x_c{\Vert }^2\log (\Vert x-x_c\Vert )$ , and $x_c$ is a constant vector with the elements uniformly distributed in a given interval. Here, $x_c$ = [−0.6848, 0.9412, 1.3715, −0.0439, 0.09008, −0.3581, −0.0782, 0.8315, 1.7532, 2.7570]^T.

Since both control schemes are designed based on MPC, the control parameters for MPC are set as the same for both control schemes: ${\mathbf{Q}}_a=0.1{\mathbf{I}}_{10\times 10}$ , ${\mathbf{Q}}_{N_p}=100{\mathbf{I}}_{10\times 10}$ , ${\mathbf{R}}_c=1\times {10}^{-6}{\mathbf{I}}_{5\times 5}$ , $N_p=N_c=5$ . The parameters for the disturbance observer are set as: $\mathbf{p}(k)={[0.001x(k),0.001y(k),0.001\phi (k),0.01{\theta }_1(k),0.01{\theta }_2(k)]}^T$ , $\mathbf{L}=\mathrm{diag}(50,50,50,10,10)$ . The sampling period of the control system is $0.01s$ .

Two typical reference trajectories are selected in the simulations. For the first trajectory, the mobile platform tracks a circle with a radius of $0.8m$ , and the joint angles of the manipulator respectively track two sinusoidal trajectories. The initial position and posture of the robot were set as ${\mathbf{q}}_0=[0.8m,0m,0\mathrm{rad},0\mathrm{rad},0\mathrm{rad}{]}^T$ . And the simulation time is $60s$ . In order to verify the robustness of the proposed control scheme against external disturbances, an external step disturbance $d_1$  = [2000, 2000, 500, 50, 50] ^T was added within 5–15s and an external time-varying disturbance $d_2$  = [2000sin(t), 2000cos(t), 1000sin(t), 50sin(t), 50cos(t)] ^T was added within 40–50s. The first reference trajectory is as follows:

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

(28)

In the second trajectory, the mobile platform tracks a square trajectory, and the joint angles of the manipulator respectively track two sinusoidal trajectories. The initial position of the robot is as ${\mathbf{q}}_0=[0m,0m,0\mathrm{rad},0\mathrm{rad},0\mathrm{rad}{]}^T$ in the world coordinate system, and the simulation time is $40s$ . Similarly, an external disturbance $d_3$  = [1000, 1000, 500, 50, 50]^T was added within 10–15s and $d_4$  = [1000sin(t), 1000cos(t), 1000sin(t), 50sin(t), 50cos(t)]^T was added within 25–30s, respectively. The second reference trajectory is as follows:

$$\begin{array}{c}\{\begin{array}{c}{x}_d\left[\mathrm{m}\right]=0.1\mathrm{t}\hfill \\ \multicolumn{1}{c}{y_d\left[\mathrm{m}\right]=0}\\ \multicolumn{1}{c}{{\phi }_d\left[\mathrm{rad}\right]=0(0s\le t\le 10s)}\\ \multicolumn{1}{c}{{\theta }_{1d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\\ \multicolumn{1}{c}{{\theta }_{2d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\end{array}\hfill \\ \{\begin{array}{c}{x}_d\left[\mathrm{m}\right]=1\hfill \\ \multicolumn{1}{c}{y_d\left[\mathrm{m}\right]=0.1(t-10)}\\ \multicolumn{1}{c}{{\phi }_d\left[\mathrm{rad}\right]=0.1(t-10)(10s\le t\le 20s)}\\ \multicolumn{1}{c}{{\theta }_{1d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\\ \multicolumn{1}{c}{{\theta }_{2d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\end{array}\hfill \\ \{\begin{array}{c}{x}_d\left[\mathrm{m}\right]=1-0.1(t-20)\hfill \\ \multicolumn{1}{c}{y_d\left[\mathrm{m}\right]=1}\\ \multicolumn{1}{c}{{\phi }_d\left[\mathrm{rad}\right]=1(20s\le t\le 30s)}\\ \multicolumn{1}{c}{{\theta }_{1d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\\ \multicolumn{1}{c}{{\theta }_{2d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\end{array}\hfill \\ \{\begin{array}{c}{x}_d\left[\mathrm{m}\right]=0\hfill \\ \multicolumn{1}{c}{y_d\left[\mathrm{m}\right]=1-0.1(t-30)}\\ \multicolumn{1}{c}{{\phi }_d\left[\mathrm{rad}\right]=1+0.1(t-30)(30s\le t\le 40s)}\\ \multicolumn{1}{c}{{\theta }_{1d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\\ \multicolumn{1}{c}{{\theta }_{2d}\left[\mathrm{rad}\right]=0.6\sin \left(\frac{\mathrm{\pi }t}{20}\right)}\end{array}\hfill \end{array}$$

(29)

In order to quantitatively compare the control performance of the two control algorithms, this paper uses the absolute error integral (IAE) as the evaluation index, which is defined as follows:

$$\{\begin{array}{c}\mathrm{I}\mathrm{AE}\mathrm{xy}\left[\mathrm{m}\right]={\int }_0^T(\left|e_x\right|+\left|e_y\right|)\mathrm{dt}\hfill \\ \multicolumn{1}{c}{\mathrm{I}\mathrm{AE}\phi \left[\mathrm{rad}\right]={\int }_0^T\left(\left|e_{\phi }\right|\right)\mathrm{dt}}\\ \multicolumn{1}{c}{\mathrm{I}\mathrm{AE}{\theta }_1{\theta }_2\left[\mathrm{rad}\right]={\int }_0^T(\left|e_{{\theta }_1}\right|+\left|e_{{\theta }_2}\right|)\mathrm{dt}}\end{array}$$

(30)

Simulation results

The simulation results are shown in Figures 2 and 3, respectively. The IAE of simulation results is shown in Tables 1 and 2, respectively. It can be seen that the proposed control scheme can achieve much better performance than Koopman-based MPC, especially when the external disturbances were added. More specifically, the control performance of the proposed control scheme keeps almost unchanged and shows strong robustness against external disturbances. This is because the external disturbances can be online estimated and compensated in the proposed control scheme. Figures 2(d) and 3(d) show the disturbance estimation performance.

Figure 2.

Simulation results of the first reference trajectory: the proposed control scheme and Koopman model based MPC (K-MPC): (a) tracking performance, (b) tracking errors, (c) control input, and (d) estimated disturbances.

Figure 3.

Simulation results of the second reference trajectory: the proposed control scheme and Koopman model based MPC (K-MPC): (a) tracking performance, (b) tracking errors, (c) control input, and (d) estimated disturbances.

Table 1.

Simulation results: IAE of the first trajectory.

Control algorithm	$\mathrm{I}\mathrm{AE}\mathrm{xy}\left[\mathrm{m}\right]$	$\mathrm{I}\mathrm{AE}\phi \left[\mathrm{rad}\right]$	$\mathrm{I}\mathrm{AE}{\theta }_1{\theta }_2\left[\mathrm{rad}\right]$
K-MPC	0.4724	0.3313	1.7231
Proposed	0.1547	0.0593	0.2235

Table 2.

Simulation results: IAE of the second trajectory.

Control algorithm	$\mathrm{I}\mathrm{AE}\mathrm{xy}\left[\mathrm{m}\right]$	$\mathrm{I}\mathrm{AE}\phi \left[\mathrm{rad}\right]$	$\mathrm{I}\mathrm{AE}{\theta }_1{\theta }_2\left[\mathrm{rad}\right]$
K-MPC	0.3046	0.1749	0.9940
Proposed	0.0978	0.0314	0.4238

Experiment

In this section, two experimental scenarios are carried out. In the first experimental scenario, the proposed control scheme is compared with Koopman model based MPC scheme, to verify the effectiveness in terms of compensation of modeling error. To further compare the robustness of the two control schemes, a payload is added at the end of the manipulator in the second experimental scenario.

Implementation

Figure 4 shows the robot prototype. The experimental setup is shown in Figure 5. MATLAB is used on a personal computer (Intel(R) Core(TM) i7-4770 CPU@3.40 GHz) to generate the control input signal (motor voltages). The control signal is sent to the control board on the OMM via a WIFI module. The position and posture feedback in this paper is realized by OptiTrack real time tracking system. As show in Figure 4, four markers are fixed on the top of the OMM mobile platform, and three markers are fixed on the manipulator.

Figure 4.

Robot prototype.

Figure 5.

Experimental setup.

In the experiments, a total of 57 groups of data were collected, in which 50 groups were used to calculate the Koopman model, and the last 7 groups were used to test the approximation accuracy of the Koopman model. Each group contains 200 sampling points and the sampling time is 0.025 s. That is, the final data set contains 11,400 data points. The basis function is the same as that in the simulations. The second reference trajectory used in the simulations was employed in the experimental tests. The control parameters for both control schemes were set as:

$$\begin{array}{c}{N}_p=N_c=15,{\mathbf{Q}}_a=\mathrm{diag}(3,5,5,5,5)\hfill \\ {\mathbf{Q}}_{\mathrm{Np}}=\mathrm{diag}(265,300,320,480,480)\hfill \\ {\mathbf{R}}_c{=10}^{-5}\mathrm{diag}(8,8,8,5,5)\hfill \\ \mathbf{L}=\mathrm{diag}(1,1,1,0.05,0.05)\hfill \\ \mathbf{p}(k)=0.001[x(k),y(k),\phi (k),{\theta }_1(k),{\theta }_2(k{)]}^T\hfill \end{array}$$

(31)

Experimental scenarios

In the first experimental scenario, the proposed control scheme is compared with Koopman model based MPC scheme without any external payload disturbances, to demonstrate the effectiveness in compensating modeling errors. Experimental results are shown in Figure 6.

Figure 6.

Experimental tests without external payload: the proposed control scheme and Koopman model based MPC (K-MPC): (a) tracking performance, (b) tracking errors, (c) control input, and (d) estimated disturbances.

It is shown that the proposed control scheme can achieve better control performance than Koopman model based MPC scheme, especially for the mobile platform of OMM. Actually, it is found in our simulations and experiments that the Koopman model can well predict the robot states, if the OMM moves without rotational motion. However, when the OMM moves with rotational motion, the Koopman model results in large modeling errors due to couplings in the dynamics. Moreover, the model accuracy of Koopman model for any systems depends on the selection of basis functions, dimension and data distribution. Therefore, the modeling error of the obtained Koopman model results from both internal uncertainties and external disturbances.

On the other hand, it is known that the performance of MPC scheme depends on the modeling accuracy. However, the disturbance observer can well estimate the modeling errors and compensate them in the control signal. Therefore, the control performance of the proposed control scheme can be guaranteed. Figure 6(d) show the estimated disturbances. Table 3 shows the IAE of experimental results.

Table 3.

IAE of experimental tests without payload.

Control algorithm	$\mathrm{I}\mathrm{AE}\mathrm{xy}\left[\mathrm{m}\right]$	$\mathrm{I}\mathrm{AE}\phi \left[\mathrm{rad}\right]$	$\mathrm{I}\mathrm{AE}{\theta }_1{\theta }_2\left[\mathrm{rad}\right]$
K-MPC	2.8474	0.9888	1.5679
Proposed	1.5470	0.4355	1.5664

In the second experimental scenario, a payload (2.53 kg) is added at the end of the manipulator, as shown in Figure 7. To reduce the shaking of the manipulator in experiments, the reference angle of ${\theta }_{1d}$ was set as 0 $\mathrm{rad}$ . The experimental results are shown in Figure 8. Since the payload was added at the end of the manipulator, the payload disturbance is mainly introduced to the dynamics of manipulator (i.e. ${\theta }_1$ and ${\theta }_2$ ). As shown in Figure 8(b), distinct performance improvement is achieved by the proposed control scheme, due to the effective compensation of the external payload. Note that the data collection in the Koopman modeling does not include the case with external payload. Therefore, the derived Koopman model cannot capture the payload information and the control performance cannot be guaranteed against external disturbances. Figure 8(d) show the estimated disturbances. Table 4 shows the IAE of experimental results.

Figure 7.

Experimental setup with payload.

Figure 8.

Experimental tests with external payload: the proposed control scheme and Koopman model based MPC (K-MPC): (a) tracking performance, (b) tracking errors, (c) control input, and (d) estimated disturbances.

Table 4.

IAE of experimental tests with payload.

Control algorithm	$\mathrm{I}\mathrm{AE}\mathrm{xy}\left[\mathrm{m}\right]$	$\mathrm{I}\mathrm{AE}\phi \left[\mathrm{rad}\right]$	$\mathrm{I}\mathrm{AE}{\theta }_1{\theta }_2\left[\mathrm{rad}\right]$
K-MPC	3.0646	1.3011	4.6519
Proposed	1.3576	0.5345	1.5388

The dimension selection about Koopman model in terms of modeling accuracy is unclear, mainly depending on trial-and-error. Actually, it is found in our simulations that the higher dimension does not mean higher modeling accuracy. Moreover, there has not been a systematic way of choosing the Koopman basis functions for a given nonlinear systems. But the basis functions of Koopman operator are quite important in improving the modeling accuracy of Koopman model.

The horizon P in (7) means the number of the collected data pairs. In our experiments, it is found that P should be relative larger. However, the model accuracy of Koopman model does not improve as the increase of the horizon P. The collected data should be rich enough to include the complex nonlinear dynamics. Therefore, the control input and initial states for the data collection are random signals with uniform distributions in our simulations and experiments.

Conclusion

In this paper, a robust data-driven control scheme has been designed and implemented for an OMM, based on Koopman operator and a disturbance observer. Firstly, a high dimensional Koopman model has been constructed for the OMM, solely using the collected input-output data from experiments. Then a disturbance observer has been selected to online estimate the modeling error as well as external disturbances. The controller has been designed by combining linear MPC with feedback compensation of the estimated disturbances. Finally, both simulations and experimental tests demonstrate that the proposed control scheme achieves much better control performance, compared with Koopman model based MPC scheme.