Senseable City Lab :.:: Massachusetts Institute of Technology
This paper might be a pre-copy-editing or a post-print
author-produced .pdf of an article accepted for publication. For
the definitive publisher-authenticated version, please referdirectly to publishing house’s archive system

SENSEABLE CITY LAB

Robust Model Predictive Control with Control Barrier Functions forAutonomous Surface Vessels
Wei Wang∗ , Wei Xiao∗ , Alejandro Gonzalez-Garcia∗ , Jan Swevers, Carlo Ratti and Daniela Rus
Abstract— In autonomous robot navigation, the trajectories
from path planners are considered to be safe regions, and deviations could endanger vessels. Model Predictive Control (MPC)
stands as a popular choice for trajectory tracking problems as
it naturally addresses operational constraints, such as dynamics
and control constraints. Nevertheless, achieving robustness inchanging environments like oceans and rivers, which are constantly subject to significant external disturbances, remains an
ongoing challenge for MPC. It must consistently keep the system
within a predefined safe region (such as a reference trajectory)
even in the presence of model inaccuracies and perturbations.
To address this challenge, we present a robust model predictive
control strategy utilizing Control Barrier Functions (CBFs),
which increases the disturbance-rejection abilities. We verify
our method on an autonomous surface vessel in simulation andnatural waters, both with external disturbances. Specifically,
compared with the traditional MPC method, our proposed
MPC-CBF strategy reduces tracking errors by 17.82% and40.26% in simulations and field experiments, respectively. Although the control effort slightly increases by 7.78% and 4.20%,
respectively, these results clearly demonstrate the enhanced
resilience of MPC-CBF to disturbances.

I. I NTRODUCTION
Due to a variety of applications like marine transportation,bathymetry, search and rescue, disaster management, andoceanic exploration, Autonomous Surface Vessels (ASVs)
have attracted attention in both industrial and academic
communities [1]–[5]. Trajectory tracking is an essential
component for ASVs to perform time-sensitive tasks such as
dynamic obstacle avoidance, and multi-robot coordination.
The safety of both the vessel and its passengers or goods
plays a crucial role in ensuring the success of autonomous
operations. Maintaining proximity to planned trajectories andadhering to time constraints is of paramount importance, as
any deviations have the potential to lead to collisions orundesirable behavior in dynamic environments. Nevertheless,factors such as model uncertainty and external disturbances
arising from wind, waves, and currents pose a significant
This work was supported by a grant from the Amsterdam Institute forAdvanced Metropolitan Solutions (AMS) in Netherlands, and by the MITBelgium - KU Leuven Seed Fund from MIT International Science andTechnology Initiatives (MISTI). A. Gonzalez-Garcia was supported by the
Flanders Make SBO project ARENA (Agile & Reliable Navigation).
W. Wang, W. Xiao, and D. Rus are with the Computer Science and Artificial Intelligence Lab (CSAIL), Massachusetts Institute of Technology, Cambridge, MA 02139 USA. {wweiwang, weixy, rus}@mit.edu
W. Wang, and C. Ratti are with the SENSEable City Laboratory,
Massachusetts Institute of Technology, Cambridge, MA 02139 USA.

{wweiwang, ratti}@mit.edu
A. Gonzalez-Garcia, and J. Swevers are with MECO Research Team,
Department of Mechanical Engineering, KU Leuven, Belgium and
Flanders Make@KU Leuven, Belgium. {alex.gonzalezgarcia,

jan.swevers}@kuleuven.be∗ These authors contributed equally to this work.

threat to the control of ASVs. Various approaches have been
proposed to enhance robustness, consequently mitigating
deviations in the face of these disturbances and uncertainties,e.g., sliding mode-based controllers [6]–[8] and data-driven
algorithms [9], [10].
A popular control system strategy within the robotics
field is model predictive control (MPC). MPC utilizes model
knowledge and multi-objective optimization to minimize
tracking error, control effort, and factor system constraints.
MPC has been successfully used for ASV trajectory tracking[5], [11], [12], achieving low tracking error and control
effort. However, MPC can be affected by model errors
and external disturbances, and the computational complexity
increases significantly as constraints increase.
To reduce computational problems, the Control Barrier
Function (CBF) method has been widely adopted as analternative to MPC. Barrier functions (BFs) are Lyapunovlike functions [13], [14], whose use can be traced backto optimization problems [15]. CBFs, on the other hand,
represent an extension of BFs tailored for control systems.
They translate a constraint defined in terms of system states
into a constraint on the control inputs. CBFs offer a statefeedback controller that is rigorously proven to be safe whileremaining computationally efficient. Specifically, CBFs are
well-suited for constraints characterized by a relative degreeof one concerning the system dynamics [16], [17]. The
High Order CBF (HOCBF), as proposed in [18] and [19],is designed to effectively handle constraints with arbitrarily
high relative degrees, making it a versatile extension of theconventional CBF framework. Robust CBFs to account for
perturbations have also been extensively studied in the CBF
framework [20].
This paper proposes to combine MPC and HOCBFs, inthe context of ASV trajectory tracking subject to external
disturbances. The HOCBFs are introduced to enforce safety
constraints on the system state, steering the vessel towards aboundary of the predefined trajectory. The main contributionsof this work include:• An MPC-CBF framework for ASV trajectory tracking;• CBFs/HOCBFs for trajectory tracking under perturbations,instead of their traditional use for obstacle avoidance;• An extensive study of numerical simulations subject toincreasing model-based environmental disturbances;• Field experiments with an ASV, verifying the effectivenessof the MPC-CBF framework;• A comparative analysis between MPC-CBF and MPC,
illustrating the increased robustness to disturbances of MPCCBF.

II. ASV OVERVIEW AND P RELIMINARIESIn this section, we introduce the ASV prototype, its dynamic model, and some preliminary concepts. The dynamic
model introduced here will serve as the foundation for the
MPC-CBF design in subsequent section.

disturbances stemming from factors such as currents, waves,and winds. More specifically, M, C(v) and D(v) can befurther elaborated as follows:
M = diag{m11 , m22 , m33 },

A. Surface Vessel
The vessel has four thrusters around the hull to achieve
holonomic motions as shown in Fig. 1. The primary con-

𝑓𝑓3
𝑓𝑓1

m22 v


−m22 v
m11 u  ,000−m11 uD(v) = diag{Xu ,Yv , Nr }.

𝑓𝑓4

Fig. 1.
The surface vessel. (a) Prototype (side view); (b) thruster
configuration (bottom view). f1 , f2 , f3 and f4 denote the forces generated
by left, right, anterior and rear thrusters, respectively.

troller for the system is an Intel NUC running the Robot
Operating System (ROS). Additionally, for real-time actuator control, an auxiliary microprocessor (STM32F103) isincorporated. The surface vessel is equipped with a rangeof onboard sensors, including a 3D LiDAR (Velodyne, Puck
VLP-16), and an IMU. The vessel weighs approximately 15
kg and has dimensions of 0.90 m in length, 0.45 m in width,and 0.15 m in height. Power is supplied by an 11.1 V LiPo battery, offering an operational duration of approximately
three hours. For more detailed information on the surface
vessel, refer to [11].
B. ASV Dynamics(6)

where B ∈ R4×3 represents the control matrix, defining thearrangement of the thrusters, u = [ f1 f2 f3 f4 ]T ∈ R4
represents the control vector, where f1 , f2 , f3 , and f4
correspond to the forces generated by the left, right, anterior,and rear thrusters, respectively. ad is the distance between themain thrusters (left and right thrusters) and bd is the distance
between the tunnel thrusters (anterior and rear thrusters). The
specific configuration of the four thrusters is illustrated inFig. 1.
By merging (1) and (2), we can reformulate the dynamic
model as follows:ẋ = f (x) + g(x)u + w(7)

where f (x) = [03×3 R(η); 03×3 − M−1 (C(v) + D(v))],g(x) = [03×4 ; M−1 B], w = [03×3 ; M−1 τenv ]. Moreover, thestate vector x(t) ∈ Rnq and the control vector u(t) ∈ Rnu are
defined as follows:x

We establish two coordinate systems to describe the vessel:
the inertial frame denoted as OXY and the body-fixed frame
represented as ob xb yb . The dynamic model for the vessel is
described by the following nonlinear differential equation, as
detailed in [11]:

(4)

(5)

τ is mathematically defined as follows:



f1
1
1
0
0
f2 

0
1
1 

τ = Bu =  0ad bdbd  f3 
ad
−
−
f4
2
2
2
2

𝑓𝑓2
(b)

(a)

C(v) = 

0
0

(3)

u

= [x
= [ f1

y

u

ψ
f2

f3

r]T ,

v
T

f4 ] .

(8)(9)

The system model (7) defines how the state x evolves withthe control input u.
C. High Order Control Barrier Functionsη̇ = R(η)v(1)

Mv̇ + C(v)v + D(v)v = τ + τenv

(2)

where η = [x y ψ]T ∈ R3 denotes the pose, comprising
position and heading, of the vessel in the inertial frame;v = [u v r]T ∈ R3 represents the vessel’s velocity inthe body-fixed frame; R(η) ∈ R3×3 stands for the rotation
matrix, which expresses the transformation from the bodyfixed frame to the inertial frame; M ∈ R3×3 represents thesymmetric positive-definite added mass and inertia matrix; C(v) ∈ R3×3 represents the skew-symmetric matrix accounting for the Coriolis and centripetal forces; D(v) ∈R3×3 represents the positive-semi-definite drag matrix; τ =[τu τv τr ]T ∈ R3 represents the total force and torque applied by the thrusters; τenv ∈ R3 characterizes environmental

We briefly introduce the concept of high order CBFs
in this section, and we start with some definitions for
CBFs/HOCBFs. Consider an affine control system:ẋ = f (x) + g(x)u(10)

where x ∈ X ⊂ Rn , f : Rn → Rn and g : Rn → Rn×q are Lipschitz continuous, and u ∈ U ⊂ Rq is the control constraint
set.
Definition 1: A set C ⊂ Rn is forward invariant for system(10) if its solutions for some controller u ∈ U starting at any
x(0) ∈ C satisfy x(t) ∈ C, ∀t ≥ 0.
Definition 2: (Relative degree [21]) The relative degree ofa (sufficiently many times) differentiable function b : Rn → R
with respect to (10) is the number of times it needs to bedifferentiated along its dynamics until any component of thecontrol u explicitly shows in the corresponding derivative.
Since function b is used to define a constraint b(x) ≥ 0,
we will also refer to the relative degree of b as the relative
degree of the constraint. For a constraint b(x) ≥ 0 withrelative degree m, b : Rn → R, and ψ0 (x) := b(x), we definea sequence of functions ψi : Rn → R, i ∈ {1, . . . , m}:
ψi (x) := ψ̇i−1 (x) + αi (ψi−1 (x)), i ∈ {1, . . . , m},(11)where αi (·), i ∈ {1, . . . , m} denotes a (m − i)th order differentiable class K function.
We further define a sequence of sets Ci , i ∈ {1, . . . , m}
associated with (11) in the form:
Ci := {x ∈ Rn : ψi−1 (x) ≥ 0}, i ∈ {1, . . . , m}.

(12)

Definition 3: (High Order Control Barrier Function(HOCBF) [18]) Let C1 , . . . ,Cm be defined by (12) andψ1 (x), . . . , ψm (x) be defined by (11). A function b : Rn → R isa High Order Control Barrier Function (HOCBF) of relative
degree m for system (10) if there exist (m − i)th orderdifferentiable class K functions αi , i ∈ {1, . . . , m − 1} anda class K function αm such thatsup [Lmf b(x) + Lg Lm−1
b(x)u + O(b(x)) + αm (ψm−1 (x))] ≥ 0,
f

u∈U

(13)
for all x ∈ C1 ∩, . . . , ∩Cm . In (13), the left part is actuallyψm (x), L f , Lg denote Lie derivatives along f and along g,irespectively, and O(b(x)) = ∑m−1
i=1 L f (αm−i ◦ ψm−i−1 )(x).
The HOCBF is a general form of the relative degree one
CBF [16], [17], i.e., setting m = 1 reduces the HOCBF to
the common CBF form: L f b(x) + Lg b(x)u + α1 (b(x)) ≥ 0.
Theorem 1: ( [18]) Given an HOCBF b(x) from Def.
3 with the associated sets C1 , . . . ,Cm defined by (12), ifx(0) ∈ C1 ∩, . . . , ∩Cm , then any Lipschitz continuous controller u(t) ∈ U that satisfies the constraint in (13), ∀t ≥ 0
renders C1 ∩, . . . , ∩Cm forward invariant for system (10).
CBFs/HOCBFs are commonly employed to transform constrained optimal control problems onto quadratic programs
that are very efficient to solve [16]. They are used toguarantee system safety. However, in this paper, we use
CBFs/HOCBFs to achieve trajectory tracking for surface
vessels under perturbations.
III. ROBUST M ODEL P REDICTIVE C ONTROL VIA
C ONTROL BARRIER F UNCTIONSIn this section, we establish a mathematical formulation
for the trajectory tracking problem and subsequently design
an MPC-CBF scheme to address the problem, consideringrobustness and optimality in the solution.

each given time. The reference trajectory which is parameterized at each time step k can be formulated as follows:
r r
r r r
r T
xref
k = [xk yk ψk uk vk rk ](14)

We then establish the tracking error at each time step k
as ek = [xke yek ψke uek vek rke ]T as the disparity between thereference state and actual state of the vessel. The objective ofthe trajectory tracking problem here is to design appropriate
control laws for the four thrusters with the aim of driving ek
to zero at each time step.
B. Nonlinear Model Predictive Control Design
For our trajectory tracking problem, we formulate the optimal control problem for MPC in the form of a least squarefunction to penalize the deviations of predicted state (xk ) andcontrol (uk ) trajectories from their specified references, overthe given prediction horizon window Nc (t j ≤ t ≤ t j+Nc ):1 n j+Nc −12
ref 2∑ (∥xk − xref
k ∥Wq + ∥uk − uk ∥Wu ) +xk ,uk 2
k= jo2∥xNc − xref
Nc ∥WNcmin(15a)s.t. x j = x̂ j ,(15b)

xk+1 = f (xk ) + g(xk )uk , k = j, · · ·, j + Nc − 1,(15c)xk,min ≤ xk ≤ xk,max , k = j, · · ·, j + Nc ,(15d)uk,min ≤ uk ≤ uk,max , k = j, · · ·, j + Nc − 1,(15e)where xk ∈ Rnq denotes the vessel state, uk ∈ Rnu denotes thecontrol input, x̂ j ∈ Rnq denotes the current state estimate, xrefkand urefk denote the time-varying state and control references,respectively; xref
Nc denotes the terminal state reference; Wq ∈Rnq ×nq , Wu ∈ Rnu ×nu , and WNc ∈ Rnq ×nq are the positive
definite weight matrices that penalize deviations from thedesired values. Furthermore, xk,min and xk,max denote thelower and upper bounds of the states, respectively; uk,min
and uk,max denote the lower and upper bounds of the control
input, respectively.
The weighting matrices Wq , Wu and WNc for the MPC usedin the experiments are selected as
Wq = diag{20, 20, 10, 10, 10, 10},(16)

WNc = diag{100, 100, 50, 50, 50, 50},

(17)

Wu = diag{1, 1, 1, 1}.

(18)

The prediction horizon was chosen as Nc = 4 s, with asampling time of 0.1 s, and the constraints on the control
input u used in the experiments are chosen as follow−64×1 N ≤ uquart ≤ 64×1 N.

(19)

Our implementation of the MPC controller is realized using
the open-source ACADO toolkit [22].

A. Trajectory Tracking Problem Formulation

C. Robust Control Barrier Function

Trajectory tracking is concerned with the design of control
laws that force a mobile robot to converge to a timeparameterized reference. Trajectory tracking usually requires
the robot to track the specified pose and velocity defined at

In scenarios subject to model inaccuracies or external perturbations, MPC may lose constraint satisfaction guarantees,
and tracking accuracy. To address this, we propose to use
CBFs/HOCBFs to equip MPC with guarantees. In the ASV

case, the dynamics (10) have different forms of perturbations.
Thus, the ASV model is extended with an additive termw ∈ Rn as follows:ẋ = f (x) + g(x)u + w,

(20)

where the perturbations are assumed to be bounded such that|w| ≤ w (inequality interpreted componentwise), w ∈ Rn .
A robust CBF [20] for system (20) should satisfy thefollowing CBF constraint:
L f b(x) + Lg b(x)u + α1 (b(x)) −db(x)w ≥ 0.
dx(21)

The satisfaction of the above constraint implies the satisfaction of L f b(x) + Lg b(x)u + α1 (b(x)) + db(x)dx w ≥ 0, and thus,
we can provably show the satisfaction of b(x) ≥ 0 using theclassic CBF theory. We can define robust HOCBFs based on(21) as in (11) and Def. 3.
Now, we focus on how to design HOCBFs for the robust
trajectory tracking control of the surface vessel. Suppose thereference trajectory is defined by h(x) = 0, where h : Rn → R
is continuously differentiable. We can define a tube (a safe
tracking state space) using two robust HOCBFs:
b1 (x) = h(x) − c,b2 (x) = −h(x) − c,(22)where c > 0 defines the size of the tube.
Any control input should satisfy the following two robust
HOCBF constraints in order to make the system state staywithin the tube:
Lmf b1 (x) + Lg Lm−1b1 (x)u + O(b1 (x))
f+αm (ψ1,m−1 (x)) ≥ 0,m
m−1
L f b2 (x) + Lg L f b2 (x)u + O(b2 (x))(23)

+αm (ψ2,m−1 (x)) ≥ 0,
where ψl,m−1 , l ∈ {1, 2} are defined as in (11) for b1 (x) andb2 (x), respectively.
Finally, we take the solution from the MPC (15), andcombine MPC and CBF/HOCBF using the following optimization:min ||u − uR ||2 , s.t., (23)(24)
uwhere uR is a reference control at the current time step computed by the MPC (15). Thus, the CBF/HOCBF calculates
a new control input u, which is used to actuate the system.
The above optimization is solved by discretizing the time[16], and only consider u as the decision variable at eachtime step while making the state as a constant within eachtime interval. Note that these constraints are linear in control
since the state value is fixed at the beginning of the interval,
therefore, each optimization problem is a quadratic program(QP) as the cost is quadratic. The optimal control obtainedby solving each QP is applied at the current time step andheld constant for the sampling interval. The state is updated
using dynamics (10), and the procedure is repeated.

IV. E XPERIMENTS AND R ESULTS
In this section, we extensively conduct simulations andexperiments using the surface vessel illustrated in Figure 1
to validate the tracking capabilities of the designed MPCCBF control strategy.
We conduct a systematic comparison between the performance of the proposed MPC-CBF controller and our prior
MPC controller [11] described by equations (15)-(19), with afocus on tracking error and control effort. We carry out identical simulation and physical experimental runs for both the
MPC-CBF and benchmark MPC approaches. Also, it’s worth
noting that our trajectory tracking, as depicted in (14), is
full-state tracking involving both pose and velocity tracking.
However, due to space limit, this paper primarily focuses on
the analysis of position tracking error. For position tracking
error, we consider the Mean Absolute Error (MAE), defined
as follows: eMAE = (∑Nk=1 |ek |)/N, where N is the total1
2
represents
number of samples, and ek = (xke )2 + (yek )2
the position error of the k-th sample. Regarding control
effort evaluation, we use the Euclidean norm ||FCE || =
1
2
∑Nk=1 (FkCE )2 , where FkCE = | f1,k | + | f2,k | + | f3,k | + | f4,k |denotes the total absolute force applied at each sample k.
To identify the unknown hydrodynamic parameters in the
dynamic model, we employ a nonlinear least squares method
based on the Trust Zone Reflection Algorithm [11]. For our
vessel, the identified parameters are as follows: m11 = 12 kg,
m22 = 24 kg, m33 = 1.5 kg m2 , d11 = 6 kg s−1 , d22 = 8 kg s−1 ,and d33 = 1.35 kg m2 s−1 . The simulations are conducted in
Matlab 2022b, utilizing an Intel i9 CPU with 64GB RAM.
Field tests on the surface vessel are carried out using its
onboard Intel® NUC. The sine curve serves as the consistentreference trajectory for both simulations and experiments,
while maintaining a constant forward speed of 0.2 m/s.
Moreover, for both simulation and experimental setups withthe surface vessel, a data sampling rate of 10 Hz is employed.
A. Simulations
In numerical simulations, we introduce model-based environmental disturbances to assess the MPC-CBF robustness.
These disturbances are represented as τenv = τwaves + τwind +(C(vc ) + D(vc ))vc . Note that the current-induced forces involve current velocity vc , and Coriolis and Drag matrices.τenv
is described in the following models [6], [23]:Waves: Wave phenomena is modeled by a combinationof two elements: zero-mean oscillatory motion and wave
drift [23]. Using an approximation of the Pierson-Moskowitz
spectrum, state-space models can be written in the shape of:     
0
1
ẋ
xF1
0
ẋF = F1 =
+
ω
(25)
ẋF2
Kw F1
−ωe2 −2λw ωe xF2
 

 xF1
Fwave = 0 1
+ dF
(26)
xF2
  
   
0
1
ẋN1
xN1
0
ẋN =
=
+
ω
(27)
ẋN2
Kw N1
−ωe2 −2λw ωe xN2
Nwave = 0

 
 xN1
1
+ dN
xN2

(28)

where Fwave is the wave force, Nwave is the wave moment, andd˙F = ωF2 , d˙N = ωN2 . There are random elements to generate
the wave shape, in the form of Gaussian white noise signals
ωF1 , ωF2 , ωN1 , ωN2 . See [6], [23] for a detailed descriptionof the wave state-space representation. Then, the wave force
and wave moment are introduced into the disturbance vector
by computing:

 

Xwave
Fwave cos(βwave − ψ)τwave =  Ywave  =  Fwave sin(βwave − ψ) 
(29)
Nwave
Nwave
Wind: The wind speed and angle of attack (Vw , γrw , respectively) induce forces on moments on ASVs, dependingon the aerodynamic characteristics of the system. The wind
disturbance vector is given by:CX (γrw )AFW
Xwind
1
2 
CY (γrw )ALW (30)
τwind =  Ywind  = ρaVrw
2
CN (γrw )ALW LOA
Nwindp
with the relative wind speed Vrw = u2rw + v2rw , considering the wind velocity vector [urw , vrw ]T = [Vw cos(βwind −ψ),Vw sin(βwind − ψ)]T , the wind durection βwind , and thewind relative angle of attack γrw = −atan2(vrw , urw ). For
more information on the aerodynamic coefficient compuation, go to [6], [23].
Currents: Commonly, currents are modeled with a GaussMarkov proccess, under the assumption of irrotational currents [23], described by:V̇c + µcVc = ωcFig. 3. Tracking performance comparison between MPC and MPC-CBF.
(a) Position tracking; (b) Position tracking errors.

disturbances shown in Figure 2. It is clear that MPC-CBF
outperforms MPC in terms of tracking error. To be specific,
in this particular run with the disturbances, the tracking errorof MPC-CBF is 0.20 m, whereas for MPC, it is 0.27 m,
resulting in a 25.9% reduction in tracking error. Furthermore,
Figure 4 illustrates a comparison of the corresponding control
forces. The control effort for MPC is 48.81 N, whereas for(31)

where Vc is the water current speed at direction βc , ωc is a
Gaussian white noise process, and µc > 0 is a parameter tochange the current shape. Finally, the current velocity vector
is written as vc = [Vc cos(βc − ψ),Vc sin(βc − ψ), 0]T .
We present two studies to demonstrate the enhanced
robustness achieved through the MPC-CBF strategy we propose. In the first study, we examine wave forces reachingapproximately 1 N, a wind speed of 2.05 m/s, and currents
extending to 0.2 m/s. The resulting disturbance is depicted
in Figure 2 and is shown in the body frame throughout the
trajectory. Figure 3 provides a comparison of the trajectory

Fig. 4. Controls comparison between MPC and MPC-CBF. (a) MPC; (b)MPC-CBF.

Fig. 2. Model-based disturbances from currents, waves, and winds, appliedin the simulation.

tracking performance in the presence of the environmental

MPC-CBF, it is slightly higher at 54.41 N. This represents
a modest increase of 11.48% compared to MPC, which canbe attributed to the improved robust performance achievedby MPC-CBF.
A second study was conducted to further distinguish theperformance of MPC-CBF from that of MPC. In this study,
a non-dimensional scale factor was introduced to scale theenvironmental disturbance τenv . This disturbance was createdusing model-based equations, and the scale factor allowedfor the controlled increase or decrease of its magnitude forthe purposes of this comparative study. 50 simulation runs
were carried out, gradually increasing the scale factor from
0.7 to 2.5, significantly amplifying the disturbance. Figure 5
presents the tracking error and control effort for each scale
factor. As environmental disturbances increase in magnitude,

Fig. 6. Tracking performance comparison between MPC and MPC-CBF
in Charles River. (a) Position tracking; (b) Position tracking errors.

Fig. 5. Tracking error and control effort with increasing environmental
disturbances in the simulation. (a) Tracking error, and (b) control effort.

it becomes evident that both MPC and MPC-CBF experiencean escalation in tracking errors. However, it’s noteworthy
that MPC-CBF consistently exhibits smaller tracking errors,
accompanied by only a slight increase in control effort, andits capacity to enhance tracking becomes more pronounced
as environmental disturbances grow in strength.
B. Experimental Results
We implemented the MPC-CBF control strategy on oursurface vessel to follow reference trajectories within theCharles River in Boston, Massachusetts, USA. Both the
MPC-CBF and MPC controllers were executed locally on
the vessel’s onboard computer, each with an approximate
execution time of 1 ms. Furthermore, the vessel utilizes realtime SLAM (Simultaneous Localization and Mapping) [24],[25], making use of its onboard 3D LiDAR and IMU for
estimating pose (η) and velocity (v).
Three experimental runs were conducted in the Charles
River, employing both MPC-CBF and MPC controllers under varying environmental disturbance conditions. Figure 6
illustrates the comparative tracking performance between
MPC-CBF and MPC. The performance comparison clearly
demonstrates that MPC-CBF exhibits robustness improvements over MPC, particularly noticeable during the curves
around x = 8 m and x = 14 m. To be specific, the average
position error in the experiments for MPC-CBF is 0.14 m,
while for MPC, it is 0.23 m, resulting in a reduction of
40.26%. Figure 7 presents the control effort for different
runs after the time t = 15 s when all runs have reacheda steady-state. The control effort for MPC is measured at

Fig. 7.

Control efforts of MPC and MPC-CBF in the experiments.

112.38 N, while for MPC-CBF, it is 117.11 N. Consequently,
the disparity between these control efforts is negligible.
These experiments provide additional confirmation that in thepresence of environmental disturbances, the proposed MPCCBF approach exhibits greater robustness compared to MPC.
V. C ONCLUSION AND F UTURE W ORK
In this paper, we introduce a control strategy that combines Model Predictive Control (MPC) with Control Barrier
Functions (CBFs) to enhance trajectory tracking robustness
in water environments subject to significant external disturbances. Our MPC-CBF strategy is validated through extensive testing, including simulations and real-world scenarioswith natural water disturbances. Despite a minor increase incontrol effort, our approach demonstrates superior resiliencein mitigating disturbances. Our future work involves extending this framework to account for factors such as model
uncertainty and measurement noise. Furthermore, we plan
to synergize our approach with perception-based techniques,addressing the intricacies of safe obstacle avoidance andthereby enhancing the applicability of our methodology innavigating unstructured water environments.

R EFERENCES[1] Z. Liu, Y. Zhang, X. Yu, and C. Yuan, “Unmanned surface vehicles:
An overview of developments and challenges,” Annual Reviews inControl, vol. 41, pp. 71–93, 2016.
[2] R. R. Murphy, E. Steimle, C. Griffin, C. Cullins, M. Hall, and K. Pratt,“Cooperative use of unmanned sea surface and micro aerial vehicles
at hurricane wilma,” Journal of Field Robotics, vol. 25, no. 3, pp.
164–180, 2008.
[3] M. Lindemuth, R. Murphy, E. Steimle, W. Armitage, K. Dreger,
T. Elliot, M. Hall, D. Kalyadin, J. Kramer, M. Palankar, K. Pratt, andC. Griffin, “Sea robot-assisted inspection,” IEEE Robotics Automation
Magazine, vol. 18, no. 2, pp. 96–107, 2011.
[4] X. Bai, B. Li, X. Xu, and Y. Xiao, “A review of current research andadvances in unmanned surface vehicles,” Journal of Marine Scienceand Application, vol. 21, no. 2, pp. 47–58, 2022.
[5] W. Wang, D. Fernández-Gutiérrez, R. Doornbusch, J. Jordan, T. Shan,
P. Leoni, N. Hagemann, J. K. Schiphorst, F. Duarte, C. Ratti,
and D. Rus, “Roboat III: An autonomous surface vessel for urban
transportation,” Journal of Field Robotics, vol. n/a, no. n/a, pp. 1–14,
2023. [Online]. Available: https://onlinelibrary.wiley.com/doi/abs/10.
1002/rob.22237[6] A. Gonzalez-Garcia and H. Castañeda, “Adaptive integral terminal
super-twisting with finite-time convergence for an unmanned surface
vehicle under disturbances,” International Journal of Robust andNonlinear Control, vol. 32, no. 18, pp. 10 271–10 291, 2022.
[7] S. Souissi and M. Boukattaya, “Time-varying nonsingular terminal
sliding mode control of autonomous surface vehicle with predefined
convergence time,” Ocean Engineering, vol. 263, p. 112264, 2022.
[8] C. Zhang and S. Yu, “Disturbance observer-based prescribed performance super-twisting sliding mode control for autonomous surface
vessels,” ISA Transactions, 2022.
[9] Y. Weng and N. Wang, “Data-driven robust backstepping control
of unmanned surface vehicles,” International Journal of Robust andNonlinear Control, vol. 30, no. 9, pp. 3624–3638, 2020.
[10] N. Wang, Y. Gao, and X. Zhang, “Data-driven performance-prescribed
reinforcement learning control of an unmanned surface vehicle,” IEEE
Transactions on Neural Networks and Learning Systems, vol. 32,
no. 12, pp. 5456–5467, 2021.
[11] W. Wang, L. Mateos, S. Park, P. Leoni, B. Gheneti, F. Duarte, C. Ratti,
and D. Rus, “Design, modeling, and nonlinear model predictive
tracking control of a novel autonomous surface vehicle,” in Proc. 2018
IEEE Int. Conf. Robot. Autom, 2018, pp. 6189–6196.
[12] W. Wang, N. Hagemann, C. Ratti, and D. Rus, “Adaptive nonlinear
model predictive control for autonomous surface vessels with largely
varying payload,” in 2021 IEEE International Conference on Robotics
and Automation (ICRA), 2021, pp. 7337–7343.
[13] K. P. Tee, S. S. Ge, and E. H. Tay, “Barrier lyapunov functions for thecontrol of output-constrained nonlinear systems,” Automatica, vol. 45,
no. 4, pp. 918–927, 2009.
[14] P. Wieland and F. Allgower, “Constructive safety using control barrier
functions,” in Proc. of 7th IFAC Symposium on Nonlinear Control
System, 2007.
[15] S. P. Boyd and L. Vandenberghe, Convex optimization. New York:
Cambridge university press, 2004.
[16] A. D. Ames, J. W. Grizzle, and P. Tabuada, “Control barrier function
based quadratic programs with application to adaptive cruise control,”in Proc. of 53rd IEEE Conference on Decision and Control, 2014, pp.
6271–6278.
[17] P. Glotfelter, J. Cortes, and M. Egerstedt, “Nonsmooth barrier functions with applications to multi-robot systems,” IEEE control systems
letters, vol. 1, no. 2, pp. 310–315, 2017.
[18] W. Xiao and C. Belta, “Control barrier functions for systems with highrelative degree,” in Proc. of 58th IEEE Conference on Decision andControl, Nice, France, 2019, pp. 474–479.
[19] W. Xiao and C. Belta, “High-order control barrier functions,” IEEE
Transactions on Automatic Control, vol. 67, no. 7, pp. 3655–3662,
2021.
[20] L. Lindemann and D. V. Dimarogonas, “Control barrier functions
for multi-agent systems under conflicting local signal temporal logic
tasks,” IEEE Control Systems Letters, vol. 3, no. 3, pp. 757–762, 2019.
[21] H. K. Khalil, Nonlinear Systems. Prentice Hall, third edition, 2002.
[22] B. Houska, H. Ferreau, and M. Diehl, “ACADO Toolkit – An Open
Source Framework for Automatic Control and Dynamic Optimization,”
Optimal Control Applications and Methods, vol. 32, no. 3, pp. 298–
312, 2011.

[23] T. I. Fossen, Handbook of Marine Craft Hydrodynamics and Motion
Control. John Wiley & Sons, 2011.
[24] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus,
“Lio-sam: Tightly-coupled lidar inertial odometry via smoothing andmapping,” in 2020 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), 2020, pp. 5135–5142.
[25] W. Wang, T. Shan, P. Leoni, D. Fernández-Gutiérrez, D. Meyers,
C. Ratti, and D. Rus, “Roboat II: A novel autonomous surface vessel
for urban environments,” in 2020 IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 2020, pp. 1740–1747.