A Novel Double Layered Hybrid Multi-Robot Framework for Guidance and Navigation of Unmanned Surface Vehicles in a Practical Maritime Environment ^†

Abstract

Formation control and cooperative motion planning are two major research areas currently being used in multi robot motion planning and coordination. The current study proposes a hybrid framework for guidance and navigation of swarm of unmanned surface vehicles (USVs) by combining the key characteristics of formation control and cooperative motion planning. In this framework, two layers of offline planning and online planning are integrated and applied on a practical marine environment. In offline planning, an optimal path is generated from a constrained A* path planning approach, which is later smoothed using a spline. This optimal trajectory is fed as an input for the online planning where virtual target (VT) based multi-agent guidance framework is used to navigate the swarm of USVs. This VT approach combined with a potential theory based swarm aggregation technique provides a robust methodology of global and local collision avoidance based on known positions of the USVs. The combined approach is evaluated with the different number of USVs to understand the effectiveness of the approach from the perspective of practicality, safety and robustness.

1. Introduction

Motivated by the increased presence of autonomous agents, research organisations and industrial firms are putting their effort in the development of unmanned vehicles, able to operate autonomously in the marine environment. Current state of high-performance marine vehicles operating in marine environment were once just a figment of our imagination as a prototype tool. They are consolidated reality of today’s maritime framework, employed in the most diverse array of applications ranging from reconnaissance in hostile areas to operations in dangerous weather conditions to name a few.

Substantial research has been conducted in the last two decades towards increasing autonomy of unmanned surface vehicles (USVs), being the basis of the near-future autonomous ships. Moreover, a step further in the development of autonomous systems is the capability of operating in a team, so as to improve the overall system performance in terms of cost and safety. With this final objective of allowing a multi-USV agent team to navigate autonomously within a commercial route such as coastal area or an harbour, a number of problems have to be solved in order to provide the essential capabilities to the system to operate in an autonomous and safe manner. Formation control and cooperative motion planning are two major areas being investigated in the literature towards the study of the multi vehicle systems. A list of key factors that needs to be considered for designing algorithms related to formation control and cooperative motion planning are already listed in the Figure 1 with formation control being the most widely investigated area. It is quite evident from the Figure 1 that both research areas share a large number of key factors and there is a strong need to develop a hybrid approach combining the key features from both areas. However, it is most important to discuss details of different approaches associated with formation control and cooperative motion planning before delving into the details of the multi USV framework adopted in the current study.

The organisation of the current study is as follows: In Section 2, an overview of the architecture of a multi vehicle system and a comprehensive comparison and analysis of the strategies associated with the multi vehicle system is provided. The adopted methodology, concepts of basic path-following with multi vehicle coordination are discussed in the Section 3 while in the Section 4, results of coordinated vehicles motion are presented. Lastly, conclusions and future work are reported in Section 5.

The current study extends the work of [2] through the incorporation of three major characteristics within the existing framework which are:

• The practical maritime environment of Portsmouth harbour in the existing framework, where an open sea environment has been considered comprising of non-static obstacles.
• The feature of external collision avoidance with shoreline by modelling shoreline as a set of repulsive points using a repulsive potential function.
• An optimal and computationally efficient path within the existing framework for navigating the swarm of USVs.

2. Multi Vehicle System and Formation

The evolution of the concept of the cooperative behaviour of the multi vehicle system is inspired from the animal behaviour such as self organisation of ants or swarming of bees, where the formations help in the survival and evolution of the species ([3,4]). The initial effort in understanding the cooperative behaviour of multi vehicle systems inspired from the animal behaviour started in the 1980s leading to development of ACTor-based robots and equipment synthetic system (ACTRESS), a multi robot system architecture developed in Japan ([5]).

The initial real world application of the multi vehicle system started with the use of UGVs in disaster management followed by the deployment of their applications in area mapping and surveillance ([6,7]). Later this advancement in the technology was extended in space research for the purpose of planetary exploration ([8]). With respect to multi UAV systems, a huge amount of research has happened over last two decades for a variety of applications including disaster management and environmental monitoring ([9,10]) and is still an open research area.

In the research area of the marine robots where a good number of studies conducted during the last decade have happened in the area of multi AUV system towards applications such as bathymetric surveys, ocean monitoring and data acquisition ([11,12]). In terms of a multi USV framework where the current thesis focuses on, fewer studies have taken place in the last decade with a focus on applications such as collaborative anti-submarine warfare, surveillance of territorial waters and underway ship replenishment ([13,14]). It is worth noting from the literature that compared to the research on developing approaches for fleet of UGVs, UAVs and AUVs very fewer studies have taken place in the area of USV formations and there is a strong need to explore the area of cooperative behaviour with a multi USV framework. In fact, owing to the nature of the USV operations, an USV plays an important role in the large scale cross platform cooperation among different unmanned vehicles in the maritime environment as shown in Figure 2. This significance is quite evident from the recent defence reports published by UK and US Department of Defence (DoD) ([15,16]).

2.1. Multi Vehicle System Architecture

A generic architecture towards cooperative behaviour of a multi vehicle formation is shown in Figure 3 which comprises of three layers namely, task management, path planning and task execution [17]. The task management layer takes care of the formation shape and formation allocation of the multi vehicle system based on the requirements of the mission i.e., improving the efficiency of the mission in terms of the spatial and temporal coverage. Towards the literature pertaining to the marine robots, the concept of self organising map (SOM) has been extensively used in the task management of the multi vehicle systems. [18] has used SOM towards dynamic task assignment and path planning of a multi AUV system in a three dimensional underwater environment along with the work of [19] which used the approach of SOM to assign a team of AUVs to achieve multiple target locations in a dynamic ocean environment. With respect to USVs, the work of [20] has used SOM approach integrated with Fast Marching (FM) based reactive method towards collision avoidance.

Figure 2. Schematic of the UAV and unmanned surface vehicle (USV) formation (Source: [21]).

Figure 2. Schematic of the UAV and unmanned surface vehicle (USV) formation (Source: [21]).

Path planning layer is the second layer of the multi vehicle architecture which comprises of three components of environmental mapping, cooperative path planning and real time trajectory modification. A detailed survey towards path planning of single and multiple USVs has been reported in the [22]. However, a number of recent studies needs a special mention in order to deal with real time trajectory modification and emergency situations. The work of [23,24,25,26,27] have made an effort recently to integrate the task planning with dynamic constraints and a practical maritime environment to generate trajectories with practical applications.

Task execution takes generated paths as an input from the path planning module where dynamic variables of the vehicle i.e., velocity, position etc., are used in closed loop control systems to modify the trajectories of the fleet of vehicles in real time. Extensive details of the review papers associated with such marine control systems have already been mentioned in the [28].

2.2. Approaches Associated with Cooperative Behaviour of Multi Vehicle Systems

Two research approaches of formation control and cooperative path planning have been investigated to understand the cooperative behaviour of the multi vehicle system in the literature. A huge amount of research has been conducted in the area of autonomous vehicles over the last four decades in the area of formation control which can be found in the different review studies of [29,30,31]. The key factors associated with the formation control and cooperative path planning are listed in Figure 1. Projects within the European Union and United States are major examples of the development of multi vehicle framework for marine robots. It started with the CADRE system ([32]) project, where a network of AUVs and USVs were used to cooperate autonomously to conduct mine countermeasure using high accuracy navigation and a multi modal architecture. The same framework was extended in the Autonomous Ocean Sampling Network II (AOSN II) project ([33]), where there is complete interconnection and cooperation of aerial, surface and underwater (including gliders) vehicles, with the aim of developing a heterogeneous and “intelligent” monitoring network. Another major milestone was the GREX project ([34]), towards creation of a conceptual framework for heterogeneous swarm of robots working in cooperation to achieve mission goals optimally. Another important work in direction of creating a reliable acoustic underwater network as basis for communication and navigation was conducted through MORPH project [35]. The following section will make a discussion on the various formation control strategies, their comparison and their implementation in the area of marine robotics.

2.3. Formation Control Strategies

The basic classification of the approaches associated with the formation control are shown in Figure 4. Three important features of formation control i.e., shape forming, shape maintenance and shape variation are being compared for their compatibility with each method of the formation control while the criteria of stability and real time for each method is being scored with high and medium as shown in Figure 4. Each of these approaches has its own advantages and disadvantages and their implementation on marine robots is explained in the following subsections.

2.3.1. Behaviour Based Approach

This approach is designed on the basis of the motion primitives associated with each robot such as obstacle avoidance, goal seeking and formation keeping where weighted sum of each of these primitives and interaction between robots is used to create motion patterns for multi vehicle systems. Sensor data are taken as input behaviour and output is sent to robots behaviours which can be viewed together as a structured network of interacting behaviours. A module coordinator finally decides a set of behaviours used to control the robot. This approach was initially proposed in the area of mobile robotics by [36] where a weighted sum of five behaviours namely, move to goal, obstacle avoidance, swirl, noise and formation keeping is used to control the formation of robots. With respect to USVs, the work of [37] used Null-Space-Based behavioural control (NSB) as guidance system aimed at guiding the fleet of surface vehicles in complex environment and simultaneously performing multiple tasks i.e., obstacle avoidance or formation keeping. The work of [38] addressed the concept of International Regulations for Collision Avoidance (COLREGs) compliance navigation of two USVs using the concept of behavioural control and optimality and validated simulations using first in-field demonstration. The studies of [39,40] have covered the area of application of behavioral approach in the area of AUVs.

The parallel and distributed communication among vehicles with less information being shared makes this approach advantageous for real time application although difficult mathematical formulation and unwarranted convergence are a few drawbacks associated with this approach.

2.3.2. Leader-Follower Approach

The leader follower approach works on the principle of few robots considered as leaders while the others act as followers where followers track the position and heading of the leader to some predefined temporal and spatial offset. A schematic of two robots using a basic leader follower controller is shown in the Figure 5.

Two popular feedback control methods as proposed by [41] are being used in this approach namely, $l-\varphi$ control and $l-l$ control. The $l-\varphi$ control method is used to maintain a desired relative distance $l_{ij}$ and a desired relative heading ${\psi }_{ij}$ between two robots as shown in Figure 5 while the $l-l$ method is used to maintain the desired relative distance between three robots i.e., $l_{23}$ and $l_{13}$ as shown in Figure 6.

This scheme has been much preferred in understanding the cooperative behaviour of marine vehicles due to its simplicity and scalability. During the last decade, a good number of studies like [42,43,44,45,46] has adopted a leader-follower framework in formation control of multiple USVs in which different techniques like non linear control, integrator backstepping, sliding mode control, Lyapunov backstepping and angular FM have been used respectively. Towards understanding the cooperative behaviour of AUVs, leader-follower approach has been used in the work of [47,48,49,50,51,52,53,54] through adoption of different control techniques for the formation control of multiple AUVs.

This approach is entirely determined by the trajectory of the leader which simplifies the problem of formation control to a tracking control. The followers follow the leader based on the WiFi communication between leader and followers. The WiFi communication is responsible for the sharing of velocity and position data between followers and leader based on which formation control approaches operate. However, a major drawback of this approach is that formation does not accept leader faults and in case of absence of a leader, a complete formation needs to be redefined.

2.3.3. Virtual Structure (VS) Approach

This approach considers the complete formation as a rigid body where the whole formation is considered as a VS. The dynamics of a single vehicle is translated into a desired motion of each robot in the VS and robot positions are updated according to the local or global path planner. The approach was initially proposed by [55] and a schematic of the approach is shown in Figure 7.

The approach comprises of four major steps as follows:

• Define the dynamics of the VS and align the VS with the initial positions of the robot.
• Define the heading of the VS.
• Compute the individual trajectory for each robot from the start to the goal point as the corresponding robot contribute to an error of $d_i$ as shown in Figure 7.
• Adjust the velocity of each robot to follow the desired trajectory closely, in addition to, maintaining the geometry of the VS.

A good number of studies has been conducted in the area of mobile and aerial robotics using a VS approach ([56,57,58]) while the work of [59] covers the area of formation control of underactuated ships using a VS approach. Despite its high fault tolerant capability, this approach suffers from lack of flexibility and high computational cost in a complex operational environment.

2.3.4. Artificial Potential Function

The concept was initially proposed by [60] and has been quite popular in the last decade in the formation control of multi robots systems and is quite evident from the work of [61,62,63,64]. In the area of marine robotics, some recent work in the application of artificial potential function towards formation control can be found in [65,66,67,68].

This approach can be applied in real time but suffers from the issues of local minima.

2.3.5. Graph Theory Based Approach

In this approach, kinematic or dynamic properties of the robots are expressed as nodes of a graph with edges representing constraints between the robots. Under this approach, control theory and dynamical systems theory is applied in combination with graph theory to study the formation controller and its stability. Many recent studies of [69,70,71] have used the communication network based control strategy, consensus based control strategy and stop-and-go strategy respectively to understand the formation control using graph theory. This method can represent any formation using graph although simulation hitherto remains a major drawback.

3. Methodology

The current study adopted a two layered approach towards the multi-USV framework problem. In the higher level of the hierarchy, a robust path planner based on a constrained A* approach was adopted to generate optimal waypoints, which were later smoothed using a polyfitting operation. This smoothed trajectory was given as an input to a lower level guidance system based on a virtual target (VT) approach integrated with a swarm aggregation algorithm based on an attraction- repulsion strategy. Figure 8 shows a schematic of the methodology adopted in the present study. The current study was simulated in a practical maritime environment of Portsmouth harbour using MATLAB. Portsmouth harbour was chosen here as the area of interest due to its significant commercial and industrial use in the UK maritime industry. All simulations were performed on a PC with Microsoft Windows 7 as OS with Intel i5 2.70 GHz quad core CPU and 16 GB RAM.

The current study adopted an A* approach with a USV enclosed by a circular boundary as a safety distance constraint on generation of optimal waypoints. The A* approach is a grid based path planning approach designed to find the shortest path in a cluttered environment by minimising the heuristic cost [72]. This resolved the problem of optimal path planning for an USV moving in a practical maritime environment, leading to generation of safer way-points with conservation of energy. Figure 9 describes a comparison of the path generated by conventional grid-based method against the path generated by conventional grid-based method considering safety distance and surface ocean currents.

A constrained A* approach as described in the [72] with a safety distance of 20 pixels was chosen in the current study to generate optimal waypoints in a constrained channel of Portsmouth harbour. A zigzag trajectory was produced from the waypoints generated from the proposed approach. The chosen waypoints for smoothed trajectory are tabulated in

Table 1. Chosen optimal waypoints (WPs) from path planner.

	Start	WP 1	WP 2	WP 3	Goal
x (pixels)	238	261	272	285	299
y (pixels)	212	251	271	284	312

. The waypoints were chosen so that complexity of the navigation in a constrained harbour was accounted for in an offline approach. The generated trajectory from the safety distance constrained A* approach and smoothed trajectory from chosen waypoints are shown in Figure 10.

The chosen waypoints were fitted to a polynomial parameterised in terms of $\gamma$∀$\gamma$∈[0,61]. Parametric equations, $P_d\left(\gamma \right)$ used as an input towards online approach of path following and swarm aggregation are shown in Equation (1).

$$P_d\left(\gamma \right)=\left\{\begin{array}{c}x\left(\gamma \right)=\gamma +238,\hfill \\ y\left(\gamma \right)=-0.000325{(\gamma +238)}^3+0.2637{(\gamma +238)}^2\hfill \\ ..-72.5508(\gamma +238)+6505\hfill \end{array}\right.$$

(1)

3.1. Preliminaries Related to Multi Vehicle Path Following

In this study, the swarm as defined by [73] was composed of a set X of n robots, each one characterised by the vector $x_i\left(t\right)\in {\mathbb{R}}^m$ with $i=1,\dots ,n$ in Cartesian coordinates on a 2D plane. The instantaneous barycentre of the swarm was defined as $\tilde{x}\left(t\right)=1/n{\sum }_{i=1}^n{x}_{i}i\left(t\right)$ and the vector distance of each robot from the centre is defined as ${ϵ}_i\left(t\right)=x_i\left(t\right)-\tilde{x}\left(t\right)$. The collection of all robots and their distances from the barycentre was defined as $X\left(t\right)=[x_1\left(t\right)\dots \dots .x_n\left(t\right)]$ and $ϵ\left(t\right)=[{ϵ}_1\left(t\right)\dots \dots .{ϵ}_n\left(t\right)]$.

The proximity graph $G_i=\{V,E\}$, where $V=\{v_i:i=1,\dots \dots ,n\}$ is the set of robots while the $E=\left\{{ϵ}_{ij}\right\}$ is the set of edges representing communication channel between the robots and the centre, defined the interaction among the robots. The communication channel existed if the agents i and j were within the visibility range $R_{ij}$ i.e., $\parallel x_i-x_j\parallel \le R_{ij}$ with unidirectional communication from node i to node j. The Laplacian matrix associated with this graph G was defined as $L\left(G_i\right)=\Delta \left(G_i\right)-A\left(G_i\right)$, where the $A\left(G_i\right)$ is the adjacency matrix of $n\times n$ elements whose generic element $a_{ij}=1$ if $i\ne j$ and ${ϵ}_{ij}\in E$ and $a_{ij}=0$ otherwise while $\Delta \left(G_i\right)$ is the degree diagonal matrix of $n\times n$ elements whose elements are ${\Delta }_i\left(G_i\right)$ i.e., degree of the node i. This matrix was a weak diagonal symmetric matrix where the sum of rows and sum of columns were equal to zero.

The current study took into account the idea of guiding a complete swarm of USVs through an instantaneous barycentre of swarm acting as a VT that moves along the reference path where following assumptions are required:

• Onboard Global Positioning System (GPS) is continuously measuring position of each USV in the swarm with reference to a common reference frame.
• WiFi systems are providing a reliable data exchange needed to support inter-robot decentralised communication for position data sharing.

In order to achieve the above mentioned idea, a proper velocity is computed and imposed to the swarm aggregation algorithm, which in turn drive the motion of each USV in the USV swarm according to such reference velocity.

3.2. Path Following Algorithm

A graphical representation of the USV kinematics adopted in the current study for development of the path following algorithm is given in Figure 11. In general, two reference frames namely, Earth fixed reference frame $<e>$, where position and orientation ${\left[xyz\mathsf{\Psi }\right]}^T$ of the USV is expressed and the body fixed reference frame $<b>$, where the relative surge and sway velocity ${\left[u_r{v}_r\right]}^T$ of the USV with respect to the water and yaw rate r are used to define the kinematics in the $<e>$ frame as follows ([74]):

$$\begin{array}{c}\dot{x}=u_{r}cos\mathsf{\Psi }-v_{r}sin\mathsf{\Psi }+\dot{x_c}\hfill \\ \dot{y}=u_{r}sin\mathsf{\Psi }+v_{r}cos\mathsf{\Psi }+\dot{y_c}\hfill \\ \dot{\mathsf{\Psi }}=r\hfill \end{array}$$

(2)

where $\left[\dot{x_c}\dot{y_c}\right]$ denotes the sea current which is supposed to be irrotational and constant. Assuming that USV is moving with a constant surge with respect to the water with negligible sway, i.e., $v_r=0$ and $\dot{u_r}=\dot{v_r}=0$, the kinematic model represented in the Equation (2) can be rewritten as follows:

$$\begin{array}{c}\dot{x}=Ucos{\mathsf{\Psi }}_e\hfill \\ \dot{y}=Usin{\mathsf{\Psi }}_e\hfill \\ \dot{{\mathsf{\Psi }}_e}=r\left[\frac{u_r^2}{U^2}+\frac{u_r}{U}(\dot{x_c}cos\mathsf{\Psi }+\dot{y_c}sin\mathsf{\Psi })\right]=r\eta \left(t\right)\hfill \end{array}$$

(3)

where

$$U=\sqrt{{\dot{x}}^2+{\dot{y}}^2};{\mathsf{\Psi }}_e=arctan\frac{\dot{y}}{\dot{x}}$$

(4)

denote the resultant velocity and orientation of the USV in $<e>$. The current work uses the concept of Serret–Frenet frame $<f>$ which moves along the reference path followed by the VT vehicle and tracked by the real vehicle. With reference to Figure 11, a point P with position vector $P={\left[x_p{y}_{p}0\right]}^T$ is defined in respect to $<e>$ frame. The point B, attached to the USV is expressed as ${\left[s_1{y}_{1}0\right]}^T$ in $<f>$ frame. The rotational matrix to transform the parameters from $<e>$ to $<f>$ is defined in terms of ${\mathsf{\Psi }}_f$ as follows:

$$R=\left[\begin{array}{ccc}cos{\mathsf{\Psi }}_f& sin{\mathsf{\Psi }}_f& 0\\ -sin{\mathsf{\Psi }}_f& cos{\mathsf{\Psi }}_f& 0\\ 0& 0& 1\end{array}\right]$$

Assuming $r_f={\mathsf{\Psi }}_f$ and defining s as the signed curvilinear abscissa along the path, following expressions hold:

$$r_f=\dot{{\mathsf{\Psi }}_f}=c_c\left(s\right)\dot{s};c_c\left(s\right)=g_c\left(s\right)\dot{s}$$

where, $c_c\left(s\right)$ and $g_c\left(s\right)=d{c}_c\left(s\right)/ds$ denote the path curvature and its derivative respectively. The velocity of P in the $<f>$ frame is defined as:

$${\left(\frac{dp}{dt}\right)}_f=\left[\begin{array}{c}\dot{s}\\ 0\\ 0\end{array}\right]$$

The velocity of B in the $<e>$ frame is defined as:

$${\left(\frac{dx}{dt}\right)}_e={\left(\frac{dp}{dt}\right)}_e+R^{-1}{\left(\frac{dd}{dt}\right)}_f+R^{-1}({\left[00r_f\right]}^T\times d)$$

where, d is the position vector from P to B. The velocity of B in the $<f>$ frame is obtained by multiplying the above equation with R and is as follows:

$${\left(\frac{dx}{dt}\right)}_f=R{\left(\frac{dx}{dt}\right)}_e={\left(\frac{dp}{dt}\right)}_f+{\left(\frac{dd}{dt}\right)}_f+{\left[00r_f\right]}^T\times d$$

(5)

Based on the following relations

$${\left(\frac{dx}{dt}\right)}_e=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ 0\end{array}\right]$$

$${\left(\frac{dd}{dt}\right)}_f=\left[\begin{array}{c}\dot{s_1}\\ \dot{y_1}\\ 0\end{array}\right]$$

and

$${\left[00r_f\right]}^T\times d=\left[\begin{array}{c}0\\ 0\\ c_c\left(s\right)\dot{s}\end{array}\right]\times \left[\begin{array}{c}\dot{s_1}\\ \dot{y_1}\\ 0\end{array}\right]=\left[\begin{array}{c}-c_c\left(s\right)\dot{s}{y}_1\\ c_c\left(s\right)\dot{s}{s}_1\\ 0\end{array}\right]$$

Equation (5) can be rewritten as

$$R\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ 0\end{array}\right]=\left[\begin{array}{c}\dot{s}[1-c_c\left(s\right)y_1]+\dot{s_1}\\ \dot{y_1}+c_c\left(s\right)\dot{s}{s}_1\\ 0\end{array}\right]$$

Solving for $\dot{s_1}$ and $\dot{y_1}$ gives

$$\dot{s_1}=\left[cos{\mathsf{\Psi }}_{f}sin{\mathsf{\Psi }}_f\right]\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]-\dot{s}(1-c_c{y}_1)$$

$$\dot{y_1}=\left[-sin{\mathsf{\Psi }}_{f}cos{\mathsf{\Psi }}_f\right]\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]-c_c\dot{s}{s}_1$$

(6)

Replacing the top two equations in Equation (6) and introducing the variable $\beta ={\mathsf{\Psi }}_e-{\mathsf{\Psi }}_f$, the final kinematic model in terms of the $<f>$ frame, i.e., ($s_1$,$y_1$) coordinates are as follows:

$$\begin{array}{c}\dot{s_1}=-\dot{s}(1-c_c{y}_1)+Ucos\beta \hfill \\ \dot{y_1}=-c_c\dot{s}{s}_1+Usin\beta \hfill \\ \dot{\beta }=r_e-c_c\dot{s}\hfill \end{array}$$

(7)

where $r_e=\dot{{\mathsf{\Psi }}_e}=r\eta \left(t\right)$

Following the geometrical and kinematical analysis carried above, the distance error model, expressed with respect to the frame $<f>$, has the following form ([74]):

$$\left\{\begin{array}{c}\dot{\rho }=\left(c_c\nu -1\right)\dot{s}+Ucos\beta \hfill \\ \dot{\nu }=-c_c\dot{s}\rho +Usin\beta \hfill \end{array}\right.$$

(8)

In order to solve the path-following problem for a single-vehicle system, the aim was to develop a proper approach angle function ${\mathsf{\Psi }}^{*}$, designed to reduce the linear error components ($\rho$ and $\nu$) to zero. The desired angle ${\mathsf{\Psi }}^{*}$ was a function of the cross-track error $\nu$ summed with the local path tangent, thus ${\mathsf{\Psi }}^{*}={\mathsf{\Psi }}_f+\phi \left(\nu \right)$, where the function $\phi \left(\nu \right)$ was required to satisfy the following constraints:

$$\left|\phi \left(\nu \right)\right|<\frac{\pi }{2};\nu \phi \left(\nu \right)\le 0;\phi \left(0\right)=0$$

Relying on a low level PI controller, providing an auto-heading regulator capable of tracking desired orientation profiles, it can be stated that considering the candidate Lyapunov function $V_{\psi }=\frac{1}{2}{(\psi -{\psi }^{*})}^2$, the low level controller provided a behaviour such that ${\dot{V}}_{\psi }\le 0$, i.e., the vehicle orientation converged to the desired angle $\psi \to {\psi }^{*}$ and it can be rewritten as $\beta \to \phi \left(\nu \right)$. Moreover it is worth noticing that when ${\dot{V}}_{\psi }=0$, an invariant set was defined, in which the condition $\beta =\phi \left(\nu \right)$ holds. The task of the path-following controller design was achieved by the definition of the Lyapunov function $V=\frac{1}{2}({\rho }^2+{\nu }^2)$; computing the time derivative of the function V, the following expression was obtained:

$$\begin{array}{cc}\hfill \dot{V}& =\rho \dot{\rho }+\nu \dot{\nu }=-\rho \dot{s}+\rho Ucos\beta +-\nu Usin\beta ={\dot{V}}_{\rho }+{\dot{V}}_{\nu }\hfill \end{array}$$

substituting $\dot{\rho }$ and $\dot{\nu }$ with the equation system Equation (8) and defining ${\dot{V}}_{\rho }=-\rho \dot{s}+\rho Ucos\phi \left(\nu \right)$ and ${\dot{V}}_{\nu }=-\nu Usin\phi \left(\nu \right)$.

The speed of the reference frame $\dot{s}$, i.e., the velocity of the VT moving along the path, could be used as an additional control variable. Imposing

$${\dot{s}}^{*}=K_{\rho }\rho +Ucos\beta$$

(9)

as the desired VT speed, where $K_{\rho }$ is a tunable controller parameter, the function ${\dot{V}}_{\rho }$ assumes the negative form ${\dot{V}}_{\rho }=-K_{\rho }{\nu }^2\le 0$. About ${\dot{V}}_{\nu }$, recalling the above-mentioned assumption on the attraction to the invariant set defined by ${\dot{V}}_{\psi }=0$, $\beta$ can be substituted by $\phi \left(\nu \right)$, obtaining ${\dot{V}}_{\nu }=\nu Usin\phi \left(\nu \right)$. Selecting the function $\phi \left(\nu \right)$ as

$$\phi \left(\nu \right)=-{\psi }_{a}tanh\left(K_{\nu }\nu \right)$$

(10)

with $K_{\nu }$ as a tunable controller parameter and ${\psi }_a$ the maximum approach angle with respect to the local tangent ${\psi }_f$, the term $\nu Usin\phi \left(\nu \right)$ is $\le 0$ because of the assumption made on the function $\phi \left(\nu \right)$. Being the terms ${\dot{V}}_{\rho }$ and ${\dot{V}}_{\nu }$$\le 0$, thus entailing $\dot{V}\le 0$, the global asymptotic stability for the path-following guidance system was proven.

3.3. Vehicle Coordination

The goal of coordinating an USV team to converge to and maintain a motion configuration, while at the same time moving along a desired reference path, was realised through the definition of the following control input:

$${\dot{x}}_i=u_i^s+u^g$$

(11)

where the term $u_i^s$, different for each USV, is the control effort required to reach a collective behavior while the term $u^g$, common to all the USV, refers to the expected trajectory of the fleet centroid, computed with reference to Section 3.2 as:

$$u^g=\left[\begin{array}{c}{u}^{*}cos{\psi }^{*}\\ u^{*}sin{\psi }^{*}\end{array}\right]$$

(12)

where $u^{*}$ is the desired speed for the formation along the path and ${\psi }^{*}$ is the reference guidance angle computed by the path-following module.

Considering a swarm composed of n robots, the following aggregation dynamics for each robot i is given:

$$u_i^s=\sum _{j\ne i}g(x_i-x_j)$$

(13)

where $g(·)$ is the interaction function representing the function of attraction and repulsion between neighbouring robots. In particular, $g(·)$ is defined as:

$$g\left(y\right)=-y\left[g_a(\parallel y\parallel )-g_r(\parallel y\parallel )\right],\forall y\in {\mathbb{R}}^m.$$

(14)

where $g_a(·)$ is the attractive function and $g_r(·)$ is the repulsive contribution, constrained by the following assumptions:

$$\begin{array}{cc}\hfill g_a(\parallel x_i-x_j\parallel )& \ge \alpha \hfill \\ \hfill g_r(\parallel x_i-x_j\parallel )& \le \frac{\beta }{\parallel x_i-x_j{\parallel }^2}\hfill \end{array}$$

(15)

In order to maintain a practical equilibrium between the swarm formation term $u_i^s$ and the path-following guidance term $u_g$, the $u_i^s$ component is modified as follows:

$${\dot{x}}_i=k_{sat}\frac{{\sum }_{j\in {\mathcal{N}}_i\left(t\right)}g(x_i-x_j)}{1+∥{\sum }_{j\in {\mathcal{N}}_i\left(t\right)}g(x_i-x_j)∥},$$

(16)

where $k_{sat}>0$ is the saturation gain.

The stability of the overall system, originated by the interconnection between the path-following and swarm aggregation modules, is defined in [2].

4. Results and Discussion

In Figure 12, the experiments were conducted for a swarm of three USVs in a practical maritime environment. The robots were initially configured by placing them parallel to each other and then the robots evolved during the motion using attractive and repulsive potential functions derived from Equation (15) which is defined as:

$$\begin{array}{c}\hfill g_a\left(d\right)=\alpha \\ \hfill g_r\left(d\right)=\frac{\beta }{d-2\eta }\end{array}$$

(17)

where d is the Euclidean distance between two robots $\parallel x_i-x_j\parallel$. In addition to this, the internal collision among the vehicles was determined by the repulsive function which determined that vehicles remained at a certain distance from each other i.e., a distance of more than $2\eta$ in all cases. This additional term has the format $g_{ca}\left(d\right)$ which is defined as:

$$\begin{array}{c}\hfill g_{ca}\left(d\right)=\frac{2\eta -d}{d^2}\end{array}$$

(18)

which holds for the condition 0 ≤ d ≤ $2\eta$.

The robots had to follow the parametrised curve from Equation (1). Figure 13 shows the reference velocity profile for three USVs, where velocity of the i-th robot is $\parallel {\dot{\mathit{x}}}_{\mathit{i}}\parallel$. It should be noted that the formation centroid converged to the reference path showing that the proposed approach was effective in nature. Furthermore, in Figure 14, for each vehicle in a swarm of three USVs, the actual surge speed and heading angle (blue lines) were compared with the generated speed and orientation references (red lines); $u_1$ and ${\psi }_1$ correspond to the blue vehicle, $u_2$ and ${\psi }_2$ correspond to the green vehicle and $u_3$ and ${\psi }_3$ correspond to the red vehicle. It should be noted that despite the presence of oscillations in the reference signals, the tracking of surge speed and orientation by means of a dynamic controller was achieved by the proposed approach.

Similarly, in Figure 15, the experiments were conducted for a swarm of four USVs in a practical maritime environment to track a reference path based on the parametrised equation. Similar to Figure 13, Figure 16 shows the reference velocity profile for four USVs. Furthermore, in Figure 17, for each vehicle in a swarm of four USVs, the actual surge speed and heading angle (blue lines) were compared with the generated speed and orientation references (red lines); $u_1$ and ${\psi }_1$ correspond to the blue vehicle, $u_2$ and ${\psi }_2$ correspond to the green vehicle, $u_3$ and ${\psi }_3$ correspond to the black vehicle and $u_4$ and ${\psi }_4$ correspond to the red vehicle.

In order to ensure that complexity of the multi-USV operation in a constrained maritime environment is captured, this section reports results of three and four vehicles performing swarm aggregation and path-following from a initial parallel configuration towards a reference path generated from proposed path planner. Figure 12 and Figure 15 show the aggregation behaviour combined with motion against the reference path for three and four USVs respectively. Initial, intermediate and final positions of the formation are shown in Figure 12 and Figure 15. The reported results accounted for external collision with the shoreline into swarm evolution through attractive and repulsive functions introduced in [2]. The path-following module parameters were set to

$$K_{\rho }=1.0,K_{\nu }=0.8,\mathrm{and}{\psi }_a=\pi /3$$

Collision avoidance with the shoreline was simply implemented by considering the shore profile as a set of repulsive fixed points which, within a certain distance, concur in the vehicle motion evolution. The repulsive function $f\left(x\right)$ used to define the shoreline is $f\left(x\right)=-(5\eta -d_i)/d_i$, where $d_i$ is the Euclidean distance between the robot and the shoreline $\parallel x_i-x_{shoreline}\parallel$. The motion of the vehicles with respect to mutual agent interactions and to distance from the shoreline could be varied acting on the parameters of the attractive and repulsive functions. For all simulations, the values of the attraction and repulsion function parameters were set to $\alpha$ = 0.2, $\beta$ = 1.2 and $\eta$ = 3. Regarding the velocity profiles, two main issues have to be discussed:

• The oscillations in the required velocity of each USVs were due to the swarm aggregation functions; the oscillatory behaviour could be reshaped by different definition of attraction/repulsion functions.
• The overall guidance system took into account the physical limits of the vehicle i.e., in the current study the maximum and minimum manoeuvring speeds of scientific USVs were considered by setting the value of $K_{sat}$ = 0.5.

It should be noted that formation is a function of initial position and evolution along a desired reference. In Figure 14 and Figure 17, actual surge speed and heading angle are compared with reference speed and orientation for three and four USVs framework respectively. In order to highlight the effectiveness of the combined approach, physical limits of a USV were accounted for in the guidance system by bounding the maximum and minimum manoeuvring speed between 0.2 m/s and 1.5 m/s as shown in Figure 13.

5. Conclusions and Future Work

In this study, the integration of constrained A* path planner with VT path following guidance for multi-agent USV is reported. By way of introduction, an exhaustive study of different approaches proposed in the literature has been discussed. Currently there are few works which have proposed the hybrid cooperative framework for multi USV systems.

In this context, the main contributions of the current study are summarised as follows:

• Integration of a constrained A* approach with a decentralised VT guidance approach combined with a potential field based swarm aggregation technique for the cooperative navigation of multi USVs.
• Combining the important features of optimal path, computational time and external collision avoidance with shoreline within the initial approach proposed by [2].
• A constrained practical maritime environment is being considered to guide and navigate the swarm of USVs through a narrow channel on Portsmouth harbour which has not been studied till now in the literature.
• Towards the collision avoidance with the external shoreline, a shore profile as a set of repulsive fixed points is being included in the existing approach proposed by [2] and a modified approach is proposed.

In future work, it is expected to implement the proposed approach on scientific USVs such as Springer [22] and Charlie [74] to experimentally validate the simulated methodology. In addition to that, most leading companies in USV operations are looking for the integration of COLREGs to abide the working guidelines of the International Maritime Organisation (IMO). A challenging extension of the current work lies in fact of incorporating rules of COLREGs in the current hybrid framework.