Hybrid Collision Avoidance for ASVs Compliant With COLREGs Rules 8 and 13–17

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.


INTRODUCTION
Motivated by the potential for reduced costs and increased safety, the maritime industry is rapidly moving toward 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,000 yearly truck journeys in urban eastern Norway 1 . 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 Finland 2 . Reports state that in excess of 75 % 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 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 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:

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

Rules 13-15
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.

Rule 16
Requires that a give-way vessel must take early and substantial action to keep clear of the standon vessel.

Rule 17
Consists of two main parts. The first part requires a stand-on vessel to maintain its course and speed, while the second part allows/requires 3 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 FIGURE 1 | Hybrid architecture with three COLAV layers, where the highlighted functions mark the areas of interest in this article. The COLAV system consists of a high-level planner, a mid-level COLAV algorithm and a short-term COLAV algorithm. The COLAV system is supported by data from electronic nautical charts, represented in a suitable manner for the algorithms, as well as situational awareness functions that track and predict obstacles, interpret the COLREGs and perform risk assessment.
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 samplebased 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 gradientbased algorithms, which are able to solve problems with large search spaces more efficiently than sample-based algorithms. The works by Loe (2008) and Švec et al. (2013) 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(Bitar et al., , 2019b, a mid-level algorithm (Eriksen and Breivik, 2017b;Bitar et al., 2019a), short-term algorithms  and the development of highperformance vessel controllers 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 Breivik (2017b, 2019), Bitar et al. (2019a,b), . The high-level planner has a long temporal horizon, and finds an energy-optimized nominal trajectory from an initial to a goal position. It considers static obstacles, which may include bathymetric constraints. 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 the nominal trajectory, while performing COLAV of static and moving obstacles in compliance with COLREGs rules 8, 13-16, and the first part of Rule 17. The midlevel 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 2. The high-level planner, mid-level and shortterm COLAV algorithms are described in sections 3-5, respectively. In section 6 we present and discuss the simulation scenarios and results, and we conclude the paper in section 7.

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 highspeed dual-use vessel propelled by a steerable outboard engine, capable of speeds up to 18 m/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 forṁ where η = [x, y, ψ] ⊤ ∈ R 2 × S is the vessel pose and V c = [V x , V y ] ⊤ describes the ocean current, both in the Earth-fixed North-East-Down frame {n}. The vector x r = [u r , r] ⊤ ∈ X r ⊂ R 2 is the vessel velocity under the assumption of zero relative sway motion (Bitar et al., 2019a), where the set X r describes the vessel-feasible steady-state velocities where (1) is valid. The transformation matrix R(ψ) is given by the heading ψ ∈ S as while r ∈ R describes the vessel yaw-rate. The matrix M(x r ) is a state-dependent inertia matrix, while σ (x r ) and τ = [τ m , τ δ ] ⊤ ∈ U ⊂ R 2 describe the vessel damping and control input, respectively. The set U describes the control inputs where (1) is valid.
In this work, we assume that the ocean current V c is constant and known. For practical applications, the ocean current can be measured via appropriate instrumentation, estimated via sensor fusion methods, or predicted based on e.g., tide tables or sensor networks, such as the European marine observation and data network 4 .

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 2 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.

Static Obstacles
The elliptical boundaries are described with the inequality: 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 α. We add a small constant ǫ > 0 to each side of the inequality, and take the logarithm to arrive at the following obstacle representation: 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 ǫ is included to avoid singularities when (4) is evaluated for (x, y) → (x c , y c ) (Bitar et al., 2019a). Modeling static obstacles as ellipses poses a challenge for handling obstacles of various shapes, from e.g., electronic nautical charts (ENCs). Generic obstacle shapes can be approximated as a set of elliptical obstacles (Wu, 2019), although this may require a large number of constraints for complex environments. Alternatively, the obstacle modeling can be modified to allow for generic shapes. Zhang et al. (2018) present an interesting solution to handle polygon-shaped obstacles by introducing a signed distance function in the optimization problem. Unfortunately, this approach introduces a large number of slack variables and constraints, limiting feasibility for more than a few static obstacles.

Trajectory Generation and Optimization
From a scenario consisting of static obstacles, as mentioned in section 3.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-parameterized fullstate trajectory by assuming a constant forward velocity, and connecting the shortest path with straight segments and circle arcs. The constant forward velocity is where L path is the length of the connected path, and t 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: subject tȯ The solution of this OCP is a trajectory of states z(·) and inputs τ (·) that minimizes the cost functional in (6a). The ASV model from section 2 is rewritten asż The cost functional (6a) is chosen to minimize energy. The cost-to-go function is with tuning parameters K e , K δ > 0. The first term consists of a function that is proportional to mechanical work performed by the ASV: ·r| .
(8) The function n : R + → R + maps the control input τ m to propeller angular velocity. The function δ : R → S maps the control input τ δ 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 3.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 non-linear program (NLP) using multiple shooting with N hi shooting intervals is found in Bitar et al. (2019b).

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.

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: Rule 8 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.

Rule 13
Overtaking. This rule states that a vessel is overtaking another if it approaches the other vessel with a course more than 22.5°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.
Rule 14 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 Figure 3A. 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 ±6°. Notice that the rule does not include sailing vessels, which are covered by Rule 12.
Rule 15 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 Figure 3B, 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. Rule 16 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.

Rule 17 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 giveway 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 standon vessel should take such action which best aids to avoid collision. In a crossing situation, the standon 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 hybrid 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.

COLREGs Interpretation
A commonly used concept for interpreting obstacles in COLAV algorithms is to 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 the required clearance to an obstacle is significantly larger if the maneuver violates the COLREGs (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 headon 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 predicted future COLREGs situations as part of the MPC prediction will be considered as future work.

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: SF Safe state. This implies that the COLREGs do not enforce any rule with respect to this obstacle.
OT 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).
HO Head-on state. This implies that COLREGs Rule 14 applies with respect to this obstacle.
GW Give-way state. This implies that COLREGs Rule 15 applies with respect to this obstacle, and the ownship has to give way.
SO Stand-on state. This implies that COLREGs Rule 15 applies with respect to this obstacle, and the ownship has to stand on.
EM Emergency state. This implies that the obstacle is so close and/or behaves unpredictably, such that special considerations must be made. 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 CPA, a CPA-like measure of the time until a critical point and a geometrical interpretation of the situation.

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 p, v and an obstacle p o , v o , the time to CPA is calculated as where ǫ > 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 CPA , we calculate the distance between the vessels at CPA as 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 crit . This critical distance describes a minimum obstacle distance that the midlevel algorithm is designed for. The time to the critical point t crit can be calculated by solving the equation In the cases where the distance between the ships does not fall below d crit , t crit is undefined. Otherwise, there are generally two solutions. The interesting solution is the one with the lowest t crit value, as this is when the obstacle enters the d crit boundary. The state-machine entry criteria in Figure 4 are defined as where d i,enter CPA , t i,enter CPA , and t i,enter CPA for i ∈ {SO,OT,GW,HO} are tuning parameters denoting thresholds on d CPA and t CPA in order to satisfy the entry criteria for the stand-on, overtaking, giveway and head-on states. The tuning parameter t EM,enter crit denotes an upper limit on t 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 CPA and d CPA need to be within some tunable thresholds. Situations with a very low d CPA , but with a high t CPA , will not trigger the entry criteria, since the situations will not occur in the near future. Similarly, if t CPA is within the thresholds, but d CPA is large, this indicates a safe passing where risk of collision does not exist. The lower bound on t CPA will typically be selected as zero, and is useful to distinguish between obstacles moving toward 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 crit being under the threshold t EM,enter crit , we require that t 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 headon, 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 where d i,exit CPA , t i,exit CPA , and t i,exit CPA for i ∈ {SO,OT,GW,HO} are tuning parameters denoting thresholds on d CPA and t 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 crit is larger than the tuning parameter t EM,enter exit , or t 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: and for all i ∈ {SO,OT,GW,HO}.

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 CPA to distinguish between situations where the obstacle moves closer toward 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 ±6°as described by the COLREGs. The reason for this is that FIGURE 5 | Illustration of the geometrical COLREGs interpretation, where the ownship course is denoted as χ and θ 1 , θ 2 , θ 3 denote symmetrical regions given as [22.5, 90, 112.5°] offset from ahead. The circles illustrate obstacles in different relative bearing regions, and have a fixed orientation with respect to the ownship. The geometrical situations are color-coded and denoted as G i , i ∈ {SF, SO, OT, GW, HO} for safe, stand-on, overtaking, give-way and head-on situations, respectively. When two situations are given, like e.g., G SF/SO , we use the former (SF) if t CPA < 0 and the latter (SO) if t CPA ≥ 0, analogous to the obstacle moving away or toward the ownship. To decide the geometrical situation, we first find which relative bearing region the obstacle resides in, before finding which obstacle region the obstacle's course resides in. The figure is inspired by Tam and Bucknall (2010).
Tam and Bucknall recommend using a larger region of 22.5°in order to increase the robustness of the geometrical COLREGs interpretation scheme.

Interface to the High-Level Planner
The high-level planner produces an energy-optimized nominal trajectory for the ownship to follow. However, since the highlevel 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 midlevel algorithm performs relative trajectory tracking, where it tracks the nominal trajectory with a time offset t b ∈ R. This results in a relative nominal trajectory for the mid-level algorithm:p where

Optimization Problem Formulation
The mid-level algorithm is formalized as an OCP: subject tȯ where T h > 0 is the prediction horizon, φ(·, ·) 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 is a vector of 5N p + 3 decision variables andp d,1 : N p = [p d,1 ,p d,2 , . . . ,p d,N p ] is a sequence of desired positions. The vectors ω ∈ R 2N p , µ ∈ R 2N p and ξ ∈ R MN p contain slack variables, where M is the number of moving obstacles to be included in the constraints.
The vector g(w, η(t 0 )) ∈ R 3N p +3 contains shooting and boundary constraints, while h(w) ∈ R (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 vectorsh k (η k , ω k , µ k ,p d,k ) ∈ R 6 , k ∈ {1, N p } contain constraints on the slack variables ω and µ.
In the following subsections, we describe the terms in (18) in more detail.

Objective Function
The objective function contains four functions, where φ p (w, ω, µ) introduces cost on deviating from the relative nominal trajectorȳ p d (t), φ c (w) introduces cost on using control input, φ COLREGs (w) is a COLREGs-specific function and φ ξ (ξ ) 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 φ p (w, ω, µ). 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 σ > 0: 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,φ p (w, ω, µ) is defined as where K p > 0 is a tuning parameter, and ω k ∈ R 2 and µ k ∈ R 2 are slack variables constrained bȳ where p k is the predicted vessel position at time step k, i.e., 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 φ c (w) introduces a non-linear cost on the change in speed and course, which makes the algorithm favor readily observable maneuvers. The function is defined as where KU , Kχ > 0 are tuning parameters, while qU (U k ) and qχ (χ k ) are the non-linear cost functions. Notice that neither the speed over ground (SOG) U nor the course χ are elements of the search space, but they can be computed as U = √ u 2 + v 2 and χ = ψ + arcsin v U . Their derivatives are then calculated by finite differencing. See Eriksen and Breivik (2017a) and Bitar et al. (2019a) for more details on the control cost function.
The φ COLREGs (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 where O HO , O GW , O SO , and O EM contain obstacles which are in the head-on, give-way, stand-on and emergency states, respectively, and K HO , K GW , K SO , K EM > 0 are tuning parameters. The functions V HO,i,k (p k ), V GW,i,k (p k ), V SO,k (w), and V EM,k (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 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: where α x,HO , α y,HO > 0 are tuning parameters controlling the steepness of the head-on potential function andx 0,HO > 0 is a tuning parameter controlling the influence of the attenuating potential. The coordinate (x {i,k} , y {i,k} ) is 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 where p o,k,i and χ i,k are the position and course of obstacle i at time step k. The head-on potential function with parameters α x,HO = 1 /500, α y,HO = 1 /400 and x 0,HO = 1, 000 m is shown in Figure 6A. 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 where α x,GW , α y,GW > 0 control the steepness of the give-way potential function andȳ 0,GW < 0 control the attenuation on the port side of an obstacle. The give-way potential function with parameters α x,GW = 1 /400, α y,GW = 1 /500 and y 0,GW = −500 m is shown in Figure 6B.
In stand-on situations, we want the mid-level algorithm to disregard the obstacle and keep the current speed and course A B FIGURE 6 | Potential functions ensuring passing on the correct side in head-on and give-way situations. Yellow indicates a positive value, blue indicates a negative value, while the yellow patch and axis cross show the obstacle location and course-fixed coordinate system. Used in a minimization scheme, this will favor starboard maneuvers in head-on situations, and passing behind obstacles in give-way situations. Note that the obstacle here has zero sideslip, resulting in the heading and course pointing in the same direction. (A) Head-on potential function. (B) Give-way potential function.
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: 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: The slack variable ξ 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 4.5. The homotopy cost function φ ξ (ξ ) introduces slack cost on ξ : where K ξ > 0 is iteratively increased as part of the homotopy scheme.

Obstacle Handling and Steady-State Feasibility
The inequality constraint h(w, ξ ) ≤ 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 α 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 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 where x c,i,k , y c,i,k , and α 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 where we include slack variables ξ ≥ 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 toward 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 ξ . Initially, this is selected as a low value to have a high amount of slack on the moving obstacles, while it is iteratively increased toward K ξ → ∞, which results in ξ = 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) and Bitar et al. (2019a), we ensure steady-state feasible trajectories at each time step through a constraint h x r,k (x r,k ) ≤ 0 ∈ R 4 , which captures the state constraint x r ∈ X r at time step k. To ensure steadstate feasibility for the entire prediction horizon, we define the constraint Finally, the inequality constraints are combined as.

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 , extended to also consider static obstacles in  and is experimentally validated in several fullscale 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 aṡ whereη(·) denotes a kinematic simulation procedure that given SOG and course trajectories,Ū(·) andχ (·), inŪ d computes the vessel pose. See  and  for more details on the trajectory generation procedure. In order to converge toward the trajectory specified by the mid-level algorithm, a desired acceleration is computed based on a line-of-sight guidance scheme. In  and , 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 toward the desired trajectory is included in the search space.
The variables w al , w av,m , w av,s , w t,U , w t,χ > 0 are tuning parameters, while align(η(·); p mid d (·)) measures the alignment between a candidate trajectoryη(·) and the desired trajectory from the mid-level algorithm p mid d (·). The function avoid m (η(·)) 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 avoid s (η(·)) ensures COLAV of static obstacles by introducing an occupancy grid, while tran U (u d (·)) and tran χ (u d (·)) 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  and  for more details and descriptions of the terms.

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.

Simulation Setup
The simulations are performed in MATLAB on a computer with an 2.8 GHz Intel Core i7 processor running macOS Mojave, 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 2, and the modelbased speed and course controller in  is used as the vessel controller.
The parameters of the high-level algorithm are listed in Table 1. The number of prediction steps N hi is chosen to achieve a time step length h = tmax /N hi < 1.5 s, which seems to be a good compromise between capturing the relevant system dynamics and having a feasible computational requirement.
The mid-level algorithm is implemented using the parameters in Table 2.
The slack variable cost K ξ 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 toward 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 s. 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 s. 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 s, with parameters as described in . An update rate of 5 s is considered sufficient due the typically large maneuvering margins at sea. It is also worth noting that the detection and tracking system can represent a significant time delay, especially for radar-based systems . For confined and congested areas the BC-MPC algorithm may need to be run at a higher rate, which also imposes requirements for high-bandwidth obstacle estimates. Static obstacles are padded with a safety margin of 150 m for the high-level and mid-level algorithms, while the BC-MPC algorithm uses a safety margin of 100 m 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 fullscale experiments using radar-based detection and tracking of obstacles . 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 radarbased 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: Scenario 1 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.
Scenario 2 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.
Scenario 3 This scenario contains two moving obstacles, which suddenly perform dangerous maneuvers close to the ownship, displaying the use of the emergency state.

Scenario 1
Scenario 1 contains two static obstacles, four moving obstacles, an ocean current of [−2, 0] ⊤ m/s and is shown in Figure 7.
The high-level planner plans a nominal trajectory between the initial and goal positions at [7000,200] ⊤ m and [0, 7900] ⊤ m, 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 Figure 7B, 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 giveway 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 toward 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.

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] ⊤ m/s. The high-level planner plans a nominal trajectory between the initial and goal positions at [200,200] ⊤ m and [5500, 7000] ⊤ m, 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 standon 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, headon 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 toward the nominal trajectory after the situation is resolved. As shown in Figure 9B, 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.

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] ⊤ m/s. The high-level planner plans a nominal trajectory between the initial and goal positions at [500, 500] ⊤ m and [3328, 5399] ⊤ m, respectively, which results in a straight line trajectory with a course angle of 60°. 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 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 Figure 11B, 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.

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. Table 3 shows the minimum distance to static and moving obstacles for the scenarios.  The minimum distance to static obstacles is in Scenario 1 below the safety region size of the BC-MPC algorithm, which is intentional and caused by the algorithm using a smooth penalty function for interpreting static obstacles. The penalty function value increases linearly when moving further into the safety region, see  for more details. The minimum distance to moving obstacles is a bit difficult to interpret, since the obstacle ship domains are non-circular, implying that the required clearance depends on relative position of the ownship with respect to the moving obstacles. However, we see that we have a larger clearance in head-on, give-way and stand-on situations where the obstacles comply with the COLREGs, and do not perform dangerous maneuvers (as in Scenario 3), compared to overtaking situations. The reason for this is that when overtaking (obstacle 2 in Scenario 2), we pass the obstacle on a parallel course, resulting in the minor axis of the moving obstacle ellipsis indicating the required clearance. Furthermore, we see that obstacle 1 in Scenario 2, which ignores its give-way obligation, comes significantly closer than other crossing obstacles except for those in Scenario 3. The reason for this is that the BC-MPC algorithm, which handles this situation, has a lower clearance requirement than the mid-level algorithm, which still should be considered as safe. In Scenario 3, the two obstacles display poor seamanship, and behave dangerously. Obstacle 1 is handled by the mid-level algorithm and passed with a clearance lower than the major axis of the mid-level algorithm, which is caused by the BC-MPC algorithm "cutting the corner." The clearance should still be considered safe since we are behind the obstacle, and the clearance requirements of the BC-MPC algorithm is enforced. Obstacle 2, which is placed in the emergency state and handled by the BC-MPC algorithm, is passed with a clearance of only 106.6 m. This is lower than the clearance to Obstacle 1 in Scenario 2 (which violated its stand-on requirement), and is due to the BC-MPC algorithm having a non-symmetric obstacle ship domain function allowing for a smaller clearance when passing behind an obstacle than in front.
For the three scenarios, the high-level planner used an average of 67 s with a maximum of 93 s to compute the solution.
Since the high-level planner is intended to be run off-line, this is well within reasonable limits. The mid-level algorithm used 0.60 s on average, and a maximum of 2.1 s, which we consider to be computationally feasible since the mid-level algorithm only is run every 60 s. The BC-MPC algorithm used 0.29 s on average, and a maximum of 0.63 s, which we also consider to be real-time feasible when the BC-MPC algorithm is run every 5 s. The BC-MPC algorithm is highly parallelizable, which could reduce the BC-MPC runtime by a large magnitude if required. The mid-and high-level algorithms may not return solutions as they are non-convex optimization problems, but the BC-MPC algorithm makes the hybrid COLAV system real-time feasible since it always will find a (potentially sub-optimal) solution.

CONCLUSION
In this paper, we have presented a three-layered hybrid COLAV system, compliant with COLREGs rules 8 and 13-17. As part of this, we have further developed the MPC-based mid-level COLAV algorithm in Eriksen and Breivik (2017b) and Bitar et al. (2019a) to comply with COLREGs rules 13-16 and parts of Rule 17, which includes developing a state machine for COLREGs interpretation. The hybrid COLAV system has a welldefined division of labor, including an inherent understanding of COLREGs Rule 17, where the mid-level algorithm obeys standon situations, while the BC-MPC algorithm handles situations where give-way vessels do not maneuver. The hybrid COLAV system is verified through simulations, where we in three scenarios challenge the system with a number of different situations. The scenarios include multi-obstacle situations with multiple simultaneously active COLREGs rules, and situations where obstacles violate the COLREGs. Collision is avoided in all the scenarios, and we show that the ownship follows an energy-optimized trajectory generated by the high-level planner when moving obstacles do not interfere with this trajectory.
For further work, we suggest to: • Investigate if using situation-dependent entry and exit criteria parameters in the state machine improves the performance. • Expand the state machine with the possibility of transitioning from head-on, give-way and overtaking states to the emergency state for situations where obstacles behave dangerously or hostile. • Develop a methodology for deciding tuning parameters. • Perform simulations with noisy obstacle estimates to investigate how the state machine and mid-level algorithm respond to this. • Explore the possibilities for integrating the COLREGs interpretation in the mid-level NLP, relaxing the assumption of the current COLREGs situation being valid for the entire prediction horizon. • Investigate the possibility of including static obstacles from e.g., ENCs in the high-and mid-level algorithms. • Simulate scenarios where multiple vessels running the hybrid COLAV system interact with each other. • Validate the hybrid COLAV system in full-scale experiments.

DATA AVAILABILITY STATEMENT
The datasets generated for this study are available on request to the corresponding author.

AUTHOR CONTRIBUTIONS
The work in this article was the result of a collaboration between B-OE and GB, supervised by MB and AL. The contributions to the mid-level and short-term algorithms are made by B-OE, while the COLREGs interpreter was developed in collaboration between GB and B-OE. GB has implemented the high-level planner, and prepared this for integration with the developed simulator. B-OE has taken lead on writing the paper, in collaboration with GB. MB and AL have provided valuable feedback in the writing process.

FUNDING
This work was supported by the Research Council of Norway through project number 269116, project number 244116, as well as the Centers of Excellence funding scheme with project number 223254.