Modeling and Control of a Micro AUV: Objects Follower Approach

Abstract

This work describes the modeling, control and development of a low cost Micro Autonomous Underwater Vehicle ($\mu$-AUV), named AR2D2. The main objective of this work is to make the vehicle to detect and follow an object with defined color by means of the readings of a depth sensor and the information provided by an artificial vision system. A nonlinear PD (Proportional-Derivative) controller is implemented on the vehicle in order to stabilize the heave and surge movements. A formal stability proof of the closed-loop system using Lyapunov’s theory is given. Furthermore, the performance of the $\mu$-AUV is validated through numerical simulations in MatLab and real-time experiments.

1. Introduction

Nowadays, the development of autonomous underwater systems is a scientific research topic which deserves a great interest from the international community. There are a huge number of possible applications wherein autonomous underwater vehicles can be used to avoid risky tasks for the human beings. The proper operation of autonomous vehicles requires the convergence of several disciplines among which the automatic control discipline may play an important role when dealing with complex nonlinear dynamics as in the case of AUVs (Autonomous Underwater Vehicles) [1]. The AUVs have traditionally been used for oceanographic research, where these vehicles have been used for mapping and monitoring specific areas; for example, 3D reefs reconstruction, marine cartography, ecosystem exploration, pipeline and structures inspection, etc. [2]. There are other water-based applications which take advantage of the use of AUVs, such as the monitoring of nuclear storage ponds, waste water treatment facilities and archaeology enclosures. These environments differ from the ocean in terms of scale, thus they can be considered as closed spaces [3]. Traditional AUVs have tended to be large scale (meters in length) and high cost, making them unsuitable for small scale environments. One interesting topic from the automatic control point of view is that very often the vertical and horizontal movements are coupled, i.e., forward momentum is required for the AUV to move in the vertical plane. This means that traditional AUV designs are unsuitable for being used as small-scale sensor platforms [4,5,6]. The AR2D2 prototype (see Figure 1) has major features, such as its size and maneuverability compared to traditional vehicles.

The $\mu$-AUV prototype has the shape of a torpedo with the following dimensions: 35 cm × 20 cm front and 30 cm long. Its weight is less than 5 kg, thus this vehicle is considered like a micro AUV [7]. The different AUV’s degrees of freedom are actuated by four thrusters. The geometric shape of the prototype is such that the vertical and horizontal movements can be considered decoupled.

This paper provides details about the platform development and real-time test of a control strategy for heave and surge movements to follow objects. The effectiveness of the proposed control strategy is validated through numerical simulations as well as real-time experiments. Section 2 presents further details on the design and capabilities of the $\mu$-AUV. In Section 3, we briefly describe the dynamic model of the vehicle, whilst Section 4 shows the control strategy for the depth and forward motion through a nonlinear PD controller. Section 5 details the computer vision algorithm developed in order to detect and follow objects, whilst Section 6 presents the simulation of the control law chosen for depth and forward movement, and the experimental results. Finally, concluding remarks and further work proposals are given in Section 7.

2. Prototype Description

2.1. Embedded System

The hardware architecture of the prototype consists of an embedded system that includes a Raspberry Pi 2B, which has a quad-core ARM (Advanced RISC Machines) Cortex-A7 CPU of 900 MHz, 1 GB RAM memory, camera interface (CSI), display interface (DSI), four USB ports, Ethernet port, Full HDMI port, 40 GPIO pins, and a slot to use a micro SD card for an operative system based on a Linux platform that allows for programming the Middleware. This embedded system also includes a compass sensor (CMPS10), an ultrasonic sensor with the driver SRM400, a depth sensor (BMP085) and a Raspberry Pi camera for artificial vision. The Raspberry processes the information from the sensors, computes de control laws and sends signals to the actuators (DC motors) controlled by Pulse Width Modulation using a micro maestro device (six-channel USB) in order to manage four thrusters through the Robbe Rookie 25A ESC (Electronic Speed Control) motor drivers. Real-time communication is provided using an Ethernet LAN interface. In Figure 2, a schematic shows components of the vehicle’s hardware and their interactions.

2.2. Computer Vision

The GNSS (Global Navigation Satellite System) signals cannot go through water, acoustic communication throughput is limited, odometry is impossible. However, vision is available. The camera provides rich data about the environment. Moreover, in the case of a Remote Operated Vehicle (ROV), it shows to the the user a lot of information to help him drive the robot. Computer Vision is a way to process these data autonomously, in order to implement obstacle avoidance, object following recognition and following, localization and mapping, without any human intervention.

However, as other sensors, underwater vision poses some particular issues to be solved with respect to usual air vision. The main problem is about picture quality, depending on the water (clearness, suspended particles, etc.), the camera will be able to see less or more details. These details are the key to success for computer vision algorithms, especially dealing with features-based vision. Moreover, many features that the camera will detect are not reliable because they are not static (usually, particles floating in water, bubbles, etc.) Thus, we cannot rely on stereo-vision, optical flow and common mapping, which need a strong feature recognition.

Sometimes, pictures quality is so bad that even a human is not able to use it. Colors often do not have enough contrasts, which does not help to detect and differentiate objects that are not close enough to the robot. Finally, the camera can be reliable only as a low-range sensor, which is not adapted for global navigation.

In fact, underwater vision is problematic only if it tries to mimic air vision. Indeed, what is seen as an issue can be turned to our advantage to have better vision quality than expected before.

Classic computer vision complains about the lack of details under water. However, it then has to filter all of these data to select only what is interesting. With an underwater vision point of view, this heavy filtering is not necessary: it is already done by the environment. Only three colors are visible in water: red, orange and yellow; all others appear as blue. This is why objects we want to detect usually have these colors. Thus, to detect them, color filtering is enough (shape and geometry does not matter).

To get around the problem of low contrast level, HSV (Hue–Saturation–Value) filtering can give good results. A lamp also helps to keep the same luminosity exposition (and avoid moving shadows). It is also a way to measure distance to the object: the better luminosity and contrast are, the closer it is.

2.3. Prototype’s Movement Description

The AR2D2 micro submarine has been designed and built, which is shown in Figure 3, with its body fixed frame $(O_b,x_b,y_b,z_b)$. The center $\left(O_b\right)$ of this frame corresponds to the vehicle’s center of gravity, and its axes are aligned with the main axes of vehicle’s symmetry. The movement in the horizontal plane is referred to as $surge$ (along $x_b$ axis) and $sway$ (along $y_b$ axis), while $heave$ represents the vertical motion (along $z_b$ axis). Roll, pitch, and yaw, denoted $(\varphi ,\theta ,\psi )$, are the Euler angles describing the orientation of the vehicle’s body fixed frame with respect to the earth-fixed frame $(O_I,x_I,y_I,z_I)$ [8], while $(x,y,z)$ denote the coordinates of the body-fixed frame center in the earth fixed frame. The propulsion system consists of four thrusters that generate the rotational and translational motion. Concerning the rotational movement of this prototype, roll motion is performed through differential speed control of thrusters 1 and 2. In the same fashion, yaw motion is obtained using thrusters 3 and 4; finally, pitch motion is unactuated with respect to the translational movement of the z axis being regulated by decreasing or increasing the combined speed of thrusters 1 and 2. In the same way, the translational movements along the $x_b$- and $y_b$-axes are obtained by using thrusters 3 and 4 and by controlling the yaw angle.

3. Dynamic Model

The dynamics of the vehicle that are expressed in the body-fixed frame can be written in a vectorial setting according to [8]:

$$\begin{array}{c}\hfill \mathbf{M}\dot{\nu }+\mathbf{C}\left(\nu \right)\nu +\mathbf{D}\left(\nu \right)\nu +\mathbf{g}\left(\eta \right)=\tau +{\mathbf{w}}_e,\end{array}$$

(1)

$$\begin{array}{c}\hfill \dot{\eta }=\mathbf{J}\left(\eta \right)\nu ,\end{array}$$

(2)

where $\mathbf{M}\in {\mathbb{R}}^{6\times 6}$ is the inertial matrix, $\mathbf{C}\left(\nu \right)\in {\mathbb{R}}^{6\times 6}$ defines the Coriolis-centripetal matrix, $\mathbf{D}\left(\nu \right)\in {\mathbb{R}}^{6\times 6}$ represents the hydrodynamic damping matrix, $\mathbf{g}\left(\eta \right)\in {\mathbb{R}}^{6\times 1}$ describes the vector of gravitational/buoyancy forces and moments, $\tau ={[{\tau }_1,{\tau }_2]}^T={[[X,Y,Z],[K,M,N]]}^T\in {\mathbb{R}}^{6\times 1}$ defines the vector of control inputs; ${\mathbf{w}}_e\in {\mathbb{R}}^{6\times 1}$ defines the vector of disturbances; $\nu ={[{\nu }_1,{\nu }_2]}^T={[[u,v,w],[p,q,r]]}^T\in {\mathbb{R}}^{6\times 1}$ denotes the linear and angular velocity vector in the body-fixed frame; $\eta ={[{\eta }_1,{\eta }_2]}^T={[[x,y,z],[\varphi ,\theta ,\psi ]]}^T\in {\mathbb{R}}^{6\times 1}$ is the position and attitude vector decomposed in the earth-fixed frame, and $\mathbf{J}\left(\eta \right)\in {\mathbb{R}}^{6\times 6}$ is the transformation matrix between body frame and earth-fixed frame (for more details, see [9,10]).

3.1. Gravity/Buoyancy Forces and Torques

According to Archimedes’s principle, the buoyant force ${\mathbf{f}}_{\mathbf{B}}$ applied in the center of buoyancy, which acts on the opposite direction of vehicle weight ${\mathbf{f}}_{\mathbf{W}},$ is expressed as follows:

$${\mathbf{f}}_{\mathbf{B}}=-\left[\begin{array}{c}0\\ 0\\ \rho g\nabla \end{array}\right]{\mathbf{f}}_{\mathbf{W}}=\left[\begin{array}{c}0\\ 0\\ mg\end{array}\right],$$

(3)

where $\rho$ represents the fluid density, g the gravitational acceleration, ∇ the displaced fluid volume and m the mass of the vehicle. Now, considering that $W=mg$ and $B=\rho g\nabla$ by using the $zyx$-convention for navigation and control application [11], the transformation matrix $J_1\left({\eta }_2\right)=R_{z,\psi }^T{R}_{y,\theta }^T{R}_{x,\varphi }^T$ is applied in order to obtain the weight and buoyancy forces respect to the body fixed coordinates system:

$$F_B=J_1{\left({\eta }_2\right)}^{-1}{f}_B{F}_W=J_1{\left({\eta }_2\right)}^{-1}{f}_W;$$

(4)

consequently,

$${\mathbf{F}}_{\mathbf{B}}=\left[\begin{array}{c}Bsin\left(\theta \right)\\ -Bcos\left(\theta \right)sin\left(\varphi \right)\\ -Bcos\left(\theta \right)cos\left(\varphi \right)\end{array}\right]{\mathbf{F}}_{\mathbf{W}}=\left[\begin{array}{c}-Wsin\left(\theta \right)\\ Wcos\left(\theta \right)sin\left(\varphi \right)\\ Wcos\left(\theta \right)cos\left(\varphi \right)\end{array}\right].$$

(5)

Thus, the restoring forces acting on the vehicle are ${\mathbf{f}}_{\mathbf{g}}$=${\mathbf{F}}_{\mathbf{B}}$+${\mathbf{F}}_{\mathbf{W}}$, this is:

$$\begin{array}{c}{\mathbf{f}}_{\mathbf{g}}\end{array}=\left[\begin{array}{c}(B-W)sin\left(\theta \right)\\ (W-B)cos\left(\theta \right)sin\left(\varphi \right)\\ (W-B)cos\left(\theta \right)cos\left(\varphi \right)\end{array}\right].$$

(6)

On the other hand, the restoring moments are described by the following equation:

$${\mathbf{m}}_{\mathbf{g}}={\mathbf{r}}_{\mathbf{w}}\times {\mathbf{F}}_{\mathbf{W}}+{\mathbf{r}}_{\mathbf{b}}\times {\mathbf{F}}_{\mathbf{B}},$$

(7)

where ${\mathbf{r}}_{\mathbf{w}}={[x_w,y_w,z_w]}^T$ and ${\mathbf{r}}_{\mathbf{b}}={[x_b,y_b,z_b]}^T$ represent the positions of the center of gravity (CG) and the center of buoyancy (CB), respectively. Based on the design of the vehicle and in order to reduce further analysis, the origin of the body fixed frame is chosen in the gravity’s center; this implies that ${\mathbf{r}}_{\mathbf{w}}={[0,0,0]}^T$, while the center of buoyancy is ${\mathbf{r}}_{\mathbf{b}}={[0,0,-z_b]}^T$. For practical purposes, the buoyancy force is greater than the weight, i.e., $W-B=-f_b$. Notice that $f_b$ should be smaller than the force produced by the thrusters. Then, from Equations (6) and (7), we have:

$$g\left(\eta \right)=\left[\begin{array}{c}{\mathbf{f}}_{\mathbf{g}}\\ {\mathbf{m}}_{\mathbf{g}}\end{array}\right]=\left[\begin{array}{c}{f}_{b}sin\left(\theta \right)\\ -f_{b}cos\left(\theta \right)sin\left(\varphi \right)\\ -f_{b}cos\left(\theta \right)cos\left(\varphi \right)\\ -z_{b}Bcos\left(\theta \right)sin\left(\varphi \right)\\ -z_{b}Bsin\left(\theta \right)\\ 0\end{array}\right].$$

(8)

3.2. Forces and Torques Generated by the Thrusters

Figure 3 shows the forces generated by the thrusters acting on the micro submarine. These are described relative to the body-fixed coordinate system as:

$$\widehat{{\mathbf{f}}_{\mathbf{1}}}=\left[\begin{array}{c}0\\ 0\\ f_1\end{array}\right];\widehat{{\mathbf{f}}_{\mathbf{2}}}=\left[\begin{array}{c}0\\ 0\\ f_2\end{array}\right];\widehat{{\mathbf{f}}_{\mathbf{3}}}=\left[\begin{array}{c}{f}_3\\ 0\\ 0\end{array}\right];\widehat{{\mathbf{f}}_{\mathbf{4}}}=\left[\begin{array}{c}{f}_4\\ 0\\ 0\end{array}\right].$$

Summarizing and using the notation of [12], it follows that:

$${\tau }_1=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{f}_3+f_4\\ 0\\ f_1+f_2\end{array}\right]$$

(9)

and the body-fixed torques generated by the above forces are defined as:

$${\tau }_2=\sum _{i=1}^4{\mathbf{l}}_{\mathbf{i}}\times \widehat{{\mathbf{f}}_{\mathbf{i}}},$$

(10)

where ${\mathbf{l}}_{\mathbf{i}}=(l_{ix},l_{iy},l_{iz})$ is the position vector of the force $\widehat{{\mathbf{f}}_{\mathbf{i}}}$∀$i=1,..,4$, with respect to the body-fixed reference frame. Then, the torques generated by the thrusters are described as:

$${\tau }_2=\left[\begin{array}{c}K\\ M\\ N\end{array}\right]=\left[\begin{array}{c}{l}_{1y}{f}_1+l_{2y}{f}_2\\ l_{1x}{f}_1+l_{2x}{f}_2\\ l_{3y}{f}_3+l_{4y}{f}_4\end{array}\right].$$

(11)

Then,

$$\tau =\left[\begin{array}{c}{f}_3+f_4\\ 0\\ f_1+f_2\\ l_{1y}{f}_1+l_{2y}{f}_2\\ l_{1x}{f}_1+l_{2x}{f}_2\\ l_{3y}{f}_3+l_{4y}{f}_4\end{array}\right].$$

(12)

4. Control Strategy

For the design of the controller, it is common to assume that the hydrodynamic parameters involved in the dynamical model of the underwater vehicle are unknown. Indeed, they depend on effects and properties that are hard to model or estimate, like added mass, skin friction, vortex shedding, fluid characteristics, etc. Therefore, we propose using a nonlinear PD controller [13].

$u\left(t\right)$ is a PD controller, which is depicted as the following equation:

$$u\left(t\right)=K_{P}e\left(t\right)+K_D\frac{de\left(t\right)}{dt},$$

(13)

where $e\left(t\right)$ = $r\left(t\right)-y\left(t\right)$ is the error, $r\left(t\right)$ represents the reference, $y\left(t\right)$ is the output measurement, and ($K_P$, $K_D$) are the proportional and derivative gains. In Equation (13), we can notice that, if $e\left(t\right)\to \infty$, then $u\left(t\right)\to \infty$; this could lead to system oscillations or in other case saturation of the actuators. In order to prevent damage on the actuators, we propose using a saturation function in each term of Equation (13).

Now, let ${\sigma }_{\overline{b_i}}$$\left(k_i{h}_i\right)$ be a saturation function ∀i = 1,2, and $\overline{b_i},k_i>0$, described in Figure 4 and defined as the next equation:

$${\sigma }_{\overline{b_i}}\left(k_i{h}_i\right)=\left\{\begin{array}{c}\overline{b_i},if{k}_i{h}_i>\overline{b_i},\hfill \\ k_i{h}_i,if\left|k_i{h}_i\right|\le \overline{b_i},\hfill \\ -\overline{b_i},if{k}_i{h}_i<-\overline{b_i}.\hfill \end{array}\right.$$

(14)

4.1. A Nonlinear PD Controller Based on Saturation Functions

According to Equations (13) and (14), we propose a nonlinear PD controller based on saturation functions as follows:

$$u\left(t\right)={\sigma }_{\overline{b_1}}\left[k_{1}e\left(t\right)\right]+{\sigma }_{\overline{b_2}}\left[k_2\frac{de\left(t\right)}{dt}\right].$$

(15)

The above equation can be represented as:

$$u\left(t\right)=\sum _{i=1}^2{u}_i,$$

(16)

where $u_i$=${\sigma }_{\overline{b_i}}\left(k_i{h}_i\right)$ represents the saturation function, $h_1$ is the error and $h_2$ is the derivative error. Then, according to Equation (14), we have:

$$u_i={\sigma }_{\overline{b_i}}\left(k_i{h}_i\right)=\left\{\begin{array}{c}\overline{b_i}if{k}_i{h}_i>\overline{b_i},\hfill \\ k_i{h}_{i}if\left|k_i{h}_i\right|\le \overline{b_i},\hfill \\ -\overline{b_i}if{k}_i{h}_i<-\overline{b_i},\hfill \end{array}\right.$$

(17)

and, for the previous equation, we can rewrite it as:

$$u_i=\left\{\begin{array}{c}sign\left(h_i\right)\overline{b_i},if\left|k_i{h}_i\right|>\overline{b_i},\hfill \\ k_i{h}_i,if\left|k_i{h}_i\right|\le \overline{b_i}.\hfill \end{array}\right.$$

(18)

In Equation (18), we can notice that the parameters tuning of the controller, which is described by Equation (15), are the gains $k_i$ and the saturation values $\overline{b_i}$, ∀i$=1,2$. Notice that the parameters tuning could be the saturation values $b_i$ and the interval of $h_i$ for which $u_i$ is lineal, thus we can choose the value of $h_i$ for which we want to saturate the law control. As a consequence, we are going to introduce a new parameter. For this, we consider the point of $h_i$ where $\left|u_i\right|=\overline{b_i}$; this is:

$$\left|u_i\right|=\left|k_i{h}_i\right|=\overline{b_i}⟹\left|h_i\right|=\overline{b_i}/k_i.$$

(19)

Then, we define:

$$d_i:=\overline{b_i}/k_i$$

(20)

as the point where:

$$u_i=sign\left(h_i\right)\overline{b_i}\forall \left|h_i\right|>d_i.$$

(21)

According to Equations (20) and (21), we can represent the control law, which is given by Equation (18), in terms of the parameters $b_i$ and $d_i$ as follows:

$$u_i=\left\{\begin{array}{c}sign\left(h_i\right)\overline{b_i},if\left|h_i\right|>d_i,\hfill \\ \overline{b_i}{d}_i^{-1}{h}_i,if\left|h_i\right|\le d_i,\hfill \end{array}\right.$$

(22)

where the parameters tuning of the controller are $b_i$ and $d_i$, ∀i$=1,2$. In order that Equation (22) will be expressed in terms of $h_i$ when $\left|h_i\right|>d_i$, we have that:

$$sign\left(h_i\right)\overline{b_i}=h_{i}sign\left(h_i\right)\overline{b_i}{h}_i^{-1}.$$

(23)

Then,

$$sign\left(h_i\right)\overline{b_i}=\left|h_i\right|\overline{b_i}{h}_i^{-1}.$$

(24)

Considering that $\left|h_i\right|h_i^{-1}={\left|h_i\right|}^{-1}{h}_i$, then Equation (22) can be rewritten as:

$$u_i=\left\{\begin{array}{c}\overline{b_i}{\left|h_i\right|}^{-1}{h}_{i}if\left|h_i\right|>d_i,\hfill \\ \overline{b_i}{d}_i^{-1}{h}_{i}if\left|h_i\right|\le d_i.\hfill \end{array}\right.$$

(25)

Finally, the control law defined by Equation (15) can be represented as:

$$u\left(t\right)=u_1+u_2=k_p\left(e\right)e\left(t\right)+k_d\left(\dot{e}\right)\dot{e}\left(t\right),$$

(26)

with:

$$k_p\left(e\right)=\left\{\begin{array}{c}\overline{b_1}{\left|e\left(t\right)\right|}^{-1}if\left|e\left(t\right)\right|>d_1,\hfill \\ \overline{b_1}{d}_1^{-1}if\left|e\left(t\right)\right|\le d_1,\hfill \end{array}\right.$$

(27)

$$k_d\left(\dot{e}\right)=\left\{\begin{array}{c}\overline{b_2}{\left|\dot{e}\left(t\right)\right|}^{-1}if\left|\dot{e}\left(t\right)\right|>d_2,\hfill \\ \overline{b_2}{d}_2^{-1}if\left|\dot{e}\left(t\right)\right|\le d_2.\hfill \end{array}\right.$$

(28)

The advantage of this controller is that the maximum forces and torques are chosen by the parameters $\overline{b_1}$ and $\overline{b_2}$. Thus, we are sure that the actuators will not be damaged, but, in other cases, it is necessary that the forces and torques are slightly larger to correct the system error.

4.2. Stability Proof

Considering Equations (1) and (2), we propose the following control input:

$$\tau =g\left(\eta \right)-J^T\left(\eta \right){\tau }_{NPD}$$

(29)

with:

$${\tau }_{NPD}={\sigma }_{\overline{b_p}}\left[k_{p}e\left(t\right)\right]+{\sigma }_{\overline{b_d}}\left[k_d\left(\dot{e}\left(t\right)\right)\right].$$

(30)

The equation before can be rewritten as:

$${\tau }_{NPD}=K_p(·)e\left(t\right)+K_d(·)\dot{e}\left(t\right),$$

(31)

with:

$$K_p(·)=\left[\begin{array}{c}{K}_{p_1}(·)0\cdots 0\hfill \\ 0K_{p_2}(·)\cdots 0\hfill \\ ⋮⋮\ddots ⋮\hfill \\ 00\cdots K_{p_n}(·)\hfill \end{array}\right]>0,$$

(32)

$$K_d(·)=\left[\begin{array}{c}{K}_{d_1}(·)0\cdots 0\hfill \\ 0K_{d_2}(·)\cdots 0\hfill \\ ⋮⋮\ddots ⋮\hfill \\ 00\cdots K_{d_n}(·)\hfill \end{array}\right]>0,$$

(33)

where:

$$k_{p_i}=\left\{\begin{array}{c}{\overline{b}}_{p_i}{\left|e_i\left(t\right)\right|}^{-1}if\left|e_i\left(t\right)\right|>d_{p_i},\hfill \\ {\overline{b}}_{p_i}{d}_{p_i}^{-1}if\left|e_i\left(t\right)\right|\le d_{p_i},\hfill \end{array}\right.$$

(34)

$$k_{d_i}=\left\{\begin{array}{c}{\overline{b}}_{d_i}{\left|\dot{e_i}\left(t\right)\right|}^{-1}if\left|\dot{e_i}\left(t\right)\right|>d_{d_i},\hfill \\ {\overline{b}}_{d_i}{d}_{d_i}^{-1}if\left|\dot{e_i}\left(t\right)\right|\le d_{d_i}.\hfill \end{array}\right.$$

(35)

Considering the regulation case:

$${\eta }_d=cte⟹\dot{\eta }=0.$$

(36)

Assuming that:

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

(37)

then Equation (29) can be rewritten as:

$$\tau =g\left(\eta \right)-J^T\left(\eta \right)\left[K_p\left(e\right)e+K_d\left(\dot{e}\right)\dot{\eta }\right].$$

(38)

Now, we have the closed-loop system as follows:

$$M\dot{\nu }+C\left(\nu \right)\nu +D\left(\nu \right)\nu =-J^T\left(\eta \right)\left[K_p\left(e\right)e+K_d\left(\dot{e}\right)\dot{\eta }\right].$$

(39)

Considering Equation (2), we have:

$$M\dot{\nu }+C\left(\nu \right)\nu +D\left(\nu \right)\nu =-J^T\left(\eta \right)\left[K_p\left(e\right)e+K_d\left(\dot{e}\right)J\left(\eta \right)\nu \right].$$

(40)

We define $K_{dd}(\eta ,\dot{e})=J^T\left(\eta \right)K_d\left(\dot{e}\right)J\left(\eta \right)$; then, the previous equation can be rewritten as:

$$M\dot{\nu }+C\left(\nu \right)\nu +D\left(\nu \right)\nu =-J^T\left(\eta \right)K_p\left(e\right)e-K_{dd}(\eta ,\dot{e})\nu .$$

(41)

Rewriting the previous equation, we have:

$$\frac{d}{dt}\left[\begin{array}{c}e\hfill \\ \nu \hfill \end{array}\right]=$$

(42)

$$\left[\begin{array}{c}J\left(\eta \right)\nu \hfill \\ M^{-1}\left[J^T\left(\eta \right)K_p\left(e\right)e-K_{dd}(\eta ,\dot{e})\nu -C\left(\nu \right)\nu -D\left(\nu \right)\nu \right]\hfill \end{array}\right].$$

(43)

Observe that the unique origin is the equilibrium point. Now, we can propose the following Lyapunov function candidate:

$$V(e,\nu )=\frac{1}{2}{\nu }^{T}M\nu +\underset{0}{\overset{e}{\int }}{\xi }^T{K}_p\left(\xi \right)d\xi .$$

(44)

According to Lemma 2 from [14], we have that $V(e,\nu )$ is globally positive definite and a radially unbounded function. The time derivative of the Lyapunov function candidate is:

$$\dot{V}(e,\nu )={\nu }^{T}M\dot{\nu }-e^T{K}_p\left(e\right)J\left(\eta \right)\nu .$$

(45)

By substituting the closed-loop Equation (41) into (45), we obtain:

$$\dot{V}(e,\nu )={\nu }^T{J}^T\left(\eta \right)K_p\left(e\right)e-{\nu }^T{K}_{dd}(\eta ,\dot{e})\nu -{\nu }^{T}C\left(\nu \right)\nu -{\nu }^{T}D\left(\nu \right)\nu -e^T{K}_p\left(e\right)J\left(\eta \right)\nu .$$

(46)

Since $K_p\left(e\right)=K_p^T\left(e\right)$ and $C\left(\nu \right)=-C{\left(\nu \right)}^T$, Equation (46) becomes:

$$\dot{V}(e,\nu )=-{\nu }^T\left[K_{dd}(\eta ,\dot{e})+D\left(\nu \right)\right]\nu .$$

(47)

Assuming that $D\left(\nu \right)>0$ and remembering that $K_d>0⟶K_{dd}>0$ and symmetric matrix, we then obtain that $\dot{V}(e,\nu )$ is a globally negative semidifinite function, and therefore we conclude stability of the equilibrium point. In order to prove asymptotic stability, we apply the Krasovskii–LaSalle’s theorem; then:

$$\Omega =\left[\left[\begin{array}{c}e\hfill \\ \nu \hfill \end{array}\right]=\dot{V}(e,\nu )=0\right]=\left[\left[\begin{array}{c}e\hfill \\ \nu \hfill \end{array}\right]=\left[\begin{array}{c}e\hfill \\ 0\hfill \end{array}\right]\in R^{2n}\right].$$

(48)

Introducing $\nu =0$ and $\dot{\nu }=0$ into Equation (41), we have $e=0$; therefore, we conclude that the equilibrium point is globally asymptotically stable.

5. Computer Vision Algorithm

5.1. Data Processing Chain

Processing an image requires many steps, each one narrowing the quantity of data until it gives what we need (see Figure 5).

Firstly, a raw image taken from the camera has to be unnoised thanks to a Gaussian blur. Then, only a pixel of wanted colors are kept, giving a binary image. At this step, the image gives a geometric representation of the interesting object, without any color considerations.

Secondly, the algorithm has to localize the object, in order to have coordinates, orientation, size and variables that can be easily manipulated. It detects all connected groups of pixels (called “blobs”), and returns their size and centroid position. The biggest is considered as the object that the robot has to follow.

Finally, depending on the status of the robot, it can decide to move or follow the detection of objects.

5.2. Image Preprocessing

Before trying to extract data from an image, we need to process some filters to remove most noise and uninteresting things. It usually involves spatial and temporal blur to fix noise variance and dynamic filters to fix noise mean.

Images provided by a camera have noise, which can corrupt computer vision algorithm. For instance, in order to detect features, we need to extract gradient, which can be distorted by noise variance. This kind of noise can be reduced by blur, especially Gaussian blur for Gaussian noise, which dilutes variance in neighboring pixels. However, this two-dimensional spatial blur should not be too strong, at the risk of producing diplopia. Moreover, the camera video has also a third dimension: time. A time blur along consecutive images could also reduce noise variance. However, it is rather effective on static video: mixing too many different images produces a ghost effect [15].

These previous filters are static, and they cannot change their parameters (which are calibrated for the hardware). We need dynamic filters to adapt automatically to the environment. Indeed, luminosity and contrast can change along time. A filter can keep the same luminosity, or change color distribution by analyzing and equalizing Hue histogram. As compensation, it increases noise variance [16]. The main issue is that we need to choose parameters depending on the environment. Completely independent methods exist, but they consume too much CPU resources, thus it could be useful to improve a single interesting image, just that is too much of a guzzler for autonomous navigation [17].

5.3. Extracting Interesting Data

The two main properties of objects in an image are color and shape. Color recognition is easy to implement, but not discriminating enough (different objects can have the same color). On the contrary, shape recognition is reliable, but not so easy to program.

We can combine advantages with these two methods: first, color recognition only keeps a little data corresponding to the right color, and then shape recognition processes these lightened data.

5.3.1. HSV Filtering

The first step is HSV filtering. It keeps pixels whose “hue” corresponds to the right color, “saturation” and “value” above a threshold to eliminate gray and dark areas. The result is a binary image, in which white pixels are kept, and blue pixels are rejected. The issue is that many good pixels are rejected, which creates little holes. The solution is to “dilate” the image to recover these pixels, and “erode” to remove the border effect of dilate (see Figure 6).

5.3.2. Blob Detection

Blob detection consists of linking adjacent pixels to create groups—each one corresponding to an object. This algorithm [18] is implemented in the “findContours” function of OpenCV. Then, each blob has its centroid computed, whose coordinates are understandable data for the navigation algorithm (see Figure 7).

5.4. Take Decision

Orders for motors are generated depending on the state of the FSM (Finite State Machine). Main states are:

• Remote control (no autonomous, the user sends orders).
• Stabilize (keep same position and posture).
• Go up and Go down (change only depth and stabilize).
• Explore (follow a planned path).
• Follow an object by the Raspberry Pi camera.

Ball Following through the Algorithm Vision

Blob detection gives the coordinates and size of each blob by giving HSV boundaries of interesting objects. It is required to know before the mission what the robot is looking for. The first step is to show to the robot an OPI (Object of Potential Interest) and manually select HSV boundaries until it is well detected. It must be done underwater because colors are not the same in the laboratory (Figure 6 and Figure 7 show pictures of the same OPI, outside and inside water). On the other hand, objects that avoid its reflection on the surface due to the surrounding light should be considered to obtain a better detection of the object. In addition, another object was detected as shown in Figure 7. However, the detection of the object was fine while the sun is neglected on the surface, due to the yellow color of this object. Therefore, the detection of the objects is good until they are not close to the surface when their reflection appears or they are confused with others of the same color.

Then, the robot is able to find the coordinate ($x_{opi}$, $y_{opi}$) of the biggest object seen by its camera, which it will try to follow. Due to the shape of AR2D2, a simple proportional law control is enough. By comparing to the center ($x_c$, $y_c$) of the image, and its size to a threshold size thresh, we obtain this kind of control law:

$$\left[\begin{array}{c}mo{t}_1\\ mo{t}_2\\ mo{t}_3\\ mo{t}_4\end{array}\right]=\left[\begin{array}{c}+K_{py}\\ +K_{py}\\ +K_{px}\\ -K_{px}\end{array}\right]*\left[\begin{array}{c}{y}_{opi}-y_c\\ y_{opi}-y_c\\ x_{opi}-x_c\\ x_{opi}-x_c\end{array}\right]+K_{size}*\left[\begin{array}{c}0\\ 0\\ siz{e}_{tresh}-size\\ siz{e}_{tresh}-size\end{array}\right].$$

(49)

$K_{px}$ and $K_{py}$ try to minimize horizontal and vertical error of the OPI’s centroid compared to the center of the image. Meanwhile, $K_{size}$ keeps the robot to a good distance from the object (here, the size gives an approximation of distance). In Figure 8, we can observe an experimental test of the detection of the ball.

6. Simulation and Results

Matlab Simulink (R2016a, MathWorks) has been used to apply the control law chosen in order to stabilize the surge and heave movements on the dynamic model equations.

Simulation results are presented in order to observe the performance of the translation movements by the proposed control law. Nonlinear PD control was tuned to observe the best behavior versus disturbances, where the proportional and derivative gains on the saturation functions relative to surge and heave control, belong to values; $b=+/-4$ and $d=+/-2$, thus the time evolution of the “x” vehicle’s position is shown in Figure 9, while its “z” position of the closed-loop system are plotted in Figure 10. Observe that the state positions $(x,z)$ are externally perturbed and the nonlinear PD control is again able to stabilize the engine position. The initial conditions used for this simulation are $x\left(0\right)=0,z\left(0\right)=0,\dot{x}\left(0\right)=0,\dot{z}\left(0\right)=0$ and the desired values are $x_d=1.5$ and $z_d=1$.

Control input U1 is directly applied in vertical forces and control input U2 is applied in horizontal forces. Figure 9 and Figure 10 show both control inputs. Observe that control input U1 converges to the weight value $f_W$ = 1, whilst that control input U2 converges to zero, due to horizontal forces’ combination.

In order to characterize the thrusters, we did some tests in the fish tank, considering the reaction of the thruster on the z-axis through a PWM signal (Pulse Width Modulation) for the maneuverability index (see Figure 11). Therefore, F = (PWM/50) − 1 is the linear equation obtained by the adjustment of the curve. On the other hand, in recent works, such as in [19], it is important to consider a greater knowledge of the dynamic of all the thrusters to obtain the best performance and efficiency with respect to the maneuvers of the vehicle in control applications. In this case, the Micro AUV has the layout configuration of the thrusters near the center of gravity to obtain better maneuverability.

Figure 12 and Figure 13 show the experimental results relative to surge and heave movements and its respective error signal in the regulation, while the torques of the thrusters on both axes are depicted in Figure 14. Notice that the control law proposed presents a good performance. The gains tuning was fixed in order to get the best behaviour of the micro submarine exposed to extern disturbs. Concerning the stability improvement, it is possible to fix the gains tuning with more accuracy, considering the controller characteristic, being able to saturate the control input and protect the actuators.

The ball followed a random path (it is not a stabilization test) with random velocity. The robot managed to follow it, even when the object goes out of image. Indeed, inertia of the robot into water preserves its movement, making the robot able to find the OPI again.

7. Conclusions

The development of this prototype was motivated by the need for having an autonomous small vehicle for operation in closed environments. We developed and presented the embedded control system of the AR2D2 Micro AUV robot. In this initial work, we have considered the problem of set-point regulation on heave and surge movement. The vehicle was designed with the aim of having a small platform able to make complex maneuvers; to this end, the dynamic decoupling of surge and sway turned to be a key issue. In turn, the navigation embedded system was reduced as much as possible. On one hand, one is restricted to work in closed volumes, and, on the other hand, we are looking for cooperative tasks as the major goal of our future research work [20]. In this paper, we have used a typical nonlinear controller with saturation functions, thus the closed loop stability was demonstrated on the basis of Lyapunov’s theory. The desired behavior was validated by Matlab’s simulation and experiments. On the other side, this vehicle performs real-time embedded image processing in order to recognize and follow an object of interest. The results show a good ability to do it, without losing the OPI. In the future, we will implement another control strategy aiming to improve the performances along yaw angle and regulation in x, in order to be close to the object for further inspection. Introducing some filters along time can also improve behavior of the robot, and stabilize recognition of object (by processing sequential video images instead of single pictures). Moreover, we will conduct experiments in a natural environment with less visibility.