Binocular-Vision-Based Obstacle Avoidance Design and Experiments Verification for Underwater Quadrocopter Vehicle

Abstract

As we know, for autonomous robots working in a complex underwater region, obstacle avoidance design will play an important role in underwater tasks. In this paper, a binocular-vision-based underwater obstacle avoidance mechanism is discussed and verified with our self-made Underwater Quadrocopter Vehicle. The proposed Underwater Quadrocopter Vehicle (UQV for short), like a quadrocopter drone working underwater, is a new kind of Autonomous Underwater Vehicle (AUV), which is equipped with four propellers along the vertical direction of the robotic body to adjust its body posture and two propellers arranged at the sides of the robotic body to provide propulsive and turning force. Moreover, an underwater binocular-vision-based obstacle positioning method is studied to measure an underwater spherical obstacle’s radius and its distance from the UQV. Due to its perfect ability of full-freedom underwater actions, the proposed UQV has obvious advantages such as a zero turning radius compared with existing torpedo-shaped AUVs. Therefore, one semicircle-curve-based obstacle avoidance path is planned on the basis of an obstacle’s coordinates. Practical pool experiments show that the proposed binocular vision can locate an underwater obstacle accurately, and the designed UQV has the ability to effectively avoid multiple obstacles along the predefined trajectory.

1. Introduction

As we know, the ocean is rich in medical resources, mineral resources, marine living resources and so on. To facilitate the investigation of the ocean, a large variety of underwater robots such as the Autonomous Underwater Vehicle (AUV) have been designed [1,2]. The AUV is one kind of intelligent marine robot with many advantages such as strong autonomy and high maneuverability, so it has gradually become an important platform to perform various underwater tasks. Nowadays, the AUV has a wide range of applications in the ocean, such as sea rescue, search range, pipeline inspection and other fields [3,4]. Since conventional AUVs are of torpedo-style shapes, they have great defects such as an underactuated system and excessive turning radius. Many researchers have devoted themselves to new and different types of underwater vehicles, and some underwater robots with new architectures have been developed recently [5,6,7]. A new type Autonomous Underwater Helicopter (AUH), which has a unique dish shape and can realize “four arbitrary” functions of full turn, fixed hover, precise landing, and free landing, was designed by Zhejiang University [5,6]. In addition, a new type of underwater snake robot, which mimics the motion of eels, was used to test a set-based guidance strategy for path-following with obstacle avoidance [7]. New multilink gliding robotic fish can swim flexibly and glide energy efficiently in three dimensions [8,9]. To summarize, underwater vehicles with new structures can make up for the defects of the torpedo AUV. In this paper, we design a new-style AUV named the underwater quadrocopter vehicle to verify the obstacle avoidance mechanism.

Due to complex and changeable underwater environments, autonomous underwater robots must be able to avoid underwater obstacles such as cliffs, wrecks and seafloor fluctuation [10]. The primary goal of obstacle avoidance is to maintain underwater robot safety in a mostly unknown environment through fully autonomous operation [11]. Increasing the autonomy of AUVs has the potential to make underwater operations safer, more efficient, cost-effective, and environmentally friendly. Unfortunately, although there are many research studies on obstacle avoidance strategies for unmanned aerial vehicles, obstacle avoidance research on autonomous underwater robots is still not common and mature. At present, underwater obstacle detection mainly depends on sonar and underwater vision. Due to the special working environment, sonar is traditionally the primary sensor for underwater robots [12,13,14]. Underwater sonar can provide low resolution images of underwater obstacles to obtain estimations of an obstacle’s position and movement trend, but they are inaccurate.

Petillot et al. [12] propose a novel obstacle avoidance and path planning framework for underwater vehicles based on multi-beam forward looking sonar. Braginsky et al. [13] provide extensive simulation and experimental results to demonstrate that the proposed three horizontal and one vertical obstacle avoidance approaches enable AUVs to navigate safely through obstacles. Belcher et al. [14] use a Dual-Frequency Identification Sonar (DIDSON) as an obstacle avoidance sonar to perform autonomous obstacle identification.

On the other hand, there are some obstacle avoidance applications based on underwater machine vision [15,16,17,18]. A vision-based obstacle detection technique using optical flow is proposed for collision avoidance of Autonomous Quadrotor Navigation in [15]. Barrett et al. [16] apply structured light laser imaging into underwater obstacle avoidance and navigation. Leonardi et al. [17] provide a proof of concept regarding a series of experiments investigating stereo vision for underwater obstacle avoidance and position estimation. Drews et al. [18] propose a new vision-based obstacle avoidance strategy using the Underwater Dark Channel Prior (UDCP) algorithm that can be applied to any underwater robot with a simple monocular camera and minimal on-board processing capabilities. Evans et al. [19] outline the sonar and video sensor processing techniques used for real-time control of the Intervention-AUV to perform tracking and 3D pose reconstruction. Xue et al. [20] propose a bio-inspired collision risk-assessment method for ensuring safe USV operation based on stereo vision.

Moreover, an underwater obstacle avoidance mechanism is often studied in tandem with path planning [21,22,23,24,25]. Path planning for ambulatory data gathering is discussed in [21], and an autonomous underwater vehicle traverses only location points in the constructed covering set with a hierarchical grid-based obstacle avoidance strategy. To solve the problem of autonomous obstacle avoidance in trajectory tracking, the model predictive control algorithm is applied to design an obstacle avoidance controller from the point of view of trajectory re-planning [22]. On the premise of realizing dynamic target obstacle avoidance, an adaptive generic model controller based on Radial Basis Function (RBF) neural networks is provided in [23]. A novel three-dimensional obstacle avoidance algorithm for an autonomous underwater robot with a sphere cross-section method is proposed in our previous work [24]. Moreover, we utilize ocean current characteristics simplified as a stream-function to design an optimal 3D trajectory with an obstacle avoidance function [25]. In conclusion, these obstacle-avoidance-based path planning methods consider only path design based on prior-known obstacles but ignore how to obtain the obstacle’s shape and the distance from the underwater robots. It is worth emphasizing that, to the best of our knowledge, the location and shape of an underwater obstacle are assumed to be obtained previously in almost all obstacle avoidance path design algorithms.

In this paper, we propose a new type of underwater robot, which is equipped with an underwater binocular vision system on the robot’s head, to verify the obstacle avoidance effect. The robot is named the underwater quadrocopter vehicle. Due to the impact of long-time water flow, convex underwater obstacles will become smooth and round gradually, so it is assumed that most underwater obstacles can be enveloped by particular spheres with various radii. Herein, for the sake of simplicity, we only discuss certain application scenarios with only spherical obstacles in this paper. Moreover, an underwater binocular-vision-based obstacle positioning algorithm is proposed to measure a spherical obstacle’s shape and its distance apart from the UQV. Subsequently, practical pool experiments verify the performance of proposed binocular vision system. Finally, simple semicircle-curve-based trajectory planning for the UQV is introduced to avoid multiple spherical obstacles effectively.

The main contributions of this paper are:

(1)

A novel binocular-vision-based obstacle ranging and recognition method for underwater applications is proposed in this paper, and the radii of spherical obstacles and the distance between them are calculated and applied to design the obstacle avoidance trajectory.

(2)

Different from existing methods, this paper not only considers an obstacle-avoidance-based path planning algorithm, but also studies an underwater obstacle recognition and processing method simultaneously.

(3)

This paper proposes a new type of underwater vehicle named the underwater quadrocopter vehicle, and its theoretical kinematic and dynamic model is investigated to verify full degree of freedom of movement and obstacle avoidance ability.

The rest of this paper is organized as follows. Section 2 describes the detailed kinematic and dynamic model for the proposed underwater quadrocopter vehicle. Section 3 proposes the underwater binocular-vision-based obstacle positioning method in detail. Section 4 illustrates spherical-obstacle-avoidance-based continuous path design method. Section 5 shows simulation results and the actual pool test of the proposed obstacle avoidance based path design. Section 6 presents the conclusion of this paper.

2. The Underwater Quadrocopter Vehicle Model

Different from a traditional torpedo-shaped AUV with one thruster and several steering engines, the well-designed UQV is equipped with four rotors on the vertical direction of the robotic body and two propellers equipped on the two sides of the robotic body. The mechanical structure of our self-made UQV is described in Figure 1. As we know, traditional torpedo-shaped AUVs often have inherent defects such as large turning radii and are unable to move vertically. However, the proposed UQV has a much smaller radius of gyration and can even turn in place with any turning radius. In conclusion, due to its perfect ability of full freedom underwater movements, the proposed UQV can perform cruise control, fixed depth hover, vertical floating and diving perfectly.

The detailed hardware structure of our self-made UQV is illustrated in Figure 2. The hardware system is mainly composed of three parts: a core processor module, a perception module and a power supply module. The core processor module using Jetson-Nano board as the core processor is responsible for driving the six propellers in a completely autonomous motion operation. The perception system takes charge of underwater data acquisition and analysis with binocular cameras, an inertial navigation system (INS), a depth sensor, an ultra-short baseline positioning system (USBL), differential GPS (DGPS), etc. Therefore, the proposed UQV has the ability to obtain its real-time position. The whole UQV is powered by a 24V DC lithium battery. Six propellers are supplied by isolated power sources; among them, two horizontal propellers are mainly used to provide forward and turning force, and four vertical propellers are controlled to maintain specific posture for the UQV.

To design an obstacle avoidance path, the kinematic and dynamic model of the UQV with unknown dynamics and disturbances will be discussed. Figure 3 illustrates the dynamic model of the UQV in the earth coordinate system ($W-XYZ$) and the body coordinate system ($B-XYZ$), respectively. Four rotors provide lift in the body coordinate system, and its direction is vertical upward and parallel to the $Z-axis$. To counteract the reverse torque and gyro effect, four rotors are divided into two groups: the left-up and right-down rotors belong to one group, and the left-down and right-up rotors belong to another group. To ensure that the yaw angle does not change when hovering, we make two groups of rotors have opposite rotation directions, i.e., the rotation directions in the same group are the same and those in different group are opposite. Obviously, this kind of mechanical structure is the same as in a traditional unmanned aerial vehicle [26].

For the convenience of formula description, the main notations used in this paper are defined in

Table 1. Symbol Definition.

Name	Definition
m	Mass of the UQV
$\rho$	Water density
V	Volume of the UQV
g	Gravitational acceleration
$\phi ,\psi ,\theta$	Roll angle, Yaw angle, Pitch angle
$x/y/z$	$X/Y/Z$ axis coordinates in earth coordinate system
$a_x/a_y/a_z$	$X/Y/Z$ axis accelerations in earth coordinate system
$u/v/w$	Linear velocity of $X/Y/Z$ axis in body coordinate system
$p/q/r$	Angular velocity of $X/Y/Z$ axis in body coordinate system, $w={[p,q,r]}^T$
$F_1$	Force provided by propeller 1
$F_2$	Force provided by propeller 2
$F_3$	Force provided by rotor No.1
$F_4$	Force provided by rotor No.2
$F_5$	Force provided by rotor No.3
$F_6$	Force provided by rotor No.4
$f_1$	Resultant force provided by two propellers
$f_2$	Resultant force provided by four rotors
$F_{all}$	Resultant forces of the UQV
$R_{B-W},T_{B-W}$	Rotation matrix
M	Inertia matrix
$\mathcal{V}$	Velocity vector
$\eta$	State vector
$C\left(\mathcal{V}\right)$	Coriolis force–centripetal force matrix
$D\left(\mathcal{V}\right)$	Damping coefficient matrix
$G\left(\mathcal{V}\right)$	Static resilience
$\tau$	Force and moment
d	Distance between obstacle and the UQV
R	Radius of spherical obstacle

.

With the symbols defined in Table 1, there is ${[\dot{x},\dot{y},\dot{z}]}^T=R_{B-W}{[u,v,w]}^T$, and ${[\dot{\phi },\dot{\theta },\dot{\psi }]}^T=T_{B-W}{[p,q,r]}^T$, where two rotation matrices $R_{B-W}$ and $T_{B-W}$ from the body coordinate system to the earth coordinate system can be expressed as:

$$\begin{array}{c}\hfill R_{B-W}=\left[\begin{array}{ccc}cos\psi cos\theta & -cos\phi sin\psi +sin\phi cos\psi sin\theta & sin\phi sin\psi +cos\phi cos\psi sin\theta \\ sin\psi cos\theta & cos\phi cos\psi +sin\phi sin\psi sin\theta & -sin\phi cos\psi +cos\phi sin\psi sin\theta \\ -sin\theta & sin\phi cos\theta & cos\phi cos\theta \end{array}\right]\end{array}$$

(1)

$$\begin{array}{c}\hfill T_{B-W}=\left[\begin{array}{ccc}1& sin\phi tan\theta & cos\phi tan\theta \\ 0& cos\phi & -sin\phi \\ 0& sin\phi /cos\theta & cos\phi /cos\theta \end{array}\right]\end{array}$$

(2)

Let $\mathcal{V}={[u,v,w,p,q,r]}^T$ denote the Velocity vector, and $\dot{\eta }={[\dot{x},\dot{y},\dot{z},\dot{\varphi },\dot{\theta },\dot{\psi }]}^T$ denote the State vector, so the completed kinematics model of the proposed UQV is expressed as,

$$\begin{array}{c}\hfill \dot{\eta }=J\mathcal{V}\end{array}$$

(3)

where $J=\left[\begin{array}{cc}{R}_{B-W}& {\prime }_{3\times 3}\\ {\prime }_{3\times 3}& T_{B-W}\end{array}\right]$.

The essence of modeling the underwater quadrocopter vehicle is to apply Newton’s second law and Euler’s formula to analyze the stress force. Therefore, the completed dynamic model of the proposed UQV is expressed as [25],

$$\begin{array}{c}\hfill M\stackrel{•}{\mathcal{V}}+C\left(\mathcal{V}\right)\mathcal{V}+D\left(\mathcal{V}\right)\mathcal{V}+G\left(\mathcal{V}\right)=\tau \end{array}$$

(4)

where M is the inertia coefficient matrix, $G\left(\mathcal{V}\right)$ is static resilience, $C\left(\mathcal{V}\right)$ and $D\left(\mathcal{V}\right)$ denote the Coriolis force–centripetal force matrix and the Damping coefficient matrix, respectively.

For simplicity, the well-designed UQV’s compound movement can be mainly divided into two separated motion modes: translational motion and rotational motion. The following subsections will analyze the two motion modes in detail.

2.1. Translational Motion

For the translational motion of the underwater quadrocopter vehicle, the force equation of the UQV in the earth coordinate system can be obtained according to Newton’s Second Law, that is,

$$\begin{array}{c}\hfill {\mathit{F}}_{all}=m\left[\begin{array}{c}{a}_x\hfill \\ a_y\hfill \\ a_z\hfill \end{array}\right]\end{array}$$

(5)

where m denotes the quality of the underwater quadrocopter vehicle and $a_x$, $a_y$ and $a_z$ denote different acceleration values of the $X/Y/Z-axis$ in the earth coordinate system, respectively.

The resultant force of the proposed UQV is mainly composed of four groups: its own gravity, the buoyancy from water, the force generated by the two propellers and the lift provided by the four rotors. For the forces $F_1$ and $F_2$ from the two horizontally placed propellers, their directions are along the $X/Y-axis$. The lifting forces $F_3,F_4,F_5,F_6$ are generated by the four rotors, and their directions are along the $Z-axis$ of the body coordinate system ($B-XYZ$). The directions of gravity and buoyancy are along the $Z-axis$ in the earth coordinate system ($W-XYZ$); hence, it is necessary to convert the force in the body coordinate system to the earth coordinate system. Through the predefined rotation matrix $R_{B-W}$, we can obtain,

$$\begin{array}{c}\hfill \left[\begin{array}{c}0\hfill \\ 0\hfill \\ \rho gV-mg\hfill \end{array}\right]+R_{B-W}\left[\begin{array}{c}{f}_1\hfill \\ 0\hfill \\ f_2\hfill \end{array}\right]=m\left[\begin{array}{c}{a}_x\hfill \\ a_y\hfill \\ a_z\hfill \end{array}\right]\end{array}$$

(6)

where $f_1=F_1+F_2$ and $f_2=F_3+F_4+F_5+F_6$. Therefore, Equation (4) can be expanded to,

$$\begin{array}{c}\hfill \left\{\begin{array}{c}m{a}_x=f_{1}cos\psi cos\theta +f_2\left(sin\phi sin\psi +cos\phi cos\psi sin\theta \right)\hfill \\ m{a}_y=f_{1}sin\psi cos\theta +f_2\left(-sin\phi cos\psi +cos\phi sin\psi sin\theta \right)\hfill \\ m{a}_z=-f_{1}sin\theta +f_{2}cos\phi cos\theta +\rho gV-mg\hfill \end{array}\right.\end{array}$$

(7)

2.2. Rotational Motion

The rotational motion of the UQV is mainly caused by the imbalance of torque. In the rotational motion, the imbalance of torque comes from different forces generated by the four rotors. As Figure 4 shows, the rotating direction of each rotor is specified with the given reference direction of $X-axis$ and $Y-axis$ in the body coordinate system. The reference direction of $Z-axis$ is determined outward from the $X/Y-axis$ plane using the right-hand rule. Consequently, the rotational motion can be analyzed as follows:

The torque calculation equation is defined as:

$$\begin{array}{c}\hfill T=L\times \mathit{F}\end{array}$$

(8)

where L denotes the distance vector from the axis of rotation to the force point and $\mathit{F}$ denotes the force vector applied to the UQV. Through the torque formula, the following equations can be established in the body coordinate system,

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{T}_x=F_1{L}_{1y}-F_2{L}_{2y}-F_3{L}_{3y}+F_4{L}_{4y}\hfill \\ T_y=-F_1{L}_{1x}-F_2{L}_{2x}+F_3{L}_{3x}+F_4{L}_{4x}\hfill \end{array}\right.\end{array}$$

(9)

where $L_{··}$ denotes the different arm of force related to the different propulsive force.

The internal stress in the $Z-axis$ direction is mainly generated by the imbalance from the moment of force when the two group rotors rotate. The rotor rotating clockwise will produce a reversal torque, making the rigid body rotate counterclockwise and vice versa. Therefore, in Figure 4, the reversal torque generated by the No.1 and No.3 rotors is along the positive $Z-axis$ direction, and the reversal torque generated by the No.2 and No.4 rotors is along the negative $Z-axis$ direction.

Through the rigid body rotation theorem and Euler equation, there is:

$$\begin{array}{c}\hfill F=M\beta =M\stackrel{•}{w}+w\times Mw\end{array}$$

(10)

where $F={[F_x,F_y,F_z]}^T$ is the total external moment of force in a certain fixed axis, M is the inertia matrix, and $\beta$ is the angular acceleration, $w={\left[p,q,r\right]}^T$, $M=\left[\begin{array}{ccc}{M}_{xx}& -M_{xy}& -M_{xz}\\ -M_{yx}& M_{yy}& -M_{yz}\\ -M_{zx}& -M_{zy}& M_{zz}\end{array}\right]$.

The rotation equation under six degrees of freedom can be obtained as,

$$\begin{array}{c}\hfill \begin{array}{c}{F}_x=M_{xx}\stackrel{•}{p}+\left(M_{zz}-M_{yy}\right)qr+M_{xy}\left(pr-\stackrel{•}{q}\right)-M_{yz}\left(q^2-r^2\right)\hfill \\ -M_{xz}\left(pq+\stackrel{•}{r}\right)+m\left[y_G\left(\stackrel{•}{w}-uq+vp\right)-z_G\left(\stackrel{•}{v}-wp+ur\right)\right]\hfill \end{array}\end{array}$$

(11)

$$\begin{array}{c}\hfill \begin{array}{c}{F}_y=M_{yy}\stackrel{•}{q}+\left(M_{xx}-M_{zz}\right)rp-M_{xy}\left(qr-\stackrel{•}{p}\right)+M_{yz}\left(pq-\stackrel{•}{r}\right)\hfill \\ +M_{xz}\left(p^2-r^2\right)+m\left[z_G\left(\stackrel{•}{u}-vr+wq\right)-x_G\left(\stackrel{•}{w}-uq+vp\right)\right]\hfill \end{array}\end{array}$$

(12)

$$\begin{array}{c}\hfill \begin{array}{c}{F}_z=M_{zz}\stackrel{•}{r}+\left(M_{yy}-M_{xx}\right)pq-M_{xy}\left(p^2-q^2\right)-M_{yz}\left(pr+\stackrel{•}{q}\right)\hfill \\ +M_{xz}\left(qr-\stackrel{•}{p}\right)+m\left[x_G\left(\stackrel{•}{v}-wp+ur\right)-z_G\left(\stackrel{•}{u}-wp-vr\right)\right]\hfill \end{array}\end{array}$$

(13)

where $r_G={\left[x_G,y_G,z_G\right]}^T\doteq {\left[0,0,0\right]}^T$ is the coordinate of the gravity centre. Since the structure of the UQV is symmetrical, the inertial matrix can be rewritten as a diagonal matrix, i.e., $M_{ij}=0,\forall i\ne j$. Hence, the above equation can be reduced to the following equation,

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{F}_x=M_{xx}\stackrel{•}{p}+qr\left(M_{zz}-M_{yy}\right)\hfill \\ F_y=M_{yy}\stackrel{•}{q}+rp\left(M_{xx}-M_{zz}\right)\hfill \\ F_z=M_{zz}\stackrel{•}{r}\hfill \end{array}\right.\end{array}$$

(14)

Let $X_u,Y_v,N_r$ denote linear damping coefficients of Surge, Sway, and Yaw, respectively; $X_{\dot{u}},Y_{\dot{v}},N_{\dot{r}}$ denote added mass coefficients of Surge, Sway, Yaw, respectively; and $X_{u\left|u\right|},Y_{v\left|v\right|},N_{r\left|r\right|}$ denote secondary damping coefficients of Surge, Sway, Yaw, respectively. Hence, the inertia coefficient matrix M, the Coriolis force–centripetal coefficient matrix $C\left(\mathcal{V}\right)$ and the hydrodynamic damping matrix $D\left(\mathcal{V}\right)$ can be derived as follows,

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{M}_{xx}=m-X_{\dot{u}}\hfill \\ M_{yy}=m-Y_{\dot{v}}\hfill \\ M_{zz}=I_z-N_{\dot{r}}\hfill \end{array}\right.\end{array}$$

(15)

where $I_z$ is the moment of inertia in the $Z-axis$.

$$\begin{array}{c}\hfill C\left(\mathcal{V}\right.)=\left.[\begin{array}{ccc}0& 0& M_{xx}u\\ 0& 0& M_{yy}v\\ -M_{xx}u& -M_{yy}v& 0\end{array}\right]\end{array}$$

(16)

$$\begin{array}{c}\hfill D\left(\mathcal{V}\right)=[\begin{array}{ccc}{X}_u+X_{u\left|u\right|}& 0& 0\\ 0& Y_v+Y_{v\left|v\right|}& 0\\ 0& 0& N_r+N_{r\left|r\right|}\end{array}]\end{array}$$

(17)

3. Binocular-Vision-Based Underwater Obstacle Positioning

Binocular stereo vision uses two parallel cameras to capture an object of interest and then calculates two similar images’ deviations, finally obtaining a clear sense of depth from the correspondence between features. Nowadays, binocular stereo vision has been widely used with the advantages of simple equipment, low cost and less human intervention [27,28]. Herein, the binocular-vision-based underwater obstacle positioning principle is described in Figure 5. Two parallel underwater cameras mounted on the head of the UQV are used to capture imaging information ahead. Using the underwater binocular-vision-based obstacle positioning method, an underwater spherical obstacle’s size and the distance from the UQV will be calculated in a simple mode.

Traditionally, the binocular vision processing procedure can be divided into three main components. First, due to the congenital difference between the left camera and the right camera, the captured images will be distorted. Therefore, the first important step is camera calibration using derived distortion parameters. Second, a stereo matching algorithm is applied to calculate the parallax between the left image and the right image. To improve the accuracy of the disparity calculation in the binocular image matching process, the two cameras’ external parameters are applied to correct the target’s position deviation on the left and the right imaging plane. Third, the two cameras’ internal parameters are used to correct pixel deviation; consequently, the actual target’s position is projected according to the parallax of the target on the left and the right imaging pictures. Similarly, the proposed binocular-vision-based underwater obstacle positioning method is divided into three steps.

3.1. Step 1: Binocular Camera Calibration

According to the pinhole imaging model, the target $P(x,y,z)$ will be projected onto the imaging plane. For an ideal pinhole camera as shown in Figure 6, the straight line distance from the pinhole to the projection plane is defined as the focal length f. Ideally, the principal point and the projection center should be on the same optical axis. However, due to the limitations of the cameras’ manufacturing process, the principal point and the projection center are not usually on the same optical axis. Therefore, it is necessary to correct the cameras’ offset using a certain calibration method. The famous Zhang Zhengyou calibration method [29] is applied in this section, and chessboard calibration images with different angles as shown in Figure 7 are used. In this paper, 14 chessboard calibration maps with different angles are collected for camera calibration. The chessboard collected by the left and right cameras is modeled and analyzed by MATLAB to obtain the correction parameters. The spatial position relationship between the binocular camera and the calibrated chessboard and the distributions of projection error of each image are described in Figure 8. Different color punctuation points distributed in the error diagram represent different errors of the chessboard corners. Figure 8 demonstrates that projection errors mostly approach zero, indicating that the image calibration effect is very good. The average error pixel points of each image indicated with the red line is 0.032489 pixels; therefore, it is verified that these camera calibration results are feasible.

Finally, the internal and external parameters of the used underwater cameras can be obtained following a traditional treatment process [27]. For the sake of space, these traditional processing methods are outside the scope of this article.

3.2. Step 2: Underwater Binocular Matching

As we know, the imaging planes of the left camera and the right camera usually cannot be the same in the binocular vision technology, so it is difficult to locate a common part in the stereo matching algorithm. Therefore, it is necessary to apply external correction parameters to align the polar line. In Figure 9, taking the center of the imaging plane of the left camera as the origin of coordinate system $O_l$, the mark of the observed target P is $P_l$. Similarly, taking the center of the imaging plane with the right camera as the origin of coordinate system $O_r$, the mark of the observed target P is $P_r$. Following the relationship between pixel coordinates and physical coordinates, there are,

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\hfill {\overrightarrow{P}}_l={\overrightarrow{R}}_l\times \overrightarrow{P}+{\overrightarrow{T}}_l\\ \hfill {\overrightarrow{P}}_r={\overrightarrow{R}}_r\times \overrightarrow{P}+{\overrightarrow{T}}_r\\ \hfill {\overrightarrow{P}}_l={\overrightarrow{R}}_T\times ({\overrightarrow{P}}_r-\overrightarrow{T})\end{array}\right.\end{array}$$

(18)

where $\overrightarrow{R}$ and $\overrightarrow{T}$ are the rotation matrix and the translation vector in Figure 9.

As a conventional method, the relationship between pixel coordinates and physical coordinates in the left and right imaging planes is described in Ref. [30].

So far, these matching points can be found in the visualization area where two camera views overlap. Once the triangulation parallax between the corresponding points of two views is known, the target’s depth can be obtained using the triangle similarity principle. The well known block matching stereo-matching algorithm with a small ’Sum of Absolute Differences (SAD)’ window [31] is applied to find the matching points between the two images as Figure 10 shows. The block matching stereo-matching algorithm is implemented in three steps. First, the binocular image is pre-processed to normalize the image’s brightness, to reduce brightness distortion, eliminate noise and enhance texture. Second, we use the previous SAD window to search for the optimal matching of each feature in the left image and the right image. After stereo-matching searching, the best matching point will be determined, i.e., the smallest SAD function value of the matching window is chosen. After block stereo matching, any conventional roundness extraction methods can be used to identify the target. For the sake of paper space, we will ignore explaining these traditional processing methods. After these steps, if the obstacle is spherical, its radius R can be obtained with an image pixel plane.

3.3. Step 3: Obstacle Depth Computing

In this paper, two deployed cameras are exactly the same with focal length f, and the positions are completely parallel along the UQV’s body. It is assumed that the baseline length of the left camera and the right camera is b, the coordinate of the target point is $P(x,y,z)$, the target point’s imaging coordinates of the left camera and the right camera are $(x_l,y_l)$ and $(x_r,y_r)$, respectively. According to the triangle similarity principle between the blue triangle and the yellow triangle in Figure 11, there is such Equation,

$$\begin{array}{c}\hfill \frac{d}{f}=\frac{x}{x_l}=\frac{x-b}{x_r}=\frac{y}{y_l}=\frac{y}{y_r}\end{array}$$

(19)

where d is the distance between the UQV and the spherical obstacle, and it is the key parameter for obstacle avoidance path planning. Hence, we can obtain the actual coordinates of the target point,

$$\begin{array}{c}\hfill \left\{\begin{array}{ccc}\hfill x& =\hfill & \hfill \frac{x_l\times b}{x_l-x_r}\\ \hfill y& =\hfill & \hfill \frac{y_l\times b}{x_l-x_r}\\ \hfill d& =\hfill & \hfill \frac{f\times b}{x_l-x_r}\end{array}\right.\end{array}$$

(20)

Finally, the distance d between the underwater obstacle and the UQV and the radius R of the spherical obstacle can be obtained with the binocular-vision-based underwater obstacle positioning method. It is worth noting that $d\gg R$ in our application scenario.

4. Obstacle-Avoidance-Based Continuous Path Design

So far, the relative distance d away from the UQV and the radius of the spherical obstacle can be determined with the previously introduced binocular visual positioning method. In this section, we will discuss how to design an obstacle-free trajectory.

The detailed framework of obstacle avoidance path planning is described in Figure 12 It can be divided into four main parts: binocular visual positioning, obstacle avoidance coordinate framework, obstacle avoidance path planning and obstacle avoidance path tracking. The binocular visual-positioning module is used to obtain the obstacle’s location, which is demonstrated in the previous sections.

The obstacle avoidance framework determines how to design the obstacle avoidance path with the knowledge of obstacle information. The applied coordinate system constructed by the self-designed UQV and the spherical obstacle is constructed in Figure 13. Although the proposed obstacle avoidance framework is a three-dimensional coordinate system, it is assumed that the UQV and the underwater obstacle are at the same depth, i.e, with the same Y coordinate. Hence, we only consider an obstacle-free trajectory in a two-dimensional plane.

Specifically, it is assumed that the center of spherical obstacle O is $(x_1,y_1,z_1)$, and its radius is R. The original heading of the UQV is ${\theta }_0$, and the front center of the UQV is $(x_0,y_0,z_0)$. The investigated obstacle avoidance path planning is to choose one smooth trajectory $\mathcal{L}$, which satisfies, each point $P_i(x_i,y_i,z_i,{\theta }_i),\forall i\in [0,1]$ in the planned trajectory such that $\mathcal{L}$ is satisfied,

$$\begin{array}{c}\hfill \sqrt{{(x_i-x_1)}^2+{(y_i-y_1)}^2+{(z_i-z_1)}^2}>R\end{array}$$

(21)

subject to the starting heading equal to the ending heading approximately: ${\theta }_1\approx {\theta }_0$.

The obstacle avoidance path planning is derived from the kinematics model of the UQV. To realize the obstacle avoidance mechanism in the embedded system, a semicircle path with radius $R+d$ is used to design the obstacle avoidance path in this paper. In Figure 13, the obstacle avoidance path is decided as a semicircle with radius $R+d$, i.e., the starting point and the ending point are $(0,0,0)$ and $(0,0,2(R+d\left)\right)$, respectively. The variable d is the relative distance between the UQV and the spherical obstacle’s center, which has been determined by the previous binocular visual positioning method. In actual execution, once the front underwater obstacle is identified, the above obstacle avoidance path calculation process will start. Furthermore, it is worth mentioning that the obstacle avoidance trajectory is designed only in the $XOZ$ plane, and the influence in the $Y-axis$ is ignored.

The last obstacle avoidance path tracking module is controlled by the dynamic model of the UQV. Due to its full degree of freedom of movement, the introduced UQV can perform various actions such as turning any angle in a fixed place following the planned trajectory. Therefore, any successive curves can be used by the motion controller as long as the heading angle constraint is satisfied in the previous obstacle avoidance model.

5. Model Verification and Experimental Results

5.1. Motion Model Simulation

In this section, special trajectory tracking in a three-dimensional underwater region is carried out to verify the proposed dynamic and kinematic models. The used parameters are listed in

Table 2. Simulation parameters.

Parameter	Value
m	50 kg
f	2.6 mm
b	6 cm
$X_u$	−1.5
$X_{\dot{u}}$	0.1
$X_{u\left|u\right|}$	8.0
$Y_v$	−40
$Y_{\dot{v}}$	10
$Y_{v\left|v\right|}$	200
$N_r$	−8.9
$N_{\dot{r}}$	5
$N_{r\left|r\right|}$	15

. Some important parameters refer to Ref. [32].

Spiral path simulation results are illustrated in Figure 14, where the yellow curve denotes the designed spiral path in a three-dimensional space, and the green curve denotes the generated trajectory with the proposed UQV’s model. It is obvious that the ideal path followed by the UQV is consistent with the generated spiral path. Figure 15 denotes different projected curves in $X-axis$, $Y-axis$ and $Z-axis$, respectively. Since the designed trajectory is a spiral path, the projection curves of $X-axis$ and $Y-axis$ belong to periodic curves, and that of $Z-axis$ is linear with the time sequence. Due to control delay, there is a certain delay on the timeline for the actual and ideal curves in the $X-axis$ and $Y-axis$. In conclusion, these trajectory tracking results show that the difference between the actual path and the ideal path is quite small, which verifies the proposed dynamic and kinematic models of the UQV. Hence, the above path-tracking effect can be guaranteed to realize any obstacle avoidance curves.

5.2. Hardware Implementation for Uqv

To verify the effect of the proposed obstacle avoidance mechanism, we have developed one kind of underwater quadrocopter vehicle by ourselves. The practical photograph of the UQV is illustrated in Figure 16. Furthermore, a tiny web server is embedded into the Jetson-Nano processor with Quad Core ARM @ 1.43 GHz GPU to perform underwater data acquisition and motion control. The well-designed web pages are illustrated in Figure 17 to display different specific functions, such as manual operation, trajectory tracking, video surveillance, etc. Due to the limitation of the thrusters used, the maximum speed of our UQV is only 1 knot. Therefore, a few meters distance away from forward the obstacle is enough to complete the obstacle’s position calculation.

5.3. Obstacle-Free Trajectory

In this section, the planned obstacle-free trajectory with our proposed method is verified. Figure 18 denotes the obstacle-avoidance-based continuous path in a three-dimensional and a two-dimensional space, respectively. In this simulation, the specific obstacle with radius 2 is located with central coordinates (10, 10, 1.5), and the starting point of the UQV is (10, 0, 1.5). The initial operation distance between the given spherical obstacle and the UQV is set to three, hence the radius of the planned semicircle path is five. In Figure 18, the purple line and the blue line denote the planned trajectory and the actual trajectory, respectively. The designed curves in the separate X-coordinate system and the Y-coordinate system are illustrated in Figure 19, and it is obvious that the actual curves are very close to the theoretical curves.

5.4. Experimental Results

To verify the aforementioned method, we conducted practical experiments in a pool at our college, which is shown in Figure 20. The measured rotation matrix is $\overrightarrow{R}=\left[\begin{array}{ccc}-0.57675& -6.9956& -0.42186\\ 0.75455& -0.65408& 0.05304\\ -0.31304& -0.28772& 0.90511\end{array}\right]$, and the measured translation vector is $\overrightarrow{T}=\left[-53.4260917.06029227.97609\right]$.

In our experimental platform, there are three spherical underwater obstacles with different radii and different colors deployed in the pool as shown in Figure 21. Figure 22 presents the binocular vision processing results for the captured images with the three obstacles. The displayed images in the upper part denote the original images and those in the lower part denote the processed results after we introduced the image processing method. It is obvious that although the captured underwater images are not very clear, the approximate circular shapes of the three obstacles can be recognized by the proposed binocular-vision-based obstacle detection method. The distance measurement accuracy of the proposed underwater ranging method is compared in

Table 3. Measured distances with binocular-vision-based underwater ranging method.

Real Distance (cm)	Measured Distance (cm)	Measurement Error (cm)
50	49.5	0.5
70	71.5	1.5
90	90.5	0.5
110	110	0
130	134	4
150	155	5
170	176	6
190	202	12
210	216	6
230	242	12
250	273	23
270	295	25
290	323	33
310	343	33

. The final ranging results prove that the measured error is less than 10% of the measured range. Furthermore, the measured radius from the the pixel plane is approaching the actual radius, so it is suitable for obstacle recognition. It is worth mentioning that the elapsed time for underwater binocular vision processing was less than 1 s, so a 3 m distance is enough to calculate the obstacle avoidance path for our low-speed UQV.

Finally, we verify the effect of the obstacle-avoidance-based path with the actual pool tests. There are three underwater obstacles distributed in the original path from the starting point to the ending point. In other words, the UQV will collide with these obstacles along the original path if the obstacle avoidance function is not considered. However, with the designed obstacle avoidance path based on binocular vision, a collision will not occur in the experiments since the UQV has the ability to escape from obstacles. The key steps of the obstacle avoidance trajectory are illustrated in Figure 23. To summarize, the final trajectory shows that the proposed binocular-vision-based obstacle avoidance mechanism can meet the needs of realistic obstacle avoidance in an underwater environment.

6. Conclusions

In this paper, we propose a kind of underwater quadrocopter vehicle, which is equipped with four thrusters in the vertical direction of the robotic body, and two horizontal thrusters on the sides of the robotic body. The kinematic and dynamic characteristics verify that the proposed UQV has its own advantages compared to existing autonomous underwater vehicles. Moreover, the detailed mechanical structure, the hardware composition and the kinematic and dynamic model of the proposed UQV are analyzed. Furthermore, we study and verify the obstacle avoidance mechanism using our self-made UQV. In this paper, underwater binocular-vision-based obstacle positioning is introduced to measure an underwater spherical obstacle’s radius and distance from the UQV. Then a simple semicircle-curve-based obstacle avoidance path is planned on the basis of the obstacle’s coordinates. The final practical pool experiments show that the planned trajectory for the UQV can avoid multiple obstacles effectively.