Distributed extension

4.3. Shared-control scenario 105

conditions are fulfilled, Lemma 4.1 applies. The optimal virtual input uv for (4.9) can be thus defined according to (4.20), by considering the estimated human parameters ˆπh in place of the respective values in the cost function (4.9).

Finally, recall that, given the virtual input uv, the object reference trajectory xv is computed according to eq. (4.6) (block 3).

Wrench estim.

fˆ_hand ˆh_int,i Human-obj.

interact. f_h

Ref. traj.

estimationⁱxˆ_v Human model

estimation ˆπh

Optim. input uv

and ref. traj. xv Distr. control law ui

Leader Leader

Follower

Figure 4.5: Distributed architecture implemented by robot i for shared con-trol. The dashed box denotes that, depending on the role of the robot, dif-ferent blocks are executed: blue blocks on the top in the case of leader, green blocks on the bottom in the case of follower.

Distributed human and internal wrench estimation

Let hint,i be the i th sub-vector of hint(i.e., the contribution of the i th robot to the internal wrenches hint). The objective is to define a local observer in order for the i th robot to estimate hint,i as well as the human force f_h (or equivalently hh). In view of (4.5) and by considering the selection matrix Γi introduced in (2.24)

Γi =

Op . . . Ip

|{z}

robot i

. . . Op

∈ R^{p×N p}

the vector hint,i can be computed as

hint,i = Γihint= hi−ΓⁱG^†Gh= hi−ΓⁱG^†(Gihi+X

j6=i

Gjhj)

= (Ip− ΓⁱG^†Gi)hi

| {z }

known

− ΓⁱG^†X^N

j=1,j6=iGjhj

| {z }

unknown

(4.24)

which is composed of a first local known term and a second un-known term depending on the wrenches exerted by the other robots. Thus, by considering the i th robot, the object model in (4.4) can be reformulated as

Mox¨o = Gihi+ XN j=1,j6=i

Gjhj+ hh− C^o˙xo− go. (4.25)

4.3. Shared-control scenario 107

Based on the approach in [98], the following residual vector θi(t) ∈ R^p is introduced

θi(t) = Kθ

Z t t0

(α − Gⁱhi− θⁱ)dτ + Kθm(t) (4.26) with Kθ ∈ R^p×p a constant diagonal positive definite matrix, m(t) = Mo˙xo the generalized momentum of the object and α= g_o− 1

2˙x^T_o ∂Mo

∂xo

˙xo. From [98], it follows

˙θ_i(t) = −K^θθi(t) + Kθ

 X^N

j=1,j6=iGjhj + hh

 (4.27)

which represents a low-pass filter whose bandwidth depends on the gain matrix Kθ ∈ R^p×p and which leads to

θi(t) ≈ XN j=1,j6=i

Gjhj(t) + hh(t), ∀t. (4.28)

Thus, by replacing (4.28) in (4.24), it holds

hint,i = Γi(IN p− G^†G)h ≈ (I^p− ΓⁱG^†Gi)hi− ΓⁱG^†(θi− h^h) (4.29) Remark 4.1. By virtue of (4.28), the approximation error made in (4.29) about the computation of hint,i is

hint,i− (I^p− ΓⁱG^†Gi)hi− ΓⁱG^†(θi− h^h)

=ΓiG^† θi− h^h

−X

j6=i

Gjhj

(4.30) that is negligible only when the filter input (namely, P

j6=iGjhj+ hh) has a bandwidth much smaller than the cut-off frequency of the filter. Therefore, as made in [98] and citing works, a practical choice is to set Kθ as high as possible subject to the potential digital implementation of the filter itself.

Equation (4.29) makes evident that hint,i can not be computed

without the knowledge of hh which is unknown as well. In this regard, the following auxiliary signal ξ_i ∈ R^p is defined

ξ_i = (Ip− ΓⁱG^†Gi)hi− ΓⁱG^†θi (4.31) which, in light of (4.29) and by computing the term ΓiG^†, can be rewritten as

ξ_i ≈ h^int,i − ΓⁱG^†hh = hint,i− 1

Nhh. (4.32) Note that neither hint,i nor hh are known in (4.32), but an ap-proximation of hint,i −N¹hh is provided by the right-hand side of (4.31), i.e., by the auxiliary variable ξ_i. By resorting to the fol-lowing lemma, the i th robot can compute the estimate ⁱhˆh ∈ R^p of the human wrench hh as well as the estimate ˆhint,i of its con-tribution to the internal wrench.

Lemma 4.2. Let each robot run the following observer















˙zi = γ X

j∈Ni

sign(^jhˆh−ⁱhˆh)

ihˆh = zi− Nξi

hˆint,i = ξ_i+ 1 N

ihˆh

(4.33)

where γ ∈ R is a positive constant, zⁱ ∈ R^p is an auxiliary state, and sign(·) is the component-wise signum function. Then,

• ⁱhˆh approaches hh in finite time Th, ∀i = 1, ..., N. Equiv-alently, by defining the estimation error ⁱh˜h = hh −ⁱhˆh, it approaches the origin in finite time Th.

• ˆhint,i approaches hint,i in finite time Th, ∀i = 1, ..., N. Equiv-alently, by defining the estimation error ˜hint,i = hint,i− ˆhint,i, it approaches the origin approaches the origin in finite time.

Proof. By leveraging the same reasoning as in [79], it can be proved that the estimate ⁱhˆh converges to the average of

4.3. Shared-control scenario 109

−Nξi in finite-time Th. Thus, by virtue of (4.32) and by recall-ing that, by construction, it holds PN

i=1hint,i = 0p, it follows

ihˆh → −N¹

PN

i=1ξ_i ≈ h^h or, equivalently, ⁱh˜h → 0^p. Therefore, based on (4.32) and (4.33), the i th robot can also estimate its contribution to the internal wrenches hint,i as follows

hˆint,i = ξ_i+ 1 N

ihˆh. (4.34)

Since ⁱh˜h → 0, from (4.32) it also holds hˆint,i → ξi+ 1

Nhh = hint,i. (4.35)

This completes the proof. 

Finally, as consequence of the lemma above, the wrench contribu-tion of robot i to the object mocontribu-tion can be estimated as

hi− ˆh^int,i= hi− ξi− 1 N

ihˆh= ΓiG^†Gihi+ΓiG^†θi− 1 N

ihˆh (4.36) which converges to the actual value as well.

Generation and estimation of the object trajectory xv

Depending on the role of the i th robot, different approaches are pursued.

Leader robot: The leader robot is in charge of estimating the hu-man model according to (4.12) based on the estimate made as in (4.33) and defining the virtual input uv (Section 4.3.1), from which the object reference trajectory is then derived as in (4.23).

Follower robot: In order for the followers to estimate the reference trajectory, the solution in [80] is exploited. Let χ_v be the stacked vector of the desired trajectory, that is χ_v =

xvT ˙xvT x¨vTT

∈ R^3pandⁱχˆ_v the respective estimate made by the follower robot i. The following distributed observer is

adopted by the followers







i˙ˆχ_v = Aχiχˆ_v−µ^v,1BχBχTPχ−1 iνˆv− µ^v,2sign P⁻¹_χ ⁱνˆv

iνˆv =X

j∈Ni, j6=l

iχˆ_v−^jχˆ_v

+ bi iχˆ_v− χv

(4.37)

where l is the index associated with the leader robot, i belongs to the set of the follower robots, bi is 1 if the leader belongs to Ni

and 0 otherwise, µv,1, µv,2 ∈ R are positive gains, A^χ ∈ R^3p×3p and Bχ ∈ R^3p are matrices selected as

Aχ =





Op Ip Op

Op Op Ip

Op Op Op



, Bχ =

Op Op Ip

T

and Pχ ∈ R^3p×3p is a positive definite matrix. By following the same reasoning as in [80], it can be proved that, under a proper selection of gains, the observer in (4.37) guarantees the finite-time leader tracking, i.e., it holds in finite-time Tχ

kⁱχˆ_v(t) − χv(t)k = 0 ∀i 6= l, t ≥ T^χ. (4.38)

Distributed low-level control

The control input ui in (2.10) to cooperatively track the ob-ject reference traob-jectory and control the internal stresses, in the hypothesis of uncertain dynamics (2.12), is here devised. Let eint,i denote the internal wrench error vector of robot i, i.e., eint,i = h^d_int,i− h^int,i ∈ R^p, being h^d_int,i = Γih^d_int, and let ∆uf,i ∈ R^p denote the following integral error

∆uf,i(t) = kf

Z t t0

eint,idτ (4.39)

4.3. Shared-control scenario 111

with kf ∈ R a positive scalar regulating the internal wrench con-trol, and the error vector ex,i∈ R^p

ex,i(t) = (ⁱxˆv− xⁱ) + kc

Z t t0

X

j∈Ni

(xj − xⁱ)dτ (4.40) with kc ∈ R a positive constant, which is composed of a first term for the tracking of the object trajectory and a second synchroniza-tion term aimed at limiting the internal forces during the transient phases or due to unmodeled dynamics. Finally, the following aux-iliary variables are introduced















ζ_i =ⁱ˙ˆxv+ kc

X

j∈Ni

(xj− xⁱ) + kpex,i ∈ R^p ζ˜_i = ζ_i− ˙xⁱ = ˙ex,i+ kpex,i ∈ R^p

ρ_i = ζ_i+ ∆uf,i ∈ R^p si = ˜ζ_i+ ∆uf,i ∈ R^p

(4.41)

with kp ∈ R a positive constant and ˆ∆uf,ithe estimate of ∆uf,i(t) made by robot i as

∆uˆ f,i(t) =kf

Z t t0

ˆ

eint,idτ (4.42)

where ˆeint,i = h^d_int,i − ˆhint,i which takes into account that the internal wrench hint,i is not known but only locally estimated as in (4.35). By extending the approach in [78], the following control law is proposed

ui = ˆ¯Mi˙ρ_i + ˆ¯Ciρ_i+ ˆ¯η_i+ Kssi+ ∆ui

= ¯Yi(xi, ˙xi, ρ_i, ˙ρ_i) ˆπi+ Kssi+ ∆ui

(4.43)

being Ks∈R^p×p a positive definite matrix and ∆ui ∈ R^p selected as

∆ui =κi(t)si+ h^d_int,i + ˆ∆uf,i+ ΓiG^†Gihi + ΓiG^†θi− 1 N

ihˆh

=κi(t)si

| {z }

a

+ h^d_int,i + ˆ∆uf,i

| {z }

b

+ ΓiG^†(Gihi+ θi)− 1

Nhh+ 1 N

ih˜h

| {z }

c

(4.44) with κi(t) ∈ R a positive time-varying gain, and where

• a) is a robust term to guarantee convergence of internal forces in the transient phase and despite model uncertainties;

• b) is a force feed-forward and integral error contribution;

• c) represents, based on (4.36), the compensation of the con-tribution to the external generalized forces made by robot i on the object.

Moreover, the dynamic parameters of robot i are updated as

˙ˆπi = K⁻¹_π Y¯^T_i (xi, ˙xi, ρ_i, ˙ρ_i)^Tsi (4.45) with Kπ ∈ R^nπi×nπi a positive definite matrix.

The analysis of control law ui is provided in the theorem.

Theorem 4.3. Consider N robots with dynamics in (2.10) for which an estimate is known as in (2.12). Consider the observer in (4.37), the control law in (4.43) and the parameters update law in (4.45). Then, it asymptotically holds xi → x^v and eint,i → 0^p

∀i = 1, ..., N.

Proof. The proof is provided in Appendix C.1. 

Nel documento Human multi-robot interaction: from workspace sharing to physical collaboration (pagine 111-118)