# Efficient Trajectory Planning and Control for USV with Vessel Dynamics and
Differential Flatness

Tao Huang1,2, Zhenfeng Xue1,2,∗, Zhe Chen1,2, Yong Liu1,2,∗ 1Tao Huang,
Zhenfeng Xue, Zhe Chen and Yong Liu are with Institute of Cyber-Systems and
Control, Zhejiang University, Hangzhou, China2Tao Huang, Zhenfeng Xue, Zhe
Chen and Yong Liu are Intelligent Perception and Control Center, Huzhou
Institute of Zhejiang University, Huzhou, China∗Corresponding: Zhenfeng Xue
(zfxue0903@zju.edu.cn), Yong Liu<EMAIL_ADDRESS>
###### Abstract

Unmanned surface vessels (USVs) are widely used in ocean exploration and
environmental protection fields. To ensure that USV can successfully perform
its mission, trajectory planning and motion tracking are the two most critical
technologies. In this paper, we propose a novel trajectory generation and
tracking method for USV based on optimization theory. Specifically, the USV
dynamic model is described with differential flatness, so that the trajectory
can be generated by dynamic RRT∗ in a linear invariant system expression form
under the objective of optimal boundary value. To reduce the sample number and
improve efficiency, we adjust the trajectory through local optimization. The
dynamic constraints are considered in the optimization process so that the
generated trajectory conforms to the kinematic characteristics of the under-
actuated hull, and makes it easier to be tracked. Finally, motion tracking is
added with model predictive control under a sequential quadratic programming
problem. Experimental results show the planned trajectory is more in line with
the kinematic characteristics of USV, and the tracking accuracy remains a
higher level.

##  I Introduction

Unmanned surface vehicles (USVs) are designed to relieve human labor in
various water surface missions, such as surface cleaning, cyanobacteria
treatment, carga transportation and military reconnaissance. It mainly
perceives and maps the surrounding environment through shipborne radar or
camera, and then performs path planning and motion control automatically by
the core controller [1, 2, 3]. In the travelling process, the USV needs to
accurately avoid various static and dynamic obstacles in the environment. To
ensure that USV can successfully complete its mission autonomously, obstacle
avoidance trajectory planning [4] con control [5] are the two most critical
technologies.

However, current trajectory planning algorithms are usually migrated directly
from other similar unmanned platforms, such as unmanned aerial vehicle (UAV)
or automated guided vehicle (AGV), which ignores the inherent kinematic
characteristics of USV and thus degrades the motion tracking control
performance. Specifically, the USV is a classical under-actuated system, which
means the number of inputs is less than the actual degree of freedom of the
system. We usually regard the USV as a system with a three degree of freedom,
i.e., surge, sway and yaw, but the drive dimension only includes propeller
thrust along surge axis and its torque along yaw axis. This makes the
generated path hard to be tracked for USV due to the lack of relevance between
path planning and tracking.

Figure 1: A composite image of USV sailing in obstacle environment with
planned trajectory that fits the dynamic characteristics.

In this paper, we propose a novel trajectory planning and control method
specifically designed for USV. Under the circumstance that the global map is
assumed to be known, the dynamic RRT∗ algorithm is firstly applied to generate
the initial obstacle avoidance trajectory. In the process of constructing the
sampling interval, we propose to combine the Dubins curve [6] and A∗
algorithm, because the smooth and evasive characteristics of Dubins curve fits
the hull dynamics well. After that, the path planning problem is expressed as
a linear time invariant (LTI) form, and an trajectory optimization is
established while concerning the spatial quality of the path, such as
smoothness, obstacle avoidance and vessel dynamics. Different from the
existing methods [4, 7] that directly use the dynamic model of USV system as
the equality constraint in the trajectory generation process, so that the
generated trajectory satisfies the motion characteristics of the USV, which
causes huge computational burden and unstable generation results. Here, we use
differential flatness to describe the USV dynamic system to solve the
optimization problem. Our proposed method greatly improves the optimization
efficiency, and the usage of differential flatness can connect path planning
with motion control well, and make the subsequent motion tracking control more
accurate.

The effect of the proposed path planning algorithm is illustrated in Fig. 1,
from which the USV is able to travel across the map of multiple obstacles
efficiently. The generated path is smooth enough such that it can be well
consistent with the motion characteristics of the USV. In the subsequent
motion control process, we successfully combine the differential flatness of
system with the nonlinear model predictive control (NMPC). A quadratic
optimization problem about the error between given trajectory state and the
forward prediction state is constructed and cyclically solved. The tracking
accuracy remains at a high level during the rapid operation of the USV no
matter whether there is directional water interference.

##  II Related works

###  II-A Path planning for USV

It is generally based on some optimization objectives for USV trajectory
planning, such as the shortest distance, the least time or the best energy
consumption, to stably generate feasible and safe routes. Similar to other
unmanned platforms, the motion planning algorithm for USV includes two key
contents, i.e., path generation and obstacle avoidance. Path generation makes
the USV to quickly acquire the feasible route to the mission target based on
the map information. Wang et al. [8] proposed a multi-layer path planner for
route generation and collision avoidance based on fast marching method (FMM)
and B-spline. It not only ensures the planning speed, but also smooths the
path due to obstacle avoidance and improves the safety of USV navigation. In
addition to the traditional graph search methods [1, 9, 2], there are also
relevant applications based on reinforcement learning [10] in the same
problems. Wen et al. [4] proposed a hierarchical trajectory planning that
constructs an optimization problem based on RRT∗ algorithm and dynamic model
of USV. Shah et al. [11] as well as Du et al. [7] modelled the problem in the
state space and proposed to sample a reasonable trajectory based on motion
primitives. In order to effectively describe obstacle information, various
improved artificial potential field (APF) methods [12, 13] are used in USV
obstacle avoidance strategy, and obstacle environment description based on
safety corridor can be used in complex water environment. Compared with above
methods, this paper proposes an USV trajectory optimization algorithm that
combines the Dubins curve [6] and the dynamic RRT∗ to improve the sampling
efficiency. While stably generating the navigation trajectory that meets the
motion characteristics of USV, it ensures the real-time performance and quick
response to unknown obstacles.

###  II-B Motion control for USV

After the feasible navigation trajectory is obtained, robust trajectory
tracking is another key component to realize the autonomous navigation of USV.
Due to the uncertain external disturbance, the travel trajectory of USV
without reliable control will be greatly different from the expected
navigation trajectory. In the field of USV motion control, different control
methods have been proposed, such as back stepping [14], sliding mode control
[15], adaptive control [16] and model predictive control [17, 18]. These
control methods deal well with the problem of robust tracking of the USV path
in the presence of external disturbances. However, these works generally only
focus on the tracking of the USV trajectory, or simply consider the planning
task of USV path, but they ignore whether the generated path can be directly
used for tracking control. In the relevant USV path tracking literature [5],
the kinematic model of USV is added to path generation to make the navigation
more suitable for USV tracking control, but the computational burden and
algorithm stability become tough problems. Referring to [19, 20, 21], this
paper proposes the differential flatness property of the USV motion system to
simplify the kinematic model as an algebraic expression of the position and
its higher order derivatives. It effectively connects the navigation motion
planning with the USV tracking control problem, so that the USV can more
stably follow the trajectory and improve the stability and successful rate.

##  III Description of USV system

An USV is an under-actuated system with six degrees of freedom. When its
dynamic characteristics are directly embedded into path planning and motion
control, the results could be unstable. One feasible solution is to use
flatness to describe the system.

###  III-A USV discrete model

Figure 2: USV earth-fixed and body-fixed coordinate frames.

As shown in Fig. 2, an USV with kinematics and dynamics in earth-fixed and
body-fixed frames can be modelled by

$\left\\{\begin{aligned} \dot{\bm{\eta}}&=\bm{J}(\bm{\eta})\bm{\nu}\\\
\bm{M}\dot{\bm{\nu}}&=\bm{\tau}-\bm{C}(\bm{\nu})\bm{\nu}-\bm{D}\bm{\nu},\end{aligned}\right.$
(1)

where $\bm{\eta}=[x,y,\varphi]^{T}$ and $\bm{\nu}=[u,v,r]^{T}$ are position,
heading and velocity vectors in the earth-fixed and body-fixed frames
respectively. $\bm{J}(\bm{\eta})$ is the rotation matrix, and $\bm{M}$ is
positive-definite inertia matrix. $\bm{C}(\bm{\nu})$ is the Coriolis and
centripetal force matrix, and $\bm{D}$ is hydrodynamic damping matrix.
$\bm{\tau}=[\tau_{u},\tau_{v},\tau_{r}]^{T}$ is the thrust matrix.

For simplicity, considering a catamaran, we suppose the hull is symmetrical
about the $u$-axis and $v$-axis. The constraint of $v$-axis symmetry is over
strict, but practice has proved it is almost feasible for a catamaran. Thus,
the relevant matrix can be expressed as

$\bm{M}=diag\\{m_{1},m_{1},m_{2}\\},$ (2)
$\bm{C}(\bm{\nu})=\begin{bmatrix}0&0&-m_{2}v\\\ 0&0&m_{1}u\\\
m_{2}v&-m_{1}u&0\end{bmatrix},$ (3) $\bm{D}=diag\\{d_{1},d_{2},d_{3}\\},$ (4)

and the thrust matrix can be expressed as

$\bm{\tau}=\begin{bmatrix}F_{r}+F_{l}\\\ 0\\\
b\cdot(F_{r}-F_{l})\end{bmatrix}.$ (5)

Combined with the above equations, the USV dynamic model is rewritten as the
form of state equation as follow

$\dot{\bm{x}}(t)=f(\bm{x}(t),\bm{u}(t)),$ (6)

where the state and input of system is $\bm{x}=[x,y,\varphi,u,v,r]^{T}$ and
$\bm{u}=[\tau_{u},\tau_{r}]^{T}$ respectively.

In order to reflect the motion characteristics of USV in discrete time domain,
we use the explicit 4th order Runge Kutta method to calculate the form

$\bm{x}_{i+1}(t)=f_{RK4}(\bm{x}_{i}(t),\bm{u}_{i}(t),\varDelta{t}),$ (7)

where $\varDelta{t}$ is the discrete step. There are unknown hydrodynamic
parameters $\bm{h}=[m_{1},m_{2},d_{1},d_{2},d_{3}]$ in the above USV system
model, which will be used in subsequent planning and control methods. Based on
Eq. 1, we can use collected hull real motion state data $v_{r}$ and $u_{r}$ to
estimate $\bm{h}$ with the following optimization problem

$\begin{gathered}\min_{\bm{h}}\sum_{t=0}^{T_{r}}\bm{\varepsilon}(t)^{T}\bm{\Omega}\bm{\varepsilon}(t)\\\
s.t.\quad\bm{h}_{l}\leq\bm{h}\leq\bm{h}_{u}\\\
\bm{x}_{i+1}(t)=f_{RK4}(\bm{x}_{i}(t),\bm{u}_{i}(t),\varDelta{t},\bm{h}),t\in[0,T_{r}],\end{gathered}$
(8)

where $\bm{\varepsilon}(t)$ is the deviation between hull real state data
$v_{e}$ and theoretical data $v$, and $\bm{\Omega}$ is weight matrix. The
solution can be solved by Gauss Newton Hessian approximation method with the
help of Casadi [22] optimization library.

###  III-B Differential flatness

Due to the USV system is under-actuated and has nonholonomic constraints,
real-time planning and accurate trajectory control exist major challenges. Wen
et al. [4] try to insert the dynamic model of USV system as the equality
constraint into trajectory generation, so that the generated trajectory meets
the motion characteristics of USV. However, this leads to heavy computational
burden and unstable results. Similar to [23], it is feasible to simplify the
problem that USV model constraints need to be considered in trajectory
generation by using the flat property of the system.

According to [24], the USV system is controllable and also has differentiable
flatness. Then the system has a flat output $\bm{z}(t)\in\mathcal{R}^{m}$, so
that the state and input can be expressed in the algebraic form of derivatives
using only $\bm{z}(t)$ and its finite order [20], that is

$\left\\{\begin{aligned}
\bm{x}(t)&=\phi_{0}(\bm{z}(t),\dot{\bm{z}(t)},\ddot{\bm{z}(t)},\bm{z}^{(3)}(t)),\\\
\bm{u}(t)&=\phi_{1}(\bm{z}(t),\dot{\bm{z}(t)},\ddot{\bm{z}(t)},\bm{z}^{(3)}(t),\bm{z}^{(4)}(t)),\end{aligned}\right.$
(9)

where the flat output can be selected in USV system is
$\bm{z}(t)=[x(t),y(t)]$. Then the state space of USV trajectory generation can
be transformed into the consideration of state $[x,y]$ and its derivatives.
The highest derivative order of $\bm{z}(t)$ in Eq. 9 is 4. That is to say, the
trajectory equation needs to reach at least 5 orders to ensure smoothness.

##  IV Trajectory generation

In complex water environment, USV trajectory generation faces the problems of
real-time and feasibility of trajectory execution. Based on the optimized
dynamic RRT∗ framework, we are able to generate trajectory that meets the
execution of USV. Then the sampling state optimization method is added to the
sampling tree expansion process to improve its speed.

###  IV-A Path searching

Figure 3: An illustration of the path search process. The black path obtained
by Dubins curve has several collision points $p_{c}$, which will be moved
outside the collision with a safe distance, resulting the green path.
Algorithm 1 Kinodynamic Trajectory Plan

Input: Environment $e$, SamplingTree $\tau$, State $\bm{x}$
Output: Trajectory $T$

1:Initialize:
$\tau_{s}\leftarrow\bm{x}_{start},\tau_{g}\leftarrow\bm{x}_{goal}$

2:$\mathcal{P}_{t}\leftarrow\bm{TopoPathFind}(e)$

3:for i = 1 to n do

4: $\bm{x}_{rand}\leftarrow\bm{SampleState}(e,\bm{p}_{t})$

5:
$\bm{x}_{front},\bm{x}_{back}\leftarrow\bm{NeighborFind}(\tau_{s},\tau_{g},\bm{x}_{rand})$

6: $\bm{x}_{s}\leftarrow\bm{TreeGrow}(\bm{x}_{back},\bm{x}_{rand})$

7: if $\bm{x}_{s}$ not empty then

8: $\tau_{s}\leftarrow\tau_{s}\cup{\bm{x}_{rand},\bm{x}_{s}}$

9: $\bm{x}_{g}\leftarrow\bm{TreeGrow}(\bm{x}_{front},\bm{x}_{rand})$

10: if $\bm{x}_{g}$ not empty then

11: $\tau_{g}\leftarrow\tau_{g}\cup{\bm{x}_{rand},\bm{x}_{g}}$

12: $\bm{Rewire}(\tau_{s},\tau_{g},\bm{x}_{rand})$

13: if $\bm{x}_{rand}\in\tau_{s}\wedge\bm{x}_{rand}\in\tau_{g}$ then

14: get one solution

15:$T\leftarrow\bm{Recall}(\tau_{s},\tau_{g})$

16:$\bm{TrajectoryOptimize}(T)$

17:return $T$

The main process of dynamic RRT∗ framework is shown in Algorithm 1. By
expanding two sampling trees $\tau_{s}$ and $\tau_{g}$, feasible trajectory
from $\bm{x}_{init}$ to $\bm{x}_{goal}$ is searched rapidly. In order to
express the characteristics of USV nonholonomic constraints in the trajectory
and improve the efficiency and stability of sampling, we combine the Dubins
[6] and A∗ algorithm to generate the key topological path point
$\mathcal{P}_{t}$, which is illustrated in Fig. 3. The Dubins curve is used
here due to its smoothness, which is very suitable for the characteristics of
under-actuated system. Based on this, the sampling interval is constructed to
effectively reduce the sampling of irrelevant areas. The $\bf{Steer}()$
function in the $\bf{TreeGrow}()$ and $\bf{Rewire}()$ steps constructs the
transfer trajectory from $\bm{x}_{rand}$ to sampling tree $\tau_{s}$ and
$\tau_{g}$ that meets the USV dynamics. In the next part, we will introduce to
use closed form minimal value cost function to ensure stability and
efficiency. If the sampling status $\bm{x}_{rand}$ exists in the sampling tree
$\tau_{s}$ and $\tau_{g}$ at the same time, then a feasible trajectory is
searched.

In the traditional RRT∗ framework, when the sampling state cannot be connected
to the sampling tree due to obstacles or dynamic constraints, the discarding
method is adopted. This increases the time-consuming of the algorithm. To
reduce the occurrence of this situation, we use optimization method to adjust
the inappropriate transfer trajectory locally, so that the trajectory meets
the feasibility requirements.

###  IV-B Trajectory planning with flatness

In section III-B, the differentiable flatness of USV has been described. Based
on this system characteristics, we only need to pay attention to the flat
output $\bm{z}(t)$ and its higher derivatives when considering the planning
state space of trajectory. Here, we use linear time invariant (LTI) system to
express the motion characteristics of the system, and the trajectory can be
denoted as a parametric polynomial about time. The state equation of USV
system is expressed as

$\begin{gathered}\dot{\bm{x}}(t)=\bm{A}\bm{x}(t)+\bm{Bu}(t),\\\
\bm{A}=\left[\begin{array}[]{ll}\bm{0}&\bm{I}\\\
\bm{0}&\bm{0}\end{array}\right],\quad\bm{B}=\left[\begin{array}[]{l}\bm{0}\\\
\bm{I}\end{array}\right],\\\
\bm{x}(t)=\left[p(t),\dot{p}(t),\ddot{p}(t)\right]^{\top},\quad\bm{u}(t)=\dddot{p}(t).\end{gathered}$
(10)

Therefore, the goal of trajectory generation is to calculate a cubic
differentiable trajectory polynomial. The aim is to guide the USV from initial
state $\bm{x}_{init}$ to final state $\bm{x}_{end}$ along the time, i.e., the
optimal boundary value problem (OBVP). Similar to [21], the following cost
function needs to be optimized.

$~{}\mathcal{J}_{seg}=\int_{0}^{T}\left(\sigma+\frac{1}{2}\|\bm{u}(t)\|^{2}\right)dt,$
(11)

where $\sigma$ denotes the weight of time. Based on the cost Eq. 11, we can
obtain the expression of the optimal trajectory from $\bm{x}_{init}$ to
$\bm{x}_{end}$ with a closed form solution by using Pontryagin minimization
method.

$\begin{gathered}\bm{x}^{*}(T)=\left[\begin{array}[]{c}\frac{\alpha}{120}T^{5}+\frac{\beta}{24}T^{4}+\frac{\gamma}{6}T^{3}+\frac{a_{0}}{2}T^{2}+v_{0}T+p_{0}\\\
\frac{\alpha}{24}T^{4}+\frac{\beta}{6}T^{3}+\frac{\gamma}{2}T^{2}+a_{0}T+v_{0}\\\
\frac{\alpha}{6}T^{3}+\frac{\beta}{2}T^{2}+\gamma T+a_{0}\end{array}\right]\\\
\bm{u}^{*}(T)=\frac{\alpha}{2}T^{2}+\beta T+\gamma,\end{gathered}$ (12)

where the parameters $\alpha$, $\beta$ and $\gamma$ can be expressed by
$\bm{x}_{init}$ and $\bm{x}_{end}$, referring to [25]. According to the form
of $\bm{u}^{*}(T)$, Eq. 11 can be simplified as

$\begin{gathered}\mathcal{J}_{seg}^{*}=\sigma
T+\sum_{i\in{x,y}}(\frac{\alpha_{i}^{2}}{20}T^{5}+\frac{\alpha_{i}\beta_{i}}{4}T^{4}+\frac{\alpha_{i}\gamma_{i}+\beta_{i}^{2}}{3}T^{3}\\\
+\beta_{i}\gamma_{i}T^{2}+\gamma_{i}^{2}T).\end{gathered}$ (13)

It can be found that $J^{*}_{seg}$ is a higher-order polynomial about time
$T$. Then the optimal trajectory can be constructed by calculating the minimum
cost time by solving $\mathcal{J}^{*}_{seg}=0$.

The $\bf{TreeGrow}()$ process is illustrated in Algorithm 2, where
$\bf{Steer}()$ quickly obtains the optimal trajectory between two states by
solving Eq. 11 in a closed form, so that the algorithm can obtain the feasible
trajectory. The obtained trajectory can be added into the sampling tree only
when it passes the feasibility test of $\bf{FeasibilityCheck}()$.

Algorithm 2 $TreeGrow$

Input: Environment $e$, SamplingTree $\tau$,
NeighborState $\bm{x}_{n}$ SamplingState $\bm{x}_{rand}$
Output: ParentState $\bm{x}_{parent}$

1:Initialize: $\bm{x}_{parent}\leftarrow null,cost_{min}\leftarrow\infty$

2:for $\bm{x}_{c}ur$ in $\bm{x}_{n}$ do

3: $T_{seg}\leftarrow\bm{Steer}(\bm{x}_{c}ur,\bm{x}_{rand})$

4: if $\bm{FeasibilityCheck}(T_{seg},e)$ then

5: if $\bm{TrajCost}(T_{seg})<cost_{min}$ then

6: $\bm{x}_{parent}\leftarrow\bm{x}_{c}ur,$

7: $cost_{min}\leftarrow\bm{TrajCost}(T_{seg})$

8: else

9: $T_{seg}\leftarrow\bm{TrajectoryOptimize}(T_{seg},e)$

10: if $\bm{FeasibilityCheck}(T_{seg},e)$ then

11: $\bm{x}_{parent}\leftarrow\bm{x}_{c}ur,$

12: $cost_{min}\leftarrow\bm{TrajCost}(T_{seg})$

13:return $\bm{x}_{parent}$

##  V Trajectory optimization and tracking

In the framework of trajectory generation, the feasibility of trajectory can
not be guaranteed. To reduce the number of samples and improve efficiency, we
adjust the trajectory to meet the relevant constraints through local
optimization according to the infeasible trajectory. Referring to [26], the
closed form of solution can be calculated by setting the objective function to
a quadratic form.

###  V-A Optimization modelling

Figure 4: An illustration of trajectory optimization. The orange path is
sampled by A∗ algorithm from $v_{in}$ to $v_{out}$, and then the anchor points
$p_{a}$ are sampled. Finally, the blue path is generated by moving $p_{a}$ to
a certain distance.

In order to optimize the trajectory, the constraints include the four
following items, i.e., trajectory smoothness, collision-free, dynamic and
original trajectory constraints. We divide the original trajectory into
multiple segments based on time, so that there are multiple free states in the
middle to prevent poor performance of whole trajectory optimization. Then, the
objective function is defined as

$~{}\mathcal{F}=\lambda_{s}f_{s}+\lambda_{c}f_{c}+\lambda_{d}f_{d}+\lambda_{o}f_{o},$
(14)

where $f_{s}$, $f_{c}$, $f_{d}$ and $f_{o}$ denote the above items, and
$\lambda_{s}$, $\lambda_{c}$, $\lambda_{d}$ and $\lambda_{o}$ are the weights
respectively.

The trajectory smoothness term $f_{s}$ is the time integral of the trajectory
jerk, i.e.,

$f_{s}=\int_{0}^{T}\|\bm{u}(t)\|^{2}dt=\bm{p}^{T}\bm{Q}_{s}\bm{p},$ (15)

where $\bm{p}^{T}=[\bm{p}_{1}^{T},\bm{p}_{2}^{T},...,\bm{p}_{n_{seg}}^{T}]$ is
composed of $n_{seg}$ trajectory segments, and $\bm{Q}_{s}$ is a diagonal
matrix composed of time $T_{i}$ with different orders.

Referring to [27], the collision-free term $f_{c}$ is described as the
integral of the distance between the safety guidance point $\rho$ and the
collision trajectory segment. The process is illustrated in Fig. 4. This makes
the trajectory of the collision part tend to the safety guidance point, so as
to keep away from obstacles.

$\displaystyle f_{c}$
$\displaystyle=\sum_{\rho\in\mathcal{P}_{t}}\int_{T_{s}^{\rho}}^{T_{e}^{\rho}}\|p(t)-p_{\rho}(t)\|^{2}dt$
(16)
$\displaystyle=\sum_{\rho\in\mathcal{P}_{t}}(\bm{p}-\bm{p}^{\rho})^{\top}\bm{Q}_{c,\rho}(\bm{p}-\bm{p}^{\rho}).$

The collision part can be obtained by $\bf{FeasibilityCheck}()$, and updated
using A∗ algorithm. $p_{\rho}(t)$ is the spatial location of $\rho$, and
$T_{s}^{\rho}$, $T_{e}^{\rho}$ are the collision part influenced by $\rho$.

The dynamic constraint term $f_{d}$ limits the range of higher-order
derivatives of the trajectory, and punishes the trajectory part that exceeds
physical dynamics of USV. That is

$\displaystyle f_{d}$
$\displaystyle=\sum_{\iota\in\mathcal{S}}\int_{T_{s}^{\iota}}^{T_{e}^{\iota}}\|v(t)-v_{max}\|^{2}+\|a(t)-a_{max}\|^{2}dt$
(17)
$\displaystyle=\sum_{\iota\in\mathcal{S}}(\bm{p}-\bm{p}^{\iota})^{\top}\bm{Q}_{d,\iota}(\bm{p}-\bm{p}^{\iota}),$

where $v_{max}$ and $a_{max}$ denote the maximal speed and acceleration of USV
respectively. $S$ is the trajectory part that exceeds the power limit.
$T_{s}^{\iota}$ and $T_{e}^{\iota}$ are the start and end time.

The original trajectory constraint term $f_{o}$ is similar to the collision-
free term, except that the original trajectory position information $p^{*}(t)$
is used as the limit.

$f_{o}=\int_{0}^{T}\|p(t)-p^{*}(t)\|^{2}dt=(\bm{p}-\bm{p}^{*})^{T}\bm{Q}_{o}(\bm{p}-\bm{p}^{*}).$
(18)

The function of term is to constrain the safe part of original trajectory to
maintain its original feasibility.

Referring to [28], trajectory optimization function 14 has positive definite
property, and the optimal solution can be obtained through a closed form. If
the trajectory still does not satisfy the requirements, multiple iterations
are used to extract the unsatisfactory trajectory. The weight of corresponding
optimization term is modified with a greater impact, so that the trajectory is
updated under new constraints.

In our framework, the trajectory generated by the front end based on dynamics
is composed of several segments, so its overall smoothness can not be
guaranteed, which is not optimal for tracking. Therefore, these trajectory
segments need to be optimized for trajectory smoothness. We use the same
optimization function as above, except that the smoothness weight
$\lambda_{s}$ is enlarged and dynamic constraint weight $\lambda_{d}$ is
reduced. Meanwhile, because there is no collision in the optimized trajectory,
we directly set the original trajectory as the safety route $\mathcal{P}_{t}$
to extract guidance point $\rho$ when constructing the collision-free term
$f_{c}$. The trajectory optimized by smoothing improves the feasibility and
stability for USV.

Figure 5: Visualization of trajectory planning over a short period of time
with velocity profile.

###  V-B Motion tracking with NMPC

The safe and feasible trajectory generated by the planning module needs
effective trajectory tracking control to realize the autonomous navigation of
USV. To make USV have stable tracking ability in complex water environment, we
set a high control frequency, i.e., 100Hz, and contain all the states $\bm{x}$
into the control law.

The nonlinear model predictive control (NMPC) method is selected as the
controller. According to the general form of MPC, a quadratic optimization
problem is constructed about the error between the given trajectory state and
the forward prediction state. The following nonlinear discrete optimization
problem is iteratively solved.

$\begin{gathered}\min_{\bm{u}}\bm{x}_{N}^{T}\bm{Q}\bm{x}_{N}+\sum_{i=0}^{N-1}\bm{x}_{i}^{T}\bm{Q}\bm{x}_{i}+\bm{u}_{i}^{T}\bm{R}\bm{u}_{i}\\\
s.t.\quad\bm{x}_{i+1}=\bm{f}_{RK4}\left(\bm{x}_{i},\bm{u}_{i},\Delta
t\right)\\\
\bm{x}_{0}=\bm{x}_{init},\bm{u}_{min}\leq\bm{u}_{i}\leq\bm{u}_{max}\end{gathered}$
(19)

where $\bm{x}_{0}$ is the actual state of USV at each solution cycle time, and
the forward prediction time $T_{f}$ is discretized into $N$ time steps. This
optimization problem can be transformed into a sequential quadratic
programming (SQP) problem to the extent of real-time iteration. The control
process is implemented through Casadi and Acados tool library [29].

##  VI Experiment

In this section, we perform simulation experiments using the open source USV
simulator Otter [30] within the ROS environment. The Otter USV is a catamaran
cloned from the real world designed by the Maritime Robotics corporation. It
has a size of 2.0m long, 1.08m wide and 0.82m high with a total mass about
55kg. The maximum velocity is about 2 knots. Sensing equipments including
lidar, camera and GPS have been installed in the simulator. The Otter USV
sails in a rugged island environment with various floating obstacles.

The experimental scenario is constructed within Gazebo, and the scene size is
about 300$\times$300 square meters. The environment includes random known
obstacles as well unknown obstacles. Moreover, the scene contains a constant
disturbance wind field and a surge that causes the USV attitude to swing. The
expansion of the barrier is set to 0.8m to prevent expansion. All experiments
are conducted on a laptop with an Intel i7-8700 CPU.

###  VI-A Path planning analysis

As shown in Fig. 5, the Otter USV is travelling across a complex island
environment with various unknown obstacles. The path planning module is able
to generate a obstacle-avoidance trajectory in real-time. The blue line shows
the planning results by dynamic RRT∗, which is random and not optimal. After
the optimization process proposed by our method, the USV is able to obtain a
smooth and kinetic affine trajectory so as to cross all kinds of terrain in
the best posture.

TABLE I: Ablation study for the proposed method. Method |  Traj. Len. |  Traj. Time |  Traj. Cost |  Node Util. |  Algo. Time |  Succ. Rate
---|---|---|---|---|---|---
($m$) |  ($s$) |  - |  ($\%$) |  ($ms$) |  ($\%$)
Baseline |  71.4 |  20.5 |  24.3 |  29.7 |  58.1 |  86
+Local Opt. |  71.8 |  20.8 |  24.2 |  40.6 |  47.8 |  96
+Global Opt. |  69.6 |  21.6 |  22.1 |  30.8 |  60.2 |  84
Proposed |  69.3 |  21.3 |  21.9 |  41.8 |  45.6 |  98

The ablation study for the proposed method is shown in Table I, where the
baseline denotes the dynamic RRT∗ algorithm, and local optimization (+Local
Opt.) means trajectory local adjustment with differential flatness, and global
optimization (+Global Opt.) means the trajectory optimization with dynamic
constraints. Trajectory length, time and cost are indicators that are
associated with generated trajectory. Node utilization is the ratio of
planning algorithm utilization to planned nodes and total sampling nodes,
which is used to evaluate the efficiency of algorithm. Algorithm time and
successful rate are used to measure the performance of the algorithm. The
algorithm is tested repeatedly for 50 times, and a successful planning should
be completed within 100$ms$.

In the baseline model test results, the node sampling rate is obviously low
(29.7$\%$), and the problem of calculation timeout frequently occurs,
resulting in a low success rate (86%). At the same time, the algorithm
consumes more time (58.1$ms$). On the other hand, the length of trajectory
generated by baseline model is long and the trajectory is not smooth. This
also can be viewed from Fig. 5.

After adding the local optimization method, the node utilization, planning
time and successful rate have been significantly improved (40.6$\%$, 47.8$ms$,
96$\%$), which indicates that it has a significant effect on the efficiency
and stability of the planning algorithm. After the global optimization method
is added, the path length and cost are reduced, because it makes the path
smoother, and the algorithm time consumption does not increase too much.

As for the proposed method, we combine the baseline model with local and
global optimization, resulting in a smoother trajectory and higher planning
efficiency. The trajectory length and cost are the lowest (69.3$m$, 21.9)
while the node utilization, algorithm time and successful rate achieve the
best (41.8$\%$, 45.6$ms$, 98$\%$).

Figure 6: State variables with time for the planned trajectory. Figure 7:
Visualization of motion tracking results of a spiral type and splayed shape
trajectory with and without disturbances.

Taking a closer look at the state variables of the planned trajectory as shown
in Fig. 6, there are few shocks in the surge and sway velocity curves, as well
as the yaw and its rate. As for the control inputs, the desired thrusts are
generally stable, which shows that the control performance requirements for
the USV are relatively low in this case.

###  VI-B Motion control analysis

As for the motion control performance, the results are illustrated in Fig. 7.
Two types of trajectory are planned to be tracked with the proposed control
method, including the spiral type and splayed shape trajectory. Meanwhile, we
test the control performance with and without disturbance. From the results,
we can see that the tracking control performance remains a high level.
Overall, the real trajectory of USV is consistent with the planned trajectory,
and the tracking error is controlled within 0.4m, in a 300$\times$300 square
meters map. The tracking velocity is quite large, ranging up to about 4m/s,
which is quite a large velocity with regard to an USV.

TABLE II: Ablation study for differential flatness (DF) in motion control. Method |  Mean Error |  Max Error |  Mean Vel. |  Max Vel. |  Ang. Vel. Integral
---|---|---|---|---|---
($m$) |  ($m$) |  ($m/s$) |  ($m/s$) |  ($rad^{2}/s$)
w.o DF |  0.257 |  0.768 |  2.93 |  4.31 |  2.01
Ours |  0.121 |  0.483 |  3.32 |  4.37 |  0.95

The quantitative comparisons between the baseline model (NMPC without DF) and
the proposed method are summarized in Table II. Without differential flatness,
although the USV trajectory tracking algorithm can still track the trajectory
in real time, the control effect is poor and the navigation is not smooth, and
the mean tracking error achieves 0.257$m$. After adding differential flatness,
the angular velocity integral has significantly decreased (from 1.97 to 0.82),
which indicates that differential flatness makes a better connection between
path planning and motion control components. It has a better improvement on
the robustness of motion control.

##  VII Conclusion

In this paper, we propose a novel yet efficient trajectory planning and motion
control algorithm for USV. Traditional methods use trajectory designed for UAV
or AGV as the guidance to generate obstacle-avoidance path, which ignores the
inherent under-actuated characteristics of USV. Based on this, we propose a
trajectory planning algorithm that concerns the hull dynamics during the
process, and use differential flatness to improve algorithm efficiency. The
generated trajectory is in good agreement with the characteristics of the
hull, so the control effect remains a high level. We perform extensive
simulation experiments to verify the effectiveness of the proposed method in
planning and control performance for USV. In the future, we will carry out
high-precision system identification on the real ship and realize it in real
objects.

## References

  * [1] H. Niu, A. Savvaris, A. Tsourdos, and Z. Ji, “Voronoi-visibility roadmap-based path planning algorithm for unmanned surface vehicles,” _The Journal of Navigation_ , vol. 72, no. 4, pp. 850–874, 2019.
  * [2] B. C. Shah and S. K. Gupta, “Long-distance path planning for unmanned surface vehicles in complex marine environment,” _IEEE Journal of Oceanic Engineering_ , vol. 45, no. 3, pp. 813–830, 2019.
  * [3] Z. Chen, T. Huang, Z. Xue, Z. Zhu, J. Xu, and Y. Liu, “A novel unmanned surface vehicle with 2d-3d fused perception and obstacle avoidance module,” in _2021 IEEE International Conference on Robotics and Biomimetics (ROBIO)_. IEEE, 2021, pp. 1804–1809.
  * [4] L. Wen, J. Yan, X. Yang, Y. Liu, and Y. Gu, “Collision-free trajectory planning for autonomous surface vehicle,” in _2020 IEEE/ASME international conference on advanced intelligent mechatronics (AIM)_. IEEE, 2020, pp. 1098–1105.
  * [5] S. Han, L. Wang, Y. Wang, and H. He, “An efficient motion planning based on grid map: Predicted trajectory approach with global path guiding,” _Ocean Engineering_ , vol. 238, p. 109696, 2021.
  * [6] L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” _American Journal of mathematics_ , vol. 79, no. 3, pp. 497–516, 1957.
  * [7] Z. Du, Y. Wen, C. Xiao, L. Huang, C. Zhou, and F. Zhang, “Trajectory-cell based method for the unmanned surface vehicle motion planning,” _Applied Ocean Research_ , vol. 86, pp. 207–221, 2019.
  * [8] N. Wang, X. Jin, and M. J. Er, “A multilayer path planner for a usv under complex marine environments,” _Ocean Engineering_ , vol. 184, pp. 1–10, 2019\.
  * [9] K. Yu, X.-f. Liang, M.-z. Li, Z. Chen, Y.-l. Yao, X. Li, Z.-x. Zhao, and Y. Teng, “Usv path planning method with velocity variation and global optimisation based on ais service platform,” _Ocean Engineering_ , vol. 236, p. 109560, 2021.
  * [10] S. Guo, X. Zhang, Y. Zheng, and Y. Du, “An autonomous path planning model for unmanned ships based on deep reinforcement learning,” _Sensors_ , vol. 20, no. 2, p. 426, 2020.
  * [11] B. C. Shah, P. Švec, I. R. Bertaska, W. Klinger, A. J. Sinisterra, K. Von Ellenrieder, M. Dhanak, and S. K. Gupta, “Trajectory planning with adaptive control primitives for autonomous surface vehicles operating in congested civilian traffic,” in _2014 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2014, pp. 2312–2318.
  * [12] S. Xie, P. Wu, Y. Peng, J. Luo, D. Qu, Q. Li, and J. Gu, “The obstacle avoidance planning of usv based on improved artificial potential field,” in _2014 IEEE International Conference on Information and Automation (ICIA)_. IEEE, 2014, pp. 746–751.
  * [13] Y. Chen, G. Bai, Y. Zhan, X. Hu, and J. Liu, “Path planning and obstacle avoiding of the usv based on improved aco-apf hybrid algorithm with adaptive early-warning,” _IEEE Access_ , vol. 9, pp. 40 728–40 742, 2021.
  * [14] G. Wen, S. S. Ge, C. P. Chen, F. Tu, and S. Wang, “Adaptive tracking control of surface vessel using optimized backstepping technique,” _IEEE transactions on cybernetics_ , vol. 49, no. 9, pp. 3420–3431, 2018.
  * [15] J. Zhang, S. Yu, D. Wu, and Y. Yan, “Nonsingular fixed-time terminal sliding mode trajectory tracking control for marine surface vessels with anti-disturbances,” _Ocean Engineering_ , vol. 217, p. 108158, 2020.
  * [16] C. Dong, Q. Ye, and S.-L. Dai, “Neural-network-based adaptive output-feedback formation tracking control of usvs under collision avoidance and connectivity maintenance constraints,” _Neurocomputing_ , vol. 401, pp. 101–112, 2020\.
  * [17] M. Abdelaal, M. Fränzle, and A. Hahn, “Nonlinear model predictive control for trajectory tracking and collision avoidance of underactuated vessels with disturbances,” _Ocean Engineering_ , vol. 160, pp. 168–180, 2018.
  * [18] W. Wang, T. Shan, P. Leoni, D. Fernández-Gutiérrez, D. Meyers, C. Ratti, and D. Rus, “Roboat ii: A novel autonomous surface vessel for urban environments,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 1740–1747.
  * [19] K. Sreenath, T. Lee, and V. Kumar, “Geometric control and differential flatness of a quadrotor uav with a cable-suspended load,” in _52nd IEEE Conference on Decision and Control_. IEEE, 2013, pp. 2269–2274.
  * [20] R. Morales, H. Sira-Ramírez, and J. Somolinos, “Linear active disturbance rejection control of the hovercraft vessel model,” _Ocean Engineering_ , vol. 96, pp. 100–108, 2015.
  * [21] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3529–3536, 2019.
  * [22] J. A. Andersson, J. Gillis, G. Horn, J. B. 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.
  * [23] M. Fliess, J. Lévine, P. Martin, and P. Rouchon, “Flatness and defect of non-linear systems: introductory theory and examples,” _International journal of control_ , vol. 61, no. 6, pp. 1327–1361, 1995.
  * [24] T. I. Fossen, “Guidance and control of ocean vehicles,” _University of Trondheim, Norway, Printed by John Wiley & Sons, Chichester, England, ISBN: 0 471 94113 1, Doctors Thesis_, 1999.
  * [25] M. W. Mueller, M. Hehn, and R. D’Andrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” _IEEE transactions on robotics_ , vol. 31, no. 6, pp. 1294–1310, 2015.
  * [26] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in _2011 IEEE international conference on robotics and automation_. IEEE, 2011, pp. 2520–2525.
  * [27] X. Zhou, Z. Wang, H. Ye, C. Xu, and F. Gao, “Ego-planner: An esdf-free gradient-based local planner for quadrotors,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 478–485, 2020.
  * [28] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in _Robotics research_. Springer, 2016, pp. 649–666.
  * [29] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. v. Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl, “acados—a modular open-source framework for fast embedded optimal control,” _Mathematical Programming Computation_ , vol. 14, no. 1, pp. 147–183, 2022.
  * [30] J. H. Lenes, “Autonomous online path planning and path-following control for complete coverage maneuvering of a usv,” Master’s thesis, NTNU, 2019.