Disturbance compensation of a dual-arm underwater robot via redundant parallel mechanism theory

Abstract

Disturbance compensation is one of the major issues for underwater robots to hover as a mobile platform and to manipulate an object in an underwater environment. This paper presents a new strategy of disturbance compensation for a mobile dual-arm underwater robot using internal torques derived from redundant parallel mechanism theory. A model of the robot was analyzed by redundant serial and parallel mechanisms at the same time. The joint torque to operate the robot is obtained from a redundant serial mechanism model with null-space projection due to redundancy. The joint torque derived from the redundant parallel kinematic model is calculated to perfectly compensate for disturbances to the mobile platform and is included in the solution of the joint torque based on the serial redundant model. The resultant joint torque can generate force on the end-effector for required tasks and forces for disturbance compensation simultaneously . A simulation shows the performance of this disturbance compensation strategy. The joint torque based on the algorithm generates the desired task force and the disturbance compensation force together, and a little additional joint torque can generate a large internal force effectively due to the characteristics of a redundant parallel mechanism. The proposed method is more effective than compensation methods using thrusting force on the mobile platform.

1 Introduction

Missions for underwater robots are expanding from inspection or monitoring to particular tasks with attached manipulators. Techniques for modeling and control of an underwater vehicle-manipulator system (UVMS) have been used by many researchers, and new mechanisms are being presented. Podder [1] presented a new unified motion planning algorithm for a UVMS. Cobos-Guzman [2] developed an underwater robot manipulator for a tele-robotic system, and Fernandez [3] developed an underwater robot arm for swallow-water intervention. Zheng [4] dynamically modeled an octopus-like manipulator for an underwater robot. Yatoh [5] designed a digital disturbance compensation controller for an underwater robot with a two-link manipulator.

Disturbance from ocean currents or manipulator reaction is a very sensitive factor for precise control of a UVMS, since the robot is floating in water during operation. The motion of human divers can give a hint for the design and control of a UVMS. Human divers grasp structures around a task point with one hand while the other arm stably performs the underwater task accurately. This configuration is achieved by a dual-arm manipulator. Such a manipulator is an intuitive solution for the reproduction of human motion.

This paper presents a UVMS with two arms for performing precise underwater tasks under disturbance. A clamping arm grasps a fixed structure and an operating arm performs a task like human divers. In this configuration, the dual-arm manipulator is considered as a kinematically redundant structure. The mobile platform to which the dual arms are attached is included as one linkage of the structure. When the operating arm maintains contact on a task point, the manipulator can be considered as a redundant parallel mechanism because of the closed kinematic chain.

A redundant manipulator can control an end-effector with task priority, which means that it can accomplish a main task with other subtasks such as optimization of the joint torque or obstacle avoidance through the redundant degrees of freedom. Using the null-space solution is a useful approach in task-priority control. Khatib [6] presented an operational space formulation for redundant manipulators. Hollerbach [7] optimized joint torque during the required motion of an end-effector through the null-space approach, and Chiaverini [8] presented a task-priority redundancy resolution that overcomes the kinematic and algorithmic singularity for real-time implementation of kinematic controls. Park [9] proposed a kinematic decoupling formulation using an augmented matrix with null-space velocities. Dietrich [10] and Sadeghian [11] designed a multi-objective control method for redundant manipulators with hierarchy.

A redundant parallel mechanism generates various effects via over-actuation. Internal force and torque are induced from additional joint torque and change the characteristics of the structure without changing the configuration. Lee [12] and Jin [13] controlled the stiffness of a structure by optimization of redundant torque. O’Brien [14] showed that redundant actuation has advantages for manipulability. Kim [15] avoided the singularity of a parallel mechanism via redundant actuation. Huda [16] designed a parallel mechanism to perform precise motion within a large workspace. Gosselin [17] presented a force distribution algorithm for cable-driven parallel mechanisms. Wu [18] designed a redundantly parallel conveyor and analyzed dynamic load capacity.

Most mobile dual-arm robots have been analyzed with the origin of the frame on the center of the robot body. In this paper, the origin is placed at the clamping point, and the kinematic serial and parallel models are built up from the original, respectively. The null-space approach based on the redundancy of this mechanism superposes the kinematic model of a parallel mechanism onto the model of a serial mechanism. The joint torque required for the main task force can be obtained from the kinematic model of a serial mechanism. The secondary joint torque for disturbance compensation is derived from the kinematic model of a parallel mechanism and then substituted into the null-space solution. A simulation was conducted and verified that the joint torque from the redundant model generates the desired task force at the end-effector with disturbance compensation at the same time.

The rest of the paper is composed of four parts. The redundant serial and parallel mechanism model of a dual-arm robot is presented in Sect. 2. Disturbance compensation via the null-space approach is presented in Sect. 3. In Sect. 4, the simulation results show the performance of disturbance compensation. Finally, the paper is concluded in Sect. 5.

2 Kinematic modeling of a mobile dual-arm underwater robot

Robots need motion and force control to perform a task successfully. Motion control allows a robot to follow a required trajectory of the end-effector and maintain the optimal posture. Force control generates the desired end-effector force. In an underwater environment, ocean currents apply a disturbance force to the robot and influence the robot control. This paper focuses on the force control through static analysis. Figure 1 shows a situation in which the robot performs a task under disturbance. Each of the arms has three revolute joints. The left arm grasps a fixed point and acts like an anchor. The right arm grasps the task object and performs a required task. Ocean current causes drag on the mobile platform as the disturbance. The drag on the arms is negligible because the arms are much slimmer than the mobile platform. This paper assumes that the forces $f_{b,x}$ and $f_{b,y}$ and the moment $m_b$ generated by the disturbance are concentrated where the manipulators are attached to the robot body. All six joints have actuators and can generate torque $tau_i$. The desired task forces and moment, $f_{e,x}$, $f_{e,y}$, and $m_e$, have to be generated at the task point.

Fig. 1

Conceptual diagram of underawter dual-arm robot under disturbance

The geometric configuration of the robot is simplified on a 2-D plane in Fig. 2. To obtain the joint torque for the end-effector forces and moment, the dual arm manipulator can be considered as a redundant serial mechanism, as shown in Fig. 2a. The clamping point O is the origin of this space, and E is the point of the end-effector. $q_i$ is the joint angle, and $r_i$ is the length of a linkage. The linkage of length $r_b$ is a virtual link representing the attachment line between the manipulator and the mobile robot. Both of the hands are described by linkages of length $r_o$ and $r_e$. The hands tightly hold objects with friction forces. We assume that the linkages for the hands keep perpendicular to the surface of clamping. When the point of the end-effector contacts the task point, the manipulator is constrained and can be considered as a redundant parallel mechanism, as shown in Fig. 2b. From the parallel mechanisms point of view, this system is a redundant mechanism which has 3-DOF with six actuated joints. If three joint angles are given, the rest of joint angles has to be dependently determined. The joint which has the required angle is called the independent joint, and the joint which is constrained dependently is called the dependent joint. The independent joint may be chosen among the actuated joints. Since this system is fully actuated, even if any joints are chosen as the independent joint, the kinematical characteristic is same [19]. In this paper, the task arm is shown as a red line and has three independent joint angles due to the task-oriented deployment. The clamping arm is shown as a blue line and has three dependent joint angles. The forces and moment induced from a disturbance can be resolved into concentrated forces and a moment at the center point B where the manipulator is attached.

Fig. 2

2-D geometrical configuration of a mobile dual-arm robot with serial kinematic model (a) and parallel kinematic model (b). (Color figure online)

The induced joint torque has to generate the task forces at point E and the disturbance compensation forces at point B simultaneously. To achieve this, a redundant serial model and parallel model have to be considered together.

2.1 Redundant serial model for end-effector force

In the serial kinematic mechanism shown in Fig. 2a, the end-effector’s velocity for the joint velocity can be expressed using the Jacobian matrix as follows:

$$\dot{x}_e=J_e(q)\dot{q},$$

(1)

where $x_e \in R^3$ is the position and orientation vector of the end-effector, $q \in R^6$ is the joint angle vector, and $J_e$ is the forward Jacobian matrix between $x_e$ and q. Since the Jacobian matrix $J_e$ is not a square matrix because of redundancy, the solution of the joint velocity for the end-effector’s velocity is not unique and is expressed as follows:

$$\dot{q}=J_e^{\dag }(q)\dot{x_e}+[I-J_e^{\dag }(q)J_e^{\top }(q)]\dot{q_0},$$

(2)

The generalized inverse of a matrix is indicated by $(\cdot )^{\dag }$, while I is the identity matrix, and $\dot{q_0}$ is an arbitrary joint velocity. The joint torque also has infinite solutions with null-space solutions. The generalized joint force vector that generates the required end-effector forces can be obtained statically via virtual work as follows [20]:

$$\tau =J_e^{\top }(q)f_e+[I-J_e^{\top }(q)J_e^{\dag }(q)]\tau _0,$$

(3)

where $\tau \in R^6$ is the joint torque vector, $f_e \in R^3$ is the end-effector’s operational force vector, and $\tau _0$ is an arbitrary joint force vector. These relations mean that internal motion and internal torque can exist in the redundant mechanisms. The internal motion, which is in the null space related to the Jacobian matrix, may exist while keeping the end-effector’s position fixed, and the internal torque does not affect the task force at the end-effector.

2.2 Redundant parallel model for disturbance force

Assuming that there is enough reaction force at the contact point E with the end-effector, the dual-arm manipulator can be considered as a redundant parallel mechanism at each moment during the motion, as shown in Fig. 2b. Appropriate joint torque can generate the forces and moment at the point B to compensate for the disturbance. There are geometrical constraints since the parallel mechanism has a kinematic close loop. The constraint relation can be expressed as:

$$\begin{aligned} g_1&= r_3\cos (q_4)+r_4\cos (q_4+q_5)+r_b\cos (q_4+q_5+q_6)+r_2\cos (q_4+q_5+q_6-q_3)\nonumber \\&+\,r_1\cos (q_4+q_5+q_6-q_3-q_2)+r_3\cos (q_4+q_5+q_6-q_3-q_2-q_1)-x_e=0, \nonumber \\ g_2 &= r_0+r_3\cos (q_4)+r_4\cos (q_4+q_5)+r_b\cos (q_4+q_5+q_6)+r_2\cos (q_4+q_5+q_6-q_3)\nonumber \\&+\,r_1\cos (q_4+q_5+q_6-q_3-q_2)+r_3\cos (q_4+q_5+q_6-q_3-q_2-q_1)-y_e=0, \nonumber \\ g_3 &= q_4+q_5+q_6-q_3-q_2-q_1-\theta _e+\pi =0. \end{aligned}$$

(4)

where, $g_i$ are the constraint equations. These are used to derive the constraint Jacobian matrix later. When the independent joint angles are given, the dependent joint angles can be calculated by solving the constraint equations. The relation between the velocity of the point B and independent joint velocity can be expressed using the Jacobian matrix as follows:

$$\dot{x}_b=J_b(q_{in})\dot{q}_{in},$$

(5)

where $x_b \in R^3$ is the position and orientation vector of the point B, $q_{in}=[q_1~q_2~q_3]^{\top } \in R^3$ is an independent joint angle vector, and $J_b$ is the forward Jacobian matrix between $x_b$ and $q_{in}$. Considering the constraints of a parallel mechanism and the actuator redundancy, the constraint Jacobian is derived as:

$${\varGamma }={\mathrm{VU}}\left[ \begin{array}{c} I_{3\times 3} \\ -G_v^{-1}G_u \end{array}\right] ,$$

(6)

where U is the selection matrix of independent joints, and V is the extraction matrix of driving joints. In this case, both of these matrices are a $6\times 6$ identity matrix, since independent and dependent joints are arranged in ascending order and all joints are fully actuated. $G_u$ and $G_v$ are the constraint Jacobians for independent and dependent joints, respectively. The constraint Jacobian $G_u$ is consist of the partial derivatives of the constraint equations for the independent joints, and the $G_v$ is consist of the partial derivatives of the constraint equations for the dependent joints.

$$G_u=\left[ \begin{array}{ccc} \frac{\partial g_1}{\partial q_1} &{}\quad \frac{\partial g_1}{\partial q_2} &{}\quad \frac{\partial g_1}{\partial q_3} \\ \frac{\partial g_2}{\partial q_1} &{}\quad \frac{\partial g_2}{\partial q_2} &{}\quad \frac{\partial g_2}{\partial q_3} \\ \frac{\partial g_3}{\partial q_1} &{}\quad \frac{\partial g_3}{\partial q_2} &{}\quad \frac{\partial g_3}{\partial q_3} \end{array}\right] ,\quad G_v=\left[ \begin{array}{ccc} \frac{\partial g_1}{\partial q_4} &{}\quad \frac{\partial g_1}{\partial q_5} &{}\quad \frac{\partial g_1}{\partial q_6} \\ \frac{\partial g_2}{\partial q_4} &{}\quad \frac{\partial g_2}{\partial q_5} &{}\quad \frac{\partial g_2}{\partial q_6} \\ \frac{\partial g_3}{\partial q_4} &{}\quad \frac{\partial g_3}{\partial q_5} &{}\quad \frac{\partial g_3}{\partial q_6} \end{array}\right] ,$$

(7)

The relation between the forces at the point B and joint torque is obtained via virtual work as follows:

$${\varGamma }^{\top }\tau _b=J_b^{\top }(q_{in})(-f_b),$$

(8)

where $f_b$ is the vector of the disturbance forces and moment calculated at point B. Then, the joint torque $\tau _b$ for generating compensation forces can be derived as:

$$\tau _b=({\varGamma }^{\top })^{\dag }J_b^{\top }(q_{in})(-f_b).$$

(9)

3 Disturbance compensation via null space approach

After substituting Eq. (9) into the arbitrary joint force vector of Eq. (3), the joint torque generates the required end-effector forces and disturbance compensation forces simultaneously as follows:

$$\tau =J_e^{\top }(q)f_e+[I-J_e^{\top }(q)J_e^{\dag }(q)][({\varGamma }^{\top })^{\dag }J_b^{\top }(q_{in})(-f_b)].$$

(10)

The additional joint torque is a null space solution and does not affect the task force at the end-effector, since $[I-J_e^{\top }(q)J_e^{\dag }(q)]$ is the null space operator subject to the Jacobian matrix. In order to perform the task successfully, the required force has to be generated exactly by controller. However, it is hard to calculate the desired force in the controller considering the disturbance effects. This decoupled compensation method suggests more efficient way to control. The desired force in the force controller can follow the task force, and the additional joint torque can compensate the disturbance force simultaneously. The null-space solutions belong to the set of infinite joint torque solutions with redundancy. The joint torque with null-space solutions still satisfied with the equation of relation between joint torque and end-effector’s force. The arbitrary joint torque in null-space solution can be fully decoupled and does not affect the force of end-effector. A simulation showing the effect of this joint torque is presented in the next section.

Considering the motion of a manipulator and for thorough modeling of the relation between joint forces and end-effector forces, the dynamics of the manipulator is needed. Wu [21, 22] presented the control of parallel mechanisms with dynamics. There are several control methods which can controls the position and force in parallel such as hybrid control [23]. For example, if this mechanism can maintain a certain configuration via positon-based force control, the internal torque can be applied to this system with actuator redundancy. The control scheme which is planed ahead is show as Fig. 3. This paper focus on design of disturbance compensator based on the kinematics as the first step. When the position controller works well with dynamics, joint torque for generating end-effector force and internal torque can only be derived from kinematic analysis. An additional joint torque may affect the dynamics. The generalized inverse in the null space operator of Eqs. (3) and (9) has to be the inertia-weighted pseudo-inverse because of dynamic consistency [24].

Fig. 3

Control diagram with disturbance compensator

4 Simulation result and discussions

4.1 Model description and task definition

In this simulation, the robot dimension is set for actual robot system which has been developed. The lengths of task arm linkage $r_e$, $r_1$, and $r_2$ are 100, 300, and 200 mm, respectively. The clamping arm lengths $r_o$, $r_3$, and $r_4$ are 100, 300, and 200 mm, respectively. The attached linkage length $r_b$ is 250 mm. The shape of mobile vehicle which causes the disturbance force is considered as rectangular parallelepiped with 300 mm (width) $\times$ 800 mm (long) $\times$ 300 mm (height).

The task for the simulation is designed to show the performance of the derived joint torque model. The end-effector has to follow a required path with contact and generate required forces simultaneously. The required path is an arc with a radius of 100 mm and center angle of $\pi /2$. The normal force and tangential force along the path have to be kept with magnitudes of 0 and 50 N, respectively. During the task, the joint configuration is determined by ideal motion control, as shown in Fig. 3. Joint motion is deduced from an optimal cost function that minimizes the variation of all joint angles. This task is similar to underwater welding or wall cleaning.

During the task, it supposes that the joint configuration is controlled well by position control, as shown in Fig. 4. This Joint motion is deduced from an optimal cost function that minimizes the variation of all joint angles. All joints move with minimum displacement from the initial position, while the end-effector moves on task curve. The joint angles can be calculated by solving the constraint equations in Eq. (4). The joint configuration is determined at first according to the trajectory, and the joint torque is derived from Eq. (10) every joint configuration because the analysis is based on the kinematics.

Fig. 4

Trajectory of the linkage configuration during the task motion: red line means the task arm with the independent joints, and blue line means the clamping arm with the dependent joints. Dotted line is a mobile vehicel, and green line means the attached linkage between dual-arm maniluator and mobile vehicle. (Color figure online)

4.2 Disturbance model

The drag force of the mobile platform from ocean current is considered as a disturbance. An average ocean current velocity can be obtained by a first-order Gauss–Markov process [25]. Ocean current velocity is generated as shown in Fig. 5a and in the lateral direction, as shown in Fig. 5b. The drag force is changed by the current velocity, side angle, and posture of the mobile platform. The disturbance force and moment at point B can be calculated from the distributed drag force during task motion, as shown in Figs. 4c, d.

Fig. 5

Disturbance model: (a) ocean current velocity, (b) side angle of ocean current, (c) disturbacne force induced by ocean current at point B, and (d) disturbacne moment induced by ocean current at point B

4.3 Results and discussion

The simulation compares a non-compensation case and a compensation case. When substituting a zero vector into the arbitrary torque vector in Eq. (3), the joint torque has no disturbance compensation and generates only the desired task force. The joint torque without disturbance compensation applied to the system is plotted in Fig. 6a. The net force on point B in Fig. 6c is the same as the disturbance force in Fig. 5c. The joint torque generates the desired task force and the net force on the end-effector is affected by the disturbance force, as shown in Fig. 6d. Two lines with the same color in Fig. 6d show the task forces in the x and y directions.

The joint torque with disturbance compensation applied to the system is plotted in Fig. 6b. The net force on point B becomes zero and the disturbance force is cancelled out, as shown in Fig. 6c. The net force on the end-effector follows the desired task force exactly. This result shows that the joint torque for generating the task force and the additional joint torque for disturbance compensation are well decoupled.

A remarkable aspect in this simulation is the small difference between the joint torque with compensation and that without compensation. Little additional joint torque can generate large internal force due to the characteristics of a redundant parallel mechanism. The disturbance compensation strategy using internal joint torque is more efficient that the way to compensate disturbance force directly. While the maximum disturbance force is 201.07 N, the maximum additional joint torque is only 19.43 Nm, as shown in Fig. 6. If the mobile platform directly compensates for the disturbance, the thrust force has to be the same as the disturbance force. Disturbance compensation using the internal joint torque of the dual-arm manipulator is more efficient than direct compensation of the mobile platform.

Fig. 6

Simulation results: a joint torque without disturbance compensation, b joint torque with disturbance compensation, c forces at point B, and d task forces at point E

Since the length of robot arm can affect the joint torque, the analysis under various cases is needed to prove the efficiency of the disturbance compensator. The repeated simulation with various linkage length was performed under the same mobile platform and the same ocean current. The length of linkage is changed 0.5, 0.75, 1,25, 1.5, 1.75, and 2 times by the preceding simulation. Simulation results are arranged as shown in Fig. 7. Size factor means the magnification of linkage length. Black points are the average joint torque for end-effector’s force, and red points are the average additional joint torque for disturbance compensation. Notice that, as the size factor increases, the amount of joint torque for end-effector’s force increases more rapidly than the additional joint torque. It is approved that the disturbance compensation using the internal joint torque is still efficient although the robot dimension is changed.

Fig. 7

Comparison of size effect between joint torque for end-effector’s force and additional joint torque for disturbance compensation

5 Conclusion

This paper presented a new strategy for disturbance compensation of a mobile dual-arm robot using the internal torque of a redundant parallel mechanism. An underwater robot with a dual-arm manipulator that grasps two points tightly can be considered as a redundant serial mechanism and parallel mechanism simultaneously. The joint torque for generating the task force on the end-effector was derived from a serial kinematic model, and a null-space solution of the joint torque based on redundancy was used for disturbance compensation. The compensation joint torque projected to the null-space of the Jacobian matrix was obtained from a parallel kinematic model. Simulation results showed that the joint torque for generating the task force and the additional joint torque for disturbance compensation are well performed with decoupling. In addition, the disturbance compensation using internal joint torque showed efficiency by generating a large compensation force with little additional joint torque. In future work, a dynamic model will be derived while considering manipulator motion, and the trajectory of the joint configuration can be optimized using redundancy. Hybrid motion and force control will be applied to a UVMS with dual arms to perform a precise underwater task.