*Article* **3-Dimensional Modeling and Attitude Control of Multi-Joint Autonomous Underwater Vehicles**

**Lin Yu 1, Qinghao Meng <sup>1</sup> and Hongwei Zhang 2,\***


**\*** Correspondence: zhanghongwei@tju.edu.cn; Tel.: +86-13821321024

**Abstract:** To achieve rapid and flexible vertical profile exploration of deep-sea hybrid structures, a multi-joint autonomous underwater vehicle (MJ-AUV) with orthogonal joints was designed. This paper focuses on the 3-dimensional (3D) modeling and attitude control of the designed vehicle. Considering the situation of gravity and buoyancy imbalance, a 3D model of the MJ-AUV was established according to Newton's second law and torque balance principle. And then the numerical simulation was carried out to verify the credibility of the model. To solve the problems that the pitch and yaw attitude of the MJ-AUV are coupled and the disturbance is unknown, a linear quadratic regulator (LQR) decoupling control method based on a linear extended state observer (LESO) was proposed. The system was decoupled into pitch and yaw subsystems, treated the internal forces and external disturbances of each subsystem as total disturbances, and estimated the total disturbances with LESO. The control law was divided into two parts. The first part was the total disturbance compensator, while the second part was the linear state feedback controller. The simulation results show that the overshoot of the controlled system in the dynamic process is nearly 0 rad, reaching the design value very smoothly. Moreover, when the controlled system is in a stable state, the control precision is within 0.005%.

**Keywords:** multi-joint autonomous underwater vehicle (MJ-AUV); 3-dimensional modeling; LQR; LESO

#### **1. Introduction**

Ocean exploration technology is one of the difficult problems in frontier science and engineering in the ocean field. The deep-sea region hybrid is an important factor in maintaining global energy balance and driving deep ocean circulation. Therefore, it is of great strategic significance to use advanced technology to explore the deep-sea region hybrid structures [1]. The complex structure of the seabed, as well as unknown extreme fluid systems such as cold springs and hydrothermal fluids, make the work of exploration more difficult.

Most of the existing deep-sea submersibles struggle to meet the capabilities necessary to explore deep-sea hybrid structures rapidly. The glider [2,3] achieves pose control by adjusting the remaining buoyancy and the position of the mass center, but its motion trajectory is jagged and single, speed is slow, and maneuverability is poor. Most autonomous underwater vehicles (AUVs) [4] are rigid single-body structures and use tail rudder and attitude adjustment systems to control the movement direction. In order to further improve the ability of flexible steering of the AUV to allow rapid observation of the deep-sea 3-dimensional (3D) environment, a multi-joint AUV (MJ-AUV) was designed. It consists of three parts in series: diversion cabin, navigation/control cabin, and propulsion cabin, each of which is connected by a pair of orthogonal joints. In addition, its tail is equipped with a propeller. To adjust the yaw attitude and pitch attitude of the body, the MJ-AUV can change the hydrodynamic appearance of the vehicle by rotating the joints.

**Citation:** Yu, L.; Meng, Q.; Zhang, H. 3-Dimensional Modeling and Attitude Control of Multi-Joint Autonomous Underwater Vehicles. *J. Mar. Sci. Eng.* **2021**, *9*, 307. https:// doi.org/10.3390/jmse9030307

Academic Editors: Rosemary Norman and Evgeny Veremey

Received: 14 January 2021 Accepted: 8 March 2021 Published: 10 March 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

The MJ-AUV is a multi-rigid-body rootless system with high nonlinear and strong coupling characteristics, the kinematic and dynamic modeling of which is the basis of studying its motion behavior characteristics and control problems. Xia et al. [5] established a horizontal dynamic model of a fish-like robot based on Kane's method. Aiming at the structure of the underwater gliding snake-like robot, Tang et al. [6] built a gliding and serpentine swimming model based on the momentum theorem, moment of momentum theorem, and recursive Newton–Euler method. Based on the principle of force and moment balance, Kelasidi et al. [7–9] built a horizontal plane and slope dynamics model of the underwater snake robot in the inertial system. In addition, the Euler–Lagrange method [10] and the Schiehlen method [11] have also been used to deal with multi-rigid body modeling.

The controller design is the key technology to enable underwater vehicles to complete a deep-sea exploration mission. Professor Pettersen's team from the University of Norway has made many contributions in the field of underwater multi-joint robot control, such as planar path tracking in the presence of ocean currents [12] and integral line-of-sight guidance for path following control [13]. Fischer et al. [14] propose an error controller using continuous robust integration to compensate for the uncertainty of the AUV model and have carried out experiments under controlled and open-water environments to verify the effectiveness of the controller. Zhao et al. [15] propose an adaptive plus disturbance observer (DOB) controller for depth and attitude control of the AUV. The controller consists of DOB as an inner-loop compensator and a non-regressor based adaptive controller as an outer-loop controller. In addition, the experiments verify that the controller has strong robustness. Wu et al. [16], Wang et al. [17] and Kang et al. [18] make improvements on the basis of the adaptive controller, which is verified in the field of motion control of AUVs. Zhang et al. [19] present a sliding mode variable structure control algorithm, and simulation results show that this algorithm has advantages of high control accuracy and strong robustness. Rodriguez et al. [20] combine sliding mode control with adaptive control and propose a sliding mode adaptive controller, which is compared with non-adaptive control and PD control to verify the effectiveness of the controller. References [21–25] have made improvements based on the active disturbance rejection controller, combining sliding mode controller, self-searching optimal algorithm, or other methods, to improve the accuracy and anti-interference performance of the AUV motion control. In addition to the above methods, in recent years, scholars are also studying the application of reinforcement learning [26,27] and artificial intelligence algorithms [28] in the field of AUV control.

MJ-AUV is a complex system with high nonlinear, strong coupling, large time delay, and unknown disturbance, and establishing an accurate mathematical model for the MJ-AUV is difficult. In the course of pitching and yaw attitude control, the MJ-AUV belongs to a typical multi-input and multi-output system, with the variation of two joint angles as the input and the pitch and yaw angles of the body as the output, making the controller design more difficult. Hence, the main contributions of this paper are as follows:


The remainder of this paper is organized as follows. Section 2 establishes the 3D motion model of the MJ-AUV. The LQR decoupling control method based on LESO is presented in Section 3. In Section 4, the pitch and yaw control of the MJ-AUV is simulated on the MATLAB/SIMULINK platform, followed by the conclusion in Section 5.

#### **2. Modelling**

This section introduces the structure design, kinematics, and dynamics analysis of the MJ-AUV and presents the 3D motion model.

#### *2.1. Structure of the MJ-AUV*

As shown in Figure 1, from front to back, the MJ-AUV is mainly composed of the diversion cabin, navigation/control cabin, and propulsion cabin, which are connected by two orthogonal (pitch and yaw degrees of freedom) joints. A propeller and a fixed rudder are installed at the tail to enhance the body stability. The sensors such as hydrophones and thermohaline depth meter can be mounted according to specific task requirements.

**Figure 1.** Reference frames of the MJ-AUV.

The technical challenge of the MJ-AUV is the joint design. The rotation of the orthogonal joint requires two motors to cooperate with each other to drive the bevel gears, so as to realize the pitch and yaw motion of the joint. The specific working process is as follows.

(1) As shown in Figure 2, when the two motors rotate in the opposite direction, the gear set is driven to rotate and the pitch motion of the joint can be realized.

**Figure 2.** Orthogonal joint pitching motion.

(2) As shown in Figure 3, when the two motors rotate in the same direction, the yaw motion of the joint can be realized.

**Figure 3.** Orthogonal joint yaw motion.

#### *2.2. Assumptions*

The MJ-AUV is a complex system with multi-rigid body configuration, high nonlinearity, and strong coupling, which brings great challenges to the modeling work. In order to further reduce the modeling difficulty without losing the generality and accuracy of the model, the following assumptions are proposed:


#### *2.3. Coordinate System Definition*

Figure 1 defines the inertial frame OEXEYEZE, body (navigation/control cabin) frame OBXBYBZB, diversion cabin frame OB1XB1YB1ZB1, and propulsion cabin frame OB3XB3YB3ZB3. The inertial frame is the north-east-and-up coordinate system [30]. OB is fixed at the centroid position of the navigation/control cabin. OBXB runs along the axis of the cabin, OBZB is perpendicular to OBXB and upwards, and the establishment of OBYB satisfies the right-hand rule. The coordinate system of the diversion cabin and the propulsion cabin is similar to that of the body frame.

#### *2.4. Motion Parameters Definition*

(1) Displacement

The displacement of the vehicle mainly includes three parts: surge, sway and heave, which are explained as follows:

Surge *X* is the projection of the origin position vector of the body frame on XE, and its direction is the same as XE.

Sway *Y* is the projection of the origin position vector of the body frame on YE, and its direction is the same as YE.

Heave *Z* is the projection of the origin position vector of the body frame on ZE, and its direction is the same as ZE.

(2) Attitude

The attitude angles of the vehicle are determined by the relationship between the body frame and the inertial frame.

Pitch *θ* is the included angle between XB and the sea level, when the body is descending, the direction is positive.

Roll *ϕ* is the included angle between ZB and the plumb plane passing through XB, when the body rolls to the right, the direction is positive.

Yaw *ψ* is the included angle between the projection of XB at sea level and XE, when the body yaws to the left, the direction is positive.

(3) Joint angles

The joint angles are determined by the relationship between the frame of the diversion cabin or propulsion cabin and the body frame. Each orthogonal joint can be used for pitch and yaw control.

The joint pitch angle *θ<sup>n</sup>* is the included angle between XB1 or XB3 and the plane XBOBYB, where *n* = 1, 2 represents the joint *n*, joint 1 is the joint between the diversion cabin and the navigation/control cabin, and joint 2 is the joint between the navigation/control cabin and the propulsion cabin. When the joint is deflected clockwise, the direction of *θ<sup>n</sup>* is specified as positive.

The joint yaw angle *ψ<sup>n</sup>* is the included angle between the projection of XB1 or XB3 onto the plane XBOBYB and XB. When the joint is deflected counterclockwise, the direction of *ψ<sup>n</sup>* is specified as positive.

For modeling convenience, the rotation order of orthogonal joints is defined here. First, it rotates around the Z-axis, then it rotates around the Y-axis of the changed frame.

(4) The linear velocity and angular velocity components of the body coordinate system

*u*, *v*, and *w* are the linear velocities along each axis of the body frame, where *u*, *v*, and *w* are in the same direction as XB, YB, and ZB, respectively. And *p*, *q*, and *r* represent the component of the attitude angular velocities around each axis of the body frame, which are roll velocity, pitch velocity, and yaw velocity, respectively.

#### *2.5. Kinematic Analysis*

The position of the body under the inertial frame is <sup>E</sup>*P*<sup>B</sup> <sup>=</sup> *XYZ* <sup>T</sup> , and the posture is <sup>E</sup>**Ω**<sup>B</sup> <sup>=</sup> *ϕθψ* <sup>T</sup> . The upper left corner of <sup>E</sup>*P*<sup>B</sup> and <sup>E</sup>**Ω**<sup>B</sup> is the reference frame, which can be omitted when the reference frame is itself, and *A*<sup>T</sup> is the transpose of *A*.

The position of the origin of each cabin <sup>B</sup>*P<sup>i</sup>* is expressed as

$$\begin{cases} \begin{aligned} \mathbf{P\_{B}} &= \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^{\mathrm{T}} \\ \mathbf{^{\mathrm{B}}P\_{\mathrm{B}1}} &= \begin{bmatrix} \begin{array}{cc} l\_{\mathrm{B}} & 0 & 0 \end{array} \end{bmatrix}^{\mathrm{T}} + \begin{array}{cc} \mathbf{^{\mathrm{B}}R\_{\mathrm{B}1}} \begin{bmatrix} l\_{\mathrm{B}1} & 0 & 0 \end{bmatrix}^{\mathrm{T}} \\ \mathbf{^{\mathrm{B}}P\_{\mathrm{B}3}} &= \begin{bmatrix} -l\_{\mathrm{B}} & 0 & 0 \end{bmatrix}^{\mathrm{T}} + \begin{bmatrix} \mathbf{^{\mathrm{B}}R\_{\mathrm{B}3}} \begin{bmatrix} -l\_{\mathrm{B}3} & 0 & 0 \end{bmatrix}^{\mathrm{T}} \end{bmatrix} \end{aligned} \end{cases} \end{cases} \tag{1}$$

where *li* is half the length of each cabin, *i* = B1, B, B3; and <sup>B</sup>*R*B1 and <sup>B</sup>*R*B3 are the conversion matrices of the diversion cabin frame and the propellant cabin frame to the body frame, respectively, as follows:

$$\begin{aligned} ^B \mathcal{R}\_{\mathcal{B}1} &= \operatorname{Rot}(z, \psi\_1) \mathcal{R} \operatorname{ot}(y, \theta\_1) \\ ^B \mathcal{R}\_{\mathcal{B}3} &= ^{\mathcal{B}3} \mathcal{R}\_{\mathcal{B}}{}^{-1} = \left( \operatorname{Rot}(z, \psi\_2) \mathcal{R} \operatorname{ot}(y, \theta\_2) \right)^{-1} \end{aligned} \tag{2}$$

$$\begin{aligned} Rot(z,\psi\_{\boldsymbol{n}}) &= \begin{bmatrix} \cos(\psi\_{\boldsymbol{n}}) & -\sin(\psi\_{\boldsymbol{n}}) & 0\\ \sin(\psi\_{\boldsymbol{n}}) & \cos(\psi\_{\boldsymbol{n}}) & 0\\ 0 & 0 & 1 \end{bmatrix} \\ Rot(y,\theta\_{\boldsymbol{n}}) &= \begin{bmatrix} \cos(\theta\_{\boldsymbol{n}}) & 0 & \sin(\theta\_{\boldsymbol{n}})\\ 0 & 1 & 0\\ -\sin(\theta\_{\boldsymbol{n}}) & 0 & \cos(\theta\_{\boldsymbol{n}}) \end{bmatrix} \end{aligned} \quad (\boldsymbol{n} = 1, \ 2), \tag{3}$$

where *R*−<sup>1</sup> is the inverse of *R*. Since *R* is an orthogonal matrix,

$$\prescript{\rm B}{}{\mathcal{R}}\_{\rm B3}{}^{-1} = \prescript{\rm B}{}{\mathcal{R}}\_{\rm B3}{}^{\rm T} = \textit{Rot}(\boldsymbol{y}, \theta\_2) \prescript{\rm T}{}{\operatorname{Rot}}(\boldsymbol{z}, \boldsymbol{\psi}\_2) \prescript{\rm T}{}{\boldsymbol{\url}}.\tag{4}$$

The angular velocity <sup>B</sup>*ω<sup>i</sup>* (*i* = *B*1, *B*, B3) and linear velocity <sup>B</sup>*v<sup>i</sup>* (*i* = *B*1, *B*, B3) are

$$\begin{cases} \begin{aligned} \boldsymbol{\omega}\_{\rm B} &= \begin{bmatrix} & \boldsymbol{p} & \boldsymbol{q} & \boldsymbol{r} \end{bmatrix}^{\mathrm{T}} \\ \boldsymbol{\up{^{\mathsf{B}}}}\boldsymbol{\omega}\_{\rm B1} &= \boldsymbol{\omega}\_{\rm B} + \begin{bmatrix} & \boldsymbol{0} & \boldsymbol{0} & \dot{\boldsymbol{\psi}}\_{1} \end{bmatrix}^{\mathrm{T}} + \mathrm{Rot}(\boldsymbol{z}, \boldsymbol{\psi}\_{1}) \begin{bmatrix} & \boldsymbol{0} & \dot{\boldsymbol{\theta}}\_{1} & \boldsymbol{0} \end{bmatrix}^{\mathrm{T}}, \end{cases} \end{cases} \end{cases}, \tag{5}$$
 
$$\begin{aligned} \boldsymbol{\up{^{\mathsf{B}}}}\boldsymbol{\omega}\_{\rm B3} &= \boldsymbol{\omega}\_{\rm B} - \begin{bmatrix} & \boldsymbol{0} & \dot{\boldsymbol{\up{^{\mathsf{B}}}}} \boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{B}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^{\mathsf{T}}}}\boldsymbol{\up{^$$

where . *<sup>θ</sup><sup>n</sup>* and . *ψ<sup>n</sup>* are the pitch and yaw velocities of the joints *n* (*n* = 1, 2), respectively. The velocity of the body is expressed in the inertial frame as

$$\begin{cases} \ ^{\text{E}}\dot{\mathbf{P}}\_{\text{B}} = ^{\text{E}}\mathbf{R}\_{\text{B}}\boldsymbol{\sigma}\_{\text{B}}\\ \ ^{\text{E}}\dot{\mathbf{D}}\_{\text{B}} = ^{\text{E}}\mathbf{G}\_{\text{B}}\boldsymbol{\omega}\_{\text{B}} \end{cases} \tag{7}$$

where <sup>E</sup> *. <sup>P</sup>*<sup>B</sup> and <sup>E</sup> *.* **<sup>Ω</sup>**<sup>B</sup> are the velocities of the body frame relative to the inertial frame, and <sup>E</sup>*R*<sup>B</sup> and <sup>E</sup>*G*<sup>B</sup> are the transformation matrices of the linear velocity and angular velocity from the body frame to the inertial frame, respectively. Tait-Bryan angles (Z-Y-X) rotation transformation is adopted to determine the two matrices as follows.

$$\begin{array}{rcl} ^{\mathsf{E}}\mathsf{R}\_{\mathsf{B}} &= \mathrm{Rot}(z,\psi)\mathrm{Rot}(y,\theta)\mathrm{Rot}(\mathbf{x},\boldsymbol{\varrho})\\ &= \begin{bmatrix} c\theta c\psi & s\varsigma s\theta c\psi - c\varsigma s\psi & c\varsigma s\theta c\psi + s\varsigma s\psi\\ c\theta s\psi & s\varsigma s\theta s\psi + c\varsigma c\psi & c\varsigma s\theta s\psi - s\varsigma c\psi\\ -s\theta & s\varsigma c\theta & c\varsigma c\theta \end{bmatrix} \end{array} \tag{8}$$

$$\mathbf{^EG\_B} = \begin{bmatrix} 1 & s\eta t\theta & c\eta t\theta \\ 0 & c\eta & -s\eta \\ 0 & s\eta/c\theta & c\eta/c\theta \end{bmatrix} \tag{9}$$

for any *j* = *ϕ*, *θ*, *ψ*, *sj* = sin *j*, *cj* = cos *j*, *tj* = tan *j*; *Rot*(*z*, *ψ*) and *Rot*(*y*, *θ*) are similar to Equation (3), and

$$\text{Rot}(x,\varphi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi) & -\sin(\phi) \\ 0 & \sin(\phi) & \cos(\phi) \end{bmatrix} \tag{10}$$

Equation (9) can be obtained by solving

$$
\begin{bmatrix} p \\ q \\ r \end{bmatrix} = \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix} + \text{Rot}(\mathbf{x}, \boldsymbol{\phi})^T \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix} + \text{Rot}(\mathbf{x}, \boldsymbol{\phi})^T \text{Rot}(\boldsymbol{y}, \boldsymbol{\theta})^T \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix}. \tag{11}
$$

Subject to the mechanical limit, the pitching angle will not reach ±90◦, so <sup>E</sup>*G*<sup>B</sup> will not be in a singular state.

The angular acceleration <sup>B</sup>*α<sup>i</sup>* (*i* = *B*1, B, B3) and linear acceleration <sup>B</sup>*a<sup>i</sup>* (*i* = *B*1, B, B3) are expressed as follows:

> ⎧ ⎪⎪⎪⎪⎪⎪⎨ ⎪⎪⎪⎪⎪⎪⎩ *<sup>α</sup>*<sup>B</sup> <sup>=</sup> . *<sup>p</sup>* . *<sup>q</sup>* . *r* <sup>T</sup> <sup>B</sup>*α*B1 <sup>=</sup> *<sup>d</sup>*B*ω*B1 *dt* <sup>+</sup> *<sup>ω</sup>*<sup>B</sup> <sup>×</sup> <sup>B</sup>*ω*B1 <sup>B</sup>*α*B3 <sup>=</sup> *<sup>d</sup>*B*ω*B3 *dt* <sup>+</sup> *<sup>ω</sup>*<sup>B</sup> <sup>×</sup> <sup>B</sup>*ω*B3 , (12) ⎧ ⎪⎪⎪⎪⎪⎨ ⎪⎪⎪⎪⎪⎩ *a*<sup>B</sup> = *<sup>d</sup>v*<sup>B</sup> *dt* + *ω*<sup>B</sup> × *v*<sup>B</sup> <sup>B</sup>*a*B1 <sup>=</sup> *<sup>d</sup>*B*v*B1 *dt* <sup>+</sup> *<sup>ω</sup>*<sup>B</sup> <sup>×</sup> <sup>B</sup>*v*B1 <sup>B</sup>*a*B3 <sup>=</sup> *<sup>d</sup>*B*v*B3 *dt* <sup>+</sup> *<sup>ω</sup>*<sup>B</sup> <sup>×</sup> <sup>B</sup>*v*B3 , (13)

where . *p*, . *<sup>q</sup>*, and . *r* are the angular accelerations of the roll, pitch, and yaw of the body, respectively.

#### *2.6. Dynamic Analysis*

In the process of moving, the MJ-AUV is mainly subject to the fluid drag, the inertial hydrodynamic force caused by additional mass, buoyancy, gravity, thrust, and interaction forces between joints.

#### 2.6.1. Hydrodynamic Analysis

Figure 1 shows the structure of the MJ-AUV equipped with multiple sensors. Notably, it is not a regular cylinder. Considering the influence of the pressure drag and the trailing vortex shedding effect, the drag suffered by each cabin is expressed as follows:

$$\prescript{\rm B}{}{\rm div}\_{\rm di} = \prescript{\rm B}{}{\rm B}\_{\rm i} \Big( -\prescript{\rm C}{}{\rm Di}\_{\rm Di} \text{sgn}(\boldsymbol{\upsilon}\_{i}) \boldsymbol{\upsilon}\_{i}^{\rm 2} - \prescript{\rm C}{}{\rm Di}\_{\rm Di} \boldsymbol{\upsilon}\_{i} \Big)\_{\prime} \ (i = \text{B1}, \text{ B, B3}) \tag{14}$$

with

$$\begin{array}{l} \mathbf{C}\_{\text{Di}}^{2} = \left[ \begin{array}{cc} \mathfrak{c}\_{\text{Di}}^{2} & 0 & 0\\ 0 & \mathfrak{c}\_{\text{Di}}^{2} & 0\\ 0 & 0 & \mathfrak{c}\_{\text{Di}}^{2} \end{array} \right], (i = \text{B1, B, B3}) \\\\ \mathbf{C}\_{\text{Di}}^{1} = \left[ \begin{array}{cc} \mathfrak{c}\_{\text{Di}}^{1} & 0 & 0\\ 0 & \mathfrak{c}\_{\text{Di}}^{1} & 0\\ 0 & 0 & \mathfrak{c}\_{\text{Di}}^{1} \end{array} \right], (i = \text{B1, B, B3}) \\\\ \text{sgn}(\mathbf{s}) = \left\{ \begin{array}{cc} 1, & \mathbf{s} > 0\\ 0, & \mathbf{s} = 0\\ -1, & \mathbf{s} < 0 \end{array} \right. \end{array} \right. \\\\ \mathbf{v}\_{i} = \text{^B R}\_{i} \mathbf{r}^{\text{TB}}\_{\text{v}\_{i}} \text{, (i = \text{B1, B2, B3}) \end{array}$$

where *c*<sup>2</sup> <sup>D</sup>*i*x, *<sup>c</sup>*<sup>2</sup> <sup>D</sup>*i*y, and *<sup>c</sup>*<sup>2</sup> <sup>D</sup>*i*z, and *<sup>c</sup>*<sup>1</sup> <sup>D</sup>*i*x, *<sup>c</sup>*<sup>1</sup> <sup>D</sup>*i*y, and *<sup>c</sup>*<sup>1</sup> <sup>D</sup>*i*<sup>z</sup> are the quadratic and the first-order coefficients of drag on 3D linear velocity, respectively.

When the MJ-AUV travels with variable speed motion, it forms a relative acceleration motion with the surrounding water bodies, causing the additional mass effect and producing the effect opposite to the direction of acceleration, which can be expressed by

$$\:^B F\_{\rm ai} = -\:^B R\_i(\lambda\_{\rm mi}\mathfrak{a}\_i), \; M\_{\rm ai} = -\lambda\_{\rm ji}\mathfrak{a}\_{i\prime} \ (i = \text{B1}, \; \text{B}, \; \text{B3}), \tag{15}$$

with

$$
\boldsymbol{\lambda}\_{i} = \begin{bmatrix}
\lambda\_{\text{mi}} & \mathbf{0}\_{3 \times 3} \\
\mathbf{0}\_{3 \times 3} & \lambda\_{\text{li}}
\end{bmatrix} = \begin{bmatrix}
\lambda\_{i11} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \lambda\_{i22} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \lambda\_{i33} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \lambda\_{i44} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \lambda\_{i55} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \lambda\_{i66}
\end{bmatrix}, \begin{pmatrix} i = \mathbf{B1, B, B3} \end{pmatrix}
$$

$$
\mathbf{a}\_{i} = \mathbf{^{B}R\_{i}^{TB}} \mathbf{a}\_{i\prime} \ (i = \mathbf{B1, B, B3})
$$

and *λ<sup>i</sup>* is the added mass matrix of each cabin.

The buoyancy of each cabin of the MJ-AUV is equal to the gravity of the water discharged from the cabin. Here, assuming that the density of seawater is almost constant at different depths. Thus, when the MJ-AUV is fully immersed in seawater, the buoyancy of each cabin is

$$\begin{aligned} \,^B F\_{\text{bi}} &= \,^E \mathbf{R}\_{\text{B}} ^T \begin{bmatrix} \; \; \; 0 & \; \; \; \; \; \; \; \; F\_{\text{bi}} \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; B3 \; \end{bmatrix} \\ &\quad \begin{aligned} \,^B F\_{\text{bi}} &= \rho\_{\text{liquid}} V\_{i} \mathbf{g}\_{\text{\textdegree}} \; \; \; (i = \text{B1}, \; \; \text{B2}, \; \text{B3} \; \;) \end{aligned} \tag{16}$$

where *ρ*liquid is the density of seawater, *Vi* is the volume of each cabin, and *g* is the gravitational acceleration. The buoyancy of each cabin acts at the center, so the moment of which is 0.

#### 2.6.2. Gravity and Gravitational Moment

In the air, the gravity on each cabin is expressed in the body frame as:

$$\begin{aligned} \,^B \mathbf{G}\_i &= \,^B \mathbf{R}\_B \,^{\text{TE}} \mathbf{G}\_{i\prime} \ (i = \text{B1}\_{\prime} \text{ B}\_{\prime} \ \text{B3}\_{\prime})\\\\ ^{\text{E}} \mathbf{G}\_i &= \begin{bmatrix} \ \ 0 & 0 & m\_i \mathbf{g} \ \end{bmatrix} ^\text{T} \ \text{,} \ (i = \text{B1}\_{\prime} \ \text{B}\_{\prime} \ \text{B3}) \end{aligned} \tag{17}$$

where *mi* is the mass of each cabin.

The center of gravity is directly below the centroid, so each cabin is affected by the gravitational moment as:

$$\mathbf{M}\_{\overline{\theta}^{\overline{i}}} = \begin{bmatrix} 0 & 0 & -l\_{\mathbb{C}} \ \end{bmatrix}^{\mathrm{T}} \times \begin{pmatrix} ^{\mathrm{B}}\mathbf{R}\_{i}^{\mathrm{TB}}\mathbf{G}\_{i} \end{pmatrix}, \begin{pmatrix} i = \mathrm{B1}, \ \mathrm{B}, \ \mathrm{B3} \end{pmatrix} \tag{18}$$

where *l*c represents the distance between the center of gravity and the centroid of each cabin.

#### 2.6.3. Thrust Analysis

The propeller is installed at the rear of the propulsion cabin of the MJ-AUV and is a one-way force. The direction is forward along the axial direction of the propulsion cabin. Therefore, the thrust does not produce a torque effect on the propulsion cabin. The thrust is expressed in the body frame as:

$$\:^B F\_{\rm th} = \:^B \mathcal{R}\_{\rm B3} F\_{\rm th'} \tag{19}$$

with *F*th = *F*<sup>t</sup> 0 0 and *F*<sup>t</sup> is the thrust from the propeller.

#### 2.6.4. Dynamical Equation

According to Newton's second law, the force analysis is shown in the following formula.

$$\rm{^BF\_{bB1} + ^BF\_{dB1} + ^BF\_{aB1} - ^BG\_{B1} + ^BF\_{B \to B1} = m\_{B1}I^B\_{B1}}$$

$$\rm{^BF\_{bB} + F\_{dB} + F\_{aB} - G\_B - ^BF\_{B \to B1} + ^BF\_{B3 \to B} = m\_{B}Ia\_{\rm B}}\tag{20}$$

$$\rm{^BF\_{bB3} + ^BF\_{dB3} + ^BF\_{aB3} - ^BG\_{B3} - ^BF\_{B3 \to B} + ^BF\_{th} = m\_{B3}I^B\_{B3}}$$

where <sup>B</sup>*F*B→B1 represents the force of the navigation/control cabin on the diversion cabin in the body frame, <sup>B</sup>*F*B3→<sup>B</sup> stands for the force of the propulsion cabin on the navigation/control cabin in the body frame, and *I* is a 3 × 3 identity matrix. By adding the three expressions in Equation (20), the interaction force between joints can be eliminated, i.e.

$$\begin{aligned} \,^B F\_{\rm bB1} + \,^B F\_{\rm dB1} + \,^B F\_{\rm aB1} - \,^B G\_{\rm B1} + F\_{\rm bB} + F\_{\rm dB} + F\_{\rm aB} - G\_{\rm B} + \,^B F\_{\rm bB3} + \\\\ \,^B F\_{\rm dB3} + \,^B F\_{\rm aB3} - \,^B G\_{\rm B3} + \,^B F\_{\rm th} - m\_{\rm B1} \,^B a\_{\rm B1} - m\_{\rm B} \,^B a\_{\rm B} - m\_{\rm B3} \,^B a\_{\rm B3} = 0 \end{aligned} \tag{21}$$

According to the torque balance principle, the torque analysis is shown in the following formula.

$$\begin{cases} \mathcal{M}\_{\text{aB}1} + \,^{\text{B1}}\mathcal{M}\_{1} - \mathcal{M}\_{\text{gB}1} + \,^{\text{B1}}\mathcal{M}\_{\text{B}\to\text{B1}} = J\_{\text{B1}}\mathfrak{a}\_{\text{B1}} \\\\ \mathcal{M}\_{\text{aB}} + \,^{\text{B1}}\mathcal{M}\_{2} - \,^{\text{B1}}\mathcal{M}\_{1} - \mathcal{M}\_{\text{gB}} - \,^{\text{B1}}\mathcal{M}\_{\text{B}\to\text{B1}} + \,^{\text{B1}}\mathcal{M}\_{\text{B3}\to\text{B}} = J\_{\text{B}}\mathfrak{a}\_{\text{B}} \\\\ \mathcal{M}\_{\text{aB}3} - \,^{\text{B3}}\mathcal{M}\_{2} - \mathcal{M}\_{\text{gB}3} - \,^{\text{B3}}\mathcal{M}\_{\text{B3}\to\text{B}} = J\_{\text{B3}}\mathfrak{a}\_{\text{B3}} \end{cases} \tag{22}$$

,

with

⎧ ⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

$$\begin{array}{rcl} \mathrm{^B1M\_{B \to B1}} = & \left[ \begin{array}{c} -I\_{\mathrm{B1}} \quad 0 \quad 0 \end{array} \right]^{\mathrm{T}} \times \left( \mathrm{^BR\_{\mathrm{B1}}\mathrm{^TB\_{\mathrm{B} \to B1}}} \right), \\\\ \mathrm{^BM\_{B \to B1}} = & \left[ \begin{array}{cc} I\_{\mathrm{B1}} & 0 & 0 \end{array} \right]^{\mathrm{T}} \times \mathrm{^BF\_{\mathrm{B} \to B1}}, \\\\ \mathrm{^BM\_{B3 \to B}} = & \left[ \begin{array}{cc} -I\_{\mathrm{B}} & 0 & 0 \end{array} \right]^{\mathrm{T}} \times \mathrm{^BF\_{\mathrm{B3} \to B}}, \\\\ \mathrm{^BM\_{B3 \to B}} = & \left[ \begin{array}{cc} I\_{\mathrm{B3}} & 0 & 0 \end{array} \right]^{\mathrm{T}} \times \left( \mathrm{^BR\_{\mathrm{B3}}\mathrm{^TB\_{\mathrm{B3} \to B}}} \right), \\\\ I\_{\mathrm{i}} = & \left[ \begin{array}{cc} I\_{\mathrm{i} \mathrm{x}} & 0 & 0 \\ 0 & I\_{\mathrm{i} \mathrm{y}} & 0 \\ 0 & 0 & I\_{\mathrm{i} \mathrm{x}} \end{array} \right], \ (i = B1, \ \mathrm{B}, \ \mathrm{B3}) \end{array}$$

$$\begin{array}{cc} \mathrm{\alpha}\_{i} = \,^{\mathrm{B}R\_{i}^{\mathrm{T}\mathrm{B}}\mathrm{\alpha}\_{i}}, \ (i = B1, \ \mathrm{B}, \ \mathrm{B3}) \end{array}$$

where B1*M*B→B1 is the torque generated by the navigation/control cabin on the diversion cabin expressed in the diversion cabin frame; the definitions of <sup>B</sup>*M*B→B1, <sup>B</sup>*M*B3→B, and B3*M*B3→<sup>B</sup> are similar to B1*M*B→B1; *<sup>J</sup><sup>i</sup>* is the matrix of the moment of inertia of each cabin; and <sup>B</sup>*F*B→B1 and <sup>B</sup>*F*B3→<sup>B</sup> can be obtained from Equation (20).

All the variables in Equation (22) are expressed in the body frame,

$$\begin{cases} \begin{aligned} \,^{\text{B}}\mathcal{R}\_{\text{B}1} \left( \mathcal{M}\_{\text{a}\text{B}1} - \mathcal{M}\_{\text{g}\text{B}1} + \,^{\text{B}1}\mathcal{M}\_{\text{B}\to\text{B}1} \right) + \,^{\text{B}}\mathcal{M}\_{1} = \,^{\text{B}}\mathcal{R}\_{\text{B}1} f\_{\text{B}1} \mathfrak{a}\_{\text{B}1} \\\\ \mathcal{M}\_{\text{a}\text{B}} + \,^{\text{B}}\mathcal{M}\_{2} - \,^{\text{B}}\mathcal{M}\_{1} - \mathcal{M}\_{\text{g}\text{B}} - \,^{\text{B}}\mathcal{M}\_{\text{B}\to\text{B}1} + \,^{\text{B}}\mathcal{M}\_{\text{B}3\to\text{B}} = f\_{\text{B}} \mathfrak{a}\_{\text{B}} \ . \end{aligned} \end{cases} \\\\ \begin{aligned} \,^{\text{B}}\mathcal{M}\_{\text{B}3} \left( \mathcal{M}\_{\text{a}\text{B}3} - \mathcal{M}\_{\text{g}\text{B}3} - \,^{\text{B}3}\mathcal{M}\_{\text{B}3\to\text{B}} \right) - \,^{\text{B}}\mathcal{M}\_{2} = \,^{\text{B}}\mathcal{R}\_{\text{B}3} f\_{\text{B}3} \ \mathfrak{a}\_{\text{B}3} \end{aligned} \tag{23}$$

The attitude of the MJ-AUV is controlled by changing the joint angle. By adding the three equations in (23), the following equation can be obtained:

$$\begin{aligned} \,^B \mathcal{R}\_{\rm B1} \left( \mathcal{M}\_{\rm aB1} - \mathcal{M}\_{\rm gB1} + \,^{\rm B1} \mathcal{M}\_{\rm B\to B1} - J\_{\rm B1} \mathfrak{a}\_{\rm B1} \right) + \mathcal{M}\_{\rm aB} - \mathcal{M}\_{\rm gB} - \,^{\rm B} \mathcal{M}\_{\rm B\to B1} + \\\\ \,^B \mathcal{M}\_{\rm B3\to B} - J\_{\rm B} \mathfrak{a}\_{\rm B} + \,^B \mathcal{R}\_{\rm B3} \left( \mathcal{M}\_{\rm aB3} - \mathcal{M}\_{\rm gB3} - \,^{\rm B3} \mathcal{M}\_{\rm B3\to B} - J\_{\rm B3} \mathfrak{a}\_{\rm B3} \right) = 0 \end{aligned} \tag{24}$$

The dynamic models of the MJ-AUV, i.e., Equation (7), Equation (21), and Equation (24), can be expressed as the state space equation as follows:

$$\begin{aligned} \mathcal{M}\dot{\mathbf{X}} &= \eta(\mathbf{X}, \mathbf{U}), \\\\ \mathcal{M} &= \begin{bmatrix} I\_{6 \times 6} & 0\_{6 \times 6} \\ 0\_{6 \times 6} & \mathcal{M}\_{22} \end{bmatrix}, \\\\ \mathcal{M}\_{22} &= \begin{bmatrix} \frac{\partial F\_{\text{total}}}{\partial u} & \frac{\partial F\_{\text{total}}}{\partial v} & \frac{\partial F\_{\text{total}}}{\partial w} & \frac{\partial F\_{\text{total}}}{\partial p} & \frac{\partial F\_{\text{total}}}{\partial q} & \frac{\partial F\_{\text{total}}}{\partial r} \\\\ \frac{\partial M\_{\text{total}}}{\partial u} & \frac{\partial M\_{\text{total}}}{\partial v} & \frac{\partial M\_{\text{total}}}{\partial w} & \frac{\partial M\_{\text{total}}}{\partial p} & \frac{\partial M\_{\text{total}}}{\partial q} & \frac{\partial M\_{\text{total}}}{\partial r} \end{bmatrix}, \end{aligned} \tag{25}$$

where <sup>M</sup> is a 12 <sup>×</sup> 12 inertial matrix, *<sup>X</sup>* <sup>=</sup> *XYZ ϕθψ uvwpqr* <sup>T</sup> , *U* = *θ*<sup>1</sup> *θ*<sup>2</sup> *ψ*<sup>1</sup> *ψ*<sup>2</sup> *Ft* T , and *η*(*X*, *U*) represents the function about *X* and *U*. Equation (25) can be rewritten as

$$
\dot{\mathbf{X}} = \mathcal{M}^{-1} \boldsymbol{\eta}(\mathbf{X}, \mathbf{U}).\tag{26}
$$

The dynamic model is an analytical model that can be used to study the motion characteristics of the MJ-AUV and design a modern controller based on this model.

#### **3. Attitude Controller Design**

By adjusting the pitch and yaw angles of joints 1 and 2, the relative attitude of each cabin is adjusted, the hydrodynamic appearance of the MJ-AUV is changed, and then the attitude angle of the body is adjusted. Because of the mechanical structure characteristics, joints can only change with one degree of freedom at the same time. To realize pitch and yaw control of the body at the same time, stipulating that joint 1 is used for the yaw attitude adjustment, and joint 2 is used for the pitch attitude adjustment, namely, *<sup>U</sup>* <sup>=</sup> <sup>0</sup> *<sup>θ</sup>*<sup>2</sup> *<sup>ψ</sup>*<sup>1</sup> <sup>0</sup> <sup>T</sup> . The MJ-AUV is a complex system with high nonlinear and strong coupling. To reduce the coupling degree and operation cost of the control system and improve the anti-disturbance performance, internal forces, coupling factors, and external disturbance are regarded as total disturbance and establish the subsystem model of the pitch and yaw as follows:

$$\begin{cases} \ddot{\theta} = f\_{\theta} + b\_{11} \psi\_{1} + b\_{12} \theta\_{2} \\\\ \ddot{\psi} = f\_{\psi} + b\_{21} \psi\_{1} + b\_{22} \theta\_{2} \end{cases} \tag{27}$$

where .. *<sup>θ</sup>* is the pitch acceleration of the body, .. *ψ* is the yaw acceleration, *bij*(*i*, *j* = 1, 2) is the control coefficient of joints 1 and 2 in pitch and yaw motion equations, and *f<sup>θ</sup>* and *f<sup>ψ</sup>* are nonlinear total perturbation functions associated with *X* and the input coupling terms.

The pitch and yaw attitude control of the MJ-AUV mainly includes the total disturbance observation compensation and LQR control of each subsystem. The designed controller structure is shown in Figure 4.

#### *3.1. Linear Extended State Observer Design*

The LESO is used to estimate the states and disturbances of the system that cannot be measured directly. Taking the pitch attitude subsystem as an example, the process of LESO design is as follows.

Let *<sup>x</sup>*<sup>1</sup> <sup>=</sup> *<sup>θ</sup>*, *<sup>x</sup>*<sup>2</sup> <sup>=</sup> . *θ*, *x*<sup>3</sup> = *fθ*, *u*<sup>1</sup> = *ψ*1, and *u*<sup>2</sup> = *θ*2; then, the pitch attitude subsystem can be rewritten as: *.*

$$\begin{aligned} \dot{\mathbf{x}} &= A\mathbf{x} + Bu + \mathbf{E}h \\ y &= \mathbf{C}\mathbf{x} + Du \end{aligned} \tag{28}$$

$$\begin{aligned} \text{where } & \mathbf{A} = \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix}, \mathbf{B} = \begin{bmatrix} 0 & 0 \\ b\_{11} & b\_{12} \\ 0 & 0 \end{bmatrix}, \mathbf{C} = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}, \mathbf{D} = \begin{bmatrix} 0 & 0 \end{bmatrix}, \mathbf{E} = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}, \\\ & \mathbf{x} = \begin{bmatrix} \ x\_{1} & x\_{2} & x\_{3} \end{bmatrix}^{\mathrm{T}}, \mathbf{u} = \begin{bmatrix} \ u\_{1} & u\_{2} \end{bmatrix}^{\mathrm{T}}, \text{and } h = \dot{f}\_{\boldsymbol{\theta}} \text{ is unknown but bounded.} \end{aligned}$$

Define *^ x* = [ *x*ˆ1 *x*ˆ2 *x*ˆ3 ] <sup>T</sup> and *<sup>y</sup>*<sup>ˆ</sup> as the observed values of *<sup>x</sup>* and *<sup>y</sup>*. Consequently, the LESO can be constructed as

$$\begin{aligned} \stackrel{\circ}{\dot{\mathbf{x}}} &= A\hat{\mathbf{x}} + Bu + L(y - \hat{y}) \\ \mathcal{Y} &= \stackrel{\circ}{\mathbf{C}} \hat{\mathbf{x}} + Du \end{aligned} \tag{29}$$

where *L* = [ *β*<sup>1</sup> *β*<sup>2</sup> *β*<sup>3</sup> ] <sup>T</sup> is the observer gain vector.

Define *<sup>~</sup> <sup>x</sup>* <sup>=</sup> *<sup>x</sup>* <sup>−</sup> *^ x* is the state estimation error. By combining (28) and (29), can be obtained *.*

$$\begin{array}{l} \stackrel{\star}{\mathbf{x}} = A\stackrel{\star}{\mathbf{x}} + Bu + L(y - \mathcal{G}) \\ \stackrel{\star}{\mathbf{y}} = \stackrel{\star}{\mathbf{C}\dot{\mathbf{x}}} + Du \\ \tilde{A} = A - LC = \begin{bmatrix} -\beta\_1 & 1 & 0 \\ -\beta\_2 & 0 & 1 \\ -\beta\_3 & 0 & 0 \end{bmatrix}. \end{array} \tag{30}$$

According to [29], through the pole assignment method, all poles are placed at −*ω*0, then *β*<sup>1</sup> = 3*ω*0, *β*<sup>2</sup> = 3*ω*<sup>0</sup> 2, and *β*<sup>3</sup> = *ω*<sup>0</sup> 3. *ω*0(*ω*<sup>0</sup> > 0) is the bandwidth of the state observer. The estimated state of the observer can be adjusted by adjusting *ω*0, especially the estimated value ˆ *f<sup>θ</sup>* = *x*ˆ3 of the total disturbance *fθ*. The design of the LESO for the yaw subsystem is similar to that for the pitch subsystem.

**Figure 4.** The structure of the LQR decoupling control method based on LESO.

#### *3.2. Control Law Design*

To reduce the input coupling degree of the system, the virtual control quantity - *<sup>u</sup>* <sup>=</sup> . *<sup>b</sup>*<sup>11</sup> *<sup>b</sup>*<sup>12</sup> *<sup>b</sup>*<sup>21</sup> *<sup>b</sup>*<sup>22</sup> / *u* is introduced.

*θ*<sup>d</sup> and *ψ*<sup>d</sup> are defined as the designed pitch and yaw angles, respectively. Then, the feedback errors are *<sup>e</sup><sup>θ</sup>* <sup>=</sup> *<sup>θ</sup>*<sup>d</sup> <sup>−</sup> *<sup>θ</sup>*, *<sup>e</sup>*<sup>ψ</sup> <sup>=</sup> *<sup>ψ</sup>*<sup>d</sup> <sup>−</sup> *<sup>ψ</sup>*. Let *<sup>z</sup>*<sup>1</sup> <sup>=</sup> *<sup>e</sup>θ*, *<sup>z</sup>*<sup>2</sup> <sup>=</sup> . *<sup>θ</sup>*, *<sup>z</sup>*<sup>3</sup> <sup>=</sup> *<sup>e</sup>*ψ, and *<sup>z</sup>*<sup>4</sup> <sup>=</sup> . *ψ*; then, the state equation of the pitch and yaw attitude control of the MJ-AUV is

$$
\stackrel{\circ}{\mathbf{Z}} = \stackrel{\circ}{\mathbf{A}} \stackrel{\circ}{\mathbf{Z}} + \stackrel{\circ}{\mathbf{B}} \stackrel{\circ}{\mathbf{u}} + \stackrel{\circ}{f}, \tag{31}
$$

where

$$
\widehat{A} = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & -1 \\ 0 & 0 & 0 & 0 \end{bmatrix}, \widehat{\mathbf{B}} = \begin{bmatrix} 0 & 0 \\ 0 & 1 \\ 0 & 0 \\ 1 & 0 \end{bmatrix}'
$$

*]* = [ *z*<sup>1</sup> *z*<sup>2</sup> *z*<sup>3</sup> *z*<sup>4</sup> ] T , *^ f* = [ 0 ˆ *f*<sup>θ</sup> 0 ˆ *f*<sup>ψ</sup> ] T , ˆ *f*<sup>θ</sup> and ˆ *f*<sup>ψ</sup> respectively represent the total disturbance of the pitch and yaw subsystems estimated by LESO.

The control law mainly includes disturbance compensation - *u*<sup>f</sup> and linear state feedback - *<sup>u</sup>*0, and - *<sup>u</sup>* <sup>=</sup> - *<sup>u</sup>*<sup>f</sup> <sup>+</sup> - *u*0.

The disturbance compensation control law is

$$
\stackrel\frown{\hat{\boldsymbol{\mu}}\_{\mathbf{f}}} = -\stackrel\frown{\hat{\mathbf{B}}}^\mathbf{T} \hat{\boldsymbol{f}}.\tag{32}
$$

Substituting Equation (32) into Equation (31) obtains the following:

$$
\stackrel{\circ}{\mathfrak{Z}} = \stackrel{\frown}{\mathfrak{A}} \mathfrak{Z} + \stackrel{\frown}{\mathfrak{B}} \stackrel{\frown}{\mathfrak{u}}\_{0}. \tag{33}
$$

Now, the problem is to design the controller - *u*0, and the linear state feedback control law can be designed as

$$
\widehat{\boldsymbol{\mu}}\_0 = -\mathbf{K} \mathbf{\mathcal{Z}}\_{\prime} \tag{34}
$$

with *K* = . *K*<sup>11</sup> *K*<sup>12</sup> *K*<sup>13</sup> *K*<sup>14</sup> *<sup>K</sup>*<sup>21</sup> *<sup>K</sup>*<sup>22</sup> *<sup>K</sup>*<sup>23</sup> *<sup>K</sup>*<sup>24</sup> / as the linear feedback gain matrix.

The feedback gain matrix has eight parameters. In this paper, LQR can be optimized by LQR to reduce the difficulty of parameter adjustment and obtain the optimal control law suitable for the control target by minimizing the performance index. The LQR performance index function is selected as

$$J = \int\_0^\infty \left( \mathbf{z}^\top \mathbf{Q} \mathbf{z} + \widehat{\boldsymbol{\mu}}\_0 \mathbf{u}^\top \mathbf{R} \widehat{\boldsymbol{\mu}}\_0 \right) dt,\tag{35}$$

where *Q* is the positive semidefinite matrix and *R* is the positive definite matrix. The former is the penalty function of the system state error, and the latter is the penalty function of the system input state. The Riccati equation corresponding to the performance index function is -

$$
\stackrel{\frown}{A}^T \mathbf{P} + \stackrel{\frown}{P} \stackrel{\frown}{A} + \mathbf{Q} - \stackrel{\frown}{P} \stackrel{\frown}{B} \stackrel{\frown}{A}^T \mathbf{P} = 0,\tag{36}
$$

where *P* is the solution of the Riccati equation.

In the case of the Riccati equation, the linear feedback gain matrix is

$$\mathbf{K} = \mathbf{R}^{-1} \mathbf{B}^{T} \mathbf{P}.\tag{37}$$

In this case, *K* is related to *Q* and *R*.

#### *3.3. Stability Analysis*

In this part, the stability of the LESO and the closed-loop system are studied according to the idea of reference [31].

A state space equation is defined in the form shown in Equation (38). *.*

$$
\dot{\mathbf{c}} = \mathbf{H} \mathbf{c} + \mathbf{o}\_{\prime} \tag{38}
$$

where *<sup>c</sup>* ∈ *<sup>n</sup>* represents state variable, *<sup>H</sup>* ∈ *n*×*n*, *<sup>o</sup>* ∈ *n*.

**Theorem 1.** *Assuming that o is bounded, if H is the Hurwitz matrix, then the state variable c is bounded stable.*

The proof of Theorem 1 is given in Appendix A.

According to Theorem 1, in Equation (30), *<sup>h</sup>* is bounded, *<sup>~</sup> <sup>A</sup>* is Hurwitz, and *<sup>~</sup> x* is bounded stable, that is, LESO is bounded stable.

The system model of the MJ-AUV is

$$
\stackrel{\circ}{\mathbf{z}} = \stackrel{\frown}{\mathbf{A}} \stackrel{\frown}{\mathbf{z}} + \stackrel{\frown}{\mathbf{B}} \stackrel{\frown}{\mathbf{u}} + \stackrel{\circ}{\mathbf{f}}, \tag{39}
$$

with *f* = [ 0 *f*<sup>θ</sup> 0 *f*<sup>ψ</sup> ] T . Because - *<sup>u</sup>* <sup>=</sup> - *<sup>u</sup>*<sup>f</sup> <sup>+</sup> - *u*0. Because, Equation (39) can be expressed as: *.*

$$\stackrel{\circ}{\mathbf{Z}} = \stackrel{\circ}{\mathbf{A}}\hat{\mathbf{z}} + \stackrel{\circ}{\mathbf{B}}\left(\stackrel{\circ}{\hat{\boldsymbol{\mu}}\_{\text{f}}} + \stackrel{\circ}{\hat{\boldsymbol{\mu}}\_{\text{0}}}\right) + \stackrel{\circ}{\mathbf{f}}.\tag{40}$$

By substituting Equations (32) and (34) into Equation (40), we can obtain

$$
\stackrel{\circ}{\mathbf{Z}} = \left(\stackrel{\frown}{\mathbf{A}} - \stackrel{\frown}{\mathbf{B}}\mathbf{K}\right)\mathbf{Z} + \stackrel{\circ}{f},\tag{41}
$$

with *<sup>~</sup> <sup>f</sup>* <sup>=</sup> *<sup>f</sup>* <sup>−</sup> *^ f*. In addition, substituting Equation (37) into Equation (41) obtains

$$\stackrel{\circ}{\mathbf{z}} = \left(\widehat{A} - \widehat{B}\mathbf{R}^{-1}\mathbf{B}^{\mathrm{T}}\mathbf{P}\right)\mathbf{z} + \tilde{f}.\tag{42}$$

*~ <sup>f</sup>* is bounded according to LESO stability analysis, and - *<sup>A</sup>* <sup>−</sup> - *BR*−1*B*T*P* is Hurwitz. According to Theorem 1, *]* is bounded stable.

#### **4. Simulation and Results**

In this part, the dynamic model of the MJ-AUV was built on the SIMULINK platform, and the fourth-order Runge–Kuta algorithm was used to solve the second-order differential dynamics equation. Then, the various motions of the model were simulated and analyzed, and the attitude control algorithm of MJ-AUV was simulated and verified. Table A1 shows the model parameters of the MJ-AUV.

#### *4.1. Model Simulation*

Figure 5 shows the simulation results of MJ-AUV's pitching motion, and the pitching angle of joint 1 is set to *θ*<sup>1</sup> = 0◦, the yaw angle of joint 1 is set to *ψ*<sup>1</sup> = 0◦, the pitch angle of joint 2 rotates in a sinusoidal manner with an amplitude of 20◦ and a period of 50 s, that *θ*<sup>2</sup> = 20◦ sin( *<sup>π</sup><sup>t</sup>* <sup>25</sup> ), the yaw angle of joint 2 is set to *ψ*<sup>2</sup> = 0◦, and the thrust of the propeller is set to *T* = 60 N. The simulation duration is 100 s. It can be seen from Figure 5a,e that the vehicle has a tendency to move upwards, which is caused by the buoyancy force of the vehicle being greater than its gravity. During the pitching motion, the results in Figure 5a,c show that the vehicle has no change in displacement and velocity in the Y direction, while the results in Figure 5b,d show that the attitudes and angular velocities of the vehicle have no changes in the yaw and roll directions. Therefore, if the vehicle is only controlled in pitching motion, the relative state quantity of the three degrees of freedom can be zero, thus simplifying the complexity of the model.

Figure 6 shows the simulation results of the sinuous motion of the MJ-AUV. The pitching angle of joint 1 is set to *θ*<sup>1</sup> = 0◦, the yaw angle of joint 1 rotates in a sinusoidal manner with an amplitude of 10◦ and a period of 50 s, i.e., *ψ*<sup>1</sup> = 10◦ sin( *<sup>π</sup><sup>t</sup>* <sup>25</sup> ), the pitch and yaw angle of joint 2 is set to 0◦, and the thrust of the propeller is set to *T* = 60 N. The simulation duration is 100 s. In the process of this movement, it can be seen from Figure 6b,d that the MJ-AUV is in roll motion. This is because when the yaw angle of the joint changes, the metacenter of the diversion cabin is not in the same vertical plane as the metacenter of the other cabins, so the roll moment is generated, and then the rolling phenomenon occurs.

Figure 7 shows the result of the spiral diving motion of the MJ-AUV. The pitching angle of joint 1 is set to *θ*<sup>1</sup> = 0◦, the yaw angle of joint 1 is set to *ψ*<sup>1</sup> = 5◦, the pitch angle of joint 2 is set to *θ*<sup>2</sup> = 10◦, the pitch angle of joint 2 is set to, the yaw angle of joint 2 is set to *ψ*<sup>2</sup> = 0◦, and the thrust of the propeller is set to *T* = 60 N. The simulation duration is 1000 s. Unfortunately, it can be found from the results in Figure 7b,d that the rolling attitude of

the vehicle is unstable, which is the same as that of the vehicle in the sinuous motion in Figure 7. It is all caused by the rolling moment that the metacenter of the diversion cabin is not in the same vertical plane as the metacenter of the other cabins. This brings a big problem for the multi-degree of freedom control.

**Figure 5.** Pitching motion of MJ-AUV: (**a**) Position changes over time; (**b**) Attitude changes over time; (**c**) Linear velocity changes over time; (**d**) Angular velocity changes over time; (**e**) 3D motion.

**Figure 6.** Yaw motion of the MJ-AUV: (**a**) Position changes over time; (**b**) Attitude changes over time; (**c**) Linear velocity changes over time; (**d**) Angular velocity changes over time; (**e**) 3D motion.

**Figure 7.** Spiral dive motion of the MJ-AUV: (**a**) Position changes over time; (**b**) Attitude changes over time; (**c**) Linear velocity changes over time; (**d**) Angular velocity changes over time; (**e**) 3D motion.

The above simulation results are consistent with the common physical phenomena, and to a certain extent, it can be considered that the established MJ-AUV 3D dynamic model is reasonable, without loss of generality and reliability. According to the simulation results, compared with the single rigid body AUV, the MJ-AUV has stronger flexible maneuvering characteristics and can perform more complex movements, such as large angle pitching movement, small radius steering movement, spiral ascending and diving movement, and so on, which satisfies the design intention.

#### *4.2. Model and Control Parameters*

The initial states of the MJ-AUV are *θ* = 0 rad, . *<sup>θ</sup>* <sup>=</sup> <sup>0</sup> rad, *<sup>θ</sup>*<sup>2</sup> <sup>=</sup> <sup>0</sup> rad, *<sup>ψ</sup>* <sup>=</sup> <sup>0</sup> rad, . *ψ* = 0 rad, and *ψ*<sup>1</sup> = 0 rad. In addition, the controller parameters are *ω*<sup>0</sup> = 30, *b*<sup>11</sup> = 0, *b*<sup>12</sup> = 0.1, *b*<sup>21</sup> = 0.1, *b*<sup>22</sup> = 0, *Q* = diag(500, 600, 500, 600), *R* = diag(0.1, 0.1), and diag represents the diagonal matrix. The control gain after LQR optimization is

$$\mathbf{K} = \begin{bmatrix} 0 & 0 & -70.7107 & 78.3672 \\ -70.7107 & 78.3672 & 0 & 0 \end{bmatrix}.$$

#### *4.3. Control Simulation*

Figures 8–12 show the simulation results of the pitch and yaw attitude control of the MJ-AUV, that is, when the pitch attitude is kept as 0 rad, the yaw attitude is adjusted according to the design signal. Figure 8 shows the pitch control results. The reason why the initial pitch attitude is not 0 rad is that the rest buoyancy of each cabin of MJ-AUV is positive, and the resultant moment of the body is not 0 rad. In other words, when the joint angle is 0 rad, the pitch attitude of the body is not 0 rad, so it is necessary to adjust the body attitude to 0 rad by adjusting the joint angle, which can also be seen from the performance of joint 2 in Figure 11. Figure 9 and the performance of joint 1 in Figure 11 show that, the yaw subsystem can be adjusted quickly after encountering multiple step signals, and the overshoot quantity is nearly 0 rad, with a very smooth transition process. Figure 10 shows that the maximum control error is within 0.005%, verifying that the designed controller has a high control accuracy. Figure 12 shows the observer's estimation results for the total disturbance of the pitching and yaw subsystem. The results show that the estimated value of the total disturbance can follow the real value in real time and accurately, further proving the effectiveness of LESO and making a great contribution to the control of the disturbance compensation.

**Figure 8.** The pitching attitude of the MJ-AUV.

**Figure 9.** The yaw attitude of the MJ-AUV.

**Figure 10.** The feedback error of the MJ-AUV.

**Figure 11.** The input of the MJ-AUV.

**Figure 12.** The estimation of the total disturbances of the MJ-AUV.

#### **5. Conclusions**

The main contributions of this paper are as follows:


**Author Contributions:** Conceptualization, Q.M. and H.Z.; methodology, L.Y.; software, L.Y.; validation, L.Y.; formal analysis, L.Y.; writing—original draft preparation, L.Y.; writing—review and editing, Q.M., H.Z. and L.Y. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was supported by National Key R&D Program of China under Grant No. 2017YFC0306200.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Data sharing not applicable.

**Acknowledgments:** The authors would like to express their appreciation to Liu Kexian for providing the hydrodynamic coefficient of the vehicle and assisting to complete the modeling work. At the same time, the authors are very grateful to Wang Runzhi for help in the controller design process.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **Appendix A**

**Proof of Theorem 1.** The Lyapunov function is defined as

$$\mathbf{V} = \mathbf{c}^{\mathrm{T}} \mathbf{P} \mathbf{c},\tag{A1}$$

where, *<sup>P</sup>* is the unique solution of the Lyapunov equation *<sup>H</sup>*T*<sup>P</sup>* <sup>+</sup> *PH* <sup>=</sup> <sup>−</sup>*Q*, *<sup>H</sup>* is the Hurwitz matrix, and *Q* is the positive definite matrix, then the derivative of Equation (A1) is obtained as follows:

$$\begin{split} \dot{\mathbf{V}} &= -\mathbf{c}^{\mathsf{T}} \mathbf{Q} \mathbf{c} + 2 \boldsymbol{\sigma}^{\mathsf{T}} \mathbf{P} \mathbf{c} \\ &= \left( \mathbf{c}^{\mathsf{T}} \mathbf{Q}^{\frac{1}{2}} - \boldsymbol{\sigma}^{\mathsf{T}} \mathbf{P} \mathbf{Q}^{-\frac{1}{2}} \right) \left( \mathbf{c}^{\mathsf{T}} \mathbf{Q}^{\frac{1}{2}} - \boldsymbol{\sigma}^{\mathsf{T}} \mathbf{P} \mathbf{Q}^{-\frac{1}{2}} \right)^{\mathsf{T}} + \left( \boldsymbol{\sigma}^{\mathsf{T}} \mathbf{P} \mathbf{Q}^{-\frac{1}{2}} \right) \left( \boldsymbol{\sigma}^{\mathsf{T}} \mathbf{P} \mathbf{Q}^{-\frac{1}{2}} \right)^{\mathsf{T}} \end{split} \tag{A2}$$

Let *. V* < 0, that

$$\left\|\boldsymbol{\varepsilon}^{\mathsf{T}}\boldsymbol{\mathsf{Q}}^{\frac{1}{2}} - \boldsymbol{\sigma}^{\mathsf{T}}\boldsymbol{\mathsf{P}}\boldsymbol{\mathsf{Q}}^{-\frac{1}{2}}\right\|\_{2} > \left\|\boldsymbol{\sigma}^{\mathsf{T}}\boldsymbol{\mathsf{P}}\boldsymbol{\mathsf{Q}}^{-\frac{1}{2}}\right\|\_{2}.\tag{A3}$$

Processing Equation (A3), can be obtained

$$\left\|\boldsymbol{\sigma}^{\mathsf{T}}\mathbf{Q}^{\frac{1}{2}}\right\|\_{2} > 2\left\|\boldsymbol{\sigma}^{\mathsf{T}}\mathbf{P}\mathbf{Q}^{-\frac{1}{2}}\right\|\_{2}.\tag{A4}$$

The choice of *P* depends on *Q*. In order to make the calculation simple, *Q* = *I* is selected as the identity matrix to meet the requirements of positive definite matrix. Then Equation (A4) is:

$$\|\|\mathbf{c}\|\|\_{2} > 2\|\|\mathbf{Po}\|\|\_{2}.\tag{A5}$$

Because *<sup>o</sup>* is bounded, when *<sup>c</sup>* satisfies Equation (A5), *. V* is negative definite, and because *V* is positive definite, *c* is bounded stable. -

#### **Appendix B**

**Table A1.** The Parameters of the MJ-AUV Model.


#### **References**

