The Branching-Course MPC Algorithm for Maritime Collision Avoidance

TL;DR

This paper introduces the BC-MPC algorithm for maritime collision avoidance, which is robust to sensor noise, does not require vessel communication, and complies with COLREGs, validated through full-scale experiments.

Contribution

The paper presents a novel branching-course MPC algorithm that enhances maritime collision avoidance by being sensor-noise robust and COLREGs-compliant, validated in real-world experiments.

Findings

Good collision avoidance performance in experiments

Robustness to sensor noise demonstrated

Compliance with COLREGs achieved

Abstract

This article presents a new algorithm for short-term maritime collision avoidance (COLAV) named the branching-course MPC (BC-MPC) algorithm. The algorithm is designed to be robust with respect to noise on obstacle estimates, which is a significant source of disturbance when using exteroceptive sensors such as e.g. radars for obstacle detection and tracking. Exteroceptive sensors do not require vessel-to-vessel communication, which enables COLAV toward vessels not equipped with e.g. automatic identification system (AIS) transponders, in addition to increasing the robustness with respect to faulty information which may be provided by other vessels. The BC-MPC algorithm is compliant with rules 8 and 17 of the International Regulations for Preventing Collisions at Sea (COLREGs), and favors maneuvers following rules 13-15. This results in a COLREGs-aware algorithm which can ignore rules 13-15 when necessary. The algorithm is experimentally validated in several full-scale experiments in the Trondheimsfjord in 2017 using a radar-based system for obstacle detection and tracking. The COLAV experiments show good performance in compliance with the desired algorithm behavior.

Tables

Table 1. Table 1 : Telemetron ASV specifications.

Component		Description
Vessel hull		Polarcirkel Sport 845
Length		$8.45m\text{/}$
Width		$2.71m\text{/}$
Weight		$1675\mathrm{kg}\text{/}$
Propulsion system		Yamaha $225$ HP outboard engine
Motor control		Electro-mechanical actuation of throttle valve
Rudder control		Hydraulic actuation of outboard engine angle with proportional-derivative (PD) feedback control
Navigation system		Kongsberg Seatex Seapath 330+
Radar		Simrad Broadband 4G™ Radar
Processing platform		Intel® i7 $3.4\mathrm{GHz}\text{/}$ CPU, running Ubuntu 16.04 Linux

Table 2. Table 2 : BC-MPC algorithm parameters.

Parameter	Value	Description
$𝑻$		Prediction horizon
${𝑵}_U$		Number of speed maneuvers
${𝑵}_{\chi }$		Number of course maneuvers
$T_{\text{ramp}}$	$1s\text{/}$	Ramp time
$T_U$	$5s\text{/}$	Speed maneuver length
$T_{\chi }$	$5s\text{/}$	Course maneuver length
$T_{\tilde{U}}$	$5s\text{/}$	Speed error model time constant
$T_{\tilde{\chi }}$	$5s\text{/}$	Course error model time constant
$\Delta$	$500m\text{/}$	LOS lookahead distance
${\gamma }_s$	$0.005\text{/}s$	LOS along track distance gain
$w_{\text{al}}$	$1$	Align weight
$w_{\text{av}}$	$6000$	Avoid weight
$w_{\text{t}}$	$4200$	Transitional cost weight
$w_{\chi }$	$100$	Angular error scaling weight
$a_0$	$50m\text{/}$	Collision region major axis
$a_1$	$150m\text{/}$	Safety region major axis
$a_2$	$250m\text{/}$	Margin region major axis
$b_0$	$25m\text{/}$	Collision region minor axis
$b_1$	$75m\text{/}$	Safety region minor axis
$b_2$	$125m\text{/}$	Margin region minor axis
$d_{\text{COLREGs}}$	$100m\text{/}$	COLREGs distance
${\gamma }_1$	$0.1$	Obstacle cost gradient parameter

Table 3. Table 3 : Key points and numbers from the experiments. *In Experiment 2.1 we passed in front of the obstacle, while COLREGs prefers that the ownship pass behind the obstacle. Passing in front is, however, not strictly forbidden.

Experiment type and number	Obstacle sensor	Rule 13–15 compliance	Minimum distance to obstacle
Head on
1.1	AIS	No	$197.8m\text{/}$
1.2	Radar	Yes	$100.8m\text{/}$
1.3	Radar	Yes	$132.5m\text{/}$
Crossing from starboard
2.1	Radar	Yes*	$214.0m\text{/}$
2.2	Radar	Yes	$106.2m\text{/}$
Overtaking
3.1	Radar	Yes	$127.3m\text{/}$
Crossing from port
4.1	Radar	N/A	$231.7m\text{/}$

Equations

$$\dot{\mathbf{\eta }}=\mathbf{R}(\psi )\mathbf{\nu }$$

$$\mathbf{M}\dot{\mathbf{\nu }}+\mathbf{C}(\mathbf{\nu })\mathbf{\nu }+\mathbf{D}(\mathbf{\nu })\mathbf{\nu }=\mathbf{\tau },$$

$$\mathbf{M}(\mathbf{x})\dot{\mathbf{x}}+\mathbf{\sigma }(\mathbf{x})=\mathbf{\tau },$$

$$\dot{\mathbf{\eta }}$$

$$\dot{\chi }$$

$$\mathbf{\tau }=\mathbf{M}(\mathbf{x}){\dot{\mathbf{x}}}_d+\mathbf{\sigma }({\mathbf{x}}_d)-\mathbf{M}(\mathbf{x}){\mathbf{K}}_p\tilde{\mathbf{\zeta }}-{\mathbf{K}}_i{\int }_{t_0}^t{\tilde{\mathbf{\zeta }}}_1(\gamma )\mathrm{d}\gamma ,$$

$$\tilde{\mathbf{\zeta }}=\left[\begin{array}{c}\tilde{U}\\ \tilde{r}\\ \tilde{\chi }\end{array}\right],{\tilde{\mathbf{\zeta }}}_1=\left[\begin{array}{c}\tilde{U}\\ \tilde{\chi }\end{array}\right],$$

$$r_d$$

$${\dot{r}}_d$$

$${\dot{\mathbf{X}}}_{\max }$$

$${\dot{\mathbf{X}}}_{\min }$$

$${\mathbf{\tau }}_{\max }$$

$${\mathbf{\tau }}_{\min }$$

$$a_i^{\ast }=\{\begin{array}{ll}{a}_{\min ,i}& ,a_i<a_{\min ,i}\\ a_{\max ,i}& ,a_i>a_{\max ,i}\\ a_i& ,\text{otherwise},\end{array}$$

$$A_{d}=\left\{(\dot{U},\dot{r})\in\mathbb{R}\times\mathbb{R}\big{|}\dot{U}\in[\dot{U}_{\min},\dot{U}_{\max}],\dot{r}\in[\dot{r}_{\min},\dot{r}_{\max}]\right\}.$$

$${\dot{\mathbf{U}}}_{\text{samples}}$$

$${\dot{\mathbf{r}}}_{\text{samples}}$$

$${\dot{U}}_{d,i}(t)=\{\begin{array}{ll}{k}_{U,i}t& ,0\le t<T_{\text{ramp}}\\ {\dot{U}}_i& ,T_{\text{ramp}}\le t<T_U-T_{\text{ramp}}\\ {\dot{U}}_i-k_{U,i}(t-(T_U-T_{\text{ramp}}))& ,T_U-T_{\text{ramp}}\le t<T_U\\ 0,& ,T_U\le t\le T,\end{array}$$

$${\dot{r}}_{d,i}(t)=\{\begin{array}{ll}{k}_{r,i}t& ,0\le t<T_{\text{ramp}}\\ 2{\dot{r}}_i-k_{r,i}t& ,T_{\text{ramp}}\le t<2T_{\text{ramp}}\\ 0& ,2T_{\text{ramp}}\le t<T_{\chi }-2T_{\text{ramp}}\\ -k_{r,i}(t-(T_{\chi }-2T_{\text{ramp}}))& ,T_{\chi }-2T_{\text{ramp}}\le t<T_{\chi }-T_{\text{ramp}}\\ -2{\dot{r}}_i+k_{r,i}(t-(T_{\chi }-T_{\text{ramp}}))& ,T_{\chi }-T_{\text{ramp}}\le t<T_{\chi }\\ 0& ,2T_{\chi }\le t<T,\end{array}$$

$$U_{d,i}(t)$$

$$r_{d,i}(t)$$

$${\chi }_{d,i}(t)$$

$${\mathcal{U}}_d=\{U_{d,1}(t),U_{d,2}(t),\dots ,U_{d,N_U}(t)\}\times \{{\chi }_{d,1}(t),{\chi }_{d,2}(t),\dots ,{\chi }_{d,N_{\chi }}(t)\},$$

$$\dot{\tilde{U}}$$

$$\dot{\tilde{\chi }}$$

$${\bar{U}}_i(t)$$

$${\bar{\chi }}_i(t)$$

$$\bar{\mathcal{U}}=\{{\bar{U}}_1(t),{\bar{U}}_2(t),\dots ,{\bar{U}}_{N_U}(t)\}\times \{{\bar{\chi }}_1(t),{\bar{\chi }}_2(t),\dots ,{\bar{\chi }}_{N_{\chi }}(t)\}.$$

$$\dot{\bar{\mathbf{p}}}$$

$$\bar{\mathcal{H}}=\left\{\bar{\boldsymbol{\eta}}(t;\bar{U}(t),\bar{\chi}(t))\big{|}(\bar{U}(t),\bar{\chi}(t))\in\bar{\mathcal{U}}\right\},$$

$${\chi }_{d,LOS}={\chi }_{path}+\arctan \left(-\frac{e}{\Delta }\right),$$

$$U_{PP}=U\cos (\chi -{\chi }_{path})+{\gamma }_{s}s,$$

$$U_{d,LOS}=\{\begin{array}{ll}\text{sat}\left(\frac{U_t-{\gamma }_{s}s}{\cos (\chi -{\chi }_{path})},0,U_{\max ,LOS}\right)& \text{if }|\cos (\chi -{\chi }_{path})|>ϵ\\ \text{sat}\left(\frac{U_{pp}-{\gamma }_{s}s}{ϵ},0,U_{\max ,LOS}\right)& \text{else,}\end{array}$$

$${\dot{U}}_d^{\prime }$$

$${\dot{r}}_d^{\prime }$$

$$G(\bar{\mathbf{\eta }}(t),{\mathbf{u}}_d(t);{\mathbf{p}}_d(t))=w_{\text{al}}\text{align}(\bar{\mathbf{\eta }}(t);{\mathbf{p}}_d(t))+w_{\text{av}}\text{avoid}(\bar{\mathbf{\eta }}(t))+w_{\text{t}}\text{tran}({\mathbf{u}}_d(t)),$$

Full text

\WarningFilter

glossariesNo

Glossary

A* A star AIS automatic identification system ASV autonomous surface vehicle AUV autonomous underwater vehicle BC-MPC branching-course MPC COLAV collision avoidance COLREGs International Regulations for Preventing Collisions at Sea DW dynamic window FF-FB feedforward feedback LOS line of sight MPC model predictive control MR Maritime Robotics NED North-East-Down OSD1 Ocean Space Drone 1 PDAF probabilistic data association filter PI proportional-integral ROS Robot Operating System yaw rate rate of turn RRT rapidly exploring random tree speed speed over ground VO velocity obstacles

or

Glossary

A* A star AIS automatic identification system ASV autonomous surface vehicle AUV autonomous underwater vehicle BC-MPC branching-course MPC COLAV collision avoidance COLREGs International Regulations for Preventing Collisions at Sea DW dynamic window FF-FB feedforward feedback LOS line of sight MPC model predictive control MR Maritime Robotics NED North-East-Down OSD1 Ocean Space Drone 1 PDAF probabilistic data association filter PI proportional-integral ROS Robot Operating System yaw rate rate of turn RRT rapidly exploring random tree speed speed over ground VO velocity obstacles

found.

The Branching-Course MPC Algorithm for Maritime Collision Avoidance

Bjørn-Olav H. Eriksen   Morten Breivik   Erik F. Wilthil   Andreas L. Flåten   Edmund F. Brekke

Centre for Autonomous Marine Operations and Systems

Department of Engineering Cybernetics

Norwegian University of Science and Technology (NTNU)

Trondheim, Norway

{bjorn-olav.h.eriksen, morten.breivik}@ieee.org

{erik.wilthil, andreas.flaten, edmund.brekke}@ntnu.no

Abstract

This article presents a new algorithm for short-term maritime collision avoidance (COLAV) named the branching-course MPC (BC-MPC) algorithm. The algorithm is designed to be robust with respect to noise on obstacle estimates, which is a significant source of disturbance when using exteroceptive sensors such as e.g. radars for obstacle detection and tracking. Exteroceptive sensors do not require vessel-to-vessel communication, which enables COLAV toward vessels not equipped with e.g. automatic identification system (AIS) transponders, in addition to increasing the robustness with respect to faulty information which may be provided by other vessels. The BC-MPC algorithm is compliant with rules 8 and 17 of the International Regulations for Preventing Collisions at Sea (COLREGs), and favors maneuvers following rules 13–15. This results in a COLREGs-aware algorithm which can ignore rules 13–15 when necessary. The algorithm is experimentally validated in several full-scale experiments in the Trondheimsfjord in 2017 using a radar-based system for obstacle detection and tracking. The COLAV experiments show good performance in compliance with the desired algorithm behavior.

1 Introduction

Today’s society moves rapidly towards an increased level of automation. The development of autonomous cars is spearheading this trend, as exemplified by the efforts made by e.g. Google and Uber. In recent years, autonomy has also become a hot topic in the maritime domain with research on autonomous passenger and goods transport, seabed surveying and military applications. An example of this is the Yara Birkeland project in Norway, where an autonomous electrically-powered cargo ship will replace approximately $40000$ diesel-powered truck journeys of fertilizer per year [Kongsberg Maritime, 2018]. Reduced cost, increased efficiency and reduced environmental impact may be the most obvious benefits of autonomy at sea, but the potential for increased safety is not to be overlooked since reports state that in excess of $75\%$ of maritime accidents are caused by human errors [Chauvin, 2011, Levander, 2017]. A prerequisite for employing autonomous surface vehicles (ASVs) in environments where other vessels may be present is, however, that the ASVs have robust collision avoidance (COLAV) systems. Such COLAV systems must make the ASVs, as other vessels, follow the International Regulations for Preventing Collisions at Sea (COLREGs) which contains a set of rules on how vessels should behave in situations where there is a risk of collision with another vessel [Cockcroft and Lameijer, 2004]. However, COLREGs is written for human interpretation with few quantitative rules, which makes it challenging to develop algorithms capturing the intention of COLREGs by machine decision-making.

COLAV algorithms have typically been divided into reactive and deliberate algorithms. Reactive algorithms are characterized by considering a limited amount of information, originally only currently available sensor information [Tan et al., 2004], and employing little motion planning in a short time frame. This makes reactive algorithms computationally cheap, and able to react to sudden changes in the environment. Examples include vessels making sudden unpredicted maneuvers, late detection of obstacles, etc. However, since reactive algorithms consider a limited amount of information and employ little motion planning, they tend to make suboptimal choices in complex situations which makes them sensitive to local minima. Examples of reactive algorithms are the velocity obstacles (VO) [Fiorini and Shiller, 1998, Kuwata et al., 2014] and the dynamic window (DW) [Fox et al., 1997] algorithms. Deliberate algorithms consider more information and plan for a longer time frame, which results in more optimal choices at the cost of increased computational requirements. Examples of deliberate algorithms include the A* [Hart et al., 1968] and the rapidly exploring random tree (RRT) [LaValle, 1998] algorithms.

The previously clear border between reactive and deliberate algorithms have become somewhat artificial since few algorithms only utilize currently available sensor information. However, the idea that the reactive algorithms are capable of responding quickly to changes in the environment and the deliberate algorithms are capable of performing optimal motion planning in a longer time frame is still relevant. We therefore choose to rather use the terms “short-term” and “long-term” algorithms to distinguish the algorithms. In a practical COLAV system, both short-term and long-term algorithms are useful. For long time frames, all available information should be included, while one may use a less detailed vessel model for planning. For short-term COLAV, one can include less spatial and temporal information but may need to use a more detailed model of the vessel to ensure dynamically feasible maneuvers. By combining short-term and long-term algorithms in a hybrid architecture, the benefits of both algorithms can be combined, ensuring both responsiveness, feasibility and optimality. An example of a hybrid architecture with three COLAV levels is shown in Fig. 1.

The topmost level, named path planning, is intended to produce a nominal path or trajectory from the initial position to the goal. The spatial and temporal distance between the initial and goal positions may be large, allowing only for a limited complexity in this algorithm. For instance, moving obstacles could be neglected at this level. The mid-level COLAV algorithm tries to follow this nominal path or trajectory, while at the same time performing COLAV with respect to all obstacles, characterized as a long-term COLAV algorithm. COLREGs is a natural part of this level, since it may be complex to decide the appropriate action with respect to COLREGs. The mid-level algorithm produces a modified trajectory which is passed to the short-term COLAV layer. This layer perform short-time COLAV making sure to avoid obstacles performing sudden maneuvers or which are detected too late to be handled by the mid-level algorithm, while also ensuring that the maneuvers are feasible with respect to the dynamic constraints of the vessel. The short-term layer can also act as a backup solution to avoid collisions in cases where the mid-level algorithm fails to produce feasible trajectories, for instance due to time constraints or numerical issues [Eriksen and Breivik, 2017b].

COLAV algorithms depend on information about obstacle position, speed and course in order to be able to avoid collisions. One possible source of such information is using automatic identification system (AIS) transponders. AIS is a vessel-to-vessel communication system where vessels transmit their current position and velocity to other vessels carrying AIS transponders [IMO, 2018]. Passenger ships and vessels with a gross tonnage of over $300$ are required to carry AIS transponders. This is of course valuable information when it comes to navigation and COLAV at sea. However, AIS transponders usually rely on satellite navigation and data inputs from the user, which results in the possibility of transmitting inaccurate or invalid data [Harati-Mokhtari et al., 2007]. Also, vessels or objects not equipped with AIS transponders will not be detected. A more robust approach to obtain information about the environment is to employ exteroceptive sensors, which have the advantage of not relying on any infrastructure or collaboration with the obstacles in order to detect them. A commonly used exteroceptive sensor at sea is radar. However, the data from a radar usually includes a fair amount of noise, which makes this sensor more complex and difficult to work with than AIS [Eriksen et al., 2018]. On-board radars have been used for full-scale COLAV experiments based on the A* algorithm in [Schuster et al., 2014], and using a modified version of the DW algorithm in [Eriksen et al., 2018]. In [Elkins et al., 2010, Kuwata et al., 2014], other exteroceptive sensors such as cameras and lidar are used for COLAV.

Model predictive control (MPC) has for a long time been a well-known and proven tool for motion planning and COLAV for e.g. ground and automotive robots [Ögren and Leonard, 2005, Keller et al., 2015, Gray et al., 2013], aerospace applications [Kuwata and How, 2011] and underwater vehicles [Caldwell et al., 2010]. In the later years, MPC has also been applied for COLAV in the maritime domain, both using sample-based approaches where one considers a finite space of control inputs [Švec et al., 2013, Johansen et al., 2016, Hagen et al., 2018] and conventional gradient-based search algorithms [Abdelaal and Hahn, 2016, Eriksen and Breivik, 2017b]. None of these algorithms does, however, consider the amounts of noise which we expect to encounter using a radar-based tracking system. Gradient-based algorithms have the benefit of exploring the entire control input space, but the complexity of the COLAV problem can make it difficult to guarantee that a feasible solution will be found within the time requirements [Eriksen and Breivik, 2017b]. This makes sample-based approaches well suited for short-term COLAV. In [Benjamin et al., 2006, Benjamin et al., 2010], a protocol-based COLAV algorithm using interval programming is presented. The algorithm optimizes over multiple functions considering different behaviors, e.g. waypoint following and adherence to different parts of COLREGs, by combining them in an objective function with adaptive weights. The algorithm does, however, use vessel-to-vessel communication in order to obtain obstacle information, and is not necessarily well suited for use with exteroceptive sensors.

1.1 The International Regulations for Preventing Collisions at Sea

COLREGs regulate how vessels should behave in situations where there exists a risk of collision. There is in total 38 rules, where rule 8 and 13–17 are the most relevant ones for designing COLAV algorithms for ASVs, although the rest must also must be addressed in a COLREGs-compliant system. Rules 8 and 13–17 can be summarized as:

Rule 8:

This rule requires, among other things, that maneuvers applied in situations where a risk of collision exists should be large enough to be readily observable for other vessels. Small consecutive maneuvers should hence be avoided.

Rule 13:

In an overtaking situation, where a vessel is approaching another from an angle of more than $22.5$\mathrm{\SIUnitSymbolDegree}\text{/}$$ abaft the other vessel’s beam, the overtaking vessel is deemed the give-way vessel and the overtaken vessel is deemed the stand-on vessel. The overtaking vessel is allowed to pass on either side. However, in a case where the overtaken vessel is required to avoid collision with another vessel it may be required to make a starboard maneuver. To avoid blocking the path of the overtaken vessel in such a situation, we consider it as most suitable to overtake a vessel on her port side.

Rule 14:

In a head-on situation, where two vessels approaches each other on reciprocal or nearly reciprocal courses (a margin of $\pm 6~{}$\mathrm{\SIUnitSymbolDegree}\text{/}$$ is often used), both vessels are required to do starboard maneuvers and pass the other vessel on her port side.

Rule 15:

This rule handles crossing situations, where a vessel is approaching another vessel from the side, but not in the regions considered as a head on or overtaking situation. The vessel with the other vessel on her starboard side is deemed the give-way vessel, while the other is deemed the stand-on vessel. The preferred give-way maneuver is to do a starboard turn and pass behind the stand-on vessel.

Rule 16:

This rule defines the action for the give-way vessel. It requires that the give-way vessel performs early and substantial action to avoid collision.

Rule 17:

This rule defines the action for the stand-on vessel. It requires that the stand-on vessel keep her current speed and course, while the give-way vessel maneuvers in order to avoid collision. However, if the give-way vessel fails in her duty of avoiding collision, the stand-on vessel is required to maneuver to avoid collision. If this occurs in a crossing situation, the stand-on vessel should avoid maneuvering to port if possible.

Fig. 2 shows a graphical illustration of the situations given by rules 13–15. The interested reader is referred to [Cockcroft and Lameijer, 2004] for more details on the COLREGs rules.

1.2 Contributions

The authors of this article have focused on short-term and reactive COLAV for ASVs for the last few years, starting with a modified version of the DW algorithm designed for use with autonomous underwater vehicles (AUVs) [Eriksen et al., 2016]. This algorithm was adapted for use with high-speed ASVs, and tested in conjunction with a radar-based tracking system [Wilthil et al., 2017] successfully demonstrating closed-loop radar-based COLAV in full scale experiments [Eriksen et al., 2018]. However, the experiments revealed challenges with using radar-based tracking systems for COLAV, especially noisy estimates of obstacle speed and course caused problems. The DW algorithm is not particularly robust with respect to such noise, causing the vessel to repeatedly change the planned maneuver. In addition, the DW algorithm assumes the ASV to keep a constant turn rate for the entire prediction horizon. This does not resemble the way vessels usually maneuver at sea, where one usually performs a corrective maneuver by changing the course and/or speed, followed by keeping the speed and course constant. These issues motivate us to develop a new short-term COLAV algorithm which is less sensitive to noisy obstacle estimates while also producing more “maritime-like” maneuvers.

In this article, we therefore present a new algorithm for short-term COLAV named the branching-course MPC (BC-MPC) algorithm. This algorithm is based on sample-based MPC and is designed to be robust with respect to noisy obstacle estimates, which is an important consideration when using radar-based tracking systems for providing obstacle estimates. In contrast to sample-based MPC algorithms previously applied to ASVs, the BC-MPC algorithm considers a sequence of maneuvers, enabling the algorithm to plan more complex trajectories than just a single avoidance maneuver. Furthermore, the BC-MPC algorithm complies with rules 8 and 17 of COLREGs, while favoring maneuvers complying with rules 13–15. In cases where the algorithm chooses to ignore rules 13–15, which can be required by rule 17, the maneuvers have increased clearance to obstacles. The term “COLREGs-compliance” is often abused in the literature by using it for algorithms only complying with parts of COLREGs. With this in mind, we consider the algorithm as being “COLREGs-aware”. The algorithm is implemented on an under-actuatuated ASV and validated through several full-scale closed-loop COLAV experiments using a radar-based tracking system for providing estimates of obstacle course, speed and position.

1.3 Outline

The rest of the article is structured as follows: Section 2 describes modeling and control of ASVs, Section 3 presents the BC-MPC algorithm, while Section 4 contains results from the full-scale closed-loop COLAV experiments. Finally, Section 5 concludes the article and presents possibilities for further work.

2 ASV modeling and control

The vessel of interest in this work is the Telemetron ASV shown in Fig. 3, which is owned and operated by Maritime Robotics (MR). The vessel is $8.45~{}$\mathrm{m}\text{/}$$ long, and uses a single steerable outboard engine for propulsion, which makes the vessel underactuated.

2.1 ASV modeling

ASVs are in general small and agile vessels, capable of operating at high speeds. At low speeds the hydrostatic pressure mainly carries the weight of the vessel, and it operates in the displacement region. When the vessel speed increases, the hydrodynamic pressure increases, eventually dominating over the hydrostatic pressure. At this point, we are in the planing region. In between the displacement and planing region we have the semi-displacement region. The Telemetron ASV is a high-speed vessel, capable of speeds up to $18~{}$\mathrm{m}\text{/}\mathrm{s}, which combined with the vessel length of $8.45~{}$\mathrm{m}\text{/} makes for a vessel operating in the displacement, semi-displacement and planing regions [Fossen, 2011, Faltinsen, 2005].

The conventional approach to modeling ASVs is by using the 3DOF model [Fossen, 2011]:

[TABLE]

where $\boldsymbol{\eta}=\begin{bmatrix}N&E&\psi\end{bmatrix}^{T}$ is the vessel pose in an earth-fixed North-East-Down (NED) reference frame, $\boldsymbol{\nu}=\begin{bmatrix}u&v&r\end{bmatrix}^{T}$ is the vessel velocity and $\boldsymbol{\tau}=\begin{bmatrix}X&Y&N\end{bmatrix}^{T}$ is a vector of forces and torque, both given in the body-fixed reference frame. See Fig. 4 for an illustration of the variables.

The matrix $\boldsymbol{R}(\psi)$ is a rotation matrix, while $\boldsymbol{M}$, $\boldsymbol{C}(\boldsymbol{\nu})$ and $\boldsymbol{D}(\boldsymbol{\nu})$ are the mass, Coriolis and centripetal and damping matrices, respectively.

There exist many versions of the model (1) [Fossen, 2011], but they require that the vessel operates in the displacement region. For the Telemetron ASV, this would require a maximum operating speed of approximately $3.5~{}$\mathrm{m}\text{/}\mathrm{s}$$ [Eriksen and Breivik, 2017a]. This is quite a big limitation, and we therefore rather use a control-oriented non-first principles model developed for high-speed ASVs [Eriksen and Breivik, 2017a], valid for the displacement, semi-displacement and planing regions:

[TABLE]

where $\boldsymbol{x}=\begin{bmatrix}U&r\end{bmatrix}^{T}$ is the vessel state, with $U=\sqrt{u^{2}+v^{2}}$ being the vessel speed over ground and $r$ being the vessel yaw rate, while $\boldsymbol{\tau}=\begin{bmatrix}\tau_{m}&\tau_{\delta}\end{bmatrix}^{T}$ is a normalized control input. In this article, we also refer to the vessel speed over ground as the vessel speed. The matrix $\boldsymbol{M}(\boldsymbol{x})$ is a diagonal state-dependent inertia matrix with nonlinear terms and $\boldsymbol{\sigma}(\boldsymbol{x})=\begin{bmatrix}\sigma_{U}(\boldsymbol{x})&\sigma_{r}(\boldsymbol{x})\end{bmatrix}^{T}$ is a vector of nonlinear damping terms. Notice that the model is in 2DOF, designed for underactuated ASVs, where the speed and course are usually controlled. Using the state variable from (2), the kinematics can be defined as:

[TABLE]

where $\chi$ is the vessel course and $\beta$ is the sideslip. For more details on the model, see [Eriksen and Breivik, 2017a].

2.2 ASV control design

As shown in Fig. 1, the COLAV system is built on top of the vessel controllers. Hence, the performance of the COLAV system can be limited by the performance of the vessel controllers. It is therefore beneficial to use high-performance vessel controllers ensuring that the maneuvers that the COLAV system specifies are properly executed, not limiting the performance of the COLAV system.

The model (2) can be used in control design, particularly using it for model-based feedforward in speed and yaw rate is shown to provide good performance [Eriksen and Breivik, 2017a]. A controller named the feedforward feedback (FF-FB) controller is presented in [Eriksen and Breivik, 2017a], which combines model-based feedforward terms with a gain-scheduled proportional-integral (PI) feedback controller for controlling the vessel speed and yaw rate. For the BC-MPC algorithm, we need a controller capable of following a speed and course trajectory. The FF-FB controller has proven to have high performance in experiments [Eriksen et al., 2018, Eriksen and Breivik, 2017a], so we therefore extend the FF-FB controller to include course control:

[TABLE]

where $\boldsymbol{x}_{d}=\begin{bmatrix}U_{d}&r_{d}\end{bmatrix}^{T}$, $\boldsymbol{K}_{p}>0$ is a matrix of proportional gains, $\boldsymbol{K}_{i}>0$ is a diagonal matrix of integral gains, and:

[TABLE]

where $\tilde{U}=U-U_{d}$, $\tilde{r}=r-r_{d}$ and $\tilde{\chi}=\Upsilon(\chi-\chi_{d})$ are the speed, yaw rate and course errors, respectively. The function $\Upsilon:\mathbb{R}\to S^{1}$ maps an angle to the domain $[-\pi,\pi)$.

In the control law (4), we use the desired yaw rate $r_{d}$ and its derivative $\dot{r}_{d}$. Through (3), the relation between the course and yaw rate is stated as $r=\dot{\chi}-\dot{\beta}$, where the derivative of the sideslip enters the equation. At this stage, we do not have a sideslip model of the Telemetron ASV. However, we have seen in experiments that at moderate speeds the sideslip is sufficiently constant to be neglected without major implications. We therefore simplify the relation by assuming constant sideslip and defining the desired yaw rate and its derivative as:

[TABLE]

The interested reader is referred to [Eriksen and Breivik, 2018] for more details on the speed and course controller.

3 The BC-MPC algorithm

The BC-MPC algorithm is intended to avoid collisions with moving obstacles while respecting the dynamic constraints of the vessel in order to ensure feasible maneuvers, which is ideal for short-term COLAV. The algorithm is based on model predictive control (MPC), and plans vessel-feasible trajectories with multiple maneuvers where only the first maneuver is executed. The trajectories have continuous acceleration, which is beneficial for vessel controllers utilizing model-based feedforward terms, such as (4). To fit well with tracking systems based on exteroceptive sensors, such as e.g. radars, the algorithm is designed to be robust with respect to noisy obstacle estimates. Furthermore, the algorithm is designed with the short-term perspective of COLREGs in mind. The algorithm is also modular, so it can easily be tailored for different applications.

The BC-MPC algorithm can be described by two steps, which will be explained in detail in the following sections:

Generate a search space consisting of feasible trajectories with respect to the dynamic constraints of the vessel. 2. 2.

Discretize the search space and compute an objective function value on the trajectories. The optimal trajectory is then selected as the one with the lowest objective function value.

The BC-MPC algorithm architecture is shown in Fig. 5. The algorithm inputs a desired trajectory, which can originate from either another COLAV algorithm or directly from a user. The guidance function receives the desired trajectory, and computes a desired acceleration given a vessel state and time specified by the trajectory generation. The trajectory generation block creates a set of possible vessel trajectories, given an initial vessel state, initial desired velocity and a desired acceleration from the guidance function. A tracking system provides obstacle estimates, which are used to calculate a part of the objective function. The optimization block computes the optimal trajectory based on an objective function, and outputs this as a desired velocity trajectory to the vessel controller (4).

3.1 Trajectory generation

The search space consists of a number of trajectories, each consisting of a sequence of sub-trajectories each containing one maneuver. This section describes how the trajectories are generated. Each trajectory is defined by a desired velocity trajectory containing a speed and course trajectory with continuous acceleration, and feedback-corrected predicted pose and velocity trajectories.

3.1.1 Trajectory generation: A single step

As mentioned, each trajectory consists of a sequence of maneuvers, resulting in trajectories that branches out from each other. Hence, the trajectory generation can be divided in repeatable steps. At each step, a set of sub-trajectories, each containing one maneuver, are computed given an initial vessel configuration, initial time and some step-specific parameters:

• •

The number of speed maneuvers $N_{U}$

• •

The number of course maneuvers $N_{\chi}$

• •

The time allowed for changing the actuator input, named the ramp time $T_{\text{ramp}}$

• •

The maneuver time length in speed $T_{U}$ and course $T_{\chi}$

• •

The total step time length $T$

We start by generating the desired velocity trajectories, which should be feasible with respect to actuator rate and magnitude saturations. To ensure feasibility with respect to the actuator rate saturations, we start from the model (2) by calculating the possible speed and course accelerations given our current configuration as:

[TABLE]

where $\dot{\boldsymbol{X}}_{\max}=\begin{bmatrix}\dot{U}_{\max}&\dot{r}_{\max}\end{bmatrix}^{T}$, $\dot{\boldsymbol{X}}_{\min}=\begin{bmatrix}\dot{U}_{\min}&\dot{r}_{\min}\end{bmatrix}^{T}$, $\boldsymbol{X}_{0}$ is the current vessel velocity and:

[TABLE]

where $T_{\text{ramp}}>0$ is the ramp time, $\boldsymbol{\tau}_{0}$ is the current control input, $\boldsymbol{\tau}_{\max}$ and $\boldsymbol{\tau}_{\min}$ are the maximum and minimum control input, respectively, and $\dot{\boldsymbol{\tau}}_{\max}$ and $\dot{\boldsymbol{\tau}}_{\min}$ are the maximum and minimum control input rate of change, respectively. The saturation function $\text{sat}(\boldsymbol{a},\boldsymbol{a}_{\min},\boldsymbol{a}_{\max})$ is defined as $\text{sat}:\mathbb{R}^{K}\times\mathbb{R}^{K}\times\mathbb{R}^{K}\rightarrow\mathbb{R}^{K}$ with:

[TABLE]

for $\boldsymbol{a}^{*}=\text{sat}(\boldsymbol{a},\boldsymbol{a}_{\min},\boldsymbol{a}_{\max})$, $i\in\{1,2,\ldots,K\}$ and $(\cdot)_{i}$ denoting element $i$ of a vector. Following this, we create a set of possible accelerations as:

[TABLE]

The set of possible accelerations is then sampled uniformly to create a discrete set of candidate maneuver accelerations:

[TABLE]

where $\dot{U}_{i}$, $i\in[1,N_{U}]$ are speed acceleration samples and $\dot{r}_{i}$, $i\in[1,N_{\chi}]$ are course acceleration samples. To be able to include a specific maneuver in the search space, which can be beneficial e.g. to converge to a specific desired trajectory, we allow to modify some of the sampled accelerations if a desired acceleration $(\dot{U}^{\prime}_{d},\dot{r}^{\prime}_{d})$ is inside the set of possible accelerations as follows: If $\dot{U}^{\prime}_{d}\in A_{d}$, we change the closest speed acceleration sample in $\dot{\boldsymbol{U}}_{\text{samples}}$ to $\dot{U}^{\prime}_{d}$. Similarly for course, if $\dot{r}^{\prime}_{d}\in A_{d}$, we change the closest course acceleration sample in $\dot{\boldsymbol{r}}_{\text{samples}}$ to $\dot{r}^{\prime}_{d}$. Following this, we create a set of candidate maneuver accelerations by combining the speed and course candidate maneuvers as $\dot{\boldsymbol{U}}_{\text{samples}}\times\dot{\boldsymbol{r}}_{\text{samples}}$. This concept is illustrated in Fig. 6, where $A_{d}$ is sampled with $N_{U}=3$ speed samples and $N_{\chi}=5$ course samples.

Given the acceleration samples, we create a set of $N_{U}$ motion primitives for speed based on the piecewise-linear speed acceleration trajectories:

[TABLE]

where $k_{U,i}=\frac{\dot{U}_{i}}{T_{\text{ramp}}}$, $\dot{U}_{i}$ is the sampled acceleration for speed motion primitive $i\in[1,N_{U}]$, $T_{U}>0$ is the speed maneuver length and $T>0$ is the total trajectory length. Similarly, we define $N_{\chi}$ course motion primitives by the piecewise-linear course acceleration trajectories:

[TABLE]

where $k_{r,i}=\frac{\dot{r}_{i}}{T_{\text{ramp}}}$, $\dot{r}_{i}$ is the sampled acceleration for course motion primitive $i\in[1,N_{\chi}]$ and $T_{\chi}>0$ is the course maneuver length. For notational simplicity and without loss of generality, we assumed zero initial time $t_{0}=0$ in (12) and (13). The acceleration trajectories and parameters for $N_{U}=5$ speed motion primitives and $N_{\chi}=5$ course motion primitives are illustrated in Fig. 7. Notice that the integral of the course acceleration maneuvers are zero, hence if the maneuver is initialized with zero yaw rate the maneuver will end with zero yaw rate.

Based on the acceleration trajectories, we create trajectories for the desired speed, yaw rate and course by integrating the expressions (12) and (13) as:

[TABLE]

The initial values $U_{d,0}$, $r_{d,0}$ and $\chi_{d,0}$ are taken as the corresponding desired values from the last BC-MPC iteration (or sub-trajectory, if computing trajectories for subsequent maneuvers), such that the desired trajectories passed to the vessel controllers are continuous. This implies that we do not include feedback in the desired trajectories. Furthermore, as in Section 2, the vessel sideslip is neglected. This could, however, be included by using a vessel model including sideslip. A numerical example of $5$ speed and $5$ course trajectories is shown in figures 8 and 9, where a maneuver length of $5~{}$\mathrm{s}\text{/}$$ is used for both speed and course. Vessels at sea usually maneuver by either keeping a constant speed and course or by performing a speed and/or course change and continuing with this new speed and course for some time. By selecting the initial yaw rate in (14) as $r_{d,0}=0$ we ensure that maneuvers start and end with constant-course motion, which mimics this behavior while also producing maneuvers that should be readily observable for other vessels, as required by rule 8 of COLREGs.

Following this, we create a union set of the desired velocity trajectories as:

[TABLE]

resulting in a total of $N_{U}\!\cdot\!N_{\chi}$ desired velocity trajectories. Notice that the speed trajectories in $\mathcal{U}_{d}$ are continuously differentiable, while the course trajectories are twice continuously differentiable. Velocity trajectories containing infeasible steady-state vessel velocities are removed from $\mathcal{U}_{d}$ by checking the feasibility using the vessel model (2) together with the actuator saturation constraints.

Given the desired velocity trajectories, we calculate the feedback-corrected pose trajectories. To do this, we first predict the resulting speed and course trajectories, $\bar{U}_{i}(t)$, $i\in[1,N_{U}]$ and $\bar{\chi}_{i}(t)$, $i\in[1,N_{\chi}]$, respectively. This is done by simulating the closed-loop error dynamics of the vessel and vessel controllers using the desired velocity trajectories as the input. In this article, we approximate the error dynamics using first order linear models, which may seem as quite rough approximations. However, this is justified by noting that the model-based speed and course controller demonstrates very good control performance for the Telemetron ASV, resulting in small control errors [Eriksen and Breivik, 2018]. Furthermore, the control errors are dominated by environmental disturbances, which is difficult to model without increasing the complexity to an unnecessarily high level. The closed-loop error models are given as:

[TABLE]

where $\tilde{U}=\bar{U}-U_{d}$, $\tilde{\chi}=\bar{\chi}-\chi_{d}$, and $T_{\tilde{U}}>0$ and $T_{\tilde{\chi}}>0$ are time constants. The time constants are heuristically determined through simulations and experiments. Using the error model, the predicted speed and course trajectories are found as:

[TABLE]

where $\tilde{U}_{0}=U_{0}-U_{d,0}$ and $\tilde{\chi}_{0}=\chi_{0}-\chi_{d,0}$ introduces feedback in the prediction through the current vessel speed and course, $U_{0}$ and $\chi_{0}$, respectively. Similarly as (15), we construct a set of predicted velocity trajectories:

[TABLE]

Combinations of speed and course trajectories that was considered infeasible when forming $\mathcal{U}_{d}$ are also removed from $\bar{\mathcal{U}}$. Following this, vessel position trajectories $\bar{\boldsymbol{p}}(t)=\begin{bmatrix}\bar{N}(t)&\bar{E}(t)\end{bmatrix}^{T}$ are calculated from the predicted velocity trajectories using a kinematic model:

[TABLE]

which is integrated using the current vessel position as the initial condition. The feedback-corrected predicted vessel pose trajectories are finally combined in the set $\bar{\mathcal{H}}$ as:

[TABLE]

where $\bar{\boldsymbol{\eta}}=\begin{bmatrix}\bar{N}(t)&\bar{E}(t)&\bar{\chi}(t)\end{bmatrix}^{T}$.

To summarize, a single step of a trajectory is defined by the set of desired velocity trajectories $\mathcal{U}_{d}$, the set of predicted velocity trajectories $\bar{\mathcal{U}}$ and the set of set of predicted pose trajectories $\bar{\mathcal{H}}$.

3.1.2 Trajectory generation: The full trajectory generation

A full trajectory consists of multiple sub-trajectories, each containing one maneuver and constructed using the single-step procedure. This naturally forms a tree structure, with nodes representing vessel states and edges representing sub-trajectories. The depth of the tree will be equal to the desired number of maneuvers in each trajectory. The tree is is initialized with the initial state as the root node, which the single-step procedure is performed on, generating a number of sub-trajectories and leaf nodes. Following this, the the single-step procedure is performed on each of the leaf nodes, adding the next sub-trajectory and leaf nodes to the existing trajectories and expanding the tree depth. This procedure is repeated until the tree has the desired depth, resulting in each trajectory having the desired number of maneuvers. Using the same number of speed and course maneuvers at each level would result in the tree growing exponentially with the number of levels. To limit the growth, we therefore allow for choosing a different number of speed and course maneuvers at each level, for instance keeping the speed constant in all levels except the first, only allowing the speed to be changed during the first maneuver of a trajectory.

The remaining parameters can also be chosen differently for each level, and in principle the acceleration trajectories (12) and (13) can also be designed using different structures. However, we choose to use the same acceleration trajectory structure for each level, while also keeping the ramp time and maneuver time lengths constant. This leaves only the step time length and number of speed and course maneuvers as parameters that can change throughout the tree depth. Choosing different step time lengths can be considered as an MPC input blocking scheme, requiring that the step lengths are integer dividable by the algorithm sample time.

A full trajectory generation can hence be defined by the following parameters:

• •

An initial vessel state including the current desired velocity.

• •

The number of maneuvers in each trajectory, or levels, defined as $B>0$.

• •

The step time at each level $\boldsymbol{T}=\begin{bmatrix}T_{1}&T_{2}&\ldots&T_{B}\end{bmatrix}$, the ramp time $T_{\text{ramp}}$ and the speed and course maneuver lengths $T_{U}$ and $T_{\chi}$, respectively.

• •

The number of speed maneuvers at each level $\boldsymbol{N}_{U}=\begin{bmatrix}N_{U,1}&N_{U,2}&\ldots&N_{U,B}\end{bmatrix}$.

• •

The number of course maneuvers at each level $\boldsymbol{N}_{\chi}=\begin{bmatrix}N_{\chi,1}&N_{\chi,2}&\ldots&N_{\chi,B}\end{bmatrix}$.

A set of predicted vessel pose trajectories with $B=3$ levels is shown in Fig. 10. The step time is chosen as \boldsymbol{T}=\begin{bmatrix}5~{}\mathrm{s}\text{/}&10~{}\mathrm{s}\text{/}&10~{}\mathrm{s}\text{/}\end{bmatrix}, making the trajectories $25~{}$\mathrm{s}\text{/} long in total. The trajectories have $5$ course maneuvers at the first level and three in the later levels, while there for illustrational purposes are only one [speed](#glo.maina.sog) maneuver at each step. Hence, $\boldsymbol{N}_{U}=\begin{bmatrix}1&1&1\end{bmatrix}$ and $\boldsymbol{N}_{\chi}=\begin{bmatrix}5&3&3\end{bmatrix}$. The ramp time and maneuver lengths are chosen as $T_{\text{ramp}}=1~{}$\mathrm{s}\text{/} and $T_{U}=T_{\chi}=5~{}$\mathrm{s}\text{/}$$, respectively. Notice that the maneuver length of $5$ seconds results in the second and third maneuver having a straight-course segment after the turn, which increases the prediction horizon without increasing the computational load while also increasing the maneuver observability.

3.1.3 Calculating a desired acceleration

In the single-step trajectory generation, a desired acceleration $(\dot{U}_{d}^{\prime},\dot{r}_{d}^{\prime})$ can be used to include a desired maneuver in the search space. We therefore use a guidance algorithm to ensure that there exists a trajectory in the search space that converges towards the desired trajectory inputted to the BC-MPC algorithm. To achieve this, we use a modified version of a path tracking algorithm ensuring vessel convergence to a curved path [Breivik and Fossen, 2004]. The control law is based on line of sight (LOS) guidance [Fossen, 2011], together with defining a desired point on the path which the velocity of is controlled, named the path particle (PP). The desired course is stated as:

[TABLE]

where $\chi_{path}$ is the path angle at the desired point, $e$ is the cross-track error and $\Delta>0$ is the lookahead distance. The path particle velocity along the path is stated as:

[TABLE]

where $U$ is the vessel speed, $\gamma_{s}>0$ is a tuning parameter and $s$ is the along-track distance. This control law controls the speed along the path $U_{PP}$ as a function of the vessel speed, course and the along-track distance to the path particle, letting the vessel converge towards the path with a constant speed. We rather want to be able to follow a desired trajectory by controlling the vessel speed and course based on the desired trajectory. We therefore fix the path particle at the desired position on the trajectory, given the current time, and by reformulating (22) we obtain a desired vessel speed given the trajectory velocity:

[TABLE]

where $U_{t}$ is the trajectory velocity and $\epsilon>0$ is a small constant to avoid division by zero. The saturation function ensures that the desired vessel speed is in the interval $[0,U_{\max}]$, where $U_{\max}>0$ is the maximum vessel operating speed. Given a desired speed and course, we compute the desired speed and course acceleration:

[TABLE]

which are found by solving (14) for the final desired speed and course. Notice that in cases where there is only one speed and/or course maneuver, the corresponding desired acceleration should be selected as zero to keep a constant speed and/or course.

The obvious singularity in (23) when the vessel course is perpendicular to the desired path (and hence $\cos(\chi-\chi_{path}=0$) is handled by avoiding division by zero and ensuring that the desired speed is inside the possible operating speed of the vessel, which makes it difficult to guarantee stability and convergence of this guidance scheme. However, the desired acceleration is only used to modify some trajectories in the BC-MPC search space, and will hence not constrain the algorithm to choose a trajectory based on (24). One could employ other schemes, e.g. [Paliotta, 2017] which guarantees convergence to curved trajectories. This does, however, increase the complexity by depending on a detailed 3DOF model of the vessel while also employing a feedback-linearizing controller to control the vessel. It is in general difficult to obtain detailed models of high-speed ASVs, while time delays, sensor noise and modeling uncertainties are shown to cause robustness issues when using feedback-linearizing controllers [Eriksen and Breivik, 2017a]. Hence, the simplicity of (21)–(23) is appealing when a guarantee of stability and convergence is not required.

3.2 Selecting the optimal trajectory

Given the set of feasible trajectories, we solve an optimization problem to select the optimal trajectory. We start by defining a cost function to assign a cost to each trajectory:

[TABLE]

where $(\bar{\boldsymbol{\eta}}(t),\boldsymbol{u}_{d}(t))$ is the predicted vessel pose and desired velocity of a candidate trajectory, $\text{align}(\bar{\boldsymbol{\eta}}(t);\boldsymbol{p}_{d}(t))$ measures the alignment between the predicted pose trajectory and a desired trajectory $\boldsymbol{p}_{d}(t)$, $\text{avoid}(\bar{\boldsymbol{\eta}}(t))$ assigns cost to trajectories traversing close to obstacles, while $\text{tran}(\boldsymbol{u}(t))$ introduces transitional cost in the objective function to avoid wobbly behavior. The parameters $w_{\text{al}},w_{\text{av}},w_{\text{t}}\geq 0$ are tuning parameters to control the weighting of the different objective terms.

Using (25), we define the optimization problem:

[TABLE]

where $\boldsymbol{u}_{d}^{*}(t)$ is the optimal desired velocity trajectory to be used as the reference for the vessel controllers. The optimization problem is solved by simply calculating the cost over the finite discrete set of trajectories and choosing the one with the lowest cost.

The next sections describe the different terms of the objective function (25). Notice that we strive to avoid using discontinuities and logic in order to improve the robustness with respect to obstacle estimate noise.

3.2.1 Trajectory alignment

The alignment between the desired trajectory and a candidate trajectory is used in the objective function (25) to motivate the algorithm to follow the desired trajectory. Given a desired trajectory $\boldsymbol{p}_{d}(t):\mathrm{R}^{+}\to\mathrm{R}^{2}$, required to be $C^{1}$, we obtain a desired course as:

[TABLE]

with $\boldsymbol{p}_{d}(t)=\begin{bmatrix}N_{d}(t)&E_{d}(t)\end{bmatrix}^{T}$. Given this, we define a weighted metric of Euclidean distance and orientation error as:

[TABLE]

where $w_{p},w_{\chi}>0$ are weights controlling the influence of the Euclidean and angular error, respectively, $T_{\text{full}}=\sum_{i=1}^{B}T_{i}$ denotes the entire trajectory prediction horizon, while $\Upsilon:\mathbb{R}\to S^{1}$ maps an angle to the domain $[-\pi,\pi)$. For simplicity, we fix $w_{p}=1$ and leave $w_{\chi}$ and $w_{\text{al}}$ to control the weighting.

3.2.2 Obstacle avoidance

Obstacle avoidance is achieved by penalizing candidate trajectories with small distances to obstacles. We define three regions around the obstacles, named the collision, safety and margin regions, respectively. The idea behind this is to make it possible use different gradients on the penalty depending on how close the ownship is to the obstacle, which should improve the robustness with respect to noise on the obstacle estimates.

We define a time-varying vector between obstacle $i$ and a predicted vessel trajectory as:

[TABLE]

where $\boldsymbol{r}_{i}=\begin{bmatrix}r_{N,i}&r_{E,i}\end{bmatrix}^{T}$ and $\boldsymbol{p}_{i}(t)$ is the position of obstacle $i$ at time $t$. The obstacle position in future time is computed under the common assumption that obstacles will keep their current speed and course [Johansen et al., 2016, Kuwata et al., 2014, Eriksen et al., 2018], which is a reasonable assumption for relatively short time periods. More complex techniques can also be applied for predicting the future position of obstacles, for instance based on historic AIS data [Dalsnes et al., 2018] or by estimating the turn rate of the obstacles [Flåten and Brekke, 2017]. Using (29), we define the distance and relative bearing to obstacle $i$ given a predicted vessel trajectory $\bar{\boldsymbol{\eta}}(t)$ as:

[TABLE]

where $\chi_{i}(t)$ is the course of obstacle $i$, calculated as $\chi_{i}(t)=\text{atan2}\left(\dot{E}_{i}(t),\dot{N}_{i}(t)\right)$ with $\boldsymbol{p}_{i}(t)=\begin{bmatrix}N_{i}(t)&E_{i}(t)\end{bmatrix}^{T}$. The distance $d_{i}$ and relative bearing $\beta_{i}$ are illustrated in Fig. 11.

The obstacle distance and relative bearing is used to calculate a penalty function, which we use to define the avoidance function as:

[TABLE]

where $M$ is the number of obstacles, $\text{penalty}_{i}(\bar{\boldsymbol{\eta}}(t))$ assigns a penalty to the predicted vessel trajectory $\bar{\boldsymbol{\eta}}(t)$ at time $t$ with respect to obstacle $i$, while $w_{i}(t)$ are time and obstacle dependent weights. The weights can be useful for prioritizing vessels in multi-encounter situations where properties like vessel type, size, speed, etc. can be used for differentiating the importance of avoiding the given vessels in severe situations. The weights can also facilitate time-dependent weighting, for instance as a heuristic method to incorporate uncertainty on obstacle estimates, combined with obstacle and time-dependent scaling of the obstacle region sizes. For simplicity, we keep the weights constant at $w_{i}(t)=1\,\forall\,i$.

The penalty function can be designed in a variety of ways, with the simplest possibly being a circular penalty function. When using a circular penalty function, the relative bearing to the obstacle does not matter, and the function can be defined as:

[TABLE]

where the parameters of $d_{i}(\bar{\boldsymbol{\eta}}(t);\boldsymbol{p}_{i}(t))$ are omitted for notational simplicity. The variables $D_{2}>D_{1}>D_{0}>0$ are the margin, safety and collision region sizes, respectively, while $\gamma_{1}\in(0,1)$ is a tuning parameter controlling the cost gradient inside the margin and safety regions.

The circular penalty function is illustrated in Fig. 12.

A circular penalty function is useful for static objects where there is no preference on which side of the object one should pass. For moving vessels, it should be considered to be more dangerous to be in front of the vessel than on the side or behind it, and COLREGs also introduce preferences on which side one should pass an obstacle. For short-term COLAV, it is not beneficial to constrain the algorithm to strictly follow the overtaking, head on and crossing rules (rules 13–15), since maneuvers ignoring these rules may be required to fulfill rule 17. However, the algorithm should choose maneuvers compliant with rules 13–15 when this is possible. We therefore motivate the algorithm to choose COLREGs-aware maneuvers by defining an elliptical COLREGs penalty function by letting the region sizes $D_{0},D_{1}$ and $D_{2}$ be dependent on the relative bearing. Each region is defined by a combination of three ellipses and one circle as:

[TABLE]

where $a_{k}$, $b_{k}$ and $c_{k}=b_{k}+d_{\text{COLREGs}}$ with $k\in\{0,1,2\}$ defines the major and minor ellipsis axis. The parameter $d_{\text{COLREGs}}$ controls the region expansion of the starboard side of the obstacle. The regions are illustrated in Figure 13(a).

If we were to use (32) with $D_{k}$ from (33) as the elliptical COLREGs penalty function, the entire collision region would have a constant penalty. This poses a potential problem since all points inside the region is considered to be equally costly. For the circular penalty function, this region is so small that the impact is quite low. For the elliptical COLREGs penalty function, however, it is natural to have a non-constant cost inside the collision region since this is rather large. We therefore define the elliptical COLREGs penalty function as:

[TABLE]

where $D_{k}$, $k\in\{0,1,2\}$ are given by (33) and $\text{inner\_penalty}_{i}(\bar{\boldsymbol{\eta}}(t))$ is an additional cost inside the collision region. This additional cost is given as:

[TABLE]

where $D_{0}^{*}$ given as:

[TABLE]

and $y_{b}(d_{i},\beta_{i})$ is the distance from the $D_{0}^{*}$ region to the point $(d_{i},\beta_{i})$ along the y-direction of the obstacle body frame, as illustrated in Figure 13(b).

3.2.3 Transitional cost

An important design criteria for the algorithm is that it should be robust with respect to noise on the obstacle estimates, making it well suited for use with tracking systems based on exteroceptive sensors. By introducing transitional cost in the objective function, a certain level of cost reduction will be required to make the algorithm change the current planned maneuver. This should increase the robustness to noise on the obstacle estimates, while also making the algorithm less affected by noise in the vessel state estimates and external disturbances, for instance wave induced motion.

Denoting the desired velocity trajectory from the previous iteration as $\boldsymbol{u}_{d}^{-}(t)$, which is currently being tracked by the vessel controllers, the transitional cost is computed as:

[TABLE]

with $\boldsymbol{u}_{d}(t)=\begin{bmatrix}U_{d}(t)&\chi_{d}(t)\end{bmatrix}^{T}$, $\boldsymbol{u}_{d}^{-}(t)=\begin{bmatrix}U_{d}^{-}(t)&\chi_{d}^{-}(t)\end{bmatrix}^{T}$ and where $T_{1}$ is the step time of the first trajectory maneuver. The variables $e_{U,\min}$ and $e_{\chi,\min}$ denote the minimum difference between the previous desired velocity trajectory and the candidates:

[TABLE]

The transitional cost term is zero if the first maneuver of the candidate desired velocity trajectory $\boldsymbol{u}_{d}(t)$ is the one closest to the desired velocity trajectory from the previous iteration $\boldsymbol{u}_{d}^{-}(t)$, and one otherwise. Notice that the transitional cost term introduces discontinuities, which we previously stated that we would like to avoid in order to improve the robustness with respect to noise on obstacle estimates. The transitional cost term does, however, not rely on obstacle estimates, making the term insensitive to noise on the obstacle estimates and justifying the use of a discontinuous transitional cost function.

4 Experimental results

Full scale experiments were conducted in the Trondheimsfjord, Norway, on the 12th of October $2017$. This chapter describes the experimental setup and results.

4.1 Experimental setup

The Telemetron ASV, briefly introduced in Section 2, was used as the ownship. The vessel is fitted with a SIMRAD Broadband 4G™ Radar, and a Kongsberg Seatex Seapath 330+ GNSS-aided inertial navigation system was used during the experiments. See Table 1 for more details on the vessel specifications. The BC-MPC algorithm was implemented in discrete time using the Euler method to discretize the algorithm, see Table 2 for the algorithm parameters. We inputted a user-specified straight line trajectory with constant speed as the desired trajectory, and used the elliptical COLREGs penalty function for obstacle avoidance. The BC-MPC algorithm was run at a rate of $0.2~{}$\mathrm{Hz}\text{/}$$.

The implementation consists of a radar-based tracking system to provide obstacle estimates, the BC-MPC algorithm and the model-based speed and course controller described in section 2.2 for low-level vessel control. The system was implemented on a processing platform with an Intel® i7 $3.4~{}$\mathrm{GHz}\text{/}$$ CPU running Ubuntu 16.04 Linux, using the Robot Operating System (ROS) [Quigley et al., 2009]. Fig. 14 shows the implementation architecture.

The tracking system receives spoke detections from the radar through a UDP interface. The detections are transformed to a local reference frame and clustered together to form one measurement per obstacle, which is a common assumption for many tracking algorithms. The obstacle measurements are used by the radar tracker, which is based on a probabilistic data association filter (PDAF). See [Wilthil et al., 2017] for more details on the tracking system.

The BC-MPC algorithm interfaces the tracking system using a ROS service, which enables request-response functionality for providing obstacle estimates. The BC-MPC outputs a desired velocity trajectory to the model-based speed and course controller, which specifies a throttle and rudder command to the on-board control system through a TCP interface. The on-board control system has an electro-mechanical actuator for controlling the motor throttle, while the rudder command is handled by steering the outboard engine angle to the desired angle using a PD controller and a hydraulic actuation system.

The system receives AIS messages over VHF to obtain ground-truth trajectories for the vessels involved in the experiments. Notice that these are subject to the uncertainty of the navigation system providing the AIS data on the given vessels. They are, however, expected to be much more precise than the estimates from the radar-based tracking system. Figure 15(a) shows the inside of the Telemetron ASV, with the navigation system and processing platform.

The Kongsberg Seatex Ocean Space Drone 1 (OSD1) was used as the obstacle. This was originally an offshore lifeboat, which has been fitted with a full control and navigation system for testing autonomous control systems, shown in fig. 15(b). The vessel is $12~{}$\mathrm{m}\text{/}$$ long, and has a mass of approximately $10$ metric tons.

During the experiments, the OSD1 was steered on constant course with a speed of approximately $2.5~{}$\mathrm{m}\text{/}\mathrm{s}$$ ($5$ knots) using an autopilot. In addition to the OSD1, several commercial and leisure crafts were present in the area, affecting some of the scenarios.

We included four different scenarios in the experiments:

Head on. The ownship and OSD1 approaches each other on reciprocal courses. With respect to COLREGs, both vessels are required to perform starboard maneuvers. 2. 2.

Crossing from starboard. The OSD1 approaches from $90$\mathrm{\SIUnitSymbolDegree}\text{/}$$ on the ownship’s starboard side. In this case, COLREGs requires the ownship to avoid collision, preferably by making a starboard maneuver and passing behind the OSD1. 3. 3.

Overtaking. The ownship approaches the OSD1 from behind with a higher speed. COLREGs requires the ownship to avoid collision by passing on either side. We prefer, however, to pass the OSD1 on its port side by doing a port maneuver. 4. 4.

Crossing from port. Similar scenario as crossing from starboard, but here the OSD1 approaches the ownship from the port side. In this case, COLREGs deems the ownship as the stand-on vessel, and the OSD1 is supposed to avoid collision. The OSD1 will, however, keep its speed and course, requiring action with respect to rule $17$ where the give-way vessel fails to avoid collision. The ownship must then avoid collision, preferably avoiding maneuvering to port.

In the following sections, we present three head-on scenarios, two crossing from starboard scenarios, one overtaking scenario and one crossing from port scenario.

4.2 Head on: Experiments 1.1–1.3

The first experiments we performed were a number of head-on scenarios. In these scenarios, the desired trajectory inputted to the BC-MPC algorithm is a straight-line trajectory approaching the OSD1 on a reciprocal course, resulting in a collision with a relative bearing of $0$\mathrm{\SIUnitSymbolDegree}\text{/}$$ if the desired trajectory is followed. With respect to COLREGs, both vessels should perform starboard maneuvers. However, in our case, the OSD1 violates COLREGs by keeping its speed and course constant throughout the scenario.

To verify that the BC-MPC algorithm worked as it was supposed to, we first used AIS for providing obstacle estimates in Experiment 1.1. The OSD1 is equipped with an AIS transceiver providing low-noise estimates of the position, speed and course, originating from a Kongsberg Seatex SeaNav 300 navigation system.

As shown in Fig. 16, we successfully avoid collision by performing a port maneuver in this scenario. This violates the preferred COLREGs action to turn to starboard in a head-on situation, and is most likely caused by the ownship approaching the obstacle on the port side of the desired trajectory, which together with the slightly angled obstacle trajectory makes a port maneuver attractive. We do, however, pass the obstacle with a large clearance, which demonstrates the COLREGs awareness of the BC-MPC algorithm. Moreover, the maneuver is smooth with a sufficient course change to be readily observable for other vessels.

Figure 17(a) shows the distance between the OSD1 and the ownship, and the predicted future distance given the trajectory the BC-MPC algorithm chose at each iteration, while Figure 17(b) shows the estimated and actual speed and course of the OSD1. The estimated values are in this case based on AIS, and hence equal to the ground truth. The OSD1 does, however, transmit AIS messages quite seldom, introducing some delay in the estimated speed and course.

Following this experiment, we performed several experiments using the radar-based tracking system for providing obstacle estimates.

Fig. 18 shows the results from Experiment 1.2, a similar experiment as the one performed with AIS. In this experiment, the ownship performs a starboard maneuver in order to avoid collision, as preferred by COLREGs. As shown in the figure, there is a fair amount of noise on the obstacle estimates, in particularly the course estimate. This is confirmed by the course estimate shown in fig. 19(b), which shows course fluctuations often in excess of $20$\mathrm{\SIUnitSymbolDegree}\text{/}$$. Despite this, the ownship performs a smooth maneuver, which demonstrates the BC-MPC algorithm’s robustness with respect to noise on the obstacle estimates.

This is also shown in fig. 19(a), where the predicted distance to the obstacle varies quite much without making the algorithm decide on a new maneuver.

The last head-on scenario, Experiment 1.3, is shown in Fig. 20, where we approach the OSD1 from north-east. The predicted future obstacle trajectories at each iteration are omitted from the following figures to improve the readability. This scenario was slightly more complex, as two other vessels unexpectedly entered the scenario. One of these was a high-speed leisure craft approaching from the west, while the other was a high-speed passenger ferry approaching from south-east, behind the ownship. The leisure craft did not have AIS, and we do therefore not have a ground-truth trajectory for this vessel. Fig. 21 shows an image captured by a drone during this experiment, with algorithm visualization embedded in the lower left corner. As in the previous scenario, we avoid the OSD1 by doing a starboard maneuver. Following this, we approach the desired trajectory before the passenger ferry approaches from abaft. With respect to COLREGs, this is an overtaking situation where we are deemed the stand-on vessel, and the passenger ferry “Trondheimfjord II” is supposed to give way to us. However, as mentioned earlier, the algorithm is designed to also handle the situations where the give-way vessel does not adhere to its obligations, requiring action by the stand-on vessel. Hence, the algorithm chooses to do a new starboard maneuver to let the passenger ferry pass. Eventually, the ferry turns towards the Trondheim Harbor allowing the ownship to approach the desired trajectory once again. There is some wobbling in the ownship trajectory which is most likely caused by obstacle estimate noise. This could possibly be avoided by changing the tuning parameters of the BC-MPC algorithm, namely increasing the transitional cost weight.

In this experiment, the amount of estimate noise is even larger than in the previous experiment, with course fluctuations up to $40$\mathrm{\SIUnitSymbolDegree}\text{/}$$ as seen in fig. 22(b). Still, as shown in figs. 22(a) and 20, the BC-MPC manages to make quite smooth maneuvers, which again shows robustness with respect to obstacle estimate noise. Fig. 23 shows similar plots for the Trondheimfjord II ferry. Notice that it takes some time before the tracking system detects that the passenger ferry makes a maneuver, which is due to a limited sample rate on the radar combined with some latency in the PDAF tracking system.

4.3 Crossing from starboard: Experiments 2.1–2.2

Crossing from starboard is a more complex scenario than the head-on scenario. We performed two experiments with the OSD1 approaching on collision course from starboard. The scenarios were constructed such that the desired trajectory coincides with the obstacle trajectory, resulting in a collision with a relative bearing of $-90$\mathrm{\SIUnitSymbolDegree}\text{/}$$ if the desired trajectory is followed. In such a scenario, the ownship is deemed the give-way vessel and should avoid collision by preferably maneuvering to starboard and passing abaft of the stand-on vessel.

In Experiment 2.1, shown in Fig. 24, we avoided collision with the OSD1 by maneuvering to port and passing in front of the obstacle. This can be considered as suboptimal with respect to the preferred action being passing abaft of the obstacle. However, the minimum distance to the obstacle is $214.0~{}$\mathrm{m}\text{/}$$, meaning that the obstacle is only slightly inside the margin region. With this in mind, the maneuver is considered to be safe. This is, once again, an example on how the algorithm demonstrates COLREGs awareness.

In Experiment 2.2, shown in Fig. 25, we avoided collision by passing abaft of the OSD1, as preferred by COLREGs. In this experiment, the minimum distance to the obstacle was $106.2~{}$\mathrm{m}\text{/}$$, significantly closer than when we passed in front of the obstacle. This is still only slightly inside the margin region, remembering that the elliptical COLREGs penalty function is smaller abaft the obstacle than in front of the obstacle.

4.4 Overtaking: Experiment 3

Another distinct situation is when the ownship approaches an obstacle from behind, overtaking it. With respect to COLREGs, the overtaking vessel is deemed the give-way vessel, while the overtaken vessel is deemed the stand-on vessel. There is no strict rules on whether the give-way vessel should pass the stand-on vessel on the port or starboard side, but we prefer to pass on the port side of the stand-on vessel, as this does not block the stand-on vessel’s possibilities in maneuvering to starboard if it finds itself in a head-on or crossing situation while being overtaken.

Fig. 26 shows Experiment 3, where the ownship overtakes the OSD1. The ownship maneuvers to port, passing the OSD1 on her port side. The ownship trajectory is quite smooth, but turns towards the desired trajectory a bit early. This was caused by the radar tracking system detecting a navigational aid in front of the ownship on the port side, which made maneuvering closer to the desired trajectory preferable. The ownship was approximately $200~{}$\mathrm{m}\text{/} in front of the obstacle when doing this maneuver. Notice that we currently do not distinguish between dynamic and static objects in the tracking system, hence this navigational aid was considered as a moving vessel. The closest distance to the obstacle during the overtaking maneuver was $127.3~{}$\mathrm{m}\text{/}, approximately equal to the size of the margin region on the port side of the obstacle.

4.5 Crossing from port: Experiment 4

The last scenario we tested was a crossing from port, which may be the most complex scenario of the ones presented in this article. This situation was generated similarly as the crossing from starboard situation, but with a relative bearing of $90$\mathrm{\SIUnitSymbolDegree}\text{/} instead of $-90$\mathrm{\SIUnitSymbolDegree}\text{/}. Here, COLREGs deems the ownship as the stand-on vessel, while the OSD1 is deemed the give-way vessel. However, the OSD1 keeps its speed and course, requiring the ownship to avoid collision. In such a situation COLREGs recommends the ownship to avoid maneuvering to port, favoring a starboard maneuver.

Fig. 27 shows the results from Experiment 4, where the BC-MPC algorithm maneuvers the ownship to starboard, following the recommendations in COLREGs regarding this situation. The algorithm chose to maneuver the ownship at the minimum speed in order to minimize the distance to the desired trajectory. This minimum speed ensures maneuverability of the ownship, and was by coincidence similar as the speed of the OSD1 during the experiment, resulting in the ownship trajectory following parallel to the obstacle trajectory. Obviously, the ownship could increase the speed and pass in front of the obstacle, but this is not apparent to the BC-MPC algorithm due to the limited prediction horizon. In a hybrid COLAV architecture, this situation would be solved by the mid-level COLAV algorithm, designed with a longer prediction horizon than the BC-MPC algorithm.

4.6 Experiment summary

The BC-MPC algorithm has been tested in four different scenarios, each with different desirable behavior. A total of 7 experiments is presented, and the key points and numbers of the experiments are given in Table 3.

In the head-on experiments, both AIS and radar tracking was used for providing obstacle estimates. In Experiment 1.1, where we used AIS for providing obstacle estimates, the ownship avoided collision by passing the obstacle on its starboard side, violating the desired behavior of COLREGs. The ownship did, however, maneuver with increased distance to the obstacle compared to experiments 1.2 and 1.3 where we passed the obstacle on its port side in accordance with the desired behavior of COLREGs. In experiments 1.2 and 1.3, we used the radar tracking system for obtaining obstacle estimates, which provided estimates with a large amount of noise compared to Experiment 1.1. The BC-MPC algorithm did, however, not seem to be significantly affected by this noise.

In the crossing from starboard experiments we only used radar tracking for providing obstacle estimates. In Experiment 2.1, we passed in front of the obstacle. This is not strictly forbidden by COLREGs, but is neither desirable. The ownship did, however, have a large clearance to the obstacle, demonstrating the COLREGs awareness of the BC-MPC algorithm. In Experiment 2.2, we passed behind the obstacle, complying with the desirable behavior of COLREGs.

In Experiment 3.1 the ownship overtook the obstacle on its port side. COLREGs does not dictate which side the obstacle should be passed on, but by maneuvering to port the obstacle is free to maneuver to starboard if it finds itself in a separate collision situation.

Experiment 4.1 is a crossing situation where the ownship is deemed the stand-on vessel, and the OSD1 is required to avoid collision. The OSD1 did, however, not fulfill her obligation to avoid collision, requiring that the ownship avoided collision in accordance with rule 17 of COLREGs. The ownship avoided collision by performing a starboard maneuver, as suggested by COLREGs.

In some of the experiments, the BC-MPC algorithm chose to ignore rules 13–15 of COLREGs. This is because the algorithm is designed to comply with rule 17 of COLREGs, which requires that the ownship avoids collision in cases where it is deemed the stand-on vessel if the other vessel does not avoid collision, ignoring rules 13-15 if required. In cases where the desired rule 13–15 behavior is ignored, the obstacle is passed with extra clearance.

5 Conclusion and further work

We have presented a new algorithm called the branching-course MPC (BC-MPC)algorithm for ASV collision avoidance (COLAV). The algorithm has been thoroughly tested in closed-loop full-scale experiments in the Trondheimsfjord in October 2017, using a radar-based system for obstacle detection and tracking. The algorithm performs well and displays good robustness with respect to noise on obstacle estimates, which is a significant source of disturbance when using tracking systems based on exteroceptive sensors to provide estimates of obstacle position, course and speed. During the experiments, leisure and commercial vessels entered some of the scenarios by coincidences and were successfully avoided by the BC-MPC algorithm without human intervention.

The BC-MPC algorithm is intended for use as a short-term COLAV algorithm, and should therefore always be able to find a feasible solution to avoiding collision. As required by COLREGs, this means that the normal COLREGs rules dictating the vessel behavior in head-on, crossing and overtaking scenarios sometimes needs to be ignored. However, the algorithm is motivated to follow the normal behavior described by COLREGs when possible. The algorithm is compliant with rules 8 and 17 of COLREGs, and motivated to follow rules 13–15 if possible, which is why the algorithm is denoted as COLREGs-aware.

In the future, we would like to do an extensive simulation study, analyzing the algorithm performance to a greater detail than what is possible through full-scale experiments. This can prove valuable in order to further develop the algorithm, and to tune the objective function to e.g. obtain smoother trajectories, which would improve the indirect communication of intention between the ASV and other vessels at sea based on their maneuvers. Furthermore, we would also like to distinguish between static and moving obstacles in the algorithm, and combine it with a long-term COLAV algorithm in a hybrid architecture.

Acknowledgments

This work was supported by the Research Council of Norway through project number 244116 and the Centres of Excellence funding scheme with project number 223254. The authors would like to express great gratitude to Kongsberg Maritime and Maritime Robotics for providing high-grade navigation technology, the Ocean Space Drone 1 and the Telemetron ASV at our disposal for the experiments.