Dynamic modeling and optimal control of a positive buoyancy diving autonomous vehicle UDC 629

The positive buoyancy diving autonomous vehicle combines the features of an Unmanned Surface Vessel (USV) and an Autonomous Underwater Vehicle (AUV) for marine measurement and monitoring. It can also be used to study reasonable and efficient positive buoyancy diving techniques for underwater robots. In order to study the optimization of low power consumption and high efficiency cruise motion of the positive buoyancy diving vehicle, its dynamic modeling has been established. The optimal cruising speed for low energy consumption of the positive buoyancy diving vehicle is determined by numerical simulation. The Linear Quadratic Regulator (LQR) controller is designed to optimize the dynamic error and the actuator energy consumption of the vehicle in order to achieve the optimal fixed depth tracking control of the positive buoyancy diving vehicle. The results demonstrate that the LQR controller has better performance than PID, and the system adjustment time of the LQR controller is reduced by approximately 56% relative to PID. The motion optimization control method proposed can improve the endurance of the positive buoyancy diving vehicle, and has a certain application value.


Introduction
The ocean is of great significance to the survival and development of mankind. In order to deepen the understanding of the ocean and better develop and protect the ocean, a large number of marine equipment are used for ocean observation and development. These widely used marine equipment mainly include marine survey vessels, Remotely Operated Vehicles (ROVs) [1,2], Autonomous Underwater Gliders (AUGs) [3], AUVs [4,5,6], USVs [7,8,9], etc. The positive buoyancy diving autonomous vehicle [10] is proposed, which combines the features of USV and AUV and better meets the needs of surface and underwater measurement In recent years, researchers have mainly conducted a lot of research on the dynamic modeling and motion control of USV, AUV, AUG, etc. Based on the six-degree-of-freedom USV kinematic model, Zeng [16] established a surface three-degree-of-freedom kinematic model of the USV in complex sea conditions. Tanakitkorn et al. [17] established the AUV dynamic model including the simplified and continuous hydrodynamic model of the auxiliary propeller on the basis of considering the hydrodynamic model of the Delphin 2 AUV main propeller and the rudder surface. Leonard N E et al. [18] carried out research on AUG dynamic modeling and established a simplified dynamic model for AUG based on Newton Euler equation. Tian X et al. [19] proposed a positive buoyancy underwater glider (without a variable buoyancy regulation system) that retains the propeller, wing, and internal moving mass. The preliminary dynamic analysis of its movement processes, such as diving and floating, is carried out.
Manley J E et al. [20] used a proportional differential (PD) controller to achieve the heading control of the ARTEMIS unmanned surface craft. Li J et al. [21] proposed a robust adaptive neural network control (RANNC) scheme to achieve the dynamic positioning (DP) of marine vessels with a prescribed performance under input saturation, external disturbances, and model uncertainties. Lamraoui H C et al. [22] applied Active Disturbance Rejection Control (ADRC) to the path tracking control of the fully driven AUV. The simulation results show that the controller can track the desired trajectory with high accuracy in the horizontal plane and space, and has strong robustness to both internal and external disturbances of the system. Maurya P et al. [23] used the LQR controller to control the depth and heading of the small autonomous underwater vehicle "Maya". Through simulation tests, the control of the 21 controller in the vertical and horizontal planes met the performance and stability requirements. Herlambang T et al. [24] designed a cuckoo search optimization LQR controller and used it for tracking control of AUV heave position, yaw angle, etc. Shan Y et al. [25] proposed a model predictive control method based on recurrent neural networks for the motion control of underwater gliders in the vertical plane. The simulation results show that the proposed control method is efficient and effective. Joo M G et al. [26] proposed an LQR controller to control the center of gravity and net buoyancy mass in a hybrid autonomous underwater vehicle (HAUV), enabling it to perform sawtooth motion functions.
According to the existing literature, although the research on the motion control of USV, AUV, etc. is becoming more mature, there is less research on the dynamic modeling and optimal control of the positive buoyancy diving autonomous vehicle with relatively large positive buoyancy and relying on the negative lift generated by the fuselage and wings to dive.
Due to its large positive buoyancy, fixed wing installation angle, and underactuated characteristics, the positive buoyancy diving autonomous vehicle is able to sail at a fixed depth near the cruising speed and maintain a zero inclination of the body under ideal conditions. However, the net buoyancy of the positive buoyancy diving vehicle will change in specific situations, and the vehicle needs to keep fixed depth sailing at a specific speed with the body maintaining a small inclination. When the speed of the positive buoyancy diving vehicle is larger, the drag is larger, and the unit energy consumption is also larger. Whereas, when the net buoyancy of the positive buoyancy diving vehicle is certain, the angle of attack required to maintain a fixed depth sailing will be smaller when its speed is larger. When the angle of attack of the positive buoyancy diving vehicle is smaller when sailing at a fixed depth, the drag is smaller and its unit energy consumption is smaller. In practice, due to the positive buoyancy diving vehicle is in the observation and detection (cruise at fixed depth) stage most of the time when working underwater. Therefore, it is very necessary to study the energy consumption model of the positive buoyancy diving vehicle in the fixed depth cruise phase, determine the optimal cruise speed of the vehicle under different net buoyancy, and carry out research on the optimization control of the positive buoyancy diving vehicle based on the LQR controller.
The main contributions of this paper are to establish the dynamics model of the positive buoyancy diving vehicle, determine its optimal cruising speed for low energy consumption by simulation, and achieve its optimal fixed depth tracking control by using the LQR controller. The simulation results demonstrate that the LQR controller has better performance than PID, especially in overshoot. Thanks to these methods, the positive buoyancy diving vehicle can improve its endurance, and these methods also have a certain application value.
The rest of the paper is organized as follows: Section 1 describes the vehicle's conceptual design, and details the model of the vehicle. Section 2 introduces the energy consumption analysis of fixed depth sailing. Section 3 shows the vehicle LQR Controller Design. Then, Section 4 provides the vehicle Simulation analysis. Finally, Section 5 presents the conclusions of the paper.

Vehicle Model
The positive buoyancy diving autonomous vehicle has a relatively large positive buoyancy. As shown in Fig. 1, it mainly uses propeller propulsion, an inverted V-shaped rudder to adjust its attitude, and uses the negative lift generated by its fuselage and wings to offset its net buoyancy to achieve underwater movement. Therefore, the positive buoyancy diving vehicle does not need to carry a complex, expensive and heavy variable buoyancy adjustment device, and is more reliable and economical, and the space saved can carry more Zhiguang Wang, Zhaoyu Wei, Caoyang Yu, Junjun Cao, Dynamic modeling and optimal control of a Baoheng Yao, Lian Lian positive buoyancy diving autonomous vehicle 22 energy or payload. It can achieve three-dimensional observation of the target sea area and is a type of ocean observation and ocean resource exploration test platform, which is of great significance to the study of the ocean. Technical specification of the positive buoyancy diving vehicle is shown in Table 2. The establishment of the motion model of the positive buoyancy diving vehicle can lay a foundation for the motion analysis and motion controller design of the vehicle. It can also accurately and unambiguously describe the motion of the positive buoyancy diving vehicle in space, and establish a unified reference system. The reference systems used in this section include three classical spatial right-angle coordinate systems (e.g., Fig. 2), namely, the inertial coordinate system, the body coordinate system, and the velocity coordinate system. The inertial coordinate system (  E ijk ) adopts the North-East-Down coordinate system (NED). Its three axes Ei , Ej , Ej respectively represent the orientation of the north, east and earth axes, and, i , j , k respectively represent the unit vectors corresponding to the three axes of the inertial coordinate system; The body frame (  O XYZ ) is defined as the origin coinciding with the buoyant center of the vehicle, the X axis points to the front of the body, Y points to the right side, and Z points to the bottom, and the three axes are orthogonal to each other. The unit vectors of the three main axes of the body coordinate system are denoted by 1 e , 2 e and 3 e respectively. The velocity coordinate system The kinematic Eq. (3) of the positive buoyancy diving vehicle can be obtained from Eqs. (1) and (2).
From Eq. (2), it can be seen that when  approaching 90  , i.e., the pitch angle of the positive buoyancy diving vehicle approaches 90 , the matrix is a singular matrix. However, there is almost no pitch angle approach 90 during the motion of the positive buoyancy diving vehicle, so this problem can be ignored.
Assuming that the positive buoyancy diving vehicle is a rigid body and its mass b m is constant, its momentum and momentum moment equations in the body coordinate system can be obtained according to reference [27] as: is the coordinate of the center of gravity of the positive buoyancy diving vehicle in the body coordinate system; I is the rotational inertia matrix of the vehicle with respect to the origin of the body coordinate system. f and  are the resultant external force and external moment on the positive buoyancy diving vehicle, respectively, and the force analysis of the vehicle shows that the external force and external moment contain the following components, respectively.
The gravitational force of the positive buoyancy diving vehicle is G f , the buoyancy force is B f , the inertial class hydrodynamic force is AM f , the viscous class hydrodynamic force is V f , the thrust of the thruster is T f , the control force generated by the maneuvering surface is  are the gravitational moment, buoyancy moment, inertial hydrodynamic moment, viscous hydrodynamic moment, thruster thrust moment, and control moment generated by the maneuvering surface of the positive buoyancy diving vehicle, respectively.

Energy Consumption Analysis of Fixed Depth Sailing
The energy consumption of the fixed depth cruise phase of the positive buoyancy diving vehicle is mainly composed of two parts: The energy consumption of carrying equipment includes controller, sensor, etc., and the energy consumption of driving equipment, mainly Dynamic modeling and optimal control of a Zhiguang Wang, Zhaoyu Wei, Caoyang Yu, positive buoyancy diving autonomous vehicle Junjun Cao, Baoheng Yao, Lian Lian 25 includes ducted thruster, electronic speed control, etc. [28] The total energy consumption of the system is shown in Eq. (8).
where D P is the power of carrying equipment, T P is the power of driving equipment, t is the sailing time, S is the range, V is the speed. From Eq. (8), it can be seen that the energy consumption per unit time of the positive buoyancy diving vehicle is related to the power of the carrying equipment and the driving equipment, and the power of the carrying equipment can be approximated as constant, then the energy consumption per unit time of the positive buoyancy diving vehicle is only related to the power of the driving equipment, and the power of the driving equipment can be expressed by Eq. (9).
where T is the thrust of the catheter thruster,  is the propulsion efficiency of the catheter thruster.
In fact, by analyzing the control process of the fixed depth cruise of the positive buoyancy diving vehicle, it is possible to find the relationship between the motion parameters (angle of attack, cruise speed, etc.) and the energy consumption of the drive equipment of the vehicle. In the process of controlling the fixed depth cruise of the positive buoyancy diving vehicle, the thrust of the vehicle is given first, then the elevator deflection angle is controlled, and then the attitude angle is controlled so that the fixed depth cruise control of the vehicle can be realized. When the net buoyancy of the positive buoyancy diving vehicle increases, its body is required to maintain a small inclination angle and fixed depth cruise at a constant speed. The detailed force analysis of the positive buoyancy diving vehicle is shown in Fig. 3. When the elevator deflection angle is e  , the thrust T , buoyancy B , drag xV F , lift zV F , and gravity G are balanced in the X axis and Z axis directions.  C and  B are the corresponding balanced moments. In addition, when the positive buoyancy diving vehicle is cruising at a fixed depth, its angle of attack  is equal to its pitch angle  .
In Eq. (12), the scalar s N B represents the size of the net buoyancy N B .
The drag xV F and lift zV F of the positive buoyancy diving vehicle are related to speed V and angle of attack  (equal to pitch angle  ), and can be expressed as: In Eqs. (13) and (14), the scalar s V represents the size of the speed V , and Combining Eqs. (8)- (14), the total energy consumption of the positive buoyancy diving vehicle system is related to the motion parameters and control variables. Therefore, the total energy consumption function of the system during the fixed depth cruise phase of the positive buoyancy diving vehicle is shown in Eq. (15). are respectively the minimum net buoyancy and maximum net buoyancy of the vehicle. The net buoyancy of a positive buoyancy diving vehicle is set to be adjustable mainly because the net buoyancy will change due to future carrying scientific sensors and due to changes in the ocean environment when the vehicle is upgraded in the future for large depth cruises in the ocean. min s T and max s T represent the minimum thrust (less than this thrust can't keep the vehicle carrying out the normally fixed depth cruise) and maximum thrust (limited by the actual performance of the ducted thruster) of the positive buoyancy diving vehicle, respectively.
Since the problem described in (16) is a complex nonlinear problem, it is difficult to obtain the optimal thrust, pitch attitude, etc. of the positive buoyancy diving vehicle by Dynamic modeling and optimal control of a Zhiguang Wang, Zhaoyu Wei, Caoyang Yu, positive buoyancy diving autonomous vehicle Junjun Cao, Baoheng Yao, Lian Lian 27 analytical methods. Therefore, the pitch angle  and velocity V of the vehicle at fixed depth cruise are determined by giving the thrust at different net buoyancy, and then calculating the energy consumption of the vehicle according to (8), which is designed as follows.  buoyancy mass when sailing at a fixed depth. The specific relationship between thrust and longitudinal speed is shown in Table 3.  The absolute value of the pitch angle changes with the thrust when the positive buoyancy diving vehicle is sailing at a fixed depth, as shown in Fig. 5. If the net buoyancy of the positive buoyancy diving vehicle is constant, the greater the thrust of the ducted thruster, the smaller the absolute value of the attitude angle during the fixed depth sailing; If the thrust is constant, the greater the net buoyancy of the vehicle, the larger the absolute value of the attitude angle during the fixed depth sailing. In addition, it can be seen from Fig. 4 and Table 3 that there is a one-to-one correspondence between the longitudinal velocity of the positive buoyancy diving vehicle and the thrust of the ducted thruster during the fixed depth sailing. Therefore, when the positive buoyancy diving vehicle Dynamic modeling and optimal control of a Zhiguang Wang, Zhaoyu Wei, Caoyang Yu, positive buoyancy diving autonomous vehicle Junjun Cao, Baoheng Yao, Lian Lian 29 is sailing at a fixed depth, the change of the absolute value of the pitch angle with the longitudinal speed is consistent with the change in the thrust, as shown in Fig. 6.  8) and (9), the energy consumption of the vehicle during the fixed depth cruise can be calculated. The law of energy consumption related to thrust and range during the fixed depth sailing of the positive buoyancy diving vehicle is shown in Fig. 7. The energy consumption of the positive buoyancy diving vehicle increases with the increase of the range. In addition, when the range of the positive buoyancy diving vehicle is 500-6000 meters and the range is determined, the energy consumption of the vehicle is gradually increasing with the increasing thrust of the ducted thruster. Therefore, when the thrust of the positive buoyancy diving vehicle can meet the requirements of fixed depth sailing control, selecting a smaller thrust can reduce the energy consumption of the vehicle. Specifically, due to the limitation of the rudder efficiency and its relatively large positive buoyancy, the longitudinal velocity (thrust) of the vehicle cannot be very small. The optimized longitudinal velocity has been determined through simulation, which can not only meet the requirements of reducing the energy of the vehicle but also meet its basic motion control requirements.

Block Diagram of the LQR Controller
LQR controller is a widely used linear quadratic controller. Its control object is a linear system, and its performance index function is a quadratic function of object state variables and control variables. In section 2, through the analysis of the energy consumption model of the positive buoyancy diving vehicle in the fixed depth cruise, we can get the thrust value that meets the fixed depth cruise requirements of the vehicle and minimizes its energy consumption. And at this thrust value, it is possible to make the positive buoyancy diving vehicle sail at optimal speed (minimum energy consumption) at a fixed depth. The LQR controller can not only optimize the depth control performance of the positive buoyancy diving vehicle but also consider the energy consumption generated by the actuator control input of the vehicle, thus achieving a comprehensive optimization of the energy consumption of the vehicle.
The control block diagram of the fixed depth sailing of the positive buoyancy diving vehicle based on the LQR controller is shown in Fig. 8. The design of the LQR controller for the fixed depth sailing of the positive buoyancy diving vehicle is carried out according to the following steps: 1) The dynamic model of the positive buoyancy diving vehicle is linearized, and the state space expression is used to express the system; 2) Determining whether the system is controllable and observable; 3) Optimization of the weighting matrices Q and R of the LQR controller; 4) Calculation of the optimal control input for the LQR controller; 5) Debug the LQR controller of the positive buoyancy diving vehicle for fixed depth sailing. Firstly, the dynamics model of a positive buoyancy diving vehicle fixed depth sailing in still water is linearized according to the definition in section 3 [29]. The state vector defining the control equation for the positive buoyancy diving vehicle is Since the system is a linear constant system, the system is judged to be controllable and observable according to the system controllability discriminant matrix and the observability discriminant matrix, respectively [30].
As shown in Eqs. (19) and (20), both the controllability discrimination matrix and the observable discrimination matrix of this system are full rank, so the system is completely controllable and observable.
Let the expected output vector of the system be zd y , Then the error vector for the positive buoyancy diving vehicle fixed depth sailing is (21).
where, z P is a positive definite matrix and satisfies the Riccati matrix differential equation as follows: The schematic diagram of the LQR controller for the positive buoyancy diving vehicle fixed depth sailing in the vertical plane is shown in Fig. 9. The design of the LQR controller for fixed depth sailing in the vertical plane of the positive buoyancy diving vehicle is a standard linear optimal control design method. By optimizing the quadratic objective function composed of state variables and input variables, the LQR controller parameters z P and z g , which minimize the objective function, are obtained to achieve the goal of fixed depth sailing control of the positive buoyancy diving vehicle. The values of z P and z g are determined by the weighting matrices Q and R, so the optimization of Q and R is particularly critical to the design of the LQR controller for the fixed depth sailing in the vertical plane of the positive buoyancy diving vehicle.
Currently, the optimization methods for the Q and R parameters of the LQR controller are genetic algorithm optimization, adaptive particle swarm optimization [32], trial-and-error method, etc. The so-called trial-and-error method is to determine different Q and R according to the physical limitations of the system and engineering experience, then calculate them separately, and then optimize the selection of Q and R according to the control effect of the LQR controller. The physical limitation of the system usually means that the output size and speed of the system actuator are limited; Q is called the state weighting matrix, which takes values that weigh the relative importance of each state in the system state vector. The low error state of the system state quantity is realized by changing the corresponding Q value; R is called the input weighting matrix and the value of R is able to determine the input state of the system actuator; When the value of Q is large and the value of R is small, the system state quantity can converge quickly, but the control quantity of the actuator needs to be large (large energy consumption); When Q is smaller and R is larger, the state quantity of the system converges slowly and requires less control quantity of the actuator.
The principle for selecting Q and R in the LQR controller for fixed depth sailing of the positive buoyancy diving vehicle in the vertical plane is as follows: 1) Q is a non-negative definite matrix and R is a positive definite matrix; 2) Q and R are generally diagonal matrices; 3) When there is only one control input, R is a scalar. 4) Consider the steady-state performance, and dynamic performance of the LQR controller of the positive buoyancy diving vehicle, and give primary consideration to making the system actuator output smaller and reducing the energy consumption of the actuator. Under the premise of satisfying the above principles, the values of Q and R in the LQR controller are optimized by the trial-and-error method according to the physical limitations of the positive buoyancy diving vehicle and the engineering experience, so that the LQR Dynamic modeling and optimal control of a Baoheng Yao, Lian Lian positive buoyancy diving autonomous vehicle 34 controller for fixed depth sailing of the vehicle in the vertical plane can achieve satisfactory control effects. The weighting matrices Q and R of fixed depth sailing LQR controller of the positive buoyancy diving vehicle in the vertical plane are selected as shown in Table 4. According to the control results of the fixed depth sailing of the positive buoyancy diving vehicle corresponding to these weighting matrices Q and R, the optimal Q and R values are then optimally determined.

Simulation Analysis
The positive buoyancy diving vehicle LQR controller is able to control the dive depth of the vehicle in the vertical plane to track the desired dive depth and make the set performance index extremely small, thus achieving the optimal fixed depth tracking control of the positive buoyancy diving vehicle. Following the steps described above for the design of a fixed depth sailing LQR controller for the positive buoyancy diving vehicle, modeling is performed in Simulink of MATLAB, and the optimal fixed depth tracking control simulation for the vehicle is performed. The block diagram of the LQR optimal tracking control for the positive buoyancy diving vehicle is shown in Fig. 10. The LQR controller module of Simulink in Fig. 10 is based on the S-function implementation of the M-file, in which the solution of the differential equation of the Riccati matrix in Eq. (24) is mainly done to obtain the value of z P . The value of z g is then derived from Eq. (25). The value of z u is then derived from the Eq. (23) as the control input for fixed depth sailing of the positive buoyancy diving vehicle. Finally, the optimal fixed depth tracking control simulation of the vehicle is performed according to the values of Q and R in Table 4, respectively. As shown in Fig. 11, the positive buoyancy diving vehicle is dived to a fixed depth of 10 meters sailing under different values of Q and R. In Fig. 11, it is shown that the positive buoyancy diving vehicle does not depth overshoot during fixed depth sailing and the control output of the vehicle actuator is smaller and consumes less energy for smaller Q value and larger R value, but takes longer to reach the desired depth; While the positive buoyancy diving vehicle at a larger Q value and smaller R value, it is able to reach the desired depth in a shorter period of time, but depth overshoot will occur and the control output of the vehicle actuator will be larger. We are more concerned with the energy consumption of the positive buoyancy diving vehicle during fixed depth sailing, so we have to ensure that the output of the actuator of this vehicle is within a reasonable range, and then finally the values of Q and R are optimized. The control inputs of the positive buoyancy diving vehicle diving to a fixed depth underwater of 10 meters sailing for different values of Q and R are shown in Fig. 12. The variation in the control input of the positive buoyancy diving vehicle can be derived from Fig. 12, and the energy consumption of its actuators can be preliminarily evaluated by the variation in the absolute value of the control input amplitude.
In order to make the control accuracy and actuator energy consumption of the positive buoyancy diving vehicle during fixed depth sailing meet the requirements, the Q and R values of the LQR controller are finally determined, as shown in (26).

5.2;
3.0  QR (26) LQR controller and PID controller are used to control the fixed depth sailing of the positive buoyancy diving vehicle, and their control performances are compared through simulation. Where the parameters of the PID controller are designed as Kp =0.78, Ki =0.05, Kd =1.85, and limit the output of PID controller to -15°~15°.  It takes about 90 seconds for a positive buoyancy diving vehicle to reach a fixed depth sailing under the control of a PID controller, while the LQR controller takes only about 40 seconds to bring the vehicle to a fixed depth sailing. The system adjustment time of the LQR controller is reduced by approximately 56% relative to PID. The control input of the actuator (rudder) can also be controlled within 13°. In addition, the positive buoyancy diving vehicle is able to achieve zero overshoot under the control of the LQR controller, while the PID controller produces a larger overshoot of up to approximately 5.3 meters. Therefore, the LQR controller is able to achieve the optimal fixed depth tracking control of a positive buoyancy submersible vehicle.

Conclusion
In this paper, the dynamic model of the positive buoyancy diving vehicle is established, which lays the foundation for the optimal control of the vehicle. Then the energy consumption model of the positive buoyancy diving vehicle is analyzed, and the energy consumption of the vehicle in different ranges is calculated by simulation. It is determined that when the thrust of the positive buoyancy diving vehicle can meet the requirements of fixed depth sailing control, selecting a smaller thrust can reduce the energy consumption of the vehicle. Thus, the problem of determining the optimal thrust of the positive buoyancy diving vehicle is solved. Finally, on the basis of determining the optimal thrust (optimal cruise speed) of the energy consumption of the positive buoyancy diving vehicle, the LQR controller is used for the optimal fixed depth tracking control of the vehicle. Firstly, the basic theory and design method of the LQR controller for the fixed depth sailing of the positive buoyancy diving vehicle is analyzed, and the LQR controller is modeled in Simulink of MATLAB. Then, according to the trial-and-error method, the weighted matrices Q and R that meet the requirements of the control accuracy and actuator energy consumption of the positive buoyancy diving vehicle during the fixed depth sailing are found, and the control simulation of the vehicle is carried out under the optimized Q and R values. The performance of the LQR controller is compared with that of the PID controller through simulation, and the results Zhiguang Wang, Zhaoyu Wei, Caoyang Yu, Junjun Cao, Dynamic modeling and optimal control of a Baoheng Yao, Lian Lian positive buoyancy diving autonomous vehicle 38 show that the LQR controller has better performance. The system adjustment time of the LQR controller is reduced by approximately 56% relative to PID. In summary, the depth control of the positive buoyancy diving vehicle at the optimal energy consumption cruise speed is realized, and the dynamic error and actuator energy consumption of the vehicle in the fixed depth tracking control process is comprehensively optimized. The research on the optimal control of the motion of the vehicle can improve endurance, and the proposed motion optimization control methods have a certain application value.