Autonomous navigation of ships by combining optimal trajectory planning with informed graph search

ABSTRACT Autonomous trajectory generation plays an essential role in the navigation of vehicles in space as well as in terrestrial scenarios, i.e. in the air, on solid ground, or water. For the latter, the navigation of ships in ports has specific challenges since ship dynamics are highly nonlinear with limited agility, while the manoeuvre space in ports is limited. Nevertheless, for providing support to humanly designed control strategies, autonomously generated trajectories have not only to be feasible, i.e. collision-free but shall also be optimal with respect to manoeuvre time and control effort. This article presents a novel approach to autonomous trajectory planning on the basis of precomputed and connectable trajectory segments, the so-called motion primitives, and an A*-search algorithm. Sequences of motion primitives provide an initial guess for a subsequent optimization by which optimal trajectories are found even in terrains with many obstacles. We illustrate the approach with different navigation scenarios.


Introduction
Today, autonomous transportation is one of the most active research areas all over the world. Certainly, the most popular branch deals with self-driving cars. However, autonomous automobiles cover only a small fraction of transportation problems, since they are mostly the solution for medium-ranged private transportation. A huge task for the logistics industry is global trade. According to [1,2,3] about 90% of all goods are carried by ships today. Thus, the development of techniques for autonomous ship navigation is of great interest, too. Thereby, ship captains can be assisted in navigation tasks, which could traditionally only be solved by well-trained and experienced personnel. Thus, this article focuses on the autonomous 1 navigation of ships by providing feasible and, even more, optimal solutions in scenarios with challenging constraints. A sophisticated modelling is an indispensable prerequisite to allow for mathematical optimization. Classically, ship dynamics are modelled by ordinary differential equations, since for this modelling approach, established simulation techniques are at hand. However, trajectory optimization then requires nonlinear optimization techniques, which, in general, rely on good initial guesses and are computationally costly. This paper does not address the algorithmic barriers in optimization, but, instead, we reconsider the modelling step, i.e. propose and analyse a model abstraction that allows a combined application of state-of-the-art optimization techniques for discrete and continuous models, respectively.
The method's development has been motivated by the research at the University of Bremen within the project GALILEOnautic [4,5,6]. This project makes use of the global navigation satellite system GALILEO for manoeuvring in safety-critical areas such as ports. Optimized manoeuvring and automated navigation are realized by onlineoptimization using the software WORHP and TransWORHP [7,8,9]. Further aspects to investigate are cooperation of ships, model-based (feedback) control design, and a virtual reality 3D test environment. The GALILEOnautic project plans an application to the ferry crossing from the port of Rostock in Germany to Gedser in Denmark, a port with demanding waters.
The proposed approach within this article is based on the idea of motion planning with motion primitives by Frazzoli et al. [10]. In the offline phase of the method, a set of motions which is consistent with the given vehicle dynamics, the so-called motion primitives, are computed. The term 'primitive' originally refers to the motions being simple in the sense that humans would intuitively choose them to e.g. stear a vehicle. In a formal setting, primitives can be selected by model-based criteria as it is done in optimization, e.g. the primitive duration or its energy efficiency. Exploiting symmetries in the model allows to use and combine these primitives in various ways. Thus, during the online phase, an optimal sequence of primitives is searched which solves the given planning task by using a modified A* method. The size of the library of motion primitives is crucial for online-applicability. However, a coarse representation of the ship model by only using a few number of motion primitives generates suboptimal solutions. We address this issue by using the sequence of motion primitives as an initial guess [11,12] and apply an optimal control method using the full nonlinear dynamical model afterwards. Due to the sophisticated initial guess, the local solver of the direct optimal control method is more likely to converge quickly (cf. to the discussion and numerical studies in [11] or [13], for instance). We apply this approach to a ship model. A crucial challenge for the planning phase as well as for the post-optimization are restrictions due to environmental constraints. Using an occupancy grid to model port layouts, we guarantee that the motion primitive sequence and the post-optimized trajectory are feasible solutions.
The combination of A* planning with optimal control overcomes several shortcomings of the individual methods and thus provides a powerful approach to real-world applications: • While classical planning focuses on geometric paths only (which are not necessarily always feasible), a sequence of motion primitives is feasible with respect to the (nonlinear) dynamics model, thus the path can be realized with less corrective feedback control.
• Using curves that combine states and controls within the A* (instead of sampling the control space only), we always have the state trajectory and control curve at hand which might be helpful for designing feedback controller to robustify solutions. • Time-consuming optimal trajectory design can be done offline in the preparation phase. The optimized manoeuvres are stored in the library and can be used sequentially by the A* planning. • Providing a sophisticated initial guess to the local optimization method within the nonlinear optimal control is likely to speed up convergence. Moreover, since the primitive sequence is an admissible solution, it can always be used as a fallback, if, for instance in a real-time Model Predictive Control (MPC) scheme, an optimal control solution cannot be provided in time.
Path planning for vehicles by concatenation of primitives has a long history dating back to Dubin's car, where arcs of circles have been used to find shortest paths, see [14] and also Reed's and Shepp's curves as a crucial extension [15]. A number of further extensions and variants are e.g. listed in the textbook [16]. Motion planning with motion primitives, as introduced by Frazzoli et al. in [10,17,18], also falls into the class of planning methods. In [19] a survey on planning methods suitable for applications to autonomous vehicles was conducted.
Optimal control, as a field of mathematical research, emerged from studying variational problems, see e.g [20] for a historical review. However, it required improved numerical techniques from the past 50-60 years in order to make optimal control applicable to real-world examples. Among different numerical approaches, direct optimal control has shown great applicability to large-scale problems [21]. A direct transcription of the optimal control problem into a nonlinear constrained optimization problem allows to use efficient NLP solvers such as IPOPT [22], KNITRO [23], SNOPT [24], or WORHP [7].
A detailed study of the numerical methods is out of the scope of this article. Instead, we focus on conceptually showing the combination of planning and optimal control methods in ship navigation.
Planning and optimization in ship manoeuvring was studied e.g. in [25,26,27,28,29]. See [29] for a recent research overview on ship collision avoidance methods, which identifies motion prediction as one of the crucial methods for collision prevention. In [30], for instance, a modelling approach based on field theory and particle simulations for collision avoidance is presented. Typically, collision avoidance considers much shorter manoeuvres (in time, as well as in deplacement) than our path planning approach [31]. also considers collision avoidance scenarios instead of large planning tasks. However, they build their solutions by pieces of trajectories and allow continuation of some pieces. This resembles our formalism of trim primitives and manoeuvres, presented in detail in the following. Contrarily to collision-avoidance, in [32] formation control of multiple autonomous vessels is addressed. The focus is on feedback control in order to address environmental disturbances induced by waves, wind and ocean currents. In e.g [26]., the focus lies on the sensor aspects, which are not covered in our work.
In [33], an overview on planning and navigation for vessels and sailboats is given. The presented methods for collision avoidance are based on discretized manoeuvres, thus sharing some similarities with our approach. The path planning literature survey is focused on sailboats. It lists graph-based search methods in contrast to potential field methods and classical sailing. Closely related to our work [34], also considers autonomous navigation tasks of vessels and proposes a discrete planning method, called fast marching method. While this method is originally a pure path planning method, the authors integrator ship kinematics. Then, a PID controller has to be designed in order to optimize the trajectory. Here, our post-optimization approach provides more flexibility due to a general nonlinear optimization solver. However, we only provide an open-loop solution, not a feedback controller.
As it is the case in our approach, the kinematics and dynamics model and also similar objective functions, e.g. control effort, are considered in [28] for planning for autonomous marine vehicles. However, they apply a projection operator method instead of our two-step approach of planning and direct optimal control. While their focus lies on computing collision-free manoeuvres for multiple vessels, we focus on navigation in ports; this makes it hard to compare the methods directly. In [25], planning on a base of trajectories is performed. In contrast to our approach, the trajectories do not stem from so-called trim primitives and optimal manoeuvres. Moreover, we use different optimization methods (nonlinear gradient-based versus ant-colony optimization). Another quite different approach is based on a potential flow design for path planning as presented in [27]. Autonomous navigation has been successfully realized for a sailboat as reported in [35]. Here, the focus is on the overall implementation and less on the guidance task. Within the proposed control architecture, our motion planning with primitives approach could provide optimal guidance trajectories as high-level control inputs.

Outline
We proceed by introducing a ship model in Section 2. Then, a formal definition of motion primitives follows, and it is applied to compute symmetry and primitives of the ship model in Section 3. In Section 4, we design a manoeuvre automaton for the ship model, which forms the basis for our planning method. Also, the A* algorithm and the occupancy map generated for port environments are introduced. The method is evaluated in several scenarios with numerical results presented and discussed in Section 5. Finally, we give concluding remarks in Section 6.

Modelling
We are using the ship model proposed in [36] with the same choice of parameters, _ xðtÞ ¼ uðtÞ cosðΨðtÞÞ À vðtÞ sinðΨðtÞÞ (1) _ yðtÞ ¼ uðtÞ sinðΨðtÞÞ þ vðtÞ cosðΨðtÞÞ (2) If we neglect Equations (4)-(6), we are left with first-order kinematic Equations (1)-(3). The evolution of the spatial displacements x 0 and y 0 depend on the longitudinal and lateral velocities u and v, and the current heading Ψ. The angular velocity is denoted by r. Considering u; v; r as the control inputs, this model resembles simple vehicle models, also known as holonomic robots, for instance (cf [37]). The system can instantaneously start to move into any direction independent of the current orientation. This is in contrast to a nonholonomic model, which includes constraints given by wheels. Compared to street or aeronautic vehicles, a ship is far less agile. This is modelled in Equations (4)- (6). Adding these equations to our model gives a system of six ordinary differential equations. Here, m represents the mass of the ship, x G denotes the centre of mass along the x-component of the coordinate system attached to the ship (see Figure 1).
In [36], a port-starboard symmetry is assumed and the ship coordinate system's origin is chosen to lie on this symmetry axis. Thus, the centre of mass must lie on this axis, i.e. y G is equal to zero. The term I zz denotes the moment of inertia with respect to the z-axis.
The capital letters X; Y denote forces and a N a moment acting on the ship. The ship model we are using in this article is independent of external forces such as water depth, wind, or time history effects, and there is no explicit dependency of the position of the ship (see [36]). The control inputs of the full model are the propeller thrust and the rudder angle, again we refer to [36] for details.

General definitions
Many dynamical systems show symmetries. Here, we are interested in continuous symmetries that express themselves in terms of invariances. Illustrative examples are given by translational or rotational invariances of a mechanical system in the plane. We consider a dynamical system on an n-dimensional manifold M, given by _ xðtÞ ¼ f ðxðtÞ; uðtÞÞ with xðtÞ 2 M � R n and uðtÞ 2 R m . Let T M denote the tangent bundle of M. We assume f : M � R m ! T M to be continuous and locally Lipschitz w.r.t. its first argument in order to guarantee existence and uniqueness of the solution φ u ð�; x 0 Þ on a compact time interval ½0; T�, 0 < T < 1, for u 2 L 1 ð½0; T�; R m Þ. L 1 ð½0; T�; R m Þ denotes the space of Lebesgue-measurable and absolutely integrable functions on the domain ½0; T�. We recall from [38] the following definitions.
A motion primitive is the equivalence class of all trajectories equivalent to φ u ð�;x 0 Þ w.r. t. the left action Γ.
Note that the same control function u is assumed for all members of a motion primitive. Definition 3.3 (Trim Primitive). Let ðG; M; ΓÞ be a symmetry group and let g denote the associated Lie algebra of G. Then, a trajectory φ u ð�;x 0 Þ is called a trim primitive if there exists a Lie algebra element � 2 g such that φ u ðt;x 0 Þ ¼ Γðexpð�tÞ;x 0 Þ and uðtÞ;� u ¼ const: "t 2 ½0; T�: The original definition of trim primitives goes back to Frazzoli in [10]. Trim primitives can be interpreted as the extension of relative equilibria (see e.g. [39],) to systems with control inputs. Loosely speaking, trim primitives are simple motions (despite nonlinear dynamics), which are generated by the symmetry action. The control input has to stay constant along a trim primitive. In fact, the 'trimmed input' is the reason why these motion primitives are called trim primitives or trims for short.

Symmetry for ship models
with group action Γ : G � M ! M acting by matrix multiplication on the homogeneous Proof. Note that the ship model can be written as a matrix-vector multiplication, i.e.
and the ODE's analytic solution is given by xðtÞ yðtÞ ΨðtÞ Then, direct calculations show that

Trim primitives for rigid bodies moving without restrictions in a plane
According to Definition 3.3, controls have to be constant along trim primitives. For the kinematic ship model, two types of solutions are possible: moving on a straight line with no angular velocity or moving on a circle with an angular velocity equal to the magnitude of the ship's velocity divided by the radius of the circle. In these cases, we can analytically compute the flow. For r;0 and u; v constant, we have which means indeed following a straight line defined by the initial heading Ψ 0 and the constant controls ðu; vÞ. Now assume r�0, but ðu; v; rÞ constant. Then, the motion is defined by Trim primitives are generated by elements of the Lie algebra that corresponds to the symmetry group. The kinematic ship model's symmetry group G (cf. (8)) is a subgroup of the special Euclidean group, that combines rotation and translation in three dimensions. With Definitions 1.3 and 1.4 from [40] it is easy to see, that G is a closed subgroup of SE (3), thus a Lie group itself. The corresponding Lie algebra (cf. e.g [41].), denoted by seð3Þ, is given by seð3Þ ¼ soð3Þ�R 3 and can be interpreted as being comprised of the three rotational and three translational velocities. The Lie algebra of rotational matrices is the group of skew symmetric matrices, so seð3Þ can be written as Thus, for the kinematic ship model, the Lie algebra is a subalgebra of seð3Þ with only three degrees of freedom The exponential map of λ 2 g can then, for instance, be computed via the Rodriguez formula (see [41]).

Proposition 3.5. Trim primitives of the kinematic ship model are given by (a) straight lines, if control values and Lie algebra elements satisfy
Proof. Case (a), 'Straight lines': Here, ω ¼ 0 in Equation (13). On the one hand, a suitable Lie algebra element ð�; ζ; 0Þ can be parametrized by t and mapped under the exponential map, such that it acts on a vector x 0 by On the other hand, from Equation (9) we know that solutions on straight lines have to be of the form Component-wise comparison leads to the following conditions for a trim primitive , 'Circular arcs': Again, we have to show that Γðexp Λt;x 0 Þ with a Lie algebra element chosen according to Equation (14) generates valid solutions of the dynamics. By Rodriguez formula, we obtain Following the definition of trims, we compute Now, comparing the third component of Equation (16) to the solution for ΨðtÞ, we see that ω ¼ r must hold. Rewriting the first two components of the solution given in Equation (10) using trigonometric identities, we see that these are equivalent to Equation (16), if the following conditions to the parameters hold Thus, we have found the defining conditions of a trim.
The conditions derived in the proposition relate the Lie algebra elements to the control values and, moreover, this relation depends on the initial point ðx 0 ; y 0 ; Ψ 0 Þ T . However, given the initial point and control values ðr; u; vÞ, the corresponding Lie algebra element that generates a trim is uniquely defined. As a consequence of Prop. 3.5, we see that any triple of constant controls generates a trim. This is important for designing the manoeuvre automaton.

Numerical computation of trims and connecting manoeuvres
We have not been able to take over the analytical computation of trim primitives via Lie group symmetry action to the full ship dynamics. However, trim primitives of the simplified ship model give crucial intuition about how trims for the full model may look like. With that intuition, it is possible to numerically approximate the trim primitives. To this aim, we have to find the corresponding controls for a given velocity. There are multiple ways of solving this task. Forward integration over a long time horizon does the trick as the forces will balance over time independent of the initial state. Alternatively, velocity-control-pairs could be obtained by an optimal control problem.
For the computation of trim-connecting manoeuvres, we formulated and solved optimal control problems. Note that these problems are typically much less complicated than the full original optimal control problem, as we solely have to connect two kinematics states of the ship without considering any geometric constraints. Thus, these kinematics states are chosen to lie in the previously computed trim primitives. Within the optimal control problem, corresponding boundary conditions are considered. Objective functions can be chosen in correspondence with the cost function of the A* algorithm (cf. Section 4) and, if applicable, consistent with the objective functions used within the postoptimization (cf. Section 4.4).

Trajectory planning algorithm
Our approach to trajectory planning is fundamentally based on motion primitives. Among them, trim primitives play a special role due to their properties derived in Section 3. In the following, we first describe the general procedure of our trajectory planning algorithm before we give more information on the application to our specific problem.
As a reminder, the goal of the algorithm is to compute feasible trajectories and fill the gap between optimal control problems, which heavily rely on sophisticated initial guesses close to the optimal trajectory, and path planning algorithms, which work great with a control algorithm, but provide too little information to be directly used as an initial guess. This is precisely where motion primitives come in handy.
Motion primitives can be computed offline before planning a trajectory. Because of their symmetry properties, these precomputed trajectory snippets can also be smoothly glued together to form a smooth complete trajectory. Hence, the main problem is to identify a sequence of motion primitives that form a smooth path from the initial starting point to the sought final location.
In order to identify this sequence, we use a variation of the A* search algorithm. An A* search algorithm works on a graph data structure and finds the shortest path from one node to another. The standard A* algorithm is complete and optimal. From a given current node, the A* algorithm explores all neighbouring nodes and adds them to a list of visited nodes. In standard A* implementations, such a neighbour is typically a discrete grid position right next to the current one (see Figure 2(a)). This is where the motion primitives come into play in our case. We alter the definition of a neighbour from the strict grid structure to a state that can be reached within the execution of one motion primitive (see Figure 2(b)) and satisfy a few feasibility conditions as detailed out below. This has the consequence that we still benefit from the excellent properties on the A* algorithm but planning trajectories instead of simple paths. Thus, we can use them far more effectively as an initial guess for a solver for NLP (Nonlinear Programming) problems. Generally, the adaptation of the A* algorithm to incorporate neighbours based on the dynamics of the system, not necessarily using motion primitives, instead of a fixed neighbourhood map is known as a hybrid A* approach, see e.g. [42].

Preparation phase
One shortcoming of many A* approaches to this setting in the literature is the search for neighbours based on the system dynamics. Short portions of a trajectory are glued together often without regard to the differential equations. This difficulty is sought to be evaded by utilizing the more formal setting of motion primitives. The underlying theory allows us to make a smart selection of trajectory snippets for the A* algorithm with provable properties.
As a prerequisite to applying the A* algorithm, a motion primitive library has to be computed, i.e. a finite number of motion primitives have to be chosen. Typically, one starts with selecting trim primitives. Every type of trim primitive, which might be of interest in control scenarios, shall be represented. An approach for generating the set of trim primitives is to define a grid on the Lie algebra of appropriate size. (Recall that Lie algebra elements typically correspond to rotational or translational velocities.) Considering the simple ship model, (1)-(3), trajectories on trim primitives can be directly connected. Thus, it is possible to build a manoeuvre automaton with trim primitives only. For more complex dynamics, e.g. the full ship model (1) -(6), a set of manoeuvres have to be computed, which connects pairs of trim primitives. It is not mandatory nor recommended to interconnect each trim primitive with all other trim primitives because it increases the computational effort of the planning. However, since the motion primitive automaton approximates the continuous nonlinear dynamics, larger automata have better reachability properties and allow for a better approximation of optimal solutions of the original dynamics.
Based on the definition given in [10], we define manoeuvre automata for the simple and full ship model.
We start with defining a minimal example automaton for the simple ship dynamics.  • δ defines the graph structure; here we have a fully connected graph, see Figure 3 • q 0 ¼ Q is the initial state, which can be on any trim, • F ¼ Q is the set of accepted final states, i.e. every trim is accepted as final trim.

Time discretization
The manoeuvre automata from Definition 4.1 and Definition 4.2 cannot be used in the A* algorithm, yet. The last step is a time-discretization of the trims. That is, the trims are transformed into manoeuvres of fixed duration, cf [43]. However, by applying multiple discretized trim snippets, which each have a short time duration, in a row, a continuous duration can be approximated.
Once we have constructed the entire graph, we are done with the preparation phase of the algorithm. In the next sections, we introduce the other building blocks of the algorithm and see how we can put motion primitives to good use.

A* search algorithm
The first building block of our algorithm is the (hybrid) A* search algorithm. This algorithm is part of the informed graph-search algorithms and was discussed in the literature many times e.g. [44]. As a quick reminder, the core steps of the A* algorithm are: Algorithm 1 (A* search algorithm).
1: Add initial state to the A* graph and initialize neighbouring nodes as unvisited 2: while List of unvisited nodes is non-empty do 3: Get cheapest node from the list of unvisited nodes and mark node as current node and visited 4: if current node is the target node then return Success 5: else 6: Find all unvisited neighbours of current node and update costs 7: return error We do not touch the first four steps of this algorithm. However, we will alter the expansion step. Traditionally, the algorithm acts on a two-dimensional grid, and the adjacent cells on that grid become the new neighbours of the current node. Our search space, which we will introduce in the next section, is four-dimensional. This and the fact that a huge ship is limited in its movement require a better fitting definition of a neighbour cell. This is where we combine motion primitives and the A* search algorithm. To find the neighbour cells, which become nodes in the A* graph, we first execute each motion primitive from our motion primitive library. Note that, in contrast to existing methods, this is particularly inexpensive as the finite set of motion primitives can be precomputed due to the underlying symmetry conditions. However, a final check whether the execution was feasible is still mandatory. If so, the final cell, together with additional information is added to the A* graph as a node, the executed primitive is added to the graph as an edge.

Search space
We ended the previous chapter with the statement that the motion primitives are being executed to grow the A* graph. The A* graph is a discrete data structure, but motion primitives are continuous motion, which has been discretized with a high resolution. We need an object that acts as a link between the discrete A* graph and the continuous motions of the ship. For that purpose, we introduce the search space. N is a set of states A is a set of arcs connecting the states S is a nonempty subset of N that contains start states G is a nonempty subset of N that contains goal states.
In our case, a single state consists of four bits of information, a two-dimensional position, the orientation of the vehicle and a discrete dynamic state of the vehicle. Thus, the set of states N we are considering is a bounded subset of the following Cartesian product: The set of arcs A is the library of motion primitives used for the exploration by the A* algorithm. In our application, S is always a one-element set, and G is a one-dimensional subspace of N where the free parameter is the orientation of the vehicle.
The search space is continuous in three dimensions. If we keep them continuous, our planning algorithm will face difficulties planning a trajectory between two discrete points in a continuous environment. A discretization of the set of states causes that we cluster near points. Experiments have shown that a spatial discretization of 15m � 15m cells and a resolution of π 50 for the orientation are reasonable choices. We further decided to exclude geometrical constraints from the search space so that the search space is an obstacle-free representation of the environment. A geometrical constraint is any obstacle that blocks an edge to connect two states. An example of a geometrical constraint that is particularly important for the examples below is quay walls. More sophisticated constraints, which we have not considered so far, but are possible in the future, are the ship's draft and a limitation of the velocity. We are using an occupancy grid for this job, which is further discussed in the next chapter.

Occupancy grid
The final component we need is a binary occupancy grid that encodes for each cell in the search space, whether it is occupied, i.e. blocked by land, or free. We decided to separate the occupancy grid from the search space to allow different cell sizes. The grid cell size of the occupancy grid should be at least as large as the cell sizes in the search space. We were interested in the question of whether a lower resolution of the occupancy map increases the quality of our results. The underlying idea is that a single grid cell of the search space is smaller than the dimensions of a large ship that means if the algorithm plans a trajectory very close to the quay walls, the ship could interfere with the walls, and the planned solution is of poor quality. We can prevent this from happening by increasing the cell sizes in the occupancy grid map. However, this problem can also be overcome by using a barrier term. The barrier term is half of the diameter of the ship. When we create the occupancy grid, we block each cell that is closer to a quay wall than the barrier term. This technique allows us to keep a higher resolution but prevents unexpected behaviour of the algorithm. A second reason for the separation of the search space and the occupancy grid is that it allows us to use the same search space for multiple ship models at the same time, because ships with different lengths and widths need distinct occupancy maps.

Function principle of the algorithm
Now we are all set to look at the algorithm at runtime, which combines the presented building blocks. For that purpose, we start with the pseudo-code of the algorithm, followed by a detailed explanation of each step.
1: Add initial state to the A* graph and initialize neighbouring nodes as unvisited 2: while List of unvisited nodes is non-empty do 3: Get cheapest node from the List of unvisited nodes and mark as current node and visited 4: if current node is the final node then return Success 5: for each feasible motion primitive do 6: Execute motion primitive 7: if Motion primitive execution was collision-free then 8: Add the final state of the motion primitive execution to the list of unvisited nodes if not already added 9: Add motion primitive as an edge to the A* graph 10: else continue 11: return error The similarity to the pseudo-code of the A* search algorithm (see Algorithm 1) is not surprising as we consider this algorithm to be an augmented version of the classic A*. Both algorithms begin with the initialization. The initial state, consisting of position orientation and a dynamic state of the ship in the form of a trim primitive, becomes the root node of the A* graph and the first element in the list of unvisited nodes. We then enter a while-loop, which begins with the extraction of the node with the cheapest combined cost. The combined cost consists of the energy spent to reach the current node, denoted as actual cost, and the Euclidean distance to the final position, as a heuristic underestimation for the remaining future costs. In our implementation, the final orientation is unrestricted. If the cheapest node is also the final node, then the optimality property of the A* ensures that we can return successfully. For most nodes, this test fails, and we continue with the loop. The next step is to explore the search space and search for the neighbours of the currently cheapest cell. For that purpose, we enter a second loop and iterate over each feasible motion primitive. We obtain the set of feasible motion primitives by mapping δ from Definition 4.1 or Definition 4.2. The execution takes place in the search space, where we begin at the current node, transcribed into a cell, and track movement of the ship. We map the current location onto the occupancy grid to monitor collisions. In the case of successful execution, the A* graph and list of unvisited nodes grow. The outer loop gets repeatedly invoked until one of two things happen. First, the list of unvisited nodes is empty, which means the algorithm was unable to find a solution to the given problem. By the completeness property of the A* algorithm, it is clear that there is no possible solution. Second, in order to assure that the programdoes not crash due to insufficient memory, if the overall number of nodes reaches a maximal value the program is stopped.
For the numerical examples in the following section, a D*-software (i.e. a dynamic A*) implementation for planning with primitives provided by Marin Kobilarov, Autonomous Systems, Control and Optimization (ASCO) Laboratory, Johns Hopkins University, available at https://github.com/jhu-asco/dsl was adapted.

Post-optimization
As shown in detail in the construction process in Sections 3 and 4, a resulting sequence of motion primitives is admissible to the (nonlinear) ship model and the given constraints as encoded in the occupancy grid. Moreover, A* provides an optimal sequence with respect to the objective function used within the planning. However, the search space of A* is only an approximation of the ship model state space. For that reason, we have not found an optimal solution to the following optimal control problem (OCP), yet, For the cost functional Jðx;ũÞ, the following criteria can be considered, for instance, which corresponds to 1) control effort, 2) duration, 2 and 3) goal state stabilization with weights w 1;2;3 and G used to denote a single goal state in Definition 4.3.
In order to solve the OCP, we employ a direct transcription method: Based on a timediscretization (not to be mixed up with the state-space discretizations used before) for the trajectory x and control curve ũ, we transform the OCP into a nonlinear constrained optimization problem (NLP) of typically high, but finite dimension. For details on this standard approach in optimal control, we confer to the textbook [45], for instance. The resulting NLP is typically high-dimensional but possess sparsity-structure in the Jacobians [9],, and should therefore be addressed by sparsity-exploiting NLP solvers such as WORHP [7] and a corresponding variant for optimal control problems called TransWORHP [9]. However, all local NLP solvers have in common that they require initial guesses and, moreover, performance and successful termination highly depend on sophisticated choices of initial guesses. Thus, we use the NLP solver WORHP for solving the OCP based on the motion primitive sequences as initial guesses. This idea of how to implement the OCP for motion primitive post-optimization is described in detail in [11,12]. In these works, the sequence was an admissible solution to the OCP. Due to the occupancy grid discretization, this only holds up to grid cell coarseness when using Algorithm 2. Note that using an already sophisticated feasible initial guess the occupancy-grid constraints do not pose as big a challenge for the NLP solver as it would without the prior A* step.
Besides faster and more robust convergence of the optimization, the quality of the resulting optimum is also important. While steered convergence to preferred local optima is quite challenging and a topic of current research, providing motion primitive sequences that are admissible to the OCP makes it likely to find an optimal solution in the vicinity, i.e. with qualitatively similar dynamic behaviour. In the following chapter, we present several scenarios for ship manoeuvring which strongly benefit from the two-step approach and exactly reflect the expected behaviour of the post-optimization as a local correction and optimization technique.

Numerical results
We will now illustrate the performance of the presented algorithm. As a preliminary step, we apply the A* algorithm to the kinematic ship model, before we show the whole methodology for a real scenario at the port of Rostock. Note that directly optimizing the OCP without first gaining an initial A* solution is not feasible as the optimization methods either take too long to reach an optimal solution or outright fail to converge.

A*-motion planning with primitives
Our first numerical example illustrates the concatenation of motion primitives. We consider the kinematic ship model introduced in (1)-(3) with trim primitives computed as detailed out in Section 3 and with the manoeuvre automaton defined in Definition 4.1. An example scenario is created as depicted in Figure 5. The ship is required to perform a turn manoeuvre, which cannot be done as turning on the spot due to its minimal turning radius. Start point and desired final point coincide at ðx; yÞ ¼ ð5:0; 6:0Þ. The hybrid A* algorithm computes the optimal sequence of motion primitives that gives a solution to the turning task. For illustration, we draw the different types of trim primitives in blue (straight ahead), yellow (right turn), and purple (left turn) in Figure 5. Note that the trim primitives are stored with a fixed duration, since the A* algorithm requires a full discretization of the continuous-time problem. However, the same trim can be concatenated several times for longer straight or turning phases. The sequence of motion primitives is an approximation to the optimal solution of the original problem with full nonlinear dynamics. A larger library with trims of different turning radii, various speed, and shorter durations would lead to better results -at the cost of high computational effort. Alternatively, we propose to perform a post-processing step via an optimal control problem as discussed in Section 4.3.

Autonomous ship navigation in the port of Rostock
In this section, we apply the algorithm to large ships in the port of Rostock, see Figure 6. We show in three examples that the motion primitive algorithm enables the postoptimization to find optimal trajectories in a narrow environment. In all three cases the automaton given by Definition 4.2 was used.
In the first experiment, we start with a long trajectory, see Figure 7 with parameters given in Table 1. The starting point lies outside of the port in the Baltic sea and the final position roughly 12 km away in the city port of Rostock. One can notice that the algorithm did not take many explorations to connect the starting point with the end point. Most of the visited positions are close to the start or to the end point, which is a consequence of the geometry of the port and the fact that a specific cell must be reached. The cluster around Figure 6. Map of the port of Rostock which is used as a testing environment. Map data copyrighted OpenStreetMap contributors and available from https://www.openstreetmap.org, see [46]. the starting point is due to the narrow entry of the port, so the algorithm explores this area in more detail. One observes cone-shaped objects on both sides of the starting point, which we assume to be caused by backward search within in the algorithm (cf. Section 4).
The next cluster is at the two berths, roughly at ð1000m; À 3000mÞ. Here, the algorithm gets stuck for a few explorations steps as these dead ends are on the connecting line between the harbour entry to the end point. Therefore, these positions have a small cost associated with. It takes the algorithm only few explorations to make good progress in the narrow canal starting at ð0m; À 2000mÞ to ð0m; À 8000mÞ. The final cluster is around the final position, which is not surprising since the algorithm has to reach a very small cell (225m 2 ). As a comparison the footprint of our ship, the ferry Mecklenburg-Vorpommern, is approx. 5800m 2 . The final motion primitive solution is indicated with the green dashed line, after post-optimization we obtain the solid green line.  Recall that the ship has limited agility. In particular, it has a minimal turning radius and cannot turn on the spot. Similar to our preliminary example in Figure 5, in the next scenario, the ship has to perform a turning manoeuvre before entering the port. As one can see in Figure 8 (with parameters in Table 2), this starting condition leads to many more visited states by the A* algorithm. Due to the narrow entrance of the port, the resulting optimal sequence of primitives (depicted by the dashed red line) performs a large turning circle. The large detour is worsen due to the small-sized manoeuvre automaton (cf. Definition 4.2). To obtain a better manoeuvre, we perform the postprocessing step as proposed in Section 4.4. The result is depicted by the solid green line. One can observe that the optimized trajectory is able to minimize the detour while still finding a feasible entering path into the port.
In our last example, the ship has to perform a 180°-turn starting directly in the narrow port entrance. For this scenario, the exploration nature of the A* algorithm is particularly crucial in order to find feasible solutions. In Figure 9 (corresponding parameters in Table 3), the resulting motion primitive solution is depicted by a dashed red line again. It performs  the turning within the inner port, obeying the obstacles, i.e. Island. Again, we observe that the turning phase has a big detour due to the limited amount of primitives in the library. However, as in the previous example, the optimal solution that is obtained from the postprocessing step (solid green line) minimized the detour. Note that the optimal solution seems to violate the state constraints given by the Island in the inner port. When checking the TransWORHP output, one sees that the constraints are fulfilled at each discretization point. To obtain obstacle avoidance also at intermediate time points, a finer time discretization has to be used. This is not within the scope of our numerical analysis, though.

Conclusion and future work
This article proposes a methodology for autonomous navigation of ships based on combined mathematical techniques: (1) Modelling of ship dynamics and identifying structural properties (i.e. symmetry),  (2) Quantization of the dynamics to a finite set of motion primitives (i.e. trim primitives and manoeuvres as in [10]), (3) Modelling of environmental constraints (i.e. the port layout) via occupancy maps, (4) Graph-based planning via the A* algorithm on motion primitives, and (5) Post-optimization by solving optimal control problems using a direct method.
While, individually, these methods are well established, e.g. A* algorithms in computer science or direct optimal control with nonlinear optimization in numerical mathematics, their combination and application to specific tasks in autonomous control is less well investigated. Thus, the presented case study of ship navigation via graph-based motion planning provides a helpful validation of the approach. The second and third scenarios in Section 5 show the advantages if planning and nonlinear optimization are combined: the A* algorithm performs a global search but provides a primitive sequence, which is only suboptimal w.r.t. the optimization criteria since it does not use the full dynamics. Contrarily, the direct optimal control method considers the nonlinear ship model, but requires a good initial guess for the nonlinear optimizer to converge to a globally efficient solution. As pointed out in the introduction, the navigation of ships in comparison to street or aeronautical vehicles has particular challenges due to limited agility of large ships and narrow, possibly congested space in a port. Since the agility and dynamics of ships are limited compared to those of road vehicles, for instance, computation times for real-world scenarios are less of a problem. Thus, autonomous navigation of ship in ports seem to be realistic in near future. Moreover, current research in GALILEOnautic [4,5,6] addresses the interaction of several ships.
Future work shall address robustness analyses of the computed solutions w.r.t. uncertainty in states and model parameters, as well as the design of a feedback control. In [38], for instance, model predictive control is designed using a motion primitive library and stability of this control scheme is shown for an academic robot example.

Notes
1. Since this article focuses on a proof-of-concept for the proposed method and does not include a real-world application, we omit a discussion whether a future version of this method would technically be considered as an assisted, an automated or an autonomous navigation system.