3.3 Trajectory scaling approach
3.3.3 Decentralized extension
af-ter the scaling phase, the update law in (3.28) or (3.27) is selected, respectively.
Note that, as already stated at the beginning of the section, the scaling procedure does not generally ensure that the safety condition in (3.7) is always met since no scaling is allowed when it leads to violation of constraints in (3.11)-(3.12). As a conse-quence, an emergency procedure needs to be foreseen until the safety condition is restored as shown in Figure 3.3.
Finally, it is worth noticing that the scaling procedure might also be extended to take into account kinematic velocity, accel-eration and jerk constraints. Indeed, as in [75] and [76], such constraints can be formulated in terms of acceleration constraints which are linear with respect to the scaling parameter ¨c(t). Thus, additional bounds for ¨c(t) can be derived as in the above and com-bined with (3.25) and (3.26) so as to deal with both kinematic and safety constraints where, clearly, the former have higher priority due to mechanical limits.
3.3. Trajectory scaling approach 61
to properly scale the task trajectory σn(t) in order to generate a reference trajectory σr(t) which ensures the safety condition
F (t) ≥ ¯¯ Fmin(t), ∀t (3.29) is always satisfied and so as to obtain results as close as possible to those of the centralized case.
In order to solve the above problem, the approach presented in Section 3.3.2 is extended to a distributed setting by integrating proper decentralized observers to counteract the lack of global in-formation. A leader-follower paradigm is adopted where a leader robot performs the scaling procedure by computing the scaling parameters c(t), ˙c(t), ¨c(t), while followers estimate them to deter-mine the reference trajectory σr(t). The distributed extension is detailed in the following.
The following assumptions are made in the chapter.
Assumption 3.2. The human position, velocity and acceleration, namely the vectors po(t), ˙po(t), ¨po(t), are known by each robot at each time. For this purpose, either each robot may be equipped with an adequate sensor system or appropriate distributed observers can be introduced to track this information which may be known only to some of the robot in the team [77].
Assumption 3.3. The communication graph G (introduced in Section 2.3) is undirected, i.e., all communication links are bidi-rectional.
Assumption 3.4. The nominal cooperative task trajectory σn(t) ( ˙σn(t), ¨σn(t)) is known by each robot.
This assumption is not restrictive given the considered frame-work; however, it can be easily overcome at the expense of trans-ferring more data between the robots, i.e., in the case the nominal trajectory is known only by a non-empty subset of robots, the re-maining ones can estimate this trajectory by means of an observer of dimension 2m, as proposed for example in [78].
At this point, observe that, as already detailed in Section 3.3.2, in order for a leader robot to perform the scaling procedure and compute the scaling parameter c(t), it is required to estimate both the cumulative safety index in (3.6) and the coefficients of its derivative in (3.24). Moreover, by looking at the virtual input of the i th robot in (3.14), it also follows that each robot requires the knowledge of the reference task trajectory σr(t) (and of ˙σr(t) and ¨σr(t)) as well as of the collective state of the multi-robot system x and its derivative ˙x in order to compute σ(x) and its derivative, respectively, according to (2.13). Thus, by appropri-ately estimating these quantities, the input yi in (3.14) is adapted to the decentralized framework as follows
yi =J†iΓiJ†σ
i¨ˆσr+ kσ,d
i˙ˆσr−i˙ˆσ
+ kσ,p iσˆr−iσˆ
− J†iJ˙i˙qi+ ¨qn,i
(3.30)
whereiσˆr,i˙ˆσr,i¨ˆσr are now the estimates of σr and of its first and second derivatives made by robot i, which by virtue of (3.10), are computed as
iσˆr(t) = σn(iˆc(t))
i˙ˆσr(t) = ∂σn(ic(t))ˆ
∂iˆc
i˙ˆc(t)
i¨ˆσr(t) = ∂2σn(ic(t))ˆ
∂iˆc2
i˙ˆc2(t) + ∂σn(iˆc(t))
∂iˆc
i¨ˆc(t)
(3.31)
being iˆc, i˙ˆc, i¨ˆc the estimates made by robot i of the scaling pa-rameters c, ˙c, ¨c computed by the leader robot, and iσ,ˆ i ˙ˆσ are the estimates of the task function σ, ˙σ in (2.13) computed as
iσˆ = Jσix,ˆ i˙ˆσ = Jσi˙ˆx (3.32) being ixˆ and i˙ˆx the estimates made by robot i of the collective vectors x and ˙x, respectively.
Summarizing, once the leader robot has computed the scaling pa-rameter c(t) and its first two derivatives, it is necessary for the
3.3. Trajectory scaling approach 63
solution to Problem 3.2 to work, that they are estimated by fol-lower robots in order to determine the reference trajectory accord-ing to (3.31) and that x and ˙x are estimated by all robots so as to compute (3.32) and, finally, the virtual input in (3.30). In the following, the distributed estimators of all required quantities are detailed.
Distributed estimation of ¯F , µ1, µ2, ∇ ¯F
The problem of estimating and tracking the cumulative safety in-dex ¯F (t) in (3.6) and the coefficients µ1(t) and µ2(t) in (3.24) is first considered. Note that the latter are required to perform the scaling procedure by the leader robot. To this aim, the following stacked vector is introduced
ηi(t) = ¯Fi(t) µ1,i(t) µ2,i(t) ∇ ¯FiT(t)T
∈ R6
with i = 1, 2, . . . , N, and the cumulative variable η(t) = PN
i=1ηi(t), where ∇ ¯Fi(t) ∈ R3 represents the cumu-lative gradient of the safety index ¯Fi with respect to the positions of the human points defined as ∇ ¯Fi(t) = n1
o
Pno
j=1∇po,jF¯i(t). In addition, the cumulative gradient of the safety index ¯F is defined as ∇ ¯F = PN
i=1∇ ¯Fi(t), which is introduced in order to make it possible its exploitation in the case of redundant robots as shown in Section 3.4. The objective is to define a distributed algorithm such that each robot tracks in finite time η(t), i.e., such that there exists a time Tη for which it holds
kiηˆ(t) − η(t)k = 0, ∀t ≥ Tη, ∀ i (3.33) being iηˆ(t) ∈ R3 the estimate of η(t) made by the i th robot. By leveraging the approach in [79] working for undirected graphs, this problem can be solved by having each robot running the following
update law
i˙ˆν(t) = kη
X
j∈Ni
sign
jˆξ(t) −iˆξ(t)
iˆξ(t) =iνˆ(t) + ηi(t)
(3.34)
where iνˆ(t) ∈ R3 is an internal state, kη is a positive constant and sign(·) is the component-wise signum function. By defining
iη(t) = Nˆ iˆξ(t), it can be proved [79] that iη(t) converges to η(t)ˆ in finite time and, then, (3.33) holds true. Therefore, the update law in (3.34) allows the leader to track η(t) and consequently to execute the scaling procedure in (3.27) or (3.28).
Note that the observer in (3.34) requires the same quantity η(t) to be estimated by the follower robots despite it being not strictly necessary. However, this quantity can be exploited to let each agent (not only the leader one) monitor the safety field, and/or, as stated before, to properly handle the possible kinematic redun-dancy in order to increase the cumulative safety index. Finally, the related computational load is totally affordable for the system at hand being η(t) ∈ R6.
Distributed estimation of c(t), ˙c(t) and ¨c(t) by the follower robots
At this point, as explained before, the problem of allowing follower robots to track the scaling parameters determined by the leader is consider. Let ς(t) denote the stacked vector of the scaling param-eters, that is
ς(t) = ˆ
c(t) ˙ˆc(t) ¨ˆc(t)T
∈ R3
and iˆς(t) denote the respective estimation made by the follower robot i. The objective is to define a distributed leader tracking algorithm such that there exists a time Tc for which it holds
kiˆς(t) − ς(t)k = 0, ∀t ≥ Tc, ∀ i. (3.35)
3.3. Trajectory scaling approach 65
To this aim, the solution in [80] is exploited and the following distributed observer is adopted by the follower robots
i˙ˆς(t) =Aςi
ˆς(t) − kς,1BςBTςP−1ς iρˆ(t) − kς,2sign P−1ς iρ(t)ˆ
iρ(t) =ˆ X
j∈Ni, j6=l
iˆς(t) −jˆς(t) + bi i
ˆς(t) − ς(t)
(3.36) 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
(l ∈ Ni) and is 0 otherwise, iρˆ ∈ R3 is an auxiliary vector, kς,1
and kς,2 are positive gains, Aς ∈ R3×3 and Bς ∈ R3 are matrices selected as
Aς =
0 1 0 0 0 1 0 0 0
, Bς =
0 0 1T
and Pς ∈ R3×3 is a positive definite matrix. It is worth reminding that, since the communication graph is undirected and connected, there always exists a path from the leader to any follower. By following the same reasoning as in [80], it can be proved that the distributed observer in (3.36) fulfills the convergence condi-tion in (3.35) under the connectedness condicondi-tion of the undirected communication graph and a proper selection of the gains kς,1 and kς,2. Thus, by exploiting the components of iˆς(t), the i th robot can compute iσˆr(t), i˙ˆσr(t) and i¨ˆσr(t) according to (3.31).
Distributed estimation of x and ˙x
Finally, the finite-time observer that allows each robot to estimate the overall multi-robot state needed for (3.32) is introduced. For this purpose, let χ ∈ R2N p denote the stacked vector of the collec-tive end effector configurations and velocities, i.e.,
χ=
xT1 ˙xT1 . . . xTN ˙xTNT
and Sp ∈ RN p×2N pand Sv ∈ RN p×2N pthe selection matrices of the positional and velocity components in χ, respectively, that is
Sp = IN ⊗
Ip Op
, Sv = IN ⊗
Op Ip
such that
Spχ=
xT1 . . . xTNT
, Svχ = ˙xT1 . . . ˙xTNT
.
The aim is to design a distributed observer which allows each robot to track in finite-time the collective state χ, i.e., such that there exists a finite time Tχ for which it holds
kiχˆ(t) − χ(t)k = 0, ∀t ≥ Tχ (3.37) beingiχˆ ∈ R2N pthe estimation of χ made by robot i. By resorting to the approach in [81], the following update law is selected
i˙ˆχ = Aχiχˆ + kχ,1Gχiζˆ+ kχ,2 sign Gχiζˆ
+ Bχiuˆχ iζˆ= Πi(χ −iχ) +ˆ X
j∈Ni
jχˆ −iχˆ (3.38)
where iˆζ ∈ R2N p is an internal state, kχ,1 and kχ,2 are positive gains, Gχ ∈ R2N p×2N pis a positive definite matrix, Aχ∈ R2N p×2N p and Bχ∈ R2N p×N p are selected as follows
Aχ = IN ⊗
Op Ip
Op Op
, Bχ = IN ⊗
Op
Ip
iuˆχ ∈ RN p is the observer input defined as
iuˆχ = J†σ
i¨ˆσr+kσ,d
i˙ˆσr−i˙ˆσ
+kσ,p i
ˆ
σr−iσˆ
(3.39)