\section{Introduction} \label{Sec:1-intro}
   
    One of the most discussed matters in the problem of automatic docking is the trajectory planning of a docking operation. Usually, a trajectory is planned through \textit{trajectory optimization}, which is a case of continuous optimal control problem (OCP). Practically a continuous OCP is discretized and transcribed into a finite-dimensional nonlinear optimization problem (NLP) via \textit{direct} methods such as \textit{shooting} method and \textit{collocation} method. In this article, the term OCP is used interchangeably with trajectory optimization.
    
   
    Trajectory planner for a docking operation is quite different from that for a general operation. The global nature of a general operation makes it possible for the trajectory to be obtained by smoothing a path in accordance to part of the ship's dynamics \citep{Vagale2021,ZHOU2020107043}. However, when planning a trajectory for a docking operation, the low speed and high safety requirement result in a more complicated dynamics and subsequently the trajectory optimization itself.
    
   
    Research on a trajectory planner for docking operation sees various efforts to balance three things: low computational cost \citep{BITAR202014488,MARTISEN-DOCK}, applicability to underactuated conventional vessel \citep{Maki2020application,maki2020pt1,MIZUNO2015305}, and feasibility with respect to the surrounding dynamic \citep{HAN2021109352,MIZUNO2015305} and geometric information \citep{BITAR202014488,LIAO201947,MARTINSEN201997,miyauchi2021planner}. This is relevant, because in practice a shipmaster will try to replan a new trajectory for docking operation if it is considered not feasible anymore. Even an experienced shipmaster might require some time to plan a new trajectory. For this reason, to realize an autonomous ship, it is necessary to design a relatively fast planner that can replan a feasible and safe trajectory, and applicable to conventional vessels. 
    
   
    This article limits the discussion within the scope of OCP-based trajectory planner that was pioneered by \cite{shouji1992automatic}. The trajectory planner introduced in \cite{Maki2020application} and \cite{miyauchi2021planner} can produce an almost-globally optimal, feasible, and safe trajectory for a conventional vessel. However, this planner requires a long computational time which makes it not possible for a real-time application. The one introduced in \cite{MIZUNO2015305} is relatively fast and also applicable to a conventional vessel, but there are no spatial constraints that can guarantee safe trajectory. On the other hand, \cite{MARTISEN-DOCK} showed a fast planner that guarantees a safe trajectory, but applicable to a fully/overly actuated vessel with a less intricate mathematical model. Still related to the discussion, \cite{HAN2021109352} presented a fast optimization-based planner for a conventional vessel, however, the ship is regarded just as a particle instead of a rigid body and the collision with the harbor's walls is avoided by expanding the harbor's boundaries.
    
   
    By exploiting the advantages of the previously mentioned works, the main intention of this article is to introduce a \textit{semionline trajectory planner} (SO-TP) that is able to plan a collision-free trajectory for an underactuated vessel under wind disturbance. The term \textit{semionline} implies the nature of the computational time that is fast enough in practice, but not strictly in real time (online). This term also implies the ability of the planner to take into account the wind dynamics measured on the vessel when the planner is executed. 
    
   
    One of the key points in the SO-TP is to use the almost-globally optimal and feasible solution from \cite{Maki2020application} as a \textit{warm start}, i.e., good initial guess in the SO-TP. This warm start is expected to give a good starting point for the optimization inside the SO-TP so that the search for the local optimum can be done faster than that without a warm start (\textit{cold start}, using any arbitrary initial guess). To evaluate the performance of the warm-started SO-TP, the following matters are discussed: 
    \begin{enumerate}
        \item The applicability of just one warm start to speed up the SO-TP for various scenarios: different initial states and wind parameters.
        \item The comparison of the resulting trajectories with and without a warm start (linear initial guess). Results from a total of 14 different scenarios are compared based on the feasibility and the computational time.
        \item The comparison of the resulting trajectories with two different warm starts: (1) obtained by the method in \cite{Maki2020application}; (2) obtained by the SO-TP itself with a linear initial guess. This is to show the necessity for a good initial guess as the warm start.
    \end{enumerate}
    
   
    The readers will find this article as a continuation and implementation of the work in \cite{Maki2020application} and \cite{miyauchi2021planner} where computational time was the biggest issue that limits their practical use. To address this issue, the first major development in this article that is unique to the previous works on OCP-based docking trajectory planners for conventional vessels is the use of the warm start to speed up the computational time. 
    
    The next major development is that this article implements a collision constraint generator via a \textit{point-in-polygon} method \citep{HORMANN2001131} that is applicable to either convex or nonconvex region. Up to the limit of the authors' knowledge, this direct implementation is the first in the field of trajectory optimization for marine use. This can be an alternative to those introduced in \cite{HAN2021109352}, \cite{LIAO201947}, and \cite{MARTISEN-DOCK}. 
 
   \begin{figure*}[tbp]
        \centering
            \includegraphics[width=0.75\textwidth]{Fig_1_Flowchart.pdf}
            \caption{Outlines of the warm-started semionline trajectory planner.} 
            \label{fig:flowchart}
    \end{figure*}
    
   
    For the remaining of the article, it is organized into another four sections. Section \ref{Sec:2-Proposed} explains the proposed scheme and the problem formulation: maneuvering model of the subject ship, brief explanation of the warm start, the SO-TP itself, and the collision constraints generator. Section \ref{Sec:3-SimulationConditions} and section \ref{Sec:4-SimRes} discuss the simulation condition and the results from several different cases, respectively. Finally, section \ref{Sec:5-ConcludeRemarks} concludes the article with additional remarks on several practical matters.

\section{Proposed Scheme and Formulation} \label{Sec:2-Proposed}
    The outlines of the proposed scheme is visualized in Fig. \ref{fig:flowchart} and can be summarized into the following steps:
    \begin{enumerate}
        \item First, optimal trajectories are generated. These optimal trajectories are termed \textit{offline trajectories} or \textit{offline solutions}, and the planner is called \textit{offline trajectory planner} (abbreviated as Off-TP). 
        \item The offline solutions can be obtained for several initial states and wind parameters. As a demonstration, this article shows how just one offline solution (without considering the wind) can be used in many scenarios.
        \item Then, the offline solution is used to warm-start the semionline trajectory planner (SO-TP).
        \item The OCP in the SO-TP is discretized and transcribed into a finite-dimensional NLP by direct Hermite-Simpson collocation method. The form of the objective function is identical to that in the Off-TP.
        \item With a \textit{point-in-polygon} method, one can check whether a point is inside or outside a polygon that defines an obstacle-free region. This results in several nonlinear and implicit spatial constraints to the NLP.
        \item Finally, the online NLP is solved by \MATLAB \lstinline{fmincon} package with SQP (\textit{sequential quadratic programming}) \citep{boggs_tolle_1995} as its main algorithm. An optimization where a good guess is provided is called a \textit{warm-started} optimization \citep{WarmStartIPForsgren}.
    \end{enumerate}
    In this article, the trajectory planner is simulated for an underactuated vessel: a 3 m \textit{Esso Osaka} 1/108 \textit{scale model} or just \textit{Esso Osaka} (the principal particulars are given in Table \ref{tab:A1-EssoOsakaPP} in the appendix). It is equipped with an electric motor that allows instantaneous mode switch and single rudder that can rotate at a rate of approximately $70$ degree/s.
    
    \subsection{Esso Osaka Maneuvering Model} \label{subSec:2.1-MMG}
        Normally a docking operation is carried out at a low surge velocity. For an underactuated conventional vessel: nonholonomic, it usually requires the dynamics for all possible combinations of surge velocity and propeller's modes to be described. For these reasons, the mathematical model of such a system can be very nonlinear and intricate.
        
        \begin{figure}[htbp]
            \centering
                \includegraphics[width=0.55\columnwidth]{Fig_2_Coordinate_system.pdf}
                \caption{Coordinate systems.} 
                \label{fig:mmgcoordinate}
        \end{figure}       
        
        Two coordinate systems are introduced: a ship-fixed coordinate system $\mathrm{o}-xy$ where midship is its origin and an earth-fixed coordinate system $\mathrm{o}_0-x_0y_0$. Angle between the two coordinate systems is the ship's yaw angle $\psi$ and $r$ denotes its angular velocity. The surge and sway velocities at the midship are denoted by $u$ and $v_{\mathrm{m}}$, respectively.
        
        The kinematics at the midship can be transformed from the ship-fixed coordinate system to the earth-fixed coordinate system by the following transformation,
        \begin{align}\label{eq:coordtransform}
            \frac{\mathrm{d}}{\dt}
            \begin{bmatrix}
                x_0 \eqskip y_0 \eqskip \psi
            \end{bmatrix} =
            \begin{bmatrix}
                \cos{\psi} & -\sin{\psi} & 0 \eqskip
                \sin{\psi} & \cos{\psi} & 0 \eqskip
                0 & 0 & 1
            \end{bmatrix}
            \begin{bmatrix}
                u \eqskip v_{\mathrm{m}} \eqskip r
            \end{bmatrix}.
        \end{align}
            
        The kinetics of Esso Osaka is based on the standard equations of motion presented by the MMG (maneuvering model group) \citep{mmgmodel} as follows:
        \begin{subequations}\label{equofmotion}
            \begin{align}
                \lpare{m+m_x}\dot{u}-\lpare{m+m_y}v_{\mathrm{m}} r-x_{\mathrm{G}} mr^2 &= X  \eqskip
                \lpare{m+m_y}\dot{v}_{\mathrm{m}}+\lpare{m+m_x}ur+x_{\mathrm{G}} m\dot{r} &= Y \eqskip
                \lpare{I_{z\mathrm{G}}+x_{\mathrm{G}}^2m+J_z}\dot{r}+x_{\mathrm{G}} m\lpare{\dot{v}_{\mathrm{m}}+ur} &= N_\mathrm{m} \text{,} 
            \end{align}
        \end{subequations}
        and can be rearranged into,
        \begin{subequations}\label{eq:rearrangedeqofmotion}
        \begin{align}
            \dot{u} &=
            \frac{X + M_y v_{\mathrm{m}} r + x_{\mathrm{G}} m r^2}{M_x} \eqskipp
            \dot{v}_{\mathrm{m}} &=
            \frac{\lpare{Y - M_x u r}I_{z\mathrm{m}} - \lpare{N_\mathrm{m} - x_{\mathrm{G}} m u r }x_{\mathrm{G}} m}{M_y I_{z\mathrm{m}}-\lpare{x_{\mathrm{G}} m}^2} \eqskipp
            \dot{r} &=
            \frac{\lpare{Y-M_x ur}x_{\mathrm{G}} m - \lpare{N_\mathrm{m} - x_{\mathrm{G}} m u r}M_y}{ \lpare{x_{\mathrm{G}} m}^2 - M_y I_{z\mathrm{m}}}\text{,}
        \end{align}
        \end{subequations}
        where simplified terms: $M_x = m + m_x$, $M_y = m + m_y$, and $I_{z\mathrm{m}}=I_{z\mathrm{G}}+x^2_{\mathrm{G}} m + J_z$ are used. Here, $m$ and $I_{z\mathrm{G}}$ denote the ship's mass and moment of inertia about center of gravity $x_{\mathrm{G}}$, respectively. Moreover, $m_x$, $m_y$, and $J_z$ denote the added mass and moment of inertia in each respective axis. 
            
        $X$ and $Y$ are the total surge and sway force, respectively, while $N_\mathrm{m}$ is the total moment about the midship. These total forces are contributions from hull forces \citep{yoshimuralow}, four-quadrant steering and thrust forces \citep{Kitagawa2015,Miyauchi2021SI,mmgmodel}, and wind forces \citep{fujiwind}. The important coefficients are given in \cite{maki2020pt1} and \cite{Miyauchi2021SI}.
            
        Combining the kinematics (\ref{eq:coordtransform}) and the kinetics, (\ref{eq:rearrangedeqofmotion}) one can obtain the dynamics that can be rearranged into a simplified expression,
        \begin{align}\label{eq:simpledyna}
            \dot{\mathbf{x}}(t) = \fbold\lpare{\xbold(t),\ubold(t), V, \chi}\text{,}
        \end{align}
        where $V$ is the true wind speed that \textit{blows-from} at an angle $\chi$ from the North (the positive $x_0$ axis), and
        \begin{align}
            \mathbf{x}(t) &\triangleq \begin{bmatrix}
                        \; x_0 \;\;\; u \;\;\; y_0 \;\;\; v_{\mathrm{m}} \;\;\; \psi \;\;\; r \;\,
                     \end{bmatrix}^\top \label{statevect} \eqskip
            \mathbf{u}(t) &\triangleq \begin{bmatrix}
                        \; \delta \;\;\; n \;\,
                     \end{bmatrix}^\top \label{controlvect}.    
        \end{align}
        $\mathbf{x}(t) \in \mathbb{R}^6$ is termed \textit{states} and $\mathbf{u}(t) \in \mathbb{R}^2$ is termed \textit{control}, where $\delta$ is rudder angle in degree and $n$ is propeller revolution number in rps (1/s).
        
    \subsection{Offline Trajectory Planner: Off-TP}
        \label{subSec:2.2-OffTrajOpt}
        The Off-TP is an OCP-based trajectory planner which role is to provide a \textit{warm start}, i.e., an almost-globally optimal solution \citep{WarmStartIPForsgren}, as the initial guess to the SO-TP so that it can converge faster. This warm start does not necessarily need to be the solution to the exact same optimization problem.
        
        In the Off-TP, the OCP is discretized and transcribed into a finite-dimensional NLP by shooting method. The goal is to find the optimal control $\mathbf{u}^{*}_{\mathrm{off}}$ at each discretization point that drive the system from the initial states $\mathbf{x}(0)$ to the desired final docking states $\displaystyle{\xbold_{\mathrm{fin}}}$. This optimal control gives optimal state trajectories $\mathbf{x}^{*}_{\mathrm{off}}$ that minimize a modified terminal objective function: minimize the deviation of the states from $\displaystyle{\xbold_{\mathrm{fin}}}$ at the final time $t_{\mathrm{f,off}}$. These offline solutions from the Off-TP: $\mathbf{u}^{*}_{\mathrm{off}}$, $\mathbf{x}^{*}_{\mathrm{off}}$, and $t_{\mathrm{f,off}}$ are used as a warm start for the SO-TP (see Fig. $\ref{fig:flowchart}$). Note that the terms with asterisk ($^*$) denote the corresponding optimal values. 
        
        The offline NLP is solved by a nonconvex and derivative-free global optimization technique known as the evolution strategy with covariance matrix adaptation (CMA-ES) \citep{hansencma2003,auger2005restart,sakamoto2017modified}\@. Despite being computationally demanding (a stochastic technique, coupled with explicit integration of the dynamics and single shooting method), it performed very well to locate the best approximation of the global optimum. So, the offline solution from the Off-TP can be considered almost-globally optimal. Details about the Off-TP are explained in \cite{Maki2020application}.        
        
        \subsection{Semionline Trajectory Planner: SO-TP} \label{subSec:2.2-OnTrajOpt}
        The SO-TP is developed upon the idea that in practice there may be a need to \textit{replan} the trajectory based on the surrounding conditions. It is desired that the updated trajectory is kept feasible and safe.
        
        Let $\bar{t}_j$ be the $j$-th sampling of the actual time $\bar{t}$ outside the SO-TP. Let $t$ be the normalized prediction time inside the SO-TP such that $t=0$ is $\bar{t}_j$. Then, the continuous (a modified terminal OCP, before transcription) semionline trajectory optimization problem at $\bar{t}_j$ can be stated as:
        \begin{subequations}\label{eq:onobj}
        \begin{flalign}
        &\text{determine} && \delta^{*}(t) \text{,} \;  n^{*}(t) \text{,} \; t_{\mathrm{f}} \nonumber \eqskip
        &\text{that min.}&& J_{\mathrm{on}}= \sum_{i=1}^{6} \lpare{x_{i}\lpare{t_{\mathrm{f}}}-x_{\mathrm{fin}, i}}^2 \nonumber \eqskip
        & && \hphantom{J_{\mathrm{on}}=} \times \int_{0}^{t_{\mathrm{f}}} \sum_{i=1}^{6} \lpare{x_{i}\lpare{t}-x_{\mathrm{fin}, i}}^2 \, \mathrm{d}t \label{eq:Jon} \eqskip 
        &\text{subject to} && \dot{\mathbf{x}}(t) = \fbold\lpare{\xbold(t),\ubold(t), V\actutjpare, \chi\actutjpare} \text{,} \label{eq:ondynocp} \eqskip
        & &&\mathbf{x}^*\lpare{0}=\mathbf{x}\lpare{\actut_j} \text{,} \; \mathbf{x}^*\lpare{\tfpla}=\displaystyle{\xbold_{\mathrm{fin}}} \text{,} \eqskip
        & &&-\displaystyle{\delta_{\mathrm{max}}} \leq \delta(t) \leq \displaystyle{\delta_{\mathrm{max}}} \text{,} \label{eq:ocprudconst} \eqskip
        & &&-n_{\mathrm{max}} \leq n(t) \leq n_{\mathrm{max}} \text{,} \label{eq:ocppropconst} \eqskip
        & && t\in \lbra{0, \tfpla} \text{,} \;\; \text{ and } \;\; t_{\mathrm{f}} \in \left(0,\infty \right). \nonumber
        \end{flalign} 
        \end{subequations}
        The wind parameters are assumed constant when planning the trajectory, i.e., $V(t)=V\lpare{\actut_j}$ and $\chi(t)=\chi\lpare{\actut_j}$ for all $t$.
        
        The objective function of the online OCP $J_{\mathrm{on}}$ (\ref{eq:Jon}) will drive the optimization to minimize the deviation of the final states $\mathbf{x}\lpare{\tfpla}$ from the desired final docking states $\displaystyle{\xbold_{\mathrm{fin}}}$. The first summation term in (\ref{eq:Jon}) is the deviation at $t_{\mathrm{f}}$ and the integral term captures the accumulation of the deviation of the states $\mathbf{x}(t)$ from $\displaystyle{\xbold_{\mathrm{fin}}}$ within the whole time. The integral term will also indirectly force the optimization to minimize the final time $t_{\mathrm{f}}$. This objective function is identical to that in the Off-TP because ideally the objective of the Off-TP and the SO-TP should be kept as identical as possible.
        
        It is practical to plan a reference trajectory that is "easy" to be tracked, i.e., with less control effort. This is to compensate for the uncertainties that may affect the feedback controller. It can be done either by: adding control cost in the objective function (\ref{eq:Jon}) or explicitly limiting the allowable control (to $\displaystyle{\delta_{\mathrm{max}}}$ and $n_{\mathrm{max}}$, less than the actual capacity). The latter is preferable since it is binding in its box constraint form: (\ref{eq:ocprudconst}) and (\ref{eq:ocppropconst}), see Table \ref{tab:1-controlmaxreason}.
        
        \begin{table}[pos=h]
        \caption{Control limitations in generating the reference trajectory.}\label{tab:1-controlmaxreason}
            \begin{tabular*}{\tblwidth}{@{}CCC@{}}
                \toprule
                 Control & Limiting values & Actual capacity \\ 
                \midrule
                $\delta$ (deg) & $\displaystyle{\delta_{\mathrm{max}}} = 25$ & $35$ \\
                $n$ (rps) & $n_{\mathrm{max}} = 15$ & $20$ \\
                \bottomrule
            \end{tabular*}
        \end{table} 
        
        \subsubsection{Separated Hermite-Simpson Collocation} \label{subsubSec:2.3.1-Herm-Sims-Coll}
            The continuous OCP in (\ref{eq:onobj}) can be discretized and transcribed into a finite-dimensional nonlinear optimization problem (NLP) by direct collocation. The dynamics, the state, and the control trajectories are discretized into $N_\mathrm{s}\in\mathbb{N}$ equal-spaced segments; each bounded by \textit{knot} points. The segment's length (between two knot points) is $h_\mathrm{s}=t_{\mathrm{f}}/N_\mathrm{s}$.
            
            In \textit{separated} Hermite-Simpson collocation method \citep{doi:BettsControl}, a \textit{collocation} point at the mid-segment is required so that the length between each discretization point is $h_\mathrm{d}=0.5h_\mathrm{s}$. Along with the knot points, the number of \textit{discretization} points is $N_\mathrm{c}=2N_\mathrm{s}+1$.
            
            Now, let $q$ denote the $q$-th segment such that $q=1,2,\dots,N_\mathrm{s}$ and let $k$ denote the $k$-th discretization point such that $k=1,2, \dots, N_\mathrm{c}$. With these notations, odd $k$ is the knot point where $k=2q-1$ and even $k$ is the collocation point where $k=2q$. Note that these two representations of the discretization points (by $k$ and $q$) are equivalent and will be used interchangeably.
            
            Moreover, let $t_k$ be the time at the $k$-th discretization point. For brevity, $\mathbf{x}_k\equiv\mathbf{x}\lpare{t_k}$ and $\mathbf{u}_k\equiv\mathbf{u}\lpare{t_k}$ denote the optimization variables, i.e., the states (\ref{statevect}) and the control (\ref{controlvect}) at the discretization point. Following the same notation, $\mathbf{f}_k\equiv\mathbf{f}\lpare{\mathbf{x}_k,\mathbf{u}_k,V,\chi}$ denotes the true dynamics (\ref{eq:simpledyna}) evaluated at the discretization point.
            
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=0.95\columnwidth]{Fig_3_HS.pdf}
                \caption{Hermite-Simpson collocation: match the slope of the interpolant $\mathbf{f}_\mathrm{H}$ with the true dynamics $\mathbf{f}$ at the collocation point.} 
                \label{fig:HS}
            \end{figure}              
            
            The idea of Hermite-Simpson collocation is to approximate the state trajectory $\mathbf{x}(t)$ within a segment (shown as black curves in Fig. \ref{fig:HS}) by first-order third-degree (cubic) \textit{Hermite interpolation polynomial}, denoted by $\mathbf{x}_\mathrm{H}(t)$, with $C^{1}$ continuity (shown as red and blue curves in Fig. \ref{fig:HS}). These piecewise polynomial functions form a continuous cubic Hermite spline that satisfies both the states and its first derivative (true dynamics) at all discretization points. On the other hand, the control trajectory in a segment can be approximated by a quadratic, linear, or constant function. This function is denoted by $\mathbf{u}_\mathrm{H}(t)$.
            
            The key point of Hermite-Simpson collocation method is to look for the states and control such that at the collocation point, the slope of the Hermite cubic interpolant, i.e., $\dot{\mathbf{x}}_\mathrm{H}$ (or $\mathbf{f}_\mathrm{H}$), is equal to the true dynamics $\mathbf{f}$. This is visualized in Fig. \ref{fig:HS} where the red dashed line should match the black dashed line. To do this, first the states at the collocation point can be interpolated by,
            \begin{align}\label{eq:statinterp}
                \begin{split}
                    \mathbf{x}_\mathrm{H}\lpare{t_{2q}}&=\frac{1}{2}\lpare{\mathbf{x}_{2q-1}+\mathbf{x}_{2q+1}}\eqskip
                    &\hphantom{=}+\frac{h_\mathrm{s}}{8}\lpare{\mathbf{f}_{2q-1}-\mathbf{f}_{2q+1}}\text{,}
                \end{split}
            \end{align}
            which results in additional constraints as follows,
            \begin{align}
                \mathbf{x}_\mathrm{H}\lpare{t_{2q}}&=\mathbf{x}_{2q} \label{eq:interpconstraintHS1}\text{.}
            \end{align}
            Similarly, the dynamics can be interpolated by,
            \begin{align}\label{eq:dynainterp}
                \begin{split}
                    \dot{\mathbf{x}}_\mathrm{H}\lpare{t_{2q}}&=\mathbf{f}_\mathrm{H}\lpare{t_{2q}}\eqskip
                    &=-\frac{3}{2h_\mathrm{s}}\lpare{\mathbf{x}_{2q-1}-\mathbf{x}_{2q+1}}\eqskip
                    &\;\hphantom{=}-\frac{1}{4}\lpare{\mathbf{f}_{2q-1}+\mathbf{f}_{2q+1}}\text{.}\eqskip
                \end{split}
            \end{align}    
            With $\mathbf{x}_{2q}$ that satisfies (\ref{eq:interpconstraintHS1}) and the control $\mathbf{u}_{2q}$, one can directly calculate the true dynamics at the collocation point $\mathbf{f}_{2q}$ from (\ref{eq:simpledyna}) and equating it with the interpolated dynamics (\ref{eq:dynainterp}). This results in additional constraints as follows,
            \begin{align}
                \mathbf{f}_\mathrm{H}\lpare{t_{2q}}&=\mathbf{f}_{2q}\text{,}\label{eq:quadconstraintHS1}
            \end{align}
            which, if satisfied, is equivalent to implicitly integrating the true dynamics within a segment by Simpson quadrature rule:
            \begin{align*}
                \mathbf{x}_{2q+1} - \mathbf{x}_{2q-1} \approx \frac{h_\mathrm{s}}{6}\lpare{\mathbf{f}_{2q-1}+4\mathbf{f}_{2q}+\mathbf{f}_{2q+1}}\text{.}
            \end{align*}
            
            In summary, at each segment $q$ there are two additional constraints for the NLP, i.e., the \textit{Hermite interpolation constraint} (\ref{eq:interpconstraintHS1}) and the \textit{Simpson quadrature constraint} (\ref{eq:quadconstraintHS1}); both imposed at the collocation points. The goal of the NLP is to find the final time along with the control and the states at all discretization points that satisfy all the constraints.\\
        
        \subsubsection{Point-in-Polygon: Collision Constraints}
            \label{subsubSec:2.3.2-PIPColConstr} 
            Suppose $\mathbf{P}_{\mathrm{b}} \in\mathbb{R}^{2\timesn_{\mathrm{v}}}$ is a matrix whose columns are the vertices position in the earth-fixed coordinate system,
            \begin{align*}
                \mathbf{P}_{\mathrm{b}} \triangleq
                    \begin{bmatrix} \; \mathbf{p}\bsubi{1} \;\;\; \mathbf{p}\bsubi{2} \;\;\; \dots \;\;\; \mathbf{p}\bsubi{n_{\mathrm{v}}} \end{bmatrix}\text{,} 
            \end{align*}
            where $\mathbf{p}_{\mathrm{b}}=\lbra{\;x_{\mathrm{b}}\;\;y_{\mathrm{b}}\;}^{\top}$. The number of the vertices, $n_{\mathrm{v}}\geq3$ is necessary to form a simple polygon: a closed region.
            
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=0.64\columnwidth]{Fig_4_Sum_of_theta.pdf}
                \caption{Sum of all $\theta_{\mathrm{s},i}$ (for $i=1,\dots,n_{\mathrm{v}}$) must be equal to $2\pi$ for point $\mathbf{p}_{\mathrm{s}}$ to be inside the polygon.} 
                \label{fig:sumofangle}
            \end{figure}            
            
            Let $\mathbf{p}_{\mathrm{s}}=\lbra{\;x_{\mathrm{s}}\;\;y_{\mathrm{s}}\;}^{\top}$ be the point to be checked. If $\mathbf{p}_{\mathrm{s}}$ is inside the polygon formed by $\mathbf{P}_{\mathrm{b}}$, then the sum of angle between all pairs of lines formed by $\mathbf{p}_{\mathrm{s}}$ and two adjacent polygon vertices, i.e., $\mathbf{p}\bsubi{i}$ and $\mathbf{p}\bsubi{i+1}$, must be equal to $2\pi$. If this is not true, then $\mathbf{p}_{\mathrm{s}}$ is outside the polygon. From Fig. \ref{fig:sumofangle}, let $\theta_{\mathrm{s},i}$ be the angle between two adjacent lines. 
        
            Suppose now $\mathbf{P}_{\mathrm{s}} \in\mathbb{R}^{2\timesn_{\mathrm{s}}}$ is a matrix whose columns are the points of the ship's boundary in the earth-fixed coordinate system,
            \begin{align*}
                \mathbf{P}_{\mathrm{s}} \triangleq
                    \begin{bmatrix} \; \mathbf{p}\ssubi{1} \;\;\; \mathbf{p}\ssubi{2} \;\;\; \dots \;\;\; \mathbf{p}\ssubi{n_{\mathrm{s}}} \end{bmatrix} \text{,} 
            \end{align*}
            where $n_{\mathrm{s}}$ is the number of the ship's boundary points (not to be confused with the discretization points in the collocation). The boundary points represent the ship as a rigid body instead of a particle; ensures stringent constraints. These points depend on the position of midship $\lpare{x_0, y_0}$ and the yaw angle $\lpare{\psi}$, i.e., some of the optimization variables. Due to this dependency and the requirement that the sum of angle is equal to $2\pi$, one can construct additional equality constraints for the NLP as follows:
            \begin{align}\begin{split}
                \left.\sum_{i=1}^{n_{\mathrm{v}}}\theta\ssubi{i}\right\rvert_{j,k} = 2\pi. \label{eq:sumofangle}
            \end{split}\end{align}
            This means that at each discretization point $k=1,2,\dots,N_\mathrm{c}$, (\ref{eq:sumofangle}) must be evaluated for all ship's boundary points $j=1,2,\dots,n_{\mathrm{s}}$, which results in $n_{\mathrm{s}}\timesN_\mathrm{c}$ equalities.
            
            This method is equivalent to \textit{winding number }method \citep{HORMANN2001131} and known as \textit{angle summation} method. In CAD, it is not considered the best method to check whether a point is inside or outside a polygon. Nevertheless, in the scope of this article, it can be regarded as one of the most practical: analytically differentiable and can handle any shape of the harbor area.
            
        \subsubsection{Transcribed OCP in the SO-TP}
            \label{subsubSec:2.3.3-TranscribedOCP} 
            Based upon the previous two sub-subsections, the semionline OCP (\ref{eq:onobj}) can be transcribed to the following finite-dimensional NLP:
            \begin{subequations}\label{eq:transOCP}
            \begin{flalign}
            &\text{determine} \quad&&\delta^{*}_1, \delta^{*}_2, \dots, \delta^{*}_{N_\mathrm{c}} \in \mathbb{R} \text{,} \nonumber \eqskip  
            & \quad && n^{*}_1, n^{*}_2, \dots, n^{*}_{N_\mathrm{c}} \in \mathbb{R} \text{,} \nonumber \eqskip
            & \quad && \mathbf{x}^{*}_1, \mathbf{x}^{*}_2, \dots, \mathbf{x}^{*}_{N_\mathrm{c}} \in \mathbb{R}^6 \text{,} \;\; t_{\mathrm{f}} \in \mathbb{R} \nonumber \eqskip
            &\text{that min.} \quad&& J_{\mathrm{on}}= \sum_{i=1}^{6} \lpare{x_{i}\lpare{t_{\mathrm{f}}}-x_{\mathrm{fin}, i}}^2 \nonumber \eqskip
            & \quad&& \hphantom{J_{\mathrm{on}}=} \times \int_{0}^{t_{\mathrm{f}}} \sum_{i=1}^{6} \lpare{x_{i}\lpare{t}-x_{\mathrm{fin}, i}}^2 \, \mathrm{d}t \label{eq:transobj} \eqskip 
            &\text{subject to} \quad&&
            \mathbf{x}^{*}_1 = \mathbf{x}\lpare{\actut_j} \text{,} \; \mathbf{x}^{*}_{N_\mathrm{c}}=\displaystyle{\xbold_{\mathrm{fin}}} \text{,} \label{eq:initstaton} \eqskip
            & \quad && \mathbf{x}_{N_\mathrm{c}-2}= \displaystyle{\xbold_{\mathrm{fin}}} \,- \nonumber \eqskip
            & \quad && \hphantom{\mathbf{x}_{N_\mathrm{c}-2} } \frac{h_\mathrm{s}}{6}\lpare{\mathbf{f}_{N_\mathrm{c}-2} + 4\mathbf{f}_{N_\mathrm{c}-1} + \mathbf{f}_{N_\mathrm{c}}} \text{,} \, \label{eq:finstaton} \eqskip
            & \quad && \mathbf{x}_\mathrm{H}\lpare{t_{2q}} = \mathbf{x}_{2q} \text{,} \;\;\, q=1,2,\dots,N_\mathrm{s} \text{,} \label{eq:intpconston} \eqskip
            & \quad && \mathbf{f}_\mathrm{H}\lpare{t_{2q}}= \mathbf{f}_{2q} \text{,} \quad q=1,2,\dots,N_\mathrm{s} \text{,} \label{eq:quadconston} \eqskip
            & \quad && \left.\sum_{i=1}^{n_{\mathrm{v}}}\theta\ssubi{i}\right\rvert_{j,k} = 2\pi \text{,}\quad j=1,2,\dots,n_{\mathrm{s}} \label{eq:sumofangconst}\text{,}\eqskip
            & \quad && -\displaystyle{\delta_{\mathrm{max}}} \leq \delta\lpare{t_k} \leq \displaystyle{\delta_{\mathrm{max}}} \text{,} \label{eq:rudconstrainton} \eqskip
            & \quad && -n_{\mathrm{max}} \leq n\lpare{t_k} \leq n_{\mathrm{max}} \text{,} \text{ and }  \label{eq:propconstrainton} \eqskip
            & \quad && t_{\mathrm{f}} \; \in\, \left(0,\infty \right).  \nonumber
            \end{flalign} 
            \end{subequations}
            The integral term in the objective function (\ref{eq:transobj}) is approximated by Simpson quadrature.
    
            The goal of the NLP (\ref{eq:transOCP}) is to find $\lpare{2+6}N_\mathrm{c}+1$ number of optimal variables that minimize the objective function (\ref{eq:transobj}). The $6\times 2$ equalities in (\ref{eq:initstaton}) and (\ref{eq:finstaton}) enforce the initial and final states constraints, while (\ref{eq:intpconston}) and (\ref{eq:quadconston}) are the $6\timesN_\mathrm{s}$ interpolation constraints and $6\timesN_\mathrm{s}$ quadrature constraints, respectively. The $n_{\mathrm{s}}\timesN_\mathrm{c}$ equalities in (\ref{eq:sumofangconst}) are the collision constraints and the last two box constraints: (\ref{eq:rudconstrainton}) and (\ref{eq:propconstrainton}) saturate the control to the given values. 
            
            Sequential quadratic programming (SQP) from \MATLAB \lstinline{fmincon} package is used to solve the NLP (\ref{eq:transOCP}). It is a Newton's method with quadratic local convergence rate \citep{boggs_tolle_1995}\@. With a good initial guess (warm start), less iterations are expected, resulting in a faster convergence to the local optimum.

            The next chapter shows how just one offline solution from the Off-TP $\lpare{\mathbf{x}^{*}_{\mathrm{off}},\mathbf{u}^{*}_{\mathrm{off}},t_{\mathrm{f,off}}}$ for a no-wind condition can be used to warm-start the SO-TP in various situations.\\ 
            
\section{Simulation Conditions}
    \label{Sec:3-SimulationConditions}
    The offline solutions $\mathbf{x}^{*}_{\mathrm{off}}$ and $\mathbf{u}^{*}_{\mathrm{off}}$ are obtained by the Off-TP with $V=0$ and $\chi=0$ (no wind). The initial states $\mathbf{x}^{*}_{\mathrm{off}}(0)$, the desired final states $\displaystyle{\xbold_{\mathrm{fin}}}$, the final states $\mathbf{x}^{*}_{\mathrm{off}}\lpare{t_{\mathrm{f,off}}}$, and the final time $t_{\mathrm{f,off}}$ of this offline solution are as follows:
    {\setlength{\mathindent}{0pt}
    \begin{align}
        \mathbf{x}^{*}_{\mathrm{off}}(0) &= \lbra{\, 16.50 \;\; 0.12 \;\, -7.50\;\; 0.00\;\; 2\pi/3 \;\; 0.00\,}^{\top}\nonumber\eqskip
        \displaystyle{\xbold_{\mathrm{fin}}} &= \lbra{\, -0.50 \;\; 0.01 \;\, -0.50\;\; 0.00\;\; \pi\;\; 0.00\,}^{\top}\nonumber\eqskip
        \mathbf{x}^{*}_{\mathrm{off}}\lpare{t_{\mathrm{f,off}}}&= \lbra{\, -0.51 \;\; 0.01 \;\, -0.51\;\; 0.00\;\; 3.14\;\; 0.00\,}^{\top}\nonumber\eqskip
        t_{\mathrm{f,off}} &= 160 \text{ s}\nonumber.
    \end{align}}
    
    At the final docking position, the ship's target berthing velocity (velocity normal to the wall, i.e., sway velocity $v_{\mathrm{m}}$ if the ship is parallel to the berth's wall) is between 0.03 m/s to 0.04 m/s as reported in a statistical study of berthing velocity in the Port of Rotterdam \citep{Roubos2017}. The longitudinal velocity ($u$) on the other hand, should be kept as low as possible, but does not necessarily need to be zero as the port mooring/rigging will be used to completely fix the ship. So, the target surge velocity is set to 0.01 m/s (equivalent to 0.1 m/s in the full-scale ship) to relax the overall optimization problem.
    
    The performance of the warm-started SO-TP is evaluated for four main cases (M1 to M4) and ten additional arbitrary trial cases (A1 to A10, see Table \ref{tab:2-CaseInit}). The initial states for each cases are expressed as $x_i\lpare{\actut_j}=d_ix_{\mathrm{off},i}^{*}(0)$ for $i=1,\dots,6$, where $d_i$ is the element of a vector of multipliers $\mathbf{d}$. To quantify "how different" the initial states are from $\mathbf{x}^{*}_{\mathrm{off}}(0)$, the Euclidean norm is introduced as, 
    \begin{align}
        L=&\sqrt{\sum_i^{6}\lbra{{\lpare{x_i\lpare{\actut_j}-x_{\mathrm{off},i}^{*}(0)}/x_{\mathrm{off},i}^{*}}(0)}^2}\nonumber\\
        =&\sqrt{\sum_i^{6}\lpare{d_i - 1}^2}.
    \end{align}
    
    \begin{table*}\centering
        \caption{Evaluation cases for the SO-TP, see Fig. \ref{fig:App1} to Fig. \ref{fig:App3} in the appendix.}
        \label{tab:2-CaseInit}       
            \begin{tabular}{cccccccc}
                \hline\noalign{\smallskip}
                \multirow{2}{*}{Case} & Multiplier $\mathbf{d}$ & \multicolumn{2}{c}{Initial velocity $u(0)$} & \multirow{2}{*}{$\psi(0)$} & $V\lpare{\actut_j}$ & $\chi\lpare{\actut_j}$ & Norm $L=$\\
                \noalign{\smallskip}\cline{3-4}\noalign{\smallskip}
                    
                & $x_i\lpare{\actut_j}=d_ix_{\mathrm{off},i}^{*}(0)$ & (m/s) & Full scale (knots) & & (m/s) & (deg) & $\sqrt{\sum_i^{6}\lpare{d_i - 1}^2}$\\
                    
                \noalign{\smallskip}\hline\noalign{\smallskip}
                    
                M1 & $\lbra{\, 1.0 \;\; 1.0 \;\; 1.0 \;\; 1.0 \;\; 1.0\;\; 1.0 \,}^\top$ & $0.12$ & $2.43$ & $2\pi/3$ & \multicolumn{2}{c}{No wind} & $0.00$ \\
                    
                M2 & $\lbra{\, 1.1 \;\; 2.0 \;\; 1.1 \;\; 1.0 \;\; 1.2\;\; 1.0 \,}^\top$ & $0.24$ & $4.86$ & $4\pi/5$ & $0.75$ & $45$ & $1.03$ \\
                    
                M3 & $\lbra{\, 0.9 \;\; 2.0 \;\; 0.8 \;\; 1.0 \;\; 0.9\;\; 1.0 \,}^\top$ & $0.24$ & $4.86$ & $3\pi/5$ & $0.50$ & $225$ & $1.03$ \\        
                    
                M4 & $\lbra{\, 1.0 \;\; 3.0 \;\; 0.9 \;\; 1.0 \;\; 1.5\;\; 1.0 \,}^\top$ & $0.36$ & $7.28$ & $\pi$ & $0.75$ & $45$ & $2.06$ \\ 
                
                A1 & $\lbra{\, 1.2 \;\; 1.2 \;\; 1.1 \;\; 1.0 \;\; 1.1\;\; 1.0 \,}^\top$ & $0.14$ & $2.83$ & $11\pi/15$ & $0.75$ & $45$ & $0.32$  \\   
                
                A2 & $\lbra{\, 1.2 \;\; 1.4 \;\; 1.2 \;\; 1.0 \;\; 1.2\;\; 1.0 \,}^\top$ & $0.17$ & $3.44$ & $4\pi/5$ & $0.75$ & $180$ & $0.53$  \\
                
                A3 & $\lbra{\, 1.3 \;\; 1.6 \;\; 0.9 \;\; 1.0 \;\; 1.4\;\; 1.0 \,}^\top$ & $0.19$ & $3.84$ & $14\pi/15$ & $0.75$ & $125$ & $0.79$  \\  
                
                A4 & $\lbra{\, 1.4 \;\; 1.8 \;\; 0.8 \;\; 1.0 \;\; 0.9\;\; 1.0 \,}^\top$ & $0.22$ & $4.45$ & $3\pi/5$ & $0.50$ & $135$ & $0.92$  \\ 
                
                A5 & $\lbra{\, 1.5 \;\; 0.8 \;\; 1.2 \;\; 1.0 \;\; 1.1\;\; 1.0 \,}^\top$ & $0.10$ & $2.02$ & $11\pi/15$ & $0.50$ & $90$ & $0.58$  \\    
                
                A6 & $\lbra{\, 1.0 \;\; 0.8 \;\; 0.7 \;\; 1.0 \;\; 1.2\;\; 1.0 \,}^\top$ & $0.10$ & $2.02$ & $4\pi/5$ & $0.50$ & $315$ & $0.41$  \\
                
                A7 & $\lbra{\, 1.2 \;\; 2.2 \;\; 0.8 \;\; 1.0 \;\; 0.8\;\; 1.0 \,}^\top$ & $0.26$ & $5.26$ & $8\pi/15$ & $0.50$ & $250$ & $1.25$  \\ 
                
                A8 & $\lbra{\, 1.2 \;\; 2.0 \;\; 0.0 \;\; 1.0 \;\; 1.5\;\; 1.0 \,}^\top$ & $0.24$ & $4.86$ & $\pi$ & $0.50$ & $90$ & $1.51$  \\  
                
                A9 & $\lbra{\, 1.0 \;\; 1.0 \;\; -1.0 \;\; 1.0 \;\; 2.0\;\; 1.0 \,}^\top$ & $0.12$ & $2.43$ & $4\pi/3$ & $0.75$ & $0$ & $2.24$  \\   
                
                A10 & $\lbra{\, 0.8 \;\; 1.5 \;\; -0.9 \;\; 1.0 \;\; 2.2\;\; 1.0 \,}^\top$ & $0.18$ & $3.64$ & $22\pi/15$ & $0.50$ & $45$ & $2.31$  \\
                \noalign{\smallskip}\hline
            \end{tabular}
        \end{table*}            
            
    \subsection{Spatial/Collision Constraints Definition}\label{subSec:3.1-SpatConstDef}
        The docking is simulated in the Inukai experiment pond that belongs to Osaka University. The predefined obstacle-free region formed by a simple polygon $\mathbf{P}_{\mathrm{b}}$ is shown in Fig. \ref{fig:fig:boundary}, with number of vertices $n_{\mathrm{v}}=11$. The origin of the earth-fixed coordinate system $\mathrm{o}_0-x_0y_0$ is located at $\mathbf{p}\bsubi{1}$. 
        \begin{figure}[htbp]
            \begin{subfigure}[b]{0.48\columnwidth}
            \centering
                \includegraphics[width=0.95\columnwidth]{Fig_5a_Obst_free_region.pdf}
                \caption{\label{fig:fig:boundary}}
            \end{subfigure}
            \begin{subfigure}[b]{0.48\columnwidth}
            \centering
                \includegraphics[width=0.5\columnwidth]{Fig_5b_Ship_boundary.pdf}
                \vspace{8pt}
                \caption{\label{fig:fig:shipvert}}
            \end{subfigure}
            \caption{(\subref{fig:fig:boundary}) Shaded region is the obstacle-free region, formed by polygon $\mathbf{P}_{\mathrm{b}}$ with $n_{\mathrm{v}}=11$ vertices (\subref{fig:fig:shipvert}) The points of the ship's boundary $\mathbf{P}_{\mathrm{s}}$ with $n_{\mathrm{s}}=5$ points.}
            \label{fig:boundary}
        \end{figure}
        
        The number of the points of the ship's boundary $n_{\mathrm{s}}$ must be chosen carefully. Adding more points will guarantee a safer trajectory but will increase the number of constraints and the computational time. In the simulation, $n_{\mathrm{s}}=5$ is considered sufficient as shown in Fig. \ref{fig:fig:shipvert}.  
        
    \subsection{On the Discretization and Interpolation}\label{subSec:3.2.-OptimSett}
        The whole trajectory is divided into $N_\mathrm{s}=20$ equal-spaced segments so that the number of discretization points is $N_\mathrm{c}=41$. Upon solving for the optimal control and states at each discretization point, it is necessary to interpolate the values between those points. Unlike in the offline OCP, here the control trajectory in a segment is not constant. 
        
        For practical consideration (mechanical limitation of the vessel's actuators), it is assumed that the control trajectory between two consecutive discretization points (half a segment) is linear. It can be computed as,
        \begin{align}\label{eq:ctrlpolyflinear}
            \mathbf{u}^{*}_\mathrm{H}(t)=\mathbf{u}^{*}_k + \lpare{\mathbf{u}^{*}_{k+1}-\mathbf{u}^{*}_k}\frac{t-t_k}{h_\mathrm{d}}\text{,}
        \end{align}
        for $t\in\lbra{t_k, t_{k+1}}$ where $k=1,2,\dots,N_\mathrm{c}-1$.
        
        The state trajectory in a segment is interpolated by \textit{natural cubic spline interpolation} method \citep{NumericalRecipeC}\@. It retains the same accuracy of Hermite cubic interpolation with $C^2$ continuity. The resulting natural cubic spline interpolant within a segment $t\in\lbra{t_k, t_{k+2}}$ can be written as,
        \begin{align}
            \begin{split}
                \mathbf{x}^{*}_\mathrm{H}(t)&=\mathbf{x}^{*}_k+\mathbf{f}_k^{*}\lpare{t-t_k}\eqskip
                &\hphantom{=}-\Bigg(3\mathbf{f}_k^{*}-4\mathbf{f}_{k+1}^{*}+\mathbf{f}_{k+2}^{*}\Bigg)\frac{\lpare{t-t_k}^2}{2h_\mathrm{s}} \eqskip
                &\hphantom{=}+\Bigg( 2\mathbf{f}_k^{*}-4\mathbf{f}_{k+1}^{*}+2\mathbf{f}_{k+2}^{*}\Bigg)\frac{\lpare{t-t_k}^3}{3h_\mathrm{s}^2}\;\text{,}
            \end{split}
        \end{align}
        for all $k$ such that $k=2q-1$ where $q=1,2,\dots,N_\mathrm{s}$. Here $\mathbf{f}_{k}^{*}\equiv\mathbf{f}\lpare{\mathbf{x}^{*}_k,\mathbf{u}^{*}_k, V, \chi}$ is the true dynamics (\ref{eq:simpledyna}) evaluated at the corresponding discretization point.

\section{Simulation Results and Evaluation}\label{Sec:4-SimRes}
    This section discusses the performance of the SO-TP. The simulations are done on a laptop with 16GB RAM and 8-core processor. 
    
    \subsection{Main Cases: Examples}
        \label{subSec:4.1.-MainCases}
        The main cases M1 to M4 are intended to give some examples of how just one almost-globally optimal and feasible solution of a slightly different problem obtained from the Off-TP can be used as a warm start for different situations in the SO-TP (adhere to the first evaluation matter), see Table \ref{tab:2-CaseInit}. Note that the Off-TP and the SO-TP differ in the OCP transcription method, the form of the objective function, and how the control trajectory is assumed within a segment.
        
        Now, to emphasize the advantages of the warm start to the SO-TP, it is relevant to show the results of the SO-TP without a warm start where a linear guess is provided, i.e., linearly changing from the initial states to the desired final states. Meanwhile, the initial guess for the control is constant: $0.5\displaystyle{\delta_{\mathrm{max}}}$ and $0.5n_{\mathrm{max}}$. In addition, the initial guess of the final time $t_{\mathrm{f}}$ is $160$ s, equal to that of the warm start.
        
        Furthermore, if $T_{\mathrm{w1}}$ denotes the computational time with warm start by the Off-TP and $T_{\mathrm{cs}}$ denotes the computational time without warm start,  one can calculate the computational speedup by,
        \begin{align}
            \text{\% Comp. speedup} = \frac{T_{\mathrm{cs}}-T_{\mathrm{w1}}}{T_{\mathrm{cs}}}\times 100 \%\text{.} 
        \end{align}
        
        \subsubsection{Case M1: No Wind + Same Initial States}
        \label{subsubSec:4.1.1-CaseM1}
            This case is intended to confirm whether the SO-TP converges to the same warm start or not, with the same condition in which the warm start was solved, i.e., no wind and same initial states (see section \ref{Sec:3-SimulationConditions}). The comparison between the optimal control and state trajectories obtained by the SO-TP with and without a warm start for this case are shown in Fig. \ref{fig:CaseM1ctrl} and Fig. \ref{fig:CaseM1stat}. In the figures, the CMA-ES guess means the warm start from the Off-TP.
                 
            The SO-TP converged to the solution that is almost similar to the provided warm start in a relatively fast computational time. This is a clear and direct demonstration that despite the OCP in the Off-TP is formulated differently in almost all aspects, its solution belongs to the feasible set of the OCP in the SO-TP (\ref{eq:Jon}).
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=1\columnwidth]{Fig_6_M1_Control.pdf}
                \caption{Case M1: summary and optimal control trajectories, no wind, with $0.478$ s calculation time.} 
                \label{fig:CaseM1ctrl}
            \end{figure}    
    
            \begin{figure}[!htbp]
            \centering
                \includegraphics[width=1\columnwidth]{Fig_7_M1_States.pdf}
                \caption{Case M1: optimal state trajectories, no wind, with $0.478$ s calculation time.} 
                \label{fig:CaseM1stat}
            \end{figure}  
            
        \subsubsection{Case M2: Wind Blows Away from the Berth + Farther Initial States}
        \label{subsubSec:4.1.2-CaseM2}
            In this case, the SO-TP is executed with initial positions farther from the berth. The initial surge velocity is doubled $u(0)=2u_{\mathrm{off}}(0)$ and the initial yaw angle is less-sharp against the berth. The wind blows at $0.75$ m/s from $45$ degree, away from the berth. The resulting optimal control and state trajectories are shown in Fig. \ref{fig:CaseM2ctrl} and Fig. \ref{fig:CaseM2stat}. 
            
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_8_M2_Control.pdf}
                \caption{Case M2: summary and optimal control trajectories, $V\lpare{\actut_j}=0.75$ m/s, $\chi\lpare{\actut_j}=45^{\circ}$, $u\lpare{\actut_j}=0.24$ m/s, $\psi\lpare{\actut_j}=4\pi/5$, with $0.645$ s calculation time.} 
                \label{fig:CaseM2ctrl}
            \end{figure}
            \begin{figure}[!htbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_9_M2_States.pdf}
                \caption{Case M2: optimal state trajectories, $V\lpare{\actut_j}=0.75$ m/s, $\chi\lpare{\actut_j}=45^{\circ}$, $u\lpare{\actut_j}=0.24$ m/s, $\psi\lpare{\actut_j}=4\pi/5$, with $0.645$ s calculation time.} 
                \label{fig:CaseM2stat}
            \end{figure}  
            
            Because now the initial surge velocity is doubled ($0.24$ m/s; equivalent to $4.86$ knots for the full scale ship), the time $t_{\mathrm{f}}$ that is required to reach the desired docking states $\displaystyle{\xbold_{\mathrm{fin}}}$ is shorter, even when the initial position is farther. The solution from the warm-started SO-TP gives a shorter $t_{\mathrm{f}}$ than that without a warm start.
            
            The warm-started SO-TP also converged to a feasible and collision-free solution in a much faster computational time than that without a warm start. This M2 case demonstrates that the warm start is still applicable to a situation which initial states are different $\lpare{\text{norm }L=1.03}$ and with wind disturbance. 

        \subsubsection{Case M3: Wind Blows to the Berth + Closer Initial States}
        \label{subsubSec:4.1.3-CaseM3}    
            In contrast to the previous case M2, the initial position of the ship in case M3 is closer to the berth and the initial yaw angle is sharper against the berth. Such initial condition limits the free area for the ship to maneuver. In this case, the wind speed is $0.5$ m/s and it blows from at $225$ degree quarterly to the berth. The resulting optimal control and state trajectories for this case are visualized in Fig. \ref{fig:CaseM3ctrl} and Fig. \ref{fig:CaseM3stat}. 
            
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_10_M3_Control.pdf}
                \caption{Case M3: summary and optimal control trajectories, $V\lpare{\actut_j}=0.50$ m/s, $\chi\lpare{\actut_j}=225^{\circ}$, $u\lpare{\actut_j}=0.24$ m/s, $\psi\lpare{\actut_j}=3\pi/5$, with $8.999$ s calculation time.} 
                \label{fig:CaseM3ctrl}
            \end{figure}
            \begin{figure}[!htbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_11_M3_States.pdf}
                \caption{Case M3: optimal state trajectories, $V\lpare{\actut_j}=0.50$ m/s, $\chi\lpare{\actut_j}=225^{\circ}$, $u\lpare{\actut_j}=0.24$ m/s, $\psi\lpare{\actut_j}=3\pi/5$, with $8.999$ s calculation time.} 
                \label{fig:CaseM3stat}
            \end{figure}       
            
            It can be seen that now the problem becomes quite difficult for the ship to reach the desired docking states $\displaystyle{\xbold_{\mathrm{fin}}}$ form the given initial states because the ship is expected to do a sharp turn in such a relatively narrow area. This is implied by the longer computational time that is required for the SO-TP to converge. Nevertheless, the warm start gives a considerable $68$\% computational speedup.
            
            Similar to the previous case, $t_{\mathrm{f}}$ is shorter than that of the warm start, this is because the initial surge velocity is doubled; small thrust from the propeller can give more momentum for the ship to do a sharp turn. From Fig. \ref{fig:CaseM3ctrl}, one can also observe that the SO-TP without a warm start results in $t_{\mathrm{f}}$ that is much longer than the warm-started SO-TP.
            
            Keen readers may find it interesting to see the same tendency that the resulting optimal $\delta$ trajectory is similar to the warm start. This shows that the thrust from the propeller contributes significantly to the overall maneuverability of the ship. An intuitive explanation to this tendency is that the warm start for $\delta$ gives the objective function value that is in the neighborhood of the curvature of the interpolation (\ref{eq:intpconston}) and quadrature equality constraints (\ref{eq:quadconston}), while that for $n$ is far from the curvature of the equality constraints. 
            
        \subsubsection{Case M4: Wind Blows Away from the Berth + Large Deviation}
        \label{subsubSec:4.1.4-CaseM4}
            Suppose now the ship arrives at an entry location with initial yaw angle that is quite different from that of the warm start: parallel to the berth. The ship also arrives at a surge velocity $0.36$ m/s; three times of that of the warm start: equivalent to $7.28$ knots for the full scale ship. In this case, the wind blows at $0.75$ m/s away from the berth. Such condition is very different from the warm start with norm $L=2.06$. The resulting optimal control and state trajectories are shown in Fig. \ref{fig:CaseM4ctrl} and Fig. \ref{fig:CaseM4stat}.     
            
            \begin{figure}[htbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_12_M4_Control.pdf}
                \caption{Case M4: summary and optimal control trajectories, $V\lpare{\actut_j}=0.75$ m/s, $\chi\lpare{\actut_j}=45^{\circ}$, $u\lpare{\actut_j}=0.36$ m/s, $\psi\lpare{\actut_j}=\pi$, with $11.664$ s calculation time.} 
                \label{fig:CaseM4ctrl}
            \end{figure}   
            
            Despite the large difference of the initial states from the warm start, the warm-started SO-TP still gives a faster computational time than that without a warm start: $51$\% speedup. This means that the warm start is still applicable to speed up the computation, even for a situation that is very much different from that when obtaining it.
            
            To conclude this subsection about the first evaluation matter, the readers now must have a good grasp of the applicability of one almost-globally optimal solution from the Off-TP to warm-start the SO-TP for various different scenarios. The resulting trajectories are guaranteed to be collision-free and satisfy all the given constraints. 
            \begin{figure}[tbp]
            \centering
                \includegraphics[width=\columnwidth]{Fig_13_M4_States.pdf}
                \caption{Case M4: optimal state trajectories, $V\lpare{\actut_j}=0.75$ m/s, $\chi\lpare{\actut_j}=45^{\circ}$, $u\lpare{\actut_j}=0.36$ m/s, $\psi\lpare{\actut_j}=\pi$, with $11.664$ s calculation time.} 
                \label{fig:CaseM4stat}
            \end{figure}   
   
    \subsection{Additional Trial Cases}
        \label{subSec:4.2-AdditionalCases}
        As to further show the effect of the warm start in the computational time and the performance of the SO-TP (adhere to the second evaluation matter), 10 additional cases (A1 to A10) are also considered (see Table \ref{tab:2-CaseInit}). The same offline solution from the Off-TP is used to warm-start the SO-TP. Note that A9 and A10 are the cases when the ship starts the docking operation from the East (positive $y_0$ axis), while the rest of the cases (M1 to A8) are from the West. For the sake of demonstration, these additional cases are chosen arbitrarily, with norm of the initial states $L$ varies between 0 to 2.3.
        
        The effect of the warm start is evaluated based on the the computational time and the resulting $t_{\mathrm{f}}$. The summary of these additional trial cases along with the main cases is presented in Table \ref{tab:3-CompareWarmandNoWarmStart}. It can be seen that the warm start by the Off-TP gives a significant reduction in the computational time. As a comparison, the average computational time of the warm-started SO-TP is $9.211$ s while that without a warm start is $34.292$ s. The warm start by just one almost-globally optimal solution from the Off-TP gives an average of $71\%$ computational time speedup. In addition, the warm-started SO-TP gives a shorter $t_{\mathrm{f}}$ for almost all of the cases, which is desired.
        
        \begin{table*}\centering
        
        \caption{Comparison of the SO-TP: with and without a warm start (16 GB RAM, 8-core processor laptop). See Fig. \ref{fig:App1} to Fig. \ref{fig:App3} in the appendix.}
        \label{tab:3-CompareWarmandNoWarmStart}      
       
            \begin{tabular}{cccc|cc|cc|c}
                \hline\noalign{\smallskip}
                
                \multirow{3}{*}{Case} & Norm $L=$ & $V\lpare{\actut_j}$ & $\chi\lpare{\actut_j}$ & \multicolumn{2}{c|}{Warm start by Off-TP} & \multicolumn{2}{c|}{No warm start (linear)} & \multirow{2}*{$\%$ Comp. speedup} \\
                
                \noalign{\smallskip}\cline{5-8}\noalign{\smallskip}
                & \multirow{2}*{$\sqrt{\sum_i^{6}\lpare{d_i - 1}^2}$} & \multirow{2}*{(m/s)} & \multirow{2}*{(deg)} & \multirow{2}{*}{$t_{\mathrm{f}}$ (s)}  & Comp. time& \multirow{2}{*}{$t_{\mathrm{f}}$ (s)} & Comp. time& \multirow{2}{*}{ $\frac{T_{\mathrm{cs}}-T_{\mathrm{w1}}}{T_{\mathrm{cs}}}\times 100\%$}\\
                
                & & & & & $T_{\mathrm{w1}}$ (s) &  & $T_{\mathrm{cs}}$ (s) & \\
                
                \noalign{\smallskip}\hline\noalign{\smallskip}
                M1 & $0.00$ & \multicolumn{2}{c|}{No wind} & $162.0$ & $0.478$ & $233.3$ & $31.877$ & $98\%$\\
                
                M2 & $1.03$ & $0.75$ & $45$ & $136.2$ & $0.645$ & $156.0$ & $18.635$ & $97\%$\\
                
                M3 & $1.03$ & $0.50$ & $225$ & $150.5$ & $8.999$ & $218.4$ & $28.289$ & $68\%$\\
                
                M4 & $2.06$ & $0.75$ & $45$ & $95.7$ & $11.664$ & $118.1$ & $23.807$ & $51\%$\\
                
                A1 & $0.32$ & $0.75$ & $45$ & $151.9$ & $3.425$ & $177.1$ & $18.361$ & $81\%$\\
                
                A2 & $0.53$ & $0.75$ & $180$ & $163.1$ & $5.881$ & $231.8$ & $17.711$ & $67\%$\\
                
                A3 & $0.79$ & $0.75$ & $125$ & $146.5$ & $20.718$ & $166.1$ & $46.836$ & $56\%$\\
                
                A4 & $0.92$ & $0.50$ & $135$ & $167.7$ & $3.426$ & $289.4$ & $47.019$ & $93\%$\\
                
                A5 & $0.58$ & $0.50$ & $90$ & $175.5$ & $6.084$ & $217.5$ & $31.397$ & $81\%$\\
                
                A6 & $0.41$ & $0.50$ & $315$ & $159.6$ & $3.442$ & $256.1$ & $54.906$ & $94\%$\\    
                
                A7 & $1.25$ & $0.50$ & $250$ & $207.0$ & $14.313$ & $205.9$ & $19.903$ & $28\%$\\ 
                
                A8 & $1.51$ & $0.50$ & $90$ & $137.4$ & $9.268$ & $159.7$ & $68.519$ & $86\%$\\
                
                A9 & $2.24$ & $0.75$ & $0$ & $180.5$ & $17.487$ & $214.5$ & $30.697$ & $43\%$\\
                
                A10 & $2.31$ & $0.50$ & $45$ & $203.7$ & $23.128$ & $192.7$ & $42.133$ & $45\%$\\
                \noalign{\smallskip}\hline\noalign{\smallskip}
                
                & & & & AVG & $9.211$ & AVG & $34.292$ & $71\%$\\
                \noalign{\smallskip}\cline{5-9}
            \end{tabular}
        \end{table*}    
        
       \begin{table*}\centering
        \caption{Comparison of the SO-TP: warm-started by the Off-TP and warm-started by the SO-TP for case M1 with no warm start (linear guess).}
        \label{tab:4-CompareWarmStart}      
            \begin{tabular}{c|cccc|cccc|c}
                \hline\noalign{\smallskip}
                
                \multirow{3}{*}{Case} & \multicolumn{4}{c|}{Warm start by Off-TP} & \multicolumn{4}{c|}{Warm start by SO-TP (M1, no warm start)} & \multirow{2}*{Preferred} \\
                
                \noalign{\smallskip}\cline{2-9}\noalign{\smallskip}
                & \multirow{2}{*}{$t_{\mathrm{f}}$ (s)} & Feasi- & Constraint & Comp. time & \multirow{2}{*}{$t_{\mathrm{f}}$ (s)} & Feasi- & Constraint & Comp. & warm start \\
                
                & & ble? & violation & $T_{\mathrm{w1}}$ (s) & &  ble? & violation & time (s) & (based on comp. time)\\
                
                \noalign{\smallskip}\hline\noalign{\smallskip}
                
                M2 & $136.2$ & Yes & $2.5\mathrm{E}-11$ & $0.645$ & $-$ & No & $2.1\mathrm{E}-3$ & $77.111$ & Off-TP\\
                
                M3 & $150.5$ & Yes & $2.5\mathrm{E}-11$ & $8.999$ & $230.7$ & Yes & $1.3\mathrm{E}-11$ & $6.094$ & SO-TP\\
                
                M4 & $95.7$ & Yes & $1.7\mathrm{E}-11$ & $11.664$ & $-$ & No & $8.3\mathrm{E}-3$ & $110.403$ & Off-TP\\
                
                A1 & $151.9$ & Yes & $4.5\mathrm{E}-12$ & $3.425$ & $172.1$ & Yes & $6.7\mathrm{E}-13$ & $17.657$ & Off-TP\\
                
                A2 & $163.1$ & Yes & $2.2\mathrm{E}-11$ & $5.881$ & $245.3$ & Yes & $5.4\mathrm{E}-13$ & $12.682$ & Off-TP\\
                
                A3 & $146.5$ & Yes & $1.4\mathrm{E}-11$ & $20.718$ & $-$ & No & $2.2\mathrm{E}-3$ & $161.069$ & Off-TP\\
                
                A4 & $167.7$ & Yes & $2.0\mathrm{E}-11$ & $3.426$ & $-$ & No & $3.1\mathrm{E}-3$ & $37.161$ & Off-TP\\
                
                A5 & $175.5$ & Yes & $2.4\mathrm{E}-12$ & $6.084$ & $-$ & No & $1.3\mathrm{E}-3$ & $66.414$ & Off-TP\\
                
                A6 & $159.6$ & Yes & $5.2\mathrm{E}-12$ & $3.442$ & $-$ & No & $3.2\mathrm{E}-3$ & $47.519$ & Off-TP\\    
                
                A7 & $207.0$ & Yes & $9.7\mathrm{E}-11$ & $14.313$ &  $248.4$ & Yes & $1.2\mathrm{E}-11$ & $27.535$ & Off-TP\\ 
                
                A8 & $137.4$ & Yes & $5.9\mathrm{E}-12$ & $9.268$ & $152.1$ & Yes & $1.1\mathrm{E}-11$ & $297.179$ & Off-TP\\
                
                A9 & $180.5$  & Yes & $9.7\mathrm{E}-14$ & $17.487$ & $210.1$ & Yes & $3.3\mathrm{E}-11$ & $44.545$ & Off-TP\\
                
                A10 & $203.7$ & Yes & $4.0\mathrm{E}-12$ & $23.128$ & $325.5$ & Yes & $6.4\mathrm{E}-11$ & $29.416$ & Off-TP\\
                    
                \noalign{\smallskip}\hline
            \end{tabular}
        \end{table*}           
        
    \subsection{The Necessity for the Off-TP}
        \label{subSec:4.3-NecessityOff-TP}
         In adherence to the last evaluation matter, the authors tried to use the locally optimal solution for case M1 obtained by the SO-TP without a warm start (use a linear initial guess, see Fig. \ref{fig:CaseM1ctrl} and Fig. \ref{fig:CaseM1stat}) to warm-start the SO-TP for the rest of the cases (M2 to A10). 
         
         Following the previous subsection, the computational time and the resulting $t_{\mathrm{f}}$ are used as comparison between the two warm start methods (by the Off-TP and by the SO-TP). In addition, the feasibility status is also reported. This feasibility is determined by how much the optimization variables violate the equality constraints described in (\ref{eq:initstaton}) to (\ref{eq:sumofangconst}). Infeasibility is reported if one or more variables result in a constraint violation larger than the default tolerance value of $1\mathrm{E}-6$. The summary is presented in Table \ref{tab:4-CompareWarmStart}.
        
        From Table \ref{tab:4-CompareWarmStart}, almost half of the cases have one or more optimization variables that violate the constraints tolerance: infeasible. It is apparent that the solution from the SO-TP without a warm start should not be used as an initial guess for different problems as it can not dictate the search to a feasible solution. This situation implies the nonlinear nature of the NLP and that the SQP requires a good "starting point" for the search to converge to a feasible solution.
        
        Therefore, to maximize the potential of the SO-TP, a good initial guess is important to warm-start the optimization. Such initial guess can readily be obtained by the Off-TP which applicability to many situations have been demonstrated in subsection \ref{subSec:4.1.-MainCases} and summarized in Table \ref{tab:3-CompareWarmandNoWarmStart}. To conclude this subsection, the Off-TP and SO-TP should be implemented together so that both can exploit their respective advantages to compromise each other's disadvantages.
            
\section{Concluding Remarks}\label{Sec:5-ConcludeRemarks}        
    This article demonstrates a warm-started semionline trajectory planner (SO-TP) that is applicable to underactuated vessels, able to guarantee that the feasible trajectory is safe, and able to consider the wind dynamics up to some extent. It has been shown that significant reduction in the computational time of the SO-TP can be achieved with the solution from the offline trajectory planner (Off-TP) as a warm start. The versatility of just one solution from the Off-TP to warm-start the SO-TP for many different scenarios have also been demonstrated. 
    
    However, there is no guarantee that the warm start or any initial guess will result in a feasible solution or computational time speedup. The underactuation and the restricted harbor area greatly limit the existence of the feasible solutions, which is reasonable. This suggests that the presence of a feedback controller is necessary, so that practically when the trajectory planner fails, the previously obtained trajectory can still be used as the set point for the feedback controller.
    
    The semionline (not in real time) nature of the computational time is a direct consequence from the complexity of the problem and the underactuation of the system: a harder NLP and more mathematical function evaluations. Nevertheless, frequently replanning the reference trajectory may lead to the instability of the feedback controller. Moreover, the average computational time from the 14 trial cases shown in this article is less than 30 seconds, which is relatively fast for a full scale ship. For this reason, this article can be regarded as a contribution to realize an online trajectory planner for an underactuated vessel in the future.  
    
    As an idea for the next work, one can construct a database of offline solution by the Off-TP from several initial states and wind conditions. These offline solutions can be organized into a grid system from which the SO-TP will choose one best offline solution to be used as its warm start. This is why the authors introduced the norm $L$ in the discussion: to measure the deviation of the initial states from that of the offline solution, i.e., chose one offline solution that gives the smallest $L$ as the warm start. By doing this, given by the demonstrated versatility of one solution from the Off-TP to speed up the SO-TP for many different situations, it is more likely that the average computational time can be further shortened. This will be implemented in the near future in an actual experiment with the model scale.
    
    Even though not related to autonomous ship, as an example of the implementation of the warm-started SO-TP, a shipmaster can use the resulting trajectory as a guide when maneuvering the ship to dock. Moreover, the feasibility status from the SO-TP can be used as a qualitative measure as to whether the shipmaster should proceed to dock or not. In this sense, when the SO-TP can not give a feasible solution, within the accuracy of the mathematical model of the ship, it is safe to say that the ship can not reach the desired docking states from the given initial states under a certain condition by itself.
    



\section{}\label{}




\begin{figure}[<options>]
	\centering
		\includegraphics[<options>]{}
	  \caption{}\label{fig1}
\end{figure}


\begin{table}[<options>]
\caption{}\label{tbl1}
\begin{tabular*}{\tblwidth}{@{}LL@{}}
\toprule
  &  \\
\midrule
 & \\
 & \\
 & \\
 & \\
\bottomrule
\end{tabular*}
\end{table}




\section{}\label{}

\printcredits

\bibliographystyle{cas-model2-names}

