In autonomous robotics, path planning is a central problem. The objective of path planning is often to find a safe path from the agent’s initial position to a target destination. In the marine industry, research on autonomous ships is gaining increasing attention, as a next evolutionary step in ship technology and operations. Conflict resolution, including how to find a collision-free path, is a main issue to be addressed [huang2020ship] to achieve safe ship navigation and maneuvering. To find a path to guide the vessel to its targets dynamically, the path-planning method should be able to characterize and update the surrounding terrain, obstacles, and any other relevant exterior information. A model incorporating such information, on which a path or maneuver can be planned, is called a guidance model hereafter.
An abundance of path-planning methods are reviewed in [huang2019collision, patle2019review, mac2016heuristic]
. The A-star (A*) algorithm is a widely used search-based method. It evaluates a grid point by using a cost function which is comprised of two parts, i.e., the path cost from the starting point to the grid point and the estimated cost from the grid point to the target point[cormen2009introduction]. An optimal path is guaranteed if the estimated cost is admissible, which means it never overestimates the actual cost from the grid point to the target point. The artificial potential field (APF) method [khatib1986real] is another classical guidance model for navigation, widely applied for real-time collision-free path planning [ge2002dynamic, zhao2006robot, yin2008improved, huang2009velocity, tan2010navigation, chiang2015path, chen2016uav, lyu2017ship]. In this method, the workspace of the vessel is represented by an artificial potential field where the target attracts the vessel and the obstacles repulse the vessel. An advantage of the APF method is that it can easily be modified to incorporate more information, such as velocity [ge2002dynamic], acceleration [yin2008improved], and navigation rules [lyu2017ship]. However, a vessel can be trapped in a local extremum and not be able to reach its target. The problem of local extrema can for instance be solved by changing the direction of the repulsive force [li2012efficient] or using the wall-following technique [yun1997wall]. However, the path found by the abovementioned methods might not be suitable for marine vessels. A sufficiently smooth path is desired, typically requiring that the three first derivatives are continuous and bounded [skjetne2005maneuvering]. The search-based methods plan in discretized space, resulting in non-smooth paths, possibly generating infeasible or impractical direction changes. Moreover, a set of preset rules, such as the International Regulations for Preventing Collisions at Sea (COLREGs) [COLREGs], should be considered to guide path planning and collision avoidance.
The stream function, also called the potential flow, is a model derived from hydrodynamics and can be applied in path planning [milne1996theoretical, axler2013harmonic, currie2016fundamental]
. Stream functions for path planning are a subset of the APF methods, since they generate a vector field for the vessel to follow. The basic idea of this method is to model a stream of fluid flowing around circular obstacles to obtain the trajectory of fluid particles, called streamlines. Since streamlines are collision-free paths, modeling the flow based on potential flow theory enables one to find a safe and smooth path[waydo2003vehicle, ye2005coordinated, hu2007autonomous, pedersen2012marine]. This method is computationally efficient, and yet it extends well to complex situations by applying potential flow theory. Existing works focus on static obstacles; see [kim2011stream, pedersen2012marine]. The collision avoidance solution in existing works is to simply follow the streamlines; however, these can be dynamically infeasible for marine vessels. Besides, there are few attempts to apply the stream function in dynamically changing environments.
In recent years, some existing navigation rules have been used to find a rule-compliant collision-free solution; for example [perera2012intelligent, kuwata2013safe, praczyk2015neural, johansen2016ship, lyu2017ship, lyu2019colregs]. Often, these methods have poor performance in congested waters and do not accurately take the vessel dynamics into account.
In this paper, we propose a stepwise (recursive) path planning method based on a guidance model derived from the stream function. Waypoints are determined in a stepwise manner, using the recursively updated stream function incorporating the latest surrounding traffic and workspace information. Unlike the existing work, we use stream functions as an intermediate decision model for waypoint selection and do not force the vessel to follow the streamline directly. By adding a component called vortex flow [milne1996theoretical, axler2013harmonic, currie2016fundamental], we demonstrate that the proposed method can be applied in dynamic environments with multiple moving obstacles. Using vortex flows to modify the stream function method also enables the ship to comply with COLREGs rules (for some important rules which can be deterministically interpreted) in more dense and complex marine traffic. To illustrate how the the path-planning algorithm works, a path is generated from the waypoints using Bézier curves, and a path-following controller is designed to make the ship move along the generated path.
The rest of the paper is organized as follows. Section 2 formulates the collision-avoidance problem. Section 3 describes the stream function method for waypoint generation. Section 4 presents the path-generation method. Section 5 presents the simulation results. Section 6 concludes the paper with directions for future work.
Notation: The set of real numbers is . A curve with continuous and bounded derivatives is denoted by . The Euclidean vector norm is . Total time derivatives of up to order are denoted , , , , . Partial differentiation is denoted by a superscript: , , , etc.
Ii Problem formulation
Ii-a System description
The proposed system in this paper is illustrated in Figure 1. The path planning module determines discrete waypoints , among a set of candidate points, to be used by the path generation module to generate the desired path with path parametrization variable to determine the desired position along the path segment . The maneuvering controller [skjetne2005maneuvering]
calculates the desired forces and momentsto track the desired path and path speed.
There are several obstacles in the workspace. We assume each obstacle moves with a constant speed and heading (i.e., not following COLREGs), and the positions and velocities of the vessel, target, and obstacles are known. The proposed control system aims to guide and control the vessel to the target position with collision avoidance and compliance to regulations. Three rules from COLREGs [COLREGs] are selected to assess the proposed system:
Rule 13 Overtaking: Any vessel overtaking any other shall keep out of the way of the vessel being overtaken.
Rule 14 Head-on situation: When two power-driven vessels are meeting on reciprocal or nearly reciprocal courses so as to involve risk of collision, each shall alter her course to starboard so that each shall pass on the port side of the other.
Rule 15 Crossing situation: When two power-driven vessels are crossing so as to involve risk of collision, the vessel which has the other on her own starboard side shall keep out of the way and shall, if the circumstances of the case admit, avoid crossing ahead of the other vessel.
Ii-B Workspace representation
The workspace is represented by a discrete grid in the North-East (NE) reference frame. An example is shown in Figure 2. The size of the workspace is . We place equidistant horizontal rows with an interval of in the -direction. Each row has equidistant discrete points with an interval of in the -direction. This results in grid points in total. We define the set of grid point as
where and are index sets.
There is a number of obstacles, and only one target in the workspace. Each obstacle is represented by a circular domain with radius , where . The center of each obstacle is assumed to be located at a grid point. It is also assumed that all obstacles are distributed without overlapping, and that the positions and velocities of the obstacles are known.
We will later refer to our vessel as ‘own ship’ (OS). If an obstacle is a COLREG-compliant motor-powered ship, we will refer to this as a ‘target ship’ (TS); otherwise, it is only referred to as an obstacle.
Ii-C Problem statement
Given the positions of the target and obstacles, the path planning module aims to find a set of discrete waypoints to guide the vessel towards its target position while avoiding collision. The path should comply with rules 13-15 from COLREGs. Generally, the position information of the target and obstacles is incorporated into the path planning method.
In the workspace information, it is assumed that the locations of the target and obstacles are on the grid points. Their positions are denoted by and , respectively. The corresponding velocity of the obstacle is , . The position of the current waypoint , where is the index of the waypoint. The path planner module is designed to find the next waypoint to generate a new path segment with sufficient continuity at the waypoints. Since the obstacles are moving, the path planner module should update the workspace information and find the next waypoint using the latest information. Once the vessel approaches the next waypoint, the path planner module can use the latest workspace information to determine the new waypoint. This procedure shall be done iteratively until the vessel reaches the target.
Iii Stream function guidance model
The proposed guidance model for path-planning is inspired by the stream function, which is widely applied in hydrodynamics. In this section, some important concepts from hydrodynamics are introduced [milne1996theoretical, axler2013harmonic, currie2016fundamental]. Based on these concepts, the workspace is characterized and the algorithm to recursively generate the waypoints using a stream function is presented.
Iii-a Potential flows and complex potential
A 3D flow is irrotational if the vorticity vector is zero everywhere in the fluid, which is given by
where denotes the gradient with respect to Cartesian coordinates, means cross product, and is the velocity vector of the flow. In a 2D flow where , eq. (2) is simplified to be
For any scalar velocity potential function , we have
The irrotational flow fields are called the potential flows, represented by . The velocity of the flow can be calculated from a velocity potential,
The velocity potential satisfies Laplace’s equation, , since an ideal flow must satisfy the condition of continuity in Cartesian coordinates, i.e., . This condition is expressed as
A stream function is defined such that
Then a stream function also satisfies Laplace’s equation by definition, i.e., . A streamline is the line along a constant value of . Physically, a streamline is the trajectory of a fluid particle as it moves in the flow. By combining (5) and (7), the velocity potential and the stream function satisfy the Cauchy-Riemann equation [riemann1867grundlagen], i.e.,
With the velocity potential and the stream function, we can now define an irrotational and inviscid 2D flow as
where with as the imaginary unit. We can see that the real part of is the velocity potential and the imaginary part is the stream function. The uniform flow, sink, source, and vortex are the most important flow types.
For a uniform flow in the -direction with , the complex potential is given by
The complex potentials for sources and sinks are
where and denotes sources and sinks, respectively.
The complex potential for a vortex is
where and indicate the fluids rotating around the vortex center in counterclockwise and clockwise directions, respectively. An example is given in Figure 3.
Iii-B Obstacle representation
Iii-B1 Circle theorem
The circle theorem is adopted to represent a circular obstacle in the flow. The theorem is valid for an obstacle at an arbitrary position. By applying the theorem, the boundary condition on the obstacle is satisfied, i.e., is constant along the boundary of the obstacle.
Theorem 1 (Circle theorem [milne1996theoretical]).
Let there be an irrotational two-dimensional flow of incompressible inviscid fluid in the -plane. Let there be no rigid boundaries and let the complex potential of the flow be , where the singularities of are all at a distance greater than from the point in the -plane. Let and be the conjugate function of and , respectively. If a circular cylinder, typified by its cross-section , is introduced into the flow, the complex potential becomes
By applying the circular theorem, we can obtain the stream function with one obstacle in a basic flow. For example, using equations (10) and (12), the stream function for a circular obstacle centered at the origin in a uniform flow is given by
Iii-B2 Multiple obstacles
In the flow field, if there are multiple circular obstacles, the Laplace’s equation with multiple boundary conditions must be solved to obtain the stream function. However, this is analytically impossible [waydo2003vehicle]. In the case of multiple obstacles, a method called addition and thresholding is applied [waydo2003vehicle, sullivan2003using].
The basic idea of addition and thresholding is that each obstacle has its influence area. The stream function at a grid point is influenced by the obstacles whose distance to is less than or equal to a range , i.e., , , with . An example is shown in Figure 5.
To apply this method in an environment with obstacles, we assume that is the stream function generated by only considering the th obstacle in the sink flow. The stream function of multiple fixed obstacles in the whole workspace is given by
This method removes the influence of those obstacles that should not have a direct impact on the streamlines, while the influences of the other obstacles are added together to create the vector field.
Iii-B3 Collision avoidance of multiple moving obstacles
For the avoidance of multiple moving obstacles, additional vortex flows with centers at are added to modify the original stream function. For all such that , the stream function of a vortex flow is constant, which implies that the boundary conditions on the obstacles can be satisfied.
The stream function of a vortex flow is
where is a tuning parameter that combiend with represents the strength of the vortex flow, is a signed function determining the direction of the vortex, given by
where is the angle between the vectors and , is the unit vector pointing from the current waypoint to the target position, and is the unit vector with the direction of the obstacle’s velocity. This is illustrated in Figure 4.
The function is used to assign a port or starboard turn to pass obstacles. Combined with a proper logic, this enables COLREGs compliance. When nearly aligns with , which indicates a head-on or overtaking situation, a counterclockwise vortex is added that forces the vessel (own ship; OS) to move towards its starboard. In a crossing situation, the obstacle might cross from either the vessel’s starboard side or port side. When the obstacle crosses from the starboard side,
and a counterclockwise vortex is generated, so that the vessel can give way to the obstacle. If the obstacle crosses from the port side, which is classified asin Eq. (18), and a clockwise vortex is generated. If the obstacle complies with COLREGs rules (e.g., the obstacle is a target ship), then is set to be 1, without further modification, to make the vessel stand on its course with respect to the obstacle. More sophisticated logic for COLREGs compliance can be considered for implementation, using for instance the classification proposed in [thyri2020reactive, eriksen2020hybrid].
The resulting stream function for the whole workspace becomes
where has the same definition as in Eq. (16).
By adding vortex flows with well-tuned , the streamlines close to the obstacles will be moved towards the opposite side of the direction of motion of the obstacle. A comparison between the flow field with and without adding the vortex flows is shown in Figure 5. In this way, using a stream function updated recursively gives collision avoidance in dynamic environments. The streamlines far away from the obstacles are only slightly influenced by the vortex flow. This ensures that a vessel following a streamline only performs collision avoidance when it gets close to an obstacle; otherwise it will move directly towards its target, avoiding unnecessary turning and extra fuel consumption.
The algorithm to generate the stream function is summarized in Algorithm 1.
Iii-C Optimal waypoint selection
Following a constant streamline , a vessel can eventually reach its target. However, since the workspace is discretized by a set of grid points, it is difficult to choose the next waypoint with the same value of as the current waypoint . Instead, we choose a next waypoint close to the current waypoint with closest to .
When the current waypoint and the target are far apart, illustrated in Figure 6, the next waypoint is chosen as the solution of the optimization problem,
where denotes set boundary, is a tuning parameter that should be sufficiently small, and the integer is a tuning parameter that reflects the search range of the candidate waypoints.
The vessel following a streamline moves toward the target, by minimizing the defined cost function (20a). To avoid the vessel being guided away from the target, the term is added and penalized, since the streamlines do not give any information about the moving direction. An example is shown in Figure 7. Instead of moving towards the target, the vessel may be guided away from the target if we only minimize . The candidate waypoints are in the set , which are defined in Eqs. (1) and (20c). An example is shown in Figure 6. In order to avoid choosing a waypoint very close to the current waypoint, Eq. (20c) ensures that all candidate waypoints are sufficiently far from the current waypoint according to the -multiple of distances and .
The algorithm is summarized as follows in Algorithm 2, which provides a discrete approximation of the streamlines. While streamlines are guaranteed to provide an obstacle-free path, the discretized approximation is not guaranteed to be obstacle-free. For this reason, the value of should be reasonably selected and not too large.
Iv Path generation with stepwise concatenation of path segments
To implement the proposed guidance model, we integrate it with a path generation module and a path-following controller. In this section, the path generation method using a Bézier basis is presented, adopted from [Mag2020thesis].
Iv-a Path generation
The scenario is this: The vessel is moving on a path segment towards waypoint when the next waypoint is determined. The path segment leading to (and through) is already fixed, implying that also the path derivatives are defined when approaching . The problem of path generation is then to construct a feasible path segment from to , while ensuring it is sufficiently smooth at . A path with continuous and bounded third derivatives will be required in the control design [skjetne2005maneuvering]. A desired path segment in the 2D plane can be represented by
where we use as the path parameter.
Iv-A1 The Bézier curve
A Bézier curve of th degree is defined by control points , where . The first and the last control points define the end points of the curve segment. A Bézier curve segment parameterized by can be expressed as a linear combination of the control points [farin2014curves], i.e.,
and is given by [joy2000matrix]
with (!) being the factorial operator.
The derivative of an th order Bézier curve is still a Bézier curve but of order , given by
Iv-A2 Optimal control points
To stepwise generate a feasible path from current to next waypoint, we follow the procedure proposed in [Mag2020thesis]. The path is constrained to lie within a corridor. Within the corridor, the path with minimum length without exceeding the maximum allowed curvature is obtained by an optimization problem. Assuming that we have three waypoints, i.e., the previous , the present , and the next in the NE frame. The path should have continuous third derivative, i.e., at the waypoints. To satisfy this requirement, we choose the degree of a Bézier curve as , which means there are at least 8 control points for each path segment [Mag2020thesis].
The path segment from to is generated by a set of control points , where . For (the first path segment), the control points are placed along the straight line from to such that
where is the radius of a ball embedded within corridors with width at waypoints and .
Consider now the path from to for . Then the path segment is obtained by solving an optimization problem. When reaching , the vessel heading should be equal to the path angle . This is achieved by placing , , and on the straight-line from to , as shown in Figure 8. Also, , , and are uniquely decided by , , , and , whereas and are the end points of the path segment from to . Therefore, , , and are selected to satisfy
where the locations of , are to be decided.
There are six decision variables to be optimized, i.e., the - and -axes of the control points , , and in the NE frame. Since these control points are located on the straight-line path from to , the decision variables can be reduced by expressing , , and in a path-fixed reference frame, which is rotated by the path angle with respect to the NE frame. In the path-fixed reference frame , the decision variables are only the -coordinates .
The objective is to minimize the arc length, i.e.,
which is simplified to be [Mag2020thesis]
The constraints of this optimization problem are introduced as follows. First, in Figure 8, we notice that , , , , , , and are arranged in order along the line through and . Similarly, the constraint on is
Besides, the path segment is designed to stay inside a corridor from to with a width , as illustrated in Figure 9. This is achieved by
where is the radius of a ball embedded within corridors and at waypoint .
In addition, the curvature should remain small, which according to [Mag2020thesis] is ensured by
where is a small tuning parameter.
V Path-following control
V-a Vessel model
The vessel is assumed to be fully actuated with motion typically at low speed so that position and heading are simultaneously controllable. Environmental disturbances are not considered. The vessel is represented by a 3 degree of freedom (DOF) maneuvering model[skjetne2020survey], with motion restricted to the horizontal plane. The ship heading is referred to as yaw, while longitudinal and lateral motion is referred to as surge and sway, respectively. The control design model is then given by
where is the ship position in a local North-East Earth-fixed reference frame with origin , is the surge and sway velocity vector in the body-fixed frame with origin , is the vessel heading/yaw, is the yaw rate, and . is the thrust load vector, is the rotation matrix, is the inertia matrix, is the coriolis and centripetal matrix, and is the linear damping matrix.
V-B The maneuvering control problem
The path-following control is solved as a maneuvering problem [skjetne2005maneuvering]. The task of position tracking and heading control is separated in order for the path parameter to only allow feedback from the position state.
V-B1 Desired path
Let be an overall path parameter, such that corresponds to for . Let then
where is the floor operator.
Given path segment , we define the desired path as
which by the above procedure is guaranteed to be .
V-B2 Control objective
The control objective for the position and velocity is to satisfy a geometric task and a dynamic task as follows:
Geometric task: Force the horizontal position to converge to and track the desired position , i.e.,
where and is the path parameter.
Dynamic task: Force the path speed to converge to a desired velocity , i.e.,
The speed assignment is designed to ensure a constant desired speed along the path. This is achived by
For the heading, the objective is to maintain the direction of the tangent vector along the path, that is, where
In the maneuvering control design, the desired parametrized position is available from the path generation module, also generating the relevant derivatives with respect to , that is, and . Assuming , the heading references are correspondingly generated from (45) by
V-C Backstepping maneuvering control design
We assume that the full state feedback is available for the control design model in (38). A backstepping design will result in our proposed control law, giving a cascade structure in the error states [arcak2001redesign, sorensen2020]. Towards this end, the state is transformed by the error signals as , , , , and , where
For the kinematics, this results in
where . Let the first step control Lyapunov function be , giving