Received 11 April 2023, accepted 6 May 2023, date of publication 11 May 2023, date of current version 19 May 2023.

Digital Object Identifier 10.1109/ACCESS.2023.3275433

Trajectory-Free Motion Planning of an Unmanned

Surface Vehicle Based on MPC and Sparse
Neighborhood Graph
SİMAY ATASOY 1,2 , (Member, IEEE), OSMAN KAAN KARAGÖZ1,3 , (Member, IEEE),
AND MUSTAFA MERT ANKARALI 1,3 , (Member, IEEE)
1 Department of Electrical and Electronics Engineering, Middle East Technical University, 06800 Ankara, Turkey
2 ASELSAN Research Center, 06200 Ankara, Turkey
3 Center for Robotics and AI, Middle East Technical University, 06800 Ankara, Turkey
Corresponding author: Simay Atasoy (simay.atasoy@metu.edu.tr)
This work was supported in part by Aselsan Inc., and in part by the Scientific and Technological Research Council of Turkey (TUBITAK)
1001 Program under Project 118E195 (initial phases) and Project 122E249 (final phases).

ABSTRACT Unmanned Surface Vehicles (USV) have gained significant attention in military, science, and
research applications in recent years. The development of new USV systems and increased application
domain of these platforms has necessitated the development of new motion planning methods to improve the
autonomy level of USVs and provide safe and robust navigation across unpredictable marine environments.
This study proposes a feedback motion planning and control methodology for dynamic fully-and under-
actuated USV models built on the recently introduced sparse random neighborhood graphs and constrained
nonlinear Model Predictive Control (MPC). This approach employs a feedback motion planning strategy
based on sparsely connected obstacle-free regions and the sequential composition of MPC policies. The
algorithm generates a sparse neighborhood graph consisting of connected rectangular zones in the discrete
planning phase. Inside each node (rectangular region), an MPC-based online feedback control policy
funnels the USV with nonlinear dynamics from one rectangle to the other in the network, ensuring no
constraint violation on state and input variables occurs. We systematically test the proposed algorithms in
different simulation scenarios, including an extreme actuator noise scenario, to test the algorithm’s validity,
effectiveness, and robustness.

INDEX TERMS Nonlinear model predictive control, feedback motion planning, sampling-based motion
planning, unmanned surface vehicles.

I. INTRODUCTION human errors, studies regarding automation of surface vehi-

There has been an increasing trend in using autonomous cles focusing on motion planning applications is a currently
systems in the maritime industry. According to the Annual attractive research area.
Overview of Marine Casualties and Incidents (2020), The main goal of robotic motion planning is to drive the
between 2014 and 2019, 44% of the casualties with ships agent from the start configuration to the goal configuration
are due to navigational casualties, which include accidents while obeying the constraints coming from the environment
resulting from contact, collision, and grounding/stranding. Of and the agent itself. In motion planning applications, the algo-
1801 accident events that occurred between 2014 and 2019, rithm’s success mainly depends on its capability to handle
54% of them were attributed to human erroneous action [2]. obstacles and dynamics. Even though motion planning is one
In order to decrease the accident rates that are attributed to of the most popular fields in robotics with a vibrant history,
safe and robust motion planning is still an active research
The associate editor coordinating the review of this manuscript and area open to advancements. Most motion planning algorithms
approving it for publication was Wonhee Kim . are built on creating a set of piecewise-smooth trajectories or
This work is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 License.
For more information, see https://creativecommons.org/licenses/by-nc-nd/4.0/
47690 VOLUME 11, 2023
S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

waypoints. Feedback control methods are later utilized in this rectangular nodes. Then, graph-search algorithms is deployed
dominant class of planning strategies to follow these trajec- to find the ‘‘optimal’’ discrete path on the graph in the
tories (or waypoints). On the other hand, some researchers second phase. Inside each node (region), an MPC controller
implemented trajectory-free motion planning concepts rather is responsible from driving the robot to the next node that
than a trajectory-based approach. A typical method in this is determined by the search algorithm in the second phase.
class segments the map with connected sub-regions. After In conclusion, via sequential composition of MPC control
that, the real-time planner and motion controller aims to drive policies, the whole algorithm can drive the robot to the goal
the robot to the goal region while respecting the constraints location.
coming from these regions [3], [4], [5]. Instead of directly A preliminary version of this work reporting early results
following a pre-defined trajectory, this approach relaxes the was presented by Karagöz et al. [5]. The current article is a
‘‘constraints’’ by increasing the possible movement area for significantly improved version with the following qualities:
the robot. (i) Karagöz et al. [5] tested the algorithm only with a (2DOF)
Model Predictive Control (MPC) is an effective tool toy model, whereas in this paper we adopted more compre-
for forcing the constraints implemented on the system for hensive motion models that can capture realistic full- and
the feedback control phase. Since real-world problems are under-actuated USV system dynamics (ii) the performance
highly nonlinear, a considerable amount of research has been of the algorithm in the presence of extreme process noise is
devoted to extending the usage of MPC to constrained non- evaluated to test the robustness of the methodology, (iii) the
linear systems for motion control applications [6], [7], [8], algorithm is modified in order to recover from failures caused
[9]. As opposed to the linear case, the infinite horizon prob- by noise, and (iv) MPC and graph search costs are modified
lem for nonlinear systems cannot be solved numerically, and to increase the performance of the proposed algorithm.
reducing the horizon may cause undesired system behaviors. The remainder of this paper is organized as follows.
In order to address this issue, Michalska and Mayne [10] Section II provides fundamentals regarding the dynamic
proposed a hybrid MPC framework that replaces the terminal modeling of a USV and nonlinear MPC concept. Section III
constraint with a terminal region. When the nonlinear system summarizes the MPC-Graph algorithm. Section IV and Sec-
reaches this region, another controller is employed, and as a tion V give details regarding the implemented robot mod-
result, the system is guaranteed to be asymptotically stable. els and simulation environments and report the obtained
However, to guarantee the stability of the system, a global results. Section VI proposes future directions for further
optimization problem is required to be solved. Chen and improvements.
Allgöwer [11] on the other hand, use an infinite horizon
approach and calculate a penalty term for the final state to II. PRELIMINARIES AND BACKGROUND
bound the infinite horizon cost. They establish the bound by A. UNMANNED SURFACE VEHICLE DYNAMICS
controlling the nonlinear system with fictitious linear state Compared to unmanned ground vehicles (UGV), unmanned
feedback in the predetermined terminal region. surface vehicles (USV) are exposed to different environment
MPC offers a framework that can handle multiple-input dynamics due to their application medium. Since USVs oper-
multiple-output (MIMO) systems and force constraints for ate in a sea/ocean environment, on top of rigid body dynamics
states and inputs, making it an effective instrument in also, added mass and damping terms should be considered in
collision-free motion planning applications. With an increas- a USV model. Let η = [x y ψ]T ∈ R2 × S denote the pose
ing demand for the utilization of autonomous surface vessels vector, where x and y are the world-fixed reference frame
in the military, search and rescue, transportation and explo- coordinates and ψ is the heading angle, let ν = [u v r]T ∈
ration, the literature regarding the control of USVs started R3 denote the velocity vector of the dynamic model where
to flourish in the last few decades [12]. Several works in u and v are linear velocities, called surge and sway, and r
literature employ MPC for USV control [13], [14], [15], [16], is the angular velocity. As usual, the equation η̇ = J(ψ)ν
[17], [18], [19]. Zhao et al. [13] propose an improved MPC defines the relationship from body reference frame to world
framework with the inclusion of global course constraints and fixed reference frame where J(ψ) is in the form of
an event-triggered mechanism. In [14], researchers propose
a finite control set model predictive control for collision 
cos(ψ) − sin(ψ) 0

avoidance problems. J(ψ) =  sin(ψ) cos(ψ) 0 . (1)
This paper proposes a trajectory-free, sampling-based 0 0 1
feedback motion planning algorithm to handle arbi-
trary obstacle configurations for fully- and under-actuated
Fig. 1 details out the abovementioned coordinate systems as
autonomous surface vehicles. We specifically focus on devel-
well as a schematic for the USV model.
oping a robust motion planning and control methodology
In our simulations, we use the 3 DOF horizontal plane
for USV systems and thus we tested our approaches in
the presence of unpredictable process noise. The algorithm model presented in [20]. The formulation is as follows,
consists of three phases. The algorithm first generates a graph
structure in the obstacle-free region by creating connected M ν̇ + C(ν)ν + D(ν)ν = τ (2a)

VOLUME 11, 2023 47691

 S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

is applicable to both stable and unstable systems, and with

the inclusion of terminal region and terminal cost matrix,
the algorithm guarantees asymptotic closed-loop stability.
The objective cost function comprises two parts: an integral
square error calculated over a finite horizon and a quadratic
terminal cost term as in the linear case.
The optimization problem of MPC can be formalized as
follows:
Z t+Tp  
J (x(t), u(·)) = ||x(τ )||2Q + ||ū(τ )||2R dτ
t
FIGURE 1. Schematic for the USV model. X-Y and u-v denote the + ||x(t + Tp )||2P (4a)
world-fixed and body fixed reference frames, respectively.
subject to ẋ = f(x, u) (4b)
u(τ ) ∈ U , τ ∈ [t, t + Tp ] (4c)
where x(t + Tp ) ∈ . (4d)
M = M A + M RB (2b) where x(·) is the state vector, u(·) is the input vector to
C(ν) = C A (ν) + C RB (ν) (2c) the system defined by a set of nonlinear differential equa-
tions, f (x(·), u(·)) and Tp is prediction horizon. Q and R
D(ν) = Dl + Dn (ν) (2d)
are positive-definite and symmetric cost matrices for state
In (2a) the general formulation for the USV dynamics is given and input variables, respectively. We obtained the terminal
where M, C(ν), D(ν) are inertia, Coriolis/centripetal, damp- state penalty matrix P and terminal region  based on the
ing matrices and τ is the thruster force vector, respectively. procedure proposed by Chen & Allgöwer [11]. First, the
A body moving in a liquid medium, transports some of the Jacobian linearization of the system at the origin is calculated,
surrounding liquid by its motion. As a result, it is observed ẋ = Ax + Bu (5)
that the body weighs more compared to its original weight.
In order to compensate this effect, added mass terms M A where A = (∂f/∂x)(0, 0) and B = (∂f/∂u)(0, 0). Provided
and C A (ν) are included in (2b) and (2c). The damping effect that (5) is stabilizable, a linear state feedback u = Kx can be
is given in (2d) where Dl and Dn (ν) denote the linear and obtained and by substituting u in (5),
nonlinear damping matrices, respectively. The subscript RB ẋ = (A + BK)x (6)
in (2b) and (2c) stands for rigid body terms. The matrices in
(2a)-(2d) is represented as the obtained matrix AK = A + BK is asymptotically stable.
    The related Lyapunov equation takes the following form,
m 0 0 Xu̇ 0 0
M RB =  0 m 0  , M A =  0 Yv̇ Yṙ  , (AK + κI)T P + P(AK + κI) = −Q⋆ (7)
0 0 Iz 0 Yṙ Nṙ where,
 
0 0 Yv̇ v + Yṙ r
Q⋆ = Q + KT RK ∈ Rn×n (8)
CA =  0 0 −Xu̇ u  ,
−Yv̇ v − Yṙ r Xu̇ u 0 admits a unique positive-definite and symmetric solution P.

0 0 −mv
 
Xu 0 0
 In (7), κ is chosen such that it satisfies the following condi-
C RB =  0 0 mu  Dl =  0 Yv Yr  , tion,


mv −mu 0

0 Nv Nr κ < −λmax (AK ) κ ∈ [0, ∞). (9)
X|u|u |u| 0 0
After determining the terminal state penalty matrix P, the pro-
Dn =  0 Y|v|v |v| 0  (3)
cedure continues with determination of the terminal region.
0 0 N|r|r |r|
Provided that, there exists a constant α ∈ (0, ∞), a region
where m is the mass and Iz is the moment of iner- α in the neighborhood of α is defined as follows,
tia of the vehicle perpendicular to the horizontal plane, α = {x ∈ Rn |xT Px ≤ α}. (10)
{Xu̇ , Yv̇ , Yṙ , Nṙ }, {Xu , Yv , Yr , Nv , Nr } and {X|u|u , Y|v|v , N|r|r }
are added mass, linear damping and nonlinear damping such that ∀x1 ∈ α , infinite horizon cost J ∞ ,
Z ∞
parameters, respectively. These parameters are scalar con-
stants that do not change over time. J ∞ (x1 , u) = (∥x(t)∥2Q + ∥u(t)∥2R )dt (11)
t1

B. MODEL PREDICTIVE CONTROL

starting from x(t1 ) = x1 and controlled by u = Kx is bounded
from above as follows,
For the control of the nonlinear USV systems, we use the
approach presented in [11]. The work of Chen and Allgöwer J ∞ (x1 , u) ≤ xT1 Px1 . (12)

47692 VOLUME 11, 2023

S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

With the inclusion of an upper bound to the infinite horizon

cost, the asymptotic stability of the system is guaranteed.

III. MPC-GRAPH
The proposed MPC-Graph algorithm executes three succes-
sive phases to sample the arena and navigate the robot: graph
generation, graph search, and motion control. The graph
generation phase takes the map as input and then samples
the obstacle-free areas with overlapping rectangular regions
until the predefined termination condition is satisfied. After
completing the graph generation phase, Dijkstra’s algorithm FIGURE 2. Generation of a node: (a) initial map of the arena, (b) a square
node is generated, (c) square node is expanded in discrete steps along
executes to search the obtained neighborhood graph for the directions indicated as 1 and 2. A more detailed information about the
shortest available path from any node to the goal node. The node generation is presented in [5].
motion control phase takes the determined nodes as input and
navigates the robot to the goal configuration while respecting
the constraints coming from the states and system inputs.

A. GRAPH GENERATION
The MPC-Graph framework starts its execution by sampling
the free space. After guaranteeing the sampled point qrand
is in the region unobstructed and not covered by previously
sampled rectangular nodes, then a new node Nodek is gener-
ated and expanded around qrand , see Fig. 2. Note that these
nodes can be in different geometric shapes such as square [4], FIGURE 3. Visualization of edge cost parameters. The subscripts i and
circle [3], [21], rectangle [5]. In this work, we use rectangular i + 1 stand for parent and child node, respectively. Dark gray region Ai
shows the overlapping area.
nodes due to their compliance with MPC and to achieve a
more sparse graph structure [5].
Then, Nodek is added to the graph, and edges are created B. GRAPH SEARCH
between this node and the neighboring nodes overlapping After the termination condition given in (13) is satisfied,
with it. Node generation and expansion process continue until the algorithm continues by searching the obtained graph, G,
the defined termination condition is satisfied. with Dijkstra’s search algorithm to find the optimal discrete
In the algorithm, we implemented the condition presented planner. The search algorithm returns a policy, Po, that gives
in [22] which mainly estimates the quality of the coverage of the order of the nodes robot should pass to reach the goal.
the sampled space. Let Cfree denote the set of configurations The constructed graph has an edge between two nodes if
in the obstacle-free region and µ be de Lebesgue measure in they have an overlapping area. We calculate the edge cost as
Cfree and B be the union of the nodes follows,
[
B= Nodek , costedge = ∥centerni − refi ∥2
k γ
+ ∥centerni+1 − refi ∥2 + (14)
then the implemented termination condition in (13), implic- Ai
itly determines estimates for the expression µ(B)/µ(Cfree ). where center is the intersection point of the diagonals of the
For that purpose, statistics collected from randomly taking corresponding node. The area of the intersection region which
samples to find a new configuration in Cfree \ B is analyzed. is indicated with a dark gray color in Fig. 3 is denoted as A
The implemented termination condition is formalized as fol- and the center of this region is given as ref. ∥·∥2 refers to the
lows, l2 norm, i.e. Euclidean distance. We include the reciprocal of
ln(1 − Pc ) the intersection area as a parameter for the edge cost since
m≥ −1 (13)
lnα larger areas provide smoother behavior for the robot. In our
where m is the number of successive failures followed by the simulations we used weight γ for the reciprocal of the inter-
first success, Pc , and α are user-determined parameters that section area which is set as γ = 1. Thus, Dijkstra’s search
affect the density of the coverage of the map. A more detailed algorithm chooses a route in favor of larger intersection areas.
information about the derivation of (13) is presented in [22]. Fig. 3 visualizes the parameters given in (14).
After satisfying the termination condition, the algorithm con-
tinues with the graph search phase to find the optimal route. C. MOTION CONTROL
The computational time regarding this stage is presented in The last phase of the proposed MPC-Graph algorithm is
detail in our previous work [5] which gives results for several motion control. At this stage of the algorithm, with the
different maps. previously determined policy Po, MPC navigates the robot

VOLUME 11, 2023 47693

 S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

FIGURE 4. Representation of world W, target T and robot frames R

FIGURE 5. (a) Robot arrives to an unsampled region. (b) A new node is
with the past, current and next nodes.
generated in the unsampled region which is indicated with dark grey
color. (c) Robot follows the red route by using the newly created region.
from an arbitrary node to the GoalNode in discrete time
steps. The motion control phase aims to safely and smoothly By using the transformation matrix Ttr , we calculate the
navigate the robot from its CurrentNode to the NextNode with position vector of the robot with respect to the target frame
the established policy Po while forcing the state and input qtr = [xtr ytr ψtr ]T . In cost function (4a), we use qtr for robot
constraints. It is important to note that, as long as the robot states. This approach aims to overlap the target frame with a
stays inside the sampled rectangular regions, it is guaranteed body-fixed robot frame. Using MPC, we calculate the optimal
that collisions with obstacles are avoided. In order to simulate finite-horizon input sequence that satisfies the constraints and
the physical limits for the robot, we implemented velocity navigates the robot towards the origin of the target frame.
and acceleration constraints. We adopted the quasi infinite When robot enters the intersection area, the NextNode
MPC [11] approach to guarantee stability of the system becomes the new CurrentNode and the new NextNode is
whose Jacobian linearization is stabilizable. determined by checking the next element in policy Po. It is
The node that the robot is currently in is denoted as the important to note that for every new region the robot enters,
CurrentNode, and the target node that the robot is traveling to the state constraints coming from the boundaries of the
is called the NextNode. The reference point, refi , robot aiming rectangular nodes are re-calculated. This process executes
to reach is chosen to be the centroid of intersection for that recursively until the robot arrives at the node, which includes
region, [xrefi yrefi ]T = Centroid(CurrentNode ∩ NextNode). the goal point qgoal . In the goal region, qgoal becomes the
After determining the centroid, by taking that point as the reference point, and NextNode is no longer applicable.
origin, a target reference frame Ti is placed. In order to deter- Due to unpredictable effects such as process noise, the
mine the orientation of that frame, a hypothetical vector − →
vi robot may end up in an unsampled region or another node
starting from the previous reference point refi−1 and ending different from its CurrentNode. For the former case, the last
at the target reference point refi is constructed. The angle θi position of the robot is treated as the sampled random point,
between the world frame W and − →vi is taken as the orientation qrand , and fed to the node generation function. This newly
for the target frame. Fig. 4 illustrates world W, target Ti and generated node is inserted into the previously obtained graph,
robot frames R. G, and taken as the new CurrentNode for the robot. Then,
After defining the necessary frames for the algorithm, the Dijkstra’s search algorithm is executed to generate a new
following transformation matrices are constructed, policy. These steps are given in lines 11-19 of Algorithm 1.

cos θ − sin θ 0 xref
 An illustration of this resampling procedure is presented in
 sin θ cos θ 0 yref  Fig. 5.
Tw ,
t =  0 For the latter case, the obtained graph is searched in order

0 1 0 
0 0 0 1 to determine the possible CurrentNode, which ensures the
minimum cost route according to (14). After determining the
cos ψ − sin ψ 0 x
 
 sin ψ CurrentNode, a new route is obtained using the previously
cos ψ 0 y
Twr =  0
  (15) generated policy, Po. These steps are given in lines 21-23
0 1 0
of Algorithm 1. A complete procedure of the motion control
0 0 0 1
phase is summarized in Algorithm 1.
where, Tw t ∈ SE(3) is the pose of the target frame with respect
to world frame and Tw IV. ROBOT MODELS
r ∈ SE(3) is the pose of the robot frame
with respect to world frame. By using the matrices given in A. FULLY ACTUATED USV MODEL
(15), we calculate the pose of the robot frame with respect to For the fully actuated USV model, we implement the outlined
target frame Ttr ∈ SE(3) as model illustrated in Fig. 6(a) that consists of four thrusters

−1 w which generates the indicated force vector, F1 , F2 , F3 and
Ttr = Tw t Tr . (16) F4 . With this outlined model, the force vector τ fa takes the

47694 VOLUME 11, 2023

S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

Algorithm 1 Motion Control

1: CurrentNode ← StartNode()
2: NextNode ← Po.Next(CurrentNode)
3: ref ← Centroid(CurrentNode ∩ NextNode)
4: while qgoal not reached do
5: if qt ∈ GoalNode then
6: ref ← qgoal
7: else if qt ∈ NextNode then
8: CurrentNode ← NextNode
9: NextNode ← Po.Next(CurrentNode)
10: ref ← Centroid(CurrentNode ∩ NextNode)
11: else if qt ∈ UnsampledRegion then
12: qt ← UniformRandConf() FIGURE 6. Placement of thrusters for (a) fully actuated USV model and
(b) differential USV model.
13: Nodei ← GenerateRectRegion(qt )
14: Nodei ← Expand(Nodei )
15: G.InsertNode(Nodei )
16: Po = DijkstraAlgorithm(G) We implemented the following state and input constraints
17: CurrentNode ← Nodei to the system,
18: NextNode ← Po.Next(CurrentNode)
19: ref ← Centroid(CurrentNode ∩ NextNode)
20: else Ai [x y]T ≤ Bi (18a)
21: CurrentNode ← SearchNodes(G, qt ) −1 rad/s ≤ r ≤ 1 rad/s (18b)
22: NextNode ← Po.Next(CurrentNode) −1.5 m/s ≤ u ≤ 1.5 m/s (18c)
23: ref ← Centroid(CurrentNode ∩ NextNode) −20 N ≤ F1 , F2 , F3 , F4 ≤ 20 N (18d)
24: end if
25: ut ← MPC(qt , ref , CurrentNode)
26: end while Equation (18a) is calculated for each node on the path.

B. DIFFERENTIAL USV MODEL

For the differential USV model with two thrusters, we imple-
form of
ment the model illustrated in Fig. 6(b). With this model, the
force vector τ ua takes the form of
 
(F1 + F2 − F3 − F4 )sinα
τ fa = (F1 + F3 − F2 − F4 )cosα  . (17) 
F1 + F2

(F1 + F4 − F2 − F3 )(sinα + cosα)b/2 τ ua = 0 . (19)
b(F1 − F2 )
With the oriented placement of the thrusters with respect to
the body, the vehicle can generate a force vector in direction For the differential USV model, the placement of thrusters
v as opposed to the differential model. In the implementation, prevents the generation of a force vector in the v direction.
we adopted the parameters given in [23] for inertia, damping, In the implementation, we adopted the parameters given
and added mass terms. The state vector and the input vector in [24] for inertia, damping, and added mass terms. The state
for the model takes the form q = [x y θ u v r]T and u = vector and the input vector for the model takes the form
[F1 F2 F3 F4 ]T , respectively. q = [x y θ u v r]T and u = [F1 F2 ]T , respectively.
In the simulations we discretize the continuous nonlinear In the simulations, we discretize the continuous nonlinear
dynamics of the system and uniform synchronous sampling dynamics of the system and uniform synchronous sampling
of measurements with a sampling frequency of fs = 10 Hz(or of measurements with a sampling frequency of fs = 10Hz
Ts = 0.1s). We navigate the vehicle throughout the map (or Ts = 0.1s). The linearized system at the origin is
with quasi-infinite horizon MPC. The finite horizon length not stabilizable for the under-actuated model, so the termi-
is Tp = 1.5s, which gives us enough degrees of freedom in nal region and terminal cost matrix cannot be calculated.
enforcing the state and input constraints. We choose the state In order to control the system and predict its future behavior,
and input matrices Q = diag(5, 5, 5, 2.5, 2.5, 0.2) and R = we selected the finite horizon length as Tp = 9s. It is
diag(0.1, 0.1, 0.1, 0.1), respectively. After determining the six times larger than the horizon length used for the fully
cost matrices, by following the procedure presented in [11], actuated USV model. We choose the state and input matrices
we obtain the state feedback gain K and the terminal penalty Q = diag(5, 5, 10, 2.5, 2.5, 0.2) and R = diag(0.1, 0.1),
matrix P. respectively.

VOLUME 11, 2023 47695

 S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

FIGURE 7. (a) A total of 114 nodes are generated in node generation phase. (b) The calculated optimal route consists of 14 nodes. (c) Green curve
indicates the trajectory followed by the robot. Pink points represent the calculated reference points. Red arrows show the calculated target frame
T orientations in positive xt direction. (d) Applied thruster input forces F1 , F2 , F3 , F4 to the system. Dark dashed lines indicate upper and lower
constraints for the input. (e) Surge speed u and angular rate r of the robot. Black and green dashed lines correspond to the constraints for surge
speed and angular rate, respectively.

We implemented the following state and input constraints from the boundaries of the sampled rectangular regions. Even
to the system, in the smaller goal region (∼ 24 times smaller concerning
the average size of the nodes in Po) robot approaches to goal
Ai [x y]T ≤ Bi (20a) point smoothly by considering the dynamics and the imposed
−2 rad/s ≤ r ≤ 2 rad/s (20b) constraints on the system.
−0.5 m/s ≤ u ≤ 3 m/s (20c) Plot presented in Fig. 7(d) shows the forces F1 , F2 , F3 , F4
calculated in the motion control phase. In order to simulate
−20 N ≤ F1 , F2 ≤ 20 N (20d)
a realistic system, we set upper and lower bounds 20 N
Equation (20a) is calculated for each node on the path. and −20 N for the input forces, respectively. Dashed black
lines in the figure indicate these constraints. Furthermore,
V. SIMULATION RESULTS we also added velocity constraints to the system. For the surge
This section reports the simulation results obtained from speed u, we set upper and lower constraints as 1.5 m/s and
implementing the MPC-Graph algorithm. We implemented −1.5 m/s, respectively. For the angular rate r, we set upper
our algorithm on MATLAB and performed simulations on a and lower constraints as 1 rad/s and −1 rad/s, respectively.
laptop with Intel i7 2.4 GHz processor running Windows OS. In Fig. 7(e), the imposed constraints on surge speed and
angular rate are indicated with black and green dashed lines,
A. FULLY ACTUATED USV MODEL respectively. It can be inferred from the figures that MPC can
force both state and input constraints successfully. CPU time
1) PERFORMANCE WITHOUT PROCESS NOISE
of MPC for each is at average tCPU = 0.021s.
In this simulation scenario, we used a map that consists
of 4 obstacles. In the graph generation phase, a total of
114 nodes are generated. In order to show the ability of the
controller to force the constraints coming from the boundaries 2) PERFORMANCE IN THE PRESENCE OF PROCESS NOISE
of the rectangular nodes, we replaced the goal node with a We also analyzed the performance of the MPC-Graph algo-
smaller square region. After executing Dijkstra’s search algo- rithm in the presence of process noise. To this end, we added
rithm, the optimal route consists of 14 nodes. Fig. 7(a) and (b) noise (SNR=1) to the calculated thruster forces, u =
visualizes the obtained rectangular regions and the node-set [F1 F2 F3 F4 ]T and performed Monte-Carlo experiments
that constitutes the followed route by the robot, respectively. (# simulations = 1000) on the same map with the same
Fig. 7(c) shows the route followed by the robot and the cal- nodes. We determine the thrusters’ input saturation limit as
culated reference points with target orientations. Red arrows F = 25 N . From 1000 Monte-Carlo experiments, in 8 of
are the positive xt directions for the target frames T . The them robot failed to reach the goal location due to process
figure points out that the robot obeys the constraints coming noise. Failures include the cases in which the robot ends up

47696 VOLUME 11, 2023

S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

FIGURE 8. Obtained trajectories from Monte-Carlo experiments in the presence of process noise. (a) Successful trajectories ended up in goal
point. (b) Red trajectory indicates the followed route in the presence of process noise. Dashed blue trajectory indicates the followed trajectory
without the process noise. (c) Plots show the calculated forces and applied forces on each thruster. Black dashed lines indicate the saturation
limits for the thrusters.

FIGURE 9. (a) Satellite view of Pearl Harbor is presented. (b) Node generation phase. As a result 255 nodes are generated. (c) Graph search
phase determines the shortest route consisting of 21 nodes. (d) Green curve indicates the trajectory followed by the robot. Orange points
represent the calculated reference points. Red arrows show the calculated target frame T orientations in positive xt direction. (e) Applied
thruster input forces F1 , F2 to the system. Dark dashed lines indicate upper and lower constraints for the input. (f) Surge speed u and
angular rate r of the robot. Black and green dashed lines correspond to the constraints for surge speed and angular rate, respectively.

outside the limits of the arena or inside the obstacles. Fig. 8(a) Po. In Fig. 8(b), red and dashed blue trajectories indicate the
visualizes the successful attempts for reaching the goal point. routes followed by the robot in the presence of process noise
If the robot ends up in a different node than the and without noise, respectively. Fig. 8(c) shows the thruster
CurrentNode due to process noise, the algorithm generates forces and process noise applied on each thruster. Even in
another route considering the previously determined policy the presence of high-level noise, in 99.2% of the experiments

VOLUME 11, 2023 47697

 S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

FIGURE 10. Obtained trajectories from Monte-Carlo experiments in the presence of process noise.
(a) Successful trajectories ended up in goal point. (b) Red trajectory indicates the followed route in
the presence of process noise. Dashed blue trajectory indicates the followed trajectory without the
process noise. (c) Plots show the calculated forces and applied forces on each thruster. Black
dashed lines indicate the saturation limits.

fully actuated USV model reaches the goal location. Results MPC parameters, the controller can drive the system to the
indicate that MPC is an effective controller for handling high goal location while obeying the imposed constraints. CPU
noise scenarios. time of MPC for each iteration for this model is at average
tCPU = 0.28s.
B. DIFFERENTIAL USV MODEL
1) PERFORMANCE WITHOUT PROCESS NOISE 2) PERFORMANCE IN THE PRESENCE OF PROCESS NOISE
In this simulation scenario, we used the map of Pearl Har- For the simulation scenario with noise, we performed Monte-
bor. Graph generation phase constructed a graph structure Carlo experiments (# simulations = 1000) on the same map
consisting of 255 nodes and Dijkstra’s search algorithm find with the same nodes and plotted the trajectories followed
the optimal route that consists of 21 nodes. Fig. 9(a) and (b) by the robot. The process noise (SNR=1) is applied to the
visualize the satellite view of Pearl Harbor and obtained calculated thruster forces u = [F1 F2 ]T which have an input
rectangular regions, respectively. Fig. 9(c) and (d) illustrate saturation limit of F = 25 N .
the node-set robot navigates in and path followed by the From 1000 Monte-Carlo experiments, in 29 of them robot
robot, respectively. failed to reach the goal location. Fig. 10 (a) visualizes the
We set the upper and lower bounds 20 N and −10 N successful attempts to reach the goal point. In Fig. 10(b), red
for the input forces, respectively. In order to restrict the and dashed blue trajectories indicate the routes followed by
backward motion of the vehicle, we set the lower limit for the robot in the presence of process noise and without noise,
the thruster forces to be −10 N . Velocity constraints for the respectively. Fig. 10(c) shows the thruster forces and process
system are 3 m/s and −0.5 m/s for the surge speed u and noise applied on each thruster. Even in the presence of high-
2 rad/s and −2 rad/s for the angular rate r, respectively. level noise, in 97.1% of the experiments differential USV
Figures 9(e) and 9(f) show the calculated forces F1 , F2 and model reaches the goal location.
surge speed-angular rate of the vehicle throughout the sim-
ulation. It is important to note that the linearized differ-
ential USV model is not stabilizable. Thus, it is not pos- VI. CONCLUSION
sible to determine a terminal region and a terminal cost This study presents a feedback motion planning and control
matrix for this system. Although the stability is not guar- methodology for dynamic fully- and under-actuated USV
anteed for the differential USV model, with the adjusted models, utilizing sparse random neighborhood graphs and

47698 VOLUME 11, 2023

S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

constrained nonlinear Model Predictive Control (MPC). The ACKNOWLEDGMENT

proposed approach enables safe and robust navigation in An earlier version of this paper has been presented in Simay
marine environments by providing an autonomous motion Atasoy’s M.Sc. thesis [1].
planning strategy that ensures no constraint violation on state
and input variables. The algorithm generates a sparse neigh- REFERENCES
borhood graph consisting of connected rectangular zones in [1] S. Atasoy, ‘‘MPC-Graph: Nonlinear feedback motion planning using
the planning phase, and inside each node, an MPC-based sparse sampling based neighborhood graph,’’ M.S. thesis, Middle East
online feedback control policy funnels the USV with nonlin- Tech. Univ., 2022.
[2] Annual Overview of Marine Casualties and Incidents 2020, Eur. Maritime
ear dynamics from one rectangle to the other in the network. Safety Agency, 2020.
The effectiveness and robustness of the proposed algorithm [3] E. Ege and M. M. Ankarali, ‘‘Feedback motion planning of unmanned
is systematically tested in different simulation scenarios, surface vehicles via random sequential composition,’’ Trans. Inst. Meas.
Control, vol. 41, no. 12, pp. 3321–3330, Aug. 2019.
including an extreme actuator noise scenario, and the results
[4] F. Golbol, M. M. Ankarali, and A. Saranli, ‘‘RG-trees: Trajectory-free
demonstrate the validity and effectiveness of the proposed feedback motion planning using sparse random reference governor trees,’’
approach. The execution time of the MPC for each iteration in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2018,
provides a bottleneck for the algorithm. For the fully actuated pp. 6506–6511.
[5] O. K. Karagoz, S. Atasoy, and M. M. Ankarali, ‘‘MPC-graph: Feed-
model, the average CPU time is 0.021s which is strictly lower back motion planning using sparse sampling based neighborhood graph,’’
than the sampling time. We believe that with the indicated in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2020,
CPU time, the proposed algorithm can also be applicable to pp. 6797–6802.
real-time. However, there are still several directions for future [6] C. Sun, X. Zhang, Q. Zhou, and Y. Tian, ‘‘A model predictive controller
with switched tracking error for autonomous vehicle path tracking,’’ IEEE
research that can be explored to further enhance the proposed Access, vol. 7, pp. 53103–53114, 2019.
approach. [7] F. Farshidian, E. Jelavic, A. Satapathy, M. Giftthaler, and J. Buchli,
Firstly, although the proposed algorithm showed promis- ‘‘Real-time motion planning of legged robots: A model predictive con-
trol approach,’’ in Proc. IEEE-RAS 17th Int. Conf. Humanoid Robot.
ing results, the under-actuated model’s performance can be (Humanoids), Nov. 2017, pp. 577–584.
further improved. One potential approach to achieve this is [8] T. A. V. Teatro, J. M. Eklund, and R. Milman, ‘‘Nonlinear model predic-
to investigate the use of different control techniques that tive control for omnidirectional robot motion planning and tracking with
avoidance of moving obstacles,’’ Can. J. Electr. Comput. Eng., vol. 37,
can guarantee the under-actuated model’s stability while still no. 3, pp. 151–156, 2014.
ensuring obstacle avoidance and robustness. [9] J. Ji, A. Khajepour, W. W. Melek, and Y. Huang, ‘‘Path planning and track-
Secondly, the simulation results showed that the main ing for vehicle collision avoidance based on model predictive control with
drawback of using an MPC-based controller is the high com- multiconstraints,’’ IEEE Trans. Veh. Technol., vol. 66, no. 2, pp. 952–964,
Feb. 2017.
putational cost. Hence, it is important to further decrease the [10] H. Michalska and D. Q. Mayne, ‘‘Robust receding horizon control of
CPU time of the algorithm to make it more practical and constrained nonlinear systems,’’ IEEE Trans. Autom. Control, vol. 38,
applicable in real-world scenarios. This can be achieved by no. 11, pp. 1623–1633, Nov. 1993.
[11] H. Chen and F. Allgower, ‘‘A quasi-infinite horizon nonlinear model
exploring the use of different and more efficient solvers, such predictive control scheme with guaranteed stability,’’ Automatica, vol. 34,
as embedded optimization solvers [25], computational opti- no. 10, pp. 1205–1217, 1998.
mization techniques [26], or efficient MPC formulations [27]. [12] A. Vagale, R. Oucheikh, R. T. Bye, O. L. Osen, and T. I. Fossen, ‘‘Path plan-
Furthermore, as USVs become more widely used in dif- ning and collision avoidance for autonomous surface vehicles I: A review,’’
J. Mar. Sci. Technol., vol. 26, pp. 1292–1306, Jan. 2021.
ferent applications, it is essential to consider more complex [13] B. Zhao, X. Zhang, C. Liang, and X. Han, ‘‘An improved model predictive
scenarios with multiple USVs and dynamic obstacles. Thus, control for path-following of USV based on global course constraint and
future research can focus on developing multi-agent motion event-triggered mechanism,’’ IEEE Access, vol. 9, pp. 79725–79734, 2021.
[14] X. Sun, G. Wang, Y. Fan, D. Mu, and B. Qiu, ‘‘Collision avoidance using
planning and control methods that can handle multiple USVs finite control set model predictive control for unmanned surface vehicle,’’
navigating in a shared environment and avoiding collisions Appl. Sci., vol. 8, no. 6, p. 926, Jun. 2018.
with each other and dynamic obstacles. [15] X. Zhou, Y. Wu, and J. Huang, ‘‘MPC-based path tracking control method
Another potential avenue for future work is to develop a for USV,’’ in Proc. Chin. Autom. Congr. (CAC), Nov. 2020, pp. 1669–1673.
[16] Z. Liu, C. Geng, and J. Zhang, ‘‘Model predictive controller design
corridor-based method to increase the robustness of the algo- with disturbance observer for path following of unmanned surface ves-
rithm, especially for the cases where narrow tunnels exist. sel,’’ in Proc. IEEE Int. Conf. Mechatronics Autom. (ICMA), Aug. 2017,
Several works in literature implement corridor-based meth- pp. 1827–1832.
[17] W. Wang, T. Shan, P. Leoni, D. Fernandez-Gutierrez, D. Meyers, C. Ratti,
ods for docking applications of USVs [28] and safe trajectory
and D. Rus, ‘‘Roboat II: A novel autonomous surface vessel for urban
planning for autonomous dispatch on flight deck [29] which environments,’’ in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS),
show promising results and can be further implemented for Oct. 2020, pp. 1740–1747.
safe trajectory planning for USVs. [18] B. Hu, Y. Wan, and Y. Lei, ‘‘Collision avoidance of USV by model
predictive control-aided deep reinforcement learning,’’ in Proc. IEEE Int.
The proposed algorithm provides a promising approach for Conf. Ind. Technol. (ICIT), Aug. 2022, pp. 1–6.
improving the autonomy level of USVs and ensuring safe and [19] X. Wang, J. Liu, H. Peng, X. Qie, X. Zhao, and C. Lu, ‘‘A simulta-
robust navigation across unpredictable marine environments. neous planning and control method integrating APF and MPC to solve
autonomous navigation for USVs in unknown environments,’’ J. Intell.
However, further research is needed to address the limitations Robotic Syst., vol. 105, no. 2, p. 36, Jun. 2022.
and enhance the proposed approach’s effectiveness and prac- [20] T. I. Fossen, Handbook of Marine Craft Hydrodynamics and Motion Con-
ticality in real-world scenarios. trol. Hoboken, NJ, USA: Wiley, 2011.

VOLUME 11, 2023 47699

 S. Atasoy et al.: Trajectory-Free Motion Planning of an USV Based on MPC and Sparse Neighborhood Graph

[21] L. Yang and S. M. LaValle, ‘‘A framework for planning feedback motion OSMAN KAAN KARAGÖZ (Member, IEEE)
strategies based on a random neighborhood graph,’’ in Proc. IEEE Int. received the B.S. and M.S. degrees in electrical and
Conf. Robot. Automat., Apr. 2000, pp. 544–549. electronics engineering from Middle East Tech-
[22] L. Yang and S. M. LaValle, ‘‘The sampling-based neighborhood graph: nical University (METU), Ankara, in 2017 and
An approach to computing and executing feedback motion strategies,’’ 2020, respectively, where he is currently pursuing
IEEE Trans. Robot. Autom., vol. 20, no. 3, pp. 419–432, Jun. 2004. the Ph.D. degree with the Department of Electrical
[23] M. Kumru, ‘‘Navigation and control of an unmanned sea surface vehicle,’’ and Electronics Engineering. His research inter-
M.S. thesis, Middle East Tech. Univ., 2015. ests include nonlinear dynamical systems, control
[24] R. Skjetne, T. I. Fossen, and P. V. Kokotovic, ‘‘Adaptive maneuvering, with
theory, and motion control in robotics and biolog-
experiments, for a model ship in a marine control laboratory,’’ Automatica,
ical systems.
vol. 41, no. 2, pp. 289–298, Feb. 2005.
[25] B. Stellato, V. V. Naik, A. Bemporad, P. Goulart, and S. Boyd, ‘‘Embedded
mixed-integer quadratic optimization using the OSQP solver,’’ in Proc.
Eur. Control Conf. (ECC), Jun. 2018, pp. 1536–1541.
[26] S. Richter, Computational Complexity Certification of Gradient Methods
for Real-Time Model Predictive Control. Zurich, Switzerland: ETH Zurich,
2012.
[27] O. Arpacik and M. M. Ankarali, ‘‘An efficient implementation of online
model predictive control with field weakening operation in surface
mounted PMSM,’’ IEEE Access, vol. 9, pp. 167605–167614, 2021.
[28] X. Wang, Z. Deng, H. Peng, L. Wang, Y. Wang, L. Tao, C. Lu, and
Z. Peng, ‘‘Autonomous docking trajectory optimization for unmanned
surface vehicle: A hierarchical method,’’ Ocean Eng., vol. 279, Jul. 2023,
Art. no. 114156.
[29] X. Wang, B. Li, X. Su, H. Peng, L. Wang, C. Lu, and C. Wang, MUSTAFA MERT ANKARALI (Member, IEEE)
‘‘Autonomous dispatch trajectory planning on flight deck: A search-
received the B.S. degree in mechanical engineering
resampling-optimization framework,’’ Eng. Appl. Artif. Intell., vol. 119,
and the M.S. degree in electrical and electronics
Mar. 2023, Art. no. 105792.
engineering from Middle East Technical Univer-
sity (METU), in 2007 and 2010, respectively,
SİMAY ATASOY (Member, IEEE) received the and the Ph.D. degree in mechanical engineering
B.S. degree in mechanical engineering and the from Johns Hopkins University (JHU), in 2015.
M.S. degree in electrical and electronics engi- Following his Ph.D. degree, he was a Postdoctoral
neering from Middle East Technical University Fellow with the Laboratory for Computational
(METU), in 2017 and 2022, respectively. She is Sensing and Robotics (LCSR), JHU, until 2016.
currently a Research Engineer with Aselsan Inc. In 2017, he joined the Electrical and Electronics Engineering Department,
Her research interests include model predictive METU, where he is currently an Associate Professor. His research interests
control, motion planning, and collision avoidance. include analyzing, identifying, and controlling dynamic behaviors in robotic
and biological systems.

47700 VOLUME 11, 2023