UVMS task-priority planning framework for underwater task goal classification optimization

This paper presents a task prioritization strategy based on a generic underwater task goal classification transformation for multitasking underwater operational tasks: attitude control, floating manipulation, collision-free motion, especially optimizing trajectory of the end-effector of an underwater vehicle manipulator system (UVMS) in a complex marine environment. The design framework aims to divide the complex underwater operational tasks into UVMS executable generic task combinations and optimize the resource consumption during the whole task. In order to achieve the corresponding underwater task settings, the system needs to satisfy different task scheduling structures. We consider the actual application scenarios of the operational goals and prioritize and define each category of task hierarchy accordingly. Multiple tasks simultaneously enable fast adaptation to UVMS movements and planning to complete UVMS autonomous movements. Finally, an underwater vehicle manipulator system implements the task prioritization planning framework for a practical scenario with different constraints on different goals. We quickly and precisely realize the interconversion of different tasks under goal constraints. The autonomous motion planning and real-time performance of UVMS are improved to cope with the increasing operational task requirements and the complex and changing practical engineering application environments.

Introduction

As the largest ecosystem on Earth, the ocean regulates not only global climate change but also supports global economic development by providing humans with productive resources such as protein, water, and energy. In the past decades, understanding and developing the oceans require various high technologies and equipment, including underwater robots, which have been the focus of attention worldwide. In particular, the underwater vehicle manipulator system (UVMS) plays a pivotal role in national projects [RAUVI (Sanz et al., 2011) or ARCHROV (Casalino et al., 2012)] and European projects [FP7 STREP TRIDENT (Sanz et al., 2010), PANDORA (Heshmati-Alamdari et al., 2018), MORPH, Eurofelts2 (Olguin-Diaz et al., 2013), etc.] about underwater robots, which indicates that the emergence as one of the most powerful tools for human research and exploitation activities of marine resources.

Due to the redundancy of degrees of freedom, nonlinearity, strong coupling, time variation, high dimensionality, low bandwidth of sensor data acquisition, and interference by hydrodynamic forces, the system design, autonomous control, and operation planning of UVMS has become a challenging topic in the field of underwater robotics at home and abroad. Since the underwater environment is different from the surface, the realization of the control task of the robot system becomes more difficult. The control problem of UVMS as a redundant system is challenging to be solved completely. However, the redundancy characteristics of the system can be explored reasonably to perform multiple tasks simultaneously and ensure that each task is eventually completed. Therefore, it is necessary to coordinate the motions of the body and the manipulator when meeting the operational task requirements (Fujie et al., 2020). The most commonly used methods for motion planning are the minimum parametric solution method, the weighted pseudo-inverse method, the gradient projection method, and the task prioritization method to achieve UVMS motion planning (Whitney, 1969). On top of this method, some scholars have improved and extended it by introducing the weight matrix into the robot body and manipulator's joints to achieve weighted parametric optimality in robot configuration. Gianluca Antonelli in Italy proposed a task priority-based planning method for UVMS motion, which sets the primary and secondary tasks, prioritizes to ensure the completion of the primary task, and completes the secondary tasks as much as possible under the premise of completing the primary task (Han et al., 2011). Task prioritization methods are used to decide the order of task execution according to the task priority level when multiple tasks are in conflict and are often used to solve redundancy problems. For example, Antonelli and Chiaverini (1998), Cieslak et al. (2015), Changmi (2022), and Gancet et al. (2016) used this approach. Tang et al. (2017) proposed an acceleration level task priority redundancy decomposition method. Simetti et al. (2018) proposed a task priority approach that can be applied to different scenarios in UVMS. The multitask weight gradient method has also been used for secondary task weight assignment (Wang et al., 2017). Sotiropoulos et al. (2015) proposed a fast motion planning algorithm for UVMS in semi-structured environments. Youakim et al. (2017) used different motion planning methods to simulate and analyze the motion of an underwater manipulator, solving the problem of “which planner to choose”. Depending on the specific situation, different strategies are needed for the dual-arm problem (Moe et al., 2014; Bae et al., 2018) and the cooperative operation problem (Xuefeng and Xinqian, 2000; Chang, 2004; Conti et al., 2015; Simetti and Casalino, 2016). Some researchers have developed analysis software packages, such as UWSim (Prats et al., 2012) and MoveIt! For example, it is guaranteed that the end position pose of UVMS reaches the desired value, and then constraints such as system energy consumption or manipulator limit are implemented. Subsequently, based on this algorithm, researchers introduced the fuzzy theory to adjust the parameters online for the problem of optimal allocation of primary and multiple secondary tasks (Antonelli and Chiaverini, 2000) to improve task self-adaptability and achieve multitask planning. The literature (Podder and Sarkar, 2000) decomposed the overall motion into the body and the manipulator motion based on the response differences between the body and the manipulator systems in the dynamics model. Based on the UVMS kinetic model, Huang Hai et al. of Harbin Engineering University proposed pairwise optimization combined with a genetic algorithm for trajectory planning of UVMS motion (Huang et al., 2016), which obtained a set of hull positions and manipulator joint angles by genetic, crossover, and variation operations. If the set of sequences satisfied the error range, then the adaptive function was compared and continuously iterated to obtain the optimal global solution.

However, most of the above studies consider a single task during planning. Few have come to deal with task coordination and planning of transitions between tasks in underwater vehicle manipulator system. Tasks can be kinematic (position) or kinetic (force) goals for robot motion control. The ability of a robot to accomplish a goal depends on its physical limitations and surrounding environmental obstacles. Nakamura et al. (1987) introduced the concept of task prioritization associated with the inverse problem of redundant degree manipulator kinematics to determine joint motions with sequential tasks. A general framework for managing multiple tasks of highly redundant robotic systems was proposed in Siciliano and Slotine (1991), but only equality tasks were considered. Researchers tended to transform inequality tasks into equality tasks with the highest priority (Sentis and Khatib, 2005; Mansard and Chaumette, 2007), which could lead to discontinuities. Mansard et al. (2009) used a weighted solution to overcome this drawback with a limited number of inequalities. Inspired by the sequential least-squares formulation of the classical task framework (Jin, 1996), Kanoun et al. (2011) extended the task prioritization framework to inequality tasks. They applied the algorithm to the humanoid robot HRP-2. While much work has been done at the UVMS control level, many scholars have proposed many approaches to the trajectory tracking control of UVMS. Han et al. considered the effect of external disturbances and proposed nonlinear H-optimal control with disturbance observer for implementing tracking control of UVMS in Ref. (Han and Chung, 2008). Mohan et al. proposed an indirect adaptive control method based on the Kalman filter for autonomous operation of UVMS, which overcomes the drawbacks of existing disturbance observer and direct adaptive control. The method can target the consideration of load compensation, underwater currents, or external disturbance compensation. Xu et al. proposed a neuro-fuzzy-based intelligent control algorithm for operational control of UVMS. The proposed decentralized neural network compensator was used to estimate UVMS dynamics, which can better cope with load variations and hydrodynamic disturbances (Xu et al., 2012). Olguin-Diaz et al. (2013) proposed a force-motion-based control framework for operational control of UVMS, which does not require prior modeling of UVMS dynamics and applies a second-order model-free sliding mode control method to calculate the control rate in the force-motion framework. Lynch and Ellery (2014) combined feedback and a feedforward approach to achieve robot attitude and position stabilization for effective manipulator control. However, the problem of task planning as the highest level of the control system, task goal assignment, and transformation during under-water tasks performed by UVMS remains a critical issue in the field of robotics. Although studies consider the uncertainty of its operation in the real world, this is an open research area that needs to produce robust planning algorithms.

Optimization is a high-level task in trajectory planning to seek safety when per-forming tasks in a cluttered, dense, and complex underwater environment. Since resources are limited in the marine environment, mission planning needs to be done under the given constraints, solving its underwater constraints, optimization objectives, physical limitations, and resource invocation problems. Therefore, this paper focuses on the UVMS-based task prioritization strategy. Here, we define the hierarchy of underwater control tasks and their priority relationships. The lowest priority optimization objective is a linear constrained quadratic programming problem. Multiple objective functions with priority equation constraints on the tasks define this optimization problem in this study. The main contributions of this paper are twofold: (1) We develop a UVMS-based task prioritization framework to select the priority of the control tasks in terms of the hierarchy of tasks used and the difference in the priority of the objectives. We give the final planning results in task sequences and resource allocation schemes for each phase, which has been achieved successfully. In addition, we deal with the difficulties of multiple task interconversion activation and multi-objective classification definition at one time. Furthermore, the proposed task prioritization framework can extend widely to underwater operations in real scenarios. (2) Based on the highest objective optimization as the lowest priority objective setting, we propose a soft constraint optimization method to avoid possible collisions and expect the optimization trajectory to reach the ideal trajectory to ensure the underwater task execution of high quality.

The rest of this paper is organized as follows. In Section UVMS system modeling, we complete the modeling of the UVMS. Section Task Planning presents the task prioritization framework composition, defines the division of control objectives and control tasks, and provides detailed definitions of the five categories of tasks. Section Trajectory planning explains the trajectory optimization algorithm and proposes an improved collision avoidance method based on soft constraints. In Section Simulation results, we develop code for the task manager and part of the kinematic control layer, do the most important related simulation experiments, and verify the method's effectiveness with two case studies. Finally, Section Conclusions gives conclusions and future work.

UVMS system modeling

As shown in Figure 1, the UVMS consists of a vehicle and a seven-function manipulator, which is very flexible and well suited for task scenarios with continuous underwater operations.

FIGURE 1

Figure 1. UVMS system structure diagram.

As shown in Figure 2, where the fixed inertial frame of the world < w >, the vehicle frame of the UVMS < v >, the sensor frame < s > and the operational target frame < o >, the tool frame < t >.

FIGURE 2

Figure 2. The UVMS and its relevant frames.

Kinematics of UVMS

The kinematics of the end-effector needs to be represented by the whole system, and the system's structure c is described by a vector of the parameters of the degrees of freedom of each part of the structure.

$\begin{array}{ll}c=\left[\begin{array}{c}q\\ \eta \end{array}\right]& (1)\end{array}$

where q is the description vector of the underwater manipulator

$\begin{array}{ll}q=\left[\begin{array}{c}{q}_1\\ :\\ q_n\end{array}\right]& (2)\end{array}$

where η is the description vector of the vehicle

$\begin{array}{ll}\eta =\left[\begin{array}{c}{\eta }_1\\ {\eta }_2\end{array}\right]\in R^6& (3)\end{array}$

The relationship between the Euler angular derivative (RPY) and the angular velocity υ₂ of the vehicle chassis is shown below.

$\begin{array}{ll}{\eta }_1=\left[\begin{array}{c}x\\ y\\ z\end{array}\right],{\eta }_2=\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]& (4)\end{array}$

$\begin{array}{ll}\stackrel{.}{y}=\left[\begin{array}{c}\stackrel{.}{q}\\ \nu \end{array}\right]\nu =\left[\begin{array}{c}{\nu }_1\\ {\nu }_2\end{array}\right]\begin{array}{c}{\nu }_1{=}^{v}v\\ {\nu }_2{=}^{v}w\end{array}& (5)\end{array}$

where the relationship between the Euler angular rate $(\stackrel{.}{\varphi },\stackrel{.}{\theta },\stackrel{.}{\psi })$ and the angular velocity $(\begin{array}{c}p,q,r\end{array})$ of the object in the ontological coordinate system is given by the following equation.

$\begin{array}{ll}\{\begin{array}{l}\stackrel{.}{\varphi }=p+q(sin\varphi tan\theta )+r(cos\varphi tan\theta )\\ \stackrel{.}{\theta }=0+q(cos\varphi )+r(-sin\varphi )\\ \stackrel{.}{\psi }=0+q(sin\varphi /cos\theta )+r(cos\varphi /cos\theta )\end{array}& (6)\end{array}$

Putting it into matrix form yields

$\begin{array}{ll}\left[\begin{array}{c}\stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\\ \stackrel{.}{\psi }\end{array}\right]=J\left[\begin{array}{c}p\\ q\\ r\end{array}\right]& (7)\end{array}$

For the angular velocity, the rotation matrix J(η) is

$\begin{array}{ll}J(\eta )=\left(\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi /cos\theta & cos\varphi /cos\theta \end{array}\right)& (8)\end{array}$

i.e.,

$\begin{array}{ll}\left[\begin{array}{c}\stackrel{.}{\varphi }\\ \stackrel{.}{\theta }\\ \stackrel{.}{\psi }\end{array}\right]=\left(\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi /cos\theta & cos\varphi /cos\theta \end{array}\right){\upsilon }_2& (9)\end{array}$

Therefore, the following relationship exists between the world frame < w > of the Earth and the vehicle frame < v > of UVMS.

$\begin{array}{ll}{}^w\stackrel{.}{x}=J^v\stackrel{.}{y}& (10)\end{array}$

Dynamics of UVMS

This part is modeled by the Lagrangian method. The kinetic energy of UVMS consists of two parts: the translational kinetic energy and the rotational kinetic energy. The mathematical expression is

$\begin{array}{ll}{T}_i=\frac{1}{2}{m}_i{\stackrel{.}{r}}_i^2+\frac{1}{2}{J}_i{\omega }_i^2(i=0,1,2)& (11)\end{array}$

where

$\begin{array}{r}{\omega }_0=\stackrel{.}{\alpha }{Z}_0;\\ {\omega }_1=(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1)Z_1;\\ {\omega }_2=(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2)Z_2& (12)\end{array}$

The kinetic energy of the entire single-arm system is

$\begin{array}{l}T={\displaystyle \sum _{i=0}^2}{T}_i\\ =\frac{1}{2}{m}_0\left({\stackrel{.}{x}}_0^2+{\stackrel{.}{y}}_0^2\right)+\frac{1}{2}{J}_0{\omega }_0^2+\frac{1}{2}{m}_1\left({\stackrel{.}{x}}_1^2+{\stackrel{.}{y}}_1^2\right)\\ +\frac{1}{2}{J}_1{\omega }_1^2+\frac{1}{2}{m}_2\left({\stackrel{.}{x}}_2^2+{\stackrel{.}{y}}_2^2\right)+\frac{1}{2}{J}_2{\omega }_2^2\\ =f_1\left({\stackrel{.}{x}}_0^2+{\stackrel{.}{y}}_0^2\right)+f_2{\stackrel{.}{\alpha }}^2+f_3{\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1\right)}^2+f_4{\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)}^2\\ +f_5\stackrel{.}{\alpha }\left({\stackrel{.}{x}}_{0}cos\alpha -{\stackrel{.}{y}}_{0}sin\alpha \right)+f_6\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1\right)\\ \left[{\stackrel{.}{x}}_{0}cos\left(\alpha +{\theta }_1\right)-{\stackrel{.}{y}}_{0}sin\left(\alpha +{\theta }_1\right)\right]\\ +f_7\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)[{\stackrel{.}{x}}_{0}cos\left(\alpha +{\theta }_1+{\theta }_2\right)\\ -{\stackrel{.}{y}}_{0}sin\left(\alpha +{\theta }_1+{\theta }_2\right)]\\ +f_8\stackrel{.}{\alpha }\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1\right)cos{\theta }_1+f_9\stackrel{.}{\alpha }\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)cos\left({\theta }_1+{\theta }_2\right)\\ +f_{10}\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1\right)\left(\stackrel{.}{\alpha }+{\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)cos{\theta }_2& (13)\end{array}$

Here, assuming the gravitational potential energy of the system V = 0, the Lagrangian function of the system: L = T − V, and using the Lagrangian equation:

$\begin{array}{ll}Q=\frac{d}{d_t}\left(\frac{\partial L}{\partial {\stackrel{.}{q}}_0}\right)-\frac{\partial L}{\partial q_0}& (14)\end{array}$

where ${\stackrel{.}{q}}_0={\left[\begin{array}{ccccc}{\stackrel{.}{x}}_0& {\stackrel{.}{y}}_0& \stackrel{.}{\alpha }& {\stackrel{.}{\theta }}_1& {\stackrel{.}{\theta }}_2\end{array}\right]}^T$ is the state vector of the system; $Q={\left[\begin{array}{ccccc}0& 0& {\tau }_0& {\tau }_1& {\tau }_2\end{array}\right]}^T$ is the control torque matrix of the system.

Substituting into Equation (14) yields the following kinetic equation.

$\begin{array}{ll}D(q_0){\ddot{q}}_0+H(q_0,{\stackrel{.}{q}}_0){\stackrel{.}{q}}_0=Q& (15)\end{array}$

where D(q₀) is the 5^*5-dimensional symmetric, positive definite mass matrix. $H(q_0,{\stackrel{.}{q}}_0)$ is the 5^*1 dimensional matrix with Koch forces and centripetal forces.

At this point, the dynamics model of the UVMS is established using the Lagrangian modeling method. This dynamical model provides the basis for the task priority control of the underwater manipulator and the underwater robot. Due to the dynamical coupling effect between the manipulator and the base, the motion of the base, and the end effector in free-floating mode is highly dependent on the joint trajectory. Therefore, rational design of task planning solves the problem of multitasking underwater operation tasks such as precise positioning, floating manipulation, or collision-free motion.

Task planning

A two-tier framework for task planning

The UVMS control system in this research consists of task planning, trajectory planning, and motion control. As the top layer of the manipulator control system, task planning is responsible for receiving, analyzing, and disassembling task targets. The purpose is to divide complex task targets into action sequences that the manipulator can directly plan and execute. Due to the diversity of ways for the manipulator to complete tasks, task planning also involves scheduling various types of resources for the manipulator system to optimize resource consumption during the entire task. We give the final planning result through task combination sequences and activation methods. Figure 3 shows the two-layer framework of task planning designed in this paper.

FIGURE 3

Figure 3. Two-tier framework diagram for task planning.

The implementation architecture of the task planning approach proposed in this research is shown in Figure 4.

1. Task Manager: Notifies the Kinematic Control Layer about the actions that must be executed based on the current mission.

2. Kinematic Control Layer: Implements the task priority control framework and generates the system reference velocities. The kinematics control layer mainly manages the end position, joint angle, and speed of UVMS in real-time. For the end-effector, it moves according to the motion trajectory generated by the optimization algorithm designed in this paper and according to the specified motion parameters.

3. Dynamic Control Layer: Tracks the system reference velocities by generating appropriate force/torque references for the vehicle and manipulator.

FIGURE 4

Figure 4. General architecture of the task prioritization approach.

In the simulation, we develop the code for the Mission Manager and parts of the Kinematic Control Layer.

Task-priority handling strategy

Considering the task requirements for autonomous UVMS underwater operations, we designed two categories of control tasks.

1. Reactive control task (R): Capable of tracking feedback-generated reference rate $\stackrel{.}{x}$.

2. Non-Reactive control task (NR): Defined directly in specific task velocity space. Thus, the reference velocity tracked is not generated by feedback.

Also, depending on the type of task, we have designed two different types of objectives.

1. Equality control objective (E): Given by x(c) = x₀, which is given as a constant equation.

2. Inequality control objective (I): Given by x(c) ≥ x_min or x(c) ≤ x_max, which is given as an interval range.

Among them, the objectives can be classified into the following five major categories according to their categories, and this feature assigns priority to each task. The constraint tasks have the highest priority, and the optimization tasks have the lowest priority:

1. Constraints (C): Objective related to the physical constraints of the system.

2. Safety (S): Objectives related to the safety of the robot.

3. Operational Prerequisite (P): Objective that is a prerequisite for the given action.

4. Action Defining (AD): Action-oriented objectives.

5. Optimization (O): Trajectory optimization objective.

Finally, the UVMS task priorities designed in Table 1 are described in conjunction with the definitions of goal types, goal categories, and control tasks elaborated above.

TABLE 1

Table 1. Control tasks and prioritization.

In Table 1, for task-related factors, the typical underwater tasks of UVMS are first decomposed into the following five basic task categories according to their requirements for autonomous underwater operations.

1. MAC—Assure safe minimum altitude control task [R, I, P]: used to keep the UVMS altitude above a certain threshold.

2. HA—Horizontal attitude task controls the horizontal attitude [R, I, S]: it is critical to maintaining the vehicle-level relative to the whole world frame < w >.

3. LA—Altitude control task, also known as the landing task [R, E, AD]: This action-defining task has the same priority as the vehicle position. The minimum task altitude is not enabled there because we need to land; therefore, UVMS needs to be below a fixed minimum altitude threshold.

4. AT—Target Control Alignment Target Task to Task Alignment [R, I, P]: this is a prerequisite task with a higher priority than the action-defining task. The error range of the inequality is equal to 0.07 m.

5. PC—The vehicle position control task [R, E, AD]: This action-defining task has a lower priority.

Next, we will explain these five types of tasks in detail regarding their task priority selection relationships and reference relationships.

Highest priority task “MAC”

The minimum altitude control task “MAC” set in this paper is the security control target. Therefore, its priority must be higher than the actions that define the objective, such as task “PC”. Task “MAC” is the highest priority because avoiding collisions with the seafloor is more important than maintaining the vehicle's horizontal altitude objective during UVMS underwater tasks. This task improves the ability of the UVMS to avoid collisions with the seafloor.

The following Jacobian relationship characterizes the task “MAC”:

$\begin{array}{ll}{}^w{\stackrel{.}{x}}_{mac}={J_{mac}}^v\stackrel{.}{y}& (16)\end{array}$

where ${}^w{\stackrel{.}{x}}_{mac}\in R^6$ represents the task description, $J_{mac}\in R^{6\times 13}$ represents the Jacobian matrix of this task; J_mac has three rows corresponding to the dimensions of the reference velocity ($\stackrel{.}{x}$_mac), where $\stackrel{.}{x}$_mac has only linear components.

$\begin{array}{ll}{}^w{\dot{x}}_{mac}{=}^w{v}_{3\times 1}={\left[\begin{array}{ccc}{0}_{3\times 7}& {}^w{R}_v{}_{3\times 3}& 0_{3\times 3}\end{array}\right]}^v\left[\begin{array}{c}{\dot{q}}_{7\times 1}\\ v_{3\times 1}\\ {}^v{\omega }_{3\times 1}\end{array}\right]& (17)\end{array}$

The task “MAC” is based on an inequality objective, the main goal of which is to ensure that the vehicle maintains its altitude above a certain minimum threshold.

$\begin{array}{ll}{h}_{\text{actual}}\ge h_{\text{mi}{\text{n}}_{-}\text{thresh}}& (18)\end{array}$

Because the control variable uses the convention [X Y Z], the task references we compute have the following structure:

$\begin{array}{ll}w{\stackrel{.}{x}}_{max}=\left[\begin{array}{c}0\\ 0\\ {}^w{v}_z\end{array}\right]& (19)\end{array}$

$\begin{array}{ll}w{\stackrel{.}{x}}_{max}=k\left[(d_{limit}+\Delta ){-}^w{d}_{sensor}\right]& (20)\end{array}$

where k is the control gain, d_limit is the desired minimum distance from the seafloor, Δ is the safety distance at which the activation of the task starts to trigger, ${}^w{d}_{sensor}$ is the third component of the distance vector, measured by the sensor and projected on the world frame < w >.

Here, we provide a direction of the velocity we need to control the movement along the z-axis. We use the same vehicle position Jacobi matrix but only select the components associated with the z-axis. To ensure that UVMS achieves the goal of “MAC”, the activation variable A_mac for this task have the following structure:

$\begin{array}{ll}{A}_{mac}=\left[\begin{array}{ccc}{a}_{1,1}& 0& 0\\ 0& a_{2,2}& 0\\ 0& 0& a_{3,3}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& a_{3,3}\end{array}\right]& (21)\end{array}$

where a_1,1 and a_2,2 are equal to zero because the intent is to constrain the system for only the velocity in the z direction of the world frame < w >, given by Equation (21). Thus, for x and y components, the activation should always remain zero.

The desired behavior for reactive control of our inequality objective is:

1. The task should be fully active only when the inequality is false.

2. The transition for activation should be smooth.

Therefore, we use DecreasingBellshaped function in order to calculate a_3,3:

$\begin{array}{ll}{a}_{3,3}\triangleq \{\begin{array}{l}1h_{actual}<h_{{min}_{-}thresh}\\ decreasingbell{h}_{{min}_{-}thresh}\le h_{actual}\\ \le h_{{min}_{-}thresh}+\Delta \\ 0h_{actual}>h_{{min}_{-}thresh}+\Delta \end{array}& (22)\end{array}$

We compute variable ${}^w{v}_z$ using the below equation:

$\begin{array}{ll}{}^w{v}_z=\lambda \left({}^w\bar{h}{-}^w{h}_{actual}\right)& (23)\end{array}$

where ${}^w{h}_{actual}$ is the distance given by the ${}^v{d}_{sensor}$ on the z-axis of the world frame < w >. This quantity represents the vehicle's distance from the seafloor seen from the vehicle itself.

Note: ^yx, where y represents the name of the frame and x represents the vector.

The division of the minimum altitude threshold defines the interval.

• h_actual < h_min−thresh: The reference velocity (Equation 23) will be positive and will drive the robot toward h_min−thresh + Δ with activation a_3,3 = 1.

• h_min−thresh < h_actual < h_min−thresh + Δ: The reference velocity (Equation 23) will be positive and will drive the robot toward h_min−thresh + Δ with activation a_3,3 < 1 and a_3,3 > 0 (transition region).

• h_actual > h_min−thresh + Δ: The reference velocity (Equation 23) will be negative, but the activation a_3,3 = 0 and therefore does not have any effect on the UVMS.

As seen from the above intervals, we choose to implement a minimum altitude threshold $w\stackrel{}{\overline{h}}=h_{{min}_{-}thresh}+\Delta$, which helps us avoid over-constraining the system.

Therefore, different thresholds apply to different types of seafloor. We have simulated different values of the minimum altitude in Table 2. These values all have the same k-gain; we summarize the possible scenarios.

TABLE 2

Table 2. Comparison between behavior for different thresholds for different types of seafloor.

The sensorDistance we used is the distance measured by the sensor on the UVMS along the z-axis of the sensor frame < s >.

$\begin{array}{ll}{}^v{d}_{sensor}=\left[\begin{array}{c}0\\ 0\\ {}^{s}sensorDistance\end{array}\right]& (24)\end{array}$

However, ${}^v{d}_{sensor}$ is the distance vector measured by the sensor and projected on the vehicle frame < v >. Since we need to project it onto the world frame < w >, we apply the following rotation matrix:

$\begin{array}{ll}{}^w{d}_{sensor}{=}^w{R_v}^v{d}_{sensor}& (25)\end{array}$

To obtain the distance between seafloor and robot in the world frame < w >, we use the below equation:

$\begin{array}{ll}{}^w{d}_{sensor}=\left[\begin{array}{c}{}^{w}X-component\\ {}^w-component\\ {}^w{h}_{actural}\\ 0\end{array}\right]{=}^w{T_v}^v{d}_{sensor}& (26)\end{array}$

where with h_actual extracted by the z component of the sensorDistance projected in the world frame < w >. We assume that sensor frame < s > and vehicle frame < v > coincide.

The next highest priority “HA” and its mutually binding “PC” and “LA” tasks

The “HA” is the horizontal attitude task set in this paper and is the next highest priority task. This task ensures that UVMS does not flip for a given reference speed. Suppose we try to swap the priority of “PC” and “HA”. In that case, the vehicle will try to achieve the direction of the target if its horizontal roll or vertical sway is different from zero. Therefore, when we change the priority, the behavior observed during the simulation is almost the same as when the horizontal attitude task is disabled. The horizontal attitude is not enabled, and it will appear that the UVMS is not parallel to the bottom. In addition, swapping priorities is wrong because a horizontal attitude is a safety task and should have a higher priority than the task that defines the action. As can be seen above, the multiple solutions of the higher priority task (“HA”) already constrained task “PC”. We give that the vehicle position control task “PC” and the horizontal attitude task “HA” are mutually constrained as follows:

$\begin{array}{ll}\stackrel{.}{y}={\rho }_1+Q_1{\stackrel{.}{z}}_1\forall {\stackrel{.}{z}}_1& (27)\end{array}$

The vehicle position control task “PC” is set in this paper. We initialize the UVMS at a place far from the seabed, i.e., at sea level, and give the target position far enough, considering the actual situation. This task aims to perform the vehicle position control task to ensure that the vehicle achieves the required position and orientation.

The Jacobian of the task “PC” is:

$\begin{array}{ll}{}^w{\stackrel{.}{x}}_2{=}^w{\stackrel{.}{x}}_{posc}=J_{posc}{}^v\stackrel{.}{y}& (28)\end{array}$

$\begin{array}{l}{\stackrel{.}{x}}_{posc}\in R^6,J_{posc}\in R^{6\times 13},\stackrel{.}{y}\in R^{13}\end{array}$

where the Jacobi matrix has 13 columns corresponding to the dimensions of the control variables at the kinematic level, the control variables are the seven joints of the manipulator and the six D.O.F of the vehicle base. Six rows correspond to the dimensions of the reference velocity. The time volume is the difference between the initial and target positions or λ-value.

$\begin{array}{ll}{}^w{\stackrel{.}{x}}_{posc6\times 1}=\left[\begin{array}{c}{}^w{v}_{3\times 1}\\ {}^w{\omega }_{3\times 1}\end{array}\right]=\left[\begin{array}{ccc}{0}_{3\times 7}& {}^w{R}_{v3\times 3}& 0_{3\times 3}\\ 0_{3\times 7}& 0_{3\times 3}& {}^w{R}_{v}3\times 3\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{q}}_{7\times 1}\\ {}^v{v}_{3\times 1}\\ {}^v{\omega }_{3\times 1}\end{array}\right]& (29)\end{array}$

Since the task “PC” is a reactive control task, the task reference is computed using the formula of the closed-loop feedback reference rate such that:

• Required position:

$\begin{array}{ll}{}^w\overline{\nu }={\lambda }_l\left({}^w{\overline{x}}_{position\text{\_}goal}{-}^w{x}_{actual\text{\_}position}\right)& (30)\end{array}$

• Required orientation:

$\begin{array}{ll}{}^w\bar{\omega }={\lambda }_{a}VersorLemma\left({}^w{\overline{x}}_{orientation\text{\_}goal}{,}^w{x}_{actual\text{\_}orientation}\right)& (31)\end{array}$

and in a compact form:

$\begin{array}{ll}w\bar{x}=\left[\begin{array}{cc}{\lambda }_1& {\lambda }_r\end{array}\right]\left[\begin{array}{cc}w& r\\ {}^w\delta & \theta \end{array}\right]& (32)\end{array}$

with:

$\begin{array}{ll}\left[\begin{array}{c}r\\ \theta \end{array}\right]=CartError({}^w{\overline{x}}_{position\text{\_}goal}{,}^w{x}_{actual\text{\_}position})& (33)\end{array}$

We use “CartError” function to calculate r and θ, and we set the two gains equal to λ_l = 0.2 and λ_a = 0.5.

The altitude control task “LA” is set in this paper, and since “LA” is an action definition (AD) target, it is placed after the security task. It is important to note that the objectives of “LA” and “PC” are so different that it is unlikely that they will be activated simultaneously. Therefore, their relative priority does not affect the solution. There are two main differences between this task and the minimum altitude control task in this paper, the first being that landing is not a safety task but rather an action that defines a safety task. While using the minimum altitude task to avoid collisions with the seafloor, the landing task “LA” defines an action as a vehicle position, therefore, has a lower priority; the second difference is that the minimum altitude is unequal to the landing is an equal task.

The Jacobian for task “LA” is:

$\begin{array}{ll}{}^w{\dot{x}}_{la}{=}^w{v}_{3\times 1}=\left[\begin{array}{ccc}{0}_{3\times 7}& {}^w{R}_v{}_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{c}{\dot{q}}_{7\times 1}\\ v_{3\times 1}\\ {}^v{\omega }_{3\times 1}\end{array}\right]& (34)\end{array}$

As with the minimum altitude control objective, only the Jacobi matrix is needed to control the components along the z-axis. We calculate the task reference as:

$\begin{array}{ll}{}^w{\stackrel{.}{\stackrel{̄}{x}}}_{\text{land}}=k\left[\left(d_{\text{landing}}+{\Delta }_{\text{safeguard}}\right){-}^w{d}_{\text{sensor}}\right]& (35)\end{array}$

where k is the control gain, d_landing is the distance from the seafloor, in this case, Δ_safeguard is set to 0.17 m to avoid interpenetration between UVMS and the seafloor. ${}^w{d}_{\mathrm{\text{sensor}}}$ is the component along the z axis of the distance vector measured by the sensor and projected on the world frame < w >.

Goal control alignment target task “AT”

If we only use the position control task “PC”, we can only guarantee that we reach the target position, but not that we are aligned with the job target to complete the job task. We must add additional constraints to make the vehicle face the target task. The approach we take is to add an alignment task between the vehicle x-axis and the target. In particular, the vehicle x-axis should be aligned with the projection of the unit vector on the inertial level that connects the vehicle frame < v > to the target frame < o >.

So we add the target control alignment task “AT” to the task hierarchy. We decide to place “AT” under “PC” to take advantage of the remaining arbitrariness to align the robot in the direction we want during the target task activation phase. Before landing, the robot tries to align itself with the operational target. When it reaches sufficient alignment (θ < 0.5), the “PC” phase is activated, during which the robot reaches the target position and aligns as much as possible.

The main goal of this task is to align the vehicle, especially the direction to the target. For this purpose, we have to calculate the rotation vector ρ between the two necessary vectors.

The two vectors are:

• a: x-axis of the vehicle frame

$\begin{array}{ll}{}^{v}a=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]& (36)\end{array}$

• b: A vector between the vehicle frame and target projected onto the inertial horizontal plane and expressed in the vehicle frame < v >.

$\begin{array}{ll}{}^{v}b{=}^v{R}_w{\left(I-k{k}^{\top }\right)}^{w}Distance\text{\_}target& (37)\end{array}$

Now, we can proceed to compute ρ vector in the following way:

$\begin{array}{ll}a\wedge b=nsin(\theta )& (38)\end{array}$

$\begin{array}{ll}\rho =n\theta & (39)\end{array}$

where that returns the direction n and the magnitude θ that vector a must perform to be aligned with b. Since our final goal is to have the x-axis of the vehicle aligned with the target, we have to study the behavior of this resulting vector during the time. Thus, we want:

$\begin{array}{ll}{\stackrel{.}{x}}_{ref}=\stackrel{.}{\rho }=\gamma n\theta & (40)\end{array}$

Considering a generic observer, we have:

$\begin{array}{ll}{D}_{\alpha }\rho =\stackrel{.}{\theta }n+\theta D_a(n)=nn{\omega }_{b/\alpha }+N_{\alpha }(\theta )({\omega }_{b/\alpha },{\omega }_{a/\alpha })& (41)\end{array}$

Considering an observer inside the rigid space of the vehicle frame < v > on which there is vector a:

$\begin{array}{ll}{D}_a\rho =nn{\omega }_{b/a}+N_{\alpha }(\theta ){\omega }_{b/a}=nn{\omega }_{b/a}& (42)\end{array}$

where the second term is exactly equal to zero due to the fact that we have ρ and $\stackrel{.}{\rho }$ aligned. Finally, we want:

$\begin{array}{ll}\gamma n\theta ={\omega }_{b/a}& (43)\end{array}$

Since the quantity ω_b/a is not easy to compute we can compute it using the law of addition of angular velocity vectors:

$\begin{array}{ll}{\omega }_{\text{b/a}}={\omega }_{\text{b/w}}\text{-}{\omega }_{\text{a/w}}& (44)\end{array}$

where ω_a/w is the angular velocity of the vehicle with respect to the world. ω_b/w is the angular velocity given by the movement of the vehicle with respect to the target that produces a change of direction of unit vector joining the vehicle frame < v > to the target frame < o >. It is important to compute this quantities using the same observer as for ρ, computed in Equation 38, thus in our case:

It is essential to calculate this quantity using the same observer as for ρ in Equation (38), therefore, in the vehicle frame < v >.

$\begin{array}{ll}{}^v{\omega }_{\text{b/a}}{=}^v{\omega }_{\text{b/w}}{-}^v{\omega }_{\text{a/w}}& (45)\end{array}$

in particular:

$\begin{array}{ll}{}^v{\omega }_{\text{b/a}}=\left(\frac{{}^{v}b}{\left|{|}^{v}b\right||}\wedge \frac{{-}^{v}vp}{\left|{|}^{v}vp\right||}\right)\frac{\left|{|}^{v}vp\right||}{\left|{|}^{v}b\right||}& (46)\end{array}$

where ^vvp is the projection of the linear velocity of the vehicle on the inertial horizontal plane expressed in vehicle frame < v > thus:

$\begin{array}{ll}{}^{v}vp{=}^v{R}_w{\left(I-k{k}^T\right)}^w{R_v}^{v}v& (47)\end{array}$

We can now proceed to compute the desired Jacobian matrix that, according to Equations 5 and 6, must be the following one:

$\begin{array}{ll}\stackrel{.}{x}=\stackrel{.}{\rho }=\left[0_{3\times 7}-\frac{\Vert {}^{v}vp\Vert }{\Vert {}^{v}b\Vert }\left(\frac{{}^{v}b}{\Vert {}^{v}b\Vert }\wedge \frac{{}^v{R}_w{(I-k{k}^T)}^w{R}_v}{\Vert {}^{v}vp\Vert }\right)1\right]\left[\begin{array}{c}\stackrel{.}{q}\\ {}^{v}v\\ {}^{v}w\end{array}\right]& (48)\end{array}$

Moreover, the Jacobian relationship for the alignment target control task “AT” is derived from the following formula.

$\begin{array}{ll}{D}_w(p)=J_{at}\stackrel{.}{y}& (49)\end{array}$

where p is the misalignment vector. D_w(p) is the derivative of the misalignment vector. J_at is the Jacobian we want to compute. We know that

$\begin{array}{ll}{D}_w(p){=}^w{\text{n}}_{\rho }\stackrel{.}{\theta }+{\displaystyle \underset{\text{orthogonal}}{{\displaystyle \underbrace{\theta {D_w}^w{\text{n}}_{\rho }}}}}& (50)\end{array}$

Since we are not interested in the orthogonal components, we can ignore them. By looking at the first term of the sum, we know that ${}^w{\text{n}}_{\rho }$ is the misalignment vector projected on the world and θ can be written as

$\begin{array}{ll}\stackrel{.}{\theta }{=}^w{\omega }_v{-}^w{\omega }_{\text{goal}}& (51)\end{array}$

where ${}^w{\omega }_v$ is the angular velocity of the vehicle, referred to the world. ${}^w{\omega }_{\mathrm{\text{goal}}}$ is the angular velocity of the projected distance between the target and the vehicle, referred to the world.

By describing ${}^w{\omega }_v$ in terms of $\stackrel{.}{y}$, we can deduce the following Jacobian

$\begin{array}{ll}{\text{J}}_{vehicle}=\left[\begin{array}{cc}{0}_{3\times 10}& {}^w{R_v}_{3\times 3}\end{array}\right]& (52)\end{array}$

In order to describe ${}^w{\omega }_{\mathrm{\text{goal}}}$ in terms of $\stackrel{.}{y}$, we use the following relationship

$\begin{array}{ll}{}^w{\upsilon }_v{=}^w{\omega }_{\text{goal}}{\wedge }^{w}d& (53)\end{array}$

where ${}^w{\upsilon }_v$ is the linear velocity of the vehicle projected on the world frame < w >.^wd is the projected distance on the horizontal inertial frame, between the target and the z-axis of the vehicle.

$\begin{array}{ll}{J}_{goal}=\frac{1}{\left|{|}^w{d}^2\right||}\left[{}^{w}d\wedge \right]\begin{array}{c}{}^w{\upsilon }_v\end{array}& (54)\end{array}$

Since we are interested only in the x and y component of ${}^w{\upsilon }_v$, we select such components by premultiplying as given below:

$\begin{array}{ll}{J}_{goal}=\frac{1}{\left|{|}^w{d}^2\right||}{\left[{}^{w}d\wedge \right]}^{}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]\begin{array}{c}{}^w{\upsilon }_v\end{array}& (55)\end{array}$

From the last equation, we deduce the following Jacobian

$\begin{array}{ll}{J}_{goal}=\frac{1}{\left|{|}^w{d}^2\right||}\left[{}^{w}d\wedge \right]\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccc}{0}_{3\times 7}& {}^w{R_v}_{3\times 3}& 0_{3\times 3}\end{array}\right]& (56)\end{array}$

The resulting Jacobian is obtained by substitution and it is equal to

$\begin{array}{ll}{J}_{at}{=}^w{n}_r^{\top }\left[J_{vehicle}-J_{goal}\right]& (57)\end{array}$

We compute the task reference as:

$\begin{array}{ll}{}^w{\stackrel{.}{\stackrel{̄}{x}}}_{\text{at}}=k(0-\left|{|}^{w}p\right||)& (58)\end{array}$

where k is the control gain. ||^wp|| is the norm of the misalignment vector, in this case, we want it to be 0.

Trajectory planning

Trajectory optimization goal

Once the task is divided into subtasks, the placement of the job manipulator is critical because it affects the subsequent manipulation tasks. Poor essential placement may even fail to reach the final target state. A significant problem with this approach is the suboptimality of the generated solution trajectories. Even though optimal solutions can be generated for each subtask, the set of these solutions does not necessarily produce a globally optimal solution. The goal state of the previous subtask will significantly affect the planning of the next task. It may even prevent the generation of feasible solutions, resulting in the need to replan the previous task. As a result, this approach will lead to local optima, global suboptimal paths, or many unsuccessful motion planning queries. Combining the system with high-degree-of-freedom maneuver planning for the entire task can alleviate the suboptimal global problem, but this requires extensive computation. Motion coordination between the vehicle and the manipulator, collision checking and self-collision checking with the environment, and motion constraints are some added complexities in this approach.

In this research, the trajectory optimization objective is the lowest priority to address this issue. After the vehicle has completed all priority tasks, we focus on considering the desired trajectory of the end-effector on the UVMS. We convert the trajectory planning problem into finding feasible joint trajectories considering the priority tasks first while optimizing the cost function given by the expectation. In velocity-resolved inverse kinematics, the task is the expectation of the robot configuration function, represented in the task description by an equation or inequality constraint. Finally, the trajectory planning problem for the end-effector can be formulated as the following optimization problem.

The optimized smooth trajectory needs to consider its boundary conditions, including the start and end states, the relay node as the waypoint through which the robot passes, and the smoothing criterion to evaluate whether the generated trajectory is smooth. Knowing the angles to be reached by M joint, a polynomial fit will result in segment M-1 trajectories, each represented by a polynomial, and the set of trajectories needs to satisfy the following constraints:

• Desired angle constraint:

$\begin{array}{ll}\{\begin{array}{cc}{f}_j^{(k)}(T_{j-1})& =x_{0,j}^{(k)}\\ f_j^{(k)}(T_j)& =x_{T,j}^{(k)}\end{array}& (59)\end{array}$

• Continuity constraint:

The velocity and acceleration of adjacent trajectories are continuous:

$\begin{array}{ll}{f}_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)& (60)\end{array}$

The cost function is chosen to minimize the Snap value for all trajectories. Snap is the fourth-order derivative of position, and minimizing Snap allows the end-effector to meet the autonomous operational movement suitable for UVMS. At the same time, its kinetic states, such as velocity and acceleration, cannot change abruptly. Reducing the range of acceleration and deceleration enables UVMS to work longer in energy-limited underwater environments.

The cost function determined to minimize snap is expressed as follows:

$\begin{array}{l}J(T)={\displaystyle {\int }_{T_{j-1}}^{T_j}}{(f^4(t))}^{2}dt\\ ={\displaystyle \sum _{i\ge 4,l\ge 4}}\frac{i(i-1)(i-2)(i-3)j(l-1)(l-2)(l-3)}{i+l-7}\\ \times (T_j^{i+l-7}-T_{j-1}^{i+l-7})p_i{p}_j& (61)\end{array}$

The coefficients of each order are extracted separately, and the cost function can be written in the quadratic form:

$\begin{array}{ll}{J}_j(T)={\text{p}}_j^T{\text{Q}}_j{\text{p}}_j& (62)\end{array}$

where Q_j is the Hessian matrix that transforms the trajectory optimization problem into a quadratic programming problem, for the final trajectory of the manipulator, each trajectory point should satisfy the following constraints.

Track point constraint, each trajectory should pass through the track point obtained by the path search, and the displacement, speed, acceleration, jerk, and snap at the track point should all exist. To satisfy $f_{\mu }^{(k)}(T_i)=d_{ik}$, where μ ∈ {x, y, z}, k ∈ {0, 1, 2, 3}, i ∈ {1, 2, ⋯ , M} continuity constraints, the displacement, velocity, acceleration, jerk, and snap at the track point should also be continuous. That is satisfied $f_{\mu }^{(k)}(T_i)=d_{ik}$, where μ ∈ {x, y, z}, k ∈ {0, 1, 2, 3}, i ∈ {1, 2, ⋯ , M}. It can be written as an equality constraint.

We need to fix each trajectory time T_j = 0.1s for all joints. Ensure that all robot joints reach the desired angle and end position at the same moment is always stable.

Summarizing the above constraints and cost functions, they are written in matrix form.

• Desired angle constraint:

$\begin{array}{ll}{f}_j^{(k)}(T_j)=x_j^{(k)}& (63)\end{array}$

$\begin{array}{ll}\Rightarrow {\displaystyle \sum _{i\ge k}}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}=x_{T,j}^{(k)}& (64)\end{array}$

$\begin{array}{ll}\Rightarrow \left[...\begin{array}{ccc}\cdots & \frac{i!}{(i-k)!}{T}_j^{i-k}\cdots & ...\end{array}\right]\left[\begin{array}{c}:\\ p_{j,i}\\ :\end{array}\right]=x_{T,j}^{(k)}& (65)\end{array}$

$\begin{array}{ll}\Rightarrow \left[\begin{array}{c}\frac{i!}{(i-k)!}{T}_{j-1}^{i-k}\\ :\\ :\\ \frac{i!}{(i-k)!}{T}_j^{i-k}\end{array}\right]\left[\begin{array}{c}:\\ p_{j,i}\\ :\end{array}\right]=\left[\begin{array}{c}{x}_{0,j}^{(k)}\\ x_{T,j}^{(k)}\end{array}\right]& (66)\end{array}$

$\begin{array}{ll}\Rightarrow {\text{A}}_j{\text{p}}_j={\text{d}}_j& (67)\end{array}$

• Continuity constraint: smoothness constraint ensures continuity between trajectory segments without giving a specific derivative.

$\begin{array}{ll}{f}_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)& (68)\end{array}$

$\begin{array}{ll}\Rightarrow {\displaystyle \sum _{i\ge k}}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}-{\displaystyle \sum _{l\ge k}}\frac{l!}{(l-k)!}{T}_j^{l-k}{p}_{j+1,l}=0& (69)\end{array}$

$\begin{array}{l}\Rightarrow \left[\begin{array}{cccccc}\cdots & \frac{i!}{(i-k)!}{T}_j^{i-k}& ...& ...& -\frac{l!}{(l-k)!}{T}_j^{l-k}& \cdots \end{array}\right]\\ \left[\begin{array}{c}{p}_{j,i}\\ :\\ :\\ p_{j+1,l}\end{array}\right]=0& (70)\end{array}$

$\begin{array}{ll}\Rightarrow \left[{\text{A}}_j-{\text{A}}_{j+1}\right]\left[\begin{array}{c}{\text{p}}_j\\ {\text{p}}_{j+1}\end{array}\right]=0& (71)\end{array}$

Substitution function:

$\begin{array}{ll}\begin{array}{c}J(T)={\int }_{T_{j-1}}^{T_j}{(f^4(t))}^{2}dt\\ ={\left[\begin{array}{c}:\\ p_i\\ :\end{array}\right]}^T\left[\dots \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}{T}^{i+l-7}\dots \right]\left[\begin{array}{c}:\\ p_l\\ :\end{array}\right]\end{array}& (72)\end{array}$

$\begin{array}{ll}{J}_j(T)={\text{p}}_j^T{\text{Q}}_j{\text{p}}_j& (73)\end{array}$

Writing the above problem as an equation constraint in standard form, then the quadratic programming problem can be expressed as:

$\begin{array}{ll}min{\left[\begin{array}{c}{\text{p}}_1\\ :\\ {\text{p}}_M\end{array}\right]}^T\left[\begin{array}{ccc}{\text{Q}}_1& 0& 0\\ 0& :& 0\\ 0& 0& {\text{Q}}_M\end{array}\right]\left[\begin{array}{c}{\text{p}}_1\\ :\\ {\text{p}}_M\end{array}\right]& (74)\end{array}$

$\begin{array}{ll}s.t.{\text{A}}_{\text{eq}}\left[\begin{array}{c}{\text{p}}_1\\ :\\ {\text{p}}_M\end{array}\right]={\text{d}}_{eq}& (75)\end{array}$

The above equation is a linear constraint quadratic programming problem (QP).

Collision avoidance

UVMS requires only six degrees of freedom to reach an arbitrary position in underwater motion. Adding a manipulator gives the entire system more than six degrees of freedom, resulting in the redundancy of degrees of freedom. Kinematic redundancy allows the planner to satisfy additional constraints, such as collision avoidance. Researchers have mainly focused on approximating the robot or the obstacle with strictly convex targets and considering only the closest points in the detection algorithm to avoid collisions to reduce computational effort.

The optimization process has no environmental constraints after the trajectory planning solution based on the minimum snap principle. When a new trajectory is encountered after optimization, the obstacles force the trajectory to be modified again, wasting computational resources and reducing the planning frequency. For example, it is necessary to add constraints on the environment during optimization, generally based on hard constraint solving. The hard constraint solution is to generate a safe region in the environment by extending the algorithm and using it as a hard constraint. Adding hard constraints in the optimization process forms a convex polygon, which transforms the QP problem into a convex optimization problem that can be solved by convex optimization algorithms such as the interior point method.

While the process of underwater obstacle avoidance, most of the surrounding objects are non-strictly convex polyhedra, and these approximation methods are not accurate enough when operating in close range. The problems in the practical application process are ignored. Because the remaining safe regions are treated equally during optimization, there is no good way to handle the extreme cases with underwater sensor noise. The optimized trajectory may go past the edge of the safety zone. Once the controller makes an error, it leads to a severe failure of the manipulator body by colliding with the internal and external environment. Inspired by the penalty function, we propose a more intuitive collision-free motion planning method oriented to UVMS.

An improved collision avoidance method based on soft constraint

By design, we use the principle of soft constraint to improve the collision avoidance method. The essence of the soft constraint method is to apply a “pushing force” to push the trajectory away from the direction of the obstacle. The core problem is the designed objective function. When the objective function is not set correctly, the path may hit an obstacle, which is the shortcoming of soft constraint. Therefore, a gradient-based optimization algorithm sets the objective function to impose a soft constraint on the underwater manipulator to push the underwater manipulator body away from the obstacle.

For Equation 16, the objective function becomes:

$\begin{array}{ll}J=J_s+J_c+J_d={\lambda }_1{J}_1+{\lambda }_2{J}_2+{\lambda }_3{J}_3& (76)\end{array}$

$\begin{array}{ll}{J}_S={\displaystyle \sum _{\mu \in \left\{x,y,z\right\}}}{\displaystyle {\int }_0^T}\left(\frac{d^k{f}_{\mu }(t)}{d{t}^k}\right)dt& (77)\end{array}$

$\begin{array}{l}{\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T{C}^T{M}^{-T}Q{M}^{-T}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\\ ={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\left[\begin{array}{cc}{R}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]& (78)\end{array}$

where the smoothness cost function J_s is the cost of smoothness generated using minimum-snap.

$\begin{array}{l}{J}_c={\displaystyle {\int }_{T_0}^{T_M}}c(p(t))ds\\ ={\displaystyle \sum _{k=0}^{T|\delta t}}c(p(T_k))\Vert \Vert v(t)\Vert \Vert \delta t,T_k=T_0+k\delta t& (79)\end{array}$

where the collision cost function J_c, i.e., the collision cost, penalizes obstacles that are too close.

where the kinetic cost function J_d penalizes exceeding the kinetic constraints. Since the objective function of penalizing the velocity and acceleration is not a convex function, it needs to be solved by step-by-step derivation. The smooth term solution is shown in the previous derivation, and the relationship between the collision term and the free variables d_pμ is as follows.

$\begin{array}{l}{J}_c={\displaystyle \int }\{{\displaystyle \sum _{k=0}^{T|\delta t}}\{{\forall }_{\mu }c(p(T_k))\Vert \Vert v\Vert \Vert F+c(p(T_k))\frac{v_{\mu }}{\Vert \Vert v\Vert \Vert }G\}\delta t{d}_{p\mu }\},\\ \mu \in \left\{x,y,z\right\}& (80)\end{array}$

where the F and G are, respectively:

$\begin{array}{ll}F=T{L}_{dp},G=T{V}_m{L}_{dp}& (81)\end{array}$

where L_dp is the right half of the matrix M⁻¹C, V_m is the mapping matrix of joint position variables to joint velocity variables, $T=\left[T_k^0,T_k^1,\dots ,T_k^n\right]$.

The second-order derivative results in:

$\begin{array}{c}{\text{H}}_o=\left[\frac{{\partial }^2{f}_o}{\partial {\text{d}}_{P_x}^2},\frac{{\partial }^2{f}_o}{\partial {\text{d}}_{P_y^2}},\frac{{\partial }^2{f}_o}{\partial {\text{d}}_{P_z^2}}\right]\\ \frac{{\partial }^2{f}_o}{\partial {\text{d}}_{P\mu }^2}={\displaystyle \sum _{k=0}^{\tau /\delta t}}\{{\text{F}}^T{\nabla }_{\mu }c(p(T_k))\frac{v_{\mu }}{||v||}\text{G}+{\text{F}}^T{\nabla }_{\mu }^{2}c(p(T_k))||v||\text{F}\\ +{\text{G}}^T{\nabla }_{\mu }c(p(T_k))\frac{v_{\mu }}{||v||}\text{F}+{\text{G}}^{T}c(p(T_k))\frac{v_{\mu }^2}{||v|{|}^3}\text{G}\}\delta t& (82)\end{array}$

Simulation results

In this section, the task priority processing strategy and soft-constrained trajectory optimization objective designed in this paper are verified on a kinematically redundant underwater vehicle manipulator system. The system consists of a free-floating underwater vehicle and a seven-function manipulator. The simulation is performed in a MATLAB/Simulink environment.

Case 1: Test the given tasks

The simulation phase first initializes the UVMS for safe waypoint navigation. Afterward, we move the vehicle to a position close to the currently defined end-effector target position, slightly above the target position. Finally, the action change is triggered, and the UVMS executes the ocean float operation. We do not consider any disturbances and assume that the robot can provide the desired speed without delays. In addition, once the robot reaches the desired position, the rest of the tasks are responsible for the end-effector reaching the desired target position and orientation, so the “PC” task will also be closed. For the end-effector to operate as a stationary-based robot, we need to constrain the vehicle not to move. Because, as we noticed, the vehicle will “help” the arm to reach the desired position by moving itself (in line with the expected behavior of the tool task). To avoid this problem, we need to perform a non-reactive task to constrain the vehicle to move or reach the operating position where the task will make the vehicle move. In this case, we test UVMS landing on the seafloor, try the vehicle to its target coordinate system, and then use the end-effector to reach the operational target position. Observe whether the vehicle does not move and perform the ocean float operation of the UVMS.

The uniform hierarchy of tasks we use and their priorities, with the addition of constrained tasks at the priority level, is described in Table 3, with non-reactive tasks (“NR”) added at the top of the hierarchy to constrain the vehicle not to move.

TABLE 3

Table 3. Control tasks and priorities for fixed base manipulation operations.

We have the following tasks in an active/inactive state for each of the different phases, is described in Table 4.

TABLE 4

Table 4. Examples of external activation states for different tasks.

We conducted a multitask prioritization strategy experiment to explain the task prioritization strategy better. First, multitasking is divided into multiple action phases.

1. Action A, safe waypoint navigation with all the safety tasks enabled. This action finishes when the position error is below a fixed threshold (in this case 0.1 m), as Figure 5 shows.

2. Action B, alignment to the nodule with all the safety tasks enabled. This action finishes when the misalignment error is below a fixed threshold (in this case 0.07 m).

3. Action C, landing, and smooth rotation align with the target. This action finishes when the vehicle touches the seafloor (in the simulation, this happens at approximately 0.17 m).

4. Action D, manipulator actuation after landing. In this action, Vehicle Null Velocity task is enabled, preventing vehicle movements. The only movement will be the extension of the manipulator to reach the desired target position.

FIGURE 5

Figure 5. Position-directional linear velocity angular velocity tracking curve of the completed vehicle initialization.

From Figure 6, we observe that the position and orientation errors of the vehicle base remain almost constant after 30 s of simulation when the UVMS has completed the landing phase and started the tool holder phase. Therefore, the tool frame phase's active task “NR” helps us achieve a fixed datum operation. Because after a reasonable time, the position error r and orientation error θ converge to near zero. When the error of the carriage position remains constant, the tool frame error of the manipulator operation is almost zero. Action A's fixed threshold of position error is within 0.1 m. Action B sets the fixed maximum of unaligned error by 1.3 mm, well below the set fixed threshold of 0.07 m, which provides the basis for the subsequent accurate completion of the operational target. Figure 7 shows the corresponding simulation results. Figure 8 shows that the maximum range of vehicle error aligned with the target is 0.17 m in Action C. The Action ends when the vehicle touches the seabed, i.e., when the height is 0.

FIGURE 6

Figure 6. (A) The change curve of vehicle base error during the test and (B) the tool frame error.

FIGURE 7

Figure 7. (A) The vehicle error completed for action A and (B) the unaligned error vehicle error completed for action B.

FIGURE 8

Figure 8. (A) The vehicle error for the completion of action C. (B) The smooth landing state of the vehicle after action C.

Since the Vehicle Null only puts vehicle velocities to zero, it cannot compensate for the disturbance. In a similar scenario, the currents will influence the UVMS: the vehicle can arrive at the desired position and land if the disturbance is not too big. However, when the vehicle Null task is triggered in Action D, the UVMS will drift, eventually losing its target position. The manipulator will keep trying to reach the goal by stretching as much as possible. With a check on the position error during the mission phase update, it is possible to return to Action A and achieve the desired position again. Then the task phase update transition requires a new command relationship to achieve it. Therefore, the transition from one activity to another when completing a given job task using the “task” “phase” variable in the Matlab structure. The “phase” variable is updated when the previous Action completes the desired precision. The task update phase starts when the UVMS approaches the desired M path point navigation target position. The “UpdateMissionPhase” of each loop performed the phase update condition check.

As shown in Figure 9, by taking into account the buffer time, achieving a seamless transition (from one activity to another) is by using a bell curve (increasing or decreasing) activation. The transition triggers Actions A and B by realizing the vehicle's target position. We calculate the Cartesian error between the target frame and the vehicle frame, and the task phase changes when the error is below a given threshold (0.1 m in this case). When we want to disable the running task, use a decreasing bell function to perform a smooth transition. In this case, the minimum height and vehicle position tasks are disabled at the beginning of the second phase.

FIGURE 9

Figure 9. (A) The task activation status used for action transitions and (B) the behavior of the landing task.

Similarly, when we want to activate the task, a smooth transition is performed using an increasing bell function, as we did for the landing task. We calculate the slope of such a function based on the task phase time, which helps us determine the time interval between vehicles switching from one activity to another (0.2 s in our case). At the same time, we obtain a smoother shape and avoid discontinuities in the motor drive. Due to the active hold state of the landing task “LA” and the horizontal attitude task “HA”, the vehicle's altitude and velocity in the Z-axis direction remain constant during this process and continue to be 0. Figure 9B indicates that the vehicle does not float with the external during the task transition. The task prioritization strategy has a strong constraint, proving its stability.

Case 2: Add an optimal control target

After the task transition is complete, begin completing a joint limits avoidance task. Attempt to reach the specified operating position using the end-effector. Moreover, we observe that the vehicle does not move and that all joints are within their soft restraint limits. This task is a safety task, so it has a higher priority than other tasks that define movements. It can control the operation of the joints without exceeding their fixed thresholds. The action is the same as before; the only difference is that the joint restraint is always active, as this is a safety task. It is essential to ensure that the end-effector performs the final operation. This case adds the optimization goal of keeping the four joints of the manipulator's end-effector to complete the trajectory optimized for the target behavior. For the rest of the tasks, we kept the same hierarchy as in the previous case and added only the optimization task “MP”. This task has the lowest priority because we can only perform trajectory optimization of the end-effector after the UVMS completes all actions.

We mainly activate the state of the four joints near the end of the end-effector, as shown in Figure 10A, and limit the motion of the remaining joints. The designation of the joint limit task is to test whether we can effectively control the activation state of each joint and motion-tracked it in real-time during the vehicle manipulation task. Figure 11 shows that while the end-effector optimization task is active, the horizontal attitude task “HA” is kept highly active to maintain the stability of the vehicle position. Figure 12 shows the optimal end-effector trajectory based on the trajectory optimization objective. The trajectory optimization is performed based on the satisfaction of the proposed constraints, and the desired optimal trajectory of the end-effector coincides with the trajectory tracking as much as possible. We ensure the accuracy and idealization of the task execution. In addition, Figure 12A shows the joint motion and Figure 12B is smoother compared to Figure 10B. The optimal solution here satisfies the primary collision-free motion task, meaning that the solution found here satisfies the primary collision-free task but is optimal compared to the suboptimal pose task. Therefore, the pure QP process cannot handle more than one task simultaneously, so we have placed the optimization task at the lowest priority.

FIGURE 10

Figure 10. (A) Joint limitation task activation of action D. (B) Robot arm joint position and velocity variation curve.

FIGURE 11

Figure 11. (A) Optimized task activation of the end-effector. (B) Task activation of the vehicle position.

FIGURE 12

Figure 12. (A) Two-joint angular tracking of end-effector. (B) Two-joint angular tracking of end-effector.

Conclusions

This paper studied the problem of multiple-task planning from the motion planning level for the underwater vehicle manipulator system. The task prioritization strategy to perform various tasks at once is considered in the mission planning to derive an optimal and feasible planning scheme; secondly, the optimization algorithm is adopted during the execution of the tasks, considering the system's limitations and the interference of the environment. We proposed soft constraints as an improved collision avoidance method to add more conditions to smooth the joint trajectory. The combination of the above two aspects can achieve the continuous planning of the phased execution of the task, ensure the stability of the end-effector work, and improve the reliability of UVMS autonomous underwater operations. We perform a series of simulations in a simulation environment established by kinematic and dynamic analysis of the underwater vehicle manipulator system. The simulation results verify the effectiveness and feasibility of this paper's task prioritization processing strategy. In this sense, we believe that the approach of using a simulation environment instead of a natural underwater application environment proves to be cost-saving in planning and effective in improvement. And in this sense, we believe that our work has achieved some progress in extending the scope of applying task prioritization planning methods based on the motion planning level in control studies of UVMS. In addition, our proposed method provides an optional way of thinking for controlling underwater robots and other types of robots. In the future, we plan to extend the research for different kinds of robots for real-time planning.

Indeed, the current research has its limitations, along with some results. In the algorithm proposed in this research, we run the simulations carried out under ideal conditions, so it needs to complete realistic experiments to verify the correctness and feasibility of the proposed method. In addition, more influencing factors should be considered, such as the reliability of the sensor, actuator, and controller execution methods. Moreover, adding and improving the controller's performance and stability to accomplish the smooth execution of the task is an essential topic for further research. Ultimately, UVMS-related research has broad application background and important theoretical and engineering significance. Our proposed method will be applied to UVMS for autonomous motion planning in unknown sea environments to enhance its subsea operation capability and meet the application requirements of keeping up with the times.

Data availability statement

The original contributions presented in the study are included in the article/supplementary material, further inquiries can be directed to the corresponding author.

Author contributions

Conceptualization: Y-eG and XZ. Methodology, writing—original draft preparation, writing—review and editing, software, formal analysis, and data curation: Y-eG. Validation: WB, JW, and QY. Supervision, project administration, and funding acquisition: WB, SY, and YS. All authors have read and agreed to the published version of the manuscript.

Funding

This research was funded by the Basic Research in Natural Science and Enterprise Joint Fund of Shaanxi (No. 2021JLM-58).

Conflict of interest

All authors were employed by company Hanjiang-Weihe River Valley Water Diversion Project Construction Co., LTD.

Publisher's note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

References

Antonelli, G., and Chiaverini, S. (1998). Task-priority redundancy resolution for underwater vehicle-manipulator systems. IEEE International Conference on Robotics and Automation. p. 768–773.

Google Scholar

Antonelli, G., and Chiaverini, S. (2000). Fuzzy approach to redundancy resolution for underwater vehicle-manipulator systems. Control Eng. Pract. 11, 445–452. doi: 10.1016/S0967-0661(02)00319-2

CrossRef Full Text | Google Scholar

Bae, J., Bak, J., Jin, S., Seo, T., and Kim, J. (2018). Optimal configuration and parametric design of an underwater vehicle manipulator system for a valve task. Me chanism and Machine Theory 123, 76–88. doi: 10.1016/j.mechmachtheory.2018.01.014

CrossRef Full Text | Google Scholar

Casalino, G., Zereik, E., Simetti, E., Torelli, S., Sperinde, A., Turetta, A. (2012). A task and subsystem priority based control strategy for underwater floating manipulators. IFAC Proceedings Volumes 45, 170–177. doi: 10.3182/20120410-3-PT-4028.00029

CrossRef Full Text | Google Scholar

Chang, W. (2004). Research on Control and Coordination of Multiple AUVs Based on Neural Network. Harbin: Harbin Engineering University.

Changmi, X. (2022). Dynamics Modeling and Motion Control of Underwater Robot-Manipulator System. Ocean University of China. doi: 10.7666/d.y1827957

CrossRef Full Text

Cieslak, P., Ridao, P., and Giergiel, M. (2015). Autonomous underwater panel operation by GIRONA500 UVMS: A practical approach to autonomous underwater manipulation. IEEE International Conference on Robotics and Automation (ICRA). doi: 10.1109/ICRA.2015.7139230

CrossRef Full Text | Google Scholar

Conti, R., Meli, A., Ridolfi, A., and Allotta, B. (2015). An innovative decentralized strategy for I-AUVs cooperative manipulation tasks. Rob. Auton. Syst..

Google Scholar

Fujie, Y., Yuan, C., and Qingzhong, L. (2020). Coordinated motion planning based on virtual workspace constraints for AUVMS. J. Mechanical Eng. 56, 249–64. doi: 10.3901/JME.2020.12.249

CrossRef Full Text

Gancet, J., Weiss, P., Antonelli, G., Pfingsthorn, M. F., Calinon, S., Turetta, A., et al. (2016). Dexterous undersea interventions with far distance onshore supervision: the DexROV project. IFAC Papersonline 49, 414–419. doi: 10.1016/j.ifacol.2016.10.439

CrossRef Full Text | Google Scholar

Han, J., and Chung, W. K. (2008). “Coordinated motion control of Underwater Vehicle-Manipulator System with minimizing restoring moments,” IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE. p. 3158–3163.

Google Scholar

Han, J., Park, J., Chung, W. K. (2011). Robust coordinated motion control of an underwater vehicle-manipulator system with minimizing restoring moments. Ocean Eng. 38, 1197–1206. doi: 10.1016/j.oceaneng.2011.05.014

CrossRef Full Text | Google Scholar

Heshmati-Alamdari, S., Bechlioulis, C. P., Karras, G. C., et al. (2018). A robust interaction control approach for underwater vehicle manipulator systems. Annu. Rev. Control. 46, 315–325. doi: 10.1016/j.arcontrol.2018.10.003

CrossRef Full Text | Google Scholar

Huang, H., Zhang, G. C., and Yang, Y. I. (2016). Dynamics modeling and coordinated motion trajectory optimization of underwater unmanned vehicle manipulator system. J. Shanghai Jiaotong Univ. 50, 1437–1443.+1451. doi: 10.16183/j.cnki.jsjtu.2016.09.015

CrossRef Full Text

Jin, Y. Y. (1996). Numerical methods for generalized least squares problems - ScienceDirect. J. Comput. Appl. Math. 66, 571–584. doi: 10.1016/0377-0427(95)00167-0

CrossRef Full Text | Google Scholar

Kanoun, O., Lamiraux, F., and Wieber, P. B. (2011). Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks. IEEE Transactions on Robotics 27, 785–792. doi: 10.1109/TRO.2011.2142450

CrossRef Full Text | Google Scholar

Lynch, B., and Ellery, A. (2014). Efficient control of an AUV-manipulator system: an application for the exploration of Europa. IEEE J. Oceanic Eng. 39, 552–570. doi: 10.1109/JOE.2013.2271390

CrossRef Full Text | Google Scholar

Mansard, N., and Chaumette, F. (2007). Task sequencing for high-level sensor-based control. IEEE Trans. Robot. 23, 60–72. doi: 10.1109/TRO.2006.889487

CrossRef Full Text | Google Scholar

Mansard, N., Khatib, O., and Kheddar, A. (2009). Integrating Unilateral Constraints Inside the Stack of Tasks. doi: 10.1109/TRO.2009.2020345

CrossRef Full Text | Google Scholar

Moe, S., Antonelli, G., and Pettersen, K. Y. (2014). Null-space-based behavior guidance of planar dual-arm UVMS. IEEE International Conference on Robotics and Biomimetics (ROBIO). doi: 10.1109/ROBIO.2014.7090419

CrossRef Full Text | Google Scholar

Nakamura, Y., Hanafusa, H., Yoshikawa, T. (1987). Task-priority based redundancy control of robot manipulators. Int. J. Robot. Res. 6. doi: 10.1177/027836498700600201

CrossRef Full Text | Google Scholar

Olguin-Diaz, E., Arechavaleta, G., Jarquin, G., et al. (2013). A passivity-based model-free force–motion control of underwater vehicle-manipulator systems. IEEE Transact. Robot. 29, 1469–1484. doi: 10.1109/TRO.2013.2277535

CrossRef Full Text | Google Scholar

Podder, T., and Sarkar, N. (2000). “Dynamic trajectory planning for autonomous underwater vehicle-manipulator systems”, international conference on robotics and automation 2000. p. 3461–3466. doi: 10.1109/ROBOT.2000.845264

CrossRef Full Text | Google Scholar

Prats, M., Perez, J., Fernandez, J. J., Sanz, P. (2012). “An open source tool for simulation and supervision of underwater intervention missions”, IEEE/RSJ International Conference on Intelligent Robots and Systems. p. 2577–2582. doi: 10.1109/IROS.2012.6385788

CrossRef Full Text | Google Scholar

Sanz, P. J., Prats, M., Ridao, P., Ribas, D., Oliver, G., Ortiz, A. (2011). Recent progress in the RAUVI project: A Reconfigurable autonomous underwater vehicle for intervention. Elmar. Zadar, Croatia: IEEE.

Google Scholar

Sanz, P. J., Ridao, P., Oliver, G., Melchiorri, C., Casalino, G., Silvestre, C., et al. (2010). TRIDENT: A framework for autonomous underwater intervention missions with dexterous manipulation capabilities. IFAC Proceedings Volumes 43, 187–192. doi: 10.3182/20100906-3-IT-2019.00034

CrossRef Full Text | Google Scholar

Sentis, L., and Khatib, O. (2005). “Control of free-floating humanoid robots through task prioritization,” in Proceedings of the 2005 IEEE International Conference on Robotics and Automation (IEEE), 1718–1723. doi: 10.1109/ROBOT.2005.1570361

CrossRef Full Text | Google Scholar

Siciliano, B., and Slotine, J. A. (1991). “General framework for managing multiple tasks in highly redundant robotic systems,” in International Conference on Advanced Robotics. IEEE. doi: 10.1109/ICAR.1991.240390

CrossRef Full Text | Google Scholar

Simetti, E., and Casalino, G. (2016). Manipulation and transportation with cooperative underwater vehicle manipulator systems. IEEE J. Oceanic Eng. 99, 1–18. doi: 10.1109/JOE.2016.2618182

CrossRef Full Text | Google Scholar

Simetti, E., Casalino, G., Wanderlingh, F., Aicardi, M. (2018). Task priority control of underwater intervention systems: theory and applications. Ocean Eng. 164, 40–54. doi: 10.1016/j.oceaneng.2018.06.026

CrossRef Full Text | Google Scholar

Sotiropoulos, P., Kolonias, V., Aspragathos, N., and Housus, E. (2015). Rapid motion planning algorithm for optimal UVMS interventions in semi-structured environments using GPUs. Rob. Auton. Syst. 74, 15–29. doi: 10.1016/j.robot.2015.06.005

CrossRef Full Text | Google Scholar

Tang, Q., Liang, L., Xie, J., Li, Y., Deng, Z. (2017). Task-priority redundancy resolution on acceleration level for underwater vehicle-manipulator system. Int. J. Adv. Robotic Systems 14, 172988141771982. doi: 10.1177/1729881417719825

CrossRef Full Text | Google Scholar

Wang, Y., Jiang, S., Yan, F., Gu, L., Chen, B. (2017). A new redundancy resolution for underwater vehicle–manipulator system considering payload. Int. J. Adv. Robotic Systems 14, 172988141773393. doi: 10.1177/1729881417733934

CrossRef Full Text | Google Scholar

Whitney, D. E. (1969). Resolved motion rate control of manipulators and human prostheses. IEEE Transact. Man Machine Syst. 10, 47–53. doi: 10.1109/TMMS.1969.299896

CrossRef Full Text | Google Scholar

Xu, B., Pandian, S. R., Sakagami, N., et al. (2012). Neuro-fuzzy control of underwater vehicle-manipulator systems. J. Franklin Institute 349, 1125–1138. doi: 10.1016/j.jfranklin.2012.01.003

CrossRef Full Text | Google Scholar

Xuefeng, D., and Xinqian, B. (2000). On coordinated control of multi-arm underwater vehicle. J. Harbin Eng. Univ. 21, 13–16. doi: 10.1007/s11804-006-6024-y

CrossRef Full Text

Youakim, D., Ridao Rodríguez, P., Rovira, N. P., Spadafora, F., Ribas, D., Muzzup, M., et al. (2017). MoveIt!: autonomous underwater free-floating manipulation. IEEE Robot. Autom. Mag. 24, 41–51. doi: 10.1109/MRA.2016.2636369

CrossRef Full Text | Google Scholar