A Unified Approach for Underwater Homing and Docking of over-Actuated AUV

During the implementation of time-consuming tasks such as underwater observation or detection, AUV has to face a difficult and urgent problem that its working duration is greatly shortened by the limited energy stored in the battery device. To solve the power problem, a docking station is installed underwater for AUV charging its battery. However, to realize the automatic underwater charging of AUV via a docking station, the accurate and efficient completion of underwater homing and docking is required for AUV. Underwater automatic homing and docking system is of great significance to improve work efficiency and prolong the endurance of AUV save cost. In this paper, a unified approach that involves such as task planning, guidance and control design, thrust allocation has been proposed to provide a complete solution to the problem of homing and docking of an over-actuated AUV. The task-based hybrid target point/line planning and following strategy are proposed for AUV homing and docking. At the beginning of homing, AUV is planned to follow a straight line via the line of sight (LoS) method. Afterward, AUV starts to follow multiple predefined target points until reaching the docking station. At the final stage of docking (within 10 m), a dedicated computer vision algorithm is applied to detect a newly designed LED light array fixed on the docking station to provide accurate guidance for the AUV to dock. The sliding mode control technique is used for the motion control of the AUV allowing robustness. As the AUV configured with eight thrusters is over-actuated, the problem of the thrust allocation is very important and successfully solved using the quadratic programming (QP) optimization method. Finally, the simulations of homing and docking tasks using the AUV are accomplished to verify the proposed approach.


Introduction
Intelligent Underwater Unmanned Vehicles such as Autonomous Underwater Vehicles (AUVs) have a wide range of applications, which can realize marine environment monitoring, seabed topography survey, underwater resource exploration, marine resource sampling, etc. However, the drawback that the duration of AUV's underwater activities is greatly limited by its own energy stands out during the implementation of time-consuming tasks such as underwater observation. Therefore, to realize an underwater automatic docking system for the AUV is of great significance to improve work efficiency, save manpower and cost. After completing a certain task, it needs to search for the docking station (DS) deployed undersea, gradually navigate to it which are called homing, and finally get clamped by it which is called docking. Once docking is completed successfully, AUV can start the process of charging energy, exchanging data, downloading new tasks, and so on. At last, the fully charged AUV then begins a new mission that has just been uploaded to it.
Recently, the topics on AUV homing and docking have drawn broad attention from researchers. Many researchers only focused on one single aspect which may be important to realize the autonomous homing and docking process such as hydrodynamic modeling [1][2][3][4], planning, decision and following strategy, controller design, etc. Xiang et al. [5] applied the maximum membership and threshold principles into the intelligent decision-making process which guides AUV to take critical operations and ensure safety. Xiong et al. [6] present elite group-based evolutionary algorithms (EGEA) for adaptive ocean sampling using multiple unmanned marine vehicles (UMVs). Peng et al. [7] investigated the distributed time-varying formation control for a fleet of under-actuated autonomous surface vehicles subject to unknown input gains based on a consensus approach, a path-following design, artificial potential functions, and an auxiliary variable approach. Xu and Guedes Soares et al. [8] presented a 2D path following control system for autonomous surface vehicles through a vector field-based way-point guidance scheme. Qin et al. [9] proposed a trajectory tracking control strategy for solving the saturation and full-state constraints problem of the unmanned surface vessels based on the anti-windup compensator and the barrier Lyapunov function. Xu et al. [10] adopted an L1 adaptive backstepping controller where the control law is derived using the Lyapunov control function to realize the path-following control of an underactuated ship. According to the flight characteristics of the parafoil, Tao et al. [11] designed a multiphase homing path. Based on the active disturbance rejection control (ADRC), a homing controller is designed to track the horizontal and vertical trajectory. Simulations show that the planned trajectory can successfully accomplish the target of fixed point homing and flare landing. The ADRC can track the homing path more rapidly, steadily, and get better control performances than the PID controller. Martinsen et al. [12] proposed an all-encompassing procedure method for performing both docking, maneuvering, dynamic positioning, and control allocation of marine vessels using numerical optimal control. The method is found capable of being implemented as a real-time MPC-based algorithm on a supply vessel. Li et al. [13] proposed a robust adaptive neural network control for the dynamic positioning of marine vessels with prescribed performance under model uncertainties and input saturation. Anderlini et al. [14] realized the control for the docking of an AUV onto a fixed station via reinforcement learning strategies. Two reinforcement learning schemes: DDPG and DQN were investigated and compared with optimal control techniques. The authors found that reinforcement learning achieves a performance similar to optimal control at a much lower computational cost at deployment, whilst also presenting a more general framework. Zhang et al. [15] proposed a virtual submerged floating operational system (VSFOS) based on parallel and serial robotic platforms. The data collected by the inertial sensor is received by the designed control system architecture, software to communicate and send instructions. Uchihori et al. [16] developed a control system for driving an AUV performing docking operations in presence of tidal current disturbances is proposed. The Linear Parameter-Varying (LPV) model was used for a Model Predictive Control (MPC) design for computing the set of forces and moments driving the nonlinear vehicle model. The LPV-MPC control action is mapped into the reference signals for the actuators by using a Thrust Allocation (TA) algorithm. The structural decomposition of MPC and TA reduces the computational burden involved in computing the control law online on an embedded control board. The proposed control system has been tested and validated in the range of control scenarios with various tidal current disturbances. Wang et al. [17] adopted a fuzzy adaptive linear active disturbance rejection control (LADRC) for precise control of the underwater glider's trajectory. Besides AUV, the control design of the autonomous surface vehicle (ASV) and other operational mechanical systems is also investigated. Bitar et al. [18] studied automatic docking of a small autonomous surface vehicle (ASV) in confined waters in Trondheim, Norway by interconnecting an optimization-based trajectory planner which provides collision-free trajectories facing static obstacles and a dynamic positioning (DP) controller which can track the planned time-parametrized position, velocity, and acceleration. Wang et al. [19] managed to carry out cloud-based mission control of the USV fleet.
Peng et al. [20,21] discussed the recent development of trajectory-guided, path-guided, and target-guided coordinated control of multiple ASVs in detail. Roman et al. [22] combined model-free adaptive control with the fuzzy component by virtual reference feedback tuning. The new proposed algorithm is validated using experimental results to the arm angular position of the nonlinear tower crane system. Turnip and Panggabean [23] developed a combination of skyhook and ground hook control-based magnetorheological lookup table technique called hybrid control for a quarter car. The simulation of the semi-active suspension design indicates that the proposed Hybrid control lookup table provides better vibration isolation capability than the skyhook controller and hybrid conventional methods. Precup et al. [24] propose a set of evolving Takagi-Sugeno-Kang (TSK) fuzzy models of the nonlinear dynamic mechanisms occurring in the myoelectric-based control of prosthetic hand fingers. By comparing with the experimental data and two recurrent neural network architectures, the proposed controlling method is found reliable and convincing.
When it comes to navigation or guidance design, the navigation aids applied for the AUV docking mainly include acoustic, optical, and electromagnetic ways, etc. The AUV guidance systems are classified into three methods: point-point method, graph search method, and optimization-based method [25]. Anderson et al. [26] simulated the docking process of the Martain AUV using a USBL system and a verified dynamic model. Sans-Muntadas et al. [27] applied an array of underwater navigation aids: ultra-short baseline (USBL), Doppler velocity log (DVL), etc., for the AUV homing and docking tasks. Meanwhile, a modular and cascaded Kalman filter (KF) approach which can estimate the navigation covariance and judge the situation of the DS is used to predict the consequence of the docking procedure. The proposed method can improve the autonomy level of the AUV and adjust measures as required. Vandavasi et al. [28] applied the bio-inspired differential magnetometry-based electromagnetic homing guidance system (EMHGS) into the docking process of a prototype twin thruster AUV operable in two degrees of freedom (DOF) with an underwater DS. By sensing a magnetic field strength, the EMHGS is found to affect AUV orientation correction by measuring the bearing angle. The authors found that higher dock magnetic field strengths could increase the guidance distance via the validated finite element model. Yahya et al. [29] develop a computer vision-based tracking system for AUV to dock underwater by recognizing the light sources placed on the DS. What is more, this study also found that the successful recognition decreases with the camera approaching the target faster. Therefore, a slow and stable movement of the AUV is necessary to complete a successful docking operation. To accomplish the target of autonomous docking of an industry-standard work-class ROV to both static and dynamic DS TMS (Tether Management System-TMS), Trslic et al. [30] present a machine vision-based docking system developed around subsea camera pose estimation. The relative position between the ROV and the DS can be estimated using a single camera and a known light marker pattern. The vision-based docking system has been tested in a real-world environment in the North Atlantic Ocean and showed comparative capability with a commercial stateof-the-art underwater navigation system. Li et al. [31] provided reliable underwater navigation and vision positioning methods using two cameras for AUV docking. Four green LED lights are fixed around the DS and two cameras are installed in AUV's head part. Vallicrosa et al. [32] proposed a method for homing and docking an AUV to a subsea DS by combining acoustic and optical sensing. Within acoustic ranging distance to the DS, AUV can estimate the DS location using a Sum of Gaussian (SoG) filter. Once the DS position becomes known, AUV gradually approaches it till reaching within visual reach of the DS. At this time, visual information obtained from a light beacon navigation system is used to update to a Simultaneous Localization And Mapping (SLAM) filter providing an AUV-pose estimate with the required accuracy. Yang et al. [33] applied a pursuit guidance algorithm with current compensation into USBL and optical sensing-based navigating and docking hybrid underwater glider (HUG) into a rotatable DS. The comparison of the performance of the guidance algorithm with other existing guidance algorithms, such as pure pursuit guidance and proportional navigation guidance by simulations and experiment validate the feasibility of the docking system and the effectiveness of the proposed guidance algorithm. Breivik et al. [34] proposed an underway docking procedure that includes two phases to help realize the safe docking of a small unmanned surface vehicle (USV) with a larger mother ship moving in transit at sea. A safety circle together with a virtual target point that can move on the circle toward the assigned docking point is defined around the mother ship for the kinematic-controlled USV to approach. Recently, Wang et al. [35] wrote a review on deep learning techniques for marine object recognition. However, only a few researchers like Park et al. [36], Li et al. [37], Palomeras et al. [38], Matsuda et al. [39], Wang et al. [40], Ferreira et al. [41], Thomas et al. [42], and Page et al. [43] proposed complete solutions or approaches which may involve almost all the technical aspects to deal with the whole underwater homing and docking problem.
In this paper, the main contribution of this paper is the new unified approach which involves task planning, guidance and control design, and thrust allocation, for example, which proposed to provide a complete solution to the problem of homing and docking of an over-actuated AUV. Task planning for AUV is introduced in the homing and docking strategy. The path is mainly planned via a predefined target point or straight line to be followed by AUV via the line of sight (LoS) method. However, at the final stage of docking (within 10 m), a higher temporal and spatial accuracy is required for efficient docking, thus, considering the relative inaccuracy of USBL, vision-based guidance is used for the docking process and providing guidance for the AUV to dock. The sliding mode control technique is applied to the motion control of the AUV allowing robustness. Since the AUV is an over-actuated system, the problem of the thrust allocation is successfully solved using the QP optimization method. When dealing with thrust allocation for 8 thrusters, the proposed quadratic programming optimization technique has the advantages of considering power consumption and deviation at the same time, much smaller deviation under saturation compared with the conventional pseudo inverse allocation method. Finally, the simulations of the whole homing and docking tasks using the AUV are accomplished to verify the proposed approach.

Problem Formulation
In the present study, AUV is an over-actuated one that can implement specific underwater tasks. Therefore, in addition to the conventional inertial motion unit (IMU), DVL, camera, and other sensing devices, AUV is also equipped with an acoustic positioning device (USBL) to determine the relative position with the underwater DS, as well as radio frequency communication device to ensure the reliable communication between AUV and DS at different distances. The DS is a bottom-mounted platform, which is equipped with many important devices such as corresponding acoustic positioning and communication devices, multiple preset light sources and cameras, AUV locking device, underwater charging, and data exchange equipment for docking.
To solve the limitations of the onboard energy storage and increase the long-endurance operational capability of this AUV, a means of enabling persistence to realize an underwater automatic homing and docking system is the critical solution. The typical homing and docking process of the AUV are shown in Figure 1a. The homing process starts once AUV finishes a task and needs to search and move towards the DS from a very far position like 1000 m away from the DS. With the AUV entering a small distance like 10 m to DS, AUV comes to the docking stage. The AUV starts to adjust the pose with reference to the DS and finally gets clamped on it. The proposed homing and docking approach concentrates on the AUV moving plan and associated technical issues including task planning, navigation, and guidance scheme, motion modeling, and control of the underwater AUV as shown in Figure 1b.

Description of Task Planning
During homing and docking process, the distance between AUV and DS is gradually reduced from far-field to nearby and to zero namely absolute docking into the DS finally. When AUV is located at different distances, the sensing devices used for communication by AUV are changed according to the required accuracy and the range of use of the sensing devices. Therefore, different task planning should be designed for homing and docking stages.

Description of Guidance Design
The homing process starts at a far-field point where only USBL will work. USBL guidance is used for homing. When AUV is reaching within a small distance to the DS, a higher temporal and spatial accuracy is required for the final docking. Thus, vision-based guidance is used for the docking process.

Description of Controller Design
To study the underwater motion of an AUV, a 4-DOF dynamic model is supposed to be established via the thorough analysis of forces acting on the AUV and the propulsion performance of the propeller. Then, based on the dynamic model of the AUV, the design of a sliding mode controller for AUV motion and thrust allocation controller for AUV propulsion are required.

Task Planning
To complete the AUV homing and docking process, reasonable task planning is required before AUV moving. The whole process is divided into homing and docking processes. During each process, the path for AUV to follow including target waypoints and target lines needs to be predefined.

Homing Task
Generally, AUV switching from normal work such as underwater observations to homing and docking tasks may happen at a far-field point from the dock station, so it is necessary to navigate to the preset DS area first. Even if the precise location of the DS is known, AUV cannot directly complete docking with the DS due to the accumulated positioning error of AUV for a long time. Only after the USBL of AUV and the USBL installed on the DS have established mutual communications, AUV starts to calculate and know the relative location of the DS, and then it can move closer towards DS.
As shown in Figure 1a, after AUV gets the position and orientation of the DS, first, a target point A which has the same height as AUV is planned near the DS. Taking the current position and attitude of AUV as the initial conditions, a target line is planned going through the current position and the planned point A. After AUV tracks to the target point A with the range of about 200 m away from the DS following the planned target line, another target point B is planned to be located 10 m directly above the DS using USBL. To move from target point A to B, AUV switches from target line tracking to target point tracking approach, multiple target points need to be updated to make AUV finally located at target point B above the DS with a certain posture as shown in Figure 1a.

Docking Task
When the AUV manages to reach the target point B directly above the DS, the camera can completely distinguish the outline of each LED light source in the light array on the DS. Correspondingly, a vision-based guidance algorithm can be used to obtain the relative position and attitude of the AUV with reference to the DS. Vision-based guidance can also calculate the attitude error of AUV through the visual information, and plan the target point C at the DS position. Initially, the line between BC points is roughly vertical downward. During the descent process, the position and attitude information of target point C are constantly updated; AUV will keep descending slowly with its attitude constantly adjusted to keep consistent with the DS, and finally ensure smooth docking.
In a nutshell, the homing and docking task can be simplified as the task to firstly track the target line and then track multiple target points.

Guidance Design
Quipped with different sensors and communication devices, AUVs can move underwater with proper guidance. However, due to the different accuracy of the required relative position information at different distances towards the DS, the guidance is mainly designed by two modes: USBL guidance, and vision-based guidance. USBL guidance is mainly used for homing to help AUV approach towards DS from a very far point, and vision-based guidance is used for AUV clamping into the DS which requires much more accurate near field guidance.

Guidance for Homing
USBL system is equipped with an acoustic transponder array under the AUV. In the homing process, the USBL serves as the navigation aid of the AUV to establish communications with the acoustic equipment of the DS. USBL positioning system localizes the underwater target, DS mainly by measuring the arrival azimuth and distance of the signal. The acoustic transmitter regularly emits acoustic pulse signals and uses the response time of the DS to calculate the slant distance between the AUV and the DS. USBL uses the phase difference of each response signal received by multiple hydrophones to calculate the azimuth of the DS. It is assumed that the transponder array composed of four hydrophones is located on baselines that are mutually perpendicular to each other as shown in Figure 2. In Figure 2, four red points represent four hydrophones, the blue point stands for the position of DS. θ x and θ y are the relative pose vector with reference to the x-axis and y-axis, respectively. The coordinate of AUV relative to the DS can be represented by (x a , y a , z a ) which has the following relation: where, c is the velocity of sound traveling in the water, T is the roundtrip period, θ x and θ y can be calculated by the phase difference of the received signals.

Guidance for Docking
In the final stage of 10 m away from the DS, the relative pose of AUV is calculated and controlled through the real-time image recognition of the LED light array captured by the bottom camera of AUV. Among them, the vision system part mainly includes two parts: the input is the real-time image and the output is the spatial position of AUV.
Design of the LED light array on the DS. The three LEDs are arranged on the same line which is the longitudinal axis of the DS. There is one LED namely LED2 located at the center of the DS with its left LED 1 and right LED3 symmetrically installed. The fourth light, LED4 is on the same side as LED3, whose connecting line is perpendicular to the longitudinal axis of DS. According to the configuration of the DS shown in Figure 3, the longitudinal axis of DS passes through the connecting line between the two charging pile centers on the right side and the center of the charging pile on the left side. As shown in Figure 3, the LED light array is designed in detail as follows: 1. LED2 is located at the center of the DS. LED1, LED2, and LED3 are arranged on the longitudinal axis of the DS. The central location and the longitudinal axis of the DS can be determined by the location of three LEDs captured in the image; 2. LED1, LED2, and LED3 consist of one axis while LED3 and LED4 make the other axis. Hence, the asymmetry makes it convenient to distinguish the axis and the heading angle of the AUV by the number of lights when the image is rotated; Image preprocessing. After the image is obtained by the camera under the AUV, there are often different kinds of noise in the image because of many impurities in the water. The important information in the image can be selectively extracted from a specific application environment. To precisely extract the required information from the image, it is necessary to preprocess the image first. Image filtering, that is, to remove the noise of the target image while retaining the details of the image, is an indispensable operation in image preprocessing. The quality of the preprocess will directly affect the effectiveness and reliability of the subsequent image processing and analysis. Filtering can remove the noise in the image, extract useful visual features, resample the image, and so on. As a kind of spatial filtering, median filtering can not only eliminate image noise but also make up for the shortcomings of the neighborhood average method to blur the edge, better retain the edge of the image, ensure relatively clear image contour. Because the edge of the image mainly includes the details and high-frequency characteristics of the image, the median filter is adopted for extracting the edge of the spot in the spot recognition. The basic idea of median filtering is to sort the gray levels of all pixels in a window and take the median value of the sorting result as the gray level value of the pixel at the center of the original window. Median filtering using the selected window is similar to the method of moving the operator on the image in the template matching operation. The process can be described into the following steps: (1) Determine the coincidence mode of the pixel at the center of the window according to the shape of the selected window on the original image; (2) Move the window pixel by pixel on the image; (3) Sort the corresponding pixels in the window according to their gray value, and find the median value of the sorting results; (4) Assign the found median value to the pixel in the resulting image corresponding to the center of the window.
The median filter is very effective for eliminating random noise and salt and pepper noise in the image. The main advantage of planting filter is a simple operation, which can filter out the noise and protect the detailed information of signals, such as edge and acute angle. In addition, the planting filter is easy to be adaptive, which can further improve its filtering characteristics.
Position recognition of LED light array. After the AUV reaches 10 m above the DS, the real-time position of the AUV is obtained via the position of the LED light array relative to the AUV. The image obtained by AUV contains many bright spots which are the position of the LEDs in the light array. In the previous step, after preprocessing the image such as filtering, edge extraction is carried out to obtain the position of the edge of the bright spot in the image, the center of each bright spot, that is, the position of each LED is calculated by the gray barycenter method.
For edge detection, the Canny operator is used in this paper. The operator is a multi-stage optimization operator with filtering, enhancement, and detection. The Canny operator is not easy to be disturbed by noise and can detect real edges, especially weak edges. The advantage of this method is that two different thresholds are used to detect the strong edge and the weak edge respectively. The weak edge is included in the output image only when connected with the strong edge. Therefore, this method is not easy to be affected by noise and is easier to detect the real weak edge. Before processing, the Canny operator first uses a Gaussian smoothing filter to smooth the image, remove noise, and then use the finite-difference of first-order partial derivative to calculate the gradient amplitude and direction. Finally, non-maximum suppression, edge detection, and connection with double threshold algorithm are conducted. Gaussian filtering is a common filtering algorithm at present. Its principle is weighted average according to the gray value of the pixel to be filtered and its neighboring points according to the parameter rules generated by the Gaussian formula, which can effectively filter the high-frequency noise superimposed in the ideal image. The transfer function of two-dimensional Gaussian filter is defined as Apply the h(x, y) into the filtering of image f (x, y) to get where " * " represents convolution. The gradient of smoothed can be calculated by using the 2 × 2 first-order finite difference approximation to calculate the two arrays GX and GY of partial derivatives of x and y. The mean value of the finite difference can be calculated in this 2 × 2 square to calculate the gradient of partial derivatives of x and y at the same point in the image. Only the global gradient is not enough to determine the edge, so to determine the edge, we must use the gradient direction to retain the point with the maximum local gradient, and suppress the non-maximum value. When the gradient angle is discretized into one of the four sectors of the circle, the window of the circle is used for suppression operation. The four sectors are numbered from 0 to 3, corresponding to four possible combinations of neighborhoods. At each point, the center pixel of the neighborhood is compared with the two pixels along the gradient line. If the gradient value of the center pixel is not greater than the gradient value of the two adjacent pixels along the gradient line, then = 0. After the above three steps, the edge quality is very high, but there are still many false edges. Therefore, the algorithm used in the Canny algorithm is the double threshold method. The specific idea is to select two thresholds. The points less than the low threshold are considered as false edges set to 0, the points greater than the high threshold are considered as strong edges set to 1, and the pixels in the middle need to be further checked.
According to the high threshold image, the edge is linked into a contour. When it reaches the end of the contour, the algorithm will find the point satisfying the low threshold in the 8 neighborhood points of the breakpoint, and then collect new edges according to this point until the whole image is closed. After obtaining the edge information of the bright spot, to obtain the center coordinates of the LED lights, it is necessary to determine the center of the bright spot. In this paper, the gray centroid method is used. For the target with uneven brightness, the gray centroid method can calculate the light power centroid coordinates according to the target light intensity distribution as the tracking point, also known as the density centroid algorithm. For an image of size m × n, if the gray value of a pixel exceeds the threshold T, it will participate in the barycenter processing. The gray center (x 0 , y 0 ) is calculated as where represents the pixel value of the i-th row and j-th column.
Positioning of AUV. According to the position of LED in the image, the relative position (x, y) and deviation angle θ between AUV and DS center are calculated by using the proportional relationship between camera field of view and image size. The plan of the LED light array is as follows: 1.
Determine the relative position(x, y) between the DS center and the AUV bottom center (where the camera is). In determining the relative position(x, y) of the DS center, it is first necessary to determine the position of the DS center light, that is, LED2 in the light array in Figure 4. After the center position of each spot is calculated by the gray centroid method, four pixels are obtained, and the sum of the distances between each pixel and the other three pixels is calculated. The least sum of the distances is LED2, which is also the position of the DS center in the image. After determining the representative spot at the center of the DS, the azimuth angle can be calculated by using the geometric relationship (the azimuth of the target on the left side of the axis is negative, and the azimuth on the right side of the axis is positive) as shown in Figure 5. The angle between the projection of the line between the center of the DS and the camera on the horizontal plane with the camera axis and the camera axis is called the horizontal azimuth θ Lenx as shown in Figure 6a, then the horizontal azimuth of the DS center has the following relationship with the abscissa of the DS center in the image:   Similarly, from Figure 6b, the relationship between θ Leny 's vertical azimuth and the longitudinal coordinate of the DS center in the image is as follows: The relative position (x, y) between the center of the DS and the center of AUV bottom can be obtained from the geometric relationship. 2.
Determine the AUV yaw angle θ(Counterclockwise is positive, clockwise is negative). The deviation angle of the DS in the image needs to be determined. To calculate the location of the DS axis via the location of LED1, LED2, and LED3 in the light array, it is necessary to calculate the slope of the line between the pixel coordinates of three LED lights except for LED2 and LED2 in the image coordinate system, and then compare the three slopes. Then, the two lines with the closest slope will pass through LED1 and LED3 respectively, and the mean value of the slopes of the two lines will be treated as the slope of the DS axis in the image. The angle between the DS axis and the horizontal direction can be determined by the slope of the line. Then LED4 is used to determine the driving direction of AUV relative to the DS. The first is to determine the position of LED3 and calculate the distance between the three points on the line and the center of LED4. The closest point is the center of LED3. Here X 2 and X 3 are the pixel values of the abscissa of LED2 and LED3 in the center of the image respectively. If X 2 > X 3 , the yaw angle of AUV θ = θ 0, otherwise θ = 180 • − θ 0 .

Dynamic Model
Generally, the dynamic model of the AUV or other ocean vehicles is in 6 degrees of freedom [2,4]. Considering the symmetry of the AUV, the two degrees of freedom of roll and pitch are ignored, that is to say, θ = Ψ = 0. According to the motion control requirements of AUV, it is necessary to control the pitch, sway, heave, and yaw. Therefore, the general 6-DOF robot model is simplified to form a 4-DOF model as expressed by Equation (13). Only the following state vectors need to be considered, T , It is noted that the coefficient of reference model ignores the small nonlinear terms. The moments produced by each propeller to roll and pitch are ignored.
where the above hydrodynamic force matrix can be also obtained via the CFD method [44][45][46][47], the inertial matrix M is The Coriolis force matrix C(v) is The damping force matrix D(v) is The resulting force of gravity and buoyancy force is The controlling force acting on the AUV is where K is the thrust allocation matrix shown as in Equation (34):

Sliding Mode Based Homing and Docking Control Design
To realize various underwater tasks, AUV needs to maintain a certain attitude, such as a certain depth, altitude, and heading, which can be solved by a motion controller. Moreover, AUV moving underwater will be inevitably affected by various external interference forces. In addition, many difficulties are obtaining hydrodynamic coefficients of the AUV in different events. The existence of these factors makes motion control of AUV more challenging. For the purpose of motion control, the sliding mode control method has the advantages of fast response, insensitive to corresponding parameter changes and disturbances, and simple subsequent physical implementation compared with the conventional control method like PID. Considering sliding mode control is very robust to the internal perturbation and external environmental disturbance and easily realized in engineering problems, the sliding mode control method is adopted to study the motion control of AUV.

Target Point Tracking
The four degrees of freedom of AUV need to be controlled separately to realize target point tracking. Taking heave motion as an example, the controller is deduced as such: according to the principle of the sliding mode VSC, the error of target controlling parameters, like depth, must be constructed to calculate the first and second-order derivatives respectively. The depth channel error and its derivatives are as follows The sliding mode surface function for the depth channel is defined as follows: where, c z > 0, the depth channel sliding mode function, Lyapunov function is designed as Substitute Equations (19) and (20) into the derivative of Equation (21): Power reaching law is applied on Equation (22): By solving Equation (23), the heaving force τ 3 can be calculated as follows: Similarly, the pitching τ 1 , surging forces τ 2 , and rolling moment τ 4 can be derived as e y − k y S y α y sgn(S y ) Theoretically, the discontinuous sign function will cause "jittering" of the system, reducing the validity of the control. To suppress the jittering, the boundary layer method is applied for improving the designed control law. Specifically, replace the sgn(S) by sat(S), and the redesigned power approaching law for Equation (24) is shown as: where, k z > 0, α z > 0, ∆ is the thickness of the boundary layer.
Substitute Equation (26) into Equation (22): At this time, the input forces for all 4 degrees of freedom are calculated as e y − k y S y α y sat S y

Target Line Tracking
To track the target line, the Line of Sight (LoS)-based guidance approach is used for calculating the heading angles of the AUV as shown in Figure 7: In the area far away from the DS, the underwater AUV is generally controlled by following a straight line. The design process of the target line controller is basically the same as that of the target point controller, but the method of obtaining the desired heading angle is different, which is obtained by the line of sight angle. In the process of following the target straight line, the forward thrust of the propeller is set to a fixed value to make AUV sail at a relatively stable forward speed, and the lateral thrust is set to 0. Therefore, the controller is designed only for the heading angle and depth in the motion process to make AUV realize the fixed depth direct motion.
In accordance with the previous section, the following control rates can be obtained by using the saturation function-based power reaching law sliding mode control method.
where a is a constant, r d is the desired heading angle, namely ψ d .

Thrust Allocation
Usually, the motion of an AUV mainly depends on the driving force generated by its actuator propeller, rudder, and wing. However, the AUV studied in this paper is not equipped with a rudder and wing mechanism. Therefore, the thrust generated by the propeller has become the main driving force and the main parameter of motion control. AUV is equipped with 8 thrusters, which are over-actuated and have the characteristics of vector arrangement. So, it needs to adopt an appropriate thrust control allocation strategy to obtain the optimal control performance index. The model of the thrusters and thrust control allocation will be discussed separately in this chapter.

The Model of the Thrusters
The layout of the 8 thrusters of the AUV is shown in Figure 8 and Table 1. No. 1-4 thrusters are vertically arranged to provide the maximum heave force and No. 5-8 thrusters are arranged at a certain angle with the ox axis to provide greater translation force and yaw moment. This redundant thrust configuration design can greatly increase the reliability of the system. If it is defined as such that α = α 1 . . . α p T ∈ R p represents the angle of azimuth thruster, and the designed underwater UUV has no azimuth thruster, so the thrust allocation matrix K(α) = K is a constant. For the 4 DOF dynamic model constructed in Equation (13), the calculating formula of thrust allocation matrix is given in Equation (36) where θ Represents the rotation angle of the propeller around the Z-axis of the appendage coordinate system, φ It represents the angle of thruster thrust direction relative to XY plane of appendage coordinate system, and X, Y, and Z represent the position of thruster relative to origin, respectively.

NO.
Location Azimuth The thrust allocation vector of each propeller can be calculated by Equation (33)

Thrust Allocation Optimization
The AUV aimed for autonomous homing and docking is an over-actuated underwater vehicle. How to allocate thrust for each thruster to achieve a certain objective such as minimum energy or power requirement becomes a realistic optimization problem. So the thrust allocation scheme for the desired control quantity of the controller output is not unique. This section mainly focuses on the thrust allocation problem of only four thrusters with a horizontal arrangement and how to achieve the optimal thrust allocation strategy.

Problem Statement
For the thrust allocation problem, the most common method is the pseudo-inverse allocation method, which has the greatest advantage of fast calculation speed and high real-time performance. However, because its basic idea is the least square optimization problem, it does not consider the problem of thrust saturation. Under some extreme control conditions, the thrust saturation happens and cannot meet the thrust demand. As a result, the final motion control effect may be changed.
Compared with the pseudo-inverse method, the quadratic programming(QP) method can effectively consider that the thrust generated by each thruster even if it is bounded, and introduce the command error term while considering the power consumption so that AUV can still make the corresponding response and guarantee the output of the thruster meet the command of the controller as much as possible when it cannot meet the command of the motion controller to achieve positioning and tracking.
As introduced in Xiang and Xiang [4], the objective function needs to be defined first. The objective function of thrust allocation optimization can be established as the following formula: min 1 2 (35) In Equation (35), the first part is the objective function, in which the first term is to achieve the minimum power, and the second term is to achieve the minimum error between thrust output and expected control command. H is the weighted coefficient matrix of the power consumption term, and W is the weighted coefficient matrix of the error term. The command error is as small as possible, so the value of W is much larger than H. The second part is the limit function which provides the upper and lower limits of command error calculation and thrust forces.

QP Optimization
According to the definition of a quadratic programming problem, the optimization objective function established in the previous section belongs to a linear convex optimization problem, so the active set method is used to solve it. Equation (35) is then transformed into standard secondary planning as follows: The active set method is an iterative method. After each iteration, the active set containing the optimal solution is predicted in the next step. The active set is some inequality constraints that contain the optimal solution. After each iteration, the information of the active set are used to calculate a new iteration direction and find the optimal solution in this direction or give a new iteration direction to the optimal solution through the calculation in this direction until the optimal solution is given.

Simulation and Analysis
It can be seen from the task planning scheme in Section 3. The three target points are the planned target point information after the DS information is obtained by USBL after tracking to P1. P4 is the position 10 m above the DS. After the AUV tracks to P4, the image and processing information are obtained by the camera, as shown in Figure 9 and Table 2. The vision-based guidance is simulated on OpenCV. The accurate position P5 of the DS is obtained by the visual relative information.   - Through the two points of P0 and P1, a straight line is planned in the horizontal plane to track. After judging that it enters a certain range of P1 points, it switches to target point tracking, to track P2, P3, P4, and P5 points one by one. In this process, it also judges that it switches to the next target point to track when it reaches a certain range of the current target point.
In the simulations, the sliding mode controller parameters are small parameters, which gradually increase the control effect from divergence to final convergence to obtain a set of more appropriate parameters. For the weight parameters in the thrust allocation problem, a set of parameters is selected empirically under the condition that the weight of the error term is much greater than that of the power consumption term. The time step is set to 0.2 s and settings for parameters required by sliding mode controller design are listed:

Simulation Results
In the visual guidance process, the simulation obtains the image of the LED light array with a certain posture and carries on the visual processing, and the result is shown in Figure 9 and Table 2

Discussion and Analysis
In the visual guidance strategy, the image processing effect is shown in Figure 9. This processing can accurately obtain the center position of the light array, and then calculate the relative position and UUV yaw angle between the docking station and the UUV. The absolute errors of the relative position and yaw angle calculated according to the obtained light array position and the real position of the light array respectively are less than 5%. Figure 10 shows the space navigation trajectory of the whole process of AUV homing and docking. From the figure, it can be seen that AUV can stably track the target line and target point through different controller methods: sliding mode controller and PID controller. Figures 11 and 12 show the specific change process of AUV's position, attitude, and speed with four degrees of freedom controlled. Compared with the tracking effect using the PID controller, the sliding mode controller does not present a significant difference at the stage of tracking the straight-line, but in the later point tracking stage, the sliding mode method can achieve faster and smoother tracking to the target endpoint. By using the sliding mode control method, AUV can reach the endpoint at 900 s which is taking much less time than 1400 s by PID controller. The sliding mode method can also achieve a much more smooth tracking effect in the process of switching from tracking target line to tracking target points and tracking between different target points, and finally, reach the end position with the expected heading angle more accurately. At the end of 1500 s 'simulations, AUV controlled by the sliding mode method can achieve X-direction error less than 0.01 m, Y-direction error less than 0.01 m, Z-direction error less than 0.01 m, and heading angle psi error less than 0.1 • . However, in the PID controller simulations, the x-direction error of AUV is less than 1 m, the y-direction error is less than 1.5 m, the zdirection error is less than 0.3 m, and the heading angle psi error is less than 0.5 • . Although the simulation time continues to increase, the tracking error of the control method may be further reduced, but more control time is obviously not desirable in practical application. Figure 13 shows the change process of the expected control quantity of four degrees of freedom. It can be clearly seen that the jump of the control force is caused by the sudden change of tracking error during the switching between target lines to points. However, the final control force is 0 stable at the end position. Compared with PID controller simulation tests, the output torque of yaw freedom of the sliding mode method is significantly lower, resulting in more effective heading control. Figure 14 shows the thrust change process of four thrusters in the horizontal plane. The four thrusters distribute the three degrees of freedom control quantities in the horizontal plane. After the final stability, the thruster output is 0.
Based on the above comparative analysis, it can be concluded that the proposed sliding mode control strategy has the advantages of fast response and strong adaptability to different state tracking parameters. What is more, the sliding mode control strategy can realize the switching process of tracking target line to target point much faster and more accurately. In a word, the simulated results presented in Figures 9-14 demonstrate that the whole homing and docking tasks can be well completed using the proposed unified approach. So, this will be the motivation for the wide practical use of the proposed approach in the future.

Conclusions
This paper mainly focuses on providing a complete solution to the problem of autonomous homing and docking of an over-actuated AUV using a unified approach that involves task planning, guidance and control design, thrust allocation, etc.
Firstly, the AUV is simplified into a four degree of freedom dynamic model according to the actual task needs. The appropriate dynamic model can effectively reflect the actual motion state of AUV, provide convenience for motion control design, and effectively guarantee the handling performance in practical application.
To provide reliable information for the docking of AUV, a vision-based algorithm is used for the guidance of AUV. The LED light array is designed and arranged on the docking station, and a complete set of visual information processing flow including image preprocessing, LED light array position extraction and relative pose analysis is established.
In the motion process of AUV, the target line or target point tracking mode can be adopted based on tasks. The sliding mode control method based on power approaching and saturated boundary layer together with the quadratic programming based thrust allocation method are designed to output the command of each thruster, to ensure the stability of the AUV tracking process.
Finally, to prove the feasibility of the algorithm, the visual algorithm and control algorithm are simulated and verified.
Author Contributions: Conceptualization, G.X. and M.Z.; methodology, M.Z. and G.W.; software, G.W. and Y.X.; investigation, G.X. and G.W.; resources, G.X.; writing-original draft preparation, G.X.; writing-review and editing, M.Z. and Y.X. All authors have read and agreed to the published version of the manuscript.