Hybrid Collision Avoidance for ASVs Compliant with COLREGs Rules 8 and 13-17

TL;DR

This paper introduces a three-layered hybrid collision avoidance system for autonomous surface vehicles that complies with COLREGs rules 8 and 13-17, effectively managing multiple obstacles and emergency situations through advanced planning and control algorithms.

Contribution

The paper develops a mid-level COLAV algorithm that complies with COLREGs rules 13-17, integrating a new obstacle classification state machine and enhancing existing algorithms.

Findings

System successfully avoids collisions in all tested scenarios.

Maintains energy-optimized trajectory when no obstacles interfere.

Handles multi-obstacle situations with multiple active COLREGs rules.

Abstract

This paper presents a three-layered hybrid collision avoidance (COLAV) system for autonomous surface vehicles, compliant with rules 8 and 13-17 of the International Regulations for Preventing Collisions at Sea (COLREGs). The COLAV system consists of a high-level planner producing an energy-optimized trajectory, a model predictive control based mid-level COLAV algorithm considering moving obstacles and the COLREGs, and the branching-course model predictive control algorithm for short-term COLAV handling emergency situations in accordance with the COLREGs. Previously developed algorithms by the authors are used for the high-level planner and short-term COLAV, while we in this paper further develop the mid-level algorithm to make it comply with COLREGs rules 13-17. This includes developing a state machine for classifying obstacle vessels using a combination of the geometrical situation, the distance and time to the closest point of approach (CPA) and a new CPA-like measure. The performance of the hybrid COLAV system is tested through numerical simulations for three scenarios representing a range of different challenges, including multi-obstacle situations with multiple simultaneously active COLREGs rules, and also obstacles ignoring the COLREGs. The COLAV system avoids collision in all the scenarios, and follows the energy-optimized trajectory when the obstacles do not interfere with it.

Tables

Table 1. Table 1 : Tuning parameters for the high-level algorithm.

Param.	Value	Comment
$t_{\text{max}}$		Maximum trajectory time
Scenario 1	$1420s\text{/}$
Scenario 2	$1420s\text{/}$
Scenario 3	$725s\text{/}$
$N_{\text{hi}}$	$1000\text{/}$	Number of prediction steps
$K_e$	$1.0s^3\text{/}m$	Energy penalty gain
$K_{\delta }$	$1.0\text{/}$	Quadratic yaw control penalty gain
$L_m$	$4.0m\text{/}$	Length between control origin and outboard motor

Table 2. Table 2 : Tuning parameters for the mid-level algorithm.

Param.	Value	Comment
${\overline{d}}_{\text{CPA}}^{\text{enter}}$	$900m\text{/}$	State machine $d_{\text{CPA}}$ entry criteria
${\underset{¯}{d}}_{\text{CPA}}^{\text{exit}}$	$2000m\text{/}$	State machine $d_{\text{CPA}}$ exit criteria
$[{\underset{¯}{t}}_{\text{CPA}}^{\text{enter}},{\overline{t}}_{\text{CPA}}^{\text{enter}}]$	$[0,270]s\text{/}$	State machine $t_{\text{CPA}}$ entry criteria
$[{\underset{¯}{t}}_{\text{CPA}}^{\text{exit}},{\overline{t}}_{\text{CPA}}^{\text{exit}}]$	$[-20,290]s\text{/}$	State machine $t_{\text{CPA}}$ exit criteria
${\overline{t}}_{\text{crit}}^{\text{EM,enter}}$	$20s\text{/}$	Emergency state $t_{\text{crit}}$ entry criteria
${\underset{¯}{t}}_{\text{crit}}^{\text{EM,exit}}$	$25s\text{/}$	Emergency state $t_{\text{crit}}$ exit criteria
$h$	$10s\text{/}$	Step size
$N_p$	$36$	Number of prediction steps
$K_p$	$0.02$	Position error scaling
$\sigma$	$1$	Huber loss function threshold
$K_{\dot{U}}$	$0.3$	SOG-derivative penalty term scaling
$K_{\dot{\chi }}$	$2.5$	Course-derivative penalty term scaling
$K_{\text{HO}}$	$40$	Head-on potential function scaling
$[{\alpha }_{x,\text{HO}},{\alpha }_{y,\text{HO}}]$	$[1/500,1/400]$	Head-on potential function steepness parameters
$x_{0,\text{HO}}$	$1000m\text{/}$	Head-on potential function attenuation parameter
$K_{\text{GW}}$	$40$	Give-way potential function scaling
$[{\alpha }_{x,\text{GW}},{\alpha }_{y,\text{GW}}]$	$[1/400,1/500]$	Give-way potential function steepness parameters
$y_{0,\text{GW}}$	$-500m\text{/}$	Give-way potential function attenuation parameter
$K_{\text{SO}}$	$3$	Stand-on function scaling
$K_{\text{EM}}$	$3$	Emergency function scaling
$K_{\xi }$	$[0.1,1,10,100,\infty ]$	Iterative slack variable cost
$x_a$	$600m\text{/}$	Moving obstacle ellipsis major axis size
$y_a$	$225m\text{/}$	Moving obstacle ellipsis minor axis size

Table 3. Table 3 : Minimum distance to static and moving obstacles for the simulation scenarios.

Scenario	Minimum distance to static obstacles	Minimum distance to moving obstacle number
		1	2	3	4	5
Scenario 1	93.7 $m\text{/}$	$634.3m\text{/}$	$596.3m\text{/}$	$522.7m\text{/}$	$726.8m\text{/}$	–

Equations

$$\begin{array}{c}\dot{\mathbf{\eta }}=\mathbf{R}(\psi ){\mathbf{x}}_r+{\left[\begin{array}{cc}{\mathbf{V}}_c^{\top }& 0\end{array}\right]}^{\top }\\ \mathbf{M}({\mathbf{x}}_r){\dot{\mathbf{x}}}_r+\mathbf{\sigma }({\mathbf{x}}_r)=\mathbf{\tau },\end{array}$$

$$\mathbf{R}(\psi )=\left[\begin{array}{cc}\cos \psi & 0\\ \sin \psi & 0\\ 0& 1\end{array}\right],$$

$${\left(\frac{x-x_c}{x_a}\right)}^2+{\left(\frac{y-y_c}{y_a}\right)}^2\ge 1,$$

$$h_{o}(x,y,x_{c},y_{c},x_{a},y_{a},\alpha)=-\log\bigg{[}\left(\frac{(x-x_{c})\cos\alpha+(y-y_{c})\sin\alpha}{x_{a}}\right)^{2}\\ +\left(\frac{-(x-x_{c})\sin\alpha+(y-y_{c})\cos\alpha}{y_{a}}\right)^{2}+\epsilon\bigg{]}+\log(1+\epsilon)\leq 0\,.$$

$$u_{\text{nom}}=\frac{L_{\text{path}}}{t_{\text{max}}},$$

$$\underset{\mathbf{z}(\cdot ),\mathbf{\tau }(\cdot )}{\min }{\int }_0^{t_{\text{max}}}{F}_{\text{hi}}(\mathbf{z}(t),\mathbf{\tau }(t))\mathrm{d}t$$

$$\dot{\mathbf{z}}(t)=\mathbf{f}(\mathbf{z}(t),\mathbf{\tau }(t))\forall t\in [0,t_{\text{max}}]$$

$${\mathbf{h}}_{\text{hi}}(\mathbf{z}(t),\mathbf{\tau }(t))\le 0\forall t\in [0,t_{\text{max}}]$$

$${\mathbf{e}}_{\text{hi}}(\mathbf{z}(0),\mathbf{z}(t_{\text{max}}))=0.$$

$$F_{\text{hi}}(\mathbf{z},\mathbf{\tau })=K_e{F}_e(\mathbf{z},\mathbf{\tau })+K_{\delta }{\tau }_{\delta }^2,$$

$$F_e(\mathbf{z},\mathbf{\tau })=|\underset{\text{∝ surge force}}{\underbrace{n({\tau }_m{)}^2\cdot \cos \delta ({\tau }_{\delta })}}\cdot u_r|+|\underset{\text{∝ yaw moment}}{\underbrace{n({\tau }_m{)}^2\cdot \sin \delta ({\tau }_{\delta })\cdot L_m}}\cdot r|.$$

$$t_{\text{CPA}}=\{\begin{array}{ll}0& \text{if }\Vert \mathbf{v}-{\mathbf{v}}_0{\Vert }_2\le ϵ\\ \frac{(\mathbf{p}-{\mathbf{p}}_o)\cdot (\mathbf{v}-{\mathbf{v}}_o)}{\Vert \mathbf{v}-{\mathbf{v}}_o{\Vert }_2^2}& \text{else},\end{array}$$

$$d_{\text{CPA}}=\Vert (\mathbf{p}+t_{\text{CPA}}\mathbf{v})-({\mathbf{p}}_o+t_{\text{CPA}}{\mathbf{v}}_o){\Vert }_2.$$

$$\Vert (\mathbf{p}+t_{\text{crit}}\mathbf{v})-({\mathbf{p}}_o+t_{\text{crit}}{\mathbf{v}}_o){\Vert }_2=d_{\text{crit}}.$$

$${\text{entry}}_i$$

$${\text{entry}}_{\text{EM}}$$

$${\text{exit}}_i$$

$${\text{exit}}_{\text{EM}}$$

$${\overline{d}}_{\text{CPA}}^{i,\text{enter}}$$

$${\underline{t}}_{\text{CPA}}^{i,\text{enter}}$$

$${\overline{t}}_{\text{CPA}}^{i,\text{enter}}$$

$${\underline{d}}_{\text{CPA}}^{i,\text{exit}}$$

$${\underline{t}}_{\text{CPA}}^{i,\text{exit}}$$

$${\overline{t}}_{\text{CPA}}^{i,\text{exit}}$$

$${\bar{\mathbf{p}}}_d(t)={\mathbf{p}}_d(t+t_b),$$

$$\underset{\mathbf{\eta }(\cdot ),{\mathbf{x}}_r(\cdot )}{\min }\varphi (\mathbf{\eta }(\cdot ),{\mathbf{x}}_r(\cdot ))$$

$$\dot{\mathbf{\eta }}(t)=\mathbf{R}(\psi (t)){\mathbf{x}}_r(t)+\left[\begin{array}{c}{\mathbf{V}}_c\\ 0\end{array}\right]\forall t\in [t_0,t_0+T_h]$$

$${\mathbf{h}}_{\text{mid}}(\mathbf{\eta }(t),{\mathbf{x}}_r(t),t)\le 0\forall t\in [t_0,t_0+T_h]$$

$${\mathbf{e}}_{\text{mid}}(\mathbf{\eta }(t_0))=0,$$

$$\underset{\mathbf{w},\mathbf{\omega },\mathbf{\mu },\mathbf{\xi }}{\min }{\varphi }_p(\mathbf{w},\mathbf{\omega },\mathbf{\mu })+{\varphi }_c(\mathbf{w})+{\varphi }_{\text{COLREGs}}(\mathbf{w})+{\varphi }_{\xi }(\mathbf{\xi })$$

$$\mathbf{g}(\mathbf{w},\mathbf{\eta }(t_0))=0$$

$$\mathbf{h}(\mathbf{w},\mathbf{\xi })\le 0$$

$${\bar{\mathbf{h}}}_k({\mathbf{\eta }}_k,{\mathbf{\omega }}_k,{\mathbf{\mu }}_k,{\bar{\mathbf{p}}}_{d,k})\le 0\forall k\in \{1,\dots ,N_p\}$$

$$\mathbf{\xi }\ge 0,$$

$$H(\rho )=\{\begin{array}{ll}\frac{1}{2}{\rho }^2& |\rho |\le \sigma \\ \sigma (|\rho |-\frac{1}{2}\sigma )& |\rho |>\sigma .\end{array}$$

$${\bar{\varphi }}_p(\mathbf{w},\mathbf{\omega },\mathbf{\mu })=K_p\sum _{k=1}^{N_p}\sigma 1^{\top }{\mathbf{\omega }}_k+\frac{1}{2}{\mathbf{\mu }}_k^{\top }{\mathbf{\mu }}_k,$$

$${\bar{\mathbf{h}}}_k(\mathbf{w},\mathbf{\omega },\mathbf{\mu },{\bar{\mathbf{p}}}_{d,k})=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{k}}+{\mathbf{\mu }}_{\mathbf{k}}+{\mathbf{p}}_k-{\bar{\mathbf{p}}}_{d,k}\\ {\mathbf{v}}_{\mathbf{k}}+{\mathbf{\mu }}_{\mathbf{k}}-({\mathbf{p}}_k-{\bar{\mathbf{p}}}_{d,k})\\ -{\mathbf{\omega }}_k\end{array}\right]\le 0\forall k\in \{1,\dots ,N_p\},$$

$${\varphi }_c(\mathbf{w})=\sum _{k=0}^{N_p-1}{K}_{\dot{U}}{q}_{\dot{U}}({\dot{U}}_k)+K_{\dot{\chi }}{q}_{\dot{\chi }}({\dot{\chi }}_k),$$

Full text

1

This paper presents a three-layered hybrid collision avoidance (COLAV) system for autonomous surface vehicles, compliant with rules 8 and 13–17 of the International Regulations for Preventing Collisions at Sea (COLREGs). The COLAV system consists of a high-level planner producing an energy-optimized trajectory, a model predictive control based mid-level COLAV algorithm considering moving obstacles and the COLREGs, and the branching-course model predictive control algorithm for short-term COLAV handling emergency situations in accordance with the COLREGs. Previously developed algorithms by the authors are used for the high-level planner and short-term COLAV, while we in this paper further develop the mid-level algorithm to make it comply with COLREGs rules 13–17. This includes developing a state machine for classifying obstacle vessels using a combination of the geometrical situation, the distance and time to the closest point of approach (CPA) and a new CPA-like measure. The performance of the hybrid COLAV system is tested through numerical simulations for three scenarios representing a range of different challenges, including multi-obstacle situations with multiple simultaneously active COLREGs rules, and also obstacles ignoring the COLREGs. The COLAV system avoids collision in all the scenarios, and follows the energy-optimized trajectory when the obstacles do not interfere with it.

\helveticabold

2 Keywords:

Hybrid collision avoidance, Autonomous surface vehicle (ASV), COLREGs, COLREGs compliant, Model predictive control (MPC), Energy-optimized control

3 Introduction

Motivated by the potential for reduced costs and increased safety, the maritime industry is rapidly moving towards autonomous operations. Following groundbreaking advances in the automotive industry, many sectors within the maritime industry are considering the benefits of autonomy, which includes more environmentally friendly operations. For instance, the agricultural chemical company Yara together with the maritime technology supplier Kongsberg Maritime are developing the electrical autonomous container vessel Yara Birkeland, which aims to replace 40 thousand yearly truck journeys in urban eastern Norway111https://www.wsj.com/articles/norway-takes-lead-in-race-to-build-autonomous-cargo-ships-1500721202, Accessed 2019-05-22.. Another example is the world’s first autonomous car ferry, Falco, developed by Rolls-Royce (recently bought by Kongsberg Maritime) and Finferries. In 2018, Falco navigated autonomously between two ports in Finland222https://www.marinemec.com/news/view,rollsroyce-and-finferries-demonstrate-worlds-first-fully-autonomous-ferry_56102.htm, Accessed 2019-04-11.. Reports state that in excess of $75\text{\,}\mathrm{\char 37\relax}\text{/}$ of maritime accidents are due to human errors (Chauvin, 2011; Levander, 2017), indicating that there is also a potential for increased safety in addition to the economical and environmental benefits.

An obvious prerequisite for autonomous ship operations is the development of robust and well-functioning collision avoidance (COLAV) systems. In addition to generating collision-free maneuvers, a COLAV system must adhere to the “rules of the road” of the oceans, i.e. the International Regulations for Preventing Collisions at Sea (COLREGs) (Cockcroft and Lameijer, 2004). These rules are written for human ship operators and include qualitative requirements on how to perform safe and readily observable maneuvers. Part B of the COLREGs concern steering and sailing, and includes the following rules that are the most relevant to a motion control system: {desclist}    [Rules 13–15]

Requires maneuvers to be readily observable and to be done in ample time.

Describe the maneuvers to perform in cases of overtaking, head-on and crossing situations. Participants in crossing situations are defined by the terms give-way and stand-on vessels.

Requires that a give-way vessel must take early and substantial action to keep clear of the stand-on vessel.

Consists of two main parts. The first part requires a stand-on vessel to maintain its course and speed, while the second part allows/requires333Rule 17 allows the stand-on vessel to maneuver when it becomes apparent that the give-way vessel does maneuver to avoid collision. If the vessels are so close that the give-way vessel cannot avoid collision by itself, Rule 17 requires the stand-on vessel to maneuver. a stand-on vessel to take action to avoid collision if the give-way vessel is not taking action.

Since the rules are written for humans, with few quantitative figures, a challenge for autonomous operation is to quantify them into behaviors that can be executed algorithmically. The focus of the work in this paper is to do that, and to design a hybrid COLAV system that performs motion planning and generates maneuvers in compliance to rules 8 and 13–17 of the COLREGs.

A number of COLAV approaches considering the COLREGs have been proposed in the past. This includes algorithms using simulation-based model predictive control (Hagen et al., 2018), velocity obstacles (Kuwata et al., 2014), rule-based repairing A* (Campbell et al., 2014) and interval programming (Benjamin et al., 2006). All these approaches are single-layer approaches, where one algorithm solves the complete COLAV problem.

Another approach to the COLAV problem is to use a hybrid architecture, where the task of planning an obstacle-free path or trajectory, complying with the COLREGs and ultimately performing safe maneuvers is divided into layers in a control hierarchy. The idea of hybrid architectures is to divide the subtasks of the COLAV problem into multiple algorithms, exploiting their complementary strengths. This also has the side effect of making it easier for human operators or supervisors to understand the system. Most single-layer algorithms use sample-based approaches that consider a finite number of discrete control inputs, as opposed to conventional gradient-based search algorithms. The reason for this is that many gradient-based algorithms are not sufficiently numerically robust, not allowing a COLAV system to solely rely on such an algorithm. This issue can be handled in hybrid architectures, constrained by the bottom-level algorithm being numerically robust and able to handle extraordinary situations where the other algorithms fail. Hence, hybrid architectures also allows using gradient-based algorithms, which are able to solve problems with large search spaces more efficiently than sample-based algorithms. The works by Švec et al. (2013) and Loe (2008) are examples of two-layered hybrid COLAV architectures. The top layers perform trajectory planning among static obstacles, while the bottom layers perform moving obstacle avoidance in compliance with COLREGs rules 13–16. Casalino et al. (2009) presents a three-layered hybrid COLAV system where the top layer also performs trajectory planning amongst static obstacles. The middle layer avoids moving obstacles, while the bottom layer implements safety functions for handling cases where the two other layers fail. This approach does, however, not consider the COLREGs.

Figure 1 shows a three-layered hybrid COLAV system for an autonomous surface vehicle (ASV). The authors have previously worked extensively on different components of this architecture. Examples include high-level COLAV algorithms (Bitar et al., 2018, 2019b), a mid-level algorithm (Eriksen and Breivik, 2017b; Bitar et al., 2019a), short-term algorithms (Eriksen et al., 2018, 2019; Eriksen and Breivik, 2019) and the development of high-performance vessel controllers (Eriksen and Breivik, 2017a, 2018).

In this paper, we demonstrate the three-layered hybrid COLAV shown in Figure 1 by combining and extending the COLAV algorithms developed in (Bitar et al., 2019a; Eriksen and Breivik, 2017b; Bitar et al., 2019b; Eriksen et al., 2019; Eriksen and Breivik, 2019). The high-level planner has a long temporal horizon, and finds an energy-optimized nominal trajectory from an initial to a goal position considering static obstacles. Since the high-level planner only considers static information, it is intended to be run offline, but it can also be run online, for instance if new static obstacles are detected. The mid-level algorithm attempts to follow this nominal trajectory, while performing COLAV of moving obstacles in compliance with COLREGs rules 8, 13–16 and the first part of Rule 17. The mid-level algorithm is run periodically with a shorter temporal horizon than the high-level algorithm, and produces a modified trajectory which is passed to the short-term layer. Both the high-level and mid-level algorithms use gradient-based optimization. The short-term algorithm attempts to follow the modified trajectory, while it in compliance with the second part of Rule 17 handles situations where obstacles ignore the COLREGs. This algorithm also handles other emergency situations, and uses sample-based optimization to achieve a high level of robustness, ensuring safe operation if the mid-level algorithm fails to find a solution. The following list summarizes our contributions:

• •

The high-level planner from (Bitar et al., 2019b) is modified to include the mathematical model of the Telemetron ASV in (Bitar et al., 2019a), including ocean currents.

• •

The development of a state-machine-based COLREGs interpretation scheme.

• •

The mid-level COLAV from (Bitar et al., 2019a) is modified to include rules 13–16 and the first part of Rule 17.

• •

The branching-course model predictive control (BC-MPC) algorithm for short-term COLAV is modified to reduce oscillatory behavior in turns.

• •

The three-layered COLAV system is verified in simulations and shown to be compliant with rules 8 and 13–17.

The rest of the paper has the following structure: The mathematical model of the ASV Telemetron is described in Section 4. The high-level planner, mid-level and short-term COLAV algorithms are described in sections 5 to 7, respectively. In Section 8 we present and discuss the simulation scenarios and results, and we conclude the paper in Section 9.

4 ASV modeling

The vessel of interest in this article is the Telemetron ASV, which is owned and operated by the Norwegian company Maritime Robotics and shown in Figure 2. The Telemetron ASV is a high-speed dual-use vessel propelled by a steerable outboard engine, capable of speeds up to $18~{}$\mathrm{m}\text{/}\mathrm{s}$$.

Eriksen and Breivik (2017a) presents a model of the Telemetron ASV, which is extended to include ocean currents in (Bitar et al., 2019a). The model has the form

[TABLE]

where $\boldsymbol{\eta}=[x,y,\psi]^{\top}\in^{2}\times S$ is the vessel pose and $\boldsymbol{V}_{c}=[V_{x},V_{y}]^{\top}$ describes the ocean current, both in the Earth-fixed North-East-Down frame $\{n\}$. The vector $\boldsymbol{x}_{r}=[u_{r},r]^{\top}\in\mathbb{X}_{r}\subset^{2}$ is the vessel velocity under the assumption of zero relative sway motion (Bitar et al., 2019a), where the set $\mathbb{X}_{r}$ describes the vessel-feasible steady-state velocities where (1) is valid. The transformation matrix $\boldsymbol{R}(\psi)$ is given by the heading $\psi\in S$ as

[TABLE]

while $r\in$ describes the vessel yaw-rate. The matrix $\boldsymbol{M}(\boldsymbol{x}_{r})$ is a state-dependent inertia matrix, while $\boldsymbol{\sigma}(\boldsymbol{x}_{r})$ and $\boldsymbol{\tau}=[\tau_{m},\tau_{\delta}]^{\top}\in\mathbb{U}\subset^{2}$ describe the vessel damping and control input, respectively. The set $\mathbb{U}$ describes the control inputs where (1) is valid.

5 High-level planner

To plan the ASV’s nominal trajectory, we use a high-level trajectory planner developed in (Bitar et al., 2019b). This trajectory planner uses the ASV model described in Section 4 to generate an energy-optimized trajectory between the start and goal positions. The planning algorithm combines an A⋆ implementation and an optimal control problem (OCP) solver to generate a feasible and optimized trajectory.

The high-level planning algorithm consists of three steps: First the A⋆ implementation finds the shortest piecewise linear path between the start and goal position. Secondly, artificial temporal information is added to the path, converting it to a trajectory of states and inputs. Finally, the trajectory is used as an initial guess for an OCP solver, which finds a locally energy-optimized trajectory near the shortest path. All steps account for static obstacles in the form of elliptical boundaries.

5.1 Static obstacles

The elliptical boundaries are described with the inequality:

[TABLE]

where $x_{c}$ and $y_{c}$ is the ellipsis center, and $x_{a},y_{a}>0$ are the ellipsis major and minor axes, respectively. To allow for angled obstacles, the ellipses are rotated clockwise by an angle $\alpha$. We add a small constant $\epsilon>0$ to each side of the inequality, and take the logarithm to arrive at the following obstacle representation:

[TABLE]

The logarithm operation is applied to reduce the numerical range of the inequality, which helps with numerical stability of the subsequently described solver, and the constant $\epsilon$ is included to avoid singularities when (4) is evaluated for $(x,y)\to(x_{c},y_{c})$ (Bitar et al., 2019a).

5.2 Trajectory generation and optimization

From a scenario consisting of static obstacles, as mentioned in Section 5.1, we find the piecewise linear shortest path by performing an A⋆ search on a uniformly decomposed grid. The resulting path is converted to a time-parametrized full-state trajectory by assuming a constant forward velocity, and connecting the shortest path with straight segments and circle arcs. The constant forward velocity is

[TABLE]

where $L_{\text{path}}$ is the length of the connected path, and $t_{\text{max}}$ is the maximum allowed time to complete the trajectory. This full-state trajectory is then used as an initial guess to solve the OCP that gives the energy-optimized trajectory:

[TABLE]

The solution of this OCP is a trajectory of states $\boldsymbol{z}(\cdot)$ and inputs $\boldsymbol{\tau}(\cdot)$ that minimizes the cost functional in (6a). The ASV model from Section 4 is rewritten as $\dot{\boldsymbol{z}}=\boldsymbol{f}(\boldsymbol{z},\boldsymbol{\tau})$, where $\boldsymbol{z}=[\boldsymbol{\eta}^{\top},\boldsymbol{x}_{r}^{\top}]^{\top}$ and $\boldsymbol{f}(\boldsymbol{z},\boldsymbol{\tau})$ represents (1).

The cost functional (6a) is chosen to minimize energy. The cost-to-go function is

[TABLE]

with tuning parameters $K_{e},K_{\delta}>0$. The first term consists of a function that is proportional to mechanical work performed by the ASV:

[TABLE]

The function $n:^{+}\to^{+}$ maps the control input $\tau_{m}$ to propeller angular velocity. The function $\delta:\to\operatorname{S}$ maps the control input $\tau_{\delta}$ to outboard motor angle. The second term in (8) is a quadratic cost to yaw control, included to avoid issues with singularity when solving the OCP.

The inequality constraints (6c) observe state boundaries as well as the static obstacles as represented in Section 5.1. The boundary conditions (6d) denote initial and final constraints, i.e. start and end states.

A detailed description of the transcription of the OCP (6) to a nonlinear program (NLP) using multiple shooting with $N_{\text{hi}}$ shooting intervals is found in (Bitar et al., 2019b).

6 Mid-level COLAV

The mid-level algorithm, initially presented in (Eriksen and Breivik, 2017b) and further developed in (Bitar et al., 2019a), is a model predictive control (MPC)-based algorithm intended for long-term COLAV. The algorithm utilizes gradient-based optimization, and takes both static and moving obstacles into account while attempting to follow an energy-optimized nominal trajectory from the high-level planner. The algorithm produces maneuvers complying with Rule 8 of the COLREGs, which requires maneuvers to be made in ample time and be readily observable for other vessels. The optimization problem is formulated as a NLP, which gives flexibility in designing the optimization problem.

In this section, the algorithm is extended to also consider COLREGs rules 13–16 and the first part of Rule 17.

6.1 The International Regulations for Preventing Collisions at Sea (COLREGs)

The COLREGs consists of a total of 37 rules and is divided into five parts (Cockcroft and Lameijer, 2004), where part B (rules 4–19) contains relevant rules on the conduct of vessels in proximity of each other. The most relevant rules for designing COLAV systems in part B are rules 8 and 13–17: {desclist}    [Rule 17]

Action to avoid collision. This rule states that actions taken to avoid collision should be large enough to be readily observable of other ships, implying that series of small alternations in speed and/or course should not be applied. The rule also recommends that course changes should be prioritized over speed changes if there is enough free space available, and that maneuvers must be made in ample time.

Overtaking. This rule states that a vessel is overtaking another if it approaches the other vessel with a course more than $22.5$\mathrm{\SIUnitSymbolDegree}\text{/}$$ abaft her beam. The overtaking vessel has to stay clear of the overtaken vessel, but there is no statements on which side of the vessel one should pass.

Head on. When two power-driven vessels approach each other on reciprocal, or nearly reciprocal, courses, they are in a head-on situation. In such a situation, both vessels should change their course to starboard, passing each other port-to-port, as shown in LABEL:subfig:head_on_colregs. This rule states no explicit definition on what should be considered to be reciprocal, or nearly reciprocal, courses, but court decisions indicate head-on situations exist for opposing courses $\pm 6$\mathrm{\SIUnitSymbolDegree}\text{/}$$. Notice that the rule does not include sailing vessels, which are covered by Rule 12.

Crossing. When two vessels approach each other such that the situation is not a head on or an overtaking, it is a crossing situation. The vessel with the other one to her starboard side is deemed the give-way vessel, while the other vessel is deemed the stand-on vessel. As shown in LABEL:subfig:crossing_colregs, the give-way vessel should maneuver to avoid collision, preferably by passing behind the stand-on vessel, while the stand-on vessel should keep her speed and course.

Action by the give-way vessel. Every vessel which is required to keep out of the way of another vessel should take early and large enough action to safely avoid collision.

Action by the stand-on vessel. This rule requires that a stand-on vessel should keep its current speed and course. The stand-on vessel may, however, maneuver to avoid collision if it becomes apparent that the give-way vessel is not taking appropriate actions to avoid collision. Furthermore, if the stand-on vessel finds itself so close to the obstacle that collision can not be avoided by the give-way vessel alone, the stand-on vessel should take such action which best aids to avoid collision. In a crossing situation, the stand-on vessel should avoid maneuvering to port, since this could lead to a collision if the give-way vessel maneuvers to starboard.

In the hybrid architecture illustrated in Figure 1, the mid-level algorithm is given the task of strictly enforcing COLREGs rules 13–16 and the stand-on requirement of Rule 17, while also complying with Rule 8.

In addition, we want the mid-level algorithm to comply with the first part of Rule 17, by not maneuvering to avoid collision in crossing situations if the ownship is the stand-on vessel. The COLAV system is inherently capable of adhering to the remaining requirement of Rule 17, where the stand-on vessel is allowed or required to maneuver, by having different prediction horizons and safety margins in the mid-level and short-term layers. The BC-MPC algorithm does not have any limitations of not maneuvering in stand-on situations, and will hence maneuver in stand-on situations if we come sufficiently close to the obstacle.

The mid-level algorithm as presented in (Bitar et al., 2019a) only complies with Rule 8. Further in this section, we therefore present improvements to the mid-level algorithm to make it comply with rules 13–16 and the stand-on requirement of Rule 17.

6.2 COLREGs interpretation

A commonly used concept for interpreting obstacles in COLAV algorithms is to use assign a spatial region to obstacles, which the ownship should not enter. This approach is commonly referred to as a domain-based approach. Specially designed ship domains are commonly used for interpreting the COLREGs in COLAV algorithms, where one usually require a larger clearance to obstacles if choosing maneuvers that violate the COLREGs (Eriksen et al., 2019; Szlapczynski and Szlapczynska, 2017). This approach is attractive since it continuously captures multiple COLREGs rules, and does not require logic or discrete decisions. However, such an approach does not strictly enforce the COLREGs rules, since it will allow maneuvers violating the rules if they are large enough. In addition, a ship-domain approach will not be able to strictly enforce the stand-on requirement of Rule 17, since a domain-based approach will avoid collision with all obstacles. One could ignore obstacles with give-way obligations, but this would require an explicit COLREGs interpretation which conflicts with domain-based approaches’ core idea of implicit COLREGs interpretation. Therefore, we pursue an alternative approach to handling the COLREGs in the mid-level algorithm.

To simplify the COLREGs interpretation task, we look at the situation from a static perspective, assuming that the current COLREGs situations are valid throughout the entire prediction horizon of the mid-level algorithm. In reality, the COLREGs situations may, however, change during the prediction horizon depending on both the ownship’s and obstacles future trajectory. For instance, an obstacle approaching from head on, but far enough away to not be considered as a danger may be put in a safe state. Hence, the mid-level algorithm will (for the current iteration) act like no COLREGs rule applies to this vessel for the entire prediction horizon, while the obstacle may get close enough during the prediction horizon to be considered as a head-on situation. An MPC scheme of only implementing a small part of the prediction horizon will reduce the implications of this, since the situation is reassessed each time mid-level algorithm is run, which justifies the assumption of considering the COLREGs from a static perspective. Investigating the possibilities for dynamically predicting future COLREGs situations as part of the MPC prediction will be considered as future work.

6.2.1 State machine

We propose to utilize a state machine in order to decide which COLREGs rule is active with respect to each obstacle in the vicinity of the ownship. The state machine contains the states: {desclist}    [GW]

Safe state. This implies that the COLREGs does not enforce any rule with respect to this obstacle.

Overtaking state. This implies that COLREGs Rule 13 applies with respect to this obstacle. The state machine does not discriminate on whether the ownship is overtaking another vessel or is being overtaken, but this can be done by looking at which vessel has the higher speed (Tam and Bucknall, 2010).

Head-on state. This implies that COLREGs Rule 14 applies with respect to this obstacle.

Give-way state. This implies that COLREGs Rule 15 applies with respect to this obstacle, and the ownship has to give way.

Stand-on state. This implies that COLREGs Rule 15 applies with respect to this obstacle, and the ownship has to stand on.

Emergency state. This implies that the obstacle is so close and/or behaves unpredictably, such that special considerations must be made.

As shown in Figure 4, all transitions have to go either from or to the safe state.

This implies that when the state machine decides that a COLREGs (or emergency) situation exists with respect to an obstacle, it will not allow switching to another state without the situation being considered as safe first. One could argue that it should be able to transition between specific states, like e.g. from head-on, give-way and overtaking to emergency. This is an interesting topic, which should receive attention in the future. To control the transitions between the different states, we combine the time to and distance at the closest point of approach (CPA), a CPA-like measure of the time until a critical point and a geometrical interpretation of the situation.

6.2.2 Entry and exit criteria

CPA is a common concept in maritime risk assessment. Given the current speed and course of the ownship and an obstacle, CPA describes the time to the point where the two vessels are the closest, and the distance to the obstacle at this point. Given the position and velocity vector of the ownship $\boldsymbol{p},\boldsymbol{v}$ and an obstacle $\boldsymbol{p}_{o},\boldsymbol{v}_{o}$, the time to CPA is calculated as (Kufoalor et al., 2018)

[TABLE]

where $\epsilon>0$ is a small constant in order to avoid division by zero in the case where the relative velocity between the ownship and obstacle is zero. Given $t_{\text{CPA}}$, we calculate the distance between the vessels at CPA as

[TABLE]

While the CPA is the point where the distance to an obstacle is at its minimum, the critical point is where the distance to an obstacle crosses underneath a critical distance $d_{\text{crit}}$. This critical distance describes a minimum obstacle distance that the mid-level algorithm is designed for. The time to the critical point $t_{\text{crit}}$ can be calculated by solving the equation

[TABLE]

In the cases where the distance between the ships does not fall below $d_{\text{crit}}$, $t_{\text{crit}}$ is undefined. Otherwise, there are generally two solutions. The interesting solution is the one with the lowest $t_{\text{crit}}$ value, as this is when the obstacle enters the $d_{\text{crit}}$ boundary.

The state-machine entry criteria in Figure 4 are defined as

[TABLE]

where $\overline{d}_{\text{CPA}}^{i,\text{enter}},\underline{t}_{\text{CPA}}^{i,\text{enter}}$ and $\overline{t}_{\text{CPA}}^{i,\text{enter}}$ for $i\in\{\text{SO,OT,GW,HO}\}$ are tuning parameters denoting thresholds on $d_{\text{CPA}}$ and $t_{\text{CPA}}$ in order to satisfy the entry criteria for the stand-on, overtaking, give-way and head-on states. The tuning parameter $\overline{t}_{\text{crit}}^{\text{EM,enter}}$ denotes an upper limit on $t_{\text{crit}}$ in order to enter the emergency state. The idea behind the stand-on, overtaking, give-way and head-on entry criterias are that in order for the obstacle to represent a risk, both $t_{\text{CPA}}$ and $d_{\text{CPA}}$ need to be within some tunable thresholds. Situations with a very low $d_{\text{CPA}}$, but with a high $t_{\text{CPA}}$, will not trigger the entry criteria, since the situations will not occur in the near future. Similarly, if $t_{\text{CPA}}$ is within the thresholds, but $d_{\text{CPA}}$ is large, this indicates a safe passing where risk of collision does not exist. The lower bound on $t_{\text{CPA}}$ will typically be selected as zero, and is useful to distinguish between obstacles moving towards of away from the ownship. For the emergency state, the entry criteria is based on the critical point, at which we are so close that the mid-level algorithm may struggle with providing meaningful maneuvers. In addition to $t_{\text{crit}}$ being under the threshold $\overline{t}_{\text{crit}}^{\text{EM,enter}}$, we require that $t_{\text{CPA}}$ is positive, indicating that we are getting closer to the obstacle. Currently, we only allow entering the emergency state if the situation is a geometrical give-way or head-on, since an overtaking situation represents a smaller danger and has less requirement for special consideration.

The state-machine exit criterias in Figure 4 are defined as

[TABLE]

where $\underline{d}_{\text{CPA}}^{i,\text{exit}},\underline{t}_{\text{CPA}}^{i,\text{exit}}$ and $\overline{t}_{\text{CPA}}^{i,\text{exit}}$ for $i\in\{\text{SO,OT,GW,HO}\}$ are tuning parameters denoting thresholds on $d_{\text{CPA}}$ and $t_{\text{CPA}}$ in order to satisfy the exit criteria for the stand-on, overtaking, give-way and head-on states. The exit criteria for the emergency state is satisfied if $t_{\text{crit}}$ is larger than the tuning parameter $\overline{t}_{\text{exit}}^{\text{EM,enter}}$, or $t_{\text{CPA}}$ is negative, implying that the obstacle is moving further away from the ownship. Note that the exit criterias are obtained by negating the entry criterias, but with other thresholds in order to implement hysteresis to avoid shattering. In general, we allow for different tuning parameters for the different states, but in our simulations we see that selecting the same tuning parameters for all states provides good results. Therefore, we define:

[TABLE]

and

[TABLE]

6.2.3 Geometrical situation interpretation

Tam and Bucknall (2010) present a geometrical interpretation scheme for deciding COLREGs situations based on the relative position, bearing and course of the obstacle with respect to the ownship. We base our geometrical interpretation on a slightly modified version of this scheme, where we include the sign of $t_{\text{CPA}}$ to distinguish between situations where the obstacle moves closer towards or farther away from the ownship. The geometrical interpretation is shown in Figure 5, where the geometrical situation is obtained by finding which region the obstacle position and course resides in.

Notice that the head-on region is larger than the threshold of $\pm 6$\mathrm{\SIUnitSymbolDegree}\text{/} as described by the COLREGs. The reason for this is that [Tam and Bucknall](#bib.bib24) recommend using a larger region of $22.5$\mathrm{\SIUnitSymbolDegree}\text{/} in order to increase the robustness of the geometrical COLREGs interpretation scheme.

6.3 Interface to the high-level planner

The high-level planner produces an energy-optimized nominal trajectory for the ownship to follow. However, since the high-level planner does not consider moving obstacles, the speed is the only time-relevant factor of the desired trajectory. In a case where the ownship for some reason, e.g. avoiding moving obstacles, lag behind the nominal trajectory, following the nominal trajectory in absolute time would cause a speed increase in order to catch up with it. Therefore, the mid-level algorithm performs relative trajectory tracking, where it tracks the nominal trajectory with a time offset $t_{b}\in$. This results in a relative nominal trajectory for the mid-level algorithm:

[TABLE]

where $\boldsymbol{p}_{d}=[N_{d}(t),E_{d}(t)]^{\top}$ is the nominal trajectory from the high-level planner. The time offset $t_{b}$ is calculated each time the mid-level algorithm is run by solving a separate optimization problem, and is selected such that $\bar{\boldsymbol{p}}_{d}(t_{0})$ is the point on the nominal trajectory closest to the ownship. See (Bitar et al., 2019a) for a detailed description of this concept.

6.4 Optimization problem formulation

The mid-level algorithm is formalized as an OCP:

[TABLE]

where $T_{h}>0$ is the prediction horizon, $\phi(\cdot,\cdot)$ is the objective functional, (17b) contains a kinematic vessel model, (17c) contains inequality constraints and (17d) contains boundary constraints.

Analytical solutions of OCPs are in general not possible to find. A more common approach is to transcribe the OCP to an NLP, and solve that using a gradient optimization scheme. In our case, we transcribe (17) into an NLP with $N_{p}$ samples using multiple shooting, where the vessel model is discretized using 4th order Runge Kutta and the cost functional is discretized using forward Euler. The resulting NLP is given as

[TABLE]

where $\boldsymbol{w}=[\boldsymbol{\eta}^{\top}_{0},\boldsymbol{x}^{\top}_{r,0},\ldots,\boldsymbol{\eta}^{\top}_{{N_{p}}-1},\boldsymbol{x}^{\top}_{r,{N_{p}}-1},\boldsymbol{\eta}^{\top}_{N_{p}}]^{\top}\in^{5N_{p}+3}$ is a vector of ${5N_{p}+3}$ decision variables and $\bar{\boldsymbol{p}}_{d,{1:N_{p}}}=[\bar{\boldsymbol{p}}_{d,1},\bar{\boldsymbol{p}}_{d,2},\ldots,\bar{\boldsymbol{p}}_{d,N_{p}}]$ is a sequence of desired positions. The vectors $\boldsymbol{\omega}\in^{2N_{p}}$, $\boldsymbol{\mu}\in^{2N_{p}}$ and $\boldsymbol{\xi}\in^{MN_{p}}$ contain slack variables, where $M$ is the number of moving obstacles to be included in the constraints.

The vector $\boldsymbol{g}(\boldsymbol{w},\boldsymbol{\eta}(t_{0}))\in^{3N_{p}+3}$ contains shooting and boundary constraints, while $\boldsymbol{h}(\boldsymbol{w})\in^{(M+D+4)N_{p}}$, where $D$ is the number of static obstacles, contain inequality constraints ensuring COLAV and steady-state vessel velocity feasibility. The vectors $\bar{\boldsymbol{h}}_{k}(\boldsymbol{\eta}_{k},\boldsymbol{\omega}_{k},\boldsymbol{\mu}_{k},\bar{\boldsymbol{p}}_{d,k})\in^{6}$, $k\in\{1,N_{p}\}$ contain constraints on the slack variables $\boldsymbol{\omega}$ and $\boldsymbol{\mu}$.

In the following subsections, we describe the terms in (18) in more detail.

6.4.1 Objective function

The objective function contains four functions, where $\phi_{p}(\boldsymbol{w},\boldsymbol{\omega},\boldsymbol{\mu})$ introduces cost on deviating from the relative nominal trajectory $\bar{\boldsymbol{p}}_{d}(t)$, $\phi_{c}(\boldsymbol{w})$ introduces cost on using control input, $\phi_{\text{COLREGs}}(\boldsymbol{w})$ is a COLREGs-specific function and $\phi_{\xi}(\boldsymbol{\xi})$ introduces slack variable cost.

To avoid that the NLP changes behavior when moving away from the nominal trajectory, we wish to have linear growth in the position error function $\phi_{p}(\boldsymbol{w},\boldsymbol{\omega},\boldsymbol{\mu})$. This is achieved by instead of using quadratic terms in the position error function, we use the Huber loss function which is quadratic around the origin and resembles the absolute value function above a given threshold $\sigma>0$:

[TABLE]

The Huber loss function has a discontinuous gradient, making it slightly complicated to implement in gradient-based optimization problems. It can, however, be implemented in a continuous fashion by utilizing lifting, where slack variables are introduced to create a problem of a higher dimensionality which is easier to solve. Using this technique, $\bar{\phi}_{p}(\boldsymbol{w},\boldsymbol{\omega},\boldsymbol{\mu})$ is defined as

[TABLE]

where $K_{p}>0$ is a tuning parameter, and $\boldsymbol{\omega}_{k}\in^{2}$ and $\boldsymbol{\mu}_{k}\in^{2}$ are slack variables constrained by

[TABLE]

where $\boldsymbol{p}_{k}$ is the predicted vessel position at time step $k$, i.e. $\boldsymbol{\eta}_{k}=[\boldsymbol{p}_{k}^{\top},\psi_{k}]^{\top}$. See (Bitar et al., 2019a) for more details.

Rule 8 of the COLREGs requires that maneuvers are readily observable for other vessels, implying that speed and course changes should have a sufficiently large magnitude, and not be performed as a sequence of small changes. In order to enforce this in the optimization problem, the control cost function $\phi_{c}(\boldsymbol{w})$ introduces a nonlinear cost on the change in speed and course, which makes the algorithm favor readily observable maneuvers. The function is defined as

[TABLE]

where $K_{\dot{U}},K_{\dot{\chi}}>0$ are tuning parameters, while $q_{\dot{U}}(\dot{U}_{k})$ and $q_{\dot{\chi}}(\dot{\chi}_{k})$ are the nonlinear cost functions. Notice that neither the speed over ground (SOG) $U$ nor the course $\chi$ are elements of the search space, but they can be computed as $U=\sqrt{u^{2}+v^{2}}$ and $\chi=\psi+\arcsin\frac{v}{U}$. Their derivatives are then calculated by finite differencing. See (Eriksen and Breivik, 2017a; Bitar et al., 2019a) for more details on the control cost function.

The $\phi_{\text{COLREGs}}(\boldsymbol{w})$ function introduces a COLREGs-specific cost with respect to obstacles based on the rule currently applicable as defined by the state machine. We hence tailor the NLP to the current situation. The function is defined as

[TABLE]

where $\mathcal{O}_{\text{HO}},\mathcal{O}_{\text{GW}},\mathcal{O}_{\text{SO}}$ and $\mathcal{O}_{\text{EM}}$ contain obstacles which are in the head-on, give-way, stand-on and emergency states, respectively, and $K_{\text{HO}},K_{\text{GW}},K_{\text{SO}},K_{\text{EM}}>0$ are tuning parameters. The functions $V_{\text{HO},i,k}(\boldsymbol{p}_{k}),V_{\text{GW},i,k}(\boldsymbol{p}_{k}),V_{\text{SO},k}(\boldsymbol{w})$ and $V_{\text{EM},k}(\boldsymbol{w})$ describe functions capturing head-on, give-way, stand-on and emergency behavior with respect to obstacle $i$, respectively. Notice that the head-on and give-way functions vary with both the obstacle number and time step number, which is due to the functions depending on the the given obstacles position and course at time step $k$.

For head-on situations, we define a potential function with a positive value on the obstacle’s starboard side, and a negative value on its port side. When used in the objective function, this will favor trajectories passing a head-on obstacle on its port side, in compliance with Rule 14 of the COLREGs. In addition, the potential function has an attenuation term, reducing the impact of the function when far away from an obstacle:

[TABLE]

where $\alpha_{x,\text{HO}},\alpha_{y,\text{HO}}>0$ are tuning parameters controlling the steepness of the head-on potential function and $\bar{x}_{0,\text{HO}}>0$ is a tuning parameter controlling the influence of the attenuating potential. The coordinate $(x^{\{i,k\}},y^{\{i,k\}})$ is $\boldsymbol{p}$ given in obstacles i’s course-fixed frame (in which the x-axis points along the obstacle’s course) at time step $k$, computed as

[TABLE]

where $\boldsymbol{p}_{o,k,i}$ and $\psi_{i,k}$ are the position and heading of obstacle $i$ at time step $k$. The head-on potential function with parameters $\alpha_{x,\text{HO}}=\nicefrac{{1}}{{500}},\alpha_{y,\text{HO}}=\nicefrac{{1}}{{400}}$ and $x_{0,\text{HO}}=1000~{}$\mathrm{m}\text{/}$$ is shown in LABEL:subfig:head_on_potential.

For give-way situations, we define a similar potential function, but rotated such that the function is positive in front of an obstacle and negative behind it. This will favor trajectories passing behind an obstacle, as desirable with respect to Rule 15 when a give-way obligation is active. The give-way potential function is defined as

[TABLE]

where $\alpha_{x,\text{GW}},\alpha_{y,\text{GW}}>0$ control the steepness of the give-way potential function and $\bar{y}_{0,\text{GW}}<0$ control the attenuation on the port side of an obstacle. The give-way potential function with parameters $\alpha_{x,\text{GW}}=\nicefrac{{1}}{{400}},\alpha_{y,\text{GW}}=\nicefrac{{1}}{{500}}$ and $y_{0,\text{GW}}=-500~{}$\mathrm{m}\text{/}$$ is shown in LABEL:subfig:give_way_potential.

In stand-on situations, we want the mid-level algorithm to disregard the obstacle and keep the current speed and course in order to comply with the first part of Rule 17. One could simply constrain the algorithm to not maneuver, but this would be perilous in situations where the ownship simultaneously finds itself in a head-on or give-way situation. In such a situation it would be of extra importance to choose readily observable maneuvers, and we therefore design the stand-on cost with the same terms as used in the control cost (22) to amplify the effect:

[TABLE]

If an obstacle is in an emergency state, the obstacle is disregarded in the mid-level algorithm and left for the short-term algorithm to handle. In such a situation, it is important that the mid-level algorithm behaves predictable, and we therefore use the same cost function as for stand-on situations:

[TABLE]

The slack variable $\boldsymbol{\xi}$ is used in a homotopy scheme, which we introduce to avoid getting trapped in local minima around moving obstacles. The homotopy scheme is described in further detail in Section 6.5. The homotopy cost function $\phi_{\xi}(\boldsymbol{\xi})$ introduces slack cost on $\boldsymbol{\xi}$:

[TABLE]

where $K_{\xi}>0$ is iteratively increased as part of the homotopy scheme.

6.5 Obstacle handling and steady-state feasibility

The inequality constraint $\boldsymbol{h}(\boldsymbol{w},\boldsymbol{\xi})\leq\boldsymbol{0}$ ensures COLAV and steady-state feasibility with respect to actuator limitations.

Static obstacles are handled similarly as in the high-level algorithm, with (4) representing an elliptical obstacle with center $(x_{c},y_{c})$, angle $\alpha$ and major and minor axes $x_{a}$ and $y_{a}$, respectively. The constraint (4) needs to be enforced at each time step. Hence, for the $i$-th static obstacle, we define the constraint

[TABLE]

Moving obstacles are handled in a similar fashion, but letting the ellipsis center position and angle be time varying. Obstacles in stand-on situations should, however, not be included in the constraints, since the mid-level algorithm is supposed to stand on in such situations. Moreover, if an obstacle has entered an emergency state, the obstacle is so close and behaving unpredictably that the mid-level algorithm should disregard it and leave it for the short-term layer. Hence, for the $i$-th moving obstacle not in a stand-on or an emergency situation, we define the constraint

[TABLE]

where $x_{c,i,k},y_{c,i,k}$ and $\alpha_{i,k}$ denote the position and course of the $i$-th moving obstacle at time step $k$.

Given $D$ static obstacles and $M$ obstacles not in stand-on or emergency situations, we define the constraint

[TABLE]

where we include slack variables $\boldsymbol{\xi}\geq\boldsymbol{0}$ on the moving obstacle constraints as part of a homotopy scheme. The reason for using homotopy is that NLP solvers in general only finds local minima, and can have issues with moving an initial guess “through” obstacles. Normally, this is not an issue, but for the mid-level algorithm the optimal solution can change drastically from one iteration to another. This can for instance happen if an obstacle enters a head-on or give-way state, where the solution can be trapped on the wrong side of an obstacle. In general, homotopy describes introducing an extra parameter which is iteratively adjusted in order to iteratively move a local solution towards a global solution (Deuflhard, 2011). In our homotopy scheme, we introduce slack variables on the moving obstacle constraints, which will allow solutions to travel through obstacles at the cost of a homotopy cost (29) scaled by the homotopy parameter $K_{\xi}$. Initially, this is selected as a low value to have a high amount of slack on the moving obstacles, while it is iteratively increased towards $K_{\xi}\to\infty$, which results in $\boldsymbol{\xi}=\boldsymbol{0}$ and hence no slack on moving obstacles. Currently, we only introduce slack on moving obstacles, but slack should also be introduced to static obstacles if they are small enough for the algorithm to be able to pass on both sides, like e.g. rocks, navigational marks, etc.

Similarly as in (Eriksen and Breivik, 2017b; Bitar et al., 2019a), we ensure steady-state feasible trajectories at each time step through a constraint $\boldsymbol{h}_{\boldsymbol{x}_{r,k}}(\boldsymbol{x}_{r,k})\leq\boldsymbol{0}\in^{4}$, which captures the state constraint $\boldsymbol{x}_{r}\in\mathcal{X}_{r}$ at time step $k$. To ensure stead-state feasibility for the entire prediction horizon, we define the constraint

[TABLE]

Finally, the inequality constraints are combined as

[TABLE]

7 Short-term COLAV

For the short-term layer, the branching-course model predictive control (BC-MPC)algorithm is used, which is a sample-based MPC algorithm intended for short-term ASV COLAV. The BC-MPC algorithm was initially developed in (Eriksen et al., 2019), extended to also consider static obstacles in (Eriksen and Breivik, 2019) and is experimentally validated in several full-scale experiments using a radar-based system for detecting and tracking obstacles. The algorithm complies with COLREGs rules 8, 13 and the second part of Rule 17, while favoring maneuvers complying with the maneuvering aspects of rules 14 and 15. Notice that Rule 17 allows a ship to ignore the maneuvering aspects of rules 14 and 15 in situations where the give-way vessel does not maneuver. The obstacle clearance will be larger if the algorithm ignores the maneuvering aspects of rules 14 and 15, like e.g. passing in front of an obstacle in a crossing situation where the ownship is the give-way vessel. Moving obstacles are in general handled by the mid-level algorithm, making this applicable only in emergency situations and for obstacles detected so late that the mid-level algorithm is unable to avoid them.

The algorithm constructs a search space consisting of a finite number of trajectories, which each contain a sequence of maneuvers. The maneuvers are constructed using a dynamic model of the ownship and a set of acceleration motion primitives, resulting in feasible trajectories being specified to the vessel controller. For each maneuver, a discrete set of SOG and course accelerations are created as

[TABLE]

where $\dot{U}_{i},i\in[1,N_{U}]$ and $\ddot{\chi}_{i},i\in[1,N_{\chi}]$ denote $N_{U}\in{\rm Nature}$ and $N_{\chi}\in{\rm Nature}$ vessel-feasible speed and course accelerations. Given the acceleration samples (35) and motion primitives for each maneuver in a trajectory, we create a set of desired SOG and course trajectories $\mathcal{U}_{d}$. These trajectories have continuous acceleration, and is designed in an open-loop fashion by using the current reference tracked by the vessel controller for initialization, rather than the current vessel SOG and course. The reason for this is that the reference to the vessel controller should be continuous in order to avoid jumps in the actuator commands. To include feedback in the trajectory prediction, a set of feedback-corrected SOG and course trajectories $\bar{\mathcal{U}}_{d}$ is predicted using a simplified error model of the vessel and vessel controller. Finally, the feedback-corrected SOG and course trajectories are used to compute a set of feedback-corrected pose trajectories:

[TABLE]

where $\bar{\boldsymbol{\eta}}(\cdot)$ denotes a kinematic simulation procedure that given SOG and course trajectories, $\bar{U}(\cdot)$ and $\bar{\chi}(\cdot)$, in $\bar{\mathcal{U}}_{d}$ computes the vessel pose. See (Eriksen et al., 2019; Eriksen and Breivik, 2019) for more details on the trajectory generation procedure.

In order to converge towards the trajectory specified by the mid-level algorithm, a desired acceleration is computed based on a line-of-sight guidance scheme. In (Eriksen et al., 2019) and (Eriksen and Breivik, 2019), the samples closest to the desired acceleration in (35) are replaced with the desired acceleration, given that this is vessel-feasible. A problem with this, is that when operating at high speeds, the possible acceleration may not be symmetric, resulting in that zero acceleration (hence keeping a constant speed and course), may not be part of the search space. This can cause undesirable behavior, since the BC-MPC algorithm will be unable to keep the speed and course constant, which can cause oscillatory behavior. In this paper, we therefore propose to move the acceleration samples closest to zero, and adding the desired acceleration as a separate sample, given that it is vessel feasible. This will make sure that keeping a constant speed and course, as well as a trajectory converging towards the desired trajectory is included in the search space.

Given the predicted trajectories, the algorithm finds the optimal desired SOG and course trajectory for the vessel controller $\boldsymbol{u}_{d}^{*}(\cdot)=[U_{d}(\cdot)^{*},\chi_{d}(\cdot)^{*}]$ as

[TABLE]

where the objective function is given as

[TABLE]

The variables $w_{\text{al}},w_{\text{av,m}},w_{\text{av,s}},w_{\text{t},U},w_{\text{t},\chi}>0$ are tuning parameters, while $\text{align}(\bar{\boldsymbol{\eta}}(\cdot);\boldsymbol{p}_{d}^{\text{mid}}(\cdot))$ measures the alignment between a candidate trajectory $\bar{\boldsymbol{\eta}}(\cdot)$ and the desired trajectory from the mid-level algorithm $\boldsymbol{p}_{d}^{\text{mid}}(\cdot)$. The function $\text{avoid}_{\text{m}}(\bar{\boldsymbol{\eta}}(\cdot))$ ensures COLAV of moving obstacles by penalizing trajectories close to obstacles, using a non-symmetric obstacle ship domain designed with the COLREGs in mind. The function $\text{avoid}_{\text{s}}(\bar{\boldsymbol{\eta}}(\cdot))$ ensures COLAV of static obstacles by introducing an occupancy grid, while $\text{tran}_{U}(\boldsymbol{u}_{d}(\cdot))$ and $\text{tran}_{\chi}(\boldsymbol{u}_{d}(\cdot))$ introduces transitional costs to avoid shattering. The transitional terms penalize deviations from the planned trajectory of the previous iteration, unless changing to the trajectory corresponding by the desired acceleration. See (Eriksen et al., 2019) and (Eriksen and Breivik, 2019) for more details and descriptions of the terms.

8 Simulation results

The hybrid COLAV system is verified through simulations, which are present in this section. The simulations include ocean current and both static and moving obstacles. We include moving obstacles both acting in compliance with the COLREGs, and violating the COLREGs.

The simulations are performed on a computer with an $2.8$ GHz Intel Core i7 processor, running macOS Mojave.

8.1 Simulation setup

The simulations are performed in MATLAB using CasADi (Andersson et al., 2019) and IPOPT (Wächter and Biegler, 2005) for implementing the high-level and mid-level algorithms. The simulator is built upon the mathematical model of the Telemetron ASV described in Section 4, and the model-based speed and course controller in (Eriksen and Breivik, 2018) is used as the vessel controller.

The parameters of the high-level algorithm are listed in Table 1.

The mid-level algorithm is implemented using the parameters in Table 2.

The slack variable cost $K_{\xi}$ has five elements, implying that we use five steps in our homotopy scheme. The mid-level NLP is initially warm started with the solution from the previous iteration, while each step in the homotopy scheme is warm started with the solution from the previous step of the homotopy scheme, converging towards the solution without slack on the constraints. To reduce the computational load and increase the predictability of the mid-level algorithm, we utilize six steps of each planned mid-level trajectory, only running the mid-level algorithm every $60~{}$\mathrm{s}\text{/}. This implies that six steps of the predicted solution will be implemented before computing a new solution, which further implies that the state machine is also only run every $60~{}$\mathrm{s}\text{/}. If the mid-level algorithm fails in finding a feasible solution, the algorithm will re-use the solution from the last iteration. This may for instance happen if the algorithm tries to compute a solution while being inside a moving obstacle ellipse, which sometimes can be the case when an obstacle is exiting an emergency or stand-on state. The BC-MPC algorithm is run every $5~{}$\mathrm{s}\text{/}, with parameters as described in (Eriksen and Breivik, [2019](#bib.bib14)). Static obstacles are padded with a safety margin of $150~{}$\mathrm{m}\text{/} for the high-level and mid-level algorithms, while the BC-MPC algorithm uses a safety margin of $100~{}$\mathrm{m}\text{/}$$ for static obstacles. The reason for having a smaller static obstacle safety margin for the BC-MPC algorithm is that it tends to struggle with following trajectories on the static obstacle boundaries. The BC-MPC algorithm would hence not be able to follow the nominal trajectory if the static obstacle safety margin was the same as for the mid-level and high-level algorithms.

The simulations are performed without any noise on the obstacle estimates, providing the algorithms with exact information about the obstacles position, course and speed. The BC-MPC algorithm has previously been shown to perform well with noisy and uncertain obstacle estimates in full-scale experiments using radar-based detection and tracking of obstacles (Eriksen et al., 2019; Eriksen and Breivik, 2019). The mid-level algorithm is likely to have a larger requirement to low noise levels on the obstacle estimates, since the state machine in the mid-level algorithm depends on logic and discrete switching. However, the algorithm is also run less frequently, reducing the required bandwidth of the obstacle estimates, possibly allowing using smoothing or tracking filters with a lower process noise if necessary. It may also be feasible to make the mid-level algorithm depend on data from the automatic identification system, which typically have much lower noise levels than radar-based tracking systems, while being subject to robustness issues (Harati-Mokhtari et al., 2007).

We present three scenarios, which demonstrate different important properties of the hybrid COLAV system: {desclist}    [Scenario 2]

This scenario contains two static obstacles, and four moving obstacles of which all comply with the COLREGs. The moving obstacles demonstrate stand-on, give-way and head-on situations.

This scenario contains one static and five moving obstacles. The moving obstacles demonstrate stand-on with an obstacle ignoring the COLREGs, an overtaking and a simultaneous head-on, give-way and stand-on situation with obstacles complying with the COLREGs.

This scenario contains two moving obstacles, which suddenly perform dangerous maneuvers close to the ownship, displaying the use of the emergency state.

8.2 Scenario 1

Scenario 1 contains two static obstacles, four moving obstacles, an ocean current of $[-2,0]^{\top}~{}$\mathrm{m}\text{/}\mathrm{s} and is shown in [Figure 7](#S8.F7). The high-level planner plans a nominal trajectory between the initial and goal positions at $[7000,200]^{\top}~{}$\mathrm{m}\text{/} and $[0,7900]^{\top}~{}$\mathrm{m}\text{/}$$, respectively. The first obstacle is in a stand-on situation, where it is required to maneuver in order to avoid collision with the ownship, which is required to stand on. As shown in LABEL:subfig:scenario_1_COLREGs, the first obstacle is quickly considered as a stand-on situation, at which the mid-level algorithm disregards the obstacle and continues with the current speed and course. Following this, the obstacle maneuvers in accordance to the COLREGs, and we avoid collision. After the first static obstacle, we encounter two crossing vessels where the ownship is deemed the give-way vessel. In accordance with the COLREGs, we maneuver to starboard in order to pass behind both obstacles. Notice that the second give-way obstacle is detected as a give-way situation later than the first, since the entry criteria in the state machine includes the time to CPA, which is higher for the second give-way obstacle. After avoiding the two give-way obstacles, we converge towards the nominal trajectory and encounter a head-on situation. This is correctly identified by the state machine as head on, and we maneuver to starboard in order to avoid collision. Notice that even though the obstacle maneuvers, we keep the obstacle in the head-on state until we have passed it.

Figure 8 shows the speed and angular trajectories during Scenario 1, where the desired speed is calculated as the nominal speed at the closest point on the nominal trajectory given the ownship position. From this, we see that the mid-level and BC-MPC algorithms manage to track the desired nominal speed before and after the first static obstacle, where no obstacles require maneuvering away from the nominal trajectory. Notice that when encountering the two crossing obstacles, the mid-level algorithm chooses to slowly change the course, which is due to the attenuation of the give-way potential function and the large distance between the vessels. It would be better to make a clear course change, which is a subject of tuning. After passing the two crossing obstacles, the mid-level algorithm increases the speed in order to get back to the nominal trajectory, which is due to the algorithm attempting to keep the speed projected on the nominal trajectory equal as the desired nominal speed. Furthermore, notice that the mid-level algorithm actively controls the relative surge speed in order achieve the desired SOG, which is clearly seen when passing the first static obstacle.

8.3 Scenario 2

Scenario 2, shown in Figure 9, is more complex than Scenario 1, with a total of five moving obstacles, and has an ocean current of $[-1,1]^{\top}~{}$\mathrm{m}\text{/}\mathrm{s}. The high-level planner plans a nominal trajectory between the initial and goal positions at $[200,200]^{\top}~{}$\mathrm{m}\text{/} and $[5500,7000]^{\top}~{}$\mathrm{m}\text{/}$$, respectively. The first obstacle is a crossing vessel, which similarly as in Scenario 1 is deemed to give way for the ownship, which should keep the current speed and course. However, in this scenario, the obstacle violates the COLREGs by not maneuvering in order to avoid collision. Therefore, the BC-MPC algorithm maneuvers to avoid collision when the obstacle gets so close that the safety margins of the BC-MPC algorithms is violated. The BC-MPC algorithm maneuvers to port, as advised by COLREGs Rule 17 for crossing situations where the stand-on vessel has to maneuver, and safely avoid the first obstacle. The second obstacle is overtaken by the ownship, and correctly considered as an overtaking situation by the state machine. For such an situation, there is no requirement on how the ownship should maneuver, except keeping clear from the overtaken vessel. After passing the second obstacle, we encounter a complex situation with simultaneous head-on, give-way and stand-on obligations. In this situation, each vessel, including the ownship, finds itself in a situation where a head-on and a give-way situation require starboard maneuvers, while a stand-on situation requires the vessel to keep the current speed and course. However, head-on and give-way obligations should be prioritized higher than stand-on situations, and the situation is quite easily solved by each vessel maneuvering to starboard and passing behind the vessel crossing from starboard. The mid-level algorithm solves this situation with the desirable behavior, and converges towards the nominal trajectory after the situation is resolved. As shown in LABEL:subfig:scenario_2_COLREGs, the state machine interprets the situations correctly.

From the speed trajectory in Figure 10 it is clear that the mid-level algorithm follows the desired nominal speed also when overtaking the second obstacle.

8.4 Scenario 3

Scenario 3, shown in Figure 11, contains two moving obstacles on parallel courses with the ownship, and has an ocean current of $[-1,1]^{\top}~{}$\mathrm{m}\text{/}\mathrm{s}. The high-level planner plans a nominal trajectory between the initial and goal positions at $[500,500]^{\top}~{}$\mathrm{m}\text{/} and $[3328,5399]^{\top}~{}$\mathrm{m}\text{/}, respectively, which results in a straight line trajectory with a course angle of $60$\mathrm{\SIUnitSymbolDegree}\text{/}. The first obstacle travels at a higher speed than the ownship, while the second one travels at a lower speed and will be overtaken by the ownship. Since the obstacles are on parallel paths with the obstacle, the time to CPA is sufficiently high such that the obstacles are in the safe state, even though the the vessels are quite close. However, both obstacles make sudden maneuvers to port dangerously close to the ownship and enters on a crossing course with the ownship. With respect to the COLREGs, the ownship is required to give way to both obstacles since they are crossing from the ownship’s starboard side. One can, however, argue that the maneuvers displayed by the obstacles are dangerous and displays poor seamanship, such that the ownship should not be held accountable if a collision occurred. Nevertheless, the hybrid COLAV system manages to avoid both obstacles. As seen in LABEL:subfig:scenario_3_COLREGs, the first obstacle is sufficiently far away from the ownship to be considered as a give-way situation when the state machine interprets the situation, and the mid-level algorithm plans a trajectory passing behind the first obstacle. The second obstacle maneuvers to port even closer to the ownship, resulting in the distance to the critical point being within the threshold for entering the emergency situation when the state machine interprets the situation. In this situation, the mid-level algorithm disregards the obstacle and leaves it to the BC-MPC algorithm to avoid collision.

As seen in Figure 12, the mid-level algorithm both reduces the speed and changes the course to avoid the first obstacle. When approaching the second obstacle, the BC-MPC algorithm initiates a speed reduction, and after some time also maneuver to starboard in order to pass behind the obstacle and resolve the situation.

8.5 Simulation summary

The simulation results show that the hybrid COLAV system is able to handle a wide range of situations, while also behaving in an energy-optimal way when moving obstacles are not interfering with the ownship trajectory. Section 8.5 shows the minimum distance to static and moving obstacles for the scenarios.