**4. Tendon excursion**

Tendon excursion is often thought of as a limiting factor in electromechanical hand design. Tendon excursion is the displacement an artificial tendon experiences when the associated muscle/actuator contracts and induces tensile forces on it. Tendon excursion is can effect hand movement negatively.

For example, wrist movement induces tendon excursion on all the FDP tendons of the fingers. This can be seen in the natural flexion of the fingers during wrist extension and their natural extension during wrist flexion (**Figure 13**).

**Figure 13.** *Tendon excursion in the FDP of each finger induced by wrist movement.*

*Biomechanical Design Principles Underpinning Anthropomorphic Manipulators DOI: http://dx.doi.org/10.5772/intechopen.105434*

The angles *ωf* and *ωe* represent the magnitude with which the wrist joint is in flexion or extension respectively. *ω* ¼ 0 is located vertically upward from the forearm bones and is measured from the pivot point of the wrist to the metacarpal creating the largest angle from the equilibrium point. In this case measurements are taken from the metacarpal of the little finger. Let us take the artificial FDP flexor tendon of each digit as an example. We can write a set of tendon displacements with respect to the wrist angle. Let, *ωf* represent wrist flexion for any *ω*>0. We can write

$$a\_f = \{d\_{\rm Fi} d\_{\rm Fm} \, d\_{\rm Fr} \, d\_{\rm Fl} \, d\_{\rm Ft} \} \tag{6}$$

Where, *d* is the displacement of the tendon in mm and the subscripts*i, m, r, l* and *t* are the index, middle, ring, little, and thumb digits respectively. The subscript, *F*, represents the flexor tendon and in the case of the thumb, *dFt* ¼ f g *dFt*1 *dFt*2 . Where, *dFt*1 *and dFt*2 are artificial replications of the the APL and FPL (**Figure 11**) tendons respectively.

The effect of tendon excursion can be quantified by measuring the elongation/ displacement flexor tendons while the wrist is moving. After measuring the artificial FDP displacement each finger was displaced by over 10 mm. A 10 mm displacement represents more than 50 percent of the fingers total motion, thus, tendon excursion cannot be ignored. In a similar manner the effect of tendon excursion on the thumb is too large to ignore, therefore we look to the following section as one solution to manage the unwanted effects of tendon excursion.

The source of displacement for each artificial tendon stems from its actuator, therefore, solutions can be found by adjusting the tendons actuator to account for the unwanted tendon excursion. Let us assume an anthropomorphic manipulator we have a series of four servomotors actuating four artificial FDP tendons. A servomotor correction factor can be implemented within the control scheme which considers the wrist angle. The correction factor allows the wrist induced excursion to meet equilibrium with the individual finger servomotors during wrist movement using:

$$
\sigma = \left(\frac{\pi R}{180}\right) \times \rho\_i \tag{7}
$$

Where, *R* is the servo horn radius and *φi* is the angle of the servo horn. By so doing, the servomotor angles for each digit can be plotted against the wrist angle, *ω* at any given time.

**Figure 14.** *Servomotor correction angles for wrist movement induced tendon excursion in the fingers.*

The excursion correction angles displayed in **Figure 14** shows how servomotor angles change with respect to wrist position.

Including these types of correction tables into the control schemes of all electromechanical anthropomorphic hands is trivial but important for hand function and control.


**Author details**

Hopestone Kayiska Chavula Macroeconomics and Governance Division, Economic Commission for Africa, Addis Ababa, Ethiopia

\*Address all correspondence to<EMAIL_ADDRESS><EMAIL_ADDRESS>
© 2020 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/ by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.


**1. Introduction**

Elevator is a large-scale complex equipment integrating mechanical, electrical and control. If it breaks down, it will directly affect the safe and efficient operation of the elevator. Nowadays, elevators are used more and more frequently in daily life and production. Therefore, the number of elevators shows a trend of continuous growth [1], and the accompanying elevator failure and maintenance problems are becoming more and more prominent [2,3]. As the power device of the elevator, the traction machine determines whether the elevator can operate normally. With the vigorous development of science and technology, the state recognition technology of elevator traction machine is also constantly improving [4,5].

In recent years, the fault diagnosis technology of elevator traction machine with the artificial intelligence [6] or image processing [7] has been widely applied and developed. However, these methods face problems, such as non-universality of diagnostic model, high cost of model training, and requirement for massive fault samples. In addition, the selection of fault features is also of great significance to the optimization of diagnosis model.

Traction machine is a complex mechanical structure, which is closely connected by various parts. Therefore, the state identification of the traction machine can be diagnosed by various signals, such as vibration, noise, current, temperature, braking torque, speed, and power. Many useful information is hidden in the vibration signal of the traction machine [8]. These signal characteristics can reflect the working condition of the equipment. By analyzing the vibration characteristics of the equipment, the safety operation, accident prevention and maintenance cost reduction can all be accomplished.

Based on the vibration signals, a lot of research have been done in which signal feature extraction methods are the most important section of fault diagnosis [9,10]. The

**Citation:** Li, D.; Yang, J.; Liu, Y. Research on State Recognition Technology of Elevator Traction Machine Based on Modulation Feature Extraction. *Sensors* **2022**, *22*, 9247. https://doi.org/10.3390/ s22239247

Academic Editors: Yongbo Li and Ruqiang Yan

Received: 6 October 2022 Accepted: 24 November 2022 Published: 28 November 2022

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

**Copyright:** © 2022 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/).

time domain method and frequency domain method have been commonly used in early fault diagnosis engineering [11]. Filtering, amplification, statistical feature calculation, correlation analysis, and other time-domain signal processing are referred to as timedomain signal analysis. However, it only reflects the change of amplitude with time and lack frequency bands information. The frequency domain analysis method is to describe the raw signal in the frequency domain, which is more intuitive than the time domain analysis method. However, traditional frequency domain analysis might fail to extract the characteristics information of traction machines due to the heavy background noise and complicated excitation sources [12]. Therefore, only relying on the frequency domain analysis method is far from meeting the current requirements of traction machine fault diagnosis. This brings huge challenges to the status identification and fault diagnosis of traction machines. Therefore, a time-frequency combination processing method has been proposed. Short-time Fourier transform (STFT) [13,14] and wavelet transform (WT) [15] are the common used processing tools with fine time localization and frequency resolution. These methods are realized by superposition of Fourier transform in different fixed window length. However, due to lacking self-adaptability, the quality of feature extraction might be affected by the selection of window function or wavelet basis function.

In addition, the above methods can extract the characteristics of vibration signals, but the collected vibration signals usually contain background noise and unknown frequency interference. To eliminate noise component and extract the fault feature information of raw vibration signals, several demodulation techniques have been applied to past research, such as Hilbert transform (HT) [16], empirical mode composition (EMD) [17], spectral kurtosis (SK) [18], nonstationary analysis [19–21], and cyclostationary analysis [22–24]. These methods have been applied to modulation frequency extraction already, which noted the modulation mechanism in a rotating machine.

Feng et al. [25,26] proposed an adaptive iterative generalized demodulation method to extract the modulation features in nonstationary analysis. The vibration characteristics of hydraulic turbine and planetary gearbox have been successfully found in the joint timefrequency domain. Most vibration signals of traction machine are non-stationary signals, but they are cyclostationary signals, namely, the correlation function of traction machine signals is periodic function of time. In view of the cyclostationary analysis theory, a variety of methodologies have been proposed, in which cyclic modulation spectrum (CMS) and fast spectral correlation (Fast-SC) are two typical cyclostationary tools [22]. However, they did not gain its deserved attention because of high computational cost.

Wang et al. [27] improved the cyclostationary methods with an application of Teager Kaiser energy operator (TKEO), which can enhance fault feature recognition with low computational burden. Song et al. [28,29] proposed a demodulation method based on time-frequency analysis (TSA) and principal component analysis (PCA) and applied it to the modulation frequency extraction of pump and permanent magnet synchronous motor (PMSM). Moreover, due to dimensionality reduction of time-frequency distribution matrix, the burden of high computational cost was greatly relieved. The main process of the algorithm is as follows: Firstly, the raw vibration signal is transformed into time-frequency domain by STFT. Then, the PCA method is used to reduce the dimensionality of the timefrequency spectrum in order to extract the eigenvalues of the principal components. Finally, the principal components are reconstructed to obtain the modulation signals.

Among the above demodulation methods, it could be found that the demodulation method base on PCA (DPCA) has great potential for applications in traction machine. In addition, although the fault diagnosis technology of elevator traction machine based on artificial intelligence or image processing has been widely applied and developed. However, few investigations have been done to extract and analyze the modulation features of traction. The modulation mechanism of traction machine has also rarely been involved. These above issues have greatly hindered the development of elevator fault diagnosis technology.

In this paper, the modulation characteristics of the traction machine vibration signal were extracted through a demodulation method based on time-frequency analysis and principal component analysis (DPCA). The characteristics extracted by DPCA is more prominent under the interference of background noise and unknown frequency, which is helpful to the state identification of the traction machine. The principle of signal demodulation method and experiential setting are introduced respectively in Section 2. In Section 3, the vibration signal of the traction machine is processed by FFT, STFT and DPCA methods. The influence of different working conditions on the vibration of traction machine is discussed, which shows the superiority of the demodulation technology. Finally, the conclusions are drawn in Section 4.


A rectangular trajectory for four waypoints was tracked by the USV, which is shown in Figure 15, where the red plusses represent the four waypoints of the expected rectangle, the black diamond represents the initial position of the USV, and the black line is the trajectory of the USV. The achieved range between the USV and the current expected waypoint is set as 5 m, i.e., when the USV reach to the range, the tracking for the current waypoint was finished and the USV turned to the next waypoint. It was shown that the performance of trajectory tracking was good except for some draft in the vertexes of the rectangle due to no special disposing for plan trajectory around the vertexes. In Figure 15, the rectangle was about 249.6 m × 308.8 m, and the voyage distance of the USV was about 1200.0 m. There was some offset between the expected trajectory and the actual trajectory, and the one reason was that the precision of the GPS was about 2.5 m and the orientation precision of the digital compass was about 0.5°; the other rea-

In view of practical engineering, a three-layered architecture for TPTH-USV's trajectory tracking was proposed and validated using the 'Jiuhang' USV in the sea experiments. Besides the conventional kinematic and kinetic layer, a motion restriction layer was added in the three-layered architecture. The proposed guidance law and controllers in the first and third layers were properly suitable for the type of TPTH USVs, which could be applied directly without considering the motion model's variety. The ALOS law can force the USV to track a curved line with a time constraint and give out speed and course variables which are taken as the expected value in the third layer. The twin PID controller can justly solve the speed and course coupled issue of the TPTH USV. The identified model of the USV was used to restrict the basis control variable and differen-

the expected line was about 249.6 m.

**5. Conclusions**

initial position of the USV, and the black line is the trajectory of the USV. It was shown that the initial course of the USV was about 266.1◦ , which was almost opposite to the expected direction in Figure 14, so the USV could track the line well by a large angle turning. It seemed that the USV did not reach the end point, thanks to an arriving radius around the end point being set. The speed was about 3.3 kn when trajectory tracking of the line was stable. The voyage distance of the USV was about 290.0 m, and the length of the expected line was about 249.6 m.

A rectangular trajectory for four waypoints was tracked by the USV, which is shown in Figure 15, where the red plusses represent the four waypoints of the expected rectangle, the black diamond represents the initial position of the USV, and the black line is the trajectory of the USV. The achieved range between the USV and the current expected waypoint is set as 5 m, i.e., when the USV reach to the range, the tracking for the current waypoint was finished and the USV turned to the next waypoint. It was shown that the performance of trajectory tracking was good except for some draft in the vertexes of the rectangle due to no special disposing for plan trajectory around the vertexes. In Figure 15, the rectangle was about 249.6 m × 308.8 m, and the voyage distance of the USV was about 1200.0 m. There was some offset between the expected trajectory and the actual trajectory, and the one reason was that the precision of the GPS was about 2.5 m and the orientation precision of the digital compass was about 0.5◦ ; the other reason was that the USV's control was disturbed by the wind and waves.

#### **5. Conclusions**

In view of practical engineering, a three-layered architecture for TPTH-USV's trajectory tracking was proposed and validated using the 'Jiuhang' USV in the sea experiments. Besides the conventional kinematic and kinetic layer, a motion restriction layer was added in the three-layered architecture. The proposed guidance law and controllers in the first and third layers were properly suitable for the type of TPTH USVs, which could be applied directly without considering the motion model's variety. The ALOS law can force the USV to track a curved line with a time constraint and give out speed and course variables which are taken as the expected value in the third layer. The twin PID controller can justly solve the speed and course coupled issue of the TPTH USV. The identified model of the USV was used to restrict the basis control variable and differential control variable simply in the motion limitator. In the future, the three-layered architecture of the TPTH-USV will be improved considering sea disturbances, such as waves and current.

**Author Contributions:** Conceptualization, J.J.; methodology, J.J.; software, D.L.; validation, D.W.; formal analysis, D.L.; investigation, D.L.; resources, Y.M.; data curation, D.L.; writing—original draft preparation, J.J.; writing—review and editing, J.J.; visualization, D.W.; supervision, Y.M.; project administration, Y.M.; funding acquisition, Y.M. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by the National Key Research and Development Program of China, grant number 2017YFC14052.

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

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Acknowledgments:** The authors would like to thank Feng Shao and Junnan Shi at FIO for experimental help.

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

## **References**

**Mingjiu Zuo 1 , Guandao Wang 2 , Yongxin Xiao 2 and Gong Xiang 2,3,4,\***

4 Hubei Key Laboratory of Naval Architecture and Ocean Engineering Hydrodynamics (HUST), Wuhan 430074, China

**\*** Correspondence<EMAIL_ADDRESS>
**Abstract:** 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 overactuated 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.

**Keywords:** AUV; homing and docking; vision-based guidance; target point/line planning and following; thrust allocation

## **1. 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.

**Citation:** Zuo, M.; Wang, G.; Xiao, Y.; Xiang, G. A Unified Approach for Underwater Homing and Docking of over-Actuated AUV. *J. Mar. Sci. Eng.* **2021**, *9*, 884. https://doi.org/ 10.3390/jmse9080884

Academic Editor: Rosemary Norman

Received: 7 July 2021 Accepted: 9 August 2021 Published: 17 August 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/).

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–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.

#### **2. 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.

**Figure 1.** Sketch of homing and docking approach.

#### *2.1. 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.

#### *2.2. 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.

#### *2.3. 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.

#### **3. Homing and Docking Strategy**

#### *3.1. 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.

#### 3.1.1. 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.

#### 3.1.2. 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.

#### *3.2. 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.

#### 3.2.1. 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.

**Figure 2.** Positioning of AUV using USBL.

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 (*xa*, *ya*, *za*) which has the following relation:

$$\begin{array}{l} R = \mathbf{c} \cdot \frac{T}{2} = \sqrt{x\_a^2 + y\_a^2 + z\_a^2} \\ x\_a = R \cos \theta\_x \\ y\_a = R \cos \theta\_y \end{array} \tag{1}$$

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.

#### 3.2.2. 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;

**Figure 3.** Arrangement of the LED light array on the Docking Station.

**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

$$h(\mathbf{x}, y) = \frac{1}{2\pi\sigma^2} e^{-\frac{\mathbf{x}^2 + y^2}{2\sigma^2}} \tag{2}$$

Apply the *h*(*x*, *y*) into the filtering of image *f*(*x*, *y*) to get

$$g(\mathbf{x}, y) = h(\mathbf{x}, y) \* f(\mathbf{x}, y) \tag{3}$$

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

$$\mathbf{x}\_0 = \frac{\sum\_{i=1}^{m} \sum\_{j=1}^{n} \mathbf{x}\_i f\_{ij}}{\sum\_{i=1}^{m} \sum\_{j=1}^{n} f\_{ij}} \tag{4}$$

$$y\_0 = \frac{\sum\_{i=1}^{m} \sum\_{j=1}^{n} y\_j f\_{ij}}{\sum\_{i=1}^{m} \sum\_{j=1}^{n} f\_{ij}} \tag{5}$$

$$f\_{\hat{l}\hat{j}} = \begin{cases} \begin{array}{c} 0, \text{gray value} < T \\ f\_{\hat{l}\hat{j}\prime} \text{gray value} \ge T \end{array} \end{cases} \tag{6}$$

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:

$$l\_{Bx} = \mathfrak{u}\_{obj} - \mathfrak{c}\_x \tag{7}$$

$$\psi\_{\text{obj\\_x}} = l\_{Bx} \frac{\theta\_{\text{Lenz}}}{W\_{\text{imgx}}} = \frac{\left(u\_{\text{obj}} - c\_x\right)\theta\_{\text{Lenz}}}{W\_{\text{imgx}}} \tag{8}$$

**Figure 4.** Arrangement of the LED light array on the DS.

**Figure 5.** Calculation of Azimuth angle.

**Figure 6.** Calculation of Azimuth angles.

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:

$$l\_{By} = \upsilon\_{obj} - \mathfrak{c}\_{y} \tag{9}$$

$$\psi\_{\rm obj} = l\_{By} \frac{\theta\_{\rm Leny}}{W\_{\rm impy}} = \frac{\left(v\_{\rm obj} - c\_y\right)\theta\_{\rm Leny}}{W\_{\rm impy}} \tag{10}$$

The relative position (*x*, *y*) between the center of the DS and the center of AUV bottom can be obtained from the geometric relationship.

$$\mathfrak{x} = \mathfrak{h} \times \tan \psi\_{obj\text{x}} \tag{11}$$

$$y = h \times \tan \psi\_{obj} \tag{12}$$

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.

## **4. Controller Design**

#### *4.1. 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, *η* = [ *x y z ψ* ] *T* , *v* = [ *u v w r* ] *T* , *τ* = [ *X Y Z N* ] *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.

$$M\dot{v} + \mathcal{C}(v)v + D(v)v + \mathcal{g}(\eta) = \tau \tag{13}$$

where the above hydrodynamic force matrix can be also obtained via the CFD method [44–47], the inertial matrix *M* is

$$M = \begin{bmatrix} m - X\_{\dot{u}} & 0 & 0 & 0 \\ 0 & m - Y\_{\dot{v}} & 0 & 0 \\ 0 & 0 & m - Z\_{\dot{w}} & 0 \\ 0 & 0 & 0 & I\_{zz} - N\_{\dot{r}} \end{bmatrix} \tag{14}$$

The Coriolis force matrix *C*(*v*) is

$$\mathbf{C}(v) = \begin{bmatrix} 0 & 0 & 0 & -(m - Y\_{\bar{v}})v \\ 0 & 0 & 0 & (m - X\_{\bar{u}})u \\ 0 & 0 & 0 & 0 \\ (m - Y\_{\bar{v}})v & -(m - X\_{\bar{u}})u & 0 & 0 \end{bmatrix} \tag{15}$$

The damping force matrix *D*(*v*) is

$$D(v) = -\begin{bmatrix} \mathbf{X}\_{\boldsymbol{\varPi}} + \mathbf{X}\_{\boldsymbol{\varPi}\_{[\boldsymbol{\varPi}]}} \Big| \boldsymbol{u} \Big| & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Y}\_{\boldsymbol{\varPi}} + \mathbf{Y}\_{\boldsymbol{[v}\_{[\boldsymbol{v}]}} \Big| \boldsymbol{v} \Big| & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{Z}\_{\boldsymbol{W}} + \mathbf{Z}\_{\boldsymbol{\varPi}\_{[\boldsymbol{v}]}} \Big| \boldsymbol{w} \Big| & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{N}\_{\boldsymbol{r}} + \mathbf{N}\_{\boldsymbol{r}\_{[\boldsymbol{r}]}} \Big| \boldsymbol{r} \Big| \end{bmatrix} \tag{16}$$

The resulting force of gravity and buoyancy force is

$$\log(\eta) = \begin{bmatrix} 0, 0, -(\mathcal{W} - B), 0 \end{bmatrix}^T \tag{17}$$

The controlling force acting on the AUV is

$$
\pi = \mathbb{K}u \tag{18}
$$

where *K* is the thrust allocation matrix shown as in Equation (34):

#### *4.2. 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.

#### 4.2.1. 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

$$e\_z = z - z\_d,\\
\dot{e}\_z = \dot{z} - \dot{z}\_d,\\
\ddot{e}\_z = \ddot{z} - \ddot{z}\_d = -\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d \tag{19}$$

The sliding mode surface function for the depth channel is defined as follows:

$$\mathcal{S}\_z = \dot{e}\_z + c\_z e\_z \dot{\mathcal{S}}\_z = \ddot{e}\_z + c\_z \dot{e}\_z \tag{20}$$

where, *cz* > 0, the depth channel sliding mode function, Lyapunov function is designed as

$$V\_z = \frac{1}{2} \mathcal{S}\_z^2 \tag{21}$$

Substitute Equations (19) and (20) into the derivative of Equation (21):

$$\dot{V}\_{z} = \mathcal{S}\_{z}\dot{\mathcal{S}}\_{z} = \mathcal{S}\_{z}(\ddot{e}\_{z} + c\_{z}\dot{e}\_{z}) = \mathcal{S}\_{z}(\ddot{e}\_{z} + c\_{z}\dot{e}\_{z}) = \mathcal{S}\_{z}(-\frac{d\_{3}w}{m\_{3}} + \frac{\tau\_{3}}{m\_{3}} - \ddot{z}\_{d} + c\_{z}\dot{e}\_{z}) \tag{22}$$

Power reaching law is applied on Equation (22):

$$-\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d + c\_z \dot{e}\_z = -k\_z |\mathbf{S}\_z|^{d\_z} \text{sgn}(\mathbf{S}\_z) \tag{23}$$

By solving Equation (23), the heaving force *τ*3 can be calculated as follows:

$$\tau\_3 = m\_3 \left( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathbf{S}\_z|^{a\_z} \text{sgn}(\mathbf{S}\_z) \right) \tag{24}$$

Similarly, the pitching *τ*1, surging forces *τ*2, and rolling moment *τ*4 can be derived as

$$\begin{cases} \tau\_1 = m\_1 \left( -\frac{m\_2 v r}{m\_1} + \frac{d\_1 u}{m\_1} + \ddot{\mathbf{x}}\_d - c\_x \dot{e}\_x - k\_x |\mathbb{S}\_x|^{a\_X} \text{sgn}(\mathcal{S}\_x) \right) \\ \tau\_2 = m\_2 \left( -\frac{m\_1 u r}{m\_2} + \frac{d\_2 v}{m\_2} + \ddot{\mathbf{y}}\_d - c\_y \dot{e}\_y - k\_y |\mathbb{S}\_y|^{a\_Y} \text{sgn}(\mathcal{S}\_y) \right) \\ \tau\_4 = m\_4 \left( -\frac{(m\_1 - m\_2) u v}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathbb{S}\_x|^{a\_r} \text{sgn}(\mathcal{S}\_r) \right) \end{cases} \tag{25}$$

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:

$$-\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d + c\_z \dot{e}\_z = -k\_z |\mathbf{S}\_z|^{a\_z} \text{sat}(\mathbf{S}\_z) \tag{26}$$

where,

$$sat(S) = \begin{cases} \begin{array}{ll} 1, & S > \Delta \\ \ S/\Delta, & |S| \le \Delta \\ -1, & S < -\Delta \end{array} \end{cases} \tag{27}$$

*kz* > 0, *αz* > 0, ∆ is the thickness of the boundary layer. Substitute Equation (26) into Equation (22):

$$\dot{V}\_z = \mathcal{S}\_z(-k\_z |\mathcal{S}\_z|^{a\_z} \text{sat}(\mathcal{S}\_z)) \tag{28}$$

When |*Sz*| > ∆

$$\dot{V}\_z = \mathcal{S}\_z(-k\_z |\mathcal{S}\_z|^{a\_z} \text{sat}(\mathcal{S}\_z)) = -k\_z |\mathcal{S}\_z|^{a\_z + 1} \le 0 \tag{29}$$

When |*Sz*| ≤ ∆

$$\dot{V}\_z = S\_z(-k\_z|S\_z|^{\underline{a}\_z}S\_z/\Delta) = -k\_z/\Delta|S\_z|^{\underline{a}\_z+2} \le 0\tag{30}$$

At this time, the input forces for all 4 degrees of freedom are calculated as

$$\begin{aligned} \tau\_1 &= m\_1 \Big( -\frac{m\_2 v r}{m\_1} + \frac{d\_1 u}{m\_1} + \ddot{\mathbf{x}}\_d - c\_x \dot{e}\_x - k\_x |\mathbf{S}\_x|^{a\_x} \text{sat}(\mathbf{S}\_x) \Big) \\ \tau\_2 &= m\_2 \Big( -\frac{m\_1 u r}{m\_2} + \frac{d\_2 v}{m\_2} + \ddot{y}\_d - c\_y \dot{e}\_y - k\_y |\mathbf{S}\_y|^{a\_y} \text{sat}(\mathbf{S}\_y) \Big) \\ \tau\_3 &= m\_3 \Big( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathbf{S}\_z|^{a\_z} \text{sat}(\mathbf{S}\_z) \Big) \\ \tau\_4 &= m\_4 \Big( -\frac{(m\_1 - m\_2) uv}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathbf{S}\_r|^{a\_r} \text{sat}(\mathbf{S}\_r) \Big) \end{aligned} \tag{31}$$

## 4.2.2. 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:

**Figure 7.** The sketch for LoS guidance.

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.

$$\begin{aligned} \tau\_1 &= a\tau\_2 = 0\\ \tau\_4 &= 0 \tau\_3 = m\_3 \left( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathcal{S}\_z|^{\alpha\_z} \text{sat}(\mathcal{S}\_z) \right) \\ \tau\_4 &= m\_4 \left( -\frac{(m\_1 - m\_2) \mu v}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathcal{S}\_r|^{\alpha\_r} \text{sat}(\mathcal{S}\_r) \right) \end{aligned} \tag{32}$$

where *a* is a constant, *rd* is the desired heading angle, namely *ψd* .

#### **5. 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.

#### *5.1. 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.

**Figure 8.** The vectoral configuration of thrusters of AUV.

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.

**Table 1.** Location and azimuth of the thrusters.

$$\begin{aligned} \mathbf{K}\_{l} = \begin{bmatrix} \text{Surge} \\ \text{Sway} \\ \text{Heave} \\ \text{Yaw} \end{bmatrix} = \begin{bmatrix} \cos(\theta)\cos(\phi) \\ \sin(\theta)\cos(\phi) \\ \sin(\phi) \\ -\mathbf{Y}\cos(\theta)\cos(\phi) + \mathbf{X}\sin(\theta)\cos(\phi) \end{bmatrix} \end{aligned} \tag{33}$$

The thrust allocation vector of each propeller can be calculated by Equation (33) combined with the installation position and angle of the propeller. The installation position and angle of the propeller are shown in the table below, where *L*1 = 0.144 m, *L*2 = 0.106 m, *L*3 = 0.1 m, *L*4 = 0.1 m, *Z*1 = 0.1 m, *Z*2 = 0.1 m, in addition, *L* = q *L* 2 1 + *L* 2 2 ,*α*= 10.4◦ . It is used in the calculation of yaw moment. The thrust allocation matrix obtained by combining the thrust allocation vectors of eight propellers is shown in Equation (34).

$$\mathbf{K} = \begin{bmatrix} \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 & 0 & 0\\ \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & -1 & -1 & 1\\ L \times \cos \mathfrak{a} & -L \times \cos \mathfrak{a} & L \times \cos \mathfrak{a} & -L \times \cos \mathfrak{a} & 0 & 0 & 0 & 0 \end{bmatrix} \tag{34}$$

## *5.2. 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.

#### 5.2.1. 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:

$$\begin{array}{ll}\min & \frac{1}{2} f^T H f + s^T W s\\ & s.t. \ \ \tau\_d - Kf = s\\ & f\_{\min} \le f \le f\_{\max} \end{array} \tag{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.

#### 5.2.2. 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:

$$\begin{array}{l} \min \frac{1}{2} f^T \boldsymbol{\Lambda} f + \boldsymbol{c}^T f \\ \text{s.t.} \quad f\_{\text{min}} \le f \le f\_{\text{max}} \end{array} \tag{36}$$

where, **Λ** = *H* + 2*K TWK*, *c* = −2*K TWτd*

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.

.

#### **6. Simulation and Analysis**

It can be seen from the task planning scheme in Section 3.1 that the path planning for AUV homing and docking tasks mainly involves the planning of the target point and target line in the AUV path. To verify the effect of the two tracking methods, the simulation experiments are carried out from the point tracking and the line tracking. In the simulation, the model parameters M = diag (9.91, 25.8, 20.61, 0.28), D = diag (34.69, 103.25, 74.23, 0.43) were selected. The continuous process of target line tracking and target point tracking in the homing and docking tasks is simulated based on the proposed sliding mode controller, QP thrust allocation optimization method, LOS, and vision-based guidance algorithm.

Assumed simulation environment: initial position and attitude of underwater AUV P0 (0 m, 0 m, −10 m, 0◦ ), In the process, because of the different sensors and sensors in different stages, simultaneous interpreting of multiple target points is carried out in the simulation process. Set the points obtained during the movement as follows:

 3 P5 (640.75 m, 848.68 m, −30 m, −7.03◦ ), This point is the location information of the DS after the AUV arrives at P4 and performs fine target resolution by vision.

**Figure 9.** The original image was captured by the camera at P4 10 m above the DS and the extraction image of the LED light array.

**Table 2.** Visually processed data (=10 m).

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:

1 Parameters for target line tracking

$$k\_z = 1.4; k\_r = 0.5;$$

$$c\_2 = 1.2; c\_r = 2.2;$$

*αz* = 0.8; *αr* = 0.8; ∆ = 0.1; *ke* = 0.2;

## *H* = 1; *W* = 10, 000;

$$\begin{array}{l l l} f\_{\text{min}} = \begin{bmatrix} & -40 & -40 & -40 & -40 & \end{bmatrix}^T; \\ f\_{\text{max}} = \begin{bmatrix} & 40 & 40 & 40 & 40 & \end{bmatrix}^T; \end{array}$$

#### *6.1. 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. The simulation results of control are shown from Figures 10–14:

**Figure 10.** Homing and docking trajectory of the AUV.

**Figure 11.** Time history of the position of the AUV.

**Figure 12.** Time history of the velocity of the AUV.

**Figure 13.** Time history of input force to control the AUV.

**Figure 14.** Time history of the in-plane thrust generated by each thruster of the AUV.

#### *6.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.

#### **7. 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.

**Funding:** This work was funded by the Open Foundation of the State Key Laboratory of Fluid Power and Mechatronic Systems, grant number GZKF-202001 and the Fundamental Research Funds for the Central Universities, grant number 2021XJJS016.

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

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

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

#### **References**