Motion Planning and Coordinated Control of Underwater Vehicle-Manipulator Systems with Inertial Delay Control and Fuzzy Compensator

Abstract

This paper presents new motion planning and robust coordinated control schemes for trajectory tracking of the underwater vehicle-manipulator system (UVMS) subjected to model uncertainties, time-varying external disturbances, payload and sensory noises. A redundancy resolution technique with a new secondary task and nonlinear function is proposed to generate trajectories for the vehicle and manipulator. In this way, the vehicle attitude and manipulator position are aligned in such a way that the interactive forces are reduced. To resist sensory measurement noises, an extended Kalman filter (EKF) is utilized to estimate the UVMS states. Using these estimates, a tracking controller based on feedback Linearization with both the joint-space and task-space tracking errors is proposed. Moreover, the inertial delay control (IDC) is incorporated in the proposed control scheme to estimate the lumped uncertainties and disturbances. In addition, a fuzzy compensator based on these estimates via IDC is introduced for reducing the undesired effects of perturbations. Trajectory tracking tasks on a five-degrees-of-freedom (5-DOF) underwater vehicle equipped with a 3-DOF manipulator are numerically simulated. The comparative results demonstrate the performance of the proposed controller in terms of tracking errors, energy consumption and robustness against uncertainties and disturbances.

1. Introduction

With increasing interest in the field of marine research, autonomous underwater vehicle manipulator systems (UVMSs) [1] have rapidly developed into important devices for exploring the ocean, completing underwater tasks, underwater sampling and so on. It is a challenging problem to accurately control the UVMS in an energy-efficient manner due to the kinematic redundancy and underwater environment with hydrodynamic uncertainties, unknown external disturbances (such as ocean currents) and inaccurate sensor information. For solving these problems, inverse kinematics and robust coordinated control techniques have been developed for the UVMS.

For the inverse kinematics of the UVMS, the solution can be obtained through mapping the end-effector’s velocities to the velocities of the vehicle and manipulator. As the UVMS has redundant degrees of freedom (DOFs), there are various combinations of vehicle and manipulator velocities without affecting the end-effector velocities. A common solution is to adopt the pseudo-inverse Jacobian matrix of the UVMS or its weighted form [2]. However, this method is not desirable for redundant exploration to avoid joint limits, improve system manipulability or save energy. Therefore, the task-priority redundancy resolution technique [3] was proposed in such a way that the fulfillment of the primary task has a higher priority than that of a secondary task. Generally, the secondary task is to optimize the performance index through assigning additional motion in the null space of the primary task. Sarkar and Podder [4] solved the inverse kinematics of the UVMS on the acceleration level to minimize the total hydrodynamic drag; however, the performance index of this method requires dynamic equations which can not be modeled exactly. Han et al. [5] proposed a new performance index designed to minimize restoring moments without using dynamic equations. However, this method was implemented for a specific configuration of the UVMS.

The task-priority strategy can be extensible to chain multiple tasks which have a lower order of priority (Siciliano and Slotine [6]). Antonelli et al. [7] used a fuzzy inference system (FIS) to handle multiple secondary tasks, such as reduction of fuel consumption and improvement of system manipulability. In such a way, a secondary task can be activated by FIS when the corresponding variable is without the safe range. Wang et al. [8] used a fuzzy logic algorithm to decide the priorities of secondary objectives, such as manipulator singularity avoidance and attitude optimization of the UVMS. The experimental validation of three difference kinematic control schemes was presented in [9]. In [10], a multitask kinematic control of the underwater biomimetic vehicle-manipulator system (UBVMS) was designed. A unifying framework for the kinematic control of UVMSs was proposed in [11]. A very recent work dedicated to motion planning for the UVMS was presented in [12].

To achieve trajectory tracking, it is very important to design a coordinated motion controller for the UVMS. The simple control methods (e.g., proportional-integral-derivative (PID) control) are not suitable for the UVMS due to the inherent nonlinear and coupled dynamics of the system [13,14,15]. Schjlberg and Fossen [16] proposed a control strategy in terms of feedback linearization. Sarkar and Podder [4] utilized a computed torque controller (CTC) for trajectory tracking of the UVMS. Taira et al. [17] proposed a model-based motion control for the UVMS, which can be applicable to three types of servo systems; i.e., a voltage-controlled, a torque-controlled and a velocity-controlled servo system. Korkmaz et al. [18] presented a trajectory tracking control for an underactuated underwater vehicle manipulator system (U-UVMS) based on the inverse dynamics. However, these model-based controllers are poor in terms of robustness against model uncertainties. In [19], a fuzzy logic control method was designed for a hybrid-driven UVMS to grasp marine products on the seabed. A model reference adaptive control approach for an UVMS was proposed in [20]. Antonelli et al. [21] proposed an adaptive controller based on virtual decomposition; however, a regressor matrix corresponding to parameter vector is required in this method. An indirect adaptive controller based on the extended Kalman filter (EKF) was proposed in [22]; meanwhile the performance would be degraded due to the estimated error via EKF. To eliminate the bias from the EKF estimation, Dai et al. [23] introduced a H∞ control in the indirect adaptive controller to achieve robust performance. However, this method results in a residual error when the bounds of the disturbance cannot be known prior. To reduce or omit the estimation error of the uncertainties and disturbances, a fuzzy compensator based on estimations was utilized [24]. To handle state and input constraints of the UVMS, a robust predictive control (RMPC) [24], a nonlinear MPC (NMPC) [25], a tube-based robust MPC [26], a fast MPC (FMPC) [27] and a fast tube MPC (FTMPC) [28] were used for the UVMS trajectory tracking, but these MPC approaches do not permit the self-motion utilized to perform energy efficient trajectory tracking.

For achieving precise and robust performance, the control design should be enhanced by the estimations of the system’s uncertainties and disturbances. The popular estimation techniques include time delay control (TDC) [29,30], the extended state observer (ESO) [31], the disturbance observer [32], the nonlinear disturbance observer (NDO) [33,34] the uncertainty and disturbance estimator (UDE) [35,36] (redefined as inertial delay control (IDC) [37]) and so on. Among them, because IDC is simple in design and easy to complete, it is widely used to estimate the effect of the lumped uncertainties and disturbances. Generally, the IDC is applied to the sliding mode control (SMC) for ensuring precise and robust performance. The combined method does not require the bounds of uncertainties and disturbances, and it does not use the discontinuous function in the control law.

However, the above-mentioned methods are based on joint-space variables, which may not be suitable for a variety of underwater tasks with high-precision end-effector position requirements [38]. These task space control schemes can easily adapt to the online modification of the end-effector’s motion [39,40]. However, the task-space controllers also have disadvantages. (a) The kinematic redundancy of the UVMS cannot be exploited. (b) The output of the task-space controller should be mapped into the joint space so as to be realized by thrusters and actuators. Li et al. [34] proposed a hybrid strategy-based coordinated controller for the UVMS. The hybrid strategy is to transform the joint-space controller (to exploit the system’s redundancy) to the task-space controller (to ensure high-accuracy tracking performance).

Inspired by the above studies, new motion planning and coordinated control schemes of the UVMS are proposed in this paper. The contribution of this work is that the proposed scheme can ensure precise, energy-efficient and robust performance in the presence of model uncertainties, external disturbances, payload and sensory noises. First, a new redundancy resolution technique is proposed, where a new secondary task with a nonlinear function is inserted for generating energy-saving trajectories for the vehicle and manipulator. Second, an EKF estimation system is employed for resisting sensory noises. Third, a coordinated motion control with joint-space errors, end-effector errors, IDC and a fuzzy compensator is proposed as a robust tracking controller against uncertainties and disturbances. Last, the effectiveness of the proposed scheme is verified through numerical simulations.

The rest of the paper is organized as follows. Section 2 is concerned with the kinematic and dynamic modeling of the UVMS. In Section 3, an improved redundancy resolution technique is presented. The proposed control scheme is proposed in Section 4. Numerical simulations and the detailed performance analysis are presented in Section 5. Section 6 holds the conclusions.

2. Modeling

The UVMS investigated in this paper is composed of an underwater vehicle with a 3 DOFs manipulator. The coordinate system of the UVMS is shown in Figure 1. In the body-fixed frame $\left\{B\right\}$, we define that the vectors of vehicle’s linear and angular velocities are ${\mathit{\nu }}_1$ and ${\mathit{\nu }}_2$, where ${\mathit{\nu }}_1={[u,v,w]}^T$, ${\mathit{\nu }}_2={[p,q,r]}^T$ and $\mathit{\nu }={[{\mathit{\nu }}_1^T,{\mathit{\nu }}_2^T]}^T$. The vector of joint positions is assumed to be $\mathit{q}={\left[\begin{array}{c}{q}_1,q_2\cdots q_n\end{array}\right]}^T$, where n is the number of manipulator’s joints. The position and orientation vector of the UVMS relative to the body-fixed frame is assumed to be $\mathit{\zeta }={[{\mathit{\nu }}_1^T,{\mathit{\nu }}_2^T,{\dot{\mathit{q}}}^T]}^T$. In the inertial frame $\left\{I\right\}$, the vectors of end-effector’s position and orientation are defined as ${\mathit{\eta }}_{E1}$ and ${\mathit{\eta }}_{E2}$, and assume ${\mathit{x}}_E={[{\mathit{\eta }}_{E1}^T,{\mathit{\eta }}_{E2}^T]}^T$.

The kinematic model of the UVMS [21] can be obtained as shown in (1), where the velocities of the UVMS in the body-fixed frame ($\mathit{\zeta }$) are mapped into end-effector velocities (${\dot{\mathit{x}}}_E$) via $\mathit{J}({\mathit{R}}_B^I,\mathit{q})$.

$$\begin{array}{c}\hfill {\dot{\mathit{x}}}_E=\left[\begin{array}{ccc}{\mathit{R}}_B^I& \mathit{S}({\mathit{R}}_B^I{\mathit{p}}_0^B+{\mathit{R}}_0^I{\mathit{p}}_E^0){\mathit{R}}_B^I& {\mathit{J}}_{\mathrm{pos},\mathrm{q}}^I\\ {\mathit{0}}_{3\times 3}& {\mathit{R}}_B^I& {\mathit{J}}_{\mathrm{ori},\mathrm{q}}^I\end{array}\right]{\left[\begin{array}{c}{\mathit{\nu }}_1^T,{\mathit{\nu }}_2^T,{\dot{\mathit{q}}}^T\end{array}\right]}^T={\mathit{J}}_{(}{\mathit{R}}_B^I,\mathit{q})\mathit{\zeta }\end{array}$$

(1)

where $\mathit{J}({\mathit{R}}_B^I,\mathit{q})\in {\mathbb{R}}^{6\times (6+n)}$, ${\mathit{R}}_0^I={\mathit{R}}_B^I{\mathit{R}}_0^B$, ${\mathit{J}}_{\mathrm{pos},\mathrm{q}}^I={\mathit{R}}_0^I{\mathit{J}}_{\mathrm{pos},\mathrm{q}}^0,$ and ${\mathit{J}}_{\mathrm{ori},\mathrm{q}}^I={\mathit{R}}_0^I{\mathit{J}}_{\mathrm{ori},\mathrm{q}}^0$. ${\mathit{R}}_0^B$ is the rotational matrix describing the transformation from the manipulator’s base frame to the body-fixed frame, and ${\mathit{p}}_E^0$ is the position vector from manipulator’s base to the center of the body-fixed frame. ${\mathit{p}}_E^0$ is the position vector from end-effector to the manipulator’s base. ${\mathit{J}}_{\mathrm{pos},\mathrm{q}}^0$ is the manipulator’s linear Jacobian matrix, and ${\mathit{J}}_{\mathrm{ori},\mathrm{q}}^0$ is the manipulator’s angular Jacobian matrix. $\mathit{S}(·)$ is the cross-product operator.

The vectors of vehicle’s position and attitude relative to the inertial frame are defined as ${\mathit{\eta }}_1$ and ${\mathit{\eta }}_2$, where ${\mathit{\eta }}_1={[x,y,z]}^T$, ${\mathit{\eta }}_2={[\varphi ,\theta ,\psi ]}^T$ and $\mathit{\eta }={[{\mathit{\eta }}_1^T,{\mathit{\eta }}_2^T]}^T$. The velocity vector of the UVMS defined in the body-fixed frame ($\mathit{\zeta }$) can be obtained by (2).

$$\begin{array}{cc}\hfill \mathit{\zeta }& =\left[\begin{array}{ccc}{\mathit{R}}_I^B& {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}\\ {\mathit{0}}_{3\times 3}& {\mathit{J}}_{\nu }& {\mathit{0}}_{3\times 3}\\ {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{\eta }}}_1\\ {\dot{\mathit{\eta }}}_2\\ \dot{\mathit{q}}\end{array}\right]={\mathit{J}}_{\xi }\left({\mathit{\eta }}_2\right)\dot{\mathit{\xi }}\hfill \end{array}$$

(2)

where ${\mathit{\nu }}_1={\mathit{R}}_I^B{\dot{\mathit{\eta }}}_1$, ${\mathit{\nu }}_2={\mathit{J}}_v{\dot{\mathit{\eta }}}_2$ and $\mathit{\xi }={[{\mathit{\eta }}_1^T,{\mathit{\eta }}_2^T,{\mathit{q}}^T]}^T$. ${\mathit{R}}_I^B$ is the linear rotational matrix describing the transformation from the inertial frame to the body-fixed frame, and ${\mathit{J}}_{\nu }$ is the angular rotational matrix. The values of ${\mathit{R}}_I^B$ and ${\mathit{J}}_{\nu }$ can be referred to the literature [41]. ${\mathit{J}}_{\xi }\left({\mathit{\eta }}_2\right)$ is the Jacobian matrix which relates the vehicle velocities with respect to the inertial frame and the body-fixed frame.

Dynamic Modeling

The nonlinear dynamic equations of the UVMS expressed in the body-fixed frame $\left\{B\right\}$ can be established as [16,21]:

$$\begin{array}{c}\hfill \mathit{M}(\mathit{q},\mathit{\eta })\dot{\mathit{\zeta }}+\mathit{C}(\mathit{q},\mathit{\zeta })\mathit{\zeta }+\mathit{D}(\mathit{q},\mathit{\zeta })\mathit{\zeta }+\mathit{g}(\mathit{q},{\mathit{R}}_I^B)=\mathit{\tau }+{\mathit{\tau }}_{dis}\end{array}$$

(3)

where

$$\mathit{M}(\mathit{q},\mathit{\eta })=\left[\begin{array}{cc}{\mathit{M}}_{\mathit{\nu }}+{\mathit{H}}_{\mathit{1}}\left(\mathit{q}\right)& {\mathit{H}}_2\left(\mathit{q}\right)\\ {\mathit{H}}_2^T\left(\mathit{q}\right)& {\mathit{M}}_m\left(\mathit{q}\right)\end{array}\right],\mathit{C}(\mathit{q},\mathit{\zeta })=\left[\begin{array}{cc}{\mathit{C}}_{\mathit{\nu }}\left(\mathit{\nu }\right)+{\mathit{C}}_1(\mathit{q},\dot{\mathit{q}},\mathit{\nu })& {\mathit{C}}_2(\mathit{q},\dot{\mathit{q}})\\ {\mathit{C}}_3(\mathit{q},\dot{\mathit{q}},\mathit{\nu })& {\mathit{C}}_m(\mathit{q},\dot{\mathit{q}})\end{array}\right]$$

$$\mathit{D}(\mathit{q},\mathit{\zeta })=\left[\begin{array}{cc}{\mathit{D}}_{\mathit{\nu }}\left(\mathit{\nu }\right)+{\mathit{D}}_1(\mathit{q},\dot{\mathit{q}},\mathit{\nu })& {\mathit{D}}_2(\mathit{q},\dot{\mathit{q}},\mathit{\nu })\\ {\mathit{D}}_3(\mathit{q},\dot{\mathit{q}},\mathit{\nu })& {\mathit{D}}_m\left(\mathit{q}\right)+{\mathit{D}}_4(\mathit{q},\dot{\mathit{q}},\mathit{\nu })\end{array}\right],\mathit{g}(\mathit{q},{\mathit{R}}_I^B)=\left[\begin{array}{c}{\mathit{g}}_{\mathit{\nu }}\left(\mathit{\eta }\right)+{\mathit{g}}_1\left(\mathit{q}\right)\\ {\mathit{g}}_m\left(\mathit{q}\right)\end{array}\right],\mathit{\tau }={[{\mathit{\tau }}_{\nu }^T,{\mathit{\tau }}_m^T]}^T$$

where $\mathit{M}(\mathit{q},\mathit{\eta })\in {\mathbb{R}}^{(6+n)\times (6+n)}$ is the inertia matrix including added mass terms, and ${\mathit{H}}_1\left(\mathit{q}\right)$ and ${\mathit{H}}_2\left(\mathit{q}\right)$ are matrices of the inertia effects due to the manipulator. $\mathit{C}(\mathit{q},\mathit{\zeta })\in {\mathbb{R}}^{(6+n)}$ is the Coriolis and centripetal matrix, and ${\mathit{C}}_i(\mathit{q},\dot{\mathit{q}},\mathit{\nu })$(i = 1,3)/${\mathit{C}}_2(\mathit{q},\dot{\mathit{q}})$ is the matrix of Coriolis and centripetal forces due to the coupling effects/due to the manipulator. $\mathit{D}(\mathit{q},\mathit{\zeta })\mathit{\zeta }\in {\mathbb{R}}^{(6+n)}$ is the vector of dissipative effects, and ${\mathit{D}}_i(\mathit{q},\dot{\mathit{q}},\mathit{\nu })(i=1\cdots 4)$ is the matrix of drag effects due to the coupling effects. $\mathit{g}(\mathit{q},{\mathit{R}}_B^I)\in {\mathbb{R}}^{(6+n)}$ is the vector of gravity and buoyancy effects, ${\mathit{g}}_{\mathit{\nu }}\left(\mathit{\eta }\right)$ is the restoring vector of the vehicle, ${\mathit{g}}_m\left(\mathit{q}\right)$ is the restoring vector of the manipulator and ${\mathit{g}}_1\left(\mathit{q}\right)$ is the restoring vector due to the manipulator. $\mathit{\tau }\in {\mathbb{R}}^{(6+n)}$ is the vector of generalized forces. ${\mathit{\tau }}_{dis}$ is the vector of disturbances. Generally, in a deep water environment, ${\mathit{\tau }}_{dis}$ comes from ocean currents, payload, etc. In particular, time-varying ocean currents increase the uncertainty of the UVMS hydrodynamic forces, making accurate control of the UVMS difficult.

As for the underwater manipulator, it is assumed that its links are composed of cylindrical elements. The hydrodynamic effects on cylinders can be referred to [16]. For a cylinder, the inertial matrix of added mass and added moment is a diagonal matrix, while the off-diagonal elements are neglected. The drag force can be expressed by a nonlinear function related to the velocity vector of the center of mass of the link. Generally, when calculating the hydrodynamic forces, the linear skin-friction force, quadratic drag force and lift force are considered. The third-order and higher order terms of the drag forces are neglected. In addition, based on the assumption that velocity of the ocean current is constant, the diffraction forces can be neglected.

In the real system, the above model parameters are usually difficult to accurately measure or estimate, especially the hydrodynamic forces acting on the UVMS. Thus, it is advisable to divide the model parameters into two parts: the normal value part and the bias part. The normal value is denoted as ${(·)}^{*}$, which can be obtained through using strip theory, pool experiment analysis or CFD computation. The bias term is denoted as Δ(·), which describes the difference between the real value and the nominal value. Then, (4) can be obtained. For control design, the normal values are available, while the bias parts are considered as model parameter uncertainties.

$$\begin{array}{c}\hfill \mathit{M}={\mathit{M}}^{*}+\Delta \mathit{M},\mathit{C}={\mathit{C}}^{*}+\Delta \mathit{C},\mathit{D}={\mathit{D}}^{*}+\Delta \mathit{D}\end{array}$$

(4)

Considering that the vehicle is driven by thrusters and the manipulator is driven by motors, the generalized force vector $\mathit{\tau }$ is related to the vector of thruster forces and actuator torques ${\mathit{F}}_{td}$ through (5).

$$\begin{array}{c}\hfill \mathit{\tau }=\left[\begin{array}{cc}{\mathit{B}}_{\nu }& {\mathit{0}}_{6\times n}\\ {\mathit{0}}_{n\times p_{\nu }}& {\mathit{I}}_n\end{array}\right]{\mathit{F}}_{td}=\mathit{B}{\mathit{F}}_{td}\end{array}$$

(5)

where ${\mathit{F}}_{td}={[{\mathit{T}}^T,{\mathit{\tau }}_m^T]}^T\in {\mathbb{R}}^{p_{\nu }+n}$. $\mathit{T}\in {\mathbb{R}}^{p_{\nu }}$ represents the vector of thruster forces, and ${\mathit{\tau }}_m\in {\mathbb{R}}^n$ represents the vector of actuator torques. ${\mathit{B}}_{\nu }\in {\mathbb{R}}^{6\times p_{\nu }}$ is the thruster configuration matrix, and $\mathit{B}\in {\mathbb{R}}^{(6+n)\times (6+n)}$ is the thruster-actuator configuration matrix. It is known that for an under-actuated underwater vehicle, $p_{\nu }<6$. Generally, for a manipulator, n joint motors are all available.

3. Proposed Redundancy Resolution

This section proposes a new redundancy resolution technique to generate energy-efficient trajectories for the vehicle and the manipulator. It is known that infinite solutions of the UVMS inverse kinematics can be obtained by inverting the mapping (1). The solution using the pseudo inverse of the Jacobian matrix is expressed as [2]

$$\begin{array}{cc}\hfill \mathit{\zeta }& ={\mathit{J}}^{+}({\mathit{R}}_B^I,\mathit{q}){\dot{\mathit{x}}}_E\hfill \end{array}$$

(6)

where ${\dot{\mathit{x}}}_E$ is the end-effector velocity vector. ${\mathit{J}}^{+}({\mathit{R}}_B^I,\mathit{q})$ is the pseudo inverse of the Jacobian matrix and ${\mathit{J}}^{+}({\mathit{R}}_B^I,\mathit{q})={\mathit{J}}^T({\mathit{R}}_B^I,\mathit{q}){\left(\mathit{J}({\mathit{R}}_B^I,\mathit{q}){\mathit{J}}^T({\mathit{R}}_B^I,\mathit{q})\right)}^{-1}$.

However, this solution does not exploit the redundant DOFs of the system, and it is not suitable from the perspective of energy consumption. Therefore, a new task-priority redundancy resolution technique is proposed in this section. In the proposed technique, the primary task is to map the end-effector variables into the joint-space variables, and two secondary tasks are provided to explore the kinematic redundancy for energy savings, joint limit avoidance and small roll and pitch angles kept for the vehicle, as shown in (7).

$$\begin{array}{c}\hfill {\mathit{\zeta }}_d={\mathit{J}}_W^{+}({\dot{\mathit{x}}}_{Ed}-{\mathit{K}}_f{\mathit{e}}_E)+(\mathit{I}-{\mathit{J}}_W^{+}{\mathit{J}}_W)[{\mathit{J}}_s^{+}(\mathit{\eta },\mathit{q})({\dot{\mathit{\eta }}}_{sd}-{\mathit{K}}_s{\mathit{e}}_s)-\alpha {\mathit{K}}_{\zeta }{\mathit{J}}_{\xi }\left({\mathit{\eta }}_2\right)\dot{\mathit{\xi }}]\end{array}$$

(7)

where ${\mathit{J}}_W^{+}={\mathit{W}}^{-1}{\mathit{J}}^T{\left(\mathit{J}{\mathit{W}}^{-1}{\mathit{J}}^T\right)}^{-1}$ is the weighted pseudo-inverse Jacobian matrix. $\mathit{J}$ is considered as the primary task Jacobian matrix and $\mathit{W}\in {\mathbb{R}}^{(6+n)\times (6+n)}$ is the motion distribution matrix with elements belonging to $[0,1]$. When the diagonal elements of the former three rows of $\mathit{W}$ are close to 1, the diagonal elements of the later n rows of $\mathit{W}$ will be close to 0. This results in greater movement of the vehicle and less movement of the manipulator. Otherwise, when the diagonal elements of the former three rows of $\mathit{W}$ are close to 0, the diagonal elements of the later n rows of $\mathit{W}$ will be close to 1. This results in less movement of the vehicle and greater movement of the manipulator. The diagonal elements of the middle three rows of $\mathit{W}$ correspond to the movement of the vehicle’s attitude. The larger they are, the greater the movement of the vehicle’s attitude. The off-diagonal elements of $\mathit{W}$ describe the degrees of the coupling effects between the DOFs of the UVMS, which can refer to our previous work [15]. The closer the off-diagonal element of $\mathit{W}$ to 1, the greater the corresponding coupling motion. ${\mathit{J}}_s(\mathit{\eta },\mathit{q})$ and ${\mathit{J}}_{\xi }\left({\mathit{\eta }}_2\right)$ are the secondary task Jacobian matrices. It can be recognized that the secondary tasks are fulfilled in the null space, which will not affect the motion of the primary task. Moreover, the two secondary tasks have the same lower priority relative to the primary task. ${\dot{\mathit{x}}}_{Ed}$ is the primary-task vector and ${\mathit{e}}_E={\mathit{x}}_E-{\mathit{x}}_{Ed}$ is the error of the primary task. ${\dot{\mathit{\eta }}}_{sd}$ is a secondary-task vector to achieve system coordination between its rotational subsystem and translational subsystem, including the vehicle attitudes and joint angles, and ${\mathit{e}}_s={\dot{\mathit{\eta }}}_s-{\dot{\mathit{\eta }}}_{sd}$ is its error. ${\mathit{K}}_f$ and ${\mathit{K}}_s$ are positive definite matrices. The other secondary task vector is the velocity vector of the UVMS $\dot{\mathit{\xi }}={[{\dot{\mathit{\eta }}}_1^T,{\dot{\mathit{\eta }}}_2^T,{\dot{\mathit{q}}}^T]}^T$, which contributes to the system’s self-motion utilized for reducing energy requirements. ${\mathit{K}}_{\zeta }$ is a diagonal matrix whose elements belong to $[0,\infty )$. The larger the diagonal element of ${\mathit{K}}_{\zeta }$, the greater the corresponding coupling motion. For instance, if the diagonal element of the secondary row of ${\mathit{K}}_{\zeta }$ is larger, the vehicle will have a larger yaw angle according to the coupling effects. Similarly, if the diagonal element of the third row of ${\mathit{K}}_{\zeta }$ is larger, the vehicle will have a larger pitch angle. $\alpha$ is a coefficient belonging to $[0,1]$, which is used to adjust the values of ${\mathit{K}}_{\zeta }$.

To effectively utilize the self-motion during the entire UVMS motion, $\alpha$ is defined as a nonlinear function related to time $t\ge 0$, as given in (8).

$$\begin{array}{c}\hfill \alpha =\left\{\begin{array}{cc}-0.5(1-e^{-\lambda (t-t_s)})+0.5\hfill & t>t_s\hfill \\ 0.5(1-e^{-\lambda (t-t_s)})+0.5\hfill & t\le t_s\hfill \end{array}\right.\end{array}$$

(8)

where $t_s$ relatives to the time at which the system enters deceleration phase. $\lambda$ is the coefficient and $\lambda >0$.

The curve of the nonlinear function is shown in Figure 2. It can be recognized that the smaller the value of $\lambda$, the smoother the variations of the nonlinear function. Therefore, it is better to choose a small value of $\lambda$ to ensure smooth movement of the UVMS.

4. Control Design

The purpose of control design is to obtain the values of thruster forces and actuator torques in order to drive the UVMS to the desired trajectory. In addition, the robustness of the designed controller is important in the presence of model parameter uncertainties, time-varying external disturbances, payload variations and sensory noises. In this section, for precise and robust control of the UVMS, a new coordinated motion controller including inertial delay control (IDC) and a fuzzy compensator is proposed. Besides, the proposed controller uses the estimated UVMS states via an EKF.

4.1. Design of an EKF

Due to the presence of sensory measurement noises, the vehicle and manipulator positions measured by sensors are not inaccurate. Therefore, it is necessary to utilize a nonlinear filter to estimate the system’s states. As the extended Kalman filter (EKF) is simple and easy to complete and has low computational complexity, the EKF is used in this study. It is necessary to obtain a linear model during the KF design process. The dynamic equations of the UVMS can be linearized by ignoring the higher order terms in the expended Taylor series. The state vector of the system, e.g., position/attitude and velocity vectors, is defined as $\mathit{X}={[{\mathit{\xi }}^T,{\mathit{\zeta }}^T]}^T$. The measurement model can be expressed as $\mathit{Z}=\mathit{h}\left(\mathit{X}\right)=\mathit{\xi }$.

Based on (2) and (3), the time derivative of the system state vector $\mathit{X}$ can be obtained as

$$\begin{array}{c}\hfill \dot{\mathit{X}}=f(\mathit{X},t)=\left[\begin{array}{c}{\mathit{J}}_{\xi }^{-1}\left({\mathit{\eta }}_2\right)\dot{\mathit{\zeta }}\\ {\mathit{M}}^{-1}(\mathit{\tau }-{\mathit{\tau }}_d-\mathit{b})\end{array}\right]\end{array}$$

(9)

where $f(\mathit{X},t)$ is considered as the estimated model of the system.

With the additive Gaussian white noise, the predicted system state vector ${\widehat{\mathit{X}}}_{k+1}^{-}$ at $t=t_{k+1}$ is given in (10), and the predicted measurement state vector ${\mathit{Z}}_{k+1}^{-}$ at $t=t_{k+1}$ is shown in (11). Then the covariance matrix of the predicted state vector can be obtained as shown in (12).

$$\begin{array}{cc}\hfill {\widehat{\mathit{X}}}_{k+1}^{-}& ={\widehat{\mathit{X}}}_k+f({\widehat{\mathit{X}}}_k,t)+{\mathit{Q}}_k\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\mathit{Z}}_{k+1}^{-}& =\mathit{h}\left({\widehat{\mathit{X}}}_{k+1}^{-}\right)+{\mathsf{\Gamma }}_k\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {\mathit{P}}_{k+1}^{-}& ={\mathit{\varphi }}_k{\mathit{P}}_k{\mathit{\varphi }}_k^T+{\mathit{Q}}_k\hfill \end{array}$$

(12)

where ${\widehat{\mathit{X}}}_k$ is the predicted state vector at $t_k$. ${\mathit{Q}}_k$ is the vector of system noises. ${\mathsf{\Gamma }}_k$ is the vector of measurement noises. ${\mathit{P}}_k$ is the covariance matrix of the estimated system states.

Therefore, the estimated system states can be obtained through the following correction step:

$$\begin{array}{cc}\hfill {\mathit{K}}_{k+1}& ={\mathit{P}}_{k+1}^{-}{\mathit{H}}_{k+1}^T{({\mathit{H}}_{k+1}{\mathit{P}}_{k+1}^{-}{\mathit{H}}_{k+1}^T+{\mathsf{\Gamma }}_{k+1})}^{-1}\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\widehat{\mathit{X}}}_{k+1}& ={\widehat{\mathit{X}}}_{k+1}^{-}+{\mathit{K}}_{k+1}({\mathit{Z}}_{k+1}-{\mathit{Z}}_{k+1}^{-})\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\mathit{P}}_{k+1}& =(\mathit{I}-{\mathit{K}}_{k+1}{\mathit{H}}_{k+1}){\mathit{P}}_{k+1}^{-}\hfill \end{array}$$

(15)

where ${\mathit{\varphi }}_k=\frac{\partial f(\mathit{X},t)}{\partial {\mathit{X}}^T}{|}_{\mathit{X}={\widehat{\mathit{X}}}_k}$ and ${\mathit{H}}_{k+1}=\frac{\partial h\left(\mathit{X}\right)}{\partial {\mathit{X}}^T}{|}_{\mathit{X}={\widehat{\mathit{X}}}_{k+1}^{-}}.$

4.2. Design of a Tracking Controller

The control objective is to make sure that the tracking errors quickly converge to zero under the conditions of model parameter uncertainties, time-varying external disturbances and payload. First, the tracking errors of the system are defined. The vector of end-effector tracking errors is shown in (16), and the vectors of tracking errors in the joint space are shown in (17)–(19).

$$\begin{array}{cc}\hfill & {\mathit{e}}_E={\widehat{\mathit{x}}}_E-{\mathit{x}}_{Ed}\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill & \mathit{e}=\left[\begin{array}{c}{\mathit{R}}_I^B({\widehat{\mathit{\eta }}}_1-{\mathit{\eta }}_{1d})\\ \eta {\mathit{\epsilon }}_d-\eta {\mathit{\epsilon }}_d+\mathit{S}\left(\mathit{\epsilon }\right){\mathit{\epsilon }}_d\\ \widehat{\mathit{q}}-{\mathit{q}}_d\end{array}\right]\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill & \dot{\mathit{e}}=\widehat{\mathit{\zeta }}-{\mathit{\zeta }}_d\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill & \ddot{\mathit{e}}=\dot{\widehat{\mathit{\zeta }}}-{\dot{\mathit{\zeta }}}_d\hfill \end{array}$$

(19)

where the superscript $\left(\widehat{·}\right)$ denotes the corresponding estimated values via EKF, and the subscript ${(·)}_d$ denotes the corresponding desired values. $[\mathit{\epsilon },\eta ]$ and $[{\mathit{\epsilon }}_d,{\eta }_d]$ are the quaternions of ${\widehat{\mathit{\eta }}}_2$ and ${\mathit{\eta }}_{2d}$.

Then, the tracking controller based on feedback linearization is given as

$$\begin{array}{c}\hfill \mathit{u}=-{\mathit{M}}^{*}({\mathit{k}}_1\mathit{e}+{\mathit{k}}_2\dot{\mathit{e}}-{\dot{\mathit{\zeta }}}_d)+{\mathit{C}}^{*}\widehat{\mathit{\zeta }}+{\mathit{D}}^{*}\widehat{\mathit{\zeta }}+\mathit{g}-{\mathit{J}}^T{\mathit{k}}_E{\mathit{e}}_E-\widehat{\mathit{\delta }}\end{array}$$

(20)

where $\mathit{J}=\mathit{J}({\mathit{R}}_B^I,\mathit{q})$ is the Jacobian matrix of the system, as given in (1). ${\mathit{k}}_1$, ${\mathit{k}}_2$ and ${\mathit{k}}_E$ are the positive symmetric matrices. $\widehat{\mathit{\delta }}$ is the estimated vector of the lumped uncertainties and disturbances, which is described in the next subsection.

4.3. Inertial Delay Control (IDC)

Due to the underwater circumstances, the dynamic equations of the UVMS include unknown external disturbances and an amount of parameter uncertainties caused by identification errors. These lumped uncertainties and disturbances can be expressed as (21) with reference to (3) and (4).

$$\begin{array}{c}\hfill \mathit{\delta }=-(\Delta \mathit{M}\dot{\widehat{\mathit{\zeta }}}+\Delta \mathit{C}\widehat{\mathit{\zeta }}+\Delta \mathit{D}\widehat{\mathit{\zeta }})+{\mathit{\tau }}_{dis}\end{array}$$

(21)

where $\dot{\widehat{\mathit{\zeta }}}$ and $\widehat{\mathit{\zeta }}$ denote the estimates of the system states via EKF.

Then, based on (3), (4) and (21), the acceleration vector of the system can be obtained as

$$\begin{array}{c}\hfill \dot{\widehat{\mathit{\zeta }}}=-{\left({\mathit{M}}^{*}\right)}^{-1}({\mathit{C}}^{*}\widehat{\mathit{\zeta }}+{\mathit{D}}^{*}\widehat{\mathit{\zeta }}+\mathit{g})+{\left({\mathit{M}}^{*}\right)}^{-1}(\mathit{u}+\mathit{\delta })\end{array}$$

(22)

Substituting the proposed control law (20) in (22), dynamical equation of the tracking errors is

$$\begin{array}{c}\hfill \ddot{\mathit{e}}+{\mathit{k}}_2\dot{\mathit{e}}+{\mathit{k}}_1\mathit{e}+{\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{J}}^T{\mathit{k}}_E{\mathit{e}}_E={\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{e}}_{\delta }\end{array}$$

(23)

where ${\mathit{e}}_{\delta }=\mathit{\delta }-\widehat{\delta }$ is the estimated error vector.

It is assumed that a slow-varying signal can be approximated and estimated by a filter with appropriate bandwidth [37]. Based on this assumption, the uncertainty and disturbance estimator (UDE) is proposed for estimating slow-varying uncertainties [35,37]. Then, the estimations of lumped uncertainties and disturbances $\widehat{\mathit{\delta }}$ can be given as

$$\begin{array}{c}\hfill \widehat{\mathit{\delta }}={\mathit{G}}_f\left(\mathit{s}\right)\mathit{\delta }\end{array}$$

(24)

where ${\mathit{G}}_f\left(\mathit{s}\right)$ is a strictly proper low-pass filter possessing a uniform steady-state gain and a sufficiently large bandwidth. Based on (24), it is found that by passing the lumped uncertainties and disturbances $\mathit{\delta }$ through a inertial filter ${\mathit{G}}_f\left(\mathit{s}\right)$, the estimation vector $\widehat{\mathit{\delta }}$ can be obtained. The UDE method is redefined as inertial delay control (IDC) [37], because it is analogous to the time delay control (TDC) which delays the plant signals in time to obtain the estimates.

Based on (23) and (24), we can obtain

$$\begin{array}{c}\hfill \widehat{\mathit{\delta }}={\mathit{G}}_f\left(\mathit{s}\right)[{\mathit{M}}^{*}(\ddot{\mathit{e}}+{\mathit{k}}_2\dot{\mathit{e}}+{\mathit{k}}_1\mathit{e})+{\mathit{J}}^T{\mathit{k}}_E{\mathit{e}}_E+\widehat{\mathit{\delta }}]\end{array}$$

(25)

A choice of ${\mathit{G}}_f\left(\mathit{s}\right)$ with first order is given by

$$\begin{array}{c}\hfill {\mathit{G}}_f\left(\mathit{s}\right)=\frac{\mathit{I}}{\mathit{I}+\mathit{T}\mathit{s}}\end{array}$$

(26)

where $\mathit{T}$ is a diagonal matrix with small positive constant. $\mathit{I}$ is the identity matrix.

Then (25) can be rewritten as

$$\begin{array}{c}\hfill \mathit{T}\dot{\widehat{\mathit{\delta }}}+\widehat{\mathit{\delta }}={\mathit{M}}^{*}(\ddot{\mathit{e}}+{\mathit{k}}_2\dot{\mathit{e}}+{\mathit{k}}_1\mathit{e})+{\mathit{J}}^T{\mathit{k}}_E{\mathit{e}}_E+\widehat{\mathit{\delta }}\end{array}$$

(27)

Therefore, the estimates of the lumped uncertainties and disturbances can be obtained as

$$\begin{array}{c}\hfill \widehat{\mathit{\delta }}={\mathit{T}}^{-1}{\mathit{M}}^{*}(\dot{\mathit{e}}+{\mathit{k}}_2\mathit{e}+{\mathit{k}}_1{\int }_0^t\mathit{e}dt)+{\mathit{T}}^{-1}{\mathit{J}}^T{\mathit{k}}_E{\int }_0^t{\mathit{e}}_{E}dt\end{array}$$

(28)

From (25) and (26), the equation of estimated errors can be written as

$$\begin{array}{c}\hfill {\dot{\mathit{e}}}_{\delta }=-{\mathit{T}}^{-1}{\mathit{e}}_{\delta }+\dot{\mathit{\delta }}\end{array}$$

(29)

If the lumped uncertainties and disturbances $\mathit{\delta }$ are slowly varying, then $\dot{\mathit{\delta }}$ is small and $\dot{\mathit{\delta }}\approx \mathit{0}$. Therefore, the estimated errors (${\mathit{e}}_{\delta }$) go to zero asymptotically. If $\dot{\mathit{\delta }}$ is not small, but $\ddot{\mathit{\delta }}$ is small, ${\mathit{e}}_{\delta }$ is ultimately bounded and the estimated accuracy can be improved by estimating $\mathit{\delta }$ and $\dot{\mathit{\delta }}$.

4.4. Fuzzy Compensator

Based on the estimates via IDC, the fuzzy compensator is given as

$$\begin{array}{c}\hfill {\mathit{u}}_{fuzzy}=\mathit{\rho }\widehat{\mathit{\delta }}+\mathit{ϵ}\end{array}$$

(30)

where $\mathit{\rho }=\mathrm{diag}({\rho }_1,{\rho }_2\cdots {\rho }_{6+n})$ is the parameter of the fuzzy compensator, and $\mathit{ϵ}$ is a constant vector.

The fuzzy compensator is a multiple-inputs-single-output fuzzy logic controller (FLC) with the joint-space system errors $e_i$ and $e_j$ as two input variables and ${\rho }_i$ after defuzzification and denormalization as an output variable. Denote the system error vector $\mathit{e}$ (as given in (17)) as $\mathit{e}=[e_1,e_2\cdots e_i\cdots e_{6+n}]$. The main advantage of this fuzzy compensator is that the required fuzzy rules take the dynamic coupling between the vehicle and the manipulator [15,16] into account. It is known that the roll, pitch and yaw motions of the vehicle are coupled with its surge, sway and heave motions. As the roll and pitch angles should be kept small for properly working of the bottom sensors, it is assumed that the surge and sway motions are mostly affected by the yaw angle. Note that the pitch and heave motions are interactive, and the manipulator’s joints 2 and 3 are interactive. The position of manipulator’s joint 1 is mostly affected by the sway motion. Based on these analysis, the fuzzy rules are given in

Table 1. Rule base for ${\rho }_i$.

		$|{\mathit{e}}_{\mathit{j}}|$	$\mathbf{ZE}$	$\mathbf{PS}$	$\mathbf{PM}$	$\mathbf{PB}$
	${\mathit{\rho }}_{\mathit{i}}$
$|{\mathit{e}}_{\mathit{i}}|$
$\mathrm{ZE}$			$\mathrm{ZE}$	$\mathrm{ZE}$	$\mathrm{PS}$	$\mathrm{PM}$
$\mathrm{PS}$			$\mathrm{PS}$	$\mathrm{PS}$	$\mathrm{PM}$	$\mathrm{PB}$
$\mathrm{PM}$			$\mathrm{PM}$	$\mathrm{PM}$	$\mathrm{PB}$	$\mathrm{PB}$
$\mathrm{PB}$			$\mathrm{PB}$	$\mathrm{PB}$	$\mathrm{PB}$	$\mathrm{PB}$

.

Table 2. Relationships between two input variables and an output variable.

$\mathrm{inputs}$	$|e_i|$	$|e_1|$	$|e_2|$	$|e_3|$	$|e_5|$	$|e_6|$	$|e_7|$	$|e_8|$	$|e_9|$
	$|e_j|$	$|e_6|$	$|e_6|$	$|e_5|$	$|e_3|$	$|e_2|$	$|e_2|$	$|e_9|$	$|e_8|$
$\mathrm{outputs}$	${\rho }_i$	${\rho }_1$	${\rho }_2$	${\rho }_3$	${\rho }_5$	${\rho }_6$	${\rho }_7$	${\rho }_8$	${\rho }_9$

shows the relationships between an output and two input variables.

The following symbols are used in Table 1: ZE (zeros), PS (positive small), PM (positive medium), PB (positive big). Figure 3a,b shows the member functions of the normalized input and output variables respectively. After the fuzzification stage, the Mamdani inference method is used for fuzzy implication, and then the centroid method is used for defuzzification. Finally, based on denormalization the actual output variables can be obtained.

Incorporating the fuzzy compensator, the proposed coordinated motion controller is given as

$$\begin{array}{c}\hfill {\mathit{u}}_c=\mathit{u}-{\mathit{u}}_{fuzzy}\end{array}$$

(31)

Then we can obtain the vector of thruster forces and actuator torques ${\mathit{F}}_{td}$, as shown in (32).

$$\begin{array}{c}\hfill {\mathit{F}}_{td}={\mathit{B}}^{+}{\mathit{u}}_c=\left[\begin{array}{cc}{\mathit{B}}_{\nu }^{+}& {\mathit{0}}_{p_{\nu }\times n}\\ {\mathit{0}}_{n\times 6}& {\mathit{I}}_n\end{array}\right]{\mathit{u}}_c\end{array}$$

(32)

where ${\mathit{B}}_{\nu }^{+}$ is the pseudo inverse of ${\mathit{B}}_{\nu }$ and ${\mathit{B}}^{+}$ is the pseudo inverse of $\mathit{B}$.

Therefore, the generalized force vector $\mathit{\tau }$ can be obtained based on (5). For an under-actuated UVMS, $\mathit{\tau }=\mathit{u}$ except that the elements of $\tau$ corresponding to the underacted motions are zeros.

The proposed control system is schematically represented by a block diagram in Figure 4. The controller block includes five sub blocks to calculate a control vector; i.e., the tracking controller, IDC, fuzzy compensator, ${\mathit{B}}^{+}$ and $\mathit{B}$ blocks. In addition to system dynamics, the tracking controller also requires tracking errors of end-effector positions and joint-space states. The end-effector position tracking errors are calculated according to the desired end-effector positions derived from the trajectory planning block and the estimated end-effector positions obtained from the forward kinematics block using the estimated joint-space states. The estimates of joint-space states are obtained from the EKF block. The proposed redundancy resolution block generates the required joint-space trajectories for the desired tasks. The IDC block estimates the lumped uncertainties and disturbances of the system. The fuzzy compensator reduces the influences of perturbation on the UVMS.

4.5. Stability Analysis

We define a Lyapunov function which is positive definite as:

$$\begin{array}{c}\hfill V(\mathit{e},\dot{\mathit{e}},{\mathit{e}}_E,{\mathit{e}}_{\delta })=\frac{1}{2}{\mathit{e}}^T{\mathit{k}}_1{\mathit{k}}_2\mathit{e}+\frac{1}{2}{\dot{\mathit{e}}}^T{\mathit{k}}_2\dot{\mathit{e}}+\frac{1}{2}{{\mathit{e}}_{\delta }}^T{\mathit{e}}_{\delta }+\frac{1}{2}{\mathit{e}}_E^T{\mathit{k}}_2{\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{k}}_E{\mathit{e}}_E\end{array}$$

(33)

Differentiating $V({\mathit{x}}_1,{\mathit{x}}_2,{\mathit{e}}_E)$ yields

$$\begin{array}{c}\hfill \dot{V}(\mathit{e},\dot{\mathit{e}},{\mathit{e}}_E,{\mathit{e}}_{\delta })={\mathit{e}}^T{\mathit{k}}_1{\mathit{k}}_2\dot{\mathit{e}}+{\dot{\mathit{e}}}^T{\mathit{k}}_2\ddot{\mathit{e}}+{{\mathit{e}}_{\delta }}^T\dot{{\mathit{e}}_{\delta }}+{\mathit{e}}_E^T{\mathit{k}}_2{\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{k}}_E{\dot{\mathit{e}}}_E\end{array}$$

(34)

where ${\mathit{M}}^{*}$ is assumed to be constant.

Substituting the proposed control law (31) in (22), and taking into account (29), (34) can be rewritten as

$$\begin{array}{cc}\hfill \dot{V}& ={\mathit{e}}^T{\mathit{k}}_1{\mathit{k}}_2\dot{\mathit{e}}+{\mathit{e}}_E^T{\mathit{k}}_2{\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{k}}_E{\dot{\mathit{e}}}_E+{{\mathit{e}}_{\delta }}^T(-{\mathit{T}}^{-1}{\mathit{e}}_{\delta }+\dot{\mathit{\delta }})\hfill \\ \hfill & +{\dot{\mathit{e}}}^T{\mathit{k}}_2(-{\mathit{k}}_2\dot{\mathit{e}}-{\mathit{k}}_1\mathit{e}-{\left({\mathit{M}}^{*}\right)}^{-1}{\mathit{J}}^T{\mathit{k}}_E{\mathit{e}}_E+{\left({\mathit{M}}^{*}\right)}^{-1}({\mathit{e}}_{\delta }-\mathit{\rho }\widehat{\mathit{\delta }}-\mathit{ϵ}))\hfill \\ \hfill & =-{\dot{\mathit{e}}}^T{\parallel {\mathit{k}}_2\parallel }^2\dot{\mathit{e}}-{{\mathit{e}}_{\delta }}^T{\mathit{T}}^{-1}{\mathit{e}}_{\delta }+{{\mathit{e}}_{\delta }}^T\dot{\mathit{\delta }}+{\dot{\mathit{e}}}^T{\mathit{k}}_2{\left({\mathit{M}}^{*}\right)}^{-1}((1+\mathit{\rho }){\mathit{e}}_{\delta }-\mathit{\rho }\mathit{\delta }-\mathit{ϵ})\hfill \\ \hfill & =-{\dot{\mathit{e}}}^T{\mathit{k}}_2[{\mathit{k}}_2\dot{\mathit{e}}-{\left({\mathit{M}}^{*}\right)}^{-1}((1+\mathit{\rho }){\mathit{e}}_{\delta }-\mathit{\rho }\mathit{\delta }-\mathit{ϵ})]-{\mathit{e}}_{\delta }({\mathit{T}}^{-1}{\mathit{e}}_{\delta }-\dot{\mathit{\delta }})\hfill \end{array}$$

(35)

By choosing large enough values of ${\mathit{k}}_2$, small values of $\mathit{\rho }$ and small enough values of $\mathit{T}$ such that

$$\begin{array}{c}\hfill {\mathit{k}}_2\dot{\mathit{e}}\ge {\left({\mathit{M}}^{*}\right)}^{-1}((1+\mathit{\rho }){\mathit{e}}_{\delta }-\mathit{\rho }\mathit{\delta }-\mathit{ϵ}),\parallel \mathit{\rho }\mathit{\delta }\parallel \le \mathit{\sigma },{\mathit{T}}^{-1}{\mathit{e}}_{\delta }\ge \dot{\mathit{\delta }},\end{array}$$

(36)

$\dot{V}(\mathit{e},\dot{\mathit{e}},{\mathit{e}}_E,{\mathit{e}}_{\delta })$ is negative semi-definite, where $\mathit{\sigma }\to \mathbf{0}$ is a vector with smaller positive values. Consequently, the tracking errors and estimated errors of the system all converge to zero asymptotically; i.e.,

$$\begin{array}{c}\hfill \underset{t\to \infty }{lim}\mathit{e}\to \mathbf{0},\underset{t\to \infty }{lim}\dot{\mathit{e}}\to \mathbf{0},\underset{t\to \infty }{lim}{\mathit{e}}_E\to \mathbf{0},\underset{t\to \infty }{lim}{\mathit{e}}_{\delta }\to \mathbf{0}\end{array}$$

(37)

Therefore, the closed-loop system is asymptotically stable in the entire state space.

5. Simulation Studies

To verify the performance of the proposed technique, numerical simulations were performed on a UVMS with a torpedo-type AUV and a 3-DOF underwater manipulator [15] shown in Figure 1. The AUV is driven by five thrusters in total and its thruster configuration is shown in Figure 5. The thruster configuration matrix ${\mathit{B}}_{\nu }$ and its pseudo inverse ${\mathit{B}}_{\nu }^{+}$ are shown in the Appendix A. The parameters for the AUV and manipulator are given in the Appendix A and

Table 3. D-H parameters of the manipulator.

Joint (k)	Offset (αk−1)	Length (ak−1)	Distance (dk)	Angle (θk)
1	0°	0	0	q1
2	90°	lq1	0	q2
3	0°	lq2	0	q3
4	0°	lq3	0	q4 = 0°

and

Table 4. The list of UVMS parameters.

	$\mathbf{Length}$	$\mathbf{Diameter}$	$\mathbf{Mass}$	$\mathbf{Buoyancy}$	$\mathbf{Weight}$
	$\left(\mathbf{m}\right)$	$\left(\mathbf{m}\right)$	$\left(\mathbf{kg}\right)$	$\left(\mathbf{N}\right)$	$\left(\mathbf{N}\right)$
$\mathrm{AUV}$	$1.78$	$0.26$	$78.2$	$771.59$	$767.14$
$\mathrm{Link}1$	$0.1$	$0.06$	$1.28$	$11.43$	$12.54$
$\mathrm{Link}2$	$0.3$	$0.0425$	$1.92$	$17.2$	$18.87$
$\mathrm{Link}3$	$0.3$	$0.0425$	$1.92$	$17.2$	$18.87$

. From Table 4, it can be seen that the whole system is neutrally buoyant, while the manipulator’s links have negative buoyancy. Thus, the UVMS used for numerical simulations in this paper can approximate to the real system.

5.1. Simulation Conditions

In the simulations, the UVMS’s end-effector is commanded to follow a spatial circle with diameter $2.24$ m and a straight line of length $7.0$ m. The simulation time of the circular trajectory is 50 s, where the initial 10 s is used for acceleration, 30 s is used to follow the circle and the final 10 s is used for deceleration. The simulation time of the straight-line trajectory is 30 s, where in the initial 10 s the acceleration is a half-period sine function, and then it maintains zeros, and in the final 10 s it is a half-period negative sine function. The UVMS’s end-effector maintains orientation during the two trajectory tracking tasks. The initial desired positions and orientations are the same as the initial actual positions and orientations. The initial desired and actual velocities and accelerations are zeros. The average speeds of the two trajectories both are $0.23\mathrm{m}/\mathrm{s}$. The sampling time for the simulation is $t=20$ ms.

In this case, the model uncertainties, external disturbances, payload and sensory noises in position and orientation measurements are introduced for simulating the real working environment. To reflect the uncertainties, it is assumed that the modeling inaccuracy for each parameter is $10\%$. The vector of time-varying ocean currents in the inertial frame is assumed to be governed by (38). It is supposed that the end-effector of the manipulator is attached with a payload of $1\mathrm{kg}$ (in water). The following sensory noises are introduced: gaussian noise of $0.01\mathrm{m}$ mean and $0.01\mathrm{m}$ standard deviation for the vehicle position measurements; ${0.5}^{\circ }$ mean and ${0.5}^{\circ }$ standard deviation for the vehicle attitude measurements; and ${0.05}^{\circ }$ mean and ${0.05}^{\circ }$ standard deviation for the manipulator’s joint position measurements. In addition, the thruster dynamic characteristics are inserted into the simulation. Suppose that the thruster response delay time is $50\mathrm{ms}$, and its efficiency is $95\%$.

$$\begin{array}{c}\hfill {\mathit{\nu }}_c={\left[\begin{array}{c}0.15+0.1\cos \left(0.3t\right),0.05\cos \left(0.1\pi t\right),0.1\cos \left(0.2t\right),0,0,0\end{array}\right]}^T\mathrm{m}/\mathrm{s}\end{array}$$

(38)

To implement solution (7), the primary task vector is ${\mathit{x}}_{Ed}={[{\mathit{\eta }}_{E1d}^T,{\mathit{\eta }}_{E2d}^T]}^T$. ${\mathit{k}}_f=\mathrm{diag}(4,4,4,6,6,6)$ and ${\mathit{k}}_s=25\mathit{I}$ for the two trajectories. Other parameters are shown in

Table 5. Parameters for the proposed redundancy resolution technique.

$\mathbf{Trajectory}$	${\mathit{\alpha }}_1$	${\mathit{\alpha }}_2$	${\mathit{\alpha }}_3$	${\mathit{\alpha }}_4$	$\mathit{\lambda }$	${\mathit{t}}_{\mathit{s}}\left(\mathit{s}\right)$	${\mathit{K}}_{\mathit{\zeta }}$
$\mathrm{Straight}\mathrm{line}$	$-0.02$	$0.13$	0	$0.05$	$0.3$	15	$\mathrm{diag}(0,2,0.2,0,1,1,1,1,1)$
$\mathrm{Circle}$	$-0.06$	$0.2$	$-0.2$	$0.15$	$0.3$	45	$\mathrm{diag}(0.1,0.3,0.1,0,1,1,1,1,1)$

. A secondary task is designed to align the vehicle orientation and the joint position with the primary task in terms of reducing the coupling effects, as shown in (39).

$$\begin{array}{c}\hfill {\dot{\mathit{\eta }}}_{sd}=\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\\ {\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -{\alpha }_1\\ 0& {\alpha }_2& 0\\ 0& -{\alpha }_3& 0\\ 0& 0& -{\alpha }_4\end{array}\right]{\dot{\mathit{\eta }}}_{E1d}\end{array}$$

(39)

where ${\dot{\mathit{\eta }}}_{E1d}$ is the linear part of ${\mathit{x}}_{Ed}$.

The distribution matrix is defined as

$$\begin{array}{c}\hfill {\mathit{W}}^{-1}=\left[\begin{array}{ccc}0.02{\mathit{I}}_{3\times 3}& {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}\\ {\mathit{W}}_1& 0.4{\mathit{I}}_{3\times 3}& {\mathit{0}}_{3\times 3}\\ {\mathit{W}}_2& {\mathit{0}}_{3\times 3}& 0.98{\mathit{I}}_{3\times 3}\end{array}\right],{\mathit{W}}_1=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0.2\\ 0& 0.35& 0\end{array}\right],{\mathit{W}}_2=\left[\begin{array}{ccc}0& 0.25& 0\\ 0& 0& 0.5\\ 0& 0& 0\end{array}\right]\end{array}$$

(40)

To illustrate the effectiveness of the proposed redundancy resolution technique in terms of energy savings, the comparative redundancy resolution technique is given in (41).

$$\begin{array}{c}\hfill {\mathit{\zeta }}_d={\mathit{J}}_W^{+}({\dot{\mathit{x}}}_{Ed}-{\mathit{K}}_f{\mathit{e}}_E)+(\mathit{I}-{\mathit{J}}_W^{+}{\mathit{J}}_W)\left[{\mathit{J}}_s^{+}(\mathit{\eta },\mathit{q})({\dot{\mathit{\eta }}}_{sd}-{\mathit{K}}_s{\mathit{e}}_s)\right]\end{array}$$

(41)

where the difference between (7) and (41) is that the secondary task vector $\dot{\mathit{\xi }}$ is not included in the compared technique.

The proposed control scheme is compared with the H∞-EKF method [23] which is given by

$$\begin{array}{c}\hfill {\mathit{u}}_c=\mathit{M}\left(\mathit{q}\right)({\ddot{\mathit{q}}}_d-\mathit{V}\dot{\mathit{e}}-\mathit{P}\mathit{e})+\mathit{H}(\mathit{q},\dot{\mathit{q}})+\mathit{M}\left(\mathit{q}\right)(-{\mathit{R}}^{-1}{\mathit{B}}^T\mathit{P}\mathit{e})+{\mathit{\tau }}_c\end{array}$$

(42)

where $\mathit{V}$ and $\mathit{P}$ are the derivative and the proportional gain matrices. $\mathit{q}$ and $\dot{\mathit{q}}$ are the estimated vectors from EKF. ${\mathit{\tau }}_c$ is the disturbance estimation from EKF as well. $\mathit{R}$ is the given positive definite matrix.

For simple representation, the proposed redundancy resolution is termed case 1 (c1), and the comparative redundancy resolution is termed case 2 (c2). Hence, the proposed control scheme based on the proposed redundancy resolution is termed proposed control_c1; the H∞-EKF method based on the proposed redundancy resolution is termed H∞-EKF_c1; and the proposed control based on the comparative redundancy resolution is termed proposed control_c2.

5.2. Results and Discussion

The results of numerical simulations are shown in Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13. Figure 6, Figure 7 and Figure 8 present the desired and actual spatial trajectories and their tracking errors. From these results it is observed that the proposed controller drives the UVMS to track the desired spatial linear and circular trajectories quite satisfactorily in both the proposed redundancy resolution technique (c1) and the comparative redundancy resolution technique (c2). Moreover, the proposed control scheme outperforms the H∞-EKF method, and has smaller tracking errors in both positions and orientations under the conditions of model uncertainties, time-varying ocean currents, payload and sensory noises. Even though the H∞-EKF method adopted a H∞ robust controller to compensate the estimated bias from the EKF, the residual tracking errors can not be fully eliminated, as shown in Figure 6b,c and Figure 6e,f. The proposed controller performs better than the H∞-EKF method in terms of robustness, which is dedicated to the IDC and fuzzy compensator for reducing the perturbation effects.

Figure 9 plots the norm of the vector ${\mathit{F}}_{td}$ (i.e., thruster forces and actuator torques) and energy consumption of the UVMS. It can be noted that the comparative redundancy resolution technique (c2) is consuming more energy in generating trajectories for the vehicle and manipulator during both the linear and circular trajectories tracking. However in the proposed redundancy resolution technique (c1), the UVMS states are adjusted by self-motion to minimize interaction effects between the vehicle and the manipulator. This is because of the introduction of the secondary task vector $\dot{\xi }$ and the nonlinear function.

For better understanding, the generated trajectories for the vehicle positions/attitudes and manipulator positions are presented in Figure 10 and Figure 11. It can be seen from the results that the generated trajectories have larger differences on vehicle attitudes and joint angles than vehicle positions. This is because the adjustment of the vehicle position has little effect on reducing the interactive forces between the vehicle and the manipulator without affecting the primary task. Consequently, the energy consumption can be reduced by changing the vehicle attitude and joint angles. In addition, it is observed from Figure 10 and Figure 11 that the small roll and pitch angles of the vehicle are kept in the proposed control scheme, which contributes to properly working of the vehicle’s onboard sensors.

Figure 12 and Figure 13 show the required thruster forces for the vehicle and actuator torques for the manipulator during the linear and circular trajectory tracking. It is observed that the thruster forces for the two trajectories are less in the proposed redundancy resolution technique (c1), which results in the reduced energy consumption. In addition, the thruster forces and actuator torques for both trajectories in the proposed control_c1 are within their constraints ($\pm 60$ N for the thrusters and $\pm 3$ N·m for the actuators).

The quantitative indexes of the time integral of tracking errors and energy consumption are listed in

Table 6. Performance analysis of the UVMS for the linear and circular trajectories tracking.

$\mathbf{Control}\mathbf{Schemes}$	$\mathbf{Linear}\mathbf{Trajectory}$		$\mathbf{Circular}\mathbf{Trajectory}$
	${\mathbf{ISE}}_{\mathit{p}}$	${\mathbf{ISE}}_{\mathit{o}}$	$\int \left|\right|{\mathit{F}}_{\mathit{td}}\left|\right|\mathit{dt}$	${\mathbf{ISE}}_{\mathit{p}}$	${\mathbf{ISE}}_{\mathbf{0}}$	$\int \left|\right|{\mathit{F}}_{\mathit{td}}\left|\right|\mathit{dt}$
$\mathrm{Proposed}{\mathrm{control}}_{\mathrm{c}2}$	$0.3083$	$22.5597$	$1161.6$	$0.3318$	$32.2834$	$1532.6$
$\mathrm{H}\infty -{\mathrm{EKF}}_{\mathrm{c}1}$	$0.6869$	$34.5853$	$607.2053$	$1.0775$	$63.1945$	$1420.4$
$\mathrm{Proposed}{\mathrm{control}}_{\mathrm{c}1}$	$0.2905$	$26.1319$	$603.8734$	$0.3708$	$40.1401$	$1428.3$

$\mathrm{Note}:$${\mathit{e}}_E={[{\left({\tilde{\mathit{\eta }}}_{E1}\right)}^T,{\left({\tilde{\mathit{\eta }}}_{E2}\right)}^T]}^T$, ${\mathrm{ISE}}_p=\int \left|\right|{\tilde{\mathit{\eta }}}_{E1}\left|\right|dt$, ${\mathrm{ISE}}_o=\int \left|\right|{\tilde{\mathit{\eta }}}_{E2}\left|\right|dt$.

. From these indices, it is indicated that the tracking error in the proposed control_c1 is smaller than that in the H∞-EKF_c1 method, and the energy consumption in the proposed control_c1 is less than that in the proposed control_c2. Overall, the proposed control scheme based on the proposed redundancy resolution technique (c1) ensures the precise and robust performance with a reduced energy requirement under the conditions of model parameter uncertainties, time-varying ocean currents, payload and sensory noises.

In the simulations, we have taken the model parameter uncertainties, time-varying external disturbances, payload and sensory noises into consideration. However, in a practical case, these lumped uncertainties and disturbances may be more complicated, and hence can not be simulated. Even though the results from computer simulations are promising, it is necessary to validate the effectiveness of the proposed control scheme and the proposed redundancy resolution technique through experiments in a water pool or at sea. This is our future work.

6. Conclusions

This paper presents a motion planning and coordinated control scheme for the trajectory tracking of the UVMS. A new secondary task with a nonlinear coefficient for redundancy resolution of the UVMS is proposed. In this way, the interactive effects between the vehicle and the manipulator can be minimized and the energy consumption of the UVMS is reduced. Simulation results show that the energy consumption based on the proposed redundancy resolution technique (proposed control_c1) is reduced by 48% (in the linear trajectory tracking) and 6% (in the circular trajectory tracking), compared with the comparative redundancy resolution technique (proposed control_c2). The proposed redundancy resolution technique is simple in design and easy to implement. Furthermore, a control scheme including a fuzzy compensator and a tracking control with joint-space errors, end-effector errors and inertial delay control (IDC) is proposed. The proposed control scheme ensures precise and robust tracking performance in the presence of model uncertainties, time-varying ocean currents, payload and sensory noises. Simulation results show that the position and orientation tracking precisions based on the proposed control_c1 are reduced by $57.7\%$ and $24.4\%$ (in the linear trajectory tracking) and $65.6\%$ and $36.5\%$ (in the circular trajectory tracking), compared with the H_∞−EKF_c1 method [23]. Even though the effectiveness of the proposed redundancy resolution technique and coordinated motion control scheme were validated through numerical simulations, experiments should be carried out on the real UVMS to further enhance the computer simulation results, which will be done in the future.