Whole-Body Vision/Force Control for an Underwater Vehicle–Manipulator System with Smooth Task Transitions

Abstract

Robots with multiple degrees of freedom (DOFs), such as underwater vehicle–manipulator systems (UVMSs), are expected to optimize system performance by exploiting redundancy with various basic tasks while still fulfilling the primary objective. Multiple tasks for robots, which are expected to be carried out simultaneously with prescribed priorities, can be referred to as sets of tasks (SOTs). In this work, a hybrid vision/force control method with continuous task transitions is proposed for a UVMS to simultaneously track the reference vision and force trajectory during manipulation. Several tasks with expected objectives and specific priorities are established and combined as SOTs in hybrid vision/force tracking. At different stages, various SOTs are carried out with different emphases. A hierarchical optimization-based whole-body control framework is constructed to obtain the solution in a strictly hierarchical fashion. A continuous transition method is employed to mitigate oscillations during the task switching phase. Finally, comparative simulation experiments are conducted and the results verify the improved convergence of the proposed tracking controller for UVMSs.

1. Introduction

Exploring and utilizing ocean resources has become a matter of strategic importance around the world against the backdrop of global resource scarcity. Underwater equipment is being increasingly investigated given the rising demands of underwater operations [1]. The underwater vehicle–manipulator system (UVMS), which is composed of an underwater vehicle and manipulator(s), is designed to expand the capacity and enhance the functionality of underwater operations. However, the attached manipulator(s) increases the complexity of the system, which brings new challenges in terms of whole-system design, multibody kinematics and dynamics and convoluted control algorithms [2]. Significant efforts have been devoted to developing a reliable and valuable UVMS that can operate in harsh underwater conditions.

Precise trajectory tracking control is the key technology to improve the autonomy of UVMSs. Generally, the control algorithms of UVMSs are mainly classified into two main categories. In the most straightforward sense, the control algorithms of the main vehicle and the manipulator are implemented for the corresponding degrees of freedom (DOFs) independently. In [3], the authors combined a standard proportional–integral–differential (PID) control with a prior model to track the desired vehicle velocity, while a cascade joint controller was employed to control the velocity and angle of each joint. This approach does not consider the couplings, so the system performance is necessarily limited. In improved methods, the dynamic couplings between the vehicle and manipulator are treated as disturbances and handled by feedforward compensation [4]. In [5], the authors discussed the coupling property and proposed a Slotine sliding mode approach to reduce the coupling effect. Active compensation is implemented in the control law to counteract the undesirable effect by measuring or estimating the interaction force from the manipulator in [6]. In general, the simplicity of this approach makes it attractive, and some control techniques from manipulators [7,8] and unmanned aerial vehicles (UAVs) [9] can be readily referenced and transferred. However, the main vehicle will usually be designed to be much heavier than the attached manipulator(s), resulting in relatively smaller reactive forces from the manipulator(s) compared to the vehicle’s own weight, as in the Phoenix+SMART3S [2].

Another control scheme is to model the vehicle and the attached manipulator(s) as a serial chain of rigid bodies by regarding the UVMS as a whole system. However, more challenges will arise due to the increased complexity of the whole system, such as inaccurate parameters of the hydrodynamic effects, kinematic redundancy of the system, etc.

Several classical control methodologies have been successfully applied to trajectory tracking in UVMSs, including robust PID in [10,11,12,13,14], sliding mode control (SMC) in [15,16,17,18] and adaptive controllers in [19,20,21,22,23], which are capable of effectively addressing external disturbances and unmodeled dynamics. Furthermore, advanced nonlinear control strategies, such as $H_{\infty }$ control in [24,25], prescribed performance control in [26,27] and neural network techniques in [28,29,30,31], have also been implemented to control underwater vehicles. However, these methods demonstrate limited capabilities in handling state constraints. Consequently, model predictive control (MPC) has been extensively adopted due to its inherent constraint handling advantages in the field of underwater vehicles, as shown in [32,33,34,35,36].

The previously mentioned works focus on tracking the trajectory based on the position, which is difficult to measure accurately in the underwater environment. Vision sensors are an important means of localization in a GPS-denied environment, which is referred to as visual servoing; this has also been widely used in underwater vehicles for dynamic positioning [37,38], path following [39], docking [40,41] and underwater manipulation [42,43].

The capacity to handle interactions is one of the fundamental requirements to accomplish the manipulation task successfully [44]. The contact force at the manipulator’s end-effector is employed to describe the state of interaction. Thus, force control strategies are often employed in scenarios where a robot intervenes in the environment, and they can be classified into ’direct force control’ and ’indirect force control’ according to whether the force is controlled directly or not [45]. Both of them are mainly constructed based on position control schemes and have been widely used for fixed-base manipulators.

However, the velocity and position of free-floating robots are difficult to measure accurately and rapidly, making it challenging to practically implement force control algorithms for freely moving manipulators [46], in which even higher precision and update rates are necessitated than in position control. Therefore, the current force control studies on mobile robots, including wheeled mobile robots [47,48,49] and UAVMs [50,51], remain largely confined to laboratory environments, with limited real-world applications. The same situation applies to UVMSs, including both impedance control ([52,53]) and direct force control ([54,55,56]).

In line with the distinction between position-based visual servoing (PBVS) and image-based visual servoing (IBVS), force control strategies can also bypass the reconstruction of the position and directly regulate contact forces using visual information. In image-based force control, the translational positions are replaced by visual features; then, visual features and forces are simultaneously tracked in a unique control law to handle the interaction with the environment. A vision-based impedance force control algorithm was introduced for a laboratory injection system in [57]. The injection force was estimated by utilizing visual feedback via the concept of visual–force integration. Meanwhile, three types of hybrid visual–impedance control schemes that directly relate image feature errors to external forces by projecting the force component onto the image plane were proposed in [58]. This approach enables stable physical interaction between an unmanned aerial manipulator and its task environment.

Challenges also arise from the complexity of handling multiple DOFs in the whole UVMS system. A set of tasks (SOT) is expected to be carried out simultaneously with prescribed priorities by utilizing the redundant DOFs of the UVMS. Task-based control methods for UVMSs have been proposed in [59,60,61] with the consideration of priorities between tasks. In [62], a whole-body control (WBC) framework for a dual-arm UVMS is proposed to deal with multidimensional inequality control objectives. In this work, WBC is firstly introduced for the controller of the UVMS. Although it is similar to some task-priority-based control methods, it offers a distinct perspective on the problem by utilizing the whole system’s DOFs.

Considering the advantages of IBVS, a vision-based force control task is established without the position reconstruction process. By employing image moments instead of point features, this method achieves enhanced robustness. Furthermore, a multitask framework considering smooth transitions can preserve the priority of the force tracking task while effectively mitigating fluctuations.

In our previous work, an image-based visual serving method for UVMSs was introduced [42]. The hierarchical control architecture is composed of a kinematic model predictive IBVS controller and a dynamic velocity controller, respectively. In this work, a whole-body vision/force control framework is proposed to simultaneously track the reference vision and force trajectory, which fully exploits the whole-body DOFs of the UVMS. The contributions of this work are summarized as follows:

• A whole-body multitask control framework is proposed to simultaneously accomplish the visual trajectory tracking and contact force tracking of a UVMS. This approach allows flexible task combinations in different scenarios while consistently maintaining strict priorities.
• By reprojecting the image points and choosing proper image moment combinations, decoupled visual features aligned along the tangential and normal directions of the target plane can be obtained, which enhances the independence among individual tasks.
• A continuous transition method for SOT switching is presented to optimize the performance in the transition process, particularly by effectively reducing fluctuations in the contact force.

This paper is organized as follows. The system formulation and visual servo model are introduced in Section 2. Several tasks for hybrid vision/force control based on WBC are established and a hierarchical WBC framework is reviewed in Section 3. In Section 4, a continuous transition method for SOT switching in WBC is proposed. The simulation results are demonstrated in Section 5. Lastly, we draw the conclusions of our paper and discuss further research problems.

2. System Model and Problem Statement

In this part, the whole system and the modeling process of the UVMS are first reviewed. Then, the contact model and the visual servo model for the hybrid vision/force control are introduced as the research basis.

2.1. UVMS Model

This work focuses on the control and analysis of a free-floating UVMS attached with an n-link manipulator. In Figure 1, the schematic provides an illustration of the system and frame definitions. The notations of the relevant frames are listed in

Table 1. The notations of frames.

Notation	Description
I	The inertial reference frame.
B	The body-fixed reference frame located at the center of mass of the vehicle.
E	The end-effector frame attached at the end of the manipulator.
C	The camera frame.
A	The auxiliary virtual camera frame, which is fixed with the camera and aligned with the target.

.

The modeling process of the UVMS follows the approach described in [2], in which the system is regarded as a serial chain of rigid bodies with hydrodynamics. In this part, a brief summary is provided for completeness, including the kinematics and the dynamics. The global pose vector of the UVMS is defined as

$$\mathit{\eta }=\left[\begin{array}{c}{\dot{\mathit{\eta }}}_1\\ {\dot{\mathit{\eta }}}_2\\ \dot{\mathbf{q}}\end{array}\right]\in {\mathbb{R}}^{6+n}$$

(1)

including the position vector ${\mathit{\eta }}_1={[x,y,z]}^{\mathrm{T}}\in {\mathbb{R}}^3$; the orientation vector ${\mathit{\eta }}_2={[\psi ,\theta ,\phi ]}^{\mathrm{T}}\in {\mathbb{R}}^3$, described by the Euler angle; and the joint vector $\mathit{q}={[q_1,q_2...q_n]}^{\mathrm{T}}\in {\mathbb{R}}^n$, representing the joint angles of the manipulator with n joints.

The following equation defines the kinematics of the UVMS:

$$\left[\begin{array}{c}{\dot{\mathit{\eta }}}_1\\ {\dot{\mathit{\eta }}}_2\\ \dot{\mathit{q}}\end{array}\right]=\left[\begin{array}{ccc}{\mathit{R}}_B^I\left({\mathit{\eta }}_2\right)& \mathit{O}& \mathit{O}\\ \mathit{O}& {\mathit{J}}_{k,e}\left({\mathit{\eta }}_2\right)& \mathit{O}\\ \mathit{O}& \mathit{O}& {\mathit{I}}_n\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_1\\ {\mathit{v}}_2\\ \dot{\mathit{q}}\end{array}\right]$$

(2)

where $\mathit{\zeta }={[{\mathit{v}}_1,{\mathit{v}}_2,\dot{\mathit{q}}]}^{\mathrm{T}}\in {\mathbb{R}}^{6+n}$ is the system velocity vector, consisting of the body-fixed linear velocity ${\mathit{v}}_1$, the body-fixed angular velocity ${\mathit{v}}_2$ of the base vehicle and the joint velocity $\dot{\mathit{q}}$. ${\mathit{R}}_B^I\left({\mathit{\eta }}_2\right)$ is the rotation matrix expressing the transformation from the body-fixed frame to the inertial frame, and ${\mathit{J}}_{k,e}\left({\mathit{\eta }}_2\right)$ is a Jacobian matrix describing the connection between ${\dot{\mathit{\eta }}}_2$ and ${\mathit{v}}_2$.

Define ${\mathit{\eta }}_e=[{\mathit{\eta }}_{e1},{\mathit{\eta }}_{eq}]\in {\mathbb{R}}^7$ as the pose vector of the end-effector in the inertial frame I, where ${\mathit{\eta }}_{e1}\in {\mathbb{R}}^3$ is the position and ${\mathit{\eta }}_{eq}\in {\mathbb{R}}^4$ is the orientation in quaternion form. According to [2], we have

$$\left[\begin{array}{c}{\dot{\mathit{\eta }}}_{e1}\\ {\mathit{w}}_e\end{array}\right]={\mathit{J}}_m^I\left({\mathit{\eta }}_2,\mathit{q}\right)\mathit{\zeta }$$

(3)

where ${\dot{\mathit{\eta }}}_{e1}$ and ${\mathit{w}}_e$ are the end-effector velocities expressed in the I-frame, and ${\mathit{J}}_m^I({\mathit{\eta }}_2,\mathit{q})$ is the mapping matrix, which maps the $(6+n)$-dimensional velocities into 6-dimensional end-effector velocities.

Remark 1. 

The system is redundant as $(6+n)>6$, which means that kinematic redundancy can be exploited to achieve additional task(s) besides the expected end-effector task.

The velocity of the end-effector in the reference frame E is defined as ${\mathit{v}}_e=[{\mathit{v}}_{e1},{\mathit{v}}_{e2}]$. Then, the relationship between ${\mathit{v}}_e$ and $\mathit{\zeta }$ can be obtained by

$${\mathit{v}}_e=\left[\begin{array}{cc}{\mathit{R}}_I^E\left({\mathit{\eta }}_{eq}\right)& \mathit{O}\\ \mathit{O}& {\mathit{R}}_I^E\left({\mathit{\eta }}_{eq}\right)\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{\eta }}}_{e1}\\ {\mathit{w}}_e\end{array}\right]={\mathit{J}}_m^E\left({\mathit{\eta }}_2,\mathit{q},{\mathit{\eta }}_{eq}\right)\mathit{\zeta }$$

(4)

where ${\mathit{R}}_I^E\left({\mathit{\eta }}_{eq}\right)$ is the rotation matrix expressing the transformation from the inertial frame I to the E frame.

In the underwater environment, the total forces and moments acting on the generic part of the serial chain, including hydrodynamics, can be derived. Then, we write them in matrix form, and the dynamic equation of the UVMS is given by

$$\mathit{M}\left(\mathit{q}\right)\dot{\mathit{\zeta }}+\mathit{C}\left(\mathit{q},\mathit{\zeta }\right)\mathit{\zeta }+\mathit{D}\left(\mathit{q},\mathit{\zeta }\right)\mathit{\zeta }+\mathit{g}(\mathit{q},{\mathit{R}}_{\mathit{B}}^{\mathit{I}})=\mathit{\tau }+{\left({\mathit{J}}_m^I\right)}^{\mathrm{T}}{\mathit{f}}^I+{\left({\mathit{J}}_m^I\right)}^{\mathrm{T}}{\mathit{f}}_d^I$$

(5)

where $\mathit{M}\in {\mathbb{R}}^{(6+n)\times (6+n)}$ and $\mathit{C}\in {\mathbb{R}}^{(6+n)\times (6+n)}$ are the inertia and coriolis/centripetal matrices considering the additional mass influences from hydrodynamic effects, and $\mathit{D}\in {\mathbb{R}}^{(6+n)\times (6+n)}$ represents the linear and quadratic damping matrix. $\mathit{g}$ is the restoring force vector due to gravity and buoyancy. $\mathit{\tau }=[{\mathit{\tau }}_v,{\mathit{\tau }}_q]\in {\mathbb{R}}^{6+n}$ is the vector of the control inputs, including the vehicle control input ${\mathit{\tau }}_v$ and joint torque vector ${\mathit{\tau }}_q$. ${\mathit{f}}^I$ and ${\mathit{f}}_d^I$ are the external contact force and disturbance force expressed in the inertial frame, respectively. The global pose vector $\mathit{\eta }$, the system velocity $\mathit{\zeta }$ and the control input $\mathit{\tau }$ are constrained as

$$\begin{array}{cc}\hfill & {\eta }_i^{min}⩽{\eta }_i⩽{\eta }_i^{max},i=1,2,\dots ,6+n\hfill \end{array}$$

(6a)

$$\begin{array}{cc}\hfill & {\zeta }_i^{min}⩽{\zeta }_i⩽{\zeta }_i^{max},i=1,2,\dots ,6+n\hfill \end{array}$$

(6b)

$$\begin{array}{cc}\hfill & {\tau }_i^{min}⩽{\tau }_i⩽{\tau }_i^{max},i=1,2,\dots ,6+n\hfill \end{array}$$

(6c)

where ${(.)}^{min}$ and ${(.)}^{min}$ are the lower and upper bounds of the corresponding variables.

2.2. Contact Model

An object–environment contact model describes the relationship between the contact force and the deformation, which is a very complex physical process. In [63], a stiffness-damping environmental model with a satisfactory matching degree is used, as shown in Figure 2.

The dynamic relationship is simplified as

$$\begin{array}{c}\hfill f=\left\{\begin{array}{cc}0& x⩽x_e\\ b_e\dot{x}+k_e(x-x_e)& x>x_e\end{array}\right.\end{array},$$

(7)

where $b_e$ and $k_e$ are the damping and stiffness coefficients; x and $x_e$ are the actual position and the equilibrium position, respectively; and f is the contact force.

2.3. Modeling of Visual Servo System

Image moments are selected as visual features due to the favorable decoupling characteristics. To further isolate the force control task from visual servo tasks, an image reprojection technique is employed by projecting the image points onto a virtual image plane that is always parallel to the target plane.

The detailed reprojection process can be found in [36] and is omitted here. For any image point, its original pixel coordinate is represented by $p_c={(u_c,n_c)}^{\mathrm{T}}$ and its reprojected coordinate in frame A is written as $p_a={(u_a,n_a)}^{\mathrm{T}}$. Considering that N virtual image points $p_{a,i}(i=1,2\dots N)$ constitute a visual target $V_t$ on the virtual image plane, we can define the image feature vector

$${\mathit{s}}_a={\left[\begin{array}{c}{x}_n,y_n,a_n\end{array}\right]}^{\mathrm{T}}.$$

(8)

where $x_n$, $y_n$ and $a_n$ are three normalized image moments ([64]) calculated form the central moments.

Given that the virtual camera frame and the target frame are always aligned, it is easy to obtain the body-fixed velocity of frame A by

$${\mathit{v}}_a=\left[\begin{array}{c}{\mathit{v}}_{a1}\\ {\mathit{v}}_{a2}\end{array}\right]=\left[\begin{array}{c}{\mathit{R}}_C^A{\mathit{v}}_{c1}\\ {\mathit{0}}_{3\times 3}\end{array}\right]=\left[\begin{array}{cc}{\mathit{R}}_C^A& {\mathit{0}}_{3\times 3}\\ {\mathit{0}}_{3\times 3}& {\mathit{0}}_{3\times 3}\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{c1}\\ {\mathit{v}}_{c2}\end{array}\right]={\mathit{J}}_c^a{\mathit{v}}_c$$

(9)

and the rotation matrix between frame C and A is given by

$${\mathit{R}}_C^A={\left({\mathit{R}}_A^I\right)}^{\mathrm{T}}{\mathit{R}}_C^I.$$

(10)

The dynamic relationship between ${\mathit{s}}_a$ and the relative motion of frame A and frame C can be described by

$${\dot{\mathit{s}}}_a={\mathit{L}}_a{\mathit{v}}_a={\mathit{L}}_a{\mathit{J}}_c^a{\mathit{v}}_c$$

(11)

where ${\mathit{v}}_a={[{\mathit{v}}_{a1}^{\mathrm{T}},{\mathit{v}}_{a2}^{\mathrm{T}}]}^{\mathrm{T}}$ is the velocity of the virtual camera in the reference frame A, including the linear velocity ${\mathit{v}}_{a1}$ and angular velocity ${\mathit{v}}_{a2}$, and ${\mathit{v}}_c$ is the velocity of the real camera. ${\mathit{L}}_a\in {\mathbb{R}}^{3\times 6}$ is the interaction matrix, which is defined as

$${\mathit{L}}_a=\left[\begin{array}{cccccc}-1& 0& 0& 0& 0& 0\\ 0& -1& 0& 0& 0& 0\\ 0& 0& -1& 0& 0& 0\end{array}\right].$$

(12)

It is clear that ${\mathit{L}}_a$ shows good decoupling characteristics between the three translational motions, and the image moments computed from the reprojected image are fully decoupled.

Since the camera is fixed on the end-effector, the relationship between the camera velocity and the end-effector velocity can be obtained by the velocity Jacobian matrix

$${\mathit{v}}_c={\mathit{J}}_e^c{\mathit{v}}_e$$

(13)

with

$${\mathit{J}}_e^c=\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}& S\left({\mathit{l}}_{\mathit{c}}\right)\\ {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$

where $S(·)$ is the skew-symmetric matrix operator, and ${\mathit{l}}_{\mathit{c}}$ is the position vector of the camera relative to the end-effector in frame E.

By substituting (4), (9) and (13) into (11), the derivative of visual features and the system velocity can be connected by

$${\dot{\mathit{s}}}_a={\mathit{L}}_a{\mathit{J}}_c^a{\mathit{J}}_e^c{\mathit{J}}_m^E\mathit{\zeta }={\mathit{J}}_{{\mathit{s}}_a,\xi }({\mathit{\eta }}_2,\mathit{\xi },\mathit{q})\mathit{\zeta }.$$

(14)

where ${\mathit{J}}_{{\mathit{s}}_a,\xi }\in {\mathbb{R}}^{3\times (6+n)}$ is the Jacobian matrix of ${\dot{\mathit{s}}}_a$.

2.4. Problem Statement

The control objective of this work is to simultaneously track the reference visual trajectory of the UVMS while maintaining a desired contact force with the target plane.

3. Vision/Force Hybrid Control for UVMSs

A hybrid vision/force control framework based on WBC for UVMSs is established in this part. The operation is decomposed into several tasks, and a hierarchical optimization-based WBC is established to construct a unified output among the tasks.

3.1. Hybrid Vision/Force Control Tasks for UVMS

The general definition of one task includes the generic task variable ${\mathit{x}}_t$ and its corresponding Jacobian matrix ${\mathit{J}}_t(\mathit{\eta },\mathit{q})$ relating its time derivative to the system velocity $\mathit{\zeta }$. Then, the differential relationship is given by

$${\dot{\mathit{x}}}_t={\mathit{J}}_t(\mathit{\eta },\mathit{q})\mathit{\zeta }$$

(15)

where the subscript ${(.)}_t$ indicates the variable related to the task notation. Thus, the task is accomplished by finding the appropriate ${\mathit{\zeta }}^{*}$ to track the reference ${\dot{\mathit{x}}}_t^{*}$, which is a simple inverse kinematic (IK) problem, as (15) is defined at the kinematic level. However, these tasks are not only limited to IK; they can also be employed at the dynamic level.

In our work, the tasks in hybrid vision/force tracking for UVMSs will be defined as follows.

3.1.1. End-Effector Orientation

The orientation of the end-effector is represented in unit quaternion form, and the task of end-effector orientation is implemented as

$${\mathit{e}}_{tq}=\mathrm{qerr}({\mathit{\eta }}_{eq}^{*},{\mathit{\eta }}_{eq})$$

(16)

$${\mathit{J}}_{tq}={\mathit{J}}_{eq}$$

(17)

$${\dot{\mathit{x}}}_{tq}^{*}={\mathit{\lambda }}_{tq}{\mathit{e}}_{tq}.$$

(18)

where ${\mathit{\eta }}_{eq}^{*}$ and ${\mathit{e}}_{tq}$ are the reference value and the state error of the orientation of the end-effector. The quaternion error is calculated by $\mathrm{qerr}(.)$, with the advantage of using the end-effector angular velocity ${\mathit{w}}_e$ and the corresponding orientation Jacobian ${\mathit{J}}_{eq}$ extracted from ${\mathit{J}}_m^I$ [2]. ${\mathit{\lambda }}_{tq}$ is the positive control gain.

3.1.2. End-Effector Trajectory of UVMS

Visual servoing is employed to track the reference trajectory of the end-effector. The tangential movement of the end-effector to the target plane is described by ${\mathit{s}}_{ps}=(x_n,y_n)$ and the task is defined as

$${\mathit{e}}_{tps}={\mathit{s}}_{ps}^{*}-{\mathit{s}}_{ps}$$

(19)

$${\mathit{J}}_{tps}={\mathit{J}}_{{\mathit{s}}_a}(x_n,y_n)$$

(20)

$${\dot{\mathit{x}}}_{tps}^{*}={\mathit{\lambda }}_{tps}{\mathit{e}}_{tps}.$$

(21)

The normal motion is connected with $a_n$ and controlled separately because it is highly coupled with the contact force $f_n$ in the normal direction of the plan, and the task of $f_n$ will be defined later.

$${\mathit{e}}_{tpn}={\mathit{a}}_n^{*}-{\mathit{a}}_n$$

(22)

$${\mathit{J}}_{tpn}={\mathit{J}}_{{\mathit{s}}_a}\left(a_n\right)$$

(23)

$${\dot{\mathit{x}}}_{tpn}^{*}={\mathit{\lambda }}_{tpn}{\mathit{e}}_{tpn}.$$

(24)

3.1.3. Normal Contact Force

The contact force in normal direction $f_n$ is considered in this work. The reprojection process also has the advantage of allowing the use of the Jacobian matrix of $a_n$; then, the control task is obtained.

$${\mathit{e}}_{tfn}={\mathit{f}}_n^{*}-{\mathit{f}}_n$$

(25)

$${\mathit{J}}_{tfn}={\mathit{J}}_{{\mathit{s}}_a}\left(a_n\right)$$

(26)

$${\dot{\mathit{x}}}_{tfn}^{*}={\mathit{\lambda }}_{tfn}{\mathit{e}}_{tfn}.$$

(27)

Only one of the two tasks of $a_n$ and $f_n$ in the same direction will be activated at the same time.

3.1.4. Main Vehicle Attitude Holding

In most cases, the main vehicle is preferably kept horizontal because of the requirements of the attached sensors (e.g., Doppler velocity logger (DVL), camera). By defining ${\mathit{\eta }}_{2,pr}={[\psi ,\theta ]}^{\mathrm{T}}\in {\mathbb{R}}^2$ and the corresponding Jacobian ${\mathit{J}}_{{\mathit{\eta }}_{2,pr}}$, a horizontal attitude holding task is considered.

$${\mathit{e}}_{tpr}={\mathit{\eta }}_{2,pr}^{*}-{\mathit{\eta }}_{2,pr}$$

(28)

$${\mathit{J}}_{tpr}={\mathit{J}}_{{\mathit{\eta }}_{2,pr}}$$

(29)

$${\dot{\mathit{x}}}_{tpr}^{*}={\mathit{\lambda }}_{tpr}{\mathit{e}}_{tpr}$$

(30)

3.1.5. Camera’s Field of View

The image features are constrained states limited by the camera’s field of view (FOV), which is an inequality-constrained task. The indirect method is to convert the unequal constraints into equal tasks. Considering any one constrained state $x_c\in [x_c^l,x_c^u]$, its lower-bound rejection task can be formulated as

$$e_{tc}^l=\left\{\begin{array}{cc}{x}_c^l+d^{*}-x_c,\hfill & \hfill x_c<x_c^l+d^{*}\\ -\infty ,\hfill & \hfill x_c⩾x_c^l+d^{*}\end{array}\right.$$

(31)

$${\mathit{J}}_{tc}={\mathit{J}}_c\left(x_c\right)$$

(32)

$${\dot{\mathit{x}}}_{tc}^{*}={\mathit{\lambda }}_{tc}^l{e}_{tc}^l$$

(33)

where $d^{*}$ is the safe buffer and $e_{tc}^l=-\infty$ means that the task is deactivated when unnecessary. In the same way, the upper-bound rejection task is defined as

$$e_{tc}^u=\left\{\begin{array}{cc}{x}_c^u-d^{*}-x_c,\hfill & \hfill x_c>x_c^u-d^{*}\\ \infty ,\hfill & \hfill x_c⩽x_c^u-d^{*}\end{array}\right.$$

(34)

$${\mathit{J}}_{tc}={\mathit{J}}_c\left(x_c\right)$$

(35)

$${\dot{\mathit{x}}}_{tc}^{*}={\mathit{\lambda }}_{tc}^u{e}_{tc}^u$$

(36)

The constraint is ensured by an equation task. It is clear that expressing such tasks via inequalities is more intuitive:

$${\dot{x}}_{tc}^l+d^{*}⩽{\mathit{J}}_{tc}\mathit{\zeta }⩽{\dot{x}}_{tc}^u-d^{*}.$$

(37)

For convenience, the $i_{th}$ task is defined as ${\mathcal{L}}_i({\mathit{J}}_{ti},{\dot{\mathit{x}}}_{ti}^l,{\dot{\mathit{x}}}_{ti}^u)$, including the Jacobian matrix and the expected lower and upper derivatives (${\dot{\mathit{x}}}_{ti}^l={\dot{\mathit{x}}}_{ti}^u$ in a task of equal constraints). For two tasks ${\mathcal{L}}_i$ and ${\mathcal{L}}_j$, if ${\mathcal{L}}_i$ has a higher priority than ${\mathcal{L}}_j$, it is called ${\mathcal{L}}_i\succ {\mathcal{L}}_i$; otherwise, ${\mathcal{L}}_i\prec {\mathcal{L}}_i$. In a hierarchical controller, N tasks with the priority of ${\mathcal{L}}_1\succ {\mathcal{L}}_2\succ \cdots \succ {\mathcal{L}}_N$ are combined into an SOT, $\mathcal{S}=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_N\}$, and deployed to accomplish the whole mission collectively.

3.2. Hierarchical Optimization-Based Whole-Body Control

One single task is a simple inverse kinematic problem, which can be solved using a pseudo-inverse matrix. Then, when considering n tasks with different priorities, a closed-form WBC based on null space projection can find a solution that always satisfies the hierarchy, but it can only handle equal constraints. To deal with unequal constraints, an optimization-based WBC (OWBC) will be constructed.

Generally, the problem can be expressed in a general optimization-based form:

$$\mathcal{P}:\underset{\mathit{\zeta }}{min}{\parallel {\mathit{J}}_t\mathit{\zeta }-{\dot{\mathit{x}}}_t^{*}\parallel }^2$$

(38)

Then, when considering n tasks with different priorities, a weighting matrix ${\mathit{Q}}_n$ can be added to the optimization problem to ensure the precedence rules:

$$\mathcal{P}:\underset{\mathit{\zeta }}{min}{({\mathit{J}}_n\mathit{\zeta }-{\dot{\mathit{x}}}_n)}^{\mathrm{T}}\mathit{Q}({\mathit{J}}_n\mathit{\zeta }-{\dot{\mathit{x}}}_n),$$

(39)

where

$${\mathit{J}}_n=\left[\begin{array}{c}{\mathit{J}}_{t1}\\ ⋮\\ {\mathit{J}}_{tn}\end{array}\right],{\dot{\mathit{x}}}_n=\left[\begin{array}{c}{\dot{\mathit{x}}}_{t1}\\ ⋮\\ {\dot{\mathit{x}}}_{tn}\end{array}\right].$$

(40)

are the corresponding Jacobian matrices and states of these tasks, and 

$${\mathit{Q}}_n=\left[\begin{array}{ccc}{\mathit{Q}}_{t1}& & \\ & \ddots & \\ & & {\mathit{Q}}_{tn}\end{array}\right]$$

(41)

consists of the weighing matrices of the tasks.

To obtain the solution to an optimization problem in a strictly hierarchical manner, a hierarchical optimization-based WBC (HWBC) can be formulated as in the following recursive optimization problem.

From $n=1,\dots ,N$, the optimization problem is constructed and solved in recursive form:

$${\mathcal{P}}_o:\underset{\mathit{\zeta }}{min}{\parallel {\mathit{w}}_n\parallel }^2$$

(42)

s.t.

$$\begin{array}{cc}\hfill & {\dot{\mathit{x}}}_{tn}^l⩽{\mathit{J}}_n\mathit{\zeta }-{\mathit{w}}_n⩽{\dot{\mathit{x}}}_{tn}^u\hfill \end{array}$$

(43a)

$$\begin{array}{cc}\hfill & {\dot{\mathit{x}}}_{tk}^l⩽{\mathit{J}}_k\mathit{\zeta }-{\mathit{w}}_k^{*}⩽{\dot{\mathit{x}}}_{tk}^u,k=1,\dots ,n-1\hfill \end{array}$$

(43b)

where $\mathit{w}=\{{\mathit{w}}_1^{*},\dots ,{\mathit{w}}_k^{*}\}$ are the slack variables obtained after solving the first k task, and ${\dot{\mathit{x}}}_{tn}^l$ and ${\dot{\mathit{x}}}_{tn}^u$ are the lower and the upper constraints of task n. The implementation in accordance with the condensed procedure is shown in Algorithm 1.

Algorithm 1 HWBC for UVMS

Input:  Initial SOT of UVMS: $\mathcal{S}=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_N\}$;

Output:  Reference value: ${\mathit{\zeta }}_N^{*}$;

Initialization: ${\mathit{w}}^{*}$={};

for each $i\in [1,N]$ do

Set $n=i$ and initialize the optimization problem (42);

Solve the optimization problem and obtain ${\mathit{\zeta }}_n^{*}$;

Compute the slack variable ${\mathit{w}}_n^{*}={\mathit{J}}_n{\mathit{\zeta }}_n^{*}-{\dot{\mathit{x}}}_{tn}$;

${\mathit{w}}^{*}=\{{\mathit{w}}^{*},{\mathit{w}}_n^{*}\}$;

end for

return ${\mathit{\zeta }}_N^{*}:=\mathit{\zeta }$;

4. Continuous and Smooth Task Transitions for Vision/Force Control

If $\mathcal{S}$ does not change in the working process, the hierarchical controller can be solved based on the proposed HWBC. However, in practical operations, the controller needs to incorporate different SOTs at different stages, e.g., tasks being added or removed, priorities changing, etc. If ${\mathcal{S}}_a$ directly switches to ${\mathcal{S}}_b$, the control output may oscillate. Inspired by the continuous transition approach in [65], a smooth transition method for SOT switching in hybrid vision/force control for UVMSs is proposed in this part, which is called continuous HWBC (CWBC).

4.1. Task Addition/Removal

Consider a single task ${\mathcal{L}}_p$ that will be inserted into ${\mathcal{S}}_1=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_N\}$ with the priority of ${\mathcal{L}}_m\succ {\mathcal{L}}_p\succ {\mathcal{L}}_{m+1}$, in which ${\mathcal{P}}_1$ with ${\mathcal{S}}_1$ switches to ${\mathcal{P}}_2$ with ${\mathcal{S}}_2=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_p,\dots ,{\mathcal{L}}_N\}$. The transition method can be expressed as

$${\mathcal{P}}_{\alpha }:\underset{\mathit{\zeta }}{min}{\parallel {\mathit{w}}_n\parallel }^2$$

(44)

s.t.

$$\begin{array}{cc}\hfill & {\dot{\mathit{x}}}_{tn}^l+{\mathit{w}}_n⩽{\mathit{J}}_n\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tn}^u+{\mathit{w}}_n\hfill \end{array}$$

(45a)

$$\begin{array}{cc}\hfill & {\dot{\mathit{x}}}_{tk}^l+{\mathit{w}}_k^{*}⩽{\mathit{J}}_k\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tk}^u+{\mathit{w}}_k^{*},k=m+1,\dots ,n-1\hfill \end{array}$$

(45b)

$$\begin{array}{cc}\hfill & \alpha {\dot{\mathit{x}}}_{tp}^l+{\mathit{w}}_p^{*}⩽{\mathit{J}}_p\mathit{\zeta }-(1-\alpha ){\mathit{J}}_p{\mathit{\zeta }}_1^{*}⩽\alpha {\dot{\mathit{x}}}_{tp}^u+{\mathit{w}}_p^{*}\hfill \end{array}$$

(45c)

$$\begin{array}{cc}\hfill & {\dot{\mathit{x}}}_{tk}^l+{\mathit{w}}_k^{*}⩽{\mathit{J}}_k\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tk}^u+{\mathit{w}}_k^{*},k=1,\dots ,m\hfill \end{array}$$

(45d)

where $\alpha$ is the transition coefficient from 0 to 1. When $\alpha =0$, Equation (45c) becomes

$${\mathit{w}}_p^{*}⩽{\mathit{J}}_p\mathit{\zeta }-{\mathit{J}}_p{\mathit{\zeta }}_1^{*}⩽{\mathit{w}}_p^{*}$$

(46)

in which ${\mathcal{P}}_{\alpha }$ has the same solution as ${\mathcal{P}}_1$. Moreover, ${\mathit{\zeta }}_{\alpha }^{*}={\mathit{\zeta }}_2^{*}$ with $\alpha =1$, which indicates the end of the transition process. By increasing $\alpha$ from 0 to 1 during the transition, the feasible solution area of ${\mathcal{P}}_1$ will be moved to ${\mathcal{P}}_2$ continuously and smoothly. The implementation of (44) is described in Algorithm 2.

Algorithm 2 CWBC for UVMS: Adding a Task

Input:  Initial SOT of UVMS: ${\mathcal{S}}_1=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_N\}$, task to be added: ${\mathcal{L}}_p({\mathit{J}}_p,{\dot{x}}_{tp}^l,{\dot{x}}_{tp}^u)$;

Output:  Reference value: ${\mathit{\zeta }}_{\alpha }^{*}$;

1:  if is empty($\alpha$) then

2:      Initialization: $\alpha =0$;

3:  end if

4:  Solve: ${\mathit{\zeta }}_1^{*}=solve\left({\mathcal{P}}_1\right)$;

5:  Set transition tasks:

${\mathcal{L}}_{p,\alpha }={\mathcal{L}}_{p,\alpha }({\mathit{J}}_p,\alpha {\dot{x}}_{tp}^l+(1-\alpha ){\mathit{J}}_p{\mathit{\zeta }}_1^{*},\alpha {\dot{x}}_{tp}^u+(1-\alpha ){\mathit{J}}_p{\mathit{\zeta }}_1^{*})$;

6:  Set transition SOT for ${\mathcal{P}}_{\alpha }$:

${\mathcal{S}}_{\alpha }=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_m,{\mathcal{L}}_{p,\alpha },{\mathcal{L}}_{m+1},\dots ,{\mathcal{L}}_N\}$;

7:  Solve: ${\mathit{\zeta }}_{\alpha }^{*}=solve\left({\mathcal{P}}_{\alpha }\right)$;

8:  Update: $\alpha \leftarrow next(\alpha ,t)$;

9:  if $\alpha ⩾1$ then

10:      End of transition;

11:  end if

12:  return  ${\mathit{\zeta }}_{\alpha }^{*}:=\mathit{\zeta }$;

The transition coefficient $\alpha$ can be computed by a nonlinear activation function with respect to the start time of the period.

4.2. Task Priority Swapping

Considering two existing tasks ${\mathcal{L}}_i$ and ${\mathcal{L}}_j$ with the priority of ${\mathcal{L}}_i\succ {\mathcal{L}}_j$ in ${\mathcal{S}}_1=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_i,{\mathcal{L}}_j,\dots ,{\mathcal{L}}_N\}$, the following swapping process will be constructed:

$${\mathcal{P}}_{\alpha }:\underset{\mathit{\zeta }}{min}{\parallel {\mathit{w}}_n\parallel }^2$$

(47)

s.t.

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{tn}^l+{\mathit{w}}_n& ⩽{\mathit{J}}_n\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tn}^u+{\mathit{w}}_n\hfill \end{array}$$

(48a)

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{tk}^l+{\mathit{w}}_k^{*}& ⩽{\mathit{J}}_k\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tk}^u+{\mathit{w}}_k^{*},k=j+1,\dots ,n-1\hfill \end{array}$$

(48b)

$$\begin{array}{cc}\hfill \alpha {\dot{\mathit{x}}}_{tj}^l+{\mathit{w}}_j^{*}& ⩽{\mathit{J}}_j\mathit{\zeta }-(1-\alpha ){\mathit{J}}_j{\mathit{\zeta }}_{ij}^{*}⩽\alpha {\dot{\mathit{x}}}_{tj}^u+{\mathit{w}}_j^{*}\hfill \end{array}$$

(48c)

$$\begin{array}{cc}\hfill (1-\alpha ){\dot{\mathit{x}}}_{ti}^l+{\mathit{w}}_i^{*}& ⩽{\mathit{J}}_i\mathit{\zeta }-\alpha {\mathit{J}}_i{\mathit{\zeta }}_{ji}^{*}⩽(1-\alpha ){\dot{\mathit{x}}}_{ti}^u+{\mathit{w}}_i^{*}\hfill \end{array}$$

(48d)

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{tk}^l+{\mathit{w}}_k^{*}& ⩽{\mathit{J}}_k\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tk}^u+{\mathit{w}}_k^{*},k=1,\dots ,i-1\hfill \end{array}$$

(48e)

where $\alpha$ is still the transition coefficient from 0 to 1, and ${\mathit{\zeta }}_{ij}^{*}$ is the solution of ${\mathcal{P}}_{\alpha }$ when (48c) and (48d) are set as

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{tj}^l+{\mathit{w}}_j^{*}& ⩽{\mathit{J}}_j\mathit{\zeta }⩽{\dot{\mathit{x}}}_{tj}^u+{\mathit{w}}_j^{*}\hfill \end{array}$$

(49a)

$$\begin{array}{cc}\hfill (1-\alpha ){\dot{\mathit{x}}}_{ti}^l+{\mathit{w}}_i^{*}& ⩽{\mathit{J}}_i\mathit{\zeta }⩽(1-\alpha ){\dot{\mathit{x}}}_{ti}^u+{\mathit{w}}_i^{*}\hfill \end{array}$$

(49b)

while ${\mathit{\zeta }}_{ji}^{*}$ is the solution of ${\mathcal{P}}_{\alpha }$ when (48c) and (48d) become

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{ti}^l+{\mathit{w}}_i^{*}& ⩽{\mathit{J}}_i\mathit{\zeta }⩽{\dot{\mathit{x}}}_{ti}^u+{\mathit{w}}_i^{*}\hfill \end{array}$$

(50a)

$$\begin{array}{cc}\hfill \alpha {\dot{\mathit{x}}}_{tj}^l+{\mathit{w}}_j^{*}& ⩽{\mathit{J}}_j\mathit{\zeta }⩽\alpha {\dot{\mathit{x}}}_{tj}^u+{\mathit{w}}_j^{*}.\hfill \end{array}$$

(50b)

The implementation of (47) is described in Algorithm 3.

Algorithm 3 CWBC for UVMS: Switching Task Priorities

Input: Initial SOT of UVMS $\mathcal{S}=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_{i-1},{\mathcal{L}}_i,{\mathcal{L}}_j,{\mathcal{L}}_{j+1},\dots ,{\mathcal{L}}_N\}$, task to be switched: ${\mathcal{L}}_i$ and ${\mathcal{L}}_j$;

Output: Reference value: ${\mathit{\zeta }}_{\alpha }^{*}$

1:  if is empty($\alpha$) then

2:      Initialization: $\alpha =0$;

3:  end if

4:  Set transition tasks: ${\mathcal{L}}_{i,\alpha }={\mathcal{L}}_{i,\alpha }({\mathit{J}}_i,(1-\alpha ){\dot{x}}_{ti}^l,(1-\alpha ){\dot{x}}_{ti}^u)$;

5:  Set transition SOT for ${\mathcal{P}}_{ij,\alpha }$: ${\mathcal{S}}_{ij,\alpha }=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_{i-1},{\mathcal{L}}_{i,\alpha },{\mathcal{L}}_j,{\mathcal{L}}_{j+1},\dots ,{\mathcal{L}}_N\}$;

6:  Solve: ${\mathit{\zeta }}_{ij}^{*}=solve\left({\mathcal{S}}_{ij,\alpha }\right)$;

7:  Set transition tasks: ${\mathcal{L}}_{j,\alpha }={\mathcal{L}}_{j,\alpha }({\mathit{J}}_j,\alpha {\dot{x}}_{tj}^l,\alpha {\dot{x}}_{tj}^u)$;

8:  Set transition SOT for ${\mathcal{P}}_{ji,\alpha }$: ${\mathcal{S}}_{ji,\alpha }=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_{i-1},{\mathcal{L}}_{j,\alpha },{\mathcal{L}}_i,{\mathcal{L}}_{j+1},\dots ,{\mathcal{L}}_N\}$;

9:  Solve: ${\mathit{\zeta }}_{ji}^{*}=solve\left({\mathcal{S}}_{ji,\alpha }\right)$;

10:  Set transition tasks: ${\mathcal{L}}_{ci,\alpha }={\mathcal{L}}_{ci,\alpha }({\mathit{J}}_i,(1-\alpha ){\dot{x}}_{ti}^l+\alpha {\mathit{J}}_i{\mathit{\zeta }}_{ji}^{*},(1-\alpha ){\dot{x}}_{ti}^u+\alpha {\mathit{J}}_i{\mathit{\zeta }}_{ji}^{*})$;

11:  Set transition tasks: ${\mathcal{L}}_{cj,\alpha }={\mathcal{L}}_{cj,\alpha }({\mathit{J}}_j,\alpha {\dot{x}}_{tj}^l+(1-\alpha ){\mathit{J}}_j{\mathit{\zeta }}_{ij}^{*},\alpha {\dot{x}}_{tj}^u+(1-\alpha ){\mathit{J}}_j{\mathit{\zeta }}_{ij}^{*})$;

12:  Set transition SOT for ${\mathcal{P}}_{\alpha }$: ${\mathcal{S}}_{\alpha }=\{{\mathcal{L}}_1,\dots ,{\mathcal{L}}_{i-1},{\mathcal{L}}_{ci,\alpha },{\mathcal{L}}_{cj,\alpha },{\mathcal{L}}_{j+1},\dots ,{\mathcal{L}}_N\}$;

13:  Solve: ${\mathit{\zeta }}_{\alpha }^{*}=solve\left({\mathcal{S}}_{\alpha }\right)$;

14:  Update: $\alpha \leftarrow next(\alpha ,t)$;

15:  if $\alpha ⩾1$  then

16:      End of transition;

17:  end if

18:  return  ${\mathit{\zeta }}_{\alpha }^{*}:=\mathit{\zeta }$;

5. Simulation Results

In this section, a visualized numerical simulation is conducted to evaluate the proposed controller. The system dynamics and control algorithm are calculated by MATLAB (version: R2024b), and the simulation visualization is performed by CoppeliaSim, as shown in Figure 3. The model of the main vehicle is based on the Kambara underwater vehicle model in [66], and a sway thruster is added to the thrust mapping matrix to render the vehicle fully actuated. The parameters of the attached manipulator can be seen in Appendix A. The links are assumed to be symmetric cylinders to simplify the calculation of the hydrodynamic forces, and the details can be found in the SIMURV 4.1 paper [2]. The optimization problem is solved by the open-source solver CasADi in [67].

The dynamic velocity controller is detailed in [42]. To verify the robustness of the controller, we apply a time-varying external force on the end-effector of the UVMS as a disturbance and add a 10% error to the model parameters. The external force is set as ${\mathit{f}}_d={[0,10\sin \left(0.6t\right)\left(\mathrm{N}\right),10\cos \left(0.6t\right)\left(\mathrm{N}\right),0,0,0]}^{\mathrm{T}}$ in the inertial frame. The parameters of the contact model are set as $x_e=1.14$, $k_e=1000$ and $b_e=300$, and Gaussian white noise of 10 db is added in the force measurement in the simulation.

Two common cases that require contact force tracking are considered in this section. One involves maintaining a contact force with a fixed point, such as button pressing. The other requires maintaining the desired contact force while moving the contact point, such as welding or flaw detection. The simulation is conducted based on these two cases.

The tasks for the hybrid vision/force control mission are listed in

Table 2. Task list.

Label	Variable	Description	Gain
${\mathcal{L}}_c$	${\mathit{s}}_c={(x_n,y_n)}^{\mathrm{T}}$	Camera’s FOV	–
${\mathcal{L}}_f$	$f_n$	Normal contact force	${\lambda }_{tfn}=0.0036$
${\mathcal{L}}_q$	${\mathit{\eta }}_{eq}$	End-effector orientation	${\lambda }_{tq}=1.5$
${\mathcal{L}}_{pn}$	${\mathit{a}}_n$	End-effector norm trajectory	${\lambda }_{tpn}=0.7$
${\mathcal{L}}_{ps}$	${\mathit{s}}_{ps}$	End-effector trajectory	${\lambda }_{tps}=1.5$
${\mathcal{L}}_{pr}$	${\mathit{\eta }}_{2,pr}={(\psi ,\theta )}^{\mathrm{T}}$	Main vehicle attitude holding	${\lambda }_{tpr}=1$

. Depending on whether task ${\mathcal{L}}_f$ is active or not, the SOTs are defined as ${\mathcal{S}}_1=\{{\mathcal{L}}_c,{\mathcal{L}}_q,{\mathcal{L}}_{pn},{\mathcal{L}}_{ps},{\mathcal{L}}_{pr}\}$ and ${\mathcal{S}}_2=\{{\mathcal{L}}_c,{\mathcal{L}}_f,{\mathcal{L}}_q,{\mathcal{L}}_{pn},{\mathcal{L}}_{ps},{\mathcal{L}}_{pr}\}$.

5.1. Fixed Contact Case

A reference trajectory for the end-effector in the fixed contact case is considered in this part. The initial states of the UVMS are set as

$$\left\{\begin{array}{c}{\mathit{\eta }}_2={\mathit{0}}_{3\times 1}\hfill \\ \mathit{s}={[-0.263,-0.530,0.502]}^{\mathrm{T}}\hfill \\ {\mathit{\eta }}_{eq}={[0.502,0.500,0,0.866,0]}^{\mathrm{T}}\hfill \\ \mathit{q}={[0,-60,-60,0,0,0]}^{\mathrm{T}}/180\times \pi \hfill \\ \mathit{\zeta }=0_{12\times 1}\hfill \end{array}\right.$$

(51)

and the fixed reference states are

$$\left\{\begin{array}{c}{f}_n^{*}=10\hfill \\ {\theta }^{*}=0\hfill \\ {\mathit{s}}^{*}={[-0.86,-0.74,0.3516]}^{\mathrm{T}}\hfill \\ {\mathit{\eta }}_{eq}^{*}={[0.707,0,0.707,0]}^{\mathrm{T}}\hfill \end{array}\right..$$

(52)

The results of the fixed contact case based on OWBC and the proposed CWBC are shown in Figure 4, Figure 5, Figure 6, Figure 7 and Figure 8. Overall, both of the controllers are capable of driving the end-effector to the desired point and maintaining a stable contact force.

The image moments in the tangential directions of the target plane and the end-effector quaternions are demonstrated in Figure 4 and Figure 5. It can be observed that both the tangential image moments and the end-effector quaternions exhibit similar performance for the two controllers.

The main difference is reflected in the normal direction in Figure 6. After task ${\mathcal{L}}_f$ is introduced into the SOT at about 4 s by $a_n<0.55$, the contact force of CWBC in green converges to the reference value more quickly and stably than the red curve of OWBC. Although the force control tasks are the same in the two controllers, the better performance comes from the strictly hierarchical priorities in CWBC.

The system velocities are demonstrated in Figure 7 and Figure 8. It can be observed that CWBC exhibits greater fluctuations in the velocities of the main vehicle, whereas OWBC shows larger fluctuations in joint motions. This results from the fact that OWBC does not impose strict task prioritization, and its performance aligns more closely with the overall energy consumption by moving the joint in the first place.

5.2. Moving Contact Case

A circular reference trajectory for the UVMS is considered and designed as

$$\left\{\begin{array}{c}{f}_n^{*}=10\hfill \\ {\theta }^{*}=0\hfill \\ x_n^{*}=-0.86+0.3\sin (0.3t+2)\hfill \\ y_n^{*}=-0.74+0.3\cos (0.3t+2)\hfill \\ a_n^{*}=0.3516\hfill \\ {\mathit{\eta }}_{eq}^{*}={[0.707,0,0.707,0]}^{\mathrm{T}}\hfill \end{array}\right.$$

(53)

and the other conditions are the same as in Section 5.1. The results are shown in Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13. The contact condition between the environment and the end-effector is subjected to change in order to evaluate the controller’s adaptability to different situations during the tracking process, which is achieved by changing the equilibrium position and stiffness coefficient to $x_e=1.12$ and $k_e=600$.

In the moving case, similar performance can be seen, as shown in Figure 9 and Figure 10. One can notice that there is little difference between the curves of the tangential image moments and the end-effector quaternions for the two controllers.

The normal image moment and contact force curves are shown in Figure 11. Although both of the controllers can maintain the reference force, thanks to the strict priority of the force control task, the result of CWBC is obviously better than that of OWBC, with fewer fluctuations. At about 12 s, the normal contact forces suddenly increase when the contact condition changes and then gradually stabilize to the expected value. The adaptability of the two controllers can be certified.

Figure 12 and Figure 13 exhibit the results regarding the system velocities. The difference between the velocities of the main vehicle and the joint velocities remains similar to that described in Section 5.1, in which CWBC shows greater fluctuations from the main vehicle movements and OWBC shows larger fluctuations in joint motions.

5.3. Parameter Selection

The control performance of the proposed CWBC controller under different control gains in the force control task is demonstrated in this part to show the selection process of the control gains. All the initial and reference states remain consistent with those specified in Section 5.1. The fixed contact case is repeated three times with different ${\lambda }_{tfn}$: Case 1 with ${\lambda }_{tfn}$, Case 2 with ${\lambda }_{tfn}/5$ and Case 3 with $5{\lambda }_{tfn}$.

The results of the three cases are shown in Figure 14. It can be observed that the the performances differs notably. Specifically, Case 2 demonstrates slow tracking performance accompanied by gradual fluctuations, while Case 3 achieves fast tracking at the cost of noticeable oscillations. In general, the control force of Case 1 achieves balanced overall performance in terms of tracking speed and fluctuations.

5.4. Comparative Results

A comparison of ${\mathcal{S}}_1$ to ${\mathcal{S}}_2$ via smooth transitions with CWBC and by direct switching with HWBC is demonstrated in this part. The reference trajectory and parameters are the same as in Section 5.2, without contact condition changes, and the results are shown in Figure 15 and Figure 16.

The contact forces are shown in Figure 15. In general, both of the curves converge to the desired values stably under the two controllers, while the two methods exhibit different behaviors during the transition phase. The force result in HWBC shows larger fluctuations, whereas CWBC approaches the desired value more smoothly. Significant fluctuations are observed in the velocity along the force control direction in Figure 16, which is consistent with the variation in the contact force. This demonstrates the effectiveness of the smooth transition method in CWBC to optimize the switching performance when the force control task is introduced in the SOT.

The root mean square error (RMSE), settling time (ST) and overshoot (OS) of the two controllers are demonstrated in

Table 3. Result comparison.

	RMSE (N)	ST (s)	OS (N)
CWBC	1.6	5.9	1.8
HWBC	1.96	6	8.1

. The RMSE and the ST are relatively close, indicating that the convergence rates of the two controllers are similar. A notable distinction between the two controllers is observed in the OS. The OS of CWBC is 1.8 (N), while the OS of HWBC is 8.1 (N). This is not only reflected in the improved tracking performance but is particularly critical in intervention tasks, as excessive overshoot of the contact force could potentially lead to system damage or instability.

6. Conclusions

In this paper, we have proposed a whole-body vision/force controller to complete the manipulation task of a UVMS. A HVS model based on reprojecting image moments and quaternions was firstly established. Then, several tasks were constructed to establish a hybrid vision/force control framework for UVMSs, and a HWBC framework was constructed to generate a solution that satisfied the priorities. To obtain a smooth output during the transition phrase, a continuous transition for SOT switching is presented. Simulation results verify the excellent performance and robustness of the proposed hybrid vision/force controller. In the near future, we will integrate trajectory planning into the controller and try to conduct real-world experiments.