Adaptive Tracking Control of Underwater Vehicle ... - IEEE Xplore

594

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 20, NO. 3, JUNE 2004

[14] A. Loria, “Global tracking control of one-degree-of-freedom Euler–Lagrange systems without velocity measurement,” Eur. J. Contr., pp. 144–151, 1996. [15] G. Besancon, “Global output feedback tracking control for a class of Lagrangian systems,” Automatica, vol. 36, pp. 1915–1921, 2000. [16] Z. P. Jiang and I. Kanellakopoulos, “Global output-feedback tracking for a benchmark nonlinear system,” IEEE Trans. Automat. Contr., vol. 45, pp. 1023–1027, May 2000. [17] Z. P. Jiang, “Lyapunov design of global state and output feedback trackers for nonholonomic control systems,” Int. J. Contr., vol. 73, no. 9, pp. 744–761, 2000. [18] H. Nijmeijer and T. I. Fossen, Eds., New Directions in Nonlinear Observer Design. London, U.K.: Springer-Verlag, 1999. [19] R. Sepulchre, M. Jankovic, and P. Kokotovic, Constructive Nonlinear Control. New York: Springer-Verlag, 1997.

Adaptive Tracking Control of Underwater Vehicle-Manipulator Systems Based on the Virtual Decomposition Approach Gianluca Antonelli, Fabrizio Caccavale, and Stefano Chiaverini

Abstract—A novel adaptive control law for the end-effector tracking problem of underwater vehicle-manipulator systems (UVMSs) is presented in this paper. By exploiting the serial-chain kinematic structure of the UVMS, the overall control problem is decomposed in a set of elementary control problems, each of them formulated with respect to a single rigid body in the system. The proposed approach results in a modular control scheme which simplifies application to UVMSs with a large number of links, reduces the required computational burden, and allows efficient implementation on distributed computing architectures. Furthermore, the occurrence of kinematic and representation singularities is overcome, respectively, by expressing the control law in body-fixed coordinates and representing the attitude via the unit quaternion. To show the effectiveness of the proposed control strategy, a simulation case study is developed for a vehicle in spatial motion carrying a six-degree-of-freedom manipulator. Index Terms—Adaptive control, control of redundant manipulators, underwater robotics, virtual decomposition control.

I. INTRODUCTION The use of autonomous underwater vehicles (AUVs) equipped with an underwater vehicle-manipulator system (UVMS) to perform complex underwater tasks gives rise to challenging control problems involving nonlinear, coupled, and high-dimensional systems. Currently, the state of the art is represented by teleoperated master/slave architectures; few research centers are equipped with autonomous systems [26]. Several control strategies based on perfect compensation of the UVMS’s dynamics have been proposed [6], [18]. However, it must Manuscript received January 14, 2003; revised April 23, 2003. This paper was recommended for publication by Associate Editor J. Leonard and Editor I. Walker upon evaluation of the reviewers’ comments. G. Antonelli and S. Chiaverini are with the Dipartimento di Automazione, Elettromagnetismo, Ingegneria dell’Informazione e Matematica Industriale, Università degli Studi di Cassino, 03043 Cassino, Italy (e-mail: [email protected]; [email protected]). F. Caccavale is with the Dipartimento di Ingegneria e Fisica dell’Ambiente, Universià degli Studi della Basilicata, 85100 Potenza, Italy (e-mail: [email protected]). Digital Object Identifier 10.1109/TRA.2004.825521

be pointed out that exact knowledge of the system dynamics rarely can be assumed, especially for underwater applications, where some dynamic terms strongly depend on the environmental conditions (e.g., hydrodynamic forces). To overcome this problem, adaptive control laws have been proposed, e.g., [13] and [14]. These approaches regard the UVMS model as a whole, thus giving rise to high-dimensional problems. In fact, differently from the case of earth-fixed manipulators, in the case of UVMS, it is not possible to reduce the number of dynamic parameters to be adapted, since the base of the manipulator (i.e., the vehicle) has full mobility. In detail, it can be stated that the computational load of such control algorithms grows as much as the fourth-order power of the number of the system’s degrees of freedom (DOFs). For this reason, practical application of adaptive control to UVMS has been usually limited, even in simulation, to vehicles carrying arms with very few joints (i.e., two or three) and usually performing planar tasks. An interesting approach is proposed in [10], where an adaptive control law, based on the micro–macro manipulator concept, is designed for UVMSs carrying a nonredundant manipulator (e.g., with 6 DOFs). The approach needs the computation of the inverse kinematics (IK) of the system; this is achieved by using the inverse of the Jacobian matrix. Hence, the approach can be applied only to nonredundant manipulators (i.e., characterized by a square Jacobian matrix) in nonsingular configurations (i.e., full-rank Jacobian matrix). As for nonadaptive approaches, in [9] the dynamic coupling between vehicle and manipulator is investigated. From the analysis of a specific UVMS structure, a sliding-mode approach, based on the approach in [24], with a feedforward compensation term is proposed. Numerical simulation results show that the knowledge of the dynamics allows improvement of the tracking performance. This approach, however, requires knowledge of the symbolic expression for the interaction between the first link and the vehicle. Reference [15] reports a control algorithm in which the importance of the compensation of the vehicle/manipulator interaction is highlighted by the use of a force/torque sensor on the manipulator base or, in alternative, by the use of a disturbance observer. An adaptive, nonregressor-based control law for UVMSs is also presented in [27]. In sum, adaptive model-based control approaches for UVMSs are often based on the Lagrange formulation of the dynamic model of the whole system. This leads to computationally intensive control algorithms. On the other hand, nonadaptive approaches typically require accurate knowledge of the dynamic model of the system or have to be designed for specific UVMSs. This drawback strongly limits the application of model-based control for underwater applications. Hence, in practice, control engineers adopt simple control laws (e.g., of the proportional-integral-derivative (PID) type), which usually provide significantly limited tracking performance of the control system in the face of a light computational effort. This paper is aimed at developing a new adaptive control scheme for the tracking problem of UVMS, which keeps the advantages of modelbased adaptive control, in terms of tracking accuracy, while limiting the computational load. Also, the control algorithm is designed so as to have a modular structure. This brings several advantages in terms of control software design and maintenance. The control scheme is based on the virtual decomposition approach in [28]. Different from previous approaches, the serial-chain structure of the UVMS is exploited to decompose the overall motion-control problem in a set of elementary control problems regarding the motion of each rigid body in the system, namely, the manipulator’s links and the vehicle. For each body, a control action is designed to assign the desired motion, to adaptively compensate for the body dynamics, and

1042-296X/04$20.00 © 2004 IEEE


to counteract force/moment exchanged with its neighborhood bodies along the chain. The resulting control scheme has a modular structure which greatly simplifies its application to systems with a large number of links; it reduces the required computational burden by replacing one high-dimensional problem with many low-dimensional ones, and allows efficient implementation on distributed computing architectures [28]; it can be embedded in a kinematic control scheme, which allows handling of kinematic redundancy, i.e., to achieve joint-limits avoidance and dexterity optimization; finally, it reduces the size of the control software code and improves its flexibility, i.e., its structure is not modified by changing the system’s mechanical structure. Remarkably, the control law is expressed in terms of body-fixed coordinates, so as to overcome the occurrence of kinematic singularities [4], while the use of a nonminimal representation of the orientation (i.e., the unit quaternion [16]) allows overcoming the occurrence of representation singularities. With respect to the features developed in [28], in this paper, the case of an underwater application is investigated. This implies considering the specific dynamics of underwater rigid bodies in the theoretical and simulation sections. Moreover, by exploiting the results achieved in [3], a nine-parameter regressor is used in the adaptive action. The proposed control scheme is tested in a numerical simulation case study. To the purpose, an effective and reliable simulation tool is adopted, so as to achieve simulation results as realistic as possible. In the case study, a UVMS composed of a 6-DOF manipulator mounted on a 6-DOF vehicle is considered. The simulation case study has been chosen so as to test the proposed control approach under severe operating conditions; this has been achieved by considering a rather complex 6-DOF task, assigned in terms of desired position and orientation trajectories for the manipulator end-effector, and aimed at emphasizing the nonlinear and coupled nature of the UVMS dynamics.

595

The defined velocities are related via the following relation:

RIB O 323 O 32n _ 1 _ 1 (1) = O 323 T ( 2 ) O 32n _ 2 = J k ( 2 ) _ 2 O n23 O n23 I n2n q_ q_ where I n2n is the (n 2 n) identity matrix; O n 2n is the n1 2 n2 null matrix; and the matrix T can be expressed in terms of Euler angles as [19]

T ( 2 ) =

The vehicle is completely described by the position and orientation of a vehicle-fixed frame with respect to an earth-fixed inertial reference frame. According to the nomenclature in [20], the position of the vehicle-fixed frame is described by the vector 1 = [x y z ]T 2 IR3 . The orientation of the vehicle-fixed frame is described by the (3 2 3) rotation matrix RB I (rotation matrix expressing the transformation from the inertial frame to the vehicle-fixed frame). As is customary in marine applications, the vehicle orientation is also represented in terms of the vector 2 = [ ]T 2 IR3 of Euler angles corresponding to RIB . T T Let us define = [ 1T 2T ] , and _ = [_ 1T _ 2T ] the corresponding time derivatives (expressed in the inertial frame). In the following, an alternative description of the orientation will be used; namely, the unit quaternion [16] that will be conveniently exploited to represent the vehicle orientation. This is a nonminimal (based on four-parameters terms) description of the orientation, which allows overcoming some typical drawbacks of the minimal (i.e., three-parameters) representations, such as Euler angles. Some basic concepts regarding the unit quaternion are summarized in the Appendix. The vehicle attitude in terms of quaternions will be denoted with Q2 = f; "g. Let us define 1 2 IR3 as the linear velocity of the vehicle-fixed frame with respect to the inertial frame expressed in the vehicle-fixed frame, and 2 2 IR3 as the angular velocity of the vehicle-fixed frame with respect to the inertial frame expressed in the vehicle-fixed frame. = [ T1 T2 ]T . Let q = [q1 1 1 1 qn ]T 2 IRn be the vector of joint positions, where n is the number of joints. The vector q_ 2 IRn is the corresponding time T T _ T] . derivative. Let us also define = [ T 1 2 q

0 0

0 0s c c s 0 s c c

(2)

where c and s are short-hand notations for cos() and sin(), respectively. Let us also consider the end-effector position of the manipulator carried by the vehicle in the inertial frame, e1 2 IR3 . This is a function of the system configuration, i.e., e1 (1 ; RIB ; q ). Let us further define e2 2 IR3 as the orientation of the end-effector in the inertial frame expressed in terms of Euler angles. Of course, also e2 is a function of the system configuration, i.e., e2 (RIB ; q ). The vectors _ e1 2 IR3 and _ e2 2 IR3 are the corresponding time derivatives; they are related to the end-effector velocities e (expressed in the frame attached to the end-effector) via relations analogous to (1), i.e.,

e1 = RIn_ e1 e2 = T ( e2 )_ e2

(3) (4)

where RIn is the rotation matrix from the inertial to the end-effector frame (i.e., frame attached to link n) and T is the matrix defined in (2), where the end-effector Euler angles have to be considered. The end-effector velocities (expressed in the earth-fixed frame) are related to the body-fixed system velocity by a suitable Jacobian matrix, i.e.,

_ e1 RnI e2

II. MODELING A. Kinematics

1

=

J w RBI ; q :

(5)

Notice that the Jacobian J w has been derived with respect to the angular velocity of the end-effector expressed in the earth-fixed frame. This is in order to keep coherence of the mapping of the force/moments acting at the end-effector to the force/moment/torques at the vehicle/joint space [see also (9)]. Moreover, this Jacobian can be easily used to implement closed-loop inverse kinematics (CLIK) using quaternions [8]. B. Dynamics The dynamic model of an UVMS can be written in the following compact form [17], [18]:

M (q )_ + C (q ; ) + D (q ; ) + g RIB ; q = (6) where M (q) 2 IR(6+n)2(6+n) is the inertia matrix including the added mass effects, C (q ; ) 2 IR6+n is the vector of Coriolis and centripetal terms including the added mass effects, D (q ; ) 2 IR6+n is the vector

of friction and hydrodynamic damping terms (e.g., drag, lift, and vortex shedding generalized forces), g (RIB ; q ) 2 IR6+n is the vector of gravT T 6+n itational and buoyant generalized forces, and = [ T v q ] 2 IR 6 is the vector composed by v 2 IR (i.e., the forces and moments acting on the vehicle) and q 2 IRn (i.e., the manipulator’s joint torques). Thrusters and control surfaces provide forces and moments on the vehicle according to a nonlinear relation. In [23], the quadratic dependence between the thrust and the propeller velocity is shown. A simplified relationship can be expressed through the linear mapping [11]

= Bu

(7)

596


Fig. 1. Block diagram of the kinematic and dynamic loops.

where B 2 IR(6+n)2p , and u 2 IRp is the vector of control inputs. In the remainder, we will assume p (6 + n) and B being full rank. It can be proven that [18]: • the inertia matrix of the total system is positive definite and symmetric, i.e., M (q ) = M T (q ) > O ; • the damping matrix is positive definite, i.e., D (q ; ) > O ; _ (q ) 0 2C (q ; ) is skew symmetric, i.e., T (M_ (q ) 0 • the matrix M )) = 0 8 2 IR6+n . 2C (q ; Exploiting the property of linearity in the parameters [19], (6) can be rewritten as

where Rii+1 2 IR323 is the rotation matrix from frame Ti+1 to frame Ti , S (1) is the matrix operator performing the cross product between i two (3 2 1) vectors, and r i;i +1 is the vector pointing from the origin of Ti to the origin of Ti+1 . The (6 2 1) vector of the total generalized force (i.e., force and moment) acting on the ith body is given by

Y RBI ;qq; ; _ =

The equations of motion of each rigid body can be written in bodyfixed reference frame in the form [11], [25]

(8)

i = h i 0 U i h i ; i = 0; . . . ; n 0 1 ht;i i i i n ht;n = hnn (11) where hii is the generalized force exerted by body i 0 1 on body i. +1

+1 +1

where 2 IR is the vector of dynamic model parameters. For an underwater vehicle carrying an n-link manipulator, the number of parameters of the whole system is = (n + 1) 1h, where h is the number of parameters for a single rigid body moving in a fluid. Notice that h usually is greater than 100 [12]. Equation (8) holds if a suitable model, i.e., linear in the parameter, for the hydrodynamic forces is considered. Defining he 2 IR6 as the contact force/moment at the end-effector, the following relation holds:

i M i_ ii + C i ii ii + D i ii ii + g ii (Ri ) = ht;i (12) where R i is the rotation matrix expressing the orientation of Ti with respect to the inertial reference frame, M i 2 IR 2 , C i ( ii ) ii 2 IR , D i ( ii ) ii 2 IR , and g ii (Ri ) 2 IR are the analogous quantities intro-

= J w he :

i Y Ri ; ii ; _ ii 1 i = ht;i

T

(9)

In the following, the dynamics of a UVMS is rewritten in a way to remark the interaction between consecutive rigid bodies of the serial chain. The vehicle and the manipulator’s links are assumed to be rigid bodies numbered from 0 (the vehicle) to n (the last link, i.e., the one carrying the end-effector). Hence, the whole system can be regarded as an open kinematic chain with floating base. A reference frame Ti is attached to the ith body, according to the Denavit–Hartenberg formalism [19], while 6i is the earth-fixed inertial reference frame. Hereafter, a superscript will denote the frame to which a vector is referred; the superscript will be dropped for quantities referred to the inertial frame. In the following, some quantities referring to the vehicle will be denoted by new symbols. In fact, coherently with the virtual decomposition approach, the vehicle is considered as link number 0. Denoting by ii 2 IR6 the vector of linear and angular velocity, the following relates the velocity propagation along the structure:

ii

+1 +1

where z ii = [0

[0 0 1 0 0 0]

U ii ii + q_i z ii 0 0 0 0 1] 2 IR

=

+1

+1

+1 6

i = 0; . . . ; n 0 1

(10)

for a rotational joint and z ii =

for a prismatic joint. The matrix U ii+1 2 IR626 is defined as i U ii+1 = S r i Ri+1 Ri i;i+1 i+1

O2 Rii 3

3

+1

6

6

6

6

6

duced in (6) referred to a single generic rigid body. According to the property of linearity in the parameters, (12) can be rewritten as (13)

where i is the vector of dynamic parameters of the ith rigid body. The input torque q;i at the ith joint of the manipulator can be obtained by projecting hi on the corresponding joint axis via

q;i = z ii0 hii T

1

i = 1; . . . ; n:

(14)

III. PROPOSED CONTROL LAW In this section, the proposed control law is devised, and the stability analysis of the resulting closed-loop system is developed. The overall control scheme is depicted in Fig. 1, where a two-stage control architecture is adopted. Namely, the motion control block represents a suitable control law (e.g., based on the virtual decomposition approach, detailed in the following) designed so as to track desired trajectories for the vehicle ( 1;d (t), 2;d (t), or Q2;d (t)) as well as for the manipulator (q d (t)). On the other hand, the manipulation task is usually assigned in the so-called task space, i.e., in terms of end-effector trajectories e1;d (t), e2;d (t), or Qe2;d (t). In other words, the desired behavior is naturally assigned in the task space, while the control commands have to be delivered to the actuators placed on the vehicle and at manipulator joints. Therefore, an IK is performed out of the motion control loop, aimed at transforming the assigned task-space variables into the corresponding desired vehicle/joint motion. The resulting two-stage control scheme is usually termed kinematic control.


In the next two subsections, the overall control scheme is detailed in terms of its building blocks.

597

vehicle linear and angular velocity is denoted with the symbol 00 in the following. Let us define

= 0d;0 + p;O0I 323 O 3I23 e0 o;0 323 323 q_r;i = q_d;i + i q~i ; i = 1; . . . ; n i i i+1 i+1 r;i+1 = U i+1 r;i + q_r;i+1z i ; i = 0; . . . ; n 0 1 0 r;0

A. Inverse Kinematics A system is kinematically redundant when it possesses more DOFs than those required to execute a given task. Since a general manipulation task is specified in terms of given position and orientation trajectories for the end-effector, strictly speaking, a UVMS is always kinematically redundant, due to the DOFs provided by the vehicle itself. Kinematic control allows exploiting such a redundancy and achieving secondary tasks, e.g., system reconfiguration, increasing manipulability, etc. Moreover, techniques to handle the occurrence of kinematic singularities are available [7]. Most motion control laws are designed directly with respect to the task-space variables by resorting to suitable mathematical modeling of the vehicle/manipulator system [10], [21]. Nevertheless, as discussed above, the control commands to the actuators are always to be given at the vehicle/joint level; hence, closure of the control loop always needs the introduction of an IK transformation in the loop, even if the control law is designed in the task space. However, this approach suffers from two main drawbacks; namely, kinematic redundancy cannot be easily exploited, and the possible occurrence of algorithmic singularities is not avoided [7]. In [17], an approach based on a second-order IK algorithm is proposed for UVMSs, aimed at reducing the drag effects; however, this approach requires compensation of the system dynamics. In this paper, an inverse kinematic algorithm based on the approach in [7] is adopted, which is capable of effectively handling all the DOFs of the system and, at the same time, is free from the occurrence of algorithmic singularities; moreover, compensation of the system dynamics is not needed to solve the IK. Let m be the number of DOFs of the mission task. The system DOFs are 6 + n. Let x p be the primary task vector and x s the secondary task vector. Usually, xp is composed by the end-effector position vector ( e1;d ) and a vector representing the end-effector orientation ( e2;d or Qe2;d ). On the other hand, xs depends on the assigned secondary task, e.g., to keep the vehicle orientation constant or still as long as the manipulator is working in a dexterous posture. Some examples of possible secondary tasks and their integration with fuzzy techniques are given in [4] and [5]. The task priority IK algorithm is based on the following CLIK update law [7]: d

= J #p (v p;d + 3pep ) +

# I 0 Jp Jp

# J s (x_ s;d + 3 s es )

(15)

where J p and J s are the configuration-dependent primary and secondary task Jacobians, respectively (the symbol # denotes any kind of matrix pseudoinversion, e.g., the Moore–Penrose pseudoinverse); v p;d (_ x s;d ) is the velocity vector relative to the assigned primary (secondary) task x p;d (xs;d ); 3p and 3s are positive definite matrix gains. The reconstruction errors e p (es ) of the primary (secondary) task are fed back so as to avoid the typical drift experienced by algorithms operated in an open-loop fashion; they are computed via forward kinematics of the system (see Fig. 1). Of course, multiplication by the inverse of matrix J k and a successive numerical integration must be performed to obtain the desired vehicle/manipulator motion variables 1;d (t), 2;d (t), or Q2;d (t), q d from d (see (1) and Fig. 1). B. Motion Control Law Let 1;d (t), Q2;d , q d (t), d (t) be the output of the IK algorithm described in the previous section (see Fig. 1). From those quantities, it is possible to compute 0d;0 (t), q_ d (t), _ 0d;0 (t), qd (t). Notice that the

(16)

where e0

=

T R0 1 "00

~

~

(17)

is the (6 2 1) vector denoting position and orientation errors for the vehicle (see the Appendix for the orientation error definition), and p;0 , o;0 , and i are positive design gains. It is useful considering the following variables:

= ir;i 0 ii ; i = 0; . . . ; n sq;i = q_r;i 0 q_i ; i = 1; . . . ; n sq = [sq;1 ; . . . ; sq;n ]T : sii

(18)

The proposed control law is based on the computation of the required generalized force for each rigid body in the system. Then, the input torques for the manipulator and the input generalized force for the vehicle are computed from the required forces according to (11) and (14). i of the In the following, it is assumed that only a nominal estimate ^ vector of dynamic parameters is available for the ith rigid body. Hence, a suitable update law for the estimates has to be adopted so as to ensure asymptotic tracking of the desired trajectory. For the ith rigid body (including the vehicle), the required force has the same structure as the total force propagation (11) i hr;i

i i+1 0 U ii+1hc;i = hc;i +1

(19)

that, including the designed required force, implies

i+1 ^ + K v;isii + U ii+1hc;i +1 (20) with K v;i > O . Then parameters estimate î is dynamically updated i hc;i

=Y

_

i i i R i ; i ; r;i ; r;i i

via

^_ = K 0;i1Y T

i

_

i i i i R i ; i ; r;i ; r;i si

(21)

with K ;i > O . The control torque at the ith manipulator’s joint is obtained applying (14), as q;i

i = z ii01 Thc;i :

(22)

Finally, by considering the force propagation, the generalized force for the vehicle needed to achieve the corresponding required force is computed as

00 hc;

= hr;0 0 + U 01 hc;1 1 :

(23)

IV. STABILITY ANALYSIS Let us consider the scalar function

~ = 1 sii TM isii + 1 ~iTK ;i~i 2 2

i Vi s i ; i

(24)

which is positive definite in view of the positive definiteness of M i and ~ = i 0 î is the dynamic parameter estimation error. Differentiating Vi with respect to time yields

K ;i . The vector i

_ = sii TM i _ ir;i 0 _ ii 0 ~iTK ;i^_ i

Vi

598


where the parameters have been considered constant, i.e., ~i = 0î . Taking into account the equations of motions (12), and considering ni = C i ( ii ) ir;i + D i ( ii ) ir;i + g ii (R i ) _

_

V_ i

=

0sii TD i

M i_ ir;i + ni ii ; ir;i ; R i

i 0 ht;i 0 ~iTK ^_i :

i i , where hc;i Adding and subtracting the term sii hc;i is the control law as introduced in (20), and exploiting the linearity in the parameters, the previous equation can be rewritten as

V_ i

=

T T sii D i sii + sii

0 i+1 i 2 Y i i 0 ht;i 0 Y i î 0 K v;i sii 0 U ii+1 hc;i +1 T T _ i i 0 ~i K ;iî + si hc;i

where Y i = Y (R i ; ii ; ir;i ; _ ir;i ) and D i = D i ( ii ). Rearranging the terms, it is

V_ i = 0sii

0~iTK ;i^_i

T i i i iT Y i~i 0 ht;i (K v;i + D i )si + si + h r;i

that, taking the update law for the estimate of the dynamic parameters (21), finally gives

V_ i

=

i 0sii T (K v;i + D i )sii + sii Th~t;i

(25)

i i ~ where h t;i = hr;i 0 ht;i . The sum of all Vi ’s is the Lyapunov candidate function for the UVMS i

n

V s00 ; . . . ; snn ; ~0 ; . . . ; ~n

=

i=0

Vi sii ; ~i

which is positive definite in view of the positive definitiveness of Vi , i = 0; . . . ; n . Its time derivative is simply given by the time derivatives of all the scalar functions

V_

n

=

i=0

0sii T (K v;i + D i )sii +

n i=0

T

i

~ sii h t;i

where the last term is null. In fact, by considering (10) and (11), it can ~ i and si be observed that the same relationships hold for h i t;i

+1 sii+1

i i+1 i = U i+1 s i + sq;i+1z i ~i ~i = h ~ i+1 h U ii+1 h t;i i i+1

0

~ = h i 0 h i . Recalling that = J T h , the last term of the where h w e c;i i i time derivative of the Lyapunov function candidate is then given by i

n i=0

T

T

T i+1

~ + s i+1 h ~ sii h t;i t;i+1 i+1

ii sii

iT +si

~ = 0. In order to since, in absence of contact at the end-effector, h e clarify the first equality, let us rewrite the first term for two consecutive links

i

~ sii h t;i

0T ~0 + = s0 h 0 0T ~0 + = s0 h 0

n i=1 n

i=1 T 0 T s00 ~ J Th e

i

~ sq;iz ii01h i

=0

sq

=

T

01 + sq;iz i U ii01 s ii0 i01 1

+ =

T

T

T

i+1

~ hi 0 sii U ii+1 h i+1

U ii+1 s ii + sq;i+1z ii+1

01 + sq;iz i U ii01 s ii0 i01 1 +

T ~i

T ~ i+1 i+1 T i+1 ~ i+2 hi+1 0 si+1 U i+2hi+2

T ~i hi

T ~ i+1 i+1 T i+1 ~ i+2 sq;i+1z ii+1 h i+1 0 s i+1 U i+2 h i+2

that, considering that the first and the last term are null for the first and the last rigid body, respectively, gives the relation required in (26). We can now prove the system stability in a Lyapunov-like sense using Barbalat’s Lemma. Since n ~ • V (s00 ; . . . ; sn ; 0 ; . . . ; ~n ) is lower bounded; • V_ 0; • V_ is uniformly continuous; • then V_ ! 0 as t ! 1. n Thus, s00 ; . . . ; sn ! 0 as t ! 1. Due to the recursive definition of i the vectors si , the position errors converge to the null value as well. In fact, the vector s00 ! 0 implies 00 ! 0 and e 0 ! 0 (p;0 > 0 and o;0 > 0). Moreover, since the rotation matrix has full rank, the vehicle position error ~1 , too, decreases to the null value. Convergence to zero of sii directly implies convergence to zero of sq;1 ; . . . ; sq;n ; consequently, q~_ ! 0 and q~ ! 0. As usual in adaptive control technique, we cannot prove asymptotic stability of the whole state, since ~i is only guaranteed to be bounded. Remarks: • This paper focuses on a specific implementation of virtual decomposition control. In [28], the same approach is demonstrated for open chains (for which rigid bodies are only a special case) using the elegant concept of virtual power flow. In a few words, it can be considered a way of handling the dynamic interactions between the subsystems of an open chain. Moreover, in [28], the interaction with the environment is also efficiently considered in the framework of virtual power flow. • Achieving null error of n + 1 rigid bodies with 6 DOFs with only 6+n inputs is physically coherent, since the control law is computed taking into account the kinematic constraints of the system. • In [28], the control law performs an implicit kinematic inversion. The approach developed in [28] has been derived for a 6-DOF robot, and a stability analysis of position/orientation control of the endeffector is also provided. In the case of a redundant robot, the authors suggest the implementation of an augmented Jacobian approach in order to have a square Jacobian to work with. However, the possible occurrence of algorithmic singularities is not avoided. On the other hand, if the kinematic control is kept separate from the dynamic loop, as in the approach proposed in this paper, it is possible to use inverse kinematic techniques robust to the occurrence of algorithmic singularities. V. SIMULATIONS

sq;i ~q;i

0 ~ + sT ~q = s0 h 0 q =

i

w

(26)

Numerical simulations have been performed to show the effectiveness of the proposed control law based on the simulation tool described in [1]. It is worth pointing out that the adopted simulation environment has been designed so as to provide a testbed for underwater robotics as realistic as possible. Namely, all the relevant dynamic terms have been modeled, including hydrodynamic effects and joint friction [18];


599

TABLE I MASSES kg, VEHICLE LENGTH m, AND DENAVIT–HARTENBERG PARAMETERS m,rad OF THE MANIPULATOR MOUNTED ON THE UNDERWATER VEHICLE

Fig. 2.

Lateral view of a sketch of the simulated system.

also, digital implementation of the control law has been considered, and noise and quantization on the measurements are added. The vehicle data are taken from [12], and are referred to the experimental NPS AUV II. For simulation purposes, a 6-DOF manipulator with large inertia has been considered, which is mounted under the vehicle’s body. The manipulator structure and the dynamic parameters are those of the Smart-3S, manufactured by COMAU. Its weight is about 40% of the vehicle weight, its length is about 2 m stretched, while the vehicle is 5 m long. The overall structure has 12 DOFs. The relevant physical parameters of the system are reported in Table I, while a sketch is reported in Fig. 2. Notice one of the features of the proposed controller. Changing the manipulator does not imply redesigning the control, but it simply modifies the Denavit–Hartenberg table and the initial estimate of the dynamic parameters. In fact, the code would be exactly the same, just reading from different data files, with obvious advantages for software debugging and maintenance. Notice that for all the simulations, the following conditions have been considered: • full DOF dynamic simulations: 6-DOF vehicle + 6-DOF manipulator; • detailed mathematical model used in the simulation, including thruster dynamics; • unknown ocean current added; • inaccurate initial parameters estimates used by the controller (about 15% for each parameter); • reduced-order regressor used in the control law; • digital implementation of the control law (at a sampling rate of 200 Hz); • low frequency and quantization of the sensor outputs (1 cm for the vehicle position at 10 Hz, 0.005 for the orientation at 10 Hz) and measurement noise. It is worth remarking that the adaptive control law uses a reduced number of dynamic parameters for each body. Each vector î is composed of nine elements. The effectiveness of using a reduced-order regressor has been demonstrated in [3].

Fig. 3. path.

Sketch of the initial configuration of the system and trace of the desired

Thruster dynamics is the cause of limit cycles in the vehicle behavior [22]; in the presented simulation, the controller has been designed neglecting thruster dynamics, while its dynamic effects have been considered in simulation. The simulation uses the hydrodynamic coefficients of [12], without control surfaces and adding a suitable number of thrusters to control the vehicle in all directions. The thruster mathematical model has been inspired from [23] with light coefficients changes; the thrust is proportional to the square of the propeller velocity. As is common in thruster control, an encoder that measures the propeller velocity is supposed to be available and fed back to a simple PID control. The gain that relates the propeller velocity to the thrust is known with a 10% error. The inverse mapping from the desired vehicle force/moment to the real vehicle force/moment is affected by both thruster dynamics and gain uncertainty. Simulation without thrusters have also been run that gives, obviously, better performance. For the sake of brevity, only the simulations with thrusters are reported in the paper. The desired end-effector trajectory is a straight line whose projection on the inertial axis is a segment of 0.2 m. The line has to be executed four times, with duration of the single trial of 4 s with a fifth-order polynomial time law. Then, 14 s of rest are imposed. The attitude of the end-effector must be kept constant during the motion. The initial configuration is (see Fig. 3)

= [0 q = [0

0 0 0 0 0]T m; deg T 180 0 0 90 180] deg :

(27)

The ocean current has been added in the simulation by properly considering the relative velocity in the mathematical model. It has been assumed an irrotational current, constant in the inertial frame

c

= [0:1 0:3 0 0 0 0]T m=s:

(28)

The redundancy of the system is used to keep the vehicle still. Notice that this choice is aimed only at simplifying analysis of the proposed controller; more complex secondary tasks can easily be managed with the proposed IK algorithm [4], [5]. For example, a secondary task might be designed so as to achieve joint limits’ avoidance. Due to the different

600


Fig. 6. Time history of the vehicle linear forces as output by the thrusters. Fig. 4. Time history of the 2-norms of the end-effector and vehicle position errors as output by the sensors. It can be observed that the manipulator recovers the vehicle position/orientation error.

Fig. 7.

Time history of the vehicle moments as output by the thrusters.

Fig. 5. Time history of the 2-norms of the end-effector and vehicle orientation errors as output by the sensors.

bandwidth between vehicle and manipulator, the positioning errors of the vehicle and the manipulator are different. Hence, the manipulator might be used to partly recover the vehicle tracking error; to this purpose, the IK algorithm has been slightly modified. In fact, to compute the end-effector pose, the algorithm implemented in simulation uses the actual vehicle position and orientation, rather than the corresponding reference variables. The control law parameters are

p;0 = 0:4 o;0 = 0:6 i = 0:9; i = 1; . . . ; n K v;0 = blockdiagf2700I 323 ; 7800I 323 g K v;i = blockdiagf630I 323 ; 770I 323 g; i = 1; . . . ; n K ;0 = 100I 929 K ;i = 40I 929 ; i = 1; . . . ; n: Figs. 4 and 5 report the time history of the end-effector and vehicle position and orientation errors. It can be observed that the vehicle is affected by large noise, quantization errors, and thruster dynamics effects; the manipulator recovers the vehicle positioning errors and allows having a small end-effector error. Notice that we do not assume external measurements; the 1-cm quantization strongly affects the end-effector minimal error. Figs. 6 and 7 present the time history of the vehicle forces and moments; notice the large initial overestimate due to the intentional wrong model compensation. Also, being the manipulator not neutrally

Fig. 8. Time history of the joint torques 1–3.

buoyant, a large force moment is experienced at rest. In Figs. 8 and 9, the time history of the joint torques is reported. Notice that only the torque of the second joint is mainly affected by the manipulator restoring forces. The control actions are characterized by a chattering phenomena caused by the sensors. Common techniques might be applied in order to smooth the input by decreasing the overall system performance. VI. CONCLUSIONS In this paper, a modular adaptive control scheme for the end-effector tracking problem of a UVMS has been proposed. The serialchain structure of the system has been exploited to decompose the control law in a set of simpler control laws, each of them designed for a single rigid body in the system. The proposed approach results in a modular control scheme which can be conveniently applied to complex


601

i

Since the quaternion associated with R i = I (i.e., representing two aligned frames) is Qi = f1; 0g, it is sufficient to represent the attitude error as "~ii . Finally, it is useful to consider the so-called propagation rule to compute the time derivative of the quaternion’s components ~_i = "~_i =

0 21 "~

T i

1

2

i

i ! ~i

~i + ~i!

1 2

i !i S (~ "i )~

(34)

i ~ ii = ! d;i 0 ! ii is the relative angular velocity expressed in the where ! frame i.

REFERENCES Fig. 9.

Time history of the joint torques 4–6.

mechanical systems, helps in reducing the computational burden, and allows its implementation on distributed computing hardware. Also, the occurrence of kinematic and representation singularities is overcome by expressing the control law in body-fixed coordinates and representing the attitude via the unit quaternion. Finally, a simulation case study has been carried out for a vehicle in spatial motion equipped with a 6-DOF manipulator. The computational burden is kept low by resorting to a reduced-order regressor for partial compensation of the UVMS dynamics. The obtained results confirm the effectiveness of the proposed scheme in terms of tracking errors and control effort. APPENDIX ATTITUDE ERROR REPRESENTATION The orientation of a given frame with respect to a fixed reference frame can be expressed in terms of the rotation matrix R r ( ) R r ( ) = cos II + (1

0 cos )rr 0 sin SS (r ) T

(29)

where is the angle and r 2 IR3 is the unit vector of the axis expressing the rotation needed to align the two frames, S (1) is the matrix operator performing the cross product between two (3 2 1) vectors, and I is the (3 2 3) identity matrix. The unit quaternion corresponding to the given rotation matrix is then defined as

Q = f; "g = cos

2

(30)

;

" = sin

2

r

(31)

where 0 for 2 [0; ]. This restriction for is necessary to ensure uniqueness of the quaternion, in that the two quaternions f; "g and f0; 0"g represent the same orientation, i.e., the same rotation matrix. The unit quaternion satisfies the unitary norm condition 2 + "T " = 1 . Let Qi fi ; "i g be the unit quaternion corresponding to R i , and Qd;i = fd;i ; "d;i g the unit quaternion corresponding to the desired frame orientation R d;i . The mutual orientation between the two frames can be expressed by the rotation matrix i

T R i = R i R d;i :

(32)

The unit quaternion Qi = f~i ; "~ii g extracted from Ri can be easily obtained via quaternion product [16] as i

T

~i = i d;i + "i "d;i i ~i

" = i "d;i

0

d;i

"i

0 S (" )" i

d;i

:

(33)

[1] G. Antonelli and S. Chiaverini, “SIMURV. A simulation package for underwater vehicle-manipulator systems,” in Proc. 3rd IMACS Symp. Mathematical Modeling, Wien, Austria, Feb. 2000, pp. 533–536. [2] G. Antonelli, F. Caccavale, and S. Chiaverini, “A modular scheme for adaptive control of underwater vehicle-manipulator systems,” in Proc. American Control Conf., San Diego, CA, June 1999, pp. 3008–3012. [3] G. Antonelli, F. Caccavale, S. Chiaverini, and G. Fusco, “A novel adaptive control law for underwater vehicles,” IEEE Trans. Control Syst. Technol., vol. 11, pp. 221–232, Feb. 2003. [4] G. Antonelli and S. Chiaverini, “Task-priority redundancy resolution for underwater vehicle-manipulator systems,” in Proc. IEEE Int. Conf. Robotics and Automation, Leuven, Belgium, May 1998, pp. 768–773. [5] , “Fuzzy redundancy resolution and motion coordination for underwater vehicle-manipulator systems,” IEEE Trans. Fuzzy Syst., vol. 11, pp. 445–452, Apr. 2003. [6] C. C. de Wit, E. O. Diaz, and M. Perrier, “Control of underwater vehicle/manipulator with composite dynamics,” in Proc. American Control Conf., Philadelphia, PA, June 1998, pp. 389–393. [7] S. Chiaverini, “Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators,” IEEE Trans. Robot. Automat., vol. 13, pp. 398–410, June 1997. [8] S. Chiaverini and B. Siciliano, “The unit quaternion: a useful tool for inverse kinematics of robot manipulators,” Syst. Anal., Modeling, Simulat., vol. 35, pp. 45–60, 1999. [9] M. W. Dunningan and G. T. Russel, “Evaluation and reduction of the dynamic coupling between a manipulator and an underwater vehicle,” IEEE J. Oceanic Eng., vol. 23, pp. 260–273, July 1998. [10] T. Fossen, “Adaptive macro–micro control of nonlinear underwater robotic systems,” in Proc. 5th Int. Conf. Advanced Robotics, vol. I, Pisa, Italy, 1991, pp. 1569–1572. , Guidance and Control of Ocean Vehicles. Chichester, U.K.: [11] Wiley, 1994. [12] A. J. Healey and D. Lienard, “Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles,” IEEE J. Oceanic Eng., vol. 18, pp. 327–339, July 1993. [13] H. Mahesh, J. Yuh, and R. Lakshmi, “A coordinated control of an underwater vehicle and robotic manipulator,” J. Robot. Syst., vol. 8, no. 3, pp. 339–370, 1991. [14] T. W. McLain, S. M. Rock, and M. J. Lee, “Experiments in the coordinated control of an underwater arm/vehicle system,” in Autonomous Robots, J. Yuh, T. Ura, and G. A. Bekey, Eds. Norwell, MA: Kluwer, 1996, pp. 213–232. [15] J. H. Ryu, D. S. Kwon, and P. M. Lee, “Control of underwater manipulators mounted on a ROV using base force information,” in Proc. IEEE Int. Conf. Robotics and Automation, Seoul, Korea, May 2001, pp. 3238–3243. [16] R. E. Roberson and R. Schwertassek, Dynamics of Multibody Systems. Berlin, Germany: Springer-Verlag, 1988. [17] N. Sarkar and T. K. Podder, “Motion coordination of underwater vehiclemanipulator systems subject to drag optimization,” in Proc. IEEE Int. Conf. Robotics and Automation, Detroit, MI, May 1999, pp. 387–392. [18] I. Schjølberg and T. I. Fossen, “Modeling and control of underwater vehicle-manipulator systems,” in Proc. 3rd Conf. Marine Craft Manoeuvring and Control, Southampton, U.K., 1994, pp. 45–57. [19] L. Sciavicco and B. Siciliano, Modeling and Control of Robot Manipulators. London, U.K.: Springer-Verlag, 2000. [20] “Nomenclature for Treating the Motion of a Submerged Body Through a Fluid,” SNAME, Soc. Naval Arch., Marine Eng., Tech. Res. Bulletin, 1950.

602


[21] T. J. Tarn and S. P. Yang, “Modeling and control for underwater robotic manipulators—An example,” in Proc. IEEE Int. Conf. Robotics and Automation, Albuquerque, NM, Apr. 1997, pp. 2166–2171. [22] L. L. Whitcomb and D. R. Yoerger, “Development, comparison, and preliminary experimental validation of nonlinear dynamic thruster models,” IEEE J. Oceanic Eng., vol. 24, pp. 481–494, Oct. 1999. [23] D. R. Yoerger, J. G. Cooke, and J. J. Slotine, “The influence of thruster dynamics on underwater vehicle behavior and their incorporation into control system design,” IEEE J. Oceanic Eng., vol. 15, pp. 167–178, Jan. 1990. [24] D. R. Yoerger and J. J. Slotine, “Robust trajectory control of underwater vehicles,” IEEE J. Oceanic Eng., vol. OE-10, pp. 462–470, Oct. 1985. [25] J. Yuh, “Modeling and control of underwater robotic vehicles,” IEEE Trans. Syst., Man, Cybern., vol. 20, pp. 1475–1483, Nov./Dec. 1990. [26] J. Yuh and M. West, “Underwater robotics,” J. Adv. Robot., vol. 15, no. 5, pp. 609–639, 2001. [27] J. Yuh, S. Zhao, and P. M. Lee, “Application of adaptive disturbance observer control to an underwater manipulator,” in Proc. IEEE Int. Conf. Robotics and Automation, Seoul, Korea, May 2001, pp. 3244–3248. [28] W. H. Zhu, Y. G. Xi, Z. J. Zhong, Z. Bien, and J. De Schutter, “Virtual decomposition-based control for generalized high-dimensional robotic systems with complicated structure,” IEEE Trans. Robot. Automat., vol. 13, pp. 411–436, June 1997.

Model-Based Space Robot Teleoperation of ETS-VII Manipulator Woo-Keun Yoon, Toshihiko Goshozono, Hiroshi Kawabe, Masahiro Kinami, Yuichi Tsumaki, Masaru Uchiyama, Mitsushige Oda, and Toshitsugu Doi Abstract—In our previous research, we developed space robot teleoperation technology to achieve control from the ground of effective manual manipulations in orbit. To solve the communication time delay in the space robot teleoperation, we propose a mixed force and motion command-based space robot teleoperation system that is a model-based teleoperation. Moreover, we have also developed a compact 6-degree-of-freedom haptic interface as a master device. The important features of our teleoperation system are its robustness against modeling errors and its ability to realize the force exerted by the operator at the remote site. In this paper, we introduce a new control method, which modified our model-based teleoperation system, to control the real robotic system Engineering Test Satellite VII manipulator. Surface-tracking and peg-in-hole tasks have been performed to confirm the effectiveness of our system. The experimental results obtained with our system including the haptic interface demonstrate its ability to perform these tasks in space without any major problems. We also evaluated different master device approaches for the model-based space teleoperation

Manuscript received October 22, 2002; revised April 1, 2003. This paper was recommended for publication by Associate Editor Y. Liu and Editor I. Walker upon evaluation of the reviewers’ comments. W.K. Yoon is with the Intelligent Systems Institute, National Institute of Advanced Industrial Science and Technology (AIST), Tsukuba 305-8568, Japan (e-mail: [email protected]). T. Goshozono and T. Doi are with Toshiba Corporation, Tokyo 105-8001, Japan. H. Kawabe and M. Uchiyama are with the Department of Aerospace Engineering, Tohoku University, Sendai 980-8579, Japan (e-mail: [email protected]; [email protected]). M. Kinami is with NS Solutions Corporation, Tokyo 104-8280, Japan. Y. Tsumaki is with the Department of Intelligent Machines and System Engineering, Hirosaki University, Hirosaki 036-8561, Japan (e-mail: [email protected]). M. Oda is with the Japan Aerospace Exploration Agency, Tsukuba 305-8505, Japan (e-mail: [email protected]). Digital Object Identifier 10.1109/TRA.2004.824700

system. For this purpose, we used two methods, which are a master–slave (MS) approach and a force-joystick approach. Our results show that the MS approach is the best control method for contact tasks in which the directions of motion of the slave arm and of the operator’s input force are different, as in the surface-tracking task. Index Terms—Engineering Test Satellite VII (ETS-VII) manipulator, force feedback, force-joystick (FJ), haptic interface, master–slave (MS), model-based teleoperation, modeling error.

I. INTRODUCTION The International Space Station (ISS) is being constructed at present by many countries. Extra vehicular activity (EVA), in which an astronaut works in space, is necessary to construct such a space structure. However, the cost of launching astronauts in space is very high, and EVA presents many dangers. Thus, there is a demand for developing an autonomous space robot to work in space in the place of astronauts. However, it is difficult to develop a perfect autonomous space robot with the present robot technology. For this reason, the technology in which an operator on the ground teleoperates a semiautonomous space robot becomes important. The development of such a technology is especially important in Japan, which does not have, at the moment, the technology to launch astronauts in space by itself. In this context, the National Space Development Agency (NASDA) of Japan launched the Engineering Test Satellite VII (ETS-VII) in 1997 [1], [2]. The aim of the ETS-VII is the development of rendezvous docking and space robot technologies. Therefore, the 6-degree-of-freedom (DOF) manipulator and experiment equipment that can be operated by remote control from the ground is carried in the ETS-VII, and various experiments were conducted [3]–[6]. Several sensors and other equipment for the rendezvous docking are also carried in the satellite. Moreover, various real space robotic projects like the manipulator flight demonstration (MFD) have been conducted [7]. The Japanese experiment module (JEM), which is a part of the ISS, consists of four components, one of them being the JEM remote manipulator system (JEMRMS). The JEMRMS that is operated by the astronaut in the ISS will be used for experiments being conducted on the JEM or for supporting JEM maintenance tasks. In Germany, the space robot technology experiment (ROTEX) also has been conducted [8]. Communication time delay is one of the biggest problems encountered by teleoperation of a space robot from the ground. It is thought generally that a master-slave (MS) approach, where the position and force information of the slave system can be displayed to an operator, is an effective method of teleoperation. Until now, however, the MS approach has not been used in a space teleoperation system [7], [8], because the bilateral system generally becomes unstable due to the inherent communication time delay. This has made it impossible for the operator to definitely “feel” the force information in the few stable bilateral systems tested so far [9]–[11]. A model-based teleoperation system has been proposed [12], [13] to solve this problem, and a predictive display using virtual-reality techniques was also introduced [14]. In this system, the operator teleoperates the virtual model of both the space robot and the environment equipment. This virtual model is calculated by a ground-based computer. Therefore, the virtual model is the result of real-time simulation on the ground. The force and position information for the master arm are also calculated in the virtual world on the ground-based computer and can be stably and clearly displayed to an operator. In the space shuttle, where no time delay exists, a joystick that cannot display the force information is used as the master device. The joystick has been used in many space teleoperation projects like the ETS-VII and is also intended to be used in the ISS [2], [15], [16].

1042-296X/04$20.00 © 2004 IEEE