Visual Odometry-Based Robust Control for an Unmanned Surface Vehicle under Waves and Currents in a Urban Waterway

Abstract

This paper proposes the study of a robust control scheme for an Unmanned Surface Vehicle (USV) in a urban waterway using Visual Odometry (VO) for position estimation of the vehicle instead of traditional sensors. For different applications, a USV is required to operate in autonomous navigation mode and under various disturbances inherent to its environment as currents and waves. To achieve efficient operation, a robust control scheme is required to cope with such disturbances. The autonomous navigation is achieved by using VO to estimate the vehicle’s position for denied Global Positioning Systems (GPS) environments. A robust control scheme based on Sliding Modes (SM) control theory is designed and its convergence is guaranteed by means of Lyapunov analysis. The tracking capabilities under disturbance conditions using VO for position estimation are verified in simulation using the virtual world tool from Matlab and Simulink.

1. Introduction

In the last decade, autonomous vehicles (AV) have attracted the attention of researchers around the world, owing to their broad range of possible applications. Unmanned Surface Vehicles (USV) are a particular class of AV which operate on the surface of water. These are commonly small boats used mainly for testing control algorithms, or in practical applications as surveillance or data acquisition in water environments in which it is not possible to send a manned vessel [1]. Other applications for USV include bathymetry, environmental monitoring and disaster management, which can be found in [2,3,4].

The dynamical model of USV is essential for the study and design of control systems that fulfill the desired objectives [5,6,7]. This dynamical model is commonly developed through kinematic and kinetic studies based on the Newton–Euler approach, which allows the consideration of forces and moments acting over the vehicle in each degree of freedom.

To guarantee efficient navigation of a USV, one of the most important parts is the autonomous navigation system used to perform sensing and perception tasks, path planning and control. The control module is designed to perform robust tracking of the desired trajectories. Hence, such robust controller design is an essential stage, since the USVs face challenges such as uncertainties, external disturbances, nonlinearities and saturation, among others. In [8], a trajectory tracking control problem for USVs with motion constraints and varying ocean currents is presented, based on backstepping and a normalization technique. To cope with actuator saturation, a saturation tracking controller based on generalized saturation functions is developed in [9]. Meanwhile, a multilayer neural network—together with adaptive control techniques—is used to obtain robust operation against unmodeled dynamics and environmental disturbances induced by waves and ocean currents. In [10] a disturbance observer-based control is developed to estimate the unknown environmental disturbances affecting the USV with a tracking control strategy for optimization of energy consumption and enhancement of tracking performance via the port-controlled Hamiltonian techniques. An adaptive sliding modes control scheme for USV modeling uncertainties and input saturation is presented in [11], in which a neural network is used to approximate unmodeled dynamics and a hyperbolic tangent function is considered to handle the chattering phenomenon.

Recently, Unmanned Surface Vehicles have had integrated cameras mounted on their structure to provide information about its surrounding environment. These cameras can be used to complement measurements taken by traditional sensors, or even substitute them; detect floating objects on the water surface; and perform collision avoidance or path planning tasks. Among the different applications for such visual input, the estimation of motion is an important task for autonomous navigation of Unmanned Autonomous Vehicles (UAV). The process of estimating the motion of a vehicle using data provided by a single or multiple cameras is called visual odometry (VO) [12]. In VO, the changes present in recorded images produced by motion are used to compute the incremental pose estimate between frames, with the consideration of a static and textured scene, sufficient illumination in the environment and enough scene overlap between consecutive frames [13]. Visual Odometry can be performed for stereo or monocular configurations. Stereo VO accounts for the case in which the relative 3D position of the features is directly measured by triangulation at every vehicle location and used to derive the relative motion, while in monocular VO, the relative motion and 3D structure must be computed from 2D bearing data. The main problem in monocular VO is that motion can only be recovered up to a scale factor, and the absolute scale should be determined from direct measurements, motion constraints or using the trifocal tensor [14]. Most applications of VO are developed for ground vehicles; however, there exists an increasing interest for applications in aerial and marine vehicles. The use of VO in marine environments is a good alternative to GPS-based systems, due to the existence of GPS-denied regions—for instance, in underwater environments, where the position estimation using VO allows the autonomous navigation of the vehicle [15,16]. One outstanding example of such an application can be found in the motion estimation of the NASA Mars Rovers as a supplement to dead reckoning [17]. In [18], VO is used in autonomous underwater vehicles for coral-reef monitoring tasks.

Another area of application of USVs is navigation through urban waterways. Urban waterways are defined as waterways and water bodies located near urban environments, including natural features (e.g., wetlands, lakes, rivers and estuaries) and those constructed as part of a drainage system. Such environments are characterized by having sceneries with several objects and textures, which allow the use of motion estimation algorithms as VO. The research on USV in urban waterways is focused on transportation and water quality monitoring. In [19] a semi-immersible USV is developed for navigation in swallow waters as ports, rivers or lakes for monitoring tasks. A cargo transport task in urban waterways networks is analyzed in [20] with a cooperative multi-vessel system. In [21], a USV robot is developed in order to act as a measurement tool to monitor water pollution in a Peruvian lake due to the presence of a hydroelectric power plant, mining industries and touristic activities near the surroundings. A localization system based on an Extended Kalman Filter for A USV in urban waterways is proposed in [22], the system integrates VO with LiDAR to achieve autonomous navigation and trajectory tracking without using GPS measurements.

There exist several methods for motion estimation and robust control of marine vehicles, but few works address the integration of computer vision algorithms and control schemes for Unmanned Surface Vehicles [23,24]. Hence, this paper proposes the generation of a robust controller for a USV to perform trajectory tracking using a vision algorithm to estimate the vehicle’s position instead of traditional sensors in urban waterways environments, mainly rivers or lakes. The controller is synthesized using sliding modes techniques, which provide robustness against environmental disturbances such as as currents and waves, and a monocular VO scheme is used to estimate the vehicle’s position. To verify the effectiveness of the proposal, a simulation platform is developed using the virtual world editor from Matlab.

The remainder of this paper is organized as follows: Section 2 presents the system modeling, including the USV and environmental disturbances; Section 3 shows the proposed robust control based on visual position estimation detailing the VO algorithm and the robust control scheme based on sliding modes. Finally, Section 4 and Section 5 illustrate the results and main conclusions of this work.

2. System Modeling

2.1. USV Model

The mathematical model of the considered USV is obtained through static and dynamic principles on a local reference frame in order to take advantage of the geometrical properties of the vehicle. The model is based on the concept of conservation of linear and angular moment, which is directly related to Newton’s second law. The motion of a USV considering six degrees of freedom is defined by a translation motion (position) in three directions: surge, sway and heave; and by a rotation motion (orientation) about three axes: roll, pitch and yaw. The interested reader can find a highly detailed explanation about the modeling process in [5].

Consider the following vectors

$$\mathbf{\eta }={\left[{\mathbf{\eta }}_1{\mathbf{\eta }}_2\right]}^T={\left[xyz\varphi \theta \psi \right]}^T$$

(1)

$$\mathit{v}={\left[uvwpqr\right]}^T$$

(2)

where $\mathbf{\eta }$ stands for the vehicle’s position and attitude vector with coordinates on the inertial frame (earth fixed) and $\mathit{v}$ represents the linear and angular velocity measured in the referential frame (body fixed). The model containing the kinematic and kinetic parts is given by

$$\dot{\mathbf{\eta }}=J\left({\mathbf{\eta }}_2\right)\mathit{v}$$

(3)

$$M\dot{\mathit{v}}+C\left(\mathit{v}\right)\mathit{v}+D\left(\mathit{v}\right)\mathit{v}={\mathbf{\tau }}_E+\mathbf{\tau }$$

(4)

in which M is the system inertia matrix, $C\left(\mathit{v}\right)$ is the Coriolis centripetal matrix, ${\mathbf{\tau }}_E$ are the forces and moments related to environmental disturbances, $\mathbf{\tau }$ is the vector containing the external forces and moments acting on the rigid body and $D\left(\mathit{v}\right)$ is the matrix related to hydrodynamic drag, including linear and quadratic terms. On the other hand, $J\left({\mathbf{\eta }}_2\right)$ is a diagonal matrix formed by two rotation matrices $J_1\left({\mathbf{\eta }}_2\right)$ and $J_2\left({\mathbf{\eta }}_2\right)$, defined as

$$J_1\left({\mathbf{\eta }}_2\right)=\left[\begin{array}{ccccc}{c}_{\psi }{c}_{\theta }& & -s_{\psi }{c}_{\varphi }+c_{\psi }{s}_{\theta }{s}_{\varphi }& & s_{\psi }s\varphi +c_{\psi }{c}_{\varphi }{c}_{\theta }\\ s_{\psi }{c}_{\theta }& & c_{\psi }{c}_{\varphi }+s_{\varphi }{s}_{\theta }{s}_{\psi }& & -c_{\psi }{s}_{\varphi }+s_{\theta }{s}_{\psi }{c}_{\varphi }\\ -s_{\theta }& & c_{\theta }{s}_{\varphi }& & c_{\theta }{c}_{\varphi }\end{array}\right]$$

(5)

$$J_2\left({\mathbf{\eta }}_2\right)=\left[\begin{array}{ccccc}1& & s_{\varphi }{t}_{\theta }& & c_{\varphi }{t}_{\theta }\\ 0& & c_{\varphi }& & -s_{\varphi }\\ 0& & \frac{s_{\varphi }}{c_{\theta }}& & \frac{c_{\varphi }}{c_{\theta }}\end{array}\right]$$

(6)

where $J_1{\left({\mathbf{\eta }}_2\right)}^{-1}=J_1{\left({\mathbf{\eta }}_2\right)}^T\in OS\left(3\right)$ and $OS\left(3\right)$: Orthogonal Special group (three dimensions) and the nomenclature $c,s,t$ stands for $cos(·),sin(·)$ and $tan(·)$, respectively.

The inertial and referential frames are illustrated in Figure 1, where r and $r_G$ are the vectors that connect the center of gravity with the origin of the inertial and referential frame, respectively, and $r_O$ is the vector that connects the origins of the referential and inertial frames.

A more detailed model can be obtained considering the following description for system variables: $M=M_{RB}+M_A$ and $C\left(\mathit{v}\right)=C_{RB}\left(\mathit{v}\right)+C_A\left(\mathit{v}\right)$, where $M_{RB}$ and $C_{RB}\left(\mathit{v}\right)$ are the inertia and Coriolis/centripetal force matrices while $M_A$ and $C_A\left(\mathit{v}\right)$ represent the added mass matrices due to the inertia of liquid surrounding the rigid body.

When the vehicle moves through an ideal fluid $M_{RB}=M_{RB}^T>0$ and $M_A=M_A^T$. The Coriolis and centripetal force matrix $C\left(\mathit{v}\right)$ is skew-symmetric, and $D\left(\mathit{v}\right)$ is nonsymmetric and strictly positive. It is worth noting that matrix $J_2\left({\mathbf{\eta }}_2\right)$ shows a singularity at $\theta =\pm {90}^{\circ }$, but it is not a problem for ships due to the limited working space for pitch angle, which is way smaller than such a value.

The USV’s motions in pitch, heave and roll can be neglected in comparison to the other motions for conventional surface ships, especially under the consideration of calm water surface; thus, ship motion modeling can be considered with a reduced number of degrees of freedom [25,26].

2.2. Environmental Disturbances

The mathematical representation of the main disturbances found in water environments is introduced to analyze its effects in the USV movement. Such disturbances are related to wind, waves and currents. The wind and waves are usually included in the dynamical model by superposition; that is,

$${\mathbf{\tau }}_E={\mathbf{\tau }}_E^{wa}+{\mathbf{\tau }}_E^{wi}$$

(7)

where ${\mathbf{\tau }}_E^{wa}$ and ${\mathbf{\tau }}_E^{wi}$ are the forces and moments of waves and wind, respectively. In this work, the wind effects are neglected and only the waves’ effects are considered.

The marine waves are a phenomenon which is present in almost any water zone, to a lesser or greater extent, described as undulation produced over the water surface. The modeling and generation process of waves have been addressed in the literature for different purposes, as shown in [27,28], and for studies about energy conversion, such as [5,29]. In this paper, the wave disturbances are modeled using a first-order model of a wave.

Let us define the slope of the wave $s_i\left(t\right)$ for the i-th component as

$$s_i\left(t\right)=A_i\frac{2\pi }{{\lambda }_i}sin({\omega }_{ei}t+{\varphi }_i)$$

(8)

where $A_i$ and ${\lambda }_i$ are the wave amplitude and length, respectively, ${\omega }_{ei}$ is the encounter frequency and ${\varphi }_i$ is a random phase uniformly distributed and constant with time in $[0,2\pi )$ which corresponds to the component of the i wave. The vector of forces and moments of waves ${\mathbf{\tau }}_E^{wa}$ is given by [30]

$${\mathbf{\tau }}_E^{wa}=\left[\begin{array}{c}\sum _{i=1}^N\rho gBLTcos\left(\beta \right)s_i\left(t\right)\\ \sum _{i=1}^N-\rho gBLTsin\left(\beta \right)s_i\left(t\right)\\ 0\\ 0\\ 0\\ \sum _{i=1}^N\frac{1}{24}\rho gBL(L^2-B^2)sin\left(2\beta \right)s_i^2\left(t\right)\end{array}\right]$$

(9)

where $\rho$ represents the water density, g the gravity, B, T and L the width, craft and length of the vehicle, respectively. The angle $\beta$ is the encounter angle of the vehicle, which is defined as the angle formed between the stern line and the incident direction of the waves. The effect of this disturbance on the vehicle can be appreciated in the drift when sailing at low speeds.

The last considered disturbance is the one caused by currents. Such currents are mainly produced by variations in temperature, density and salinity between different water volumes, but can also originate from the wind friction at the water’s surface. This kind of disturbance is characterized by water displacements produced inside the water body.

Let the generalized velocity vector $\mathit{v}$ be replaced with the hydrodynamic terms of the vector of relative velocities, as follows

$${\mathit{v}}_r=\mathit{v}-{\mathit{v}}_c$$

(10)

where ${\mathit{v}}_c\in {\mathbb{R}}^6$ is the velocity of the currents with respect to the reference frame. Considering an irrotational fluid, this vector can be defined as [31]

$${\mathit{v}}_c={\left[\begin{array}{cccccc}{u}_c& b_c& w_c& 0& 0& 0\end{array}\right]}^T$$

(11)

with ${\mathit{v}}_c^b={\left[\begin{array}{ccc}{u}_c& b_c& w_c\end{array}\right]}^T$ which is given by

$${\mathit{v}}_c^b=J_1{\left({\mathbf{\eta }}_2\right)}^T{\mathit{v}}_c^E$$

(12)

where ${\mathit{v}}_c^E\in {\mathbb{R}}^3$ represents the current velocity vector measured in the inertial frame.

Finally, the complete dynamic model considering the disturbances associated with currents can be written as

$$\dot{\mathbf{\eta }}=J\left({\mathbf{\eta }}_2\right){\mathit{v}}_r+\left[\begin{array}{c}{\mathit{v}}_c^E\\ 0\end{array}\right]$$

(13)

$$M{\dot{\mathit{v}}}_r+C\left({\mathit{v}}_r\right){\mathit{v}}_r+D\left({\mathit{v}}_r\right){\mathit{v}}_r=\mathbf{\tau }+{\mathbf{\tau }}_E$$

(14)

Owing to the fact that major changes in water environments’ characteristics are related to the time of year, parameters such as velocities and directions of real environmental disturbances are usually described as stationary. In this work, first order Gauss–Markov processes are used for the generation of waves and current parameters. We define the speed of currents as $V_c$, the angle of currents relative to the inertial frame as ${\alpha }_c$ and the attack angle of currents relative to the bow as ${\beta }_c$ [31,32]. Then, the corresponding Gaussian-Markov processes are defined as

$${\dot{V}}_c+{\mu }_1{V}_c=w_1$$

(15)

$${\dot{\alpha }}_c+{\mu }_2{\alpha }_c=w_2$$

(16)

$${\dot{\beta }}_c+{\mu }_3{\beta }_c=w_3$$

(17)

with $w_i$ $(i=1,2,3)$ representing zero mean quasi-Gaussian white noise processes and ${\mu }_i\ge 0$ $(i=1,2,3)$ constants. A 3D current model can be obtained by transformation of current speed ($V_c$) and directions (${\alpha }_c,{\beta }_c$) from flow axes to North-Eastern velocities through the following relation

$$v_c^E=\left[\begin{array}{c}{V}_{c}cos\left({\alpha }_c\right)cos\left({\beta }_c\right)\\ V_{c}sin\left({\beta }_c\right)\\ V_{c}sin\left({\alpha }_c\right)cos\left({\beta }_c\right)\end{array}\right]$$

(18)

For a study in the horizontal plane only, considering ${\alpha }_c=0$ and $\varphi =\theta =0$, (18) can be reduced to a 2D current model as

$$u_c=V_{c}cos({\beta }_c-\psi )$$

(19)

$$v_c=V_{c}sin({\beta }_c-\psi )$$

(20)

Additionally, the direction of the current can be fixed by defining constant values for ${\alpha }_c\mathrm{and}{\beta }_c$.

3. Robust Control Based on Visual Position Estimation

This section details the proposed robust control scheme based on visual position estimation, where sliding modes theory is used to synthesize the robust controller and a monocular VO technique is selected to perform position estimation at every instant. Such a configuration is illustrated in Figure 2.

3.1. VO Mathematical Background

This section presents some important concepts associated with the VO technique, providing the basis for the position estimation process.

Monocular VO schemes process a stream of images provided by a single camera to estimate its motion. The image points representation is performed using homogeneous vectors $q\in {\mathcal{R}}^3$ and $q^{\prime }\in {\mathcal{R}}^3$ in the first and second view, respectively. On the other hand, the points in global coordinates are represented by homogeneous vectors $Q\in {\mathcal{R}}^4$. A perspective view is described using a camera matrix $P\in {\mathcal{R}}^{3\times 4}$, which indicates the image projection $q\sim PQ$, where the symbol ∼ defines equality to scale. To represent a view with a finite projection center, the matrix $P=K\left[R\right|t]$ is used, where R is a rotation matrix, $t={[t_x,t_y,t_z]}^T$ is the translation vector and $K\in {\mathcal{R}}^{3\times 3}$ is a calibration matrix containing the camera’s intrinsic parameters [14,33]. The consideration of affine transformations formed by a rotation and translation, as described before, leads to good results when the surface is static. However, it is worth considering that the effect of waves in the water surface can lead a USV to move in such a way that the corresponding transformation is not affine any more, and some distortion in the scaling can arise. This situation may affect the motion estimation but can be avoided using sensor data to recover the appropriate scale [13].

Consider camera matrices for both views represented as $K_1\left[I\right|0]$ and $K_2\left[R\right|t]$. Let $\widehat{t}$ denote the skew symmetric matrix

$$\widehat{t}=\left[\begin{array}{ccc}0& -t_z& t_y\\ t_z& 0& -t_x\\ -t_y& t_x& 0\end{array}\right]$$

(21)

so that $\widehat{t}x=t\times x$ for all x. Then, the fundamental matrix is

$$F=K_2^{-T}\widehat{t}R{K}_1^{-1}$$

(22)

The epipolar constraint determining the relation between points q and $q^{\prime }$ can be described in terms of the fundamental matrix by

$$q^{\prime }Fq=0$$

(23)

When the camera is calibrated, matrices $K_1$ and $K_2$ are known, so that the epipolar constraint can be rewritten as

$$q^{\prime }Eq=0$$

(24)

where the matrix E is called the essential matrix.

3.2. Monocular Visual Odometry

This section describes the main components of the VO technique, focusing on the monocular scheme with 2D–2D feature correspondences. Figure 3 presents the main steps involved in the visual odometry process.

The monocular VO technique is based on the geometric relations between two images taken by the same camera. Consider a pair of images $I_k$ and $I_{k-1}$ from a calibrated camera; the geometric relations for this case are described by the essential matrix E. This matrix contains the camera motion parameters up to an unknown scale for the translation, and can be defined as

$$E=\widehat{t}R$$

(25)

The essential matrix can be computed using 2D to 2D feature correspondences. In this work, Speeded Up Robust Features (SURF) are selected, but other types can also be used, and Random Sampling Consensus (RANSAC) can be applied for outlier rejection [34]. Then, the translation and rotation for each camera pose can be obtained directly from E. To this end, consider

$$D=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]$$

(26)

and let the singular value decomposition of the essential matrix be $E\sim Udiag(1,1,0)V^T$, where U and V are chosen, such that $det\left(U\right)>0$ and $det\left(V\right)>0$. Then $t\sim t_u\equiv {\left[u_{13}{u}_{23}{u}_{33}\right]}^T$ and R is equal to $R_a\equiv UD{V}^T$ or $R_b\equiv U{D}^T{V}^T$ [14,35].

Any combination of R and t satisfying the above conditions satisfies the epipolar constraint (24). The first camera matrix is defined as $\left[I\right|0]$, while t is considered of unit length. Under such considerations, there are four possible solutions for the second camera matrix: $P_A\equiv \left[R_a\right|t_u]$, $P_B\equiv \left[R_a\right|-t_u]$, $P_C\equiv \left[R_b\right|t_u]$, $P_D\equiv \left[R_b\right|-t_u]$. Only one of these solutions corresponds to the true configuration, where the point is in front of the camera. The remaining ones correspond to the twisted pair obtained by rotating one of the views 180 degrees around the baseline, and reflections of the true configuration and the twisted pair. In order to determine which choice corresponds to the true configuration, the cheirality constraint is imposed, and one point is sufficient to resolve the ambiguity [14].

The final steps concerns a scale adjustment of the translation vector which can be performed based on sensor data (e.g., measuring the size of an element in the scene), motion constraints, or from the integration with other sensors and concatenation of the transformations to recover the incremental motion of the camera.

The algorithm which synthesize the above concepts to perform monocular VO is described below:

1.

Capture a new input image $I_k$.

2.

Detection and matching of features between the new ($I_k$) and old ($I_{k-1}$) image.

3.

Compute fundamental matrix F with RANSAC for outlier rejection.

4.

Compute essential matrix from fundamental matrix.

5.

Extract R and t from essential matrix using SVD.

6.

Use cheirality constraint to obtain the true configuration.

7.

Perform scale adjustment.

8.

Concatenate transformation.

9.

Repeat from step 1.

3.3. Control Design

In order to fulfill the main objective of autonomous navigation, an autonomous mobile system, requires another system that dictates where it should go, with appropriate speed and direction that satisfies the desired performance. Such systems are known as Guidance, Navigation and Control (GNC) systems.

It is desirable that the perturbed system (14) possesses robustness against the disturbances described previously; it is also desired that the control scheme has a robust performance in the presence of modeling errors and parameter uncertainty. To achieve such robust performance, different approaches have been applied. In [27], a nonlinear adaptive controller is used for maneuvering with experimental tests in a laboratory. The same technique is used in [36] but for achieving path-following objectives.

Another control technique that can provide a robust performance, and has been broadly applied to several problems, is Sliding Modes Control (SMC). The main properties of this technique are accuracy and robustness, along with ease of implementation and tuning. The basic idea behind SMC systems is to bring the states of the system to a particular predefined surface in the state space, which is called the sliding surface. Once the states reach the sliding surface, the controller guarantees that states remain in a close neighborhood of the surface. Hence, SMC design can be divided into two main parts; the first involves the design of an appropriate sliding surface, such that the sliding motion satisfies the design specifications. The second part is devoted to the selection of a control law that makes the switching surface attractive for system states. More details on the complete design procedure can be found in [37].

In this section, a control law based on SMC is designed. First, a sliding surface function is proposed as

$$\sigma \left(e\right)=ce+\dot{e},c>0$$

(27)

where c is a constant, and the errors and its derivatives are defined as

$$e={\eta }_d-\eta$$

(28)

$$\dot{e}={\dot{\eta }}_d-\dot{\eta }$$

(29)

where the desired states ${\eta }_d={\left[\begin{array}{ccc}{x}_d& y_d& z_d\end{array}\right]}^T$ and the current states $\eta ={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T$.

Now, let us rewrite (14) in a more suitable form as

$${\dot{\mathit{v}}}_r=M^{-1}\left(f\left({\mathit{v}}_r\right)+{\mathbf{\tau }}_E\right)+M^{-1}\mathbf{\tau }$$

(30)

where the elements associated with Coriolis, centripetal and damping forces are contained in the function

$$f\left({\mathit{v}}_r\right)=C\left({\mathit{v}}_r\right){\mathit{v}}_r+D\left({\mathit{v}}_r\right){\mathit{v}}_r$$

(31)

substituting (13) in (29), the error dynamics results in

$$\dot{e}={\dot{\eta }}_d-J\left({\eta }_2\right){\mathit{v}}_r-{\mathit{v}}_c^E$$

(32)

$$\ddot{e}=\ddot{{\eta }_d}-J\left({\dot{\eta }}_2\right){\mathit{v}}_r-J\left({\eta }_2\right){\dot{v}}_r-{\dot{\mathit{v}}}_c^E$$

(33)

Since it is treated with a vector of irrotational constant currents, the derivative of the vector is

$${\dot{\mathit{v}}}_C^E=0$$

(34)

Stability Analysis

Consider a candidate Lyapunov function V defined as

$$V=\frac{1}{2}{\sigma }^2$$

(35)

In order to achieve asymptotic stability in $\sigma =0$, the condition $\dot{V}<0$ for $\sigma \ne 0$ must be satisfied. To prove this condition, the derivative of (35) is obtained as

$$\begin{array}{cc}\hfill \dot{V}=\sigma \dot{\sigma }=& \sigma [{\ddot{\mathbf{\eta }}}_d+{\dot{\mathbf{\eta }}}_d-\left(J\left({\dot{\mathbf{\eta }}}_2\right)+J\left({\mathbf{\eta }}_2\right)\right){\mathit{v}}_r\hfill \\ & -J\left({\mathbf{\eta }}_2\right)M^{-1}(f\left({\mathit{v}}_r\right)-{\mathbf{\tau }}_E)+J\left({\mathbf{\eta }}_2\right)M^{-1}\mathbf{\tau }]\hfill \end{array}$$

(36)

and defining

$$\varphi ({\mathbf{\eta }}_2,{\mathit{v}}_r)={\ddot{\mathbf{\eta }}}_d+{\dot{\mathbf{\eta }}}_d-\left(J\left(\dot{{\mathbf{\eta }}_2}\right)+J\left({\mathbf{\eta }}_2\right)\right){\mathit{v}}_r-J\left(\mathbf{\eta }\right)M^{-1}(f\left({\mathit{v}}_r\right)-{\mathbf{\tau }}_E)$$

(37)

Then, Equation (36) can be rewritten as

$$\dot{V}=\sigma \left[\varphi ({\mathbf{\eta }}_2,{\mathit{v}}_r)-J\left({\mathbf{\eta }}_2\right)M^{-1}\mathbf{\tau }\right]$$

(38)

considering the following relation

$$\mathbf{\tau }=-M{J}^T\left({\mathbf{\eta }}_2\right)u$$

(39)

The derivative of V can be defined as

$$\dot{V}=\sigma (L+u)$$

(40)

where $L=\varphi ({\mathbf{\eta }}_2,{\mathit{v}}_r)$ contains the undesirable dynamics of the system. A control law based on sliding modes is defined as

$$u=-Ksign\left(\sigma \right)$$

(41)

with

$$sign\left(\sigma \right)=\left\{\begin{array}{cc}-1\hfill & if\sigma >0\hfill \\ 1\hfill & if\sigma <0\hfill \\ \in [-1,1]\hfill & if\sigma =0\hfill \end{array}\right.$$

(42)

and substituting into (40), we obtain the following inequality

$$\dot{V}\le \left|\sigma \right|L-\left|\sigma \right|K=\left|\sigma \right|(L-K)$$

(43)

In order to obtain asymptotic stability in $\sigma =0$, consider the following relation

$$\dot{V}\le -\alpha \sqrt{2V},\alpha >0$$

(44)

Finally, substituting (35) in (44) and matching with (43), its easily shown that the gain K for ensuring asymptotic stability can be obtained as

$$K=\alpha +L$$

(45)

4. Results

In order to show the performance of the proposed VO-based robust control, a simulation test was developed using the virtual world toolbox from Matlab. This tool allows the creation of a virtual environment which can be used to perform the position estimation. The video is acquired using a Kinect V2 sensor from Microsoft and its corresponding Matlab toolbox, which enables the use of this device together with Simulink and virtual world. The ship’s parameters correspond to a catamaran configuration prototype developed for experimental tests. The matrices of such prototype and the disturbance vector for waves are:

$$M=\left[\begin{array}{ccc}13.117& 0& 0\\ 0& 13.117& 0\\ 0& 0& 3.653\end{array}\right]$$

(46)

$$C\left(v\right)=\left[\begin{array}{ccc}0& 0& -13.117v\\ 0& 0& 13.117u\\ 13.117v& -13.117u& 0\end{array}\right]$$

(47)

$$D\left(v\right)=\left[\begin{array}{ccc}14.1041u-0.5262& 0& 0\\ 0& 0.03725v+0.169& 0\\ 0& 0& 0.1663r+0.0913\end{array}\right]$$

(48)

$${\tau }_E^{wa}=\left[\begin{array}{c}2.1631\sum _{i=1}^{N}cos\left(\beta \right)s_i\left(t\right)\\ -2.1631\sum _{i=1}^{N}sin\left(\beta \right)s_i\left(t\right)\\ 2.9016\sum _{i=1}^{N}sin\left(2\beta \right)s_i^2\left(t\right)\end{array}\right]$$

(49)

The selected sliding surface function is characterized by parameter $c=1$, while for the first-order Gauss–Markov processes which models the currents (15) and (18), ${\mu }_1=1$, $w_1=0.1$${\alpha }_c={15}^{\circ }$ and ${\beta }_c={10}^{\circ }$ are selected. Using a recursive method based on (39)–(45) and the waves and currents disturbances boundaries, the robust controller gain K was selected at a value of six.

The virtual world environment developed to test the proposed vision-based control scheme is composed of objects with different shape and texture, as illustrated in Figure 4. The ship’s vision, considering that the camera is attached in its frontal part, is shown in Figure 5. The selected trajectory follows a triangular path on x and z axes, while the y axis is considered to present very small variations, so a constant value is assigned to this axis.

The features detection method used is SURF [38], and an example of feature detection and matching is presented in Figure 6. It is verified that the designed virtual world has the properties of texture and illumination required to provide enough feature correspondences.

Test 1 considers following a triangular path for the $x,z$ axes, while the y axis is selected as constant at every instant. The path following of the x and z axes is shown in Figure 7 and Figure 8, together with the Ground Truth (GT). It can be seen that VO-based controller is capable of tracking the desired position of both axes with good accuracy, even in the presence of waves and currents, which is realized using position estimations instead of measurements. The trajectory performed by the USV is shown in Figure 9. The tracking error for both axes is presented in Figure 10, showing values lower than 0.3 m, which demonstrates the good performance of the proposed controller.

Another scenario is developed in Test 2 considering a sinusoidal trajectory and stronger disturbances with the following values:

$${\tau }_E^{wa}=\left[\begin{array}{c}35.2736\sum _{i=1}^{N}cos\left(\beta \right)s_i\left(t\right)\\ -25.1631\sum _{i=1}^{N}sin\left(\beta \right)s_i\left(t\right)\\ 18.4214\sum _{i=1}^{N}sin\left(2\beta \right)s_i^2\left(t\right)\end{array}\right]$$

(50)

$${\alpha }_c={35}^{\circ },{\beta }_c={20}^{\circ },w_1=0.5$$

(51)

The x-axis and z-axis GT and their corresponding tracking signals are illustrated in Figure 11 and Figure 12, respectively. The horizontal trajectory in the xz plane performed by the USV is shown in Figure 13. Finally, the tracking errors are depicted in Figure 14, where we can observe an increment in the maximum values in comparison with the previous example.

5. Conclusions

This article presented the integration of a monocular visual odometry algorithm for position estimation with a robust control scheme to perform path following tasks for unmanned surface vehicles in a urban waterway. The monocular VO scheme allows us to use a video camera as the position sensor instead of traditional sensors as GPS, which could be useful in environments in which such sensors cannot be utilized. The designed robust controller guarantees stability of the USV trajectories under common disturbances such as waves and currents, which is demonstrated analytically by means of Lyapunov theory and performing simulations using the Virtual World and Simulink tools. The simulation results showed a good position estimation with the monocular VO scheme, which is accurately tracked by the robust controller even when disturbances are present. Such results indicate that integration of VO for robust control schemes is a good alternative for environments in which the traditional sensors cannot be used.