1

Adaptive Control of Manipulators Forming ClosedKinematic Chain with Inaccurate Kinematic Model

Farhad Aghili,Senior Member, IEEE

Abstract—The problem of self-tuning control of cooperativemanipulators forming closed kinematic chain in the presence ofinaccurate kinematics model is addressed in this paper. Thekine-matic parameters pertaining to the relative position/orientationuncertainties of the interconnected manipulators are updatedonline by two cascaded estimators in order to tune a cooperativecontroller for achieving accurate motion tracking with min imum-norm actuation force. This technique permits accurate calibrationof the relative kinematics of the involved manipulators withoutneeding high precision end-point sensing or force measurements,and hence it is economically justified. Investigating the stabilityof the entire real-time estimator/controller system reveals thatthe convergence and stability of the adaptive control process canbe ensured if i) the direction of angular velocity vector does notremain constant over time, andii) the initial kinematic parametererror is upper bounded by a scaler function of some known pa-rameters. The adaptive controller is proved to be singularity-freeeven though the control law involves inverting the approximationof a matrix computed at the estimated parameters. Experimentalresults demonstrate the sensitivity of the tracking performanceof the conventional inverse dynamic control scheme to kinematicinaccuracies, while the tracking error is significantly reduced bythe self-tuning cooperative controller.

Index Terms—Cooperative manipulators, adaptive control,robot control, self-tuning control, least-square estimation, kine-matic uncertainty.

I. I NTRODUCTION

In some industrial applications, it is necessary to move alarge and heavy payload, such as a vehicle chases, by two ormore manipulators to share the load and also to gain stiffness.Cooperative multi-robot systems form a closed kinematicchain by each robot end-effector grasping a location on theobject. This configuration literally combines the cooperativeserial robots into a single redundant parallel mechanism witha high structural stiffness and large lifting capacity. However,when cooperative manipulators pick up objects of differentdimensions, i.e., uncertain orientation and length at gripingpoints, the overall kinematics of the interconnected roboticsystem changes and therefore the kinematic model is requiredto be updated unless the relative position/orientation of themanipulators are precisely known. However, to calibrate therelative kinematic parameters of the manipulators requiresprecision endpoint or camera-based measurement systems [1].This work proposes a self-tuning control of cooperative ma-nipulators to track a motion trajectory, without knowing thetrue kinematic parameters, and at the same time identify theparameters.

F. Aghili is research scientist with Canadian Space Agency (CSA), Saint-Hubert, Quebec, Canada, J3Y 8Y9, tel: (450) 926-4686, fax:(450) 926-4695,email: farhad.aghili@asc-csa.gc.ca.

The underlying control issue of how to synchronouslymove the involved end-effectors of a cooperative manipulatorsystem to track a predefined motion trajectory and at thesame time regulate the internal forces has been the focus ofmany research works [2]–[9]. However, one of the remainingproblem in synchronous control of cooperative manipulatorsis the sensitivity of the interconnected robotic system tothe uncertainties of the closed-kinematic loop. Even smallkinematic errors can give rise to significant control error and/oractuation force unless an impedance control or force feedbackis used to relieve the build-up internal forces. Although theforward kinematics of commercial manipulators are usuallyknown, the relative position/orientation of manipulatorswhichare set up to perform cooperative manipulation tasks may notbe precisely known. That introduces modeling uncertainty inthe overall closed kinematic chain and therefore precise andeffective cooperative control of the manipulators may requireclosed-loop kinematic calibration of one sort or another.

There are various non-adaptive and adaptive methods forcontrol of multiple cooperative manipulators. The earliestapproach was based on master/slave configuration of multiplerobots [2]–[4] in which the master robot is position controlledwhile and slave robots are under force control to maintainkinematic constraints. Another approach developed later con-siders the cooperative manipulators and the grasped objectas a closed kinematic chain [5]–[9]. Then, a hybrid posi-tion/force control strategy is utilized to simultaneouslycontrolboth the pose of the object and the internal forces [10]. Aninternal force-based impedance control scheme for cooperatingmanipulators is introduced in [11], [12]. The controller usesthe forces sensed at the robot end effectors to compensatefor the effects of the objects’ dynamics. Active complianceis employed in [13] in order to accommodate positioningerrors between the double cooperative manipulators. Adaptivecooperative control of manipulators with uncertain inertial pa-rameters has been also investigated in the literature [14]–[20].Walkeret al.proposed an adaptive cooperative control schemefor manipulators carrying an object with unknown parameters.Adaptive control of cooperative manipulators with an uncertaindynamic model to achieve asymptotic convergence of bothposition tracking errors and force tracking errors was presentedin [15]. Robust adaptive motion control of multiple robotsin the face of bounded disturbance was addressed in [16].A decentralized adaptive control scheme where each robot iscontrolled separately by its own local controller is presentedin [17]. The adaptive coordinated control of multiple robotstransporting an object where all dynamics parameters of therobots and object are unknown constants is further developed

2

in [18]. Adaptive hybrid force/motion control of cooperativemanipulators interacting with unknown environment is pro-posed in [19]. Although these adaptive cooperated controlschemes can deal with modeling uncertainties in dynamics ofthe robots and object, the kinematics of interconnected roboticsystem is assumed to be exactly known. On the other hand,experiments have shown that even small kinematic inaccuracy,due to tolerance and geometric uncertainties, can significantlydeteriorate the tracking performance. This work is motivatedby development of a self-tuning adaptive control of coopera-tive manipulators that can deal with the kinematic inaccuracyof the interconnected robotic system. The main advantage ofthe cooperative control approach is that it does not use forcemeasurements to accommodate the position/oritntation errorsbetween the cooperative manipulators. Rather the controlleris able to tune itself adaptively by identifying the parameterscontributing to the errors. Moreover, since no external posi-tioning device is required, such as a high precision camerasystem, the control approach is advantageous for calibratingthe relative pose of cooperative manipulators. However, itshould be pointed out that the control method works only ifthe common object is a rigid-body and that the stability can beensured upon certain conditions. The contributions of the thiswork are the development of an estimator for the kinematicparameters and rigorous analysis of the entire estimator/controlsystem leading to derivation of the stability conditions.

This paper is the extension of our previous work reportedin [21] on adaptive control of cooperative manipulators carry-ing a common object in the presence of geometric uncertain-ties. The self-tuning control method allows precise calibrationof the relative kinematic parameters of the manipulators whileperforming a cooperative manipulation task. The unknownposition/orintation variables of the closed-chain loop are esti-mated in real-time to tune a cooperated controller in orderto achieve motion tracking of a reference trajectory whileminimizing the Euclidean norm of the actuator forces. Themajor development in this work compared to [21] is thecompletion of the stability analysis of the adaptive controller.It is systematically proved that under the conditions thatthe direction of vector the angular velocity of the objectchanges over time and that initial kinematic error is sufficientlysmall, the convergence and stability of the combined on-lineestimator and controller can be ensured. Finally, comparativeexperimental results demonstrating the tracking performanceof the adaptive controller are presented.

NOMENCLATURE

a forgetting factorA(·) rotation matrix as function of quaternionfi generalized force exerted on theith end-effectorg perturbation vectorGp,Gd feedback gain matricesh sampling timehi nonlinear vector of theith manipulatorho nonlinear vector of the objecth, h actual nonlinear vector of the interconnect

manipulators and its estimate

A1 A2

B1 B2

v1 v2

f1 f2

ω2ω1

ρ

payload

Fig. 1. Cooperative robots carrying a common object.

I Identity matrix of proper dimensionJvi , Jωi

cost functionsJi Jacobian of theith manipulatorK Least-squares estimator gainMi Cartesian mass matrix of theith manipulatorMo Cartesian mass matrix of the objectM ,M actual mass matrix of of the interconnected

manipulators and its estimateqi joint angles of theith manipulatorT velocity transformation matrixui force input of theith manipulatoru force input of the reduced systemvi linear velocity ofith end-effector inBixi end-effector posez vector of the state errorωi angular velocity ofith end-effector inBiτi joint torques of theith manipulatorρ, ρ actual displacement vector and its estimationη, η actual quaternion and its estimationλ eigenvalue of a matrixλmax maximum eigenvalue of a matrixεvi , εωi

residual errorsµ fading memory factor‖·‖ Euclidean norm[·×] matrix form of cross-product

II. A DAPTIVE COOPERATIONCONTROL WITH

K INEMATICS UNCERTAINTIES

A. On-line Parameter Estimation

Fig.1 illustrates two cooperative manipulators carrying acommon object. Coordinate framesAi and Bi are at-tached to the base and end-effector of theith manipula-tor, respectively. Here, we make the following fundamentalassumptions on the kinematics model of the interconnectedrobotic system

i) The relative position/orientation of the end-effectorsgrasping on to the object, i.e.,A1 w.r.t. A2, arenot precisely known.

ii) The relative position/orientation of the manipulators’bases, i.e.,B1 w.r.t. B2, are not precisely known.

3

However, all manipulators forward kinematics are supposedtobe known. Assume thatvj andωj be the linear and angularvelocities of thejth robot’s end-effector expressed in the toolframe Aj. Notice thatω1 and ω2 are the same vectorexpressed in different coordinate frames. The end-effectorvelocities can be obtained from the kinematics model of themanipulators and the measurements of the manipulators’ jointangles and joint velocities as:

[

vj

ωj

]

= Jj(qj)qj j = 1, 2. (1)

where Jj is the Jacobian matrix of thejth manipulatorexpressed in the tool frame1 Aj andqj represent the jointangles of the corresponding manipulator. Also assume thatthe position and orientation of the end-effectors graspingtheobject are represented by vectorρ, which is expressed inA2, and rotation matrixA, which represents the orientationof coordinate frameA2 w.r.t. A1. The orientation can bealso represented by the unit quaternionη, which is related tothe rotation matrix by

A(η) = (2η2o − 1)I + 2ηo[ηv×] + 2ηvηTv . (2)

Here,I is the identity matrix with adequate dimension,ηv isthe vector part of the quaternion andηo is the scalar part, i.e.,η = [ηT

v ηo]T . Then, due to the closed-loop kinematics, the

velocities of the end-effectors are related to one other by

ω2 = A(η)ω1, (3a)

v2 = A(η)v1 − ρ× ω2. (3b)

Our estimation strategy is to solve (3) for parametersρ andη such that the residual errors are minimized in the least-squares sense. That is, given the measurements of the linearand angular velocitiesv1i , v2i , ω1i , andω2i at time ti, wehave

minη,ρ

1

2

k∑

i=1

ai

∥

∥

∥

∥

[

εωi

εvi

]∥

∥

∥

∥

2

s.t. ‖η‖ = 1

(4)

where

εωi(η) = ω2i −A(η)ω1i (5a)

εvi(η,ρ) = v2i −A(η)v1i + ρ× ω2i (5b)

are the residual errors, andai are non-negative weights,which can be selected according to the time varying forgettingfunction

ai = e−µ(tk−ti) (6)

with µ < 1 being a fading memory factor. Problem (4)is difficult to simultaneously solve forρ and η. However,one may notice that the residual errorεωi

is a function of

1The Jacobian expressed in the tool frameAJ is related to Jacobianexpressed in the base frameBJ by

AJ =

[

RT0

0 RT

]

BJ ,

whereR is the rotation matrix representing the end-effector orientation. Here,AJ = J for simplicity of notation.

only η. This may suggest that estimations ofη and ρ canbe sequentially obtained as follow: Firstly, find quaternionestimateη by minimizing the residual error in (5a). Then, usethe quaternion estimate to obtain displacement vector estimateρ by minimizing the second residual error in (5b).

Finding the orientation from the vector observation (3a) istantamount to the Wahba’s problem [22] that minimizes thecost function

Jω(tk) =1

2

k∑

i=1

ai‖εωi‖2, (7)

There are many algorithm to solve the above optimizationproblem [23]–[26], of which the parametrization of the orien-tation by a unit quaternion is the most elegant and useful one.It becomes immediately clear from expanding the expressionon RHS of (7) in terms of the angular velocities,

Jω =k∑

i=1

ai(

‖ω1i‖2 + ‖ω2i‖2)

−k

∑

i=1

aiωT2iAω1i , (8)

that the cost function is minimized when the last term in RHSof (8) is maximized. Moreover, using identity (2), one canshow that

ωT2iAω1i = ηT

Ω(ω1i ,ω2i)η, (9)

where

Ω(ω1,ω2) =

[

ω2ωT1 + ω1ω

T2 − ωT

1 ω2I ω2 × ω1

(ω2 × ω1)T ωT

1 ω2

]

.

Therefore, finding the optimal quaternion minimizing costfunction (7) is tantamount to solving the following quadraticprogramming

max ηTk Γkηk (10)

s.t. ‖ηk‖2 − 1 = 0,

whereηk is the optimal quaternion estimate at timetk, and

Γk =k∑

i=1

aiΩ(ω1i ,ω2i). (11)

Denote the Lagrangian equation of the above constrainedquadratic programming as

L = ηTΓkη − λ(ηTη − 1),

where λ is the Lagrangian multiplier. Then, the optimalsolution has to satisfy the stationary condition, which leadsto the following equation:

∂L∂η

= 0 ⇒ Γkηk − λmaxηk = 0. (12)

Notice that (12) is equivalent to the characteristic equationof Γk for the largest eigenvalueλmax = λmax(Γk). In otherwords, the optimal quaternion is the eigenvector of matrixΓk

corresponding to its largest eigenvalue.One can readily show that instead of summation (11), matrix

Γk can be updated recursively as

Γk = %Γk−1 +Ω(ω1k ,ω2k), (13)

where% = e−µh, (14)

4

and h is the sample interval. Despite the fact that (13) and(11) are equivalent, the recursive formula (13) is advantageousbecause of its computational efficiency and admitting initialquaternionη0, whereηT

0 η0 = 1. By inspection, one can showthat initial matrix

Γ0 = η0ηT0 ,

with λmax = 1, satisfies the eigenvalue equation (12).In the absence of measurement noise, the optimal solution

zeros the cost function (7). However, the estimated quaterniondoes not necessarily converge to the actual value unless thevelocity signals satisfy thepersistent excitationcondition.

Proposition 1: If the direction of the angular velocity doesnot remain constant over interval[tk tk+p], then the estima-tor (12) is convergent, i.e.,η converges toη.

Proof: Assume that the direction of the object angularvelocity vector changes at two instances of time,tm and tn,within the time interval. Then, denotingA = A(η), we cansay

ω2m − Aω1m = 0 (15a)

ω2n − Aω1n = 0, (15b)

which can be written as

Aω2m − ω2m = 0

Aω2n − ω2n = 0,

whereA = AAT is the rotation matrix of the estimation error.The above equations are equivalent to the characteristic equa-tion of matrix A for two linearly independent eigenvectors,ω2m ∦ ω2n , with eigenvalue1. Moreover, since the productsof all eigenvalues of a rotation matrix is one, i.e.,

3∏

i=1

λi(A) = det(A) = 1

the third eigenvalue ofA must be 1 too. Consequently,the only possibility for the rotation matrixA with all ofeigenvalues equal to one is the identity matrix meaning thatA = A.

Now with the orientation estimate in hand, one can obtainan estimate of the displacement vectorρ by making use of(5b). To this end, (3b) is rewritten in the form which is linearin terms of the unknown parameterρ, i.e.,

[ω2i×]ρ = A(ηi)v1i − v2i . (16)

Analogous to (7), the problem of finding the displacementvectorρ is formulated as minimizing the prediction error

Jv(tk) =1

2

k∑

i=1

ai‖εvi‖2, (17)

whereai, as defined in (6), incorporates exponential forgettingof data. Then, the parameter update law minimizing (17) isgiven by the following recursive least-squares estimator [27]

ρk+1 = ρk +Kk

(

v2k −A(ηk)v1k − ω2k × ρk

)

(18)

where the estimator gain is updated according to

Kk = −Pk[ω2k×](

%I − [ω2k×]Pk[ω2k×])−1

Pk+1 =1

%

(

I −Kk[ω2k×])

Pk (19)

and scalar% was defined in (14); see Appendix A.If regressor[ω2×] satisfies thepersistently excitingcondi-

tion, then estimator (18) is convergent. The persistent excita-tion condition dictates there existsp > 1 such that

Π =

k+p∑

i=k

−[ω2i×]2 > 0 ∀k > 0 (20)

Proposition 2: If the direction of the angular velocity vectordoes not remain constant over the time interval[tk tk+p],then the persistent excitation condition (20) is satisfied andestimator (18) is convergent, i.e.,ρ → ρ as t → ∞.

Proof: Matrix −[ω2i×]2 ≥ 0 is rank deficient and henceit is positive semi-definite but not positive definite. This isbecause for any vectorω ∈ R3 we have

eigenvalues of− [ω×]2 := 0, ‖ω‖2.

Although matrix−[ω2i×]2 is singular at every instance oftime, the persistent excitation condition requires that its inte-gral over an interval of length[tk, tk+p] be uniformly positivedefinite. Assuming that the direction of the angular velocityvector changes from timetm to tn, one can rewrite summation(20) as

Π = Υ−k+p∑

i = ki 6= n,m

[ω2i×]2, (21)

where

Υ = −[ω2m×]2 − [ω2n×]2.

The summation in the right-hand side of (21) is at least apositive semi-definite matrix. Since adding a positive semi-definite to a positive definite matrix always results in anotherpositive definite matrix, positive definiteness of matrixΠrests on showing thatΥ is a full-rank matrix. In a proofby contradiction, we show thatΥ must be a full-rank matrixprovided thatω2m andω2n are not collinear. SinceΥ is thesum of two positive semi-definite matrices, the only possibilityfor Υ being rank deficient is that there exists a non-zero vectorv 6= 0 such thatvT

Υv = 0. The latter equation in conjunctionwith the positive definiteness of−[ω2m×]2 and −[ω2n×]2

require that both identities

vT [ω2m×]2v = 0 and vT [ω2n×]2v = 0 (22)

are satisfied. The former and latter equations imply thatv mustbe parallel with vectorsω2m andω2n , respectively. However,the only possibility of these to happen is thatv is the zerovector becauseω2m and ω2n are linearly independent. Thismeans that matrixΥ is full-rank and thereforeΠ is a positivedefinite matrix.

5

τ1, τ2

ω1,v1

ω2,v2

ρ, η

q, qxd, xd, xd

Estimator

Controller Manipulators

J

Fig. 2. The sef-tuning cooperative control scheme.

B. Parametric Design of Coordination Control

Fig.2 schematically illustrates the self-tuning cooperativecontroller which incorporates the updated kinematic param-eters from the estimator. The concern is the stability of theentire estimation and control process and the tracking per-formance of the controller, given the time-varying kinematicparameters which starts from an initial guess at the parameters.

Assume that vectorsxi is the minimal representation of theposition and orientation of theith robot end-effector. Then,the following mapping is in order

xi , Li

[

vi

ωi

]

∀i = 1, 2 (23)

whereLi = diag (I,Lo(qi)), and transformation matrixLo

depends on a particular set of parameters used to representthe orientation [28]. Also, assume that the unknown kinematicparameters are placed in a single vector

θ =

[

ρ

η

]

.

Then, in view of (3) and (23), one can show that the general-ized velocities of the two manipulators are related by

x2 = L2T (θ)L−11 x1 (24)

where

T (θ) =

[

A(η)T A(η)T [ρ×]0 A(η)T

]

(25)

is the velocity transformation matrix. Time-derivative of(24)yields

x2 = L2TL−11 x1 +Dx1, (26)

whereD = L2T −L2TL−11 L1L

−11 .

The dynamics of theith manipulator in the task space [28]can be concisely written as

Mixi + hi(qi, qi) = ui − fi, ∀i = 1, 2 (27)

whereui = J−T

i τi,

see Appendix B. Here,τi is the vector of joint torques,fi is the vector of generalized force exerted at the end-effector, Mi is the Cartesian mass matrix, and nonlinearvectorhi contains the Coriolis, centrifugal, and gravity terms.Assuming kinematic singularity does not occur, there is aone-to-one correspondence with the torque vectorτi and thecorresponding auxiliary inputui. Therefore, in the followingderivation, we will takeui as the control input for the sake

of simplicity. Moreover, writing the balance of the forces onthe common object yields

Mox+ ho = f1 + T Tf2 (28)

wherex is the Cartesian coordinate of the object,Mo andho are the object inertia matrix and the associated Coriolis,centrifugal, and gravity terms, andT T is the correspondingforce transformation. Define the augmented control input andgeneralized coordinates, respectively, as

u =

[

u1

u2

]

and q =

[

q1q2

]

.

The velocity of the object can be related to one of the involvedend-effectors, say the first one, byx1 = Λx, where constantmatrixΛ represents the corresponding velocity transformation.Thus, upon substitution off1, f2, andx2 from (26) and (27)into(28), the latter equation can be arranged in the followingform

Mx+ h = u (29)

where

M(q, θ) , Mo +(

M1 + T (θ)TM2L2T (θ)L−11

)

Λ

h(q, q, θ) , ho + h1 + T (θ)T (h2 +M2DΛx)

N(θ) ,[

I T (θ)T]

(30a)

u , Nu (30b)

The reciprocal of (30b) is given by

u = N+(θ)u ⇐ min ‖u‖, (31)

whereN+ = NT (NNT )−1 is the generalized inverseofmatrix N , and henceNN+ = I. In view of (25) and (30a),the expression of the pseudo-inverse matrix can be derived inthe following form

N+ =

[

Q−1

TQ−1

]

, (32)

where the symmetric matrix

Q = NNT = I + T TT (33)

is turned out to be a function of onlyρ as follow:

Q(ρ) =

[

2I [ρ×]−[ρ×] 2I − [ρ×]2

]

. (34)

Therefore, the manipulators’ force inputs can be uniquelycomputed from the input of the reduced order system (29)by

u1 = Q−1u, and u2 = Tu1 (35)

Note that relation (35) requires that approximation of thematrix Q is non-singular. One can observe from (33) thatmatrix Q is the sum of two positive definite matrices andtherefore is positive definite and invertible.

Suppose the matrices and vector below are computed fromthe estimated parameters,θ,

T = T (θ),

Q = Q(ρ),

M = M(q, θ),

h = h(q, q, θ).

6

Consider the following inverse dynamics control law

u :=

u1 = Q−1[

M(

xd −Gde−Gpe)

+ h]

u2 = T u1,(36)

wherexd is the desired position trajectory,e = x−xd is theposition error, andGp > 0 andGd > 0 are the feedback gains.Notice that despite the control law (36) involves the matrixinversion computed at the estimated parameters, the matrixisalways invertible. In other words, the adaptive controllerissingularity-free. Supposez = [eT eT ]T is the state vectorerror, θ = θ − θ is error in the total parameter set,G =[Gp Gd] is the overall gain matrix. Then substituting (36)into (29), we will arrive at the equation of the closed-loopsystem as

z = Fz + g(z, t) (37)

where

F =

[

0 I

−Gp −Gd

]

, and

g =

[

0

M−1(

(NM − M)(

xd −Gz)

− h+ Nh)

]

(38)

is a perturbation term, in which

T = T (θ)− T (θ),

N = NN+ − I =[

0 T T]

N+ (39)

h = h− h

= T T (h2 +M2DΛx) + T TM2DΛx,

M = M − M

=(

T TM2L2T + T TM2L2T)

L−11 Λ.

If the velocity and acceleration commands are assumedbounded, i.e.,

‖xd‖ ≤ cv and ‖xd‖ ≤ ca, (40)

and the estimators are convergent, then the perturbation termsatisfies the following quadratic growth bound

‖g(z, t)‖ ≤(

κ0 + κ1 ‖z‖ + κ2 ‖z‖2)

‖θ(t)‖, (41)

for some positive constantsκ2, κ1, andκ0; see the Appendix Cfor detailed derivations. Moreover, for convergent estimator itis reasonable to assume bounded estimation error, i.e.,

supτ≥0

‖θ(τ)‖ = rθ (42)

for rθ < ∞. Since a small initial parameter errorθ(0) resultsin small parameter error all the time [27], there must existscalarα > 0 such that‖θ(τ)‖ ≤ α‖θ(0)‖ for all τ ≥ 0.The remainder of this section proves thatz(t) is ultimatelyboundedunder the assumption onrθ being sufficiently small.Furthermore, if‖θ‖ converges to zero, then so does‖z(t)‖for sufficiently larget.

SinceF is Hurwitz, there exists Lyapunov function

V (z) =√zTPz (43)

with P > 0 satisfying

PF + F TP = −I.

Suppose that the domain of analysis isz ∈ D, whereD =z ∈ R6| ‖z‖ ≤ rz. Then, the time-derivative ofV alongtrajectories of the (37) satisfies

V =(

− 1

2‖z‖2 + zTPg(z, t)

)

/V

≤ −σ ‖z‖2V

+b ‖z‖ ‖θ‖

V. (44)

whereσ = 12λ

− κ1rθ, b = λ(κ2r2z + κ0), andλ = λmax(P ).

Using the lower and upper bounds of the Lyapunov function,√

λ‖z‖ ≤ V ≤√

λ‖z‖, (45)

whereλ = λmin(P ), into (44) yields the following Bernoullidifferential inequality

V < −σV +b√λ‖θ‖. (46)

Here, the coefficientσ is positive if

rθ <1

2κ1λ(47)

is satisfied. Then, according to the comparison lemma [29, p.222], the solution of (46) must satisfy the inequality

V ≤ V (0)e−σt +b√λ

∫ t

0

e−σ(t−τ)‖θ(τ)‖dτ.

Using the upper and lower limits (45) in the above inequalitygives

‖z(t)‖ ≤ ‖z(0)‖√γe−σt +b

λ

∫ t

0

e−σ(t−τ)‖θ(τ)‖dτ (48)

whereγ = λ/λ is the condition number of matrixP . It canbe verified that

‖z(0)‖ ≤ 1√γrz and rθ ≤ σλ

brz (49)

ensures the validity of the earlier assumption that‖z‖ ≤ rz .Using the expression ofσ andb in the last inequality in (49)yields

rθ ≤ 0.5λrz

λ2κ0 + λλκ1rz + λ

2κ2r2z

(50)

Notice that restriction (50) automatically satisfies (47) becausethe RHS of the former inequality is always smaller than thatof the latter one. In other words, (50) ensuresσ > 0. It canbe easily verified that

rz =κ0

κ2

maximize the RHS of (50) and therefore

rθ ≤ α‖θ(0)‖ ≤ 1

2λ(

κ1 + γ(κ0 + κ2)) (51)

is the largest bound on the parameter estimation error that stillguarantees ultimate boundedness ofz(t). In this case, it is ap-parent from the response of the force system (48), that‖z(t)‖converges to zero if the input signal‖θ(t)‖ does so. In otherwords, restrictions (40), (51) and‖z(0)‖ ≤ κ0/(κ2

√γ) ensure

that the control errorz(t) is ultimate bounded. Furthermore,if ‖θ‖ → 0 as t → ∞ then‖z(t)‖ → 0 as t → ∞.

7

Fig. 3. The cooperative dual-arm manipulator system.

TABLE IDYNAMICS PARAMETERS OF THE MANIPULATORS.

Link mass Ixx Iyy Izz CM locationkg kgm2 kgm2 kgm2 m

1 27.31 0.31 0.38 0.30 −0.140i− 0.044j + 0.170k

2 21.00 0.25 3.35 3.30 −0.490i+ 0.000j + 0.125k

3 10.00 0.17 2.60 2.50 −0.401i+ 0.000j + 0.030k

4 4.33 0.035 0.05 0.02 −0.166i+ 0.079j + 0.171k

5 4.02 0.026 0.04 0.02 −0.162i− 0.009j + 0.204k

6 1.59 0.01 0.01 0.00 0.000i+ 0.000j + 0.220k

The above development can be summarized in the follow-ing. Assume the following assumptions

i) The direction of the angular velocity vector does notremain constant over time, i.e., convergence of theestimators according to Propositions 1 and 2.

ii) The velocity and acceleration command signals arebounded

iii) The initial kinematic parameter error is sufficientlysmall, i.e., condition (51) is satisfied

Then, the convergence and stability of the estimation andcontrol process are guaranteed.

III. E XPERIMENTAL RESULTS

This section describes experimental results obtained fromimplementation of the proposed self-tuning cooperative con-trol scheme using a dual-arm robotic system at the roboticslaboratory of the Canadian Space Agency (CSA), see Fig. 3.The proposed self-tuning cooperative controller was designedusing the kinematics and dynamics models of the manipulatorsas reported in [30]. The two manipulators have almost identicalinertial parameters as listed in Table I. The manipulators’jointsare equipped with torque sensors, which are used by an innerloop feedback controller to minimize joint frictions.

In this experiment, the two manipulators form a closed-chain by rigidly grasping a rigid bar. The initial guess values

0 2 4 6 8 10 12

−0.5

0

0.5

0 2 4 6 8 10 12

−0.2

−0.1

0

0.1

0.2

0.3

Time (sec)

vxvyvz

ωx

ωy

ωz

ω1

(rad

/s)

v1

(m/s

)

Fig. 4. Angular and linear velocities of the first robot end-effector.

of the position and orientation of grasping locations are:

ρ0 =

0.500

(m) and η0 =

0√22√220

(52)

The desired object attitude is specified by the following time-varying trajectories

Desired Euler angles:

00.15 cos(1.4t)0.06 cos(2.5t)

(rad)

Since the corresponding angular velocity is not with a fixedaxis, the attitude trajectories are suitable for identification ofthe kinematics parameters. The control objective is that thecommon object tracks the following position trajectories:

Desired poisition:

1.4 + 0.1 sin(t)2.2 + 0.05 sin(1.6t)0

(m)

In this experiment, the fading memory factors for the estima-tors is set to be

µ = 0.5.

This choice provides a reasonable compromise between thesmoothness and settling time of the estimated parametertrajectories. The controller gains are selected to beGp = 150andGd = 18, which corresponds to the closed-loop bandwidthof 2 Hz.

Fig.4 illustrates the linear and angular velocities of thefirst manipulator’s end-effector. This information is usedbycascaded estimators to estimate the kinematic parameters.Thetime-histories of the estimated kinematic parameters of theclosed-chain loop are shown in Fig.5. The graphs in the figureindicate that the steady-state estimates of the position andorientation are:

ρ∞ =

0.460−0.0190.033

(m) and η∞ =

0.03980.67450.7366−0.0302

, (53)

8

0 1 2 3 4 5 6 7 8 9 10

0

0.2

0.4

0.6

0.8

0 1 2 3 4 5 6 7 8 9 10−0.2

0

0.2

0.4

0.6

Time (sec)

ρxρyρz

η1η2η3η4

ηρ

(m)

Fig. 5. Time-histories of the estimated kinematic parameters with initialguess (52).

0 2 4 6 8 10 121.1

1.2

1.3

1.4

1.5

1.6

non−adaptive

0 2 4 6 8 10 122

2.1

2.2

2.3

2.4

reference

adaptive

x(m

)y

(m)

0 2 4 6 8 10 120.6

0.65

0.7

0.75

0.8

Time (sec)

z(m

)

Fig. 6. Position tracking trajectories with and without tuning the kinematicparameters with initial guess (52).

which are slightly different from the initial values (52).However, it turns out that even this small parameter errorsignificantly deteriorates the motion tracking performance.Fig. 6 comparatively demonstrates the tracking performanceof the adaptive and non-adaptive cooperative controllers.Thenon-adaptive controller uses the initial kinematic parameters(52), while the parameters are updated in real-time in thecase of the adaptive cooperative controller according to (12),(13), (18), and (19). However, for a fair comparison, bothadaptive and non-adaptive controllers use the same feedback

0 1 2 3 4 5 6 7 8 9 10

0

0.2

0.4

0.6

0.8

0 1 2 3 4 5 6 7 8 9 10−0.2

0

0.2

0.4

0.6

Time (sec)

ρxρyρz

η1η2η3η4

ηρ

(m)

Fig. 7. Time-histories of the estimated kinematic parameters with initialguess (54).

0 2 4 6 8 10 121.1

1.2

1.3

1.4

1.5

1.6

0 2 4 6 8 10 122

2.1

2.2

2.3

2.4

referenceactual

x(m

)y

(m)

0 2 4 6 8 10 120.6

0.65

0.7

0.75

0.8

Time (sec)

z(m

)

Fig. 8. Position tracking trajectories with and without tuning the kinematicparameters with initial guess (54).

gains. It is evident from the graph that the kinematic errorresults in relatively large tracking error of the non-adaptivecooperative controller. The self-tuning cooperative controller,however, significantly reduces the tracking error.

Now that the estimation of the kinematics parameters (53)in hand, we can assess the robustness of the adaptive controllerfor the case when the initial guess is far off. To this end, theexperiment was repeated with the following new set of initial

9

parameters:

ρ0 =

0.7−0.10.1

(m) and η0 =

0.20.60.75−0.2

(54)

Trajectories of the estimated parameters and the position areshown in Figs. 7 and 8, respectively. The graph show that theadaptive controller needs more time to learn the parametersand achieve tracking. In this case, the steady-state estimatesof the position and orientation are:

ρ∞ =

0.457−0.0220.031

(m) and η∞ =

0.03880.67400.7372−0.0289

. (55)

A comparison between (53) and (55) reveals that changing theinitial guess has negligible impact (about one percent) on thefinal values of the estimated parameters.

IV. CONCLUSIONS

Adaptive self-tuning control of cooperative manipula-tors carrying a common object in the presence of posi-tion/orientation uncertainties in the closed-kinematic loopwas presented. This allows accurate motion tracking perfor-mance in the presence of uncertainties in the relative posi-tion/orinetaiton of the manipulators’ bases as well as those oftheir end-effectors while grasping the object. The advantage ofthe control scheme is that it does not use either the measure-ment of the contact forces to compensate for the geometricuncertainties or a high precision end-point sensing devicefor fine calibration. However, the practical limitations ofthemethod are that it assumesi) a rigid-body common objectandii) an initial coarse estimate of the kinematic parameters.Two cascaded recursive least-squares estimators were usedtoestimate the kinematic parameters in real-time. A cooperatedcontroller was developed to take the estimated parameters inorder to achieve motion tracking of a reference trajectorywhile minimizing the weighted Euclidean norm of the actu-ator forces. The proposed adaptive controller is singularity-free because it involves inverting a matrix computed at theestimated parameters that turned out to be always invertible.The convergence and stability of the combined system ofthe estimator and controller have been thoroughly analyzedand the results has shown that tracking performance can beachieved provided thati) the direction of the angular velocityof the object changes over time;ii) the initial value error ofthe kinematic parameters is sufficiently small. Experimentalresults demonstrated that small errors in the kinematics modelof the interconnected two cooperative manipulator resulted insomewhat large tracking error, while the tracking error wassignificantly reduced by the self-tuning cooperative controller.

APPENDIX A

Consider the following linear parameterization form

yk = Wkak + ek. (56)

The least-squares method finds the current parameter estimateak which minimizes the cost function

J =

k∑

i=1

e−µ(tk−ti)‖(yi −Wiak)‖2 (57)

The parameter update law is

ak+1 = ak +Kk(yk − yk) (58)

whereyk = Wkak, % = e−µh, and

Kk = PkWTk

(

%I +WkPkWTk )−1

Pk+1 = (I −KkWk)Pk/%

are the gain and covariance matrices.

APPENDIX B

Dynamics equations of every manipulator in the joint spaceis described by

M ′i qi + h′

i(qi, qi) = τi + JTi fi (59)

Substituting the joint acceleration fromqi = J−1i xi −

J−1i Jiqi into (59) and then multiply the resultant equation

by J−Ti yields (27), in which the dynamics parameters in the

task space are related to those in the joint space by

Mi , J−Ti M ′

iJ−1i

hi , J−Ti h′

i −MiJiq

APPENDIX C

It is known the mass matrix is a positive definite matrixwith lower and upper bounded limited norms such that thefollowing inequality holds forcm, cM > 0

cmI ≤ Mi(q) ≤ cMI ∀i = 1, 2. (60)

The nonlinear vectorh(q, q) contains the gravitational plusthe Coriolis and centrifugal terms. For revolute joint robots,the function has a periodic dependence ofq and quadraticdependence onq [28, p.144]. Therefore, the nonlinear vectorcan be assumed bounded as

‖hi(q, q)‖ ≤ cg + ch ‖x‖2

≤ cg + chcv + ch ‖z‖2 . (61)

Similar to (60) and (61), the bound limits can be expressedfor the interconnected system (29) as

cmI ≤ M(q) ≤ cMI, (62)

and∥

∥h(q, q)∥

∥ ≤ cg + chcv + ch ‖z‖2 . (63)

The inverse matrixN+ always exists and hence the matrixmust be upper bounded, i.e.,

N+ ≤ cnI ∀θ ∈ Θ (64)

10

for cn > 0. Moreover, sinceT andD are Lipschitz in terms ofthe parameter variables, we can say there exist positive scalarεt andεd such that

‖T ‖ = ‖T (θ)− T (θ)‖ ≤ εt‖θ‖ (65)

‖D‖ = ‖D(θ)−D(θ)‖ ≤ εd‖θ‖ (66)

From inequalities (64) and (65) and identity (39) we get

‖N‖ ≤ ctcn‖θ‖. (67)

Using the norm properties and the above inequalities, we arriveat the following inequality

‖h‖ ≤(

εtcg + εtchcv + (εt + εd)cMcdcλcv

+ (εt + εd)cMcdcλ‖z‖+ εtch‖z‖2)

‖θ‖‖NM − M‖ ≤ εtcM (cn + 2clcλct)‖θ‖‖Nh‖ ≤ εtcn(cg + chcv‖z‖+ ch‖z‖2)‖θ‖ (68)

wherect = ‖T ‖ and cd = ‖D‖ for all θ ∈ Θ, cλ = ‖Λ‖,and cl = σ(L2)/σ(L1). Finally using bound limits (67) inthe expression of the perturbation term (38) yields inequality(41), in which

κ0 =(εtcMca(cn + 2clcλct) + εtcg + εtchcv

+ (εt + εd)cMcdcλcv + εtcncg)/cm

κ1 =(εtcM (cn + 2clcλct)‖G‖+ (εt + εd)cMcdcλ

+ εtcnchcv)/cm

κ2 =(ch + cnch)εt/cm

ACKNOWLEDGEMENT

This work was supported by Canadian Space Agency (CSA)and Natural Sciences and Engineering Research Council ofCanada (NSERC) under grant RGPIN/288255-2011.

REFERENCES

[1] I.-W. Park, B.-J. Lee, S.-H. Cho, Y.-D. Hong, and J.-H. Kim, “Laser-based kinematic calibration of robot manipulator using differentialkinematics,”Mechatronics, IEEE/ASME Transactions on, vol. PP, no. 99,pp. 1–9, 2011.

[2] C. O. Alford and S. M. Belyen, “Coordinated control of tworobot arms,”in IEEE Int. Conf. on Robotics & Automation, Atalnta, GA, 1984, pp.468–473.

[3] T. Tarn, A. Bejczy, and X. Yuan, “Control of two coordinated robots,” inIEEE Int. Conf. on Robotics & Automation, Apr 1986, pp. 1193–1212.

[4] J. M. Tao, J. S. Luh, and Y. F. Zheng, “Compliant coordination controlof two moving industrial robots,” inIEEE Int. Conf. on Robotics &Automation, Apr 1987, pp. 1407–1412.

[5] S. Hayati, “Hybrid position/force control of multiarm cooperatingrobots,” in IEEE Int. Conf. on Robotics & Automation, Apr 1986, pp.82–89.

[6] Y. Nakamura, K. Nagai, and T. Yoshikawa, “Dynamics and stability incoordination of multiple robotic mechanism,”Int. Journal of RoboticsResearch, vol. 8, no. 2, Apr 1989.

[7] B. Yao, W. Gao, S. Chan, and M. Cheng, “Vsc coordinated controlof two manipulator arms in the presence of environmental constraints,”Automatic Control, IEEE Transactions on, vol. 37, no. 11, pp. 1806–1812, nov. 1992.

[8] S. Arimoto, F. Miyazaki, and S. Kawamura, “Cooperative motioncontrol of multiple robot arms or fingers,”IEEE Trans. on Robotics& Automation, vol. 9, pp. 226–231, Apr 1993.

[9] P. Pagilla and M. Tomizuka, “Hybrid force-motion control of two armscarrying an object,” inIEEE American Control Conf, 1994, pp. 195–199.

[10] M. H. Raibert and J. J. Craig, “Hybrid position/force control of manip-ulators,” vol. 102, pp. 126–133, Jun. 1981.

[11] R. C. Bonitz and T. C. Hsia, “Internal force-based impedance control forcooperating manipulators,”Robotics and Automation, IEEE Transactionson, vol. 12, no. 1, pp. 78–89, feb. 1996.

[12] F. Caccavale, P. Chiacchio, A. Mario, and L. Villani, “Six-DOFimpedance control of dual-arm coopertive manipulators,”IEEE/ASMETrans. on Mechatronics, vol. 13, no. 5, pp. 576–586, 2008.

[13] T. Arai, H. Osumi, T. Fukuoka, and K. Moriyama, “A cooperative as-sembly system using two manipulators with precise positioning devices,”CIRP Annals – Manufaturing Technology, vol. 44, no. 1, pp. 23–26,1995.

[14] M. Walker, D. Kim, and J. Dionise, “Adaptive coordinated control oftwo manipulator arms,” inIEEE Int. Conf. on Robotics & Automation,Scottsdale, AZ, May 1989, pp. 1084–1090.

[15] Y. Hu and A. A. Goldenberg, “An adaptive approach to motion andforce control of multiple coordinated robot arms,” inIEEE Int. Conf. onRobotics & Automation, Scottsdale, AZ, May 1989, pp. 1091–1096.

[16] M. Zribi and S. Ahmed, “Robust adaptice control of multiple robotsin cooperative motion usingσ modification,” in IEEE Int. Conf. onRobotics & Automation, Sacramento, CA, Apr 1991, pp. 2160–2165.

[17] D. Sun and J. K. Mills, “Adaptive synchronized control for coordinationof multirobot assembly tasks,”Robotics and Automation, IEEE Trans-actions on, vol. 18, no. 4, pp. 498 – 510, aug. 2002.

[18] J. H. Jean and L. C. Fu, “An adaptive control scheme for coordi-nated multimanipulator systems,”IEEE Trans. Robotics and Automation,vol. 9, no. 2, pp. 226–231, 1993.

[19] M. Namvar and F. Aghili, “Adaptive force-motion control of coordinatedrobots interacting with geometrically unknown environments,” IEEETrans. on Robotics, vol. 21, no. 4, pp. 678–694, Aug. 2005.

[20] W. Gueaieb, F. Karray, and S. Al-Sharhan, “A robust hybrid intelligentposition/force control scheme for cooperative manipulators,” Mechatron-ics, IEEE/ASME Transactions on, vol. 12, no. 2, pp. 109–125, april 2007.

[21] F. Aghili, “Self-tuning cooperative control of manipulators with positionand orientation uncertainties in the closed-kinematic loop,” in Proc. ofIEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), San Francisco, CA, Sep. 2011, pp. 4187–4193.

[22] G. Wahba, “A least-squares estimate of satellite attitude,” SIAM Review,vol. 7, no. 3, p. 409, 1965.

[23] G. M. Lerner, Spacecraft Attitude Determination and Control. TheNetherlands: Kluwr Academic Publishers, 1978, ch.q Method, pp. 426–428.

[24] M. D. Shuster and S. D. Oh, “Three-axis attitude determination fromvector observation,”AIAA Journal of Guidance, Control, and Dynamics,vol. 4, no. 1, pp. 70–77, 1981.

[25] F. L. Markley and D. Mortari, “Quaternion attitude estimation usingvector observations,”Journal of the Astronautical Sciences, vol. 44, no.2/3, pp. 359–380, 2000.

[26] J. L. Crassidid, F. L. Markley, and Y. Cheng, “Survey of nonlinearattitude estimation methods,”AIAA Journal of Guidance, Control, andDynamics, vol. 30, no. 1, pp. 12–28, 2007.

[27] J.-J. Slotine and W. Li,Applied Nonlinear Control, Englewood Cliffs,New Jersey, 1991.

[28] C. Canudas de Wit, B. Siciliano, and G. Bastin, Eds.,Theory of RobotControl. London, Great Britain: Springer, 1996.

[29] H. K. Khalil, Nonlinear Systems. New-York: Macmillan PublishingCompany, 1992.

[30] T. Lamarche, “CSA Automation and Robotics Testbed: Dynamicsmodel,” Canadian Space Agency, Tech. Rep. CSA-ST-CART-2002-002,2002.

11

Farhad Aghili received the B.Sc. in Mechanical En-gineering (’88) and M.Sc. in Biomedical Engineer-ing (’91) both from Sharif University of Technology,Tehran, Iran, and Ph.D. in Mechanical Engineering(’98) from McGill University, Montreal, Canada. InJanuary 1998, Dr. Aghili joined the Canadian SpaceAgency (CSA) where he is currently Research Scien-tist with the Space Exploration directory. He is alsoan Affiliate Associate Professor in the Department ofMechanical & Industrial Engineering of ConcordiaUniversity. Dr. Aghili is the author of more than 120

scientific papers, 3 book chapters, and 8 patents and patent applications in USand Canada. He was the recipient of the 2007 ASME/IEEE International Con-ference on Mechatronic and Embedded Systems Best Paper Award, 2005 IEEEInt. Conference on Control Applications (CCA) Best StudentPaper AwardFinalist (co-author/supervisor), and the CSA Inventor & Innovator Certificate.Dr. Aghili is the co-chair of IEEE Montreal Robotics & Automation TechnicalChapter and Associate Editor (AE) for the Conference Editorial Board (CEB)of the IEEE Robotics & Automation Society (RAS). His research interestscombine robotics and vision systems, spacecraft dynamics &control, andmechatronics systems.