2.1 Vessel model

Let $\boldsymbol{\eta }(t )= [{x,y,\varphi } ]$ be the state of vessel, which contains the $x,y$ coordinates and heading ($\varphi$) in the global reference frame. Let $\boldsymbol{v}(t )= [{u,v,r} ]$ be the velocity state, which contains the linear speed ($u,v$) and rate of turning ($r$) in the body-fixed reference frame. The 3-degree of freedom (DoF) marine craft equations of motion can be introduced as follows:

(1)\begin{gather}\dot{\boldsymbol{\eta }}(t )= \boldsymbol{T}({\boldsymbol{\eta }(t )} )\,\cdot\, \boldsymbol{v}(t )\end{gather}

(2)\begin{gather}\boldsymbol{\; M} \,\cdot\, \dot{\boldsymbol{v}}(t )+ \boldsymbol{D} \,\cdot\, \boldsymbol{v}(t )= \boldsymbol{\tau }(t )+ {\boldsymbol{\tau }_\textrm{e}}\end{gather}

(3)\begin{gather}\boldsymbol{T}({\boldsymbol{\eta }(t )} )= \left[ {\begin{array}{*{20}{c}} {\cos \varphi }& { - \sin \varphi }& 0\\ {\sin \varphi }& {\cos \varphi }& 0\\ 0& 0& 1 \end{array}} \right]\end{gather}

Here, $\boldsymbol{M}$ and $\boldsymbol{D}$ denote the time-invariant mass and damping matrix. The element of matrix $\boldsymbol{M}$ contain the mass, the moment of inertia and added mass. The linear drag coefficients and nonlinear drag coefficients are considered in the damping matrix $\boldsymbol{D}$. More detailed description of the elements of these matrices can be found in Fossen (Reference Fossen2011). $\boldsymbol{\tau }(t )$ and ${\boldsymbol{\tau }_\textrm{e}}$ indicate the propulsive force and environmental load on the marine craft. Let $\boldsymbol{x}(t )= [{\boldsymbol{\eta }(t ),\, \boldsymbol{v}(t )} ]$ be the state vector, the state dynamics are given by,

(4)\begin{equation}\dot{\boldsymbol{x}}(t )= {A_c} \,\cdot\, \boldsymbol{x}(t )+ {B_c} \,\cdot\, \boldsymbol{u}(t )+ \boldsymbol{\omega }(t )\end{equation}

For a vessel,

\begin{align*}\boldsymbol{u}(t )& = \boldsymbol{\tau }(t ),\quad \boldsymbol{\omega }(t )= {\boldsymbol{\tau }_\textrm{e}}\\ {A_c} & = \left[ {\; \begin{array}{*{20}{c}} 0& {\boldsymbol{T}({\boldsymbol{\eta }(t )} )}\\ 0& { - {\boldsymbol{M}^{ - 1}}\boldsymbol{D}} \end{array}\; } \right],\quad \; {B_c} = \left[ {\; \begin{array}{*{20}{c}} 0\\ {{\boldsymbol{M}^{ - 1}}} \end{array}\; } \right] \end{align*}

Let $\boldsymbol{\tau }(k )= \boldsymbol{\tau }({{t_k}} ),\boldsymbol{\; x}(k )= \boldsymbol{x}({{t_k}} ),\boldsymbol{\; x}({k + 1} )= \boldsymbol{x}({{t_{k + 1}}} )$, where ${t_k}$ denotes the discrete time. Then the discrete-time state space model can be described as follows:

(5)\begin{align}\boldsymbol{x}({k + 1} )= \boldsymbol{A} \,\cdot\, \boldsymbol{x}(k )+ \boldsymbol{B} \,\cdot\, \boldsymbol{\tau }(k )\nonumber\\ \boldsymbol{A} = \boldsymbol{I} + \Delta t \,\cdot\, {A_c},\quad \boldsymbol{B} = \Delta t \,\cdot\, {B_c}\end{align}

where the time duration $\Delta t = {t_{k + 1}} - {t_k}$. Such a discrete–time state space model enables us to perform the real-time Kalman filtering (Bishop and Welch, Reference Bishop and Welch2001).

2.2 Collision risk assessment

In this study, collision risk is assessed using the closest point of approach (CPA) and time to closest point of approach (TCPA). The ${d_{\textrm{CPA}}}$ (see Figure 2) is used to describe the closest distance if the own ship and target ship maintain course and speed. A small ${d_{\textrm{CPA}}}$ means the two ships will collide. Let $[{{x_0},\; {y_0}} ]$ be the initial position of the own ship, $[{v_x},\; {v_y}$] denote the velocity vector of the own ship, and the position of the own ship can be given by:

\[x(t )= {v_x}t + {x_0}\]

\[y(t )= {v_y}t + {y_0}\, \]

Figure 2. CPA and TCPA

Let $[{{{\hat{x}}_0},\; {{\hat{y}}_0}} ]$ and $[{\hat{v}_x},\; {\hat{v}_y}$] be the initial position and velocity of target ship:

\[\hat{x}(t )= {\hat{v}_x}t + {\hat{x}_0}\]

\[\hat{y}(t )= {\hat{v}_y}t + {\hat{y}_0}\, \]

The distance between the own ship and target ship can be computed:

\[{D^2} = {({x(t )- \hat{x}(t )} )^2} + {({y(t )- \hat{y}(t )} )^2}\]

The time derivative of ${D^2}$:

\[\frac{{d{D^2}}}{{dt}} = ({({{v_x} - {{\hat{v}}_x}} )t + ({{x_0} - {{\hat{x}}_0}} )} )({{v_x} - {{\hat{v}}_x}} )+ ({({{v_y} - {{\hat{v}}_y}} )t + ({{y_0} - {{\hat{y}}_0}} )} )({{v_y} - {{\hat{v}}_y}} )= 0\]

With such information, we can compute the position $[{{x_{\textrm{CPA}}},\; {y_{\textrm{CPA}}}} ]$ of target ship at ${t_{\textrm{CPA}}}$(TCPA):

(6)\begin{gather}{t_{\textrm{CPA}}} = \textrm{max}\left( {0,\frac{{({{x_0} - {{\hat{x}}_0}} )({{v_x} - {{\hat{v}}_x}} )+ ({{y_0} - {{\hat{y}}_0}} )({{v_y} - {{\hat{v}}_y}} )}}{{{{({{v_x} - {{\hat{v}}_x}} )}^2} + {{({{v_y} - {{\hat{v}}_y}} )}^2}}}\; } \right)\end{gather}

(7)\begin{gather}\; \; {x_{\textrm{CPA}}} = {\hat{v}_x}{t_{\textrm{CPA}}} + {\hat{x}_0},\quad {y_{\textrm{CPA}}} = {\hat{v}_y}{t_{\textrm{CPA}}} + {\hat{y}_0}\; \end{gather}

The collision risk can be evaluated based on these formulas, as well as the relative distance, velocity and azimuth of target ship measured by the mmWave radar.

2.3 Frenet frame

The trajectory tracking and planning of ASVs forces a vehicle to reach and follow a time-parameterised geometric path (i.e. orientation, curvature), namely the reference line. Such a reference line can be represented in different forms (e.g. spline, Euler spiral, etc.). However, the vehicle will adjust the motion behaviour for obstacle avoidance. Instead, such behaviours cannot follow through the reference line exactly and thus result in the trajectory replanning.

The Frenet frame (Frenet, Reference Frenet1852) introduces a novel way to plan trajectories to manoeuvre a robot. It is more intuitive for Frenet frame to represent a vehicle's motion than the traditional Cartesian coordinate. With the Frenet coordinate (see Figure 3), we use the variables $s(t )$ to represent the arclength along which the vehicle has moved on the reference line in time t, and $d(t )$ to describe the perpendicular offset with respect to the reference line. These variables $s(t )$ and $d(t )$ are also known as longitudinal and lateral displacement, respectively. And the Cartesian coordinate of the resulting trajectory $\boldsymbol{x}({s(t ),\; d(t )} )$ can be given by the normal and tangential vector ${\boldsymbol{n}_{\boldsymbol{r}}},\boldsymbol{\; }{\boldsymbol{t}_{\boldsymbol{r}}}$ at a certain point on the reference line:

(8)\begin{equation}\boldsymbol{x}({s(t ),\; d(t )} )= \boldsymbol{r}({s(t )} )+ d(t )\; \,\cdot\, {\boldsymbol{n}_{\boldsymbol{r}}}({s(t )} )\end{equation}

where the vectors ${\boldsymbol{n}_{\boldsymbol{r}}},\boldsymbol{\; }{\boldsymbol{t}_{\boldsymbol{r}}}$ are defined at the projection of the vehicle onto the reference line, and$\boldsymbol{\; r}({s(t )} )$ denotes the Cartesian coordinate of such a projection. ${\boldsymbol{n}_{\boldsymbol{x}}},\boldsymbol{\; }{\boldsymbol{t}_{\boldsymbol{x}}}$ are normal and tangential vectors of the resulting trajectory. Since the resulting trajectory and reference line provide the tracking reference for the vehicle's controller, higher-order parameters must be given: the speed v, the orientation $\theta$, the curvature $\kappa$ and the acceleration a.

Figure 3. Trajectory generation based on the reference line in the Frenet frame

2.4 Coordinate transformation

The coordinate transformation between the Frenet and Cartesian frames makes it possible to perform online path planning for a vehicle. Given the motion $[{s,\dot{s},\ddot{s};d,\dot{d},\ddot{d}} ]$ or $[{s,\dot{s},\ddot{s};d,\, d^{\prime},d^{\prime\prime}} ]$ in the Frenet frame, and $[{\boldsymbol{x},{\theta_x},{\kappa_x},\, {v_x},{a_x}} ]$ in the Cartesian frame, we let $\mathop {({\,\cdot\,} )}\limits^{\dot{ }} := \frac{\partial }{{\partial t}}({\,\cdot\,} )$ be the partial derivative with respect to time and ${({\,\cdot\,} )^\prime } := \frac{\partial }{{\partial s}}({\,\cdot\,} )$ denote the partial derivative with respect to arclength. In this paper, the vehicle is assumed to travel along the reference line, excluding extreme situations. In this case, we have $1 - {\kappa _r}d > 0$ and $|{\mathrm{\Delta }\theta } |< \pi /2$ with $\mathrm{\Delta }\theta := {\theta _x} - {\theta _r}$.

Applying the Frenet-Serret formulas (Kühnel, Reference Kühnel2015):

(9)\begin{align}\frac{{\textrm{d}{\boldsymbol{n}_r}}}{{\textrm{d}s}} & ={-} {\kappa _r}{\boldsymbol{t}_r},\; \; \frac{{\textrm{d}{\boldsymbol{n}_r}}}{{\textrm{d}t}} ={-} \dot{s}{\kappa _r}{\boldsymbol{t}_r}\end{align}

(10)\begin{align}\frac{{\textrm{d}{\boldsymbol{t}_r}}}{{\textrm{d}s}} & = {\kappa _r}{\boldsymbol{n}_r},\; \; \frac{{\textrm{d}{\boldsymbol{t}_r}}}{{\textrm{d}t}} = \dot{s}{\kappa _r}{\boldsymbol{n}_r}\end{align}

Where ${\kappa _r}$ denotes the curvature of reference line:

\[{\boldsymbol{t}_r}(s )= {[{\cos {\theta_r}(s )\, \sin {\theta_r}(s )} ]^\textrm{T}}\quad {\boldsymbol{n}_r}(s )= {[{ - \sin {\theta_r}(s )\, \cos {\theta_r}(s )} ]^\textrm{T}}\]

With Equation (8) and Frenet-Serret formulas, we have:

(11)\begin{gather}d = {[{\boldsymbol{x} - \boldsymbol{r}(s )} ]^\textrm{T}} \,\cdot\, {\boldsymbol{n}_r}\end{gather}

(12)\begin{gather}\dot{d} = {[{\dot{\boldsymbol{x}} - \dot{\boldsymbol{r}}(s )} ]^\textrm{T}} \,\cdot\, {\boldsymbol{n}_r} + {[{\boldsymbol{x} - \boldsymbol{r}(s )} ]^\textrm{T}} \,\cdot\, {\dot{\boldsymbol{n}}_r} = {v_x}\sin \mathrm{\Delta }\theta \end{gather}

For the vehicle's speed ${v_x}$ on the resulting trajectory, the time derivative of $\boldsymbol{x}$ yields:

\[{v_x} = {||{\dot{\boldsymbol{x}}} ||_2} = \sqrt {{{({1 - {\kappa_r}d} )}^2}{{\dot{s}}^2} + {{\dot{d}}^2}} \]

With ${v_x}$, we calculate:

(13)\begin{equation}d^{\prime} = \frac{{\dot{d}}}{{\dot{s}}} = \frac{{{v_x}\sin \mathrm{\Delta }\theta }}{{\dot{s}}} = \sin \mathrm{\Delta }\theta \,\cdot\, \sqrt {{{({1 - {\kappa_r}d} )}^2} + {{d^{\prime}}^2}} \, \Rightarrow d^{\prime} = ({1 - {\kappa_r}d} )\tan \mathrm{\Delta }\theta \end{equation}

Where ${||\,\cdot\, ||_2}$ is the Euclidean norm. Taking the time derivative for ${({\boldsymbol{x} - \boldsymbol{r}(s )} )^\textrm{T}}{\boldsymbol{t}_{\boldsymbol{r}}} = 0$, we also have:

(14)\begin{equation}\dot{s} = \frac{{{v_x}\cos \mathrm{\Delta }\theta }}{{1 - {\kappa _r}d}}\end{equation}

Let ${s_x}$ represent the arclength of the resulting trajectory $\boldsymbol{x}$, and the curvatures ${\kappa _x} = \textrm{d}{\theta _x}/\textrm{d}{s_x}$, yields:

\[\Delta \theta ^{\prime} = \frac{{\textrm{d}({{\theta_x} - {\theta_r}} )}}{{\textrm{d}s}} = \frac{{1 - {\kappa _r}d}}{{\cos \mathrm{\Delta }\theta }}{\kappa _x} - {\kappa _r}\]

Taking derivative of Equation (13) with respect to s yields:

(15)\begin{equation}d^{\prime\prime} ={-} {({{\kappa_r}d} )^\prime }\tan \mathrm{\Delta }\theta + \frac{{1 - {\kappa _r}d}}{{{{\cos }^2}({\mathrm{\Delta }\theta } )}}\mathrm{\Delta }\theta ^{\prime}\end{equation}

The acceleration of resulting trajectory is given by:

\[{a_x} := {\dot{v}_x} = \ddot{s}\frac{{1 - {\kappa _r}d}}{{\cos \mathrm{\Delta }\theta }} + \frac{{{{\dot{s}}^2}}}{{\cos \mathrm{\Delta }\theta }}[{d^{\prime}\Delta \theta^{\prime} - {{({{\kappa_r}d} )}^\prime }} ]\]

With ${a_x}$, we can calculate the second order derivative of arclength:

(16)\begin{equation}\ddot{s} = \frac{{{a_x}\cos \mathrm{\Delta }\theta - {{\dot{s}}^2}[{d^{\prime}\mathrm{\Delta }\theta^{\prime} - {{({{\kappa_r}d} )}^\prime }} ]}}{{1 - {\kappa _r}d}}\end{equation}

The time derivative of d is given by:

(17)\begin{gather}\dot{d} = \frac{{\textrm{d}s}}{{\textrm{d}t}}\frac{\textrm{d}}{{\textrm{d}s}}d = \dot{s}d^{\prime}\; \; \; \end{gather}

(18)\begin{gather}\; \; \ddot{d} = d^{\prime\prime}{\dot{s}^2} + d^{\prime}\ddot{s}\end{gather}

2.4.1 Transform Cartesian frame to Frenet frame

Given the vehicle's motion $[{\boldsymbol{x},{\theta_x},{\kappa_x},\, {v_x},{a_x}} ]({{t_0}} )$, we need to project it onto the reference line and derive the corresponding states $[{{s_0},{{\dot{s}}_0},{{\ddot{s}}_0};{d_0},{{\dot{d}}_0},{{\ddot{d}}_0}} ]$ or $[{{s_0},{{\dot{s}}_0},{{\ddot{s}}_0};{d_0},\, {{d^{\prime}}_0},{{d^{\prime\prime}}_0}} ]$. For instance, when the vehicle is located at the starting point of the reference line, we have ${s_0} = 0,\; {d_0} = 0$ in the Frenet frame. Firstly, the arclength ${s_0}$ can be determined by:

\[{s_0} = \arg \mathop {\min }\limits_\mathrm{\sigma } {||{\boldsymbol{x} - \boldsymbol{r}(\sigma )} ||_2}\]

The numerical method for solving the previous minimum problem is trivial. As the function of the reference line is known, we can calculate more information, including the curvature ${\kappa _r},\; {\kappa ^{\prime}_r}$ and the orientation ${\theta _r}$ when the arclength ${s_0}$ is determined. With $\mathrm{\Delta }\theta = {\theta _x} - {\theta _r}$ and Equations (9)–(16), the remain variables such as ${\dot{s}_0},{\ddot{s}_0},{d_0},{\dot{d}_0},{\ddot{d}_0},\; {d^{\prime}_0},{d^{\prime\prime}_0}$ can also be computed.

2.5 Optimal trajectory generation

To reduce the computational complexity of path planning, the heuristic trajectory-generation algorithm is proposed by selecting the optimal trajectory from a discrete set of manoeuvres covering the free configuration space. When constructing the manoeuvre sets, referred to as ‘state lattice’, we consider comfort, vehicle constraints, curve smoothness and obstacle avoidance. The comfort is mathematically described by the jerk, which is defined by the rate of acceleration change with respect to time. The following theorem enables us to generate a feasible and comfortable path.

Theorem: Given the state ${P_0} = [{{p_0},{{\dot{p}}_0},\; {{\ddot{p}}_0}} ]$ at the start time ${t_0}$, and ${P_1} = [{{{\dot{p}}_1},{{\ddot{p}}_1}} ]$ at ${t_1} = {t_0} + \mathrm{\Delta }T$, a quantic polynomial is the minima of the cost function:

\[C = {k_J}{J_t} + {k_g}g({\mathrm{\Delta }T} )+ {k_h}h({{p_1}} )\]

\[{J_t}({p(t )} ) := \int_{{t_0}}^{{t_1}} {{{\dddot p}^2}(\tau )\textrm{d}\tau } \]

Where $g({\,\cdot\,} )$ and $h({\,\cdot\,} )$ are arbitrary functions and ${k_J},{k_g},{k_h} > 0$.

The proof of this theorem is given in Takahashi et al. (Reference Takahashi, Hongo, Ninomiya and Sugimoto1989). The term $\dddot p(t )$ is known as ‘jerk’ and ${J_t}({p(t )} )$ can qualitatively describe the jerk within the time interval $\Delta T$. From Werling et al. (Reference Werling, Ziegler, Kammel and Thrun2010), we know that, in the Frenet frame, the lateral $d(t )$ and longitudinal movement $s(t )$ can be determined independently for vehicles at higher speed. Thus, the state lattice could be generated by combining lateral and longitudinal trajectories. As for the jerk, $\dddot d$ and $\dddot s$ are introduced.

2.5.1 Lateral movement

Given the start state ${D_0} = [{{d_0},{{\dot{d}}_0},{{\ddot{d}}_0}} ]$, the cost function is expressed as:

\[{C_{\textrm{lat}}} = {k_J}{J_t}({d(t )} )+ {k_t}\mathrm{\Delta }T + {k_d}d_1^2\]

We let $g({\mathrm{\Delta }T} )= \mathrm{\Delta }T$ and $h({{d_1}} )= d_1^2$, to penalise the slow convergence and large lateral offset. The vehicle is suggested to travel parallel to the reference line, which means ${\dot{d}_1} = {\ddot{d}_1} = 0$ at end time ${t_0} + \Delta T$. The optimal solution for Equation (19) is a quintic polynomial in the indeterminate $t$

(19)\begin{equation}d(t )= {a_5}{t^5} + {a_4}{t^4} + {a_3}{t^3} + {a_2}{t^2} + {a_1}t + {a_0}\end{equation}

Different coefficients in the end state are considered to generate a set of sufficiently covered and collision-free manoeuvres. It means the state lattice in the lateral direction ${\Pi _{lat}}$ consists of polynomials with different end conditions ${d_1}$ and $\mathrm{\Delta }T$:

\[{[{{d_1},{{\dot{d}}_1},\; {{\ddot{d}}_1},\mathrm{\Delta }T} ]_{ik}} = [{{d_i},0,0,\mathrm{\Delta }{T_k}} ]\]

2.5.2 Longitudinal movement

Usually, marine vehicle moves by either keeping a desired speed ${\dot{s}_d} = \textrm{const}$ or by changing the course. The course-changing manoeuvre can be achieved by the lateral movement that we have mentioned. For the speed-keeping behaviour, a quartic polynomial:

\[\dot{s}(t )= {a_4}{t^4} + {a_3}{t^3} + {a_2}{t^2} + {a_1}t + {a_0}\]

can be used to minimise the cost function:

(20)\begin{equation}{C_{lon}} = {k_J}{J_t}({s(t )} )+ {k_t}\mathrm{\Delta }T + {k_{\dot{s}}}{({{{\dot{s}}_1} - {{\dot{s}}_d}} )^2}\end{equation}

whose start state ${S_0} = [{{s_0},{{\dot{s}}_0},{{\ddot{s}}_0}} ]$ at ${t_0}$, and end state ${S_1} = [{{{\dot{s}}_1},\; {{\ddot{s}}_1}} ]$ at ${t_1} = {t_0} + \mathrm{\Delta }T$. The acceleration is undesirable, resulting in $\ddot{s} _1 = 0$. By slightly varying the end conditions ${\dot{s}_1}$, we can determine the coefficients for the quartic polynomial:

\[{[{{{\dot{s}}_1},\; {{\ddot{s}}_1},\mathrm{\Delta }T} ]_{jk}} = [{{{\dot{s}}_d} + \mathrm{\Delta }{{\dot{s}}_j},\, 0,\mathrm{\Delta }{T_k}} ]\]

In this case, the state lattice in the longitudinal direction ${\Pi _{\textrm{lon}}}$ is generated.

2.5.3 Combined trajectory

The state lattice is generated by taking the Cartesian product of lateral movement and longitudinal movement ${\Pi _{\textrm{lat}}} \times {\Pi _{\textrm{lon}}}$. The total cost of each trajectory in the state lattice can be computed by simple algebraic operation:

\[{C_{\textrm{tot}}} = {k_{\textrm{lat}}}{C_{\textrm{lat}}} + {k_{\textrm{lon}}}{C_{\textrm{lon}}}\]

where ${k_{\textrm{lat}}},\; {k_{\textrm{lon}}} > 0$. The state lattice is shown in Figure 4.

Figure 4. Optimal trajectory generation: blue triangle indicates static obstacles, red line indicates the reference line, pink lines indicate the state lattices, and blue dotted line indicates the best planning path

2.6 Path-following algorithm

The path-following algorithm forces an underactuated ship to follow the target path generated by the path-planning module. In this paper, the pure-pursuit algorithm is used to generate the target speed and course for the ASV. As shown in Figure 5, the look-ahead distance ${L_T}$ is a predefined value, which is related to the ship length. A point on the target path is said to be a target point if the distance between such a point and ship CoG is nearest to ${L_T}$. The target point will determine the angle error ${\theta _T}$, and a PD controller is applied to compute the thruster command based on the speed and course error. By virtue of the calculation of future course error, the pure-pursuit algorithm ensures robust and efficient tracking capability.

Figure 5. Pure pursuit algorithm: ${L_T}$ indicates the look ahead distance, ${\theta _T}$ means the angle error between target and estimated course

2.7 Framework of path-planning algorithm

Given a desired trajectory known as the ‘reference line’, the algorithm only requires the estimated motion in the Cartesian coordinate as an input to compute the output, as shown in Figure 6. The vehicle motion is mapped to the state in the Frenet frame, which enables the state lattice generation. Then we exclude the trajectories exceeding the maximum acceleration of vehicle. It is also undesirable to use the trajectory with high risk of causing a collision, on which the ship will pass close to the static obstacles or CPA of dynamic obstacles. The final step is to find the minimum cost ${C_{\textrm{tot}}}$ among the collision-free and feasible trajectories. The desired trajectory is fed to the pure-pursuit method and tracking controller.

Figure 6. Algorithm overview. The path-planning algorithm inputs a desired trajectory from route planner or an operator, and outputs the local replanning trajectory for the vehicle's controller (‘Cart2Frenet’ means the coordinate transform from Cartesian to Frenet frame, and ‘Frenet2Cart’ means the inversion)