# Policy Learning for Nonlinear Model Predictive Control with Application to
USVs

Rizhong Wang, Huiping Li, Bin Liang, , Yang Shi, and Demin Xu R. Wang, H. Li
and D. Xu are with School of Marine Science and Technology, Northwestern
Polytechnical University, Xi’an 710072, China (e-mail:
<EMAIL_ADDRESS><EMAIL_ADDRESS>xudm@nwpu.edu.cn); B.
Liang is with Tsinghua University (bliang@tsinghua.edu.cn); Y. Shi is with
Department of Mechanical Engineering, University of Victoria
(email:yshi@uvic.ca).National Natural Science Foundation of China (NSFC) under
Grant 61922068, 61733014; Shaanxi Provincial Funds for Distinguished Young
Scientists under Grant 2019JC-14; Aoxiang Youth Scholar Program under Grant.

###### Abstract

The unaffordable computation load of nonlinear model predictive control (NMPC)
has prevented it for being used in robots with high sampling rates for
decades. This paper is concerned with the policy learning problem for
nonlinear MPC with system constraints, and its applications to unmanned
surface vehicles (USVs), where the nonlinear MPC policy is learned offline and
deployed online to resolve the computational complexity issue. A deep neural
networks (DNN) based policy learning MPC (PL-MPC) method is proposed to avoid
solving nonlinear optimal control problems online. The detailed policy
learning method is developed and the PL-MPC algorithm is designed. The
strategy to ensure the practical feasibility of policy implementation is
proposed, and it is theoretically proved that the closed-loop system under the
proposed method is asymptotically stable in probability. In addition, we apply
the PL-MPC algorithm successfully to the motion control of USVs. It is shown
that the proposed algorithm can be implemented at a sampling rate up to $5Hz$
with high-precision motion control. The experiment video is available
via:https://v.youku.com/v_show/id_XNTkwMTM0NzM5Ng==.html.

###### Index Terms:

policy learning, model predictive control, deep neural networks, constraints,
nonlinear systems, unmanned surface vessels (USVs).

##  I Introduction

As one of the most advanced control technologies, model predictive control
(MPC) has been widely used in process control[1, 2]. Recently, MPC strategies
have become popular to solve robot control problems because of its optimal
control performance and the ability to handle complex system constraints [3,
4, 5]. However, MPC requires solving an optimal control problem (OCP)
periodically online, which is time consuming and cannot guarantee the real-
time implementation in robot control applications. This brings great
challenges to the application of MPC in the field of robotics[6, 7].

In order to improve the online computational efficiency of OCP, several
improved MPC algorithms have been proposed. In [8], the sequential quadratic
programming (SQP) optimization strategy is adopted to decrease the
computational time of OCP in nonlinear MPC (NMPC). The works in [9] and [10]
simplify the nonlinear quadruped robot model to a linear model. In that
framework, the original NMPC optimization problem was replaced by a quadratic
program (QP) problem, which can greatly reduce computational burden. In [5], a
penalty term is added to the cost function to relax the inequality constraints
of OCP, which improves the OCP efficiency.

Another approach to accelerate solution efficiency of OCP is to perform the
online MPC optimization process offline. Explicit MPC is a typical method of
is this regard [11, 12]. The works in [13] and [14] obtain the explicit
solution of the linear MPC optimal problem by solving the multi-parameter QP
problem or using the piecewise affine function, respectively. In [15], an
approximate explicit MPC control policy is constructed using the barycentric
interpolation method, and the system stability and feasibility of the
algorithm is theoretically guaranteed. Note that all the above algorithms are
only suitable for linear systems. For the non-convex OCP corresponding to the
nonlinear system, the explicit MPC is no longer available because of the high
computational complexity.

Recently, the learning MPC has become a promising approach to handle OCP based
on machine learning [16]. Since the control accuracy of MPC heavily relies on
the accuracy of system model, the works in [17, 18, 19] focused on learning
the system model for MPC. Besides, machine learning methods can also assist
the design of MPC controller. For example, the works in [20, 21, 22] used a
machine learning method to design the cost function. Alternatively, the work
in [23] used the machine learning method to obtain a terminal region of MPC.
Besides, machine learning algorithms can also learn the best system
constraints of MPC[24, 25]. On the other hand, the MPC can provide expert data
for machine learning algorithms. The work in [26] used MPC to guide
reinforcement learning (RL) training to improve the training efficiency of RL
and guarantee the safety of the RL results.

Different from the traditional machine learning methods, deep neural networks
(DNN) have strong capability to learn complex functions and policies. It can
be implemented with faster computational speed of forward propagation after
appropriate training, without solving complex optimization problems online.
Therefore, a promising method to solve the real-time implementation issue of
MPC is to use DNNs to learn the control policy offline and then deploy it
online. In [27], the primal-dual DNN is used to learn the approximate policy
of MPC. To improve the speed of solving large-scale linear MPC, the work in
[28] used DNNs to learn the optimal solution, and the system stability and
feasibility of optimization problem was guaranteed by primal active sets. The
work in [29] utilized the DNN to approximate control policy of robust MPC, and
the feasibility and stability of the closed-loop system is also analyzed. In
[30], constraint items are added to the traditional DNN to ensure that the
approximate policy satisfies the constraints. However, most of the above works
are developed for linear systems, which are not suitable for nonlinear systems
such as robotic and vehicle systems. In addition, seldom have been implemented
and verified via hardware systems.

In this paper, we propose a new policy learning MPC (PL-MPC) scheme for
constrained nonlinear systems using DNN and implement it to the motion control
of USVs. The main contributions of the paper include:

  * •

We propose a deep supervised learning method to learn the policy of the
constrained nonlinear MPC offline to greatly reduce the computational load.
The detailed PL-MPC algorithm, the design of the DNN model and its training
methods are developed, which provides a feasible tool to deploy nonlinear MPC
for robotic control and planning in real-time application.

  * •

We conduct rigorous analysis of the proposed PL-MPC algorithm to make the
proposed method theoretically valid. The proximal operator-based optimization
and the quadratic penalty method are developed to ensure the practical
feasibility of the proposed algorithm. With the PL-MPC algorithm, the closed-
loop system is proved to be asymptotically stable in probability under mild
conditions.

  * •

We implement the developed PL-MPC algorithm successfully to the motion control
of an underactuated USV via lake experiments. The experimental results show
that the PL-MPC algorithm can run up to the sampling rate of $5Hz$, and the
control performance is almost the same as the ideal NMPC in simulation. This
verifies the effectiveness and advantage of the proposed policy learning MPC
method for robotic.

The rest of this paper is organized as follows: Section II presents the
preliminaries and Section III discusses the detailed PL-MPC algorithm. In
Section IV, the feasibility of the algorithm and the stability of the closed-
loop system are presented. Then real-time experiments on USV motion control
with the proposed method are conducted in Section V. Finally, Section VI draws
the conclusions.

Notations: In this paper, the following notations are used. The quadratic norm
$\bm{x}^{T}\bm{Q}\bm{x}$ is denoted by
$\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}$. $[\cdot]$ represents the integer
operation, for example, $[x]$ denotes the integer part of $x$. $[x]^{+}$
denotes the function $\max(x,0)$, that is, if $x<0$, it has $[x]^{+}=0$,
otherwise, $[x]^{+}=x$. If $X$ is a random variable, $P\\{X>x\\}$ denotes the
probability of $X>x$. $E[X]$ and $V[X]$ represent the expectation and variance
of the random variable $X$, respectively.

##  II Preliminaries

###  II-A System Model

Consider a general discrete-time nonlinear dynamic system:

$\bm{x}(k+1)=\bm{f}(\bm{x}(k),\bm{u}(k)).$ (1)

Here, $\bm{x}(k)\in\mathbb{R}^{n}$ and $\bm{u}(k)\in\mathbb{R}^{m}$ are the
system state and the control input, respectively. The system state and control
input are constrained by the compact sets $\mathcal{X}$ and $\mathcal{U}$ as
follows:

$\displaystyle\mathcal{U}=\left\\{\bm{u}(k)\in\mathbb{R}^{m}|\bm{u}_{\min}\leq\bm{u}(k)\leq\bm{u}_{\max}\right\\},$
(2)
$\displaystyle\mathcal{X}=\left\\{\bm{x}(k)\in\mathbb{R}^{n}|\bm{g}(\bm{x}(k))\leq\bm{0}\right\\}.$

We make the following assumptions about sets $\mathcal{U}$ and $\mathcal{X}$.

###### Assumption 1

$\mathcal{X}$ is a connected set, and $(\bm{0},\bm{0})$ is included within
$\mathcal{U}\times\mathcal{X}$.

To facilitate the controller design, the following assumption is made for the
system in (1).

###### Assumption 2

The function
$\bm{f}(\cdot,\cdot):\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{n}$ is
second-order continuous differentiable, and $\bm{0}\in\mathbb{R}^{n}$ is an
equilibrium point of the system, i.e., $\bm{f}(\bm{0},\bm{0})=\bm{0}$.

###  II-B Optimal Control Problem

For MPC algorithm, the optimization object is a quadratic penalty on the error
between predicted system state and control input with the target system state
and control input. Without loss of generality, we choose the equilibrium point
as the target system state and control input. The cost function is designed as

$\displaystyle J(\bm{x}(k),\bar{\bm{u}}(\cdot;k))$ (3) $\displaystyle=$
$\displaystyle\sum_{i=0}^{N-1}{F(\bar{\bm{x}}(k+i;k),\bar{\bm{u}}(k+i;k))}+V_{f}(\bar{\bm{x}}(k+N;k))$
$\displaystyle=$
$\displaystyle\sum_{i=0}^{N-1}({\left\|{{\bar{{\bm{x}}}}(k+i;k)}\right\|_{{Q}}^{2}+\left\|{{{\bar{\bm{u}}}}(k+i;k))}\right\|_{{R}}^{2}})$
$\displaystyle+\left\|{{{\bar{\bm{x}}}}(k+N;k)}\right\|_{{P}}^{2},$

where $\bar{\bm{u}}(k+i;k)$ and $\bar{\bm{x}}(k+i;k)$ are control input
sequence and system state sequence under cost function emanating from
$\bm{x}(k)$. $N$ represents the step of the MPC algorithm in predicting the
future state, which is called prediction horizon. $Q\geq 0$, $R\geq 0$ and
$P\geq 0$ represent the adjustable weighting matrices of MPC.

Then, OCP is formulated as follows:

Problem $\mathcal{P}1$ :

$\displaystyle{{\bm{u}}^{*}}(\cdot;k)=\arg$
$\displaystyle\min_{{\bar{\bm{u}}}(\cdot;k)}J(\bm{x}(k),\bar{\bm{u}}(\cdot;k))$
$\displaystyle s.t.$
$\displaystyle{{{\bar{\bm{x}}}}}(k+i+1;k)=\bm{f}({{\bar{\bm{x}}}}(k+i;k),{{\bar{\bm{u}}}}(k+i;k))$
$\displaystyle{{\bar{\bm{u}}}}(k+i;k)\in\mathcal{U}$
$\displaystyle{{\bar{\bm{x}}}}(k+i;k)\in\mathcal{X}$
$\displaystyle{{\bar{\bm{x}}}}(k+N;k)\in\mathcal{X}_{f},$

where $i=0,...,N-1$,
$\mathcal{X}_{f}=\\{\bm{x}\in\mathbb{R}^{n}|\bm{x}^{T}\bm{P}\bm{x}\leq\alpha\\}\subseteq\mathcal{X}$
is the terminal region,
${\bm{u}}^{*}(\cdot;k)=\\{{\bm{u}}^{*}(k;k),{\bm{u}}^{*}(k+1;k),...,{\bm{u}}^{*}(k+N-1;k)\\}$
is the optimal control input sequence.

The following standard assumption is made.

###### Assumption 3

For a system terminal state $\bm{x}\in\mathcal{X}_{f}$, there exists a local
stabilizing control policy $\bm{\kappa}(\bm{x})\in\mathcal{U}$ satisfying

$\displaystyle{V_{f}}({f}({{\bm{x}}},\bm{\kappa}({\bm{x}})))-{V_{f}}({{\bm{x}}})\leq-F({\bm{x}},{\bm{\kappa}({\bm{x}})}),$
(4)

with
$F(\bm{x},\bm{\kappa}(\bm{x}))={\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}+\left\|{{{\bm{\kappa}(\bm{x})}}}\right\|_{{R}}^{2}}$.

Here, given the initial system state $\bm{x}(k)$ and a control sequence
$\bm{U}(k)=\bar{\bm{u}}(\cdot;k)$, the state sequence
$\bar{\bm{x}}(k+i;k)=\bm{\phi}(i,\bm{U}(k),\bm{x}(k))$ satisfies:

$\displaystyle\bm{\phi}(i,\bm{U}(k),\bm{x}(k))$ (5)
$\displaystyle=\bm{f}(...\bm{f}(\bm{x}(k),\bm{\mu}(0,\bm{U}(k))),...,\bm{\mu}(i-1,\bm{U}(k))),$

where $i=0,...,N-1$ and $\bm{\mu}(i,\bm{U}(k))=\bar{\bm{u}}(k+i;k)$.

On this basis, we rewrite problem $\mathcal{P}1$ as the following compact form
to facilitate discussions:

Problem $\mathcal{P}2$ :

$\displaystyle\bm{U}^{*}(k)=\arg$
$\displaystyle\min_{\bm{U}(k)}J(\bm{x}(k),\bm{U}(k))$ $\displaystyle{\rm
s.t.}$ $\displaystyle\bm{G}(\bm{x}(k),\bm{U}(k))\leq\bm{0}$
$\displaystyle{{\bar{\bm{x}}}}(k+N;k)\in\mathcal{X}_{f}.$

According to (2), $\bm{G}(\bm{x}(k),\bm{U}(k))$ is defined as:

$\bm{G}(\bm{x}(k),\bm{U}(k))=\left(\begin{gathered}\bm{g}(\bm{\phi}(0,\bm{U}(k),\bm{x}(k)))\\\
\vdots\\\ \bm{g}(\bm{\phi}(N-1,\bm{U}(k),\bm{x}(k)))\\\
\bm{\mu}(0,\bm{U}(k))-\bm{u}_{max}\\\ \vdots\\\
\bm{\mu}(N-1,\bm{U}(k))-\bm{u}_{max}\\\ \bm{u}_{min}-\bm{\mu}(0,\bm{U}(k))\\\
\vdots\\\ \bm{u}_{min}-\bm{\mu}(N-1,\bm{U}(k))\\\ \end{gathered}\right)$ (6)

Figure 1: Feedforward neural networks.

###  II-C Feedforward Neural Network

This paper adopts feedforward neural networks (FNN), a type of widely used DNN
model, to learn the control policy of NMPC. As shown in figure 1, FNN provides
a L-layer neural networks architecture, which forms a mapping from the input
$\bm{z}$ to the output $\bm{f_{\theta}}(\bm{z})$. The detailed formulation is
as follows:

$\displaystyle\bm{f_{\theta}}(\bm{z})=\bm{h}_{L}(\bm{h}_{L-1}(\cdots\bm{h}_{2}(\bm{h}_{1}(\bm{z})))),$
(7)

with the function

$\displaystyle\bm{h}_{l}(\bm{z})=\delta(\bm{\omega}_{\theta}^{l}\bm{z}+\bm{b}_{\theta}^{l}).$
(8)

Here, $\delta$ is the activation function of DNN. The commonly used activation
functions are: sigmoid function $\delta(x)=1/(1+e^{(-x)})$, tanh function
$\delta(x)=(2/(1+e^{(-2x)}))-1$ and ReLU function $\delta(x)=\max(0,x)$ [31].

To fitting a function $\bm{f}(\bm{z})\in\mathcal{F}$ with the given training
data $\\{\bm{z}^{(i)},\bm{f}(\bm{z}^{(i)})\\}_{i=1}^{M}$, the DNN optimizes
its parameter
$\bm{\theta}:=\\{\bm{\omega}_{\theta}^{l},\bm{b}_{\theta}^{l}\\}_{l=1}^{L}$ in
(8) by minimizing a designed loss function $\mathcal{L}(\cdot)$. It has been
proven that the multilayer FNN can approximate any finite discontinuous
functions by an arbitrary accuracy[32, 33]. That is, DNN is a universal
approximator for Borel measurable function, which is suitable for the NMPC
policy learning task in this paper.

##  III POLICY Learning MPC

###  III-A Policy Learning MPC

In the framework of NMPC, Problem $\mathcal{P}2$ is recursively solved while
the new system state $\bm{x}(k)$ is obtained, and only the first element of
$\bm{U}^{*}(k)$ is applied to the system. Therefore, we can establish a
function map between the system state $\bm{x}(k)$ and $\bm{U}^{*}(k)$ as
follows:

$\displaystyle\bm{U}^{*}(k)=\bm{\pi}(\bm{x}(k)).$ (9)

Different from the traditional NMPC algorithms utilizing the online
optimization method, we use the DNN to fit $\bm{\pi}(\bm{x}(k))$ by
$\bm{\pi_{\theta}}(\bm{x}(k))$ and denote

$\displaystyle\bm{U_{\theta}}(k)=\bm{\pi_{\theta}}(\bm{x}(k)),$ (10)

where
$\bm{U_{\theta}}(k)=\\{\bm{u_{\theta}}(k;k),\bm{u_{\theta}}(k+1;k),...,\bm{u_{\theta}}(k+N-1;k)\\}$.
As introduced in Section II, the parameter $\bm{\theta}$ of DNN is
$\\{\bm{\omega}_{\theta}^{l},\bm{b}_{\theta}^{l}\\}_{l=1}^{L}$. The detailed
policy learning processes for NMPC are shown in Fig. 2.

Figure 2: policy Learning for MPC.

The loss function of deep learning is designed as

$\displaystyle\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})=J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),\bm{U}^{*}(k)),$
(11)

so that it can approximate $\bm{\pi}(\bm{x}(k))$ more accurately.

It is straightforward that the loss function
$\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})\geq 0$ because of the
optimality property of $\bm{U}^{*}(k)$. By utilizing the training data set
$\mathcal{D}=\\{(\bm{x}(k),\bm{U}^{*}(k)\\}_{k=1}^{M}$ and the loss function
$\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})$, the supervised learning
problem of $\bm{U_{\theta}}(k)$ is converted into the following optimization
problem:

$\displaystyle{{\bm{\theta}}^{*}}=\arg$
$\displaystyle\min_{{{\bm{\theta}}}}\sum_{k=1}^{M}\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)}).$
(12)

Considering the constraints of system defined in (2), we obtain the following
optimization problem:

Problem $\mathcal{P}3$ :

$\displaystyle{{\bm{\theta}}^{*}}=\arg$
$\displaystyle\min_{{{\bm{\theta}}}}\sum_{k=1}^{M}\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})$
$\displaystyle s.t.$
$\displaystyle\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))\leq\bm{0}.$

###  III-B Dual Optimization Learning Algorithm

Traditional learning algorithms only use the unconstrained optimization
problems to update $\bm{\theta}$, which may lead to the failure of the learned
control policy in practical implementation because of violating constraints.
To deal with this problem, we fulfil the system state and input constraints in
$\mathcal{P}3$ by means of Lagrange duality theory[34].

First, an augmented Lagrangian loss function is defined as follows:

$\displaystyle\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)},\bm{v})$ (13)
$\displaystyle=$
$\displaystyle\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})+{\bm{v}}[\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))]^{+},$

where $\bm{v}=[v_{1},...v_{3N}]$ is the dual variable associated with the
constraint $\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))$.

Then, the dual form of problem $\mathcal{P}3$ is:

Problem $\mathcal{P}4$ :

$\displaystyle{\bm{\theta}}^{*}(\bm{v})=\arg\min_{{{\bm{\theta}}}}\sum_{k=1}^{M}\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)},\bm{v}),$
$\displaystyle{\bm{v}}^{*}=\arg\max_{\bm{v}}\min_{\bm{\theta}}\sum_{k=1}^{M}\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)},\bm{v}).$

Finally, for Problem $\mathcal{P}4$, we integrate the dual gradient descent
algorithm and back propagation algorithm to solve it. The concrete steps of
the dual optimization learning algorithm is described in Algorithm 1.

Note that $\bm{v}$ and $\bm{\theta}$ are designed as an alternative update
process in the framework of Algorithm 1. The procedure will be finished once
the stopping criteria are satisfied, which means that $\bm{v}$ may not
converge to the optimal value at the end of iteration. Therefore, although the
existence of the dual variable $\bm{v}$ encourages parameter $\bm{\theta}$ to
converge to the approximate optimal value while satisfying the constraints in
the process of updating, the non-optimal value $\bm{v}$ would lead to a slight
violation of the constraints in probability. This issue will be further
discussed and tackled in Section IV-A.

Algorithm 1 Dual Optimization Learning Algorithm

0: Initial training dataset
$\mathcal{D}=\\{(\bm{x}(k),\bm{U}^{*}(k)\\}_{k=1}^{M}$, initial DNN parameter
$\bm{\theta}$, initial optimal step size of DNN $\varepsilon$, initial dual
variable $\bm{v}$, initial dual updated step size
$\alpha=\\{\alpha_{0},\alpha_{1}...\\}$.

for each $j=0,1,2,...$ do

while Loss function gradient greater than expected do

Randomly sample $m$ small batch samples
$\\{(\bm{x}(1),\bm{U}^{*}(1)),...,(\bm{x}(m),\bm{U}^{*}(m))\\}\in\mathcal{D}$

Calculate the output of the DNN in system state $\bm{x}(k)$:
$\bm{U_{\theta}}(k)=\bm{\pi_{\theta}}(\bm{x}_{k})$

Calculate $J(\bm{x}(k),\bm{U_{\theta}}(k))$ and $J(\bm{x}(k),\bm{U}^{*}(k))$
using (1) and (3).

Calculate $\mathcal{L}({\bm{U}^{*}(k)},{\bm{U_{\theta}}(k)})$ using (11).

Convert the dual form
$\mathcal{L}({\bm{U}^{*}(k)},\bm{U_{\theta}}(k),\bm{v}^{j})$ using to (13).

Calculate the average loss functions of $m$ Samples
$\frac{1}{m}\sum_{k=0}^{m}\mathcal{L}({\bm{U}^{*}(k)},\bm{U_{\theta}}(k),\bm{v}^{j})$.

Calculate the gradient of the average loss functions
$\nabla_{\bm{\theta}}\frac{1}{m}\sum_{k=0}^{m}\mathcal{L}({\bm{U}^{*}(k)},\bm{U_{\theta}}(k),\bm{v}^{j})$
for parameter $\bm{\theta}$.

Update parameters
$\bm{\theta}\leftarrow\bm{\theta}-\varepsilon\nabla_{\bm{\theta}}\frac{1}{m}\sum_{k=0}^{m}\mathcal{L}({\bm{U}^{*}(k)},\bm{U_{\theta}}(k),\bm{v}^{j})$

end while

Update the Lagrangian dual variable $v_{i}^{j+1}\leftarrow
v_{i}^{j}+\alpha_{j}\sum_{k=1}^{n}[\bm{G}_{i}(\bm{x}(k),\bm{U_{\theta}}(k))]^{+}\quad
i\in\\{1,2,...,3N\\}$

end for

###  III-C PL-MPC Algorithm

The approximate control policy $\bm{\pi_{\theta}}(\bm{x}(k))$ is learned
through the training dataset. In order to have a more comprehensive learning
control policy, we need training samples that can cover the whole feasible
region of MPC.

We use a rejection random sampling algorithm to sample the initial state
${\bm{x}}(0)$ of control trajectories in the feasible region. In order to
reduce the similarity of the samplings and improve the coverage of samples in
the feasible region, we determine whether to adopt the new sample by comparing
the distance between the new sample $\bm{x}^{k}(0)$ and the existing one.

Assuming that there are $h$ existing samples
$\\{\bm{x}^{1}(0),\bm{x}^{2}(0)...,\bm{x}^{h}(0)\\}$, their center of gravity
can be expressed as:

$\displaystyle\bm{x}^{g}(0)=\frac{1}{h}\sum_{i=1}^{h}\bm{x}^{i}(0).$ (14)

We can get the distance from the new sampling point $\bm{x}_{0}^{k}$ to the
center of gravity $\bm{x}_{0}^{g}$ as:

$\displaystyle dis=\|\bm{x}^{k}(0)-\bm{x}^{g}(0)\|_{2}.$ (15)

If $dis>\tau_{k}$ is satisfied, $\bm{x}^{k}(0)$ is taken as the new initial
point, otherwise the initial point will be sampled again. The minimum distance
$\tau_{k}$ decreases with the increase of sampling number $k$ as

$\displaystyle\tau_{k}=\gamma^{[\frac{k}{K}]}\tau.$ (16)

Here, $\tau>0$ is a constant value used to filter the near sampling points and
$\gamma$ is a discount factor satisfying $0<\gamma<1$. $K$ is a positive
integer, which means that $\tau_{k}$ is updated every $K$ samples.

Next, the center of gravity is updated as follows:

$\displaystyle\bm{x}^{g}(0)=\frac{1}{k}\bm{x}^{k}(0)+\frac{k-1}{k}\bm{x}^{g}(0).$
(17)

Using the above algorithm, we obtain a series of initial points ${\bm{x}}(0)$.
In order to obtain training samples, we use the NMPC algorithm to get L-step
control trajectory
$\bm{\tau}(\bm{x}(0))=\\{(\bm{x}(0),\bm{U}^{*}(0)),(\bm{x}(1),\bm{U}^{*}(1)),...,(\bm{x}(L),\bm{U}^{*}(L))\\}$.
We combine control trajectory from $H$ different initial states to get the
training dataset

$\displaystyle\mathcal{D}=\bm{\tau}(\bm{x}^{1}(0))\cup\bm{\tau}(\bm{x}^{2}(0))\cup...\cup\bm{\tau}(\bm{x}^{H}(0)).$
(18)

The same method is used to generate the test set for supervised learning.

There is a correlation between the sample data obtained by the above method,
because the control trajectory is obtained in time sequence. In order to break
the association of training data and make it independent and identically
distributed (i.i.d), we use a data buffer to store and uniform randomly sample
training data.

The training goal of the supervised learning algorithm is that the empirical
error of the DNN loss function reaches the expected value. We reduce the
empirical error of DNN training by increasing the number of training samples
in the training dataset and increasing the number of training steps, until the
experience error satisfies the condition.

When the DNN is trained, it will be deployed into the controller. To ensure
stability, design the state region
$\mathcal{X}_{0}=\\{\bm{x}\in\mathbb{R}^{n}|\bm{x}^{T}\bm{Q}\bm{x}\leq\gamma\alpha\\}\subseteq\mathcal{X}_{f}$,
where $\gamma\in(0,1)$. If $\bm{x}(k)\in\mathcal{X}_{0}$, there exists
$\bm{u}(k)=\bm{\kappa}(\bm{x}(k))$, otherwise use approximate control policy
$\bm{\pi_{\theta}}(\bm{x}(k))$. The detailed learning algorithm is presented
in Algorithm 2.

Algorithm 2 PL-MPC Algorithm

0: Initialize training data buffer.

Offline:

Use rejection random sampling sampling method to randomly sample initial state
$\bm{x}(0)$.

Control trajectory $\bm{\tau}(\bm{x}(0))$ obtained by NMPC algorithm.

Store Control trajectory data in data buffer.

Collect batch data from data buffer.

Train DNN Using Algorithm 1.

Verify the learning effect by using empirical error

if The empirical error is less than the expected error then

Stop training.

return The approximate control policy $\bm{\pi}_{\theta}$.

else

Return to offline step 1 and continue training.

end if

Online:

for $k=0,1,2,3...$ do

Obtain the state of the controlled object $\bm{x}(k)$.

if $\bm{x}(k)\in\mathcal{X}_{0}$ then

Set control input $\bm{u}(k)=\bm{\kappa}(\bm{x}(k))$.

else

Set $N$ control sequences
$\bm{U}(k)=\bar{\bm{u}}(\cdot;k)=\bm{\pi_{\theta}}(\bm{x}(k))$.

Set control input $\bm{u}(k)=\bar{\bm{u}}(k;k)$.

end if

end for

##  IV FEASIBILITY HANDLING AND STABILITY ANALYSIS

###  IV-A Feasibility Handling

This subsection deals with the constraint satisfaction issue of Problem
$\mathcal{P}3$ to ensure the feasibility of the proposed Algorithm 2. As
mentioned at the end of Section III-B, although the dual optimization learning
algorithm provides an effective method to handle the constraints of PL-MPC,
there exists a small chance that the approximate solution may violate
constraints. In this particular case, we use the proximal operator to
guarantee the feasibility of PL-MPC algorithm.

By adding a projection mapping layer on the basis of the original DNN, the
proximal operator is able to translate the approximate solution
$\bm{U_{p}}(k)$ generated by DNN into a high-quality feasible solution
$\bm{U_{\theta}}(k)$. The projection mapping layer is designed as follows:

Problem $\mathcal{P}5$ :

$\displaystyle\arg$
$\displaystyle\min_{{\bm{U}_{\theta}}(k)}\left\|{\bm{U_{\theta}}(k)-\bm{U_{p}}(k)}\right\|_{2}$
(19) $\displaystyle s.t.$
$\displaystyle\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))\leq\bm{0}.$

Generally, $\bm{U_{p}}(k)$ belongs to the feasible region by using the dual
optimization learning algorithm, i.e., $\bm{U}_{\theta}(k)=\bm{U_{p}}(k)$.
When the approximate solution $\bm{U_{p}}(k)$ does not exist in the feasible
region, we can obtain the corresponding feasible solution $\bm{U_{\theta}}(k)$
by solving the optimization problem $\mathcal{P}5$.

Here, the problem $\mathcal{P}5$ is solved through exterior point penalty
function methods. Define a penalty function as:

$\displaystyle\bm{Q}(\bm{U_{\theta}}(k),\mu)=$
$\displaystyle\left\|{\bm{U_{\theta}}(k)-\bm{U_{p}}(k)}\right\|_{2}+\bm{\mu}([\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))]^{+})^{2},$
(20)

where $\bm{\mu}>\bm{0}$ is the penalty parameter. Obviously, as the increase
of $\mu$, the penalty function $\bm{Q}(\bm{U_{\theta}}(k),\bm{\mu})$ will be
far away from the optimal value $\bm{Q}({\bm{U_{\theta}}}^{*}(k),\bm{\mu})$ if
$\bm{U_{\theta}}(k)$ violates the constraint
$\bm{G}(\bm{x}(k),\bm{U_{\theta}}(k))\leq\bm{0}$, which makes the
unconstrained problem
$\arg\min_{{\bm{U_{\theta}}(k)}}\bm{Q}(\bm{U_{\theta}}(k),\bm{\mu})$
equivalent to problem $\mathcal{P}5$. The detailed quadratic penalty method is
presented in Algorithm 3.

Algorithm 3 Quadratic Penalty Method

0: Initialize $\bm{\mu}_{0}=\bm{1}$ and $\bm{\gamma}$, starting point
$\bm{U_{\theta}}^{0}(k)=\bm{U_{p}}(k)$ and $\bm{\xi}>0$.

for $i=0,1,2,3...$ do

Optimize $\bm{Q}(\bm{U_{\theta}}(k),\bm{\bm{\mu}}_{i})$ under
$\bm{U_{\theta}}(k)$ starting at $\bm{U_{\theta}}^{i}(k)$, until
$\left\|\nabla_{\mathbf{u}}\bm{Q}(\bm{U_{\theta}}(k),{\bm{\mu}}_{i})\right\|<\bm{\xi}$.

if satisfy restraint condition then

break.

end if

${\bm{\mu}}_{i+1}=\bm{\gamma}{\bm{\mu}}_{i}$.

Choose new starting point $\bm{U_{\theta}}^{i}(k)$.

end for

Now, by integrating the dual optimization learning algorithm and the proximal
operator, there always exists a feasible solution for the problem
$\mathcal{P}3$.

###### Remark 1

Note that after the pre-processing of $\bm{U}_{p}(k)$ by the dual optimization
learning algorithm, $\bm{U_{p}}(k)$ is not far away from $\bm{U_{\theta}}(k)$
in the case of constraint violations. Therefore, a small number of iterations
are required to find $\bm{U_{\theta}}(k)$ in Algorithm 3, which can still
obtain a fast solution without much computation resources.

###  IV-B Stability Analysis

In this subsection, we develop the stability result of the closed-loop system
for the proposed learning algorithm.

We start with considering the loss function in (11) with the learned control
policy $\bm{U_{\theta}}$ designed in this paper. To improve the control
performance of the proposed algorithm, we require the loss function to be less
than a positive real number $\varepsilon>0$,

$\displaystyle\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})<\varepsilon.$ (21)

In the policy learning procedure, due to the randomness of learning data and
samples, the loss function $\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})$ is a
random variable. In what follows, we develop a result ensuring (21) holds in
probability.

###### Theorem 1

Suppose the training data input $\bm{x}(k)$ satisfies the uniform distribution
and the training data samples are sampled independently from this
distribution. For the learned control policy $\bm{U_{\theta}}$ generated by
Algorithm 2 and the optimal one $\bm{U^{*}}$, given $\delta\in(0,1]$ and
$\varepsilon>0$, there exists a large number of training samples $M$, such
that:

$\displaystyle
P\\{\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})<\varepsilon\\}\geq
1-\frac{R_{emp}(\bm{U_{\theta}},\bm{U^{*}})}{(1-\sqrt{\frac{c-1}{M\delta}})\varepsilon},$
(22)

where $R_{emp}(\bm{U_{\theta}},\bm{U^{*}})$ is the empirical error of $M$
training data samples defined as follows:

$\displaystyle R_{emp}(\bm{U_{\theta}},\bm{U^{*}})$ (23) $\displaystyle=$
$\displaystyle\frac{1}{M}\sum_{k=1}^{M}(J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),{\bm{U^{*}}(k)})),$

$c$ is a constant value satisfying $c\geq 1$.

###### Proof 1

Considering the nonnegativity of $\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})$
and applying the Markov’s Inequality to it for $\varepsilon>0$, we have:

$\displaystyle
P\\{\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})\geq\varepsilon\\}\leq\frac{E[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]}{\varepsilon},$
(24)

where $E[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]$ is the generalization
error of the learning algorithm defined as[36]:

$\displaystyle
E[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]=R_{gen}(\bm{U_{\theta}},\bm{U^{*}})$
(25) $\displaystyle=$
$\displaystyle\int(J(\bm{x},\bm{U_{\theta}})-J(\bm{x},\bm{U^{*}}))dF(J(\bm{x},\bm{U_{\theta}})|\bm{x}).$

Where $F$ is the probability density function of the distribution $\bm{x}(k)$.
Then, by utilizing the inequality in (24), the probability of (21) is bounded
by:

$\displaystyle
P\\{\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})<\varepsilon\\}\geq
1-\frac{R_{gen}(\bm{U_{\theta}},\bm{U^{*}})}{\varepsilon}.$ (26)

However, the generalization error $R_{gen}(\bm{U_{\theta}},\bm{U^{*}})$ of the
learning algorithm usually cannot be determined. To deal with this issue, we
derive the upper bound of $R_{gen}(\bm{U_{\theta}},\bm{U^{*}})$ in probability
in the following.

First, we show that

$\displaystyle\frac{E[\mathcal{L}^{2}({\bm{U_{\theta}}},{\bm{U^{*}}})]}{E^{2}[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]}\leq
c.$ (27)

Because $\mathcal{L}({\mathbf{u_{\theta}}(\bm{x})},{\mathbf{u^{*}}(\bm{x})})$
can be formulated as:

$\displaystyle
V[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]=E[\mathcal{L}^{2}({\bm{U_{\theta}}},{\bm{U^{*}}})]-E^{2}[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})],$
(28)

it can be obtained that $c\geq 1$. Since the variance
$V[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]$ is bounded and greater than
zero[37], there exists a positive integer $c\geq 1$ such that the inequality
in (27) holds. Since $\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})\geq 0$ and
$\bm{x}(k)$ satisfy a uniform distribution, we have:

$\displaystyle
E[\mathcal{L}^{2}({\bm{U_{\theta}}},{\bm{U^{*}}})]\geq\frac{4}{3}E^{2}[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})],$
(29)

$c$ can take the value $c=2$.

Then, substituting (27) into (29), we obtain:

$\displaystyle V[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]\leq$
$\displaystyle(c-1)E^{2}[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]$ (30)
$\displaystyle\leq$
$\displaystyle(c-1)R_{gen}^{2}({\bm{U_{\theta}}},{\bm{U^{*}}}).$

Next, by using the inequality in (30), the upper bound of the variance of
$R_{emp}(\bm{U_{\theta}},\bm{U^{*}})$ can be calculated as:

$\displaystyle V[R_{emp}({\bm{U_{\theta}}},{\bm{U^{*}}})]$ (31)
$\displaystyle=$ $\displaystyle
V[\frac{1}{M}\sum_{k=1}^{M}(J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),{\bm{U^{*}}(k)}))]$
$\displaystyle=$
$\displaystyle\frac{1}{M^{2}}\sum_{k=1}^{M}V[(J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),{\bm{U^{*}}(k)}))]$
$\displaystyle=$
$\displaystyle\frac{1}{M}V[\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})]$
$\displaystyle\leq$
$\displaystyle{\frac{c-1}{M}}R_{gen}^{2}(\bm{U_{\theta}},\bm{U^{*}}).$

In addition, the expectation of $R_{emp}({\bm{U_{\theta}}},{\bm{U^{*}}})$ can
be calculated as:

$\displaystyle E[R_{emp}({\bm{U_{\theta}}},{\bm{U^{*}}})]$ (32)
$\displaystyle=E[\frac{1}{M}\sum_{k=1}^{M}(J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),{\bm{U^{*}}(k)}))]$
$\displaystyle=\frac{1}{M}\sum_{k=1}^{M}E[(J(\bm{x}(k),\bm{U_{\theta}}(k))-J(\bm{x}(k),{\bm{U^{*}}(k)}))]$
$\displaystyle=\frac{1}{M}\sum_{k=1}^{M}R_{gen}(\bm{U_{\theta}},\bm{U^{*}})=R_{gen}(\bm{U_{\theta}},\bm{U^{*}}).$

By combining the inequality in (31) and (32), we can get:

$\displaystyle
P\\{|R_{emp}(\bm{U_{\theta}},\bm{U^{*}})-R_{gen}(\bm{U_{\theta}},\bm{U^{*}})|<\sigma\\}$
(33) $\displaystyle\geq
1-\frac{(c-1)R_{gen}^{2}(\bm{U_{\theta}},\bm{U^{*}})}{\sigma^{2}M}$

where the Chebyshev inequality is utilized.

Setting $\sigma$ as
$\sigma=\sqrt{\frac{c-1}{M\delta}}R_{gen}(\bm{U_{\theta}},\bm{U^{*}})$ and
substituting it into (33), we have:

$\displaystyle
P\\{|R_{emp}(\bm{U_{\theta}},\bm{U^{*}})-R_{gen}((\bm{U_{\theta}},\bm{U^{*}})|$
(34)
$\displaystyle<\sqrt{\frac{c-1}{M\delta}}R_{gen}(\bm{U_{\theta}},\bm{U^{*}})\\}\geq
1-\delta.$

Finally, considering that the empirical error is less than the generalization
error[38], the upper bound of $R_{gen}(\bm{U_{\theta}},\bm{U^{*}})$ satisfies
the following:

$\displaystyle P\\{$ $\displaystyle R_{gen}(\bm{U_{\theta}},\bm{U^{*}})\leq$
(35)
$\displaystyle(1-\sqrt{\frac{c-1}{M\delta}})^{-1}R_{emp}(\bm{U_{\theta}},\bm{U^{*}})\\}\geq
1-\delta.$

When $\delta\rightarrow 0$ and the number of training samples $M$ is large
enough, the probability of
$R_{gen}(\bm{U_{\theta}},\bm{U^{*}})\leq(1-\sqrt{\frac{c-1}{M\delta}})^{-1}R_{emp}(\bm{U_{\theta}},\bm{U^{*}})$
approaches to $1$. Applying (35) into (26), the inequality in (22) is derived,
which completes the proof.

$\hfill\Box$

###### Remark 2

(31) and (32) require the training sampling data $\bm{x_{k}}$ to be
independently and identically distributed (i.i.d), which are widely used in
machine learning algorithms[39, 40]. In this paper, we set the data buffer and
uniform random sampling to ensure the uniform distribution and i.i.d. property
of the training sampling data.

Next, the asymptotic stability in probability of the closed-loop system with
the learned policy is presented.

###### Theorem 2

Suppose that Assumptions 1-3 hold, consider any state region
$\mathcal{X}_{0}=\subseteq\mathcal{X}_{f}$. Then the closed-loop system (1)
under the suboptimal control policy ${\bm{U}_{\theta}}$ obtained by Algorithm
2 and Algorithm 3 is asymptotically stable in probability.

###### Proof 2

Consider that the system state is $\bm{x}(k)$, and the feasible suboptimal
solution obtained from DNN is
${\bm{U}_{\theta}}(k)=[{\bm{u}_{\theta}}(k;k),...,{\bm{u}_{\theta}}(k+N-1;k)]$.
Then we can get the state of the next moment
$\bm{x}(k+1)=\bm{f}(\bm{x}(k),{\bm{u}_{\theta}}(k;k))$. For state
$\bm{x}(k+1)$, there is a feasible control input sequence
${\bm{U}_{\theta}}(k+1)=[{\bm{u}_{\theta}}(k+1;k),...,{\bm{u}_{\theta}}(k+N-1;k),\bm{v}]$
due to Assumptions 3, where $\bm{v}=\bm{\kappa}(\tilde{\bm{x}}(k+N;k))$.
According to [41], we can get the following inequality:

$\displaystyle J(\bm{x}(k+1),{\bm{U}^{*}(k+1)})\leq
J(\bm{x}(k+1),{\bm{U}_{\theta}}(k+1))$ (36)
$\displaystyle=J(\bm{x}(k),{{\bm{U}_{\theta}}(k)})+({\left\|{{\tilde{{\bm{x}}}}(k+N;k)}\right\|_{{Q}}^{2}+\left\|{\bm{v}}\right\|_{{R}}^{2}})$
$\displaystyle-(\left\|{{{{\bm{x}}}}(k;k)}\right\|_{{Q}}^{2}+\left\|{{{{\bm{u}}}}(k;k))}\right\|_{{R}}^{2})$
$\displaystyle+\left\|{{{\tilde{\bm{x}}}}(k+N+1;k)}\right\|_{{P}}^{2}-\left\|{{{\tilde{\bm{x}}}}(k+N;k)}\right\|_{{P}}^{2}.$

Here, $\bm{U}^{*}(k+1)=[\bar{\bm{u}}^{*}(k+1;k),...,\bar{\bm{u}}^{*}(k+N;k)]$
is the optimal solution. According to Assumption 3, it follows that:

$\displaystyle
J(\bm{x}(k+1),{\bm{U}^{*}(k+1)})-J(\bm{x}(k),{{\bm{U}_{\theta}}(k)})\leq$ (37)
$\displaystyle-(\left\|{{{{\bm{x}}}}(k)}\right\|_{{Q}}^{2}+\left\|{{{{\bm{u}}}}(k))}\right\|_{{R}}^{2}).$

Using Theorem 1, when the sampling number $M$ is large enough, then there is
small enough $\varepsilon>0$, such that
$\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})<\varepsilon\leq\mathop{max}\limits_{\bm{x}\in\mathcal{X}_{0}}\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}$.
Then we can get
$J(\bm{x},{\bm{U}_{\theta}})<J(\bm{x},{\bm{U^{*}}})+\mathop{max}\limits_{\bm{x}\in\mathcal{X}_{0}}\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}$.
Since $\left\|{{{{\bm{u}}}}(k))}\right\|_{{R}}^{2}\geq 0$, the inequality (37)
can be transformed into:

$\displaystyle
J(\bm{x}(k+1),{\bm{U}^{*}(k+1)})-J(\bm{x}(k),{{\bm{U}^{*}}(k)})<$ (38)
$\displaystyle\mathop{max}\limits_{\bm{x}\in\mathcal{X}_{0}}\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}-\left\|{{{{\bm{x}}}}(k)}\right\|_{{Q}}^{2}.$

For $\bm{x}(k)\notin\mathcal{X}_{0}$, we can get[42]:

$\displaystyle
J(\bm{x}(k+1),{\bm{U}^{*}(k+1)})-J(\bm{x}(k),{{\bm{U}^{*}}(k)})<0.$ (39)

This indicates that the suboptimal control law is asymptotically stable with
respect to a set
$\mathcal{X}_{0}=\\{\bm{x}\in\mathbb{R}^{n}|\bm{x}^{T}\bm{P}\bm{x}\leq\gamma\alpha\\}\in\mathcal{X}_{f}$.
From this we can conclude that the closed-loop system under suboptimal control
policy obtained by Algorithm 2 is asymptotically stable, because for
$\bm{x}(k)\in\mathcal{X}_{0}$, $\bm{u}(k)=\bm{\kappa}(\bm{x}(k))$ is a local
stabilizing control policy for the closed-loop system.

Finally, from Theorem 1 it follows that,
$P\\{\mathcal{L}({\bm{U_{\theta}}},{\bm{U^{*}}})<\mathop{max}\limits_{\bm{x}\in\mathcal{X}_{0}}\left\|{{{\bm{x}}}}\right\|_{{Q}}^{2}\\}\rightarrow
1$. As a result, the probability that the system is asymptotically stable will
also be close to $1$. The proof is completed.

$\hfill\Box$

Figure 3: State definition of unmanned surface vessel.

##  V IMPLEMENTATION TO USVs

In this section, we implement the proposed PL-MPC algorithm for an
underactuated USV.

###  V-A USV Dynamics

(a)

(b)

Figure 4: State trajectory with initial system state $[-64,-64,0,0,0,0]^{T}$.
(a) Position trajectory. (b) Velocity trajectory.

(a)

(b)

Figure 5: Comparison of PL-MPC and NMPC algorithms. (a) Position comparison.
(b) Velocity comparison.

(a)

(b)

Figure 6: Cost function with initial system state $[-5,-5,0,0,0,0]^{T}$ to
$[5,5,0,0,0,0]^{T}$. (a) Optimal cost function of NMPC. (b) suboptimal cost
function of PL-MPC.

To generate training data and validate performance of the proposed algorithm,
we identify the model for the USV shown in Fig. 3. The USV is driven by two
rear thrusters, which are $T_{l}$ and $T_{r}$, respectively. The control input
$F=T_{l}+T_{r}$, $M=(T_{l}-T_{r})*d_{l}$, where $d_{l}$ is axis distance of
USV equals to $0.277m$. The state space of the USV is from the north-east-down
frame (n-frame) to the body frame (b-frame). It has the nonlinear discrete-
time system dynamics as follows:

$\displaystyle\begin{bmatrix}x_{k+1}\\\ y_{k+1}\\\ {\psi}_{k+1}\\\ u_{k+1}\\\
v_{k+1}\\\ r_{k+1}\end{bmatrix}=\begin{bmatrix}x_{k}\\\ y_{k}\\\ {\psi}_{k}\\\
u_{k}\\\ v_{k}\\\ r_{k}\end{bmatrix}+\Delta T\begin{bmatrix}\Delta x_{k}\\\
\Delta y_{k}\\\ \Delta{\psi}_{k}\\\ \Delta u_{k}\\\ \Delta v_{k}\\\ \Delta
r_{k}\end{bmatrix},$ (40)

where $[\Delta x_{k},\Delta y_{k},\Delta{\psi}_{k},\Delta u_{k},\Delta
v_{k},\Delta r_{k}]^{T}$ are described as:

$\displaystyle\begin{bmatrix}\Delta x_{k}\\\ \Delta y_{k}\\\
\Delta{\psi}_{k}\end{bmatrix}=\begin{bmatrix}\cos{\psi}_{k}&-\sin{\psi}_{k}&0\\\
\sin{\psi}_{k}&\cos{\psi}_{k}&0\\\ 0&0&1\end{bmatrix}\begin{bmatrix}u_{k}\\\
v_{k}\\\ r_{k}\end{bmatrix},$ (41)

$\left\\{\begin{aligned} \Delta
u_{k}&=(m_{22}v_{k}r_{k}-d_{11}u_{k}+F)/m_{11}\\\ \Delta
v_{k}&=(-m_{11}u_{k}r_{k}-d_{22}v_{k})/m_{22}\\\ \Delta
r_{k}&=(m_{11}-m_{22})u_{k}v_{k}-d_{33}r_{k}+M)/m_{33}\\\
\end{aligned}\right.$ (42)

Here, the state $[x_{k},y_{k},\psi_{k}]^{T}$ represents x-axis position,
y-axis position and orientation in the $n$-frame, respectively. The state
$[u_{k},v_{k},r_{k}]^{T}$ is x-axis velocity, y-axis velocity and angular
velocity in the $b$-frame. The control input $[F,M]^{T}$ denotes the force and
the torque generated by the thrusters.

By practical system identification of the USV, the system control inputs
(i.e., the torque and the force limits of the two thrusts) are constrained by
$-19.6N\leqslant F\leqslant 39.2N$ and $-5Nm\leqslant M\leqslant 5Nm$,
respectively. The system state is constrained by $\left|x\right|\leqslant
70m$, $\left|y\right|\leqslant 70m$, $-1m/s\leqslant u\leqslant 2m/s$,
$\left|v\right|\leqslant 1m/s$ and $\left|r\right|\leqslant 0.2rad/s$.
$m_{11}=493.8$, $m_{22}=455.8$, $m_{33}=55.8$ are the diagonal elements of the
inertia matrix. $d_{11}=29.2$, $d_{22}=2173.7$, $d_{33}=17.7$ are the diagonal
elements of the damping matrix.

###  V-B Learning NMPC

Figure 7: Comparison computational time of PL-MPC and NMPC algorithms.

The training data set is generated by the traditional NMPC algorithm. The
detailed parameters are: The three weighting matrices
$Q=\rm{diag}(10,10,20,0.1,0.1,0.1)$, $R=\rm{diag}(0.01,0.2)$ and
$P=\rm{diag}(10,10,20,0.1,0.1,0.1)$, respectively. The prediction horizon is
$N=15$. For such an underactuated USV system, the specific design of the
terminal region and terminal control law $\bm{\kappa}({\bm{x}})$ can refer to
[43]. The simulation nonlinear optimization problem of NMPC is solved by
CasADi[44].

For DNN, the node of the input layer is set as $6$, and the node of the output
layer is $2N$. There are five layers in the hidden layer. Each layer has
$150$, $250$, $250$, $250$ and $50$ nodes, and the activation function is
chosen as the ReLU function. We set $\delta=0.01$, $\varepsilon=0.05$ and let
$\mathcal{D}$ contain $10^{6}$ training data, i.e., $M=10^{6}$. We approximate
$E[\mathcal{L}^{2}({\mathbf{u_{\theta}}(\bm{x})},{\mathbf{u^{*}}(\bm{x})})]$
and $E[\mathcal{L}({\mathbf{u_{\theta}}(\bm{x})},{\mathbf{u^{*}}(\bm{x})})]$
through the test set $\mathcal{T}$ containing $m=10^{5}$ data.

To verify the learning effect, we apply the PL-MPC algorithm to the simulation
model (40) and compare it with the NMPC algorithm. Fig. 4a and Fig. 4b present
the simulation trajectories with the initial system state
$\bm{x}(0)=[-64,-64,0,0,0,0]^{T}$, which verifies that the closed-loop system
is stable. Fig. 4a and Fig. 4b compare the system state curves of the NMPC
algorithm and the proposed PL-MPC algorithm. It can be observed that the
difference of the two algorithms is very small. Fig. 5a and Fig. 5b show the
optimal cost function of the NMPC algorithm and the suboptimal cost function
of the PL-MPC algorithm, from which it can be seen that the two algorithms
have almost the same cost function.

Simulations of PL-MPC and NMPC with different prediction horizons are also
carried out for comparison in the same environment (CPU i7-8550U). By choosing
different prediction horizons of MPC, Fig. 7 shows the detailed one-step
computational time of NMPC and PL-MPC. It can be seen that, the PL-MPC
algorithm greatly reduces the computation time compared with the NMPC
algorithm, especially with the increase of the prediction horizon.

###  V-C Hardware Implementation

In this section, we implement the designed PL-MPC algorithm to the USV
platform and conduct the lake experiments. The experimental equipment and the
environment are shown in Fig. 8. In order to verify the effectiveness of the
algorithm, we have conducted two experiments corresponding to two control
missions: point stabilization and trajectory tracking.

The experimental parameters of point stabilization are the same as those of
simulation experiments. By training the policy using the DNN offline with data
generated by simulation and deploying it for the USV, we conduct the point
stabilization experiments for USV in the lake. Fig. 9a is the trajectory of
USV point stabilization experiment on the map, and Fig. 9b shows the execution
process of the proposed method in the experiment.

Figure 8: USV for experimental and satellite map of experimental environment.

Fig. 10 shows the USV position in the experiment. Under the control algorithm,
the USV can move to the equilibrium point and remain stable. Figs. 11 \- 12
show the position error and velocity error, respectively. As shown in Table I,
the root mean square (RMS) values of position error are $0.151m$ and $0.442m$,
and the RMS value of angle error is $0.043rad$. The RMS values of velocity
error are $0.076m/s$, $0.019m/s$ and $0.036rad/s$, respectively. Note that
there exist unavoidably disturbances from waves and winds in the lake when the
experiments were conducted. Even though the unknown disturbances, the PL-MPC
controller has achieved satisfactory control performance in terms of the
relatively small errors in real time experiments.

(a)

(b)

Figure 9: USV point stabilization experiment and trajectory. (a) USV trajectory. (b) Experimental picture. Figure 10: Position trajectory of point stabilization experiment with initial position $[64,64]^{T}$. Figure 11: Position error of point stabilization experiment. Figure 12: Velocity error of point stabilization experiment. TABLE I: RMS Values of State Errors state error | $x$ | $y$ | $\psi$ | $u$ | $v$ | $r$
---|---|---|---|---|---|---
RMS vlaue | $0.151$ | $0.442$ | $0.043$ | $0.076$ | $0.019$ | $0.036$
unit | $m$ | $m$ | $rad$ | $m/s$ | $m/s$ | $rad/s$

For trajectory tracking experiments, we design a reference trajectory
$(\bm{x}_{d}(k),\bm{u}_{d}(k))$ as a circle. The reference trajectory and USV
share the same kinetic model in (40)-(42), it is denoted as
$\bm{x}_{d}(k+1)=\bm{f}(\bm{x}_{d}(k),\bm{u}_{d}(k))$. Here,
$\bm{x}_{d}(k)=[x_{d}(k),y_{d}(k),\psi(k),u_{d}(k),v(k),r(k)]^{T}$ and
$\bm{u}_{d}(k)=[F_{d}(k),M_{d}(k)]^{T}$. In this experiment, the reference
force and moment are set as: $F_{d}=17.5N$ and $M_{d}=1Nm$, respectively. The
reference trajectory under this control input is a circle, as shown in Fig.
14.

To track this trajectory, we define the difference between the system state
and the reference trajectory state as $\bm{x}_{e}(k)=\bm{x}(k)-\bm{x}_{d}(k)$
and the difference between the system control input and the reference input as
$\bm{u}_{e}(k)=\bm{u}(k)-\bm{u}_{d}(k)$.

We use $\bm{x}_{e}$ and $\bm{u}_{e}$ instead of $\bm{x}$ and $\bm{u}$ as the
NMPC cost function optimization variables. Naturally, the input of the neural
network changes to $\bm{x}_{e}(k)$ and the output changes to $\bm{U}_{e}(k)$,
where
$\bm{U}_{e}(k)=[\bm{u}_{e}(k;k),\bm{u}_{e}(k+1;k),...,\bm{u}_{e}(k+N-1;k)]$.
The three weighting matrices
$Q=diag(q_{11},q_{22},q_{33},q_{44},q_{55},q_{66})=diag(10,10,0.1,1,1,1)$,
$R=diag(r_{11},r_{22})=diag(0.01,0.01)$ and $P=diag(10,10,0.1,1,1,1)$,
respectively. The MPC constraints and neural network parameters are the same
as the simulation experiments. The terminal region and terminal control law
are designed as reference [4]. We deploy the designed DNN-based policy into
the trajectory control of USV, and the trajectory tracking control algorithm
is executed in real time with a sampling rate at $5Hz$.

(a)

(b)

Figure 13: USV trajectory tracking experiment and trajectory. (a) USV
trajectory. (b) Experimental picture. Figure 14: Trajectory of USV with
initial position $[-5,-5]^{T}$. Figure 15: Position error of trajrctory
tracking experiment.

Fig. 13a is the trajectory of USV point stabilization experiment on the map,
and Fig. 13b shows the execution process of the proposed method in the
experiment. Fig. 14 shows the USV position trajectory and reference
trajectory, from which it can be seen that the proposed PL-MPC algorithm
provides excellent tracking performance. The position errors are shown in Fig.
15 and the velocity errors are shown in Fig. 16. The RMS values of position
and velocity error are $0.094m$, $0.088m$ and $0.044m/s$, $0.079m/s$
demonstrated in Table II. It can be observed that the position and velocity
errors under the PL-MPC algorithm are small. This verifies the effectiveness
of the proposed PL-MPC algorithm in the actual environment.

Figure 16: Velocity error of trajrctory tracking experiment. TABLE II: RMS Values of Position and Velocity Errors state error | $x$ | $y$ | $u$ | $v$
---|---|---|---|---
RMS vlaue | $0.094$ | $0.088$ | $0.044$ | $0.079$
unit | $m$ | $m$ | $m/s$ | $m/s$

##  VI CONCLUSIONS

In this paper, we have developed a PL-MPC method for nonlinear discrete-time
system with state and control inputs constraints, and implemented it
successfully to the motion control of USVs. The DNN is trained to approximate
NMPC control policy to accelerate the computational speed. By this method, we
have designed the dual optimization learning algorithm, making the
approximation control policies satisfy the control input and state
constraints. The sufficient conditions on ensuring the closed-loop stability
have also been developed. The hardware experiment for the motion control of
USV have verified the feasibility and advantages of the proposed method.

## References

  * [1] D. Q. Mayne, J. B. Rawlings C. V. Rao and P. O. M. Scokaert, “Constrained model predictive control: Stability and optimality,” Control Engineering Practice, vol. 36, no. 6, pp. 789–814, 2000.
  * [2] S. Joe and T. Badgwell, “A survey of industrial model predictive control technology,” Control Engineering Practice, vol. 11, no. 7, pp. 733–764, 2003.
  * [3] A. Bemporad, D. Bernardini, R. Long, and J. Verdejo, “Model predictive control of turbocharged gasoline engines for mass production,” SAE Technical Paper, vol. 1, no. 8, pp. 75, 2018.
  * [4] H. Liang, H. Li and D. Xu, “Nonlinear model predictive trajectory tracking control of underactuated marine vehicles: theory and experiment,” IEEE Transactions on Industrial Electronics, vol. 68, no. 5, pp. 4238–4248, 2021.
  * [5] D. Wang, Q. Pan, Y. Shi, J. Hu and C. Zhao, “Efficient nonlinear model predictive control for quadrotor trajectory tracking: algorithms and experiment,” IEEE Transactions on Cybernetics, vol. 51, no. 10, pp. 5057–5068, 2021.
  * [6] M. Cannon and B. Kouvaritakis, “Continuous-time predictive control of constrained nonlinear systems,” F. Allgöwer, A. Zheng (eds) Nonlinear Model Predictive Control. Progress in Systems and Control Theory, vol. 26, 2000.
  * [7] Y. Shi, C. Shen, H. Fang, and H. Li, “Advanced control in marine mechatronic systems:A survey,” IEEE/ASME Transactions on Mechatronics, vol. 22, no. 3, pp. 1121-1131, 2017.
  * [8] C. Mark, “Efficient nonlinear model predictive control algorithms,” Annual Reviews in Control, vol. 28, no. 2, pp. 229-237, 2004.
  * [9] J. Carlo, P. Wensing, B. Katz, G. Bledt and S. Kim, “Dynamic locomotion in the MIT cheetah 3 through convex model-predictive control,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018, pp. 1-9. DOI: 10.1109/IROS.2018.8594448.
  * [10] B. Katz, J. Carlo, and S. Kim, “Mini Cheetah: a platform for pushing the limits of dynamic quadruped control,” in 2019 International Conference on Robotics and Automation (ICRA), Montreal, Canada, 2019, pp. 6295-6301. DOI: 10.1109/ICRA.2019.8793865.
  * [11] P. Bemporad, A survey on explicit model predictive control, Berlin, Germany: Springer Berlin Heidelberg, 2007.
  * [12] H. Ferreau, H. Bock and M. Diehl, “An online active set strategy to overcome the limitations of explicit MPC,” International Journal of Robust and Nonlinear Control, vol. 18, no. 5, pp. 816-830, 2008.
  * [13] P. Tondel, T. A. Johansen and A. Bemporad, “An algorithm for multi-parametric quadratic programming and explicit MPC solutions,” in Proceedings of the 40th IEEE Conference on Decision and Control, Orlando, FL, USA, 2001, pp. 1199-1204. DOI: 10.1109/CDC.2001.981048.
  * [14] C. T. Wen, X. Y. Ma and B. Ydstie, “Analytical expression of explicit MPC solution via lattice piecewise-affine function,” Automatica, vol. 39, no. 3, pp. 489-497, 2003.
  * [15] E. C. Kerrigan, “Predictive control for linear and hybrid systems,” IEEE Control Systems Magazine, vol. 38, no. 2, pp. 94–96, 2018.
  * [16] L. Hewing, P. Wabersich, M. Menner and M. Zeilinger, “Learning-based model predictive control: toward safe learning in control,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 3, pp. 269–296, 2020.
  * [17] A. Aswani, H. Gonzalez, S. Sastry and C. Tomlin, “Provably safe and robust learning-based model predictive control,” Automatica, vol. 49, no. 12, pp. 16–26, 2013.
  * [18] D. Limon, J. Calliess and J. Maciejowski, “Learning-based nonlinear model predictive control,” IFAC-PapersOnLine, vol. 50, no. 1, pp. 7769–76, 2017.
  * [19] P. Bouffard, A. Aswani and C. Tomlin, “Learning-based model predictive control on a quadrotor: onboard implementation and experimental results,” in 2012 IEEE International Conference on Robotics and Automation, St. Paul, MN, USA, 2012, pp. 279-284. DOI: 10.1109/ICRA.2012.6225035.
  * [20] S. Bansal, R. Calandra, T. Xiao, S. Levine and C. Tomlin, “Goal-driven dynamics learning via Bayesian optimization,” in 2017 IEEE 56th Annual Conference on Decision and Control (CDC), Melbourne, Australia, 2017, pp. 5168-5173. DOI: 10.1109/CDC.2017.8264425.
  * [21] D. Piga, M. Forgione, S. Formentin and A. Bemporad, “Performance-oriented model learning for data-driven MPC design,” IEEE Control Syst. Lett, vol. 3, no. 3, pp. 577-582, 2019.
  * [22] D. Gros and M. Zanon, “Data-driven economic NMPC using reinforcement learning,” IEEE Transactions on Automatic Control, vol. 65, no. 2, pp. 636-648, 2020.
  * [23] D. Rosolia and F. Borrelli, “Learning model predictive control for iterative tasks. A data-driven control framework,” IEEE Transactions on Automatic Control, vol. 63, no. 7, pp. 1883-1896, 2018.
  * [24] M. Menner, P. Worsnop and M. Zeilinger, “Constrained inverse optimal control with application to a human manipulation task,” IEEE Transactions on Control Systems, vol. 29, no. 2, pp. 826–834, 2021.
  * [25] G. Chou, D. Berenson and N. Ozay, “Learning constraints from demonstrations,” M. Morales, L. Tapia, G. Sánchez-Ante, S. Hutchinson, (eds) Algorithmic Foundations of Robotics XIII, vol. 14, 2018.
  * [26] T. Zhang, G. Kahn, S. Levine and P. Abbeel, “Learning deep control policies for autonomous aerial vehicles with MPC-guided policy search,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 2016, pp. 528-535. DOI: 10.1109/ICRA.2016.7487175.
  * [27] X. Zhang, M. Bujarbaruah and F. Borrelli, “Near-optimal rapid MPC using neural networks: a primal-dual policy learning framework,” IEEE Transactions on Control Systems Technology, vol. 29, no. 5, pp. 2102–2114, 2021.
  * [28] W. Chen, T. Wang, N. Atanasov, V. Kumar and M. Morari, “Large scale model predictive control with neural networks and primal active sets,” Automatica, vol. 135, 2022.
  * [29] M. Hertneck, J. Köhler, S. Trimpe and F. Allgöwer, “Learning an approximate model predictive controller with guarantees,” IEEE Control Systems Letters, vol. 2, no. 3, pp. 543–548, 2018.
  * [30] S. Chen, K. Saulnier, N. Atanasov, D. Lee, V. Kumar, J. Pappas and M. Morari, “Approximating explicit model predictive control using constrained neural networks,” in 2018 Annual American Control Conference (ACC), Milwaukee, WI, USA, 2018, pp. 1520-1527. DOI: 10.23919/ACC.2018.8431275.
  * [31] G. Ian, B. Yoshua and H. Aaron, Deep Learning. Boston, MA, USA, MIT Univ. Press, 2016.
  * [32] K. Hornik, M. Stinchcombe and H. White, “Multilayer feedforward networks are uni-versal approximators,” Neural Networks, vol. 2, no. 5, pp. 359-366, 1989.
  * [33] M. Leshno, V. Lin, A. Pinkus, and S. Schocken, “Multilayer feedforward networks with a nonpolynomial activation function can approximate any function,” Neural Networks, vol. 6, no. 6, pp. 861–867, 1993.
  * [34] F. Fioretto, P. Hentenryck, T. Mak, C. Tran, F. Baldo and M. Lombardi, “Lagrangian duality for constrained deep learning,” Y. Dong, G. Ifrim, D. Mladenić, C. Saunders, S. Van Hoecke, (eds) Machine Learning and Knowledge Discovery in Databases. Applied Data Science and Demo Track, vol. 12461, 2021.
  * [35] H. Chen, and F. Allgöwer, “A quasi-infinite horizon nonlinear model predictive control scheme with guaranteed stability,” Automatica, vol. 34, no. 10, pp. 1205–1217, 1998.
  * [36] D. Jakubovitz, R. Giryes, M. R. D. Rodrigues, “Generalization Error in Deep Learning,” H. Boche, G. Caire, R. Calderbank, G. Kutyniok, G. Mathar, P. Petersen, (eds) Compressed Sensing and Its Applications. Applied and Numerical Harmonic Analysis, 2019.
  * [37] Z. Yang, Y. Yu, C. You, J. Steinhardt, Y. Ma, “Rethinking bias-variance trade-off for generalization of neural networks,” in Proceedings of the 37th International Conference on Machine Learning, Vienna, AUSTRIA, 2020, pp. 10767–10777. DOI: 10.5555/3524938.3525936.
  * [38] R. Kil and K. Imhoi, “Generalization bounds for the regression of real-valued functions,” in Proceedings of the 9th International Conference on Neural Information Processing(ICONIP), Singapore, 2002, pp. 1766-1770. DOI: 10.1109/ICONIP.2002.1198977.
  * [39] V. Mnih, K. Kavukcuoglu, D. Silver, et al, “Human-level control through deep reinforcement learning,” Nature, vol. 518, pp. 529–533, 2015.
  * [40] E. Alpaydin, Introduction to Machine Learning. Boston, MA, USA, MIT Univ. Press, 2004.
  * [41] A. Bemporad, C. Filippi, “Suboptimal explicit MPC via approximate multiparametric quadratic programming,” in Proceedings of the 40th IEEE Conference on Decision and Control, Orlando, FL, USA, 2001, pp. 4851-4856. DOI: 10.1109/CDC.2001.980975.
  * [42] T. A. Johansen, “Approximate explicit receding horizon control of constrained nonlinear systems,” Automatica, vol. 40, pp. 293–300, 2004.
  * [43] H. P. Li, W. S. Yan, “Model Predictive Stabilization of Constrained Underactuated Autonomous Underwater Vehicles With Guaranteed Feasibility and Stability,” IEEE-ASME Transactions on Mechatronics, vol. 22, no. 3, pp. 1185–1194, 2017.
  * [44] J. Andersson, J. Gillis, G. Horn, J. Rawlings and M. Diehl, “CasADi-A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, vol. 11, no. 1, pp. 1–36, 2019..